mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS_DCM: update _accel_ef_blended
This commit is contained in:
parent
033ee3c900
commit
4975cefd84
|
@ -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
|
// keep a sum of the deltat values, so we know how much time
|
||||||
// we have integrated over
|
// we have integrated over
|
||||||
_ra_deltat += deltat;
|
_ra_deltat += deltat;
|
||||||
|
|
|
@ -50,7 +50,8 @@ public:
|
||||||
_last_wind_time(0),
|
_last_wind_time(0),
|
||||||
_last_airspeed(0.0f),
|
_last_airspeed(0.0f),
|
||||||
_last_consistent_heading(0),
|
_last_consistent_heading(0),
|
||||||
_last_failure_ms(0)
|
_last_failure_ms(0),
|
||||||
|
_imu1_weight(0.5f)
|
||||||
{
|
{
|
||||||
_dcm_matrix.identity();
|
_dcm_matrix.identity();
|
||||||
|
|
||||||
|
@ -195,6 +196,10 @@ private:
|
||||||
// estimated wind in m/s
|
// estimated wind in m/s
|
||||||
Vector3f _wind;
|
Vector3f _wind;
|
||||||
|
|
||||||
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
|
||||||
|
float _imu1_weight;
|
||||||
|
#endif
|
||||||
|
|
||||||
// last time AHRS failed in milliseconds
|
// last time AHRS failed in milliseconds
|
||||||
uint32_t _last_failure_ms;
|
uint32_t _last_failure_ms;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue