diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 7a38f7e9aa42f..58b6cc2a5ef68 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 }; 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 83b76bbffcad7..baa4830370201 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--; @@ -611,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 @@ -1530,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; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index c7b246803d2ea..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)