From 78b0ddcf457376e9ba103583231c038fa5522cdf Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 21 Nov 2023 18:10:49 +1100 Subject: [PATCH] AP_GPS: add support for setting L5 override --- libraries/AP_GPS/AP_GPS.cpp | 2 +- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 40 +++++++++++++++++++++++++++++-- libraries/AP_GPS/AP_GPS_UBLOX.h | 11 +++++++-- 4 files changed, 49 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 3257eb8230..27226db02b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -347,7 +347,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 + // @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 // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index eedc1a0c5a..a5ca9cdcea 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -624,6 +624,7 @@ protected: UBX_Use115200 = (1U << 2U), UAVCAN_MBUseDedicatedBus = (1 << 3U), HeightEllipsoid = (1U << 4), + GPSL5HealthOverride = (1U << 5) }; // 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 323a0f0f1c..720bf23c0c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -241,6 +241,18 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_M10[] { }; +/* + config changes for L5 modules +*/ +const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_ena[] { + {ConfigKey::CFG_SIGNAL_GPS_L5_ENA, 1}, + {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 1}, +}; + +const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] { + {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0}, +}; + void AP_GPS_UBLOX::_request_next_config(void) { @@ -436,7 +448,24 @@ AP_GPS_UBLOX::_request_next_config(void) } break; } - + + case STEP_L5: { + if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) { + const config_list *list = config_L5_ovrd_ena; + const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena); + if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_ALL)) { + _next_message--; + } + } else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) { + const config_list *list = config_L5_ovrd_dis; + const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis); + if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_ALL)) { + _next_message--; + } + } + break; + } + default: // this case should never be reached, do a full reset if it is hit _next_message = STEP_PVT; @@ -1314,6 +1343,12 @@ AP_GPS_UBLOX::_parse_gps(void) } _hardware_generation = UBLOX_F9; } + // check if L1L5 in extension + if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) { + supports_l5 = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox supports L5 Band"); + _unconfigured_messages |= CONFIG_L5; + } if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) { // a M9 _hardware_generation = UBLOX_M9; @@ -1995,7 +2030,8 @@ static const char *reasons[] = {"navigation rate", "Time mode settings", "RTK MB", "TIM TM2", - "M10"}; + "M10", + "L5 Enable Disable"}; 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 68afe623b4..cbf21f925d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -100,7 +100,8 @@ #define CONFIG_RTK_MOVBASE (1<<17) #define CONFIG_TIM_TM2 (1<<18) #define CONFIG_M10 (1<<19) -#define CONFIG_LAST (1<<20) // this must always be the last bit +#define CONFIG_L5 (1<<20) +#define CONFIG_LAST (1<<21) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) @@ -317,6 +318,8 @@ private: CFG_SIGNAL_NAVIC_ENA = 0x10310026, CFG_SIGNAL_NAVIC_L5_ENA = 0x1031001d, + CFG_SIGNAL_L5_HEALTH_OVRD = 0x10320001, + // other keys CFG_NAVSPG_DYNMODEL = 0x20110021, @@ -513,7 +516,7 @@ private: struct PACKED ubx_mon_ver { char swVersion[30]; char hwVersion[10]; - char extension; // extensions are not enabled + char extension[50]; // extensions are not enabled }; struct PACKED ubx_nav_svinfo_header { uint32_t itow; @@ -738,6 +741,7 @@ private: STEP_RTK_MOVBASE, // setup moving baseline STEP_TIM_TM2, STEP_M10, + STEP_L5, STEP_LAST }; @@ -872,7 +876,10 @@ private: RTCM3_Parser *rtcm3_parser; #endif // GPS_MOVING_BASELINE + bool supports_l5; static const config_list config_M10[]; + static const config_list config_L5_ovrd_ena[]; + static const config_list config_L5_ovrd_dis[]; }; #endif