From 00bf548f87c21b8163cde21b40b04e73ac8e5d47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Sep 2012 20:50:07 +1000 Subject: [PATCH] AHRS: limit wind speed estimate changes sudden yaw changes can cause bad wind estimates --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 20daebbf6b..83f26d456f 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -624,7 +624,9 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) wind.z = velocitySum.z - V * fuselageDirectionSum.z; wind *= 0.5; - _wind = _wind * 0.95 + wind * 0.05; + if (wind.length() < _wind.length() + 20) { + _wind = _wind * 0.95 + wind * 0.05; + } _last_wind_time = now; } else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) {