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..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; @@ -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; + 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 < WINDESTIMATOR_VALIDITY_THRESHOLD - 15) 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; } @@ -123,12 +142,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 +154,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 +202,8 @@ void updateWindEstimator(timeUs_t currentTimeUs) lastUpdateUs = currentTimeUs; lastValidWindEstimate = currentTimeUs; - hasValidWindEstimate = true; lastValidEstimateAltitude = currentAltitude; + if (validityScore < WINDESTIMATOR_VALIDITY_THRESHOLD + 15) validityScore++; } }