AP_GPS: fixed init strings to all be in progmem

and sent async ...

libraries/AP_GPS/AP_GPS.cpp

@@ -22,14 +22,6 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define GPS_DEBUGGING 0
-
-#if GPS_DEBUGGING
- # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(0); } while(0)
-#else
- # define Debug(fmt, args ...)
-#endif
-
 // table of user settable parameters
 const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
     // @Param: TYPE
@@ -70,6 +62,38 @@ const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U};
 // right mode
 const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
 
+/*
+  send some more initialisation string bytes if there is room in the
+  UART transmit buffer
+ */
+void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size)
+{
+    initblob_state[instance].blob = _blob;
+    initblob_state[instance].remaining = size;
+}
+
+/*
+  send some more initialisation string bytes if there is room in the
+  UART transmit buffer
+ */
+void AP_GPS::send_blob_update(uint8_t instance)
+{
+    // see if we can write some more of the initialisation blob
+    if (initblob_state[instance].remaining > 0) {
+        AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE;
+        int16_t space = port->txspace();
+        if (space > (int16_t)initblob_state[instance].remaining) {
+            space = initblob_state[instance].remaining;
+        }
+        while (space > 0) {
+            port->write(pgm_read_byte(initblob_state[instance].blob));
+            initblob_state[instance].blob++;
+            space--;
+            initblob_state[instance].remaining--;
+        }
+    }
+}
+
 /*
   run detection step for one GPS instance. If this finds a GPS then it
   will fill in drivers[instance] and change state[instance].status
@@ -104,22 +128,10 @@ AP_GPS::detect_instance(uint8_t instance)
 		uint16_t baudrate = pgm_read_word(&_baudrates[dstate->last_baud]);
 		port->begin(baudrate, 256, 16);		
 		dstate->last_baud_change_ms = now;
-        dstate->init_blob_offset = 0;
-    }
-
-    // see if we can write some more of the initialisation blob
-    if (dstate->init_blob_offset < sizeof(_initialisation_blob)) {
-        int16_t space = port->txspace();
-        if (space > (int16_t)sizeof(_initialisation_blob) - dstate->init_blob_offset) {
-            space = sizeof(_initialisation_blob) - dstate->init_blob_offset;
-        }
-        while (space > 0) {
-            port->write(pgm_read_byte(&_initialisation_blob[dstate->init_blob_offset]));
-            dstate->init_blob_offset++;
-            space--;
-        }
+        send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
     }
 
+    send_blob_update(instance);
 
     while (port->available() > 0 && new_gps == NULL) {
         uint8_t data = port->read();
@@ -174,7 +186,7 @@ AP_GPS::detect_instance(uint8_t instance)
 
 
 /*
-  update one GPS instance
+  update one GPS instance. This should be called at 10Hz or greater
  */
 void
 AP_GPS::update_instance(uint8_t instance)
@@ -191,6 +203,8 @@ AP_GPS::update_instance(uint8_t instance)
         return;
     }
 
+    send_blob_update(instance);
+
     // we have an active driver for this instance
     bool result = drivers[instance]->read();
     uint32_t tnow = hal.scheduler->millis();
@@ -222,6 +236,9 @@ AP_GPS::update(void)
     }
 }
 
+/*
+  set HIL (hardware in the loop) status for a GPS instance
+ */
 void 
 AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms, 
                Location &_location, Vector3f &_velocity, uint8_t _num_sats)

libraries/AP_GPS/AP_GPS.h

@@ -89,6 +89,10 @@ public:
         GPS_ENGINE_AIRBORNE_4G = 8
     };
 
