AP_GPS: allow logging of UBX-RXM-RTCM

ArduPilot · Apr 16, 2024 · 7c11b32 · 7c11b32
1 parent 7d00c77
commit 7c11b32
Show file tree

Hide file tree

Showing 5 changed files with 99 additions and 5 deletions.
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
@@ -274,6 +274,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     AP_SUBGROUPINFO(params[1], "2_", 33, AP_GPS, AP_GPS::Params),
 #endif
 
+    // @Param: _RTCM_DATA
+    // @DisplayName: RTCM data logging
+    // @Description: Handles logging RTCM data; on uBlox chips that support RTCM data this will log RXM messages into logger;
+    // @Values: 0:Ignore,1:Always log
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("_RTCM_DATA", 34, AP_GPS, _rtcm_data, 0),
+
     AP_GROUPEND
 };
 

diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
@@ -615,6 +615,7 @@ class AP_GPS
     AP_Enum<SBAS_Mode> _sbas_mode;
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
+    AP_Int8 _rtcm_data;
     AP_Int8 _save_config;
     AP_Int8 _auto_config;
     AP_Int8 _blend_mask;

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -405,6 +405,17 @@ AP_GPS_UBLOX::_request_next_config(void)
         }
 #else
         _unconfigured_messages & = ~CONFIG_RATE_RAW;
+#endif
+        break;
+    case STEP_RTCM:
+#if UBLOX_RXM_RTCM_LOGGING
+        if(gps._rtcm_data == 0) {
+            _unconfigured_messages &= ~CONFIG_RATE_RTCM;
+        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RTCM)) {
+            _next_message--;
+        }
+#else
+        _unconfigured_messages & = ~CONFIG_RATE_RTCM;
 #endif
         break;
     case STEP_VERSION:
@@ -546,9 +557,9 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
             return;
         }
         break;
-#if UBLOX_RXM_RAW_LOGGING
     case CLASS_RXM:
         switch(msg_id) {
+#if UBLOX_RXM_RAW_LOGGING
         case MSG_RXM_RAW:
             desired_rate = gps._raw_data;
             config_msg_id = CONFIG_RATE_RAW;
@@ -557,11 +568,17 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
             desired_rate = gps._raw_data;
             config_msg_id = CONFIG_RATE_RAW;
             break;
+#endif // UBLOX_RXM_RAW_LOGGING
+#if UBLOX_RXM_RTCM_LOGGING
+        case MSG_RXM_RTCM:
+            desired_rate = gps._rtcm_data;
+            config_msg_id = CONFIG_RATE_RTCM;
+            break;
+#endif // UBLOX_RXM_RTCM_LOGGING
         default:
             return;
         }
         break;
-#endif // UBLOX_RXM_RAW_LOGGING
 #if UBLOX_TIM_TM2_LOGGING
     case CLASS_TIM:
         if (msg_id == MSG_TIM_TM2) {
@@ -939,6 +956,29 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
 }
 #endif // UBLOX_RXM_RAW_LOGGING
 
+#if UBLOX_RXM_RTCM_LOGGING
+void AP_GPS_UBLOX::log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm)
+{
+#if HAL_LOGGING_ENABLED
+    if (!should_log()) {
+        return;
+    }
+
+    uint64_t now = AP_HAL::micros64();
+
+    struct log_GPS_RTCM header = {
+        LOG_PACKET_HEADER_INIT(LOG_GPS_RTCM_MSG),
+        time_us    : now,
+        version : rtcm.version,
+        flags : rtcm.flags,
+        refStation : rtcm.refStation,
+        msgType : rtcm.msgType
+    };
+    AP::logger().WriteBlock(&header, sizeof(header));
+#endif
+}
+#endif // UBLOX_RXM_RTCM_LOGGING
+
 void AP_GPS_UBLOX::unexpected_message(void)
 {
     Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
@@ -1405,6 +1445,13 @@ AP_GPS_UBLOX::_parse_gps(void)
     }
 #endif // UBLOX_RXM_RAW_LOGGING
 
+#if UBLOX_RXM_RTCM_LOGGING
+    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RTCM && gps._rtcm_data != 0) {
+        log_rxm_rtcm(_buffer.rxm_rtcm);
+        return false;
+    }
+#endif // UBLOX_RXM_RTCM_LOGGING
+
 #if UBLOX_TIM_TM2_LOGGING
     if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
         log_tim_tm2();
@@ -2058,7 +2105,8 @@ static const char *reasons[] = {"navigation rate",
                                 "RTK MB",
                                 "TIM TM2",
                                 "M10",
-                                "L5 Enable Disable"};
+                                "L5 Enable Disable",
+                                "rtcm rate"};
 
 static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
 

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -56,6 +56,7 @@
 #define UBLOX_SET_BINARY_460800 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,460800,0*11\r\n"
 
 #define UBLOX_RXM_RAW_LOGGING 1
