AP_GPS: add logging of MSG_STATUS

ArduPilot · Sep 28, 2024 · 794a35d · 794a35d
1 parent ee29a94
commit 794a35d
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git 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
@@ -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