Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS_DCM: update _accel_ef_blended

This commit is contained in:
Jonathan Challinger 2014-10-31 16:19:11 -07:00 committed by Randy Mackay
parent 033ee3c900
commit 4975cefd84
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -526,6 +526,20 @@ AP_AHRS_DCM::drift_correction(float deltat)
}
}
//update _accel_ef_blended
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
if (_ins.get_accel_count() == 2 && _ins.get_accel_health(0) && _ins.get_accel_health(1)) {
float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
// slew _imu1_weight over one second
_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);
} else {
_accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
}
#else
_accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
#endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;

7
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -50,7 +50,8 @@ public:
_last_wind_time(0),
_last_airspeed(0.0f),
_last_consistent_heading(0),
_last_failure_ms(0)
_last_failure_ms(0),
_imu1_weight(0.5f)
{
_dcm_matrix.identity();
@ -195,6 +196,10 @@ private:
// estimated wind in m/s
Vector3f _wind;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
float _imu1_weight;
#endif
// last time AHRS failed in milliseconds
uint32_t _last_failure_ms;
};