From 8a80434778aa55025a07fe6af0b9078640e1662f Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Thu, 14 Jul 2022 03:48:34 +0300
Subject: [PATCH] fixed wind estimator

---
 src/main/flight/wind_estimator.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c
index e0f0de658..7fe49a459 100644
--- a/src/main/flight/wind_estimator.c
+++ b/src/main/flight/wind_estimator.c
@@ -102,8 +102,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
 
     // Fuselage direction in earth frame
     fuselageDirection[X] = rMat[0][0];
-    fuselageDirection[Y] = rMat[1][0];
-    fuselageDirection[Z] = rMat[2][0];
+    fuselageDirection[Y] = -rMat[1][0];
+    fuselageDirection[Z] = -rMat[2][0];
 
     timeDelta_t timeDelta = cmpTimeUs(currentTimeUs, lastUpdateUs);
     // scrap our data and start over if we're taking too long to get a direction change
@@ -131,7 +131,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         // when turning, use the attitude response to estimate wind speed
         groundVelocityDiff[X] = groundVelocity[X] - lastGroundVelocity[X];
         groundVelocityDiff[Y] = groundVelocity[Y] - lastGroundVelocity[Y];
-        groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
+        groundVelocityDiff[Z] = groundVelocity[Z] - lastGroundVelocity[Z];
 
         // estimate airspeed it using equation 6
         float V = (calc_length_pythagorean_3D(groundVelocityDiff[X], groundVelocityDiff[Y], groundVelocityDiff[Z])) / fast_fsqrtf(diffLengthSq);
-- 
2.11.4.GIT