+    /*
+      The GPS_State structure is filled in by the backend driver as it
+      parses each message from the GPS.
+     */
     struct GPS_State {
         uint8_t instance; // the instance number of this GPS
 
@@ -242,6 +246,10 @@ public:
     AP_Int8 _type[GPS_MAX_INSTANCES];
     AP_Int8 _navfilter;
 
+    // handle sending of initialisation strings to the GPS
+    void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
+    void send_blob_update(uint8_t instance);
+
 private:
     struct GPS_timing {
         // the time we got our last fix in system milliseconds
@@ -262,7 +270,6 @@ private:
         uint32_t detect_started_ms;
         uint32_t last_baud_change_ms;
         uint8_t last_baud;
-        uint8_t init_blob_offset;
         struct UBLOX_detect_state ublox_detect_state;
         struct MTK_detect_state mtk_detect_state;
         struct MTK19_detect_state mtk19_detect_state;
@@ -270,6 +277,11 @@ private:
         struct NMEA_detect_state nmea_detect_state;
     } detect_state[GPS_MAX_INSTANCES];
 
+    struct {
+        const prog_char *blob;
+        uint16_t remaining;
+    } initblob_state[GPS_MAX_INSTANCES];
+
     static const uint16_t  _baudrates[];
     static const prog_char _initialisation_blob[];
 

libraries/AP_GPS/AP_GPS_MTK.cpp

@@ -24,25 +24,24 @@
 #include <AP_GPS.h>
 #include "AP_GPS_MTK.h"
 
+// initialisation blobs to send to the GPS to try to get it into the
+// right mode
+const prog_char AP_GPS_MTK::_initialisation_blob[] PROGMEM = MTK_OUTPUT_5HZ SBAS_ON WAAS_ON MTK_NAVTHRES_OFF;
+
 AP_GPS_MTK::AP_GPS_MTK(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
     AP_GPS_Backend(_gps, _state, _port),
     _step(0),
     _payload_counter(0)
 {
-    // initialize serial port for binary protocol use
-    port->print(MTK_SET_BINARY);
-
-    // set 5Hz update rate
-    port->print(MTK_OUTPUT_5HZ);
-
-    // set SBAS on
-    port->print(SBAS_ON);
-
-    // set WAAS on
-    port->print(WAAS_ON);
+    gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob));
+}
 
-    // Set Nav Threshold to 0 m/s
-    port->print(MTK_NAVTHRES_OFF);
+/*
+  send an initialisation blob to configure the GPS
+ */
+void AP_GPS_MTK::send_init_blob(uint8_t instance, AP_GPS &gps)
+{
+    gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
 }
 
 

libraries/AP_GPS/AP_GPS_MTK.h

@@ -35,6 +35,7 @@ public:
     bool read(void);
 
     static bool _detect(struct MTK_detect_state &state, uint8_t data);
+    static void send_init_blob(uint8_t instance, AP_GPS &gps);
 
 private:
     struct PACKED diyd_mtk_msg {
@@ -76,6 +77,8 @@ private:
 
     // Buffer parse & GPS state update
     void        _parse_gps();
+
+    static const prog_char _initialisation_blob[];
 };
 
 #endif  // __AP_GPS_MTK_H__

libraries/AP_GPS/AP_GPS_MTK19.cpp

