mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
parent
e83b6bcfcb
commit
f8a4dd02d9
|
@ -2871,7 +2871,6 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const
|
||||||
int8_t imu = -1;
|
int8_t imu = -1;
|
||||||
switch (active_EKF_type()) {
|
switch (active_EKF_type()) {
|
||||||
case EKFType::NONE:
|
case EKFType::NONE:
|
||||||
imu = AP::ins().get_primary_gyro();
|
|
||||||
break;
|
break;
|
||||||
#if HAL_NAVEKF2_AVAILABLE
|
#if HAL_NAVEKF2_AVAILABLE
|
||||||
case EKFType::TWO:
|
case EKFType::TWO:
|
||||||
|
@ -2895,7 +2894,7 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
if (imu == -1) {
|
if (imu == -1) {
|
||||||
imu = AP::ins().get_primary_accel();
|
imu = AP::ins().get_primary_gyro();
|
||||||
}
|
}
|
||||||
return imu;
|
return imu;
|
||||||
}
|
}
|
||||||
|
@ -2933,19 +2932,13 @@ int8_t AP_AHRS::_get_primary_core_index() const
|
||||||
// get the index of the current primary accelerometer sensor
|
// get the index of the current primary accelerometer sensor
|
||||||
uint8_t AP_AHRS::_get_primary_accel_index(void) 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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// get the index of the current primary gyro sensor
|
// get the index of the current primary gyro sensor
|
||||||
uint8_t AP_AHRS::_get_primary_gyro_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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// see if EKF lane switching is possible to avoid EKF failsafe
|
// see if EKF lane switching is possible to avoid EKF failsafe
|
||||||
|
|
Loading…
Reference in New Issue