From 2a010623b29677d51f97915138304ec9d56b4b37 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Mar 2024 22:39:21 +1100 Subject: [PATCH] AP_GPS: allow forwards/backwards for DroneCAN type autoconfig ArduPilot configures a connected DroneCAN GPS based on its GPS Type. Given parameter name changes, ArduPilot must be able to configure both new and old AP_Periphs, and new AP_Periphs have to cope with being configured by old ArduPilots. --- libraries/AP_GPS/AP_GPS.cpp | 2 +- libraries/AP_GPS/AP_GPS.h | 5 +++++ libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 5 ++++- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index f5864bbe9e..671199d5db 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -318,7 +318,7 @@ bool AP_GPS::needs_uart(GPS_Type type) const void AP_GPS::init() { // set the default for the first GPS according to define: - params[0].type.set_default(HAL_GPS_TYPE_DEFAULT); + params[0].type.set_default(HAL_GPS1_TYPE_DEFAULT); convert_parameters(); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 4449e02fb5..ae4dcdf4f4 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -268,6 +268,11 @@ public: /// Startup initialisation. void init(); + // ethod for APPPeriph to set the default type for the first GPS instance: + void set_default_type_for_gps1(uint8_t default_type) { + params[0].type.set_default(default_type); + } + /// Update GPS state based on possible bytes received from the module. /// This routine must be called periodically (typically at 10Hz or /// more) to process incoming data. diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 972cc83938..927090a36f 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -676,7 +676,10 @@ bool AP_GPS_DroneCAN::do_config() switch(cfg_step) { case STEP_SET_TYPE: + // GPS_TYPE was renamed GPS1_TYPE. Request both and + // handle whichever we receive. ap_dronecan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb); + ap_dronecan->get_parameter_on_node(node_id, "GPS1_TYPE", ¶m_int_cb); break; case STEP_SET_MB_CAN_TX: if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) { @@ -836,7 +839,7 @@ void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len) bool AP_GPS_DroneCAN::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, int32_t &value) { Debug("AP_GPS_DroneCAN: param set/get response from %d %s %ld\n", node_id, name, value); - if (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) { + if (((strcmp(name, "GPS_TYPE") == 0) || (strcmp(name, "GPS1_TYPE") == 0)) && (cfg_step == STEP_SET_TYPE)) { if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) { value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE; requires_save_and_reboot = true;