From f8a4dd02d977a277b210f84179f85bfc44b1ce93 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 18 Sep 2023 17:28:56 +1000 Subject: [PATCH] AP_AHRS: simplify specification of primary IMU indices given that DCM doesn't specify to use the primary accel or gyro when fetching the data from the Ins library, it shouldn't be special-cased here when asked what the primary IMU and accel are. Note that this was asking for the *configured* backend type, rather than the active EKF type, making these clauses even stranger. This also changes the definition of the "primary IMU index" to be whichever gyro is active rather than the accel. Since we don't currently split primary gyro/primary accel, this is a reasonable change. --- libraries/AP_AHRS/AP_AHRS.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index bf08a9b19e..23b42b7838 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2871,7 +2871,6 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const int8_t imu = -1; switch (active_EKF_type()) { case EKFType::NONE: - imu = AP::ins().get_primary_gyro(); break; #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -2895,7 +2894,7 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const #endif } if (imu == -1) { - imu = AP::ins().get_primary_accel(); + imu = AP::ins().get_primary_gyro(); } return imu; } @@ -2933,19 +2932,13 @@ int8_t AP_AHRS::_get_primary_core_index() const // get the index of the current primary accelerometer sensor uint8_t AP_AHRS::_get_primary_accel_index(void) const { - if (ekf_type() != EKFType::NONE) { - return _get_primary_IMU_index(); - } - return AP::ins().get_primary_accel(); + return _get_primary_IMU_index(); } // get the index of the current primary gyro sensor uint8_t AP_AHRS::_get_primary_gyro_index(void) const { - if (ekf_type() != EKFType::NONE) { - return _get_primary_IMU_index(); - } - return AP::ins().get_primary_gyro(); + return _get_primary_IMU_index(); } // see if EKF lane switching is possible to avoid EKF failsafe