diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b07b58e350e319..83b76bbffcad7a 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -552,7 +552,11 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { config_msg_id = CONFIG_RATE_POSLLH; break; case MSG_STATUS: +#if UBLOX_RXM_RTCM_LOGGING + desired_rate = RATE_STATUS; +#else desired_rate = havePvtMsg ? 0 : RATE_STATUS; +#endif config_msg_id = CONFIG_RATE_STATUS; break; case MSG_SOL: @@ -1013,6 +1017,20 @@ void AP_GPS_UBLOX::log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm) AP::logger().WriteBlock(&header, sizeof(header)); #endif } + +void AP_GPS_UBLOX::log_status(const struct ubx_nav_status &status) +{ +#if HAL_LOGGING_ENABLED + if (!should_log()) { + return; + } + + AP::logger().WriteStreaming("UBX3", "TimeUS,iTOW,fixType,fixStat,diffStat,res,fixTime,uptime", "s-------", "F-------", "QIBBBBII", + AP_HAL::micros64(), + status.itow, status.fix_type, status.fix_status, status.differential_status, + status.res, status.time_to_first_fix, status.uptime); +#endif +} #endif // UBLOX_RXM_RTCM_LOGGING void AP_GPS_UBLOX::unexpected_message(void) @@ -1563,6 +1581,9 @@ AP_GPS_UBLOX::_parse_gps(void) Debug("MSG_STATUS fix_status=%u fix_type=%u", _buffer.status.fix_status, _buffer.status.fix_type); +#if UBLOX_RXM_RTCM_LOGGING + log_status(_buffer.status); +#endif _check_new_itow(_buffer.status.itow); if (havePvtMsg) { _unconfigured_messages |= CONFIG_RATE_STATUS; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 75a4975f5ff0db..c7b246803d2ea4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -861,6 +861,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend void log_rxm_raw(const struct ubx_rxm_raw &raw); void log_rxm_rawx(const struct ubx_rxm_rawx &raw); void log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm); + void log_status(const struct ubx_nav_status &status); #if GPS_MOVING_BASELINE // see if we should use uart2 for moving baseline config