Improved wind estimator validity check

src/main/fc/fc_tasks.c

@@ -167,7 +167,7 @@ void taskProcessGPS(timeUs_t currentTimeUs)
     // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
     // change this based on available hardware
     if (feature(FEATURE_GPS)) {
-        if (gpsUpdate()) {
+        if (gpsUpdate() && STATE(AIRPLANE)) {
 #ifdef USE_WIND_ESTIMATOR
             updateWindEstimator(currentTimeUs);
 #endif
@@ -462,8 +462,8 @@ void fcTasksInit(void)
 
 #ifdef USE_ADAPTIVE_FILTER
     setTaskEnabled(TASK_ADAPTIVE_FILTER, (
-        gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && 
-        gyroConfig()->adaptiveFilterMinHz > 0 && 
+        gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE &&
+        gyroConfig()->adaptiveFilterMinHz > 0 &&
         gyroConfig()->adaptiveFilterMaxHz > 0
     ));
 #endif

src/main/flight/wind_estimator.c

@@ -42,10 +42,12 @@
 
 #include "io/gps.h"
 
-
+// Based on WindEstimation.pdf paper
 #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
+
+#define WINDESTIMATOR_VALIDITY_THRESHOLD    15
+#define WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR   40
 
 static bool hasValidWindEstimate = false;
 static float estimatedWind[XYZ_AXIS_COUNT] = {0, 0, 0};    // wind velocity vectors in cm / sec in earth frame
@@ -54,7 +56,7 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
 
 bool isEstimatedWindSpeedValid(void)
 {
-    return hasValidWindEstimate 
+    return hasValidWindEstimate
 #ifdef USE_GPS_FIX_ESTIMATION
         || STATE(GPS_ESTIMATED_FIX)  //use any wind estimate with GPS fix estimation.
 #endif
@@ -85,22 +87,44 @@ float getEstimatedHorizontalWindSpeed(uint16_t *angle)
 void updateWindEstimator(timeUs_t currentTimeUs)
 {
     static timeUs_t lastUpdateUs = 0;
-    static timeUs_t lastValidWindEstimate = 0;
+    static timeUs_t lastValidWindEstimateUs = 0;
     static float lastValidEstimateAltitude = 0.0f;
     float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
+    static uint8_t validityScore = 0;
+    bool updateTimedout = false;
+    static uint8_t spikeFilterDynAdjustment = WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR;
+    static bool initialEstimate = true;
 
-    if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) {
+    if ((US2S(currentTimeUs - lastValidWindEstimateUs) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) {
         hasValidWindEstimate = false;
     }
 
-    if (!STATE(FIXED_WING_LEGACY) ||
-        !isGPSHeadingValid() ||
-        !gpsSol.flags.validVelNE ||
-        !gpsSol.flags.validVelD 
+    /* validityScore used to indicate validity of wind estimate in a more reactive way compared to the basic method used above.
+     * Each new estimate calc adds to score and each updateTimedout decrements from score.
+     * hasValidWindEstimate considered valid when score > WINDESTIMATOR_VALIDITY_THRESHOLD with max count limit of WINDESTIMATOR_VALIDITY_THRESHOLD + 15.
+     * WINDESTIMATOR_VALIDITY_THRESHOLD should result in a valid estimate based on the spike elimination and filtering used.
+     * hasValidWindEstimate considered invalid when score = 0 which approximates to around 2.5 to 5 minutes if no new estimate calcs occur */
+
+    if (US2S(cmpTimeUs(currentTimeUs, lastUpdateUs)) > 10 || lastUpdateUs == 0) {
+        if (validityScore > 0) validityScore--;
+        if (!validityScore) hasValidWindEstimate = false;
+
+        lastUpdateUs = currentTimeUs;
+        updateTimedout = true;
+
+        // Rapidly reset spikeFilterDynAdjustment if no new wind calcs
+        if (!initialEstimate && spikeFilterDynAdjustment >= 5) {
+            spikeFilterDynAdjustment -= 5;
+        }
+    }
+
+    if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) hasValidWindEstimate = true;
+
+    if (!isGPSHeadingValid() || !gpsSol.flags.validVelNE || !gpsSol.flags.validVelD
 #ifdef USE_GPS_FIX_ESTIMATION
-            || STATE(GPS_ESTIMATED_FIX)
+        || STATE(GPS_ESTIMATED_FIX)
 #endif
-            ) {
+        ) {
         return;
     }
 
@@ -112,8 +136,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     float fuselageDirectionDiff[XYZ_AXIS_COUNT];
     float fuselageDirectionSum[XYZ_AXIS_COUNT];
 
-    // Get current 3D velocity from GPS in cm/s
-    // relative to earth frame
+    // Get current 3D velocity from GPS in cm/s relative to earth frame
     groundVelocity[X] = posEstimator.gps.vel.x;
     groundVelocity[Y] = posEstimator.gps.vel.y;
     groundVelocity[Z] = posEstimator.gps.vel.z;
@@ -123,12 +146,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     fuselageDirection[Y] = -HeadVecEFFiltered.y;
     fuselageDirection[Z] = -HeadVecEFFiltered.z;
 
-    timeDelta_t timeDelta = cmpTimeUs(currentTimeUs, lastUpdateUs);
-    // scrap our data and start over if we're taking too long to get a direction change
-    if (lastUpdateUs == 0 || timeDelta > 10 * USECS_PER_SEC) {
-
-        lastUpdateUs = currentTimeUs;
-
+    // scrap our data and start over if we're taking too long (> 10s) to get a direction change
+    if (updateTimedout) {
         memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection));
         memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity));
         return;
@@ -139,13 +158,15 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     fuselageDirectionDiff[Z] = fuselageDirection[Z] - lastFuselageDirection[Z];
 
     float diffLengthSq = sq(fuselageDirectionDiff[X]) + sq(fuselageDirectionDiff[Y]) + sq(fuselageDirectionDiff[Z]);
-    
+
     // Very small changes in attitude will result in a denominator
     // very close to zero which will introduce too much error in the
     // estimation.
     //
     // TODO: Is 0.2f an adequate threshold?
     if (diffLengthSq > sq(0.2f)) {
+        lastUpdateUs = currentTimeUs;
+
         // when turning, use the attitude response to estimate wind speed
         groundVelocityDiff[X] = groundVelocity[X] - lastGroundVelocity[X];
         groundVelocityDiff[Y] = groundVelocity[Y] - lastGroundVelocity[Y];
@@ -165,29 +186,51 @@ void updateWindEstimator(timeUs_t currentTimeUs)
         memcpy(lastFuselageDirection, fuselageDirection, sizeof(lastFuselageDirection));
         memcpy(lastGroundVelocity, groundVelocity, sizeof(lastGroundVelocity));
 
-        float theta = atan2f(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2f(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]);// equation 9
+        float theta = atan2f(groundVelocityDiff[Y], groundVelocityDiff[X]) - atan2f(fuselageDirectionDiff[Y], fuselageDirectionDiff[X]);    // equation 9
         float sintheta = sinf(theta);
         float costheta = cosf(theta);
 
         float wind[XYZ_AXIS_COUNT];
-        wind[X] = (groundVelocitySum[X] - V * (costheta * fuselageDirectionSum[X] - sintheta * fuselageDirectionSum[Y])) * 0.5f;// equation 10
-        wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;// equation 11
-        wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;// equation 12
-
-        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;
-            estimatedWind[Y] = estimatedWind[Y] * 0.98f + wind[Y] * 0.02f;
-            estimatedWind[Z] = estimatedWind[Z] * 0.98f + wind[Z] * 0.02f;
+        wind[X] = (groundVelocitySum[X] - V * (costheta * fuselageDirectionSum[X] - sintheta * fuselageDirectionSum[Y])) * 0.5f;    // equation 10
+        wind[Y] = (groundVelocitySum[Y] - V * (sintheta * fuselageDirectionSum[X] + costheta * fuselageDirectionSum[Y])) * 0.5f;    // equation 11
+        wind[Z] = (groundVelocitySum[Z] - V * fuselageDirectionSum[Z]) * 0.5f;  // equation 12
+
+        /* Spike filter used to filter out large spikes that can occur in the raw wind calcs.
+         * Filter is based on a threshold between new wind updates and current estimated wind.
+         * A baseline threshold of 3 m/s is used with an additional dynamic threshold to clear a stuck estimate.
+         * The dynamic threshold relaxes spike filtering until the estimate recovers then falls back to the baseline threshold.
+         * The dynamic threshold is active on initialisation and also if new updates haven't made it past the spike filter > 30s.
+         * New wind values are discarded if a single axis exceeds the spike threshhold */
+
+        if (initialEstimate) {
+            if (validityScore == WINDESTIMATOR_VALIDITY_THRESHOLD + 15) {
+                initialEstimate = false;
+                spikeFilterDynAdjustment = 0;
+            }
+        } else if (spikeFilterDynAdjustment || US2S(cmpTimeUs(currentTimeUs, lastValidWindEstimateUs)) > 30) {  // 30s estimate update timeout
+            if (spikeFilterDynAdjustment < WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR) spikeFilterDynAdjustment++;
         }
 
-        lastUpdateUs = currentTimeUs;
-        lastValidWindEstimate = currentTimeUs;
-        hasValidWindEstimate = true;
+        // Spike filter
+        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            if (ABS(wind[axis] - estimatedWind[axis]) > (300 + spikeFilterDynAdjustment * WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR)) {
+                return;
+            }
+        }
+
+        // Spike free filter
+        float filterAlpha = 0.1f;
+        estimatedWind[X] = estimatedWind[X] + filterAlpha * (wind[X] - estimatedWind[X]);
+        estimatedWind[Y] = estimatedWind[Y] + filterAlpha * (wind[Y] - estimatedWind[Y]);
+        estimatedWind[Z] = estimatedWind[Z] + filterAlpha * (wind[Z] - estimatedWind[Z]);
+
+        if (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD + 15) validityScore++;
+
+        if (spikeFilterDynAdjustment) {
+            spikeFilterDynAdjustment -= (spikeFilterDynAdjustment == 1 || initialEstimate) ? 1 : 2;
+        }
+
+        lastValidWindEstimateUs = currentTimeUs;
         lastValidEstimateAltitude = currentAltitude;
     }
 }

src/main/sensors/pitotmeter.c

@@ -462,7 +462,7 @@ bool pitotValidateAirspeed(void)
 
     // For virtual pitot, we need GPS fix
     if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) {
-        ret = ret && STATE(GPS_FIX);
+        ret = ret && STATE(GPS_FIX) && isEstimatedWindSpeedValid();
     }
 
     // For hardware pitot sensors, validate readings against GPS when armed