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

DCM: limit the contribution of the baro to vertical acceleration

Browse Source 
limit to a max of 0.5g
This commit is contained in:
Andrew Tridgell 2012-09-20 15:24:30 +10:00
parent a84d7b6f4f
commit 4cfea44f9f
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 // equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s // are m/s/s
Vector3f GA_e; Vector3f GA_e;
float v_scale = 1.0/(_ra_deltat*_gravity); float v_scale = gps_gain.get()/(_ra_deltat*_gravity);
v_scale *= gps_gain; Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e = Vector3f(0, 0, -1.0) + ((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(); GA_e.normalize();
if (GA_e.is_inf()) { if (GA_e.is_inf()) {
// wait for some non-zero acceleration information // wait for some non-zero acceleration information