diff --git a/docs/Settings.md b/docs/Settings.md index b886f6702ac..4e577bbedd5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -434,11 +434,11 @@ Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allo ### apa_pow -Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation; +Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. Scales P/D/FF with airspeed (I-term scaled less aggressively). Gains range from 30% (high speed) to 150% (low speed). Set to 0 to disable and use throttle-based attenuation. Recommended: 120 for aircraft with validated pitot sensor. | Default | Min | Max | | --- | --- | --- | -| 120 | 0 | 200 | +| 0 | 0 | 200 | --- @@ -1478,7 +1478,7 @@ TPA smoothing and delay time constant to reflect non-instant speed/throttle resp | Default | Min | Max | | --- | --- | --- | -| 1500 | 0 | 5000 | +| 2000 | 0 | 5000 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01ca6149bfb..1e1932531e5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1380,10 +1380,10 @@ groups: min: 0 max: 100 - name: apa_pow - description: "Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation;" + description: "Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. Scales P/D/FF with airspeed (I-term scaled less aggressively). Gains range from 30% (high speed) to 150% (low speed). Set to 0 to disable and use throttle-based attenuation. Recommended: 120 for aircraft with validated pitot sensor." type: uint16_t field: throttle.apa_pow - default_value: 120 + default_value: 0 min: 0 max: 200 - name: tpa_rate @@ -1411,7 +1411,7 @@ groups: default_value: OFF - name: fw_tpa_time_constant description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details." - default_value: 1500 + default_value: 2000 field: throttle.fixedWingTauMs min: 0 max: 5000 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7a112ff8f4f..0a2faa648ca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -443,13 +443,28 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate) } static float calculateFixedWingAirspeedTPAFactor(void){ - const float airspeed = getAirspeedEstimate(); // in cm/s + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); // cm/s, clamped to 3.6-720 km/h const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s - float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); + float tpaFactor= powf(referenceAirspeed/airspeed, currentControlProfile->throttle.apa_pow/100.0f); tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); return tpaFactor; } +// Calculate I-term scaling factor (less aggressive than P/D/FF) +static float calculateFixedWingAirspeedITermFactor(void){ + const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); // cm/s, clamped to 3.6-720 km/h + const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s + const float apa_pow = currentControlProfile->throttle.apa_pow; + + if (apa_pow <= 100.0f) { + return 1.0f; + } + + float iTermFactor = powf(referenceAirspeed/airspeed, (apa_pow/100.0f) - 1.0f); + iTermFactor = constrainf(iTermFactor, 0.3f, 1.5f); + return iTermFactor; +} + static float calculateFixedWingTPAFactor(uint16_t throttle) { float tpaFactor; @@ -535,14 +550,18 @@ void updatePIDCoefficients(void) } float tpaFactor=1.0f; + float iTermFactor=1.0f; // Separate factor for I-term scaling if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){ tpaFactor = calculateFixedWingAirspeedTPAFactor(); + iTermFactor = calculateFixedWingAirspeedITermFactor(); // Less aggressive I-term scaling }else{ tpaFactor = calculateFixedWingTPAFactor(calculateTPAThtrottle()); + iTermFactor = tpaFactor; // Use same factor for throttle-based TPA } } else { tpaFactor = calculateMultirotorTPAFactor(calculateTPAThtrottle()); + iTermFactor = tpaFactor; // Multirotor uses same factor } if (tpaFactor != tpaFactorprev) { pidGainsUpdateRequired = true; @@ -560,9 +579,9 @@ void updatePIDCoefficients(void) //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { if (usedPidControllerType == PID_TYPE_PIFF) { - // Airplanes - scale all PIDs according to TPA + // Airplanes - scale PIDs according to TPA (I-term scaled less aggressively) pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; - pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; + pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * iTermFactor; // Less aggressive scaling pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; pidState[axis].kCD = 0.0f; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c7a40e982cc..6ad55632c17 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -6568,6 +6568,16 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) } } #endif + +#if defined(USE_PITOT) + // Pitot sensor validation failure (blocked/failed pitot tube) + if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) { + if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1, &warningsCount)) { + messages[messageCount++] = "PITOT FAIL"; + } + } +#endif + // Vibration levels TODO - needs better vibration measurement to be useful // const float vibrationLevel = accGetVibrationLevel(); // warningCondition = vibrationLevel > 1.5f; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index edcc5b9219d..b4b61f57669 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -48,6 +48,13 @@ #include "sensors/barometer.h" #include "sensors/sensors.h" +#include "io/gps.h" + +#ifdef USE_WIND_ESTIMATOR +#include "flight/wind_estimator.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#endif //#include "build/debug.h" @@ -58,6 +65,16 @@ extern baro_t baro; pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0}; +// Pitot sensor validation state +static bool pitotHardwareFailed = false; +static uint16_t pitotFailureCounter = 0; +static uint16_t pitotRecoveryCounter = 0; +#define PITOT_FAILURE_THRESHOLD 20 // 0.2 seconds at 100Hz - fast detection per LOG00002 analysis +#define PITOT_RECOVERY_THRESHOLD 200 // 2 seconds of consecutive good readings to recover + +// Forward declaration for GPS-based airspeed fallback +static float getVirtualAirspeedEstimate(void); + PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); #define PITOT_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise @@ -301,9 +318,18 @@ void pitotUpdate(void) /* * Airspeed estimate in cm/s + * Returns hardware pitot if valid, GPS-based virtual airspeed if pitot failed, + * or raw pitot value as last resort */ float getAirspeedEstimate(void) { + // If hardware pitot has failed validation, use GPS-based virtual airspeed + if (pitotHardwareFailed) { + float virtualAirspeed = getVirtualAirspeedEstimate(); + if (virtualAirspeed > 0.0f) { + return virtualAirspeed; + } + } return pitot.airSpeed; } @@ -312,13 +338,158 @@ bool pitotIsHealthy(void) return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; } +/** + * Calculate virtual airspeed estimate (same as virtual pitot) + * + * Uses GPS velocity with wind correction when available, providing a reference + * airspeed that already accounts for wind conditions. + * + * @return virtual airspeed in cm/s, or 0 if GPS unavailable + */ +static float getVirtualAirspeedEstimate(void) +{ +#if defined(USE_GPS) && defined(USE_WIND_ESTIMATOR) + if (!STATE(GPS_FIX)) { + return 0.0f; + } + + float airSpeed = 0.0f; + + // Use wind estimator if available (matches virtual pitot logic) + if (isEstimatedWindSpeedValid()) { + uint16_t windHeading; // centidegrees + float windSpeed = getEstimatedHorizontalWindSpeed(&windHeading); // cm/s + float horizontalWindSpeed = windSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(windHeading - posControl.actualState.yaw)); + airSpeed = posControl.actualState.velXY - horizontalWindSpeed; + airSpeed = calc_length_pythagorean_2D(airSpeed, getEstimatedActualVelocity(Z) + getEstimatedWindSpeed(Z)); + } else { + // Fall back to raw GPS velocity if no wind estimator + airSpeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); + } + + return airSpeed; +#elif defined(USE_GPS) + // No wind estimator, use raw GPS velocity + if (!STATE(GPS_FIX)) { + return 0.0f; + } + return calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); +#else + return 0.0f; +#endif +} + +/** + * Pitot sensor sanity check against virtual airspeed + * + * Compares hardware pitot reading against virtual airspeed (GPS + wind estimator) + * to detect gross sensor failures like blocked pitot tubes. + * + * Uses wide thresholds to catch implausible readings while avoiding false positives: + * - Compares against wind-corrected virtual airspeed (not raw GPS groundspeed) + * - Wide tolerance (30%-200%) catches gross failures only + * - Detects: blocked pitot reading 25 km/h when virtual shows 85 km/h + * - Avoids: false positives from sensor accuracy differences + * + * @return true if pitot reading appears plausible, false if likely failed + */ +static bool isPitotReadingPlausible(void) +{ +#ifdef USE_GPS + if (!STATE(GPS_FIX)) { + return true; + } + + const float virtualAirspeedCmS = getVirtualAirspeedEstimate(); + const float minValidationSpeed = 700.0f; // 7 m/s + + if (virtualAirspeedCmS < minValidationSpeed) { + return true; + } + + const float pitotAirspeedCmS = pitot.airSpeed; + + // Wide thresholds to catch gross failures (blocked pitot) only + const float minPlausibleAirspeed = virtualAirspeedCmS * 0.3f; // 30% of virtual + const float maxPlausibleAirspeed = virtualAirspeedCmS * 2.0f; // 200% of virtual + + if (pitotAirspeedCmS < minPlausibleAirspeed || pitotAirspeedCmS > maxPlausibleAirspeed) { + return false; + } + + return true; +#else + return true; +#endif +} + +/** + * Check if pitot sensor has failed validation + * + * @return true if pitot has failed sanity checks and should not be trusted + */ +bool pitotHasFailed(void) +{ + return pitotHardwareFailed; +} + bool pitotValidForAirspeed(void) { bool ret = false; ret = pitotIsHealthy() && pitotIsCalibrationComplete(); + + // For virtual pitot, we need GPS fix if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { ret = ret && STATE(GPS_FIX); } + + // For hardware pitot sensors, validate readings against GPS when armed + // This detects blocked or failed pitot tubes + if (ret && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL && + detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { + + if (ARMING_FLAG(ARMED)) { + // Check if pitot reading is plausible + if (!isPitotReadingPlausible()) { + pitotFailureCounter++; + } else if (pitotFailureCounter > 0) { + // Decay counter if sensor appears healthy + pitotFailureCounter--; + } + + // Declare failure after sustained implausible readings + if (pitotFailureCounter >= PITOT_FAILURE_THRESHOLD) { + pitotHardwareFailed = true; + pitotRecoveryCounter = 0; // Start recovery tracking + } + + // Recovery: require sustained consecutive good readings to clear failure + if (pitotHardwareFailed) { + if (isPitotReadingPlausible()) { + pitotRecoveryCounter++; + if (pitotRecoveryCounter >= PITOT_RECOVERY_THRESHOLD) { + pitotHardwareFailed = false; // Sensor has recovered + pitotFailureCounter = 0; + pitotRecoveryCounter = 0; + } + } else { + // Bad reading resets recovery progress + pitotRecoveryCounter = 0; + } + } + } else { + // Reset on disarm for next flight + pitotHardwareFailed = false; + pitotFailureCounter = 0; + pitotRecoveryCounter = 0; + } + + // If pitot has failed sanity checks, require GPS fix (like virtual pitot) + if (pitotHardwareFailed) { + ret = ret && STATE(GPS_FIX); + } + } + return ret; } #endif /* PITOT */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 68f5a9c1a02..69451098ec8 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -70,5 +70,6 @@ void pitotUpdate(void); float getAirspeedEstimate(void); bool pitotIsHealthy(void); bool pitotValidForAirspeed(void); +bool pitotHasFailed(void); #endif