AP_GPS: ability to log GPS detection state to dataflash

libraries/AP_GPS/AP_GPS.cpp

@@ -396,7 +396,7 @@ void AP_GPS::detect_instance(uint8_t instance)
     switch (_type[instance]) {
 #if CONFIG_HAL_BOARD == HAL_BOARD_QURT
     case GPS_TYPE_QURT:
-        _broadcast_gps_type("QURTGPS", instance, -1); // baud rate isn't valid
+        dstate->detect_started_ms = 0; // specified, not detected
         new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]);
         goto found_gps;
         break;
@@ -405,7 +405,7 @@ void AP_GPS::detect_instance(uint8_t instance)
     // user has to explicitly set the MAV type, do not use AUTO
     // do not try to detect the MAV type, assume it's there
     case GPS_TYPE_MAV:
-        _broadcast_gps_type("MAV", instance, -1);
+        dstate->detect_started_ms = 0; // specified, not detected
         new_gps = new AP_GPS_MAV(*this, state[instance], nullptr);
         goto found_gps;
         break;
@@ -414,8 +414,8 @@ void AP_GPS::detect_instance(uint8_t instance)
     // user has to explicitly set the UAVCAN type, do not use AUTO
     // do not try to detect the UAVCAN type, assume it's there
     case GPS_TYPE_UAVCAN:
+        dstate->detect_started_ms = 0; // specified, not detected
         if (AP_BoardConfig::get_can_enable() >= 1) {
-            _broadcast_gps_type("UAVCAN", instance, -1);
             new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr);
 
             // register new listener at first empty node
@@ -451,17 +451,14 @@ void AP_GPS::detect_instance(uint8_t instance)
     switch (_type[instance]) {
     // by default the sbf/trimble gps outputs no data on its port, until configured.
     case GPS_TYPE_SBF:
-        _broadcast_gps_type("SBF", instance, -1); // baud rate isn't valid
         new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
         break;
 
     case GPS_TYPE_GSOF:
-        _broadcast_gps_type("GSOF", instance, -1); // baud rate isn't valid
         new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]);
         break;
 
     case GPS_TYPE_NOVA:
-        _broadcast_gps_type("NOVA", instance, -1); // baud rate isn't valid
         new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]);
         break;
 
@@ -511,7 +508,6 @@ void AP_GPS::detect_instance(uint8_t instance)
             ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) ||
              _baudrates[dstate->current_baud] == 115200) &&
             AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
-            _broadcast_gps_type("u-blox", instance, dstate->current_baud);
             new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
         }
 #if !HAL_MINIMIZE_FEATURES
@@ -519,41 +515,34 @@ void AP_GPS::detect_instance(uint8_t instance)
         // and are surprisingly large
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
                  AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
-            _broadcast_gps_type("MTK19", instance, dstate->current_baud);
             new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
         } else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
                    AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
-            _broadcast_gps_type("MTK", instance, dstate->current_baud);
             new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
         }
 #endif
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
                  AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
-            _broadcast_gps_type("SBP", instance, dstate->current_baud);
             new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
         }
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
                  AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
-            _broadcast_gps_type("SBP", instance, dstate->current_baud);
             new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
         }
 #if !HAL_MINIMIZE_FEATURES
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
                  AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
-            _broadcast_gps_type("SIRF", instance, dstate->current_baud);
             new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
         }
 #endif
         else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) &&
                  AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) {
-            _broadcast_gps_type("ERB", instance, dstate->current_baud);
             new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]);
         } else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) {
             // prevent false detection of NMEA mode in
             // a MTK or UBLOX which has booted in NMEA mode
             if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
                 AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
-                _broadcast_gps_type("NMEA", instance, dstate->current_baud);
                 new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
             }
         }
@@ -561,6 +550,7 @@ void AP_GPS::detect_instance(uint8_t instance)
 
 found_gps:
     if (new_gps != nullptr) {
+        _broadcast_gps_type(instance);
         state[instance].status = NO_FIX;
         drivers[instance] = new_gps;
         timing[instance].last_message_time_ms = now;
@@ -958,25 +948,38 @@ bool AP_GPS::blend_health_check() const
     return (_blend_health_counter < 50);
 }
 
-void AP_GPS::_broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index)
+void AP_GPS::_detection_message(char *buffer, uint8_t buflen, uint8_t instance)
 {
-    char buffer[64];
-    if (baud_index >= 0) {
-        hal.util->snprintf(buffer, sizeof(buffer),
-                           "GPS %d: detected as %s at %d baud",
-                           instance + 1,
-                           type,
-                           _baudrates[baud_index]);
+    AP_GPS_Backend *driver = drivers[instance];
+    if (driver == nullptr) {
+        // we really shouldn't have been called
+        buffer[0] = '\0';
+        return;
+    }
+
+    const struct detect_state dstate = detect_state[instance];
+    if (dstate.detect_started_ms > 0) {
+        hal.util->snprintf(buffer, buflen,
+                 "GPS %d: detected as %s at %d baud",
+                 instance + 1,
+                 driver->name(),
+                 _baudrates[dstate.current_baud]);
     } else {
-        hal.util->snprintf(buffer, sizeof(buffer),
-                           "GPS %d: detected as %s",
-                           instance + 1,
-                           type);
+        hal.util->snprintf(buffer, buflen,
+                 "GPS %d: specified as %s",
+                 instance + 1,
+                 driver->name());
     }
-    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer);
 }
 
 
