From e7f79f235de96ac43f7379cac9797be4fd89c340 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 Feb 2023 09:17:43 +1100 Subject: [PATCH] AP_GPS: don't try and configure M10 options on non-M10 GPS this fixes a pre-arm failure "GPS 1 failing configuration checks" on non-M10 GPS modules, including AP_Periph it also adds the ublox unconfigured msgs to the DroneCAN GNSS.Status errors field for easier diagnosis of this type of issue in the future --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 14 ++++++++------ libraries/AP_GPS/AP_GPS_UBLOX.h | 5 +++++ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 45de386f2e..3060f1fd69 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -425,12 +425,14 @@ AP_GPS_UBLOX::_request_next_config(void) case STEP_M10: { - // special handling of M10 config - const config_list *list = config_M10; - const uint8_t list_length = ARRAY_SIZE(config_M10); - Debug("Sending M10 settings"); - if (!_configure_config_set(list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { - _next_message--; + if (_hardware_generation == UBLOX_M10) { + // special handling of M10 config + const config_list *list = config_M10; + const uint8_t list_length = ARRAY_SIZE(config_M10); + Debug("Sending M10 settings"); + if (!_configure_config_set(list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { + _next_message--; + } } break; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 6e8dc05901..e387ae4ed3 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -143,6 +143,11 @@ public: #endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL } + bool get_error_codes(uint32_t &error_codes) const override { + error_codes = _unconfigured_messages; + return true; + }; + void broadcast_configuration_failure_reason(void) const override; void Write_AP_Logger_Log_Startup_messages() const override;