From e33554a1f91b4a65532d63b6814eafb4f1fe58e4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jul 2012 16:28:29 +1000 Subject: [PATCH] AHRS: only enable barometer for AHRS if it is smooth enough if we used less than 5 samples to compute the pressure then don't use it for climb rate --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index e9ef74b74e..3e96bed467 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -411,17 +411,15 @@ AP_AHRS_DCM::drift_correction(float deltat) _have_gps_lock = true; } -#if 1 /* - NOTE: The barometric vertical acceleration correction is disabled - until we work out how to filter it sufficiently to be usable - on ArduCopter + The barometer for vertical velocity is only enabled if we got + at least 5 pressure samples for the reading. This ensures we + don't use very noisy climb rate data */ - if (_barometer != NULL) { + if (_barometer != NULL && _barometer->get_pressure_samples() >= 5) { // 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