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

DCM: limit the contribution of the baro to vertical acceleration 

limit to a max of 0.5g
This commit is contained in:
Andrew Tridgell 2012-09-20 15:24:30 +10:00
parent 458a42654b
commit ccf8ba3ce9
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -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