mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
DCM: limit the contribution of the baro to vertical acceleration
limit to a max of 0.5g
This commit is contained in:
parent
a84d7b6f4f
commit
4cfea44f9f
@ -467,9 +467,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
|
||||
// equation 9: get the corrected acceleration vector in earth frame. Units
|
||||
// are m/s/s
|
||||
Vector3f GA_e;
|
||||
float v_scale = 1.0/(_ra_deltat*_gravity);
|
||||
v_scale *= gps_gain;
|
||||
GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
|
||||
float v_scale = gps_gain.get()/(_ra_deltat*_gravity);
|
||||
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
|
||||
// limit vertical acceleration correction to 0.5 gravities. The
|
||||
// barometer sometimes gives crazy acceleration changes.
|
||||
vdelta.z = constrain(vdelta.z, -0.5, 0.5);
|
||||
GA_e = Vector3f(0, 0, -1.0) + vdelta;
|
||||
GA_e.normalize();
|
||||
if (GA_e.is_inf()) {
|
||||
// wait for some non-zero acceleration information
|
||||
|
Loading…
Reference in New Issue
Block a user