diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 880569ba08..a731f7aee2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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; instanceLog_Write_Message(buffer); + } +} + /* return the expected lag (in seconds) in the position and velocity readings from the gps */ diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 742229fc21..6393048e56 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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. diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h index 1ddbf871d3..c5404ff37a 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h index c14aa374de..374c77f427 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/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); diff --git a/libraries/AP_GPS/AP_GPS_MAV.h b/libraries/AP_GPS/AP_GPS_MAV.h index b2e01ba404..541af76c78 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.h +++ b/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; }; diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 1c8e64017d..21c2523c5e 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_MTK19.h b/libraries/AP_GPS/AP_GPS_MTK19.h index ea83206171..7933b8311a 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index e64e461002..73b94a9ab2 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/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. diff --git a/libraries/AP_GPS/AP_GPS_NOVA.h b/libraries/AP_GPS/AP_GPS_NOVA.h index d36ddf8a5a..7fb6415dd6 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.h +++ b/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); diff --git a/libraries/AP_GPS/AP_GPS_QURT.h b/libraries/AP_GPS/AP_GPS_QURT.h index e67ce350c2..520bc093ce 100644 --- a/libraries/AP_GPS/AP_GPS_QURT.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 5d574d6144..0289bf7071 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/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); diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h index a1dd3c6003..bbe2cb5391 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.h +++ b/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: // ************************************************************************ diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h index 306b28b1c2..7e8ad457e9 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.h +++ b/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: // ************************************************************************ diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 6779984000..962c657b40 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index ed8865bb7a..7449a73d51 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 22da37a644..d489d42716 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/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 { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index efb09c6cbd..097f138174 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/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)