From 906315ea268b98e20005e2aff1b60bfaa0fd3750 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 17 Apr 2024 18:30:24 +0100 Subject: [PATCH] AP_GPS: add RTKT and RTKR messages add logging of MSG_STATUS configure via GPS_DRV_OPTIONS --- libraries/AP_GPS/AP_GPS.cpp | 34 ++++++++++++++++++++++--------- libraries/AP_GPS/AP_GPS.h | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 29 ++++++++++++++++++++++---- libraries/AP_GPS/AP_GPS_UBLOX.h | 5 +++-- libraries/AP_GPS/LogStructure.h | 6 +++--- 5 files changed, 56 insertions(+), 20 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 4dfb50aaae0d2..ff2731d8f72a2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -239,7 +239,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health, 6:Enable RTCM full parse even for a single channel, 7:Disable automatic full RTCM parsing when RTCM seen on more than one channel, 8:Log RTCM data // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), @@ -279,14 +279,6 @@ 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 }; @@ -1646,7 +1638,14 @@ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm const uint32_t crc = crc_crc32(0, buf, len); #if HAL_LOGGING_ENABLED - AP::logger().WriteStreaming("RTCM", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI", +// @LoggerMessage: RTKR +// @Description: RTCM parser data +// @Field: TimeUS: microseconds since system startup +// @Field: Chan: RTCM channel data +// @Field: RTCMId: RTCM parser ID +// @Field: Len: RTCM injection packet length +// @Field: CRC: RTCM injection packet CRC + AP::logger().WriteStreaming("RTKR", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI", AP_HAL::micros64(), uint8_t(chan), rtcm.parsers[chan]->get_id(), @@ -1670,6 +1669,21 @@ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm rtcm.sent_idx = (rtcm.sent_idx+1) % ARRAY_SIZE(rtcm.sent_crc); if (buf != nullptr && len > 0) { +#if HAL_LOGGING_ENABLED +// @LoggerMessage: RTKT +// @Description: RTCM parser data +// @Field: TimeUS: microseconds since system startup +// @Field: Chan: RTCM channel data +// @Field: RTCMId: RTCM parser ID +// @Field: Len: RTCM injection packet length +// @Field: CRC: RTCM injection packet CRC + AP::logger().WriteStreaming("RTKT", "TimeUS,Chan,RTCMId,Len,CRC", "s#---", "F----", "QBHHI", + AP_HAL::micros64(), + uint8_t(chan), + rtcm.parsers[chan]->get_id(), + len, + crc); +#endif inject_data(buf, len); } rtcm.parsers[chan]->reset(); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a72ae4091a905..1b70ffee652f1 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -614,7 +614,6 @@ 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; @@ -632,6 +631,7 @@ class AP_GPS GPSL5HealthOverride = (1U << 5), AlwaysRTCMDecode = (1U << 6), DisableRTCMDecode = (1U << 7), + LogRTCMData = (1U << 8), }; // check if an option is set diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b07b58e350e31..0cbde9b44012f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -409,7 +409,7 @@ AP_GPS_UBLOX::_request_next_config(void) break; case STEP_RTCM: #if UBLOX_RXM_RTCM_LOGGING - if(gps._rtcm_data == 0) { + if(!option_set(AP_GPS::DriverOptions::LogRTCMData)) { _unconfigured_messages &= ~CONFIG_RATE_RTCM; } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RTCM)) { _next_message--; @@ -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: @@ -607,7 +611,7 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { #endif // UBLOX_RXM_RAW_LOGGING #if UBLOX_RXM_RTCM_LOGGING case MSG_RXM_RTCM: - desired_rate = gps._rtcm_data; + desired_rate = option_set(AP_GPS::DriverOptions::LogRTCMData) ? 1 : 0; config_msg_id = CONFIG_RATE_RTCM; break; #endif // UBLOX_RXM_RTCM_LOGGING @@ -1002,7 +1006,7 @@ void AP_GPS_UBLOX::log_rxm_rtcm(const struct ubx_rxm_rtcm &rtcm) uint64_t now = AP_HAL::micros64(); - struct log_GPS_RTCM header = { + const struct log_GPS_RTCM header = { LOG_PACKET_HEADER_INIT(LOG_GPS_RTCM_MSG), time_us : now, version : rtcm.version, @@ -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) @@ -1512,7 +1530,7 @@ 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) { + if (_class == CLASS_RXM && _msg_id == MSG_RXM_RTCM && option_set(AP_GPS::DriverOptions::LogRTCMData)) { log_rxm_rtcm(_buffer.rxm_rtcm); return false; } @@ -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 75a4975f5ff0d..9768ede90f9d2 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -105,8 +105,8 @@ #define CONFIG_F9 (1<<19) #define CONFIG_M10 (1<<20) #define CONFIG_L5 (1<<21) -#define CONFIG_RATE_RTCM (1<<21) -#define CONFIG_LAST (1<<22) // this must always be the last bit +#define CONFIG_RATE_RTCM (1<<22) +#define CONFIG_LAST (1<<23) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) @@ -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 diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h index 8016d405e831c..07654a298a361 100644 --- a/libraries/AP_GPS/LogStructure.h +++ b/libraries/AP_GPS/LogStructure.h @@ -207,9 +207,9 @@ struct PACKED log_GPS_RAWS { // @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: ver: Message version (0x02 for this version) +// @Field: flgs: RTCM input status flags (see graphic below) +// @Field: ref: Reference station ID // @Field: msgType: Message type struct PACKED log_GPS_RTCM { LOG_PACKET_HEADER;