From 8fc494cfcde0ce4a3e7822e054e53fb3b2f5c602 Mon Sep 17 00:00:00 2001 From: masierra Date: Wed, 16 Sep 2015 10:30:13 -0700 Subject: [PATCH] AP_AHRS_DCM: changed estimate to use prev values for dir & vel --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 9ed22104fa..dbe2915ba8 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -907,12 +907,12 @@ void AP_AHRS_DCM::estimate_wind(void) // estimate airspeed it using equation 6 V = velocityDiff.length() / diff_length; - _last_fuse = fuselageDirection; - _last_vel = velocity; - Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; Vector3f velocitySum = velocity + _last_vel; + _last_fuse = fuselageDirection; + _last_vel = velocity; + float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x); float sintheta = sinf(theta); float costheta = cosf(theta);