From ccf8ba3ce9ec2c1c34f091438ec9bf31b7b46429 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Sep 2012 15:24:30 +1000 Subject: [PATCH] DCM: limit the contribution of the baro to vertical acceleration limit to a max of 0.5g --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5c884df841..20daebbf6b 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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