From 0019658d73db052b08d14eaf1fe8dd7268796335 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 15 Apr 2024 16:06:04 +0100 Subject: [PATCH] AP_GPS: allow logging of UBX-RXM-RTCM --- libraries/AP_GPS/AP_GPS.cpp | 8 +++++ libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 54 +++++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS_UBLOX.h | 22 +++++++++++-- libraries/AP_GPS/LogStructure.h | 19 +++++++++++ 5 files changed, 99 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 91b7c0dcd1c651..bed14a745dc348 100644 --- 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 index 70879831078eb0..81ca2186a6d6e6 100644 --- 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; 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 index 179c5d34d482e7..00e7ab19b952bd 100644 --- 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 index 617a072e79d3d7..3e3619784c234c 100644 --- 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 index 5ed1d2794693d3..6a12ff4b2362d6 100644 --- 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