Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: cope with INS_MAX_INSTANCES below 3

Browse Source
This commit is contained in:
Andrew Tridgell 2020-11-07 17:43:32 +11:00
parent f6c45c3bcd
commit dab091a801
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -664,12 +664,15 @@ AP_AHRS_DCM::drift_correction(float deltat)
} }
//update _accel_ef_blended //update _accel_ef_blended
#if INS_MAX_INSTANCES > 1
if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) { if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
const float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f; const float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
// slew _imu1_weight over one second // slew _imu1_weight over one second
_imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat); _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
_accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight); _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
} else { } else
#endif
{
_accel_ef_blended = _accel_ef[_ins.get_primary_accel()]; _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
} }