Refactor calculation for horizontal wind speed to wind_estimator.c

Add float getEstimatedHorizontalWindSpeed(uint16_t *angle)
shellixyz · May 27, 2018 · 4f58602 · 4f58602
1 parent e0862c3
commit 4f58602
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 5 deletions.
diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c
@@ -59,6 +59,22 @@ float getEstimatedWindSpeed(int axis)
     return estimatedWind[axis];
 }
 
+float getEstimatedHorizontalWindSpeed(uint16_t *angle)
+{
+    float xWindSpeed = getEstimatedWindSpeed(X);
+    float yWindSpeed = getEstimatedWindSpeed(Y);
+    if (angle) {
+        float horizontalWindAngle = atan2_approx(yWindSpeed, xWindSpeed);
+        // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
+        // We want [0, 360) in degrees
+        if (horizontalWindAngle < 0) {
+            horizontalWindAngle += 2 * M_PIf;
+        }
+        *angle = RADIANS_TO_CENTIDEGREES(horizontalWindAngle);
+    }
+    return sqrtf(sq(xWindSpeed) + sq(yWindSpeed));
+}
+
 void updateWindEstimator(timeUs_t currentTimeUs)
 {
     static timeUs_t lastUpdateUs = 0;

diff --git a/src/main/flight/wind_estimator.h b/src/main/flight/wind_estimator.h
@@ -28,5 +28,8 @@
 bool isEstimatedWindSpeedValid(void);
 // wind velocity vectors in cm / sec relative to the earth frame
 float getEstimatedWindSpeed(int axis);
+// Returns the horizontal wind velocity as a magnitude in cm/s and,
+// optionally, its heading in EF in 0.01deg ([0, 360*100)).
+float getEstimatedHorizontalWindSpeed(uint16_t *angle);
 
 void updateWindEstimator(timeUs_t currentTimeUs);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -1788,11 +1788,9 @@ static bool osdDrawSingleElement(uint8_t item)
             bool valid = isEstimatedWindSpeedValid();
             float horizontalWindSpeed;
             if (valid) {
-                float xWindSpeed = getEstimatedWindSpeed(X);
-                float yWindSpeed = getEstimatedWindSpeed(Y);
-                horizontalWindSpeed = sqrtf(sq(xWindSpeed) + sq(yWindSpeed));
-                float horizontalWindAngle = atan2_approx(yWindSpeed, xWindSpeed);
-                int16_t windDirection = osdGetHeadingAngle(RADIANS_TO_DEGREES(horizontalWindAngle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+                uint16_t angle;
+                horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
+                int16_t windDirection = osdGetHeadingAngle((int)angle - DECIDEGREES_TO_DEGREES(attitude.values.yaw));
                 buff[1] = SYM_DIRECTION + (windDirection * 2 / 90);
             } else {
                 horizontalWindSpeed = 0;