+void AP_GPS::_broadcast_gps_type(uint8_t instance)
+{
+    char buffer[64];
+    _detection_message(buffer, sizeof(buffer), instance);
+    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer);
+}
+
 /*
    re-assemble GPS_RTCM_DATA message
  */
@@ -1060,6 +1063,22 @@ void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
     }
 }
 
+void AP_GPS::Write_DataFlash_Log_Startup_messages()
+{
+    if (_DataFlash == nullptr) {
+        return;
+    }
+
+    for (uint8_t instance=0; instance<num_instances; instance++) {
+        char buffer[64];
+        _detection_message(buffer, sizeof(buffer), instance);
+        if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
+            continue;
+        }
+        _DataFlash->Log_Write_Message(buffer);
+    }
+}
+
 /*
   return the expected lag (in seconds) in the position and velocity readings from the gps
  */

libraries/AP_GPS/AP_GPS.h

@@ -364,6 +364,8 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    void Write_DataFlash_Log_Startup_messages();
+
 protected:
 
     // dataflash for logging, if available
@@ -441,7 +443,9 @@ private:
 
     void detect_instance(uint8_t instance);
     void update_instance(uint8_t instance);
-    void _broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index);
+
+    void _detection_message(char *buffer, uint8_t buflen, uint8_t instance);
+    void _broadcast_gps_type(uint8_t instance);
 
     /*
       buffer for re-assembling RTCM data for GPS injection.

libraries/AP_GPS/AP_GPS_ERB.h

@@ -36,6 +36,8 @@ public:
 
     static bool _detect(struct ERB_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "ERB"; }
+
 private:
     struct PACKED erb_header {
         uint8_t preamble1;

libraries/AP_GPS/AP_GPS_GSOF.h

@@ -34,6 +34,8 @@ public:
     // Methods
     bool read();
 
+    const char *name() const override { return "GSOF"; }
+
 private:
 
     bool parse(uint8_t temp);

libraries/AP_GPS/AP_GPS_MAV.h

@@ -35,6 +35,8 @@ public:
 
     void handle_msg(const mavlink_message_t *msg);
 
+    const char *name() const override { return "MAV"; }
+
 private:
     bool _new_data;
 };

libraries/AP_GPS/AP_GPS_MTK.h

@@ -36,6 +36,8 @@ public:
     static bool _detect(struct MTK_detect_state &state, uint8_t data);
     static void send_init_blob(uint8_t instance, AP_GPS &gps);
 
+    const char *name() const override { return "MTK"; }
+
 private:
     struct PACKED diyd_mtk_msg {
         int32_t latitude;

libraries/AP_GPS/AP_GPS_MTK19.h

@@ -36,6 +36,8 @@ public:
 
     static bool _detect(struct MTK19_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "MTK19"; }
+
 private:
     struct PACKED diyd_mtk_msg {
         int32_t latitude;

libraries/AP_GPS/AP_GPS_NMEA.h

@@ -62,6 +62,8 @@ public:
 
 	static bool _detect(struct NMEA_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "NMEA"; }
+
 private:
     /// Coding for the GPS sentences that the parser handles
     enum _sentence_types {      //there are some more than 10 fields in some sentences , thus we have to increase these value.

libraries/AP_GPS/AP_GPS_NOVA.h

@@ -34,6 +34,8 @@ public:
 
     void inject_data(const uint8_t *data, uint16_t len) override;
 
+    const char *name() const override { return "NOVA"; }
+
 private:
 
     bool parse(uint8_t temp);

libraries/AP_GPS/AP_GPS_QURT.h

@@ -29,6 +29,8 @@ public:
     
     bool read() override;
 
+    const char *name() const override { return "QURTGPS"; }
+
 private:
     bool initialised = false;
     uint32_t last_tow;

libraries/AP_GPS/AP_GPS_SBF.h

@@ -34,6 +34,8 @@ public:
     // Methods
     bool read();
 
+    const char *name() const override { return "SBF"; }
+
 private:
 
     bool parse(uint8_t temp);

libraries/AP_GPS/AP_GPS_SBP.h

@@ -38,6 +38,8 @@ public:
 
     static bool _detect(struct SBP_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "SBP"; }
+
 private:
 
    // ************************************************************************

libraries/AP_GPS/AP_GPS_SBP2.h

@@ -38,6 +38,8 @@ public:
 
     static bool _detect(struct SBP2_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "SBP2"; }
+
 private:
 
    // ************************************************************************

libraries/AP_GPS/AP_GPS_SIRF.h

@@ -35,6 +35,8 @@ public:
 
 	static bool _detect(struct SIRF_detect_state &state, uint8_t data);
 
+    const char *name() const override { return "SIRF"; }
+
 private:
     struct PACKED sirf_geonav {
         uint16_t fix_invalid;

libraries/AP_GPS/AP_GPS_UAVCAN.h

@@ -33,6 +33,9 @@ public:
 
     // This method is called from UAVCAN thread
     void handle_gnss_msg(const AP_GPS::GPS_State &msg) override;
+
+    const char *name() const override { return "UAVCAN"; }
+
 private:
     bool _new_data;
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -118,6 +118,8 @@ public:
     // return velocity lag
     float get_lag(void) const override;
 
+    const char *name() const override { return "u-blox"; }
+
 private:
     // u-blox UBX protocol essentials
     struct PACKED ubx_header {

libraries/AP_GPS/GPS_Backend.h

@@ -56,6 +56,8 @@ public:
     // driver specific lag
     virtual float get_lag(void) const { return 0.2f; }
 
+    virtual const char *name() const = 0;
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)