From e0916a2a85c8b49cceaf1138260ee89432c4c985 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Tue, 26 Nov 2024 22:15:03 -0500 Subject: [PATCH] AP_InertialSensor: adjust the instance if External AHRS is enabled --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8c6f05fd1e..267bc05094 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1142,8 +1142,19 @@ AP_InertialSensor::detect_backends(void) // backend count. Its important the IMUs are listed in order of precedence globally // (i.e. INSTANCE:0 IMUs are listed before INSTANCE:1 IMUs) and locally (i.e. IMUs // on the same bus are listed in order of detection precedence) +// if External AHRS is enabled, we need to adjust the instance number since External AHRS is made the first IMU -#define ADD_BACKEND_INSTANCE(x, instance) if (instance == _backend_count) { ADD_BACKEND(x); } +#define ADD_BACKEND_INSTANCE(x, instance) do { \ + uint8_t adjusted_instance = instance; \ + if (HAL_EXTERNAL_AHRS_ENABLED) { \ + if (AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::IMU) >= 0) { \ + adjusted_instance += 1; \ + } \ + } \ + if (adjusted_instance == _backend_count) { \ + ADD_BACKEND(x); \ + } \ +} while (0) // Can be used by adding AUX: keyword in hwdef. // AUX: keyword is used to check for the presence of the sensor