From 37fc6c46b9f66c3eb05f03cf83d247642543026b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Jun 2012 11:12:49 +1000 Subject: [PATCH] AHRS: disable barometer for vertical acceleration this seems to have been the cause of the 'flips' seen by Marco and others. Testing by Craig and Alan shows that the flips are gone when the barometric acceleration is removed. It looks like a 5 point average filter is not enough to keep the vertical acceleteration noise low. With high noise in the z axes, the x and y axes are scaled back when the ge vector is normalised. --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index f04f11ea19..7683546fac 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -411,11 +411,17 @@ AP_AHRS_DCM::drift_correction(float deltat) last_correction_time = _gps->last_fix_time; } - // even without GPS lock we can correct for the vertical acceleration +#if 0 + /* + NOTE: The barometric vertical acceleration correction is disabled + until we work out how to filter it sufficiently to be usable + on ArduCopter + */ if (_barometer != NULL) { // Z velocity is down velocity.z = - _barometer->get_climb_rate(); } +#endif // see if this is our first time through - in which case we // just setup the start times and return