Merge pull request #8523 from shota3527/wind_estimator

Updates in the display of wind estimator
iNavFlight · Jan 19, 2023 · 37af691 · 37af691
2 parents b4d18ef + 62552eb
commit 37af691
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 24 deletions.
diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c
@@ -41,7 +41,8 @@
 #include "io/gps.h"
 
 
-#define WINDESTIMATOR_TIMEOUT       60 //60s
+#define WINDESTIMATOR_TIMEOUT       60*15 // 15min with out altitude change
+#define WINDESTIMATOR_ALTITUDE_SCALE WINDESTIMATOR_TIMEOUT/500.0f //or 500m altitude change
 // Based on WindEstimation.pdf paper
 
 static bool hasValidWindEstimate = false;
@@ -79,8 +80,10 @@ void updateWindEstimator(timeUs_t currentTimeUs)
 {
     static timeUs_t lastUpdateUs = 0;
     static timeUs_t lastValidWindEstimate = 0;
+    static float lastValidEstimateAltitude = 0.0f;
+    float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
 
-    if (US2S(currentTimeUs - lastValidWindEstimate) > WINDESTIMATOR_TIMEOUT)
+    if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT)
     {
         hasValidWindEstimate = false;
     }
@@ -165,6 +168,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         float prevWindLength = calc_length_pythagorean_3D(estimatedWind[X], estimatedWind[Y], estimatedWind[Z]);
         float windLength = calc_length_pythagorean_3D(wind[X], wind[Y], wind[Z]);
 
+        //is this really needed? The reason it is here might be above equation was wrong in early implementations
         if (windLength < prevWindLength + 4000) {
             // TODO: Better filtering
             estimatedWind[X] = estimatedWind[X] * 0.98f + wind[X] * 0.02f;
@@ -175,6 +179,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         lastUpdateUs = currentTimeUs;
         lastValidWindEstimate = currentTimeUs;
         hasValidWindEstimate = true;
+        lastValidEstimateAltitude = currentAltitude;
     }
 }
 

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -532,10 +532,10 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
             suffix = SYM_KMH;
             break;
     }
-    if (isValid) {
-        osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
-    } else {
-        buff[0] = buff[1] = buff[2] = '-';
+    osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
+    if (!isValid)
+    {
+        suffix = '*';
     }
     buff[3] = suffix;
     buff[4] = '\0';
@@ -2942,16 +2942,11 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             bool valid = isEstimatedWindSpeedValid();
             float horizontalWindSpeed;
-            if (valid) {
-                uint16_t angle;
-                horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
-                int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
-                buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
-            } else {
-                horizontalWindSpeed = 0;
-                buff[1] = SYM_BLANK;
-            }
+            uint16_t angle;
+            horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
+            int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
             buff[0] = SYM_WIND_HORIZONTAL;
+            buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
             osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
             break;
         }
@@ -2966,16 +2961,12 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[1] = SYM_BLANK;
             bool valid = isEstimatedWindSpeedValid();
             float verticalWindSpeed;
-            if (valid) {
-                verticalWindSpeed = -getEstimatedWindSpeed(Z);  //from NED to NEU
-                if (verticalWindSpeed < 0) {
-                    buff[1] = SYM_AH_DECORATION_DOWN;
-                    verticalWindSpeed = -verticalWindSpeed;
-                } else if (verticalWindSpeed > 0) {
-                    buff[1] = SYM_AH_DECORATION_UP;
-                }
+            verticalWindSpeed = -getEstimatedWindSpeed(Z);  //from NED to NEU
+            if (verticalWindSpeed < 0) {
+                buff[1] = SYM_AH_DECORATION_DOWN;
+                verticalWindSpeed = -verticalWindSpeed;
             } else {
-                verticalWindSpeed = 0;
+                buff[1] = SYM_AH_DECORATION_UP;
             }
             osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
             break;