Merge pull request #1148 from paparazzi/fix_gps_furuno

Fix GPS furuno (Bebop)

The init function of the furuno GPS tried to add all settings messages to the transmit buffer in one go.
Since the uart buffer can't hold that many chars, only the first 4 or so config sentences were actually sent.

- Added nmea_configure function that is called in GpsEvent until configuration is finished.
- updated settings, enabled GSV message

sw/airborne/subsystems/gps/gps_furuno.c

@@ -31,51 +31,67 @@
 #include <stdio.h>
 #include <string.h>
 
-#define GPS_FURUNO_SETTINGS_NB    10
+#define GPS_FURUNO_SETTINGS_NB    18
 static const char *gps_furuno_settings[GPS_FURUNO_SETTINGS_NB] = {
+  "PERDAPI,FIRSTFIXFILTER,STRONG",
   "PERDAPI,FIXPERSEC,5",        // Receive position every 5 Hz
+  "PERDAPI,FIXMASK,SENSITIVITY",
+  "PERDAPI,STATIC,0,0",
+  "PERDAPI,LATPROP,-1",         // Disable latency position propagation
+  "PERDAPI,OUTPROP,0",          // Disable position output propagation
+  "PERDAPI,PIN,OFF",
+  "PERDAPI,GNSS,AUTO,2,2,0,-1,-1",
+  "PERDSYS,ANTSEL,FORCE1L",
   "PERDAPI,CROUT,ALLOFF",       // Disable all propriarty output
+  "PERDAPI,CROUT,V",            // Enable proprietary PERDCRV raw velocity message
   "PERDCFG,NMEAOUT,GGA,1",      // Enable GGA every fix
   "PERDCFG,NMEAOUT,RMC,1",      // Enable RMC every fix
   "PERDCFG,NMEAOUT,GSA,1",      // Enable GSA every fix
   "PERDCFG,NMEAOUT,GNS,0",      // Disable GNS
   "PERDCFG,NMEAOUT,ZDA,0",      // Disable ZDA
-  "PERDCFG,NMEAOUT,GSV,0",      // Disable GSV
-  "PERDCFG,NMEAOUT,GST,0",      // Disable GST
-  "PERDAPI,CROUT,V"             // Enable raw velocity
+  "PERDCFG,NMEAOUT,GSV,1",      // Enable GSV
+  "PERDCFG,NMEAOUT,GST,0"       // Disable GST
 };
 
+static uint8_t furuno_cfg_cnt = 0;
+
 static void nmea_parse_perdcrv(void);
 
 #define GpsLinkDevice (&(GPS_LINK).device)
 
-void nmea_parse_prop_init(void)
+/**
+ * Configure furuno GPS.
+ * Sets gps_nmea.is_configured to TRUE if all config msgs are sent.
+ */
+void nmea_configure(void)
 {
-  static uint8_t i = 0;
-  uint8_t j, len, crc;
+  uint8_t i, j, len, crc;
   char buf[128];
 
-  // Return when doen
-  if (i == GPS_FURUNO_SETTINGS_NB) {
-    return;
-  }
-
-  for (; i < GPS_FURUNO_SETTINGS_NB; i++) {
+  for (i = furuno_cfg_cnt; i < GPS_FURUNO_SETTINGS_NB; i++) {
     len = strlen(gps_furuno_settings[i]);
-    crc = nmea_calc_crc(gps_furuno_settings[i], len);
-    sprintf(buf, "$%s*%02X\r\n", gps_furuno_settings[i], crc);
-
     // Check if there is enough space to send the config msg
     if (GpsLinkDevice->check_free_space(GpsLinkDevice->periph, len + 6)) {
+      crc = nmea_calc_crc(gps_furuno_settings[i], len);
+      sprintf(buf, "$%s*%02X\r\n", gps_furuno_settings[i], crc);
       for (j = 0; j < len + 6; j++) {
         GpsLinkDevice->put_byte(GpsLinkDevice->periph, buf[j]);
       }
+      furuno_cfg_cnt++;
     } else {
-      break;
+      // Not done yet...
+      return;
     }
   }
+  gps_nmea.is_configured = TRUE;
 }
 
+void nmea_parse_prop_init(void)
+{
+  furuno_cfg_cnt = 0;
+}
+
+
 void nmea_parse_prop_msg(void)
 {
   if (gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "PERDCRV", 7)) {

sw/airborne/subsystems/gps/gps_nmea.c

@@ -64,12 +64,14 @@ static void nmea_parse_GSV(void);
 void gps_impl_init(void)
 {
   gps.nb_channels = GPS_NB_CHANNELS;
+  gps_nmea.is_configured = FALSE;
   gps_nmea.msg_available = FALSE;
   gps_nmea.pos_available = FALSE;
   gps_nmea.have_gsv = FALSE;
   gps_nmea.gps_nb_ovrn = 0;
   gps_nmea.msg_len = 0;
   nmea_parse_prop_init();
+  nmea_configure();
 }
 
 void gps_nmea_msg(void (* _cb)(void))
@@ -87,6 +89,11 @@ void gps_nmea_msg(void (* _cb)(void))
   gps_nmea.msg_available = FALSE;
 }
 
+void WEAK nmea_configure(void)
+{
+  gps_nmea.is_configured = TRUE;
+}
+
 void WEAK nmea_parse_prop_init(void)
 {
 }

sw/airborne/subsystems/gps/gps_nmea.h

@@ -39,6 +39,7 @@
 struct GpsNmea {
   bool_t msg_available;
   bool_t pos_available;
+  bool_t is_configured;       ///< flag set to TRUE if configuration is finished
   bool_t have_gsv;            ///< flag set to TRUE if GPGSV message received
   uint8_t gps_nb_ovrn;        ///< number if incomplete nmea-messages
   char msg_buf[NMEA_MAXLEN];  ///< buffer for storing one nmea-line
@@ -55,6 +56,7 @@ extern struct GpsNmea gps_nmea;
 /** The function to be called when a characted from the device is available */
 #include "mcu_periph/link_device.h"
 
+extern void nmea_configure(void);
 extern void nmea_parse_char(uint8_t c);
 extern void nmea_parse_msg(void);
 extern uint8_t nmea_calc_crc(const char *buff, int buff_sz);
@@ -66,6 +68,10 @@ static inline void GpsEvent(void (* _sol_available_callback)(void))
 {
   struct link_device *dev = &((GPS_LINK).device);
 
+  if (!gps_nmea.is_configured) {
+    nmea_configure();
+    return;
+  }
   if (dev->char_available(dev->periph)) {
     while (dev->char_available(dev->periph)) {
       nmea_parse_char(dev->get_byte(dev->periph));