diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 987c980cf6..bf08a9b19e 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2934,7 +2934,7 @@ int8_t AP_AHRS::_get_primary_core_index() const uint8_t AP_AHRS::_get_primary_accel_index(void) const { if (ekf_type() != EKFType::NONE) { - return get_primary_IMU_index(); + return _get_primary_IMU_index(); } return AP::ins().get_primary_accel(); } @@ -2943,7 +2943,7 @@ uint8_t AP_AHRS::_get_primary_accel_index(void) const uint8_t AP_AHRS::_get_primary_gyro_index(void) const { if (ekf_type() != EKFType::NONE) { - return get_primary_IMU_index(); + return _get_primary_IMU_index(); } return AP::ins().get_primary_gyro(); } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9c1f3033a6..3cdc4074c8 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -727,9 +727,6 @@ private: EKFType ekf_type(void) const; void update_DCM(); - // get the index of the current primary IMU - uint8_t get_primary_IMU_index(void) const { return state.primary_IMU; } - /* * home-related state */