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..569011fa962 100644 --- a/src/main/flight/wind_estimator.c +++ b/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,47 @@ 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--; + + lastUpdateUs = currentTimeUs; + updateTimedout = true; + + // Rapidly reset spikeFilterDynAdjustment if no new wind calcs + if (!initialEstimate && spikeFilterDynAdjustment >= 5) { + spikeFilterDynAdjustment -= 5; + } + } + + 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 - || STATE(GPS_ESTIMATED_FIX) + || STATE(GPS_ESTIMATED_FIX) #endif - ) { + ) { return; } @@ -112,8 +139,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 +149,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 +161,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 +189,54 @@ 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++; + if (hasValidWindEstimate && validityScore > 1) validityScore -= 2; // degrade valid estimate if update stuck too long + } } - 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; } } 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