diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7f8a9c9117..8af3af9f76 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -810,11 +810,13 @@ AP_GPS_UBLOX::_parse_gps(void) break; case MSG_MON_VER: _have_version = true; + strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion)); + strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "u-blox %d HW: %s SW: %s", state.instance + 1, - _buffer.mon_ver.hwVersion, - _buffer.mon_ver.swVersion); + _version.hwVersion, + _version.swVersion); break; default: unexpected_message(); @@ -1319,3 +1321,15 @@ float AP_GPS_UBLOX::get_lag(void) const break; }; } + +void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const +{ + AP_GPS_Backend::Write_DataFlash_Log_Startup_messages(); + + if (_have_version) { + gps._DataFlash->Log_Write_MessageF("u-blox %d HW: %s SW: %s", + state.instance+1, + _version.hwVersion, + _version.swVersion); + } +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index d489d42716..9705042c2a 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -114,6 +114,7 @@ public: } void broadcast_configuration_failure_reason(void) const override; + void Write_DataFlash_Log_Startup_messages() const override; // return velocity lag float get_lag(void) const override; @@ -522,6 +523,7 @@ private: uint8_t _next_message; uint8_t _ublox_port; bool _have_version; + struct ubx_mon_ver _version; uint32_t _unconfigured_messages; uint8_t _hardware_generation;