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

@@ -45,6 +45,7 @@
 
 #define WINDESTIMATOR_TIMEOUT       60*15 // 15min with out altitude change
 #define WINDESTIMATOR_ALTITUDE_SCALE WINDESTIMATOR_TIMEOUT/500.0f //or 500m altitude change
+#define WINDESTIMATOR_VALIDITY_THRESHOLD    15
 // Based on WindEstimation.pdf paper
 
 static bool hasValidWindEstimate = false;
@@ -54,7 +55,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
@@ -88,19 +89,37 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     static timeUs_t lastValidWindEstimate = 0;
     static float lastValidEstimateAltitude = 0.0f;
     float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m
+    static uint8_t validityScore = 0;
+    static bool forcedUpdate = false;
+    bool updateTimedout = false;
 
     if ((US2S(currentTimeUs - lastValidWindEstimate) + 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 (cmpTimeUs(currentTimeUs, lastUpdateUs) > 10 * USECS_PER_SEC || lastUpdateUs == 0) {
+        lastUpdateUs = currentTimeUs;
+        updateTimedout = true;
+        forcedUpdate = true;
+        if (validityScore > 0) validityScore--;
+        if (!validityScore) hasValidWindEstimate = false;
+    }
+
+    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 +131,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 +141,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,7 +153,7 @@ 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.
@@ -165,30 +179,36 @@ 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
+        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 prevEstWindLength = 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;
-        }
+        /* Initial "rough and rapid" estimate calculated with validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD.
+         * Estimate then refined with a max threshold of 3 m/s between windLength and estimated wind which is necessary
+         * to prevent large transient spikes in windLength upsetting the estimate.
+         * forcedUpdate used to ensure estimate doesn't get stuck should a large misatch between windLength and estimate occur */
+        if ((ABS(windLength - prevEstWindLength) < 300.0f) || validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD || forcedUpdate) {
+            float filterAlpha = 0.1f;
 
-        lastUpdateUs = currentTimeUs;
-        lastValidWindEstimate = currentTimeUs;
-        hasValidWindEstimate = true;
-        lastValidEstimateAltitude = currentAltitude;
+            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++;
+
+            lastUpdateUs = currentTimeUs;
+            lastValidWindEstimate = currentTimeUs;
+            lastValidEstimateAltitude = currentAltitude;
+            forcedUpdate = false;
+        }
     }
 }
 

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