From d172df31d9ed4dc8745aafd96d2129d784ab2656 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 Jan 2025 11:35:55 +0000 Subject: [PATCH] AP_AHRS: remove changes to primary accel since it is always the same as the gyro --- libraries/AP_AHRS/AP_AHRS.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 05421e2720782..81c09dc7d92a9 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -341,20 +341,15 @@ void AP_AHRS::reset_gyro_drift(void) void AP_AHRS::update_state(void) { const uint8_t primary_gyro = _get_primary_gyro_index(); - const uint8_t primary_accel = _get_primary_accel_index(); #if AP_INERTIALSENSOR_ENABLED // tell the IMUS about primary changes if (primary_gyro != state.primary_gyro) { - AP::ins().set_primary_gyro(primary_gyro); - } - - if (primary_accel != state.primary_accel) { - AP::ins().set_primary_accel(primary_accel); + AP::ins().set_primary(primary_gyro); } #endif state.primary_IMU = _get_primary_IMU_index(); state.primary_gyro = primary_gyro; - state.primary_accel = primary_accel; + state.primary_accel = _get_primary_accel_index(); state.primary_core = _get_primary_core_index(); state.wind_estimate_ok = _wind_estimate(state.wind_estimate); state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();