@@ -34,21 +34,7 @@ AP_GPS_MTK19::AP_GPS_MTK19(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
     _mtk_revision(0),
     _fix_counter(0)
 {
-    // initialize serial port for binary protocol use
-    // XXX should assume binary, let GPS_AUTO handle dynamic config?
-    port->print(MTK_SET_BINARY);
-
-    // set 5Hz update rate
-    port->print(MTK_OUTPUT_5HZ);
-
-    // set SBAS on
-    port->print(SBAS_ON);
-
-    // set WAAS on
-    port->print(WAAS_ON);
-
-    // Set Nav Threshold to 0 m/s
-    port->print(MTK_NAVTHRES_OFF);
+    AP_GPS_MTK::send_init_blob(_state.instance, _gps);
 }
 
 // Process bytes available from the stream

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -46,30 +46,28 @@ extern const AP_HAL::HAL& hal;
 // for NMEA.  GPS_AUTO will try to set any SiRF unit to binary mode as part of
 // the autodetection process.
 //
-const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
-    "$PSRF103,0,0,1,1*25\r\n"   // GGA @ 1Hz
-    "$PSRF103,1,0,0,1*25\r\n"   // GLL off
-    "$PSRF103,2,0,0,1*26\r\n"   // GSA off
-    "$PSRF103,3,0,0,1*27\r\n"   // GSV off
-    "$PSRF103,4,0,1,1*20\r\n"   // RMC off
-    "$PSRF103,5,0,1,1*20\r\n"   // VTG @ 1Hz
-    "$PSRF103,6,0,0,1*22\r\n"   // MSS off
-    "$PSRF103,8,0,0,1*2C\r\n"   // ZDA off
-    "$PSRF151,1*3F\r\n"                 // WAAS on (not always supported)
-    "$PSRF106,21*0F\r\n"                // datum = WGS84
-    "";
+#define SIRF_INIT_MSG \
+        "$PSRF103,0,0,1,1*25\r\n"   /* GGA @ 1Hz */ \
+        "$PSRF103,1,0,0,1*25\r\n"   /* GLL off */   \
+        "$PSRF103,2,0,0,1*26\r\n"   /* GSA off */   \
+        "$PSRF103,3,0,0,1*27\r\n"   /* GSV off */   \
+        "$PSRF103,4,0,1,1*20\r\n"   /* RMC off */   \
+        "$PSRF103,5,0,1,1*20\r\n"   /* VTG @ 1Hz */ \
+        "$PSRF103,6,0,0,1*22\r\n"   /* MSS off */   \
+        "$PSRF103,8,0,0,1*2C\r\n"   /* ZDA off */   \
+        "$PSRF151,1*3F\r\n"         /* WAAS on (not always supported) */ \
+        "$PSRF106,21*0F\r\n"        /* datum = WGS84 */
 
 // MediaTek init messages //////////////////////////////////////////////////////
 //
 // Note that we may see a MediaTek in NMEA mode if we are connected to a non-DIYDrones
 // MediaTek-based GPS.
 //
-const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
-    "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
-    "$PMTK330,0*2E\r\n"                                                                         // datum = WGS84
-    "$PMTK313,1*2E\r\n"                                                                 // SBAS on
-    "$PMTK301,2*2E\r\n"                                                                 // use SBAS data for DGPS
-    "";
+#define MTK_INIT_MSG \
+    "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" /* GGA & VTG once every fix */ \
+    "$PMTK330,0*2E\r\n"                                 /* datum = WGS84 */ \
+    "$PMTK313,1*2E\r\n"                                 /* SBAS on */ \
+    "$PMTK301,2*2E\r\n"                                 /* use SBAS data for DGPS */
 
 // ublox init messages /////////////////////////////////////////////////////////
 //
@@ -80,11 +78,12 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
 // We don't attempt to send $PUBX,41 as the unit must already be talking NMEA
 // and we don't know the baudrate.
 //
-const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
-    "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n"   // GGA on at one per fix
-    "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n"   // VTG on at one per fix
-    "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n"   // RMC off (XXX suppress other message types?)
-    "";
+#define UBLOX_INIT_MSG \
+    "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n"   /* GGA on at one per fix */ \
+    "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n"   /* VTG on at one per fix */ \
+    "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n"   /* RMC off (XXX suppress other message types?) */
+
+const prog_char AP_GPS_NMEA::_initialisation_blob[] PROGMEM = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG;
 
 // NMEA message identifiers ////////////////////////////////////////////////////
 //
@@ -105,14 +104,7 @@ AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
     _term_offset(0),
     _gps_data_good(false)
 {
-    // send the SiRF init strings
-    port->print_P((const prog_char_t *)_SiRF_init_string);
-
-    // send the MediaTek init strings
-    port->print_P((const prog_char_t *)_MTK_init_string);
-
-    // send the ublox init strings
-    port->print_P((const prog_char_t *)_ublox_init_string);
+    gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob));
 }
 
 bool AP_GPS_NMEA::read(void)

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -152,6 +152,8 @@ private:
     static const prog_char _gpgga_string[];
     static const prog_char _gpvtg_string[];
     //@}
+
+    static const prog_char _initialisation_blob[];
 };
 
 #endif // __AP_GPS_NMEA_H__

libraries/AP_GPS/AP_GPS_SIRF.cpp

@@ -28,9 +28,9 @@
 //
 // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
 //
-static const uint8_t init_messages[] PROGMEM = {
+const uint8_t AP_GPS_SIRF::_initialisation_blob[] PROGMEM = {
     0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
-    0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
+    0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 
 };
 
 AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
@@ -41,6 +41,7 @@ AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
     _payload_counter(0),
     _msg_id(0)
 {
+    gps.send_blob_start(state.instance, (const prog_char *)_initialisation_blob, sizeof(_initialisation_blob));
 }
 
 

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -102,6 +102,8 @@ private:
 
     bool        _parse_gps(void);
     void        _accumulate(uint8_t val);
+
+    static const uint8_t _initialisation_blob[];
 };
 
 #endif // AP_GPS_SIRF_h