+#define UBLOX_RXM_RTCM_LOGGING 1
 #define UBLOX_MAX_RXM_RAW_SATS 22
 #define UBLOX_MAX_RXM_RAWX_SATS 32
 #define UBLOX_GNSS_SETTINGS 1
@@ -101,13 +102,14 @@
 #define CONFIG_TIM_TM2       (1<<18)
 #define CONFIG_M10           (1<<19)
 #define CONFIG_L5            (1<<20)
-#define CONFIG_LAST          (1<<21) // this must always be the last bit
+#define CONFIG_RATE_RTCM     (1<<21)
+#define CONFIG_LAST          (1<<22) // this must always be the last bit
 
 #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
 
 #define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
                     | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
-                    | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
+                    | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS | CONFIG_RATE_RTCM)
 
 //Configuration Sub-Sections
 #define SAVE_CFG_IO     (1<<0)
@@ -568,6 +570,16 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     };
 #endif
 
+#if UBLOX_RXM_RTCM_LOGGING
+    struct PACKED ubx_rxm_rtcm {
+        uint8_t version;
+        uint8_t flags;
+        uint16_t reserved1;
+        uint16_t refStation;
+        uint16_t msgType;
+    };
+#endif
+
     struct PACKED ubx_ack_ack {
         uint8_t clsID;
         uint8_t msgID;
@@ -627,6 +639,9 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 #if UBLOX_RXM_RAW_LOGGING
         ubx_rxm_raw rxm_raw;
         ubx_rxm_rawx rxm_rawx;
+#endif
+#if UBLOX_RXM_RTCM_LOGGING
+        ubx_rxm_rtcm rxm_rtcm;
 #endif
         ubx_ack_ack ack;
         ubx_ack_nack nack;
@@ -683,6 +698,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         MSG_NAV_SVINFO = 0x30,
         MSG_RXM_RAW = 0x10,
         MSG_RXM_RAWX = 0x15,
+        MSG_RXM_RTCM = 0x32,
         MSG_TIM_TM2 = 0x03
     };
     enum ubx_gnss_identifier {
@@ -739,6 +755,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         STEP_MON_HW2,
         STEP_RAW,
         STEP_RAWX,
+        STEP_RTCM,
         STEP_VERSION,
         STEP_RTK_MOVBASE, // setup moving baseline
         STEP_TIM_TM2,
@@ -820,6 +837,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     void log_tim_tm2(void);
     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);
 
 #if GPS_MOVING_BASELINE
     // see if we should use uart2 for moving baseline config

diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
@@ -9,6 +9,7 @@
     LOG_GPS_RAW_MSG,                            \
     LOG_GPS_RAWH_MSG,                           \
     LOG_GPS_RAWS_MSG,                           \
+    LOG_GPS_RTCM_MSG,                           \
     LOG_GPS_UBX1_MSG,                           \
     LOG_GPS_UBX2_MSG,                           \
     LOG_IDS_FROM_GPS_SBP
@@ -203,6 +204,22 @@ struct PACKED log_GPS_RAWS {
     uint8_t trkStat;
 };
 
+// @LoggerMessage: GRTK
+// @Description: RTCM input status
+// @Field: TimeUS: Time since system startup
+// @Field: version: Message version (0x02 for this version)
+// @Field: flags: RTCM input status flags (see graphic below)
+// @Field: refStation: Reference station ID
+// @Field: msgType: Message type
+struct PACKED log_GPS_RTCM {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t version;
+    uint8_t flags;
+    uint16_t refStation;
+    uint16_t msgType;
+};
+
 #define LOG_STRUCTURE_FROM_GPS \
     { LOG_GPS_MSG, sizeof(log_GPS), \
       "GPS",  "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
@@ -218,4 +235,6 @@ struct PACKED log_GPS_RAWS {
       "GRXH", "QdHbBB", "TimeUS,rcvTime,week,leapS,numMeas,recStat", "s-----", "F-----" , true }, \
     { LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \
       "GRXS", "QddfBBBHBBBBB", "TimeUS,prMes,cpMes,doMes,gnss,sv,freq,lock,cno,prD,cpD,doD,trk", "s------------", "F------------" , true }, \
+    { LOG_GPS_RTCM_MSG, sizeof(log_GPS_RTCM), \
+      "GRTK", "QBBHH", "TimeUS,ver,flgs,ref,msgType", "s----", "F----" , true }, \
     LOG_STRUCTURE_FROM_GPS_SBP