diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a731f7aee2..300362411f 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -550,10 +550,10 @@ 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; + new_gps->broadcast_gps_type(); } } @@ -948,38 +948,6 @@ bool AP_GPS::blend_health_check() const return (_blend_health_counter < 50); } -void AP_GPS::_detection_message(char *buffer, uint8_t buflen, uint8_t instance) -{ - 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, buflen, - "GPS %d: specified as %s", - instance + 1, - driver->name()); - } -} - - -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 */ @@ -1070,12 +1038,10 @@ void AP_GPS::Write_DataFlash_Log_Startup_messages() } for (uint8_t instance=0; instanceLog_Write_Message(buffer); + drivers[instance]->Write_DataFlash_Log_Startup_messages(); } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 6393048e56..8d5f8b1d0c 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -19,7 +19,6 @@ #include #include #include -#include #include #include "GPS_detect_state.h" #include @@ -444,9 +443,6 @@ private: void detect_instance(uint8_t instance); void update_instance(uint8_t instance); - 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. The 8 bit flags field in GPS_RTCM_DATA is interpreted as: diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 5039d3d9fb..8c31fa53a6 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -24,6 +24,8 @@ # define Debug(fmt, args ...) #endif +#include // for send_statustext_all + extern const AP_HAL::HAL& hal; AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : @@ -134,3 +136,36 @@ AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) } } +void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const +{ + const uint8_t instance = state.instance; + const struct AP_GPS::detect_state dstate = gps.detect_state[instance]; + + if (dstate.detect_started_ms > 0) { + hal.util->snprintf(buffer, buflen, + "GPS %d: detected as %s at %d baud", + instance + 1, + name(), + gps._baudrates[dstate.current_baud]); + } else { + hal.util->snprintf(buffer, buflen, + "GPS %d: specified as %s", + instance + 1, + name()); + } +} + + +void AP_GPS_Backend::broadcast_gps_type() const +{ + char buffer[64]; + _detection_message(buffer, sizeof(buffer)); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, buffer); +} + +void AP_GPS_Backend::Write_DataFlash_Log_Startup_messages() const +{ + char buffer[64]; + _detection_message(buffer, sizeof(buffer)); + gps._DataFlash->Log_Write_Message(buffer); +} diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 097f138174..8094cfafb1 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -58,6 +58,9 @@ public: virtual const char *name() const = 0; + void broadcast_gps_type() const; + virtual void Write_DataFlash_Log_Startup_messages() const; + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters) @@ -77,4 +80,6 @@ protected: assumes MTK19 millisecond form of bcd_time */ void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds); + + void _detection_message(char *buffer, uint8_t buflen) const; };