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..fd5b671e5eb 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 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; + } } } 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