From 07a4b1158d10ffe7ece28407c0a347b7ceacef19 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 2 Jun 2026 11:43:32 +0100 Subject: [PATCH 1/8] improved wind estimator validity check --- src/main/fc/fc_tasks.c | 6 ++--- src/main/flight/wind_estimator.c | 44 +++++++++++++++++++++----------- 2 files changed, 32 insertions(+), 18 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 17dfecbf71a..b2bd16966b5 100755 --- a/src/main/fc/fc_tasks.c +++ b/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 diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 7567d0ad257..c0392973ebc 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -54,7 +54,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 +88,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; + 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 > 100 with max count limit of 115. + * 100 seems to be the number of estimate calcs required to get a reasonable estimate based on current filtering. + * hasValidWindEstimate considered invalid when score < 85. + * 85 approximates to around 2.5 to 5 minutes for hasValidWindEstimate to become invalid if no new estimate calcs occur */ + + if (cmpTimeUs(currentTimeUs, lastUpdateUs) > 10 * USECS_PER_SEC || lastUpdateUs == 0) { + lastUpdateUs = currentTimeUs; + updateTimedout = true; + + if (validityScore > 0) validityScore--; + if (validityScore < 85) hasValidWindEstimate = false; + } + + if (!hasValidWindEstimate && validityScore > 100) { + 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; } @@ -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. @@ -187,8 +201,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) lastUpdateUs = currentTimeUs; lastValidWindEstimate = currentTimeUs; - hasValidWindEstimate = true; lastValidEstimateAltitude = currentAltitude; + if (validityScore < 115) validityScore++; } } From 9eaef9542a5cae0afb2bec00079f89f96b6961d5 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 2 Jun 2026 23:00:31 +0100 Subject: [PATCH 2/8] Update wind_estimator.c --- src/main/flight/wind_estimator.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index c0392973ebc..50a3419d84c 100644 --- a/src/main/flight/wind_estimator.c +++ b/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 100 // Based on WindEstimation.pdf paper static bool hasValidWindEstimate = false; @@ -107,10 +108,10 @@ void updateWindEstimator(timeUs_t currentTimeUs) updateTimedout = true; if (validityScore > 0) validityScore--; - if (validityScore < 85) hasValidWindEstimate = false; + if (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD - 15) hasValidWindEstimate = false; } - if (!hasValidWindEstimate && validityScore > 100) { + if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { hasValidWindEstimate = true; } @@ -202,7 +203,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) lastUpdateUs = currentTimeUs; lastValidWindEstimate = currentTimeUs; lastValidEstimateAltitude = currentAltitude; - if (validityScore < 115) validityScore++; + if (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD + 15) validityScore++; } } From 6491789674401c3d89d9ff8d6b5cd323c212b4a0 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 7 Jun 2026 23:14:01 +0100 Subject: [PATCH 3/8] reduce threshold + add virtual pitot validity --- src/main/flight/wind_estimator.c | 2 +- src/main/sensors/pitotmeter.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 50a3419d84c..a19b82fb0af 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -45,7 +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 100 +#define WINDESTIMATOR_VALIDITY_THRESHOLD 50 // Based on WindEstimation.pdf paper static bool hasValidWindEstimate = false; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index a1510577a00..6c45136042e 100755 --- a/src/main/sensors/pitotmeter.c +++ b/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 From 24fdafa3a7ca6a97fd16227376b3e061737aae7d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 9 Jun 2026 14:29:26 +0100 Subject: [PATCH 4/8] improve wind est spike filtering --- src/main/flight/wind_estimator.c | 55 +++++++++++++++++--------------- 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index a19b82fb0af..fd5b671e5eb 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -45,7 +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 50 +#define WINDESTIMATOR_VALIDITY_THRESHOLD 15 // Based on WindEstimation.pdf paper static bool hasValidWindEstimate = false; @@ -90,6 +90,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) 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) { @@ -98,17 +99,16 @@ void updateWindEstimator(timeUs_t currentTimeUs) /* 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 > 100 with max count limit of 115. - * 100 seems to be the number of estimate calcs required to get a reasonable estimate based on current filtering. - * hasValidWindEstimate considered invalid when score < 85. - * 85 approximates to around 2.5 to 5 minutes for hasValidWindEstimate to become invalid if no new estimate calcs occur */ + * 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 < WINDESTIMATOR_VALIDITY_THRESHOLD - 15) hasValidWindEstimate = false; + if (!validityScore) hasValidWindEstimate = false; } if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { @@ -131,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; @@ -180,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; - lastValidEstimateAltitude = currentAltitude; - if (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD + 15) validityScore++; + 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; + } } } From 87b8d60fb5d32375ceb8b2adc12baf8da3509dbb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 13 Jun 2026 10:36:26 +0100 Subject: [PATCH 5/8] Change spike filtering method --- src/main/flight/wind_estimator.c | 79 +++++++++++++++++++++----------- 1 file changed, 51 insertions(+), 28 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index fd5b671e5eb..21634e43124 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -42,11 +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 + #define WINDESTIMATOR_VALIDITY_THRESHOLD 15 -// Based on WindEstimation.pdf paper +#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 @@ -86,14 +87,15 @@ 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; - static bool forcedUpdate = false; 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; } @@ -103,18 +105,21 @@ void updateWindEstimator(timeUs_t currentTimeUs) * 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 (US2S(cmpTimeUs(currentTimeUs, lastUpdateUs)) > 10 || lastUpdateUs == 0) { if (validityScore > 0) validityScore--; if (!validityScore) hasValidWindEstimate = false; - } - if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { - hasValidWindEstimate = true; + 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) @@ -160,6 +165,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) // // 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]; @@ -188,27 +195,43 @@ void updateWindEstimator(timeUs_t currentTimeUs) 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 prevEstWindLength = calc_length_pythagorean_3D(estimatedWind[X], estimatedWind[Y], estimatedWind[Z]); - float windLength = calc_length_pythagorean_3D(wind[X], wind[Y], wind[Z]); + /* 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++; + } - /* 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; + // 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; + } + } - 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]); + // 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 (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD + 15) validityScore++; - lastUpdateUs = currentTimeUs; - lastValidWindEstimate = currentTimeUs; - lastValidEstimateAltitude = currentAltitude; - forcedUpdate = false; + if (spikeFilterDynAdjustment) { + spikeFilterDynAdjustment -= (spikeFilterDynAdjustment == 1 || initialEstimate) ? 1 : 2; } + + lastValidWindEstimateUs = currentTimeUs; + lastValidEstimateAltitude = currentAltitude; } } From 9a70e4247d9ce9c27094efe87fc066df14df74bc Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 18 Jun 2026 11:19:24 +0100 Subject: [PATCH 6/8] Update wind_estimator.c --- src/main/flight/wind_estimator.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 21634e43124..569011fa962 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -107,7 +107,6 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (US2S(cmpTimeUs(currentTimeUs, lastUpdateUs)) > 10 || lastUpdateUs == 0) { if (validityScore > 0) validityScore--; - if (!validityScore) hasValidWindEstimate = false; lastUpdateUs = currentTimeUs; updateTimedout = true; @@ -118,7 +117,11 @@ void updateWindEstimator(timeUs_t currentTimeUs) } } - if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) hasValidWindEstimate = true; + if (!validityScore) { + hasValidWindEstimate = false; + } else if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { + hasValidWindEstimate = true; + } if (!isGPSHeadingValid() || !gpsSol.flags.validVelNE || !gpsSol.flags.validVelD #ifdef USE_GPS_FIX_ESTIMATION @@ -208,7 +211,10 @@ void updateWindEstimator(timeUs_t currentTimeUs) spikeFilterDynAdjustment = 0; } } else if (spikeFilterDynAdjustment || US2S(cmpTimeUs(currentTimeUs, lastValidWindEstimateUs)) > 30) { // 30s estimate update timeout - if (spikeFilterDynAdjustment < WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR) spikeFilterDynAdjustment++; + if (spikeFilterDynAdjustment < WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR) { + spikeFilterDynAdjustment++; + if (hasValidWindEstimate && validityScore > 1) validityScore -= 2; // degrade valid estimate if update stuck too long + } } // Spike filter From 44de5fdb5edd66093045595d663b8702846cec09 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 19 Jun 2026 20:39:12 +0100 Subject: [PATCH 7/8] fixes --- src/main/flight/wind_estimator.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 569011fa962..f68d1dd4d06 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -119,7 +119,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) if (!validityScore) { hasValidWindEstimate = false; - } else if (!hasValidWindEstimate && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { + } else if (!hasValidWindEstimate && !spikeFilterDynAdjustment && validityScore > WINDESTIMATOR_VALIDITY_THRESHOLD) { hasValidWindEstimate = true; } @@ -213,7 +213,7 @@ void updateWindEstimator(timeUs_t currentTimeUs) } else if (spikeFilterDynAdjustment || US2S(cmpTimeUs(currentTimeUs, lastValidWindEstimateUs)) > 30) { // 30s estimate update timeout if (spikeFilterDynAdjustment < WINDESTIMATOR_SPIKE_FILTER_ADJ_FACTOR) { spikeFilterDynAdjustment++; - if (hasValidWindEstimate && validityScore > 1) validityScore -= 2; // degrade valid estimate if update stuck too long + if (hasValidWindEstimate && validityScore) validityScore -= validityScore == 1 ? 1 : 2; // degrade valid estimate if update stuck too long } } From 1e2a8a2610f4c4f48c3f13e572c97ba1ddcbdb1f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 22 Jun 2026 14:21:52 +0100 Subject: [PATCH 8/8] Update fc_tasks.c --- src/main/fc/fc_tasks.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b2bd16966b5..14adc09771c 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -167,9 +167,9 @@ 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() && STATE(AIRPLANE)) { + if (gpsUpdate()) { #ifdef USE_WIND_ESTIMATOR - updateWindEstimator(currentTimeUs); + if (STATE(AIRPLANE)) updateWindEstimator(currentTimeUs); #endif } }