From 9ff99432e9afbb819b3f6bea82c45436e84e7c8b Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 3 Jan 2026 00:14:13 -0600 Subject: [PATCH 1/6] Add GPS-based pitot sensor validation with automatic fallback Implements pitot sensor validation by comparing hardware pitot readings against virtual airspeed (GPS + wind estimator). Detects blocked or failed pitot tubes and automatically falls back to GPS-based airspeed. Features: - Compares pitot against virtual airspeed (wind-corrected, not raw GPS) - Wide thresholds (30%-200%) catch gross failures while avoiding false positives - Sustained failure detection (1 second) before declaring sensor failed - Automatic fallback to GPS airspeed when pitot fails validation - OSD warning displays "PITOT FAIL" when sensor invalid - Automatic recovery after 0.5 seconds of good readings - Conservative approach: only validates when GPS available and moving >7 m/s Safety improvements: - Detects blocked pitot tubes (forgotten cover, insects, ice) - Prevents dangerous high gains with invalid pitot data - Maintains aircraft controllability when pitot fails - Clear pilot awareness via OSD warning Addresses GitHub issue #11208 --- src/main/io/osd.c | 10 +++ src/main/sensors/pitotmeter.c | 152 ++++++++++++++++++++++++++++++++++ src/main/sensors/pitotmeter.h | 1 + 3 files changed, 163 insertions(+) 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..ce0f65df64a 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,12 @@ extern baro_t baro; pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0}; +// Pitot sensor validation state +static bool pitotHardwareFailed = false; +static uint16_t pitotFailureCounter = 0; +#define PITOT_FAILURE_THRESHOLD 100 // 1 second at 100Hz (sustained failure required) +#define PITOT_RECOVERY_THRESHOLD 50 // 0.5 seconds of good readings to recover + 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 @@ -312,13 +325,152 @@ 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; + } + + // Recovery: require sustained good readings to clear failure + if (pitotHardwareFailed && isPitotReadingPlausible()) { + if (pitotFailureCounter > 0) { + pitotFailureCounter--; + } + if (pitotFailureCounter <= (PITOT_FAILURE_THRESHOLD - PITOT_RECOVERY_THRESHOLD)) { + pitotHardwareFailed = false; // Sensor has recovered + pitotFailureCounter = 0; + } + } + } else { + // Reset on disarm for next flight + pitotHardwareFailed = false; + pitotFailureCounter = 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 From 80ec5cd435e341078c961fbe0bc93e59d5323316 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 3 Jan 2026 00:14:29 -0600 Subject: [PATCH 2/6] Improve APA safety: reduce I-term scaling and maximum gain MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Changes to Airspeed-based PID Attenuation (APA) for fixed-wing aircraft: 1. Reduced I-term scaling aggressiveness - I-term now scales with (apa_pow/100 - 1) instead of apa_pow/100 - Example: apa_pow=120 → I uses 0.20 exponent vs 1.20 for P/D/FF - Prevents integral windup and overshoot - Follows industry best practice (Betaflight, ArduPilot) - Maintains trim stability across speed range 2. Reduced maximum gain increase from 200% to 150% - Changed upper constraint from 2.0 to 1.5 - Prevents excessive gain multiplication at low speeds - More conservative approach reduces control sensitivity spikes - Still provides adequate authority for slow-speed flight 3. Changed default apa_pow from 120 to 0 (disabled) - APA now opt-in for safety - Users must explicitly enable after validating pitot sensor - Updated description to reflect new behavior - Safer default for new users Control theory rationale: - P/D/FF scaling compensates for dynamic pressure (½ρV²) - I-term serves different purpose (steady-state trim) - Aggressive I scaling causes windup and oscillation - Conservative I scaling improves control stability Combined with pitot validation (previous commit), these changes provide comprehensive safety improvements for APA feature. Addresses GitHub issue #11208 --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- src/main/flight/pid.c | 25 ++++++++++++++++++++++--- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index b886f6702ac..3e512137c14 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 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01ca6149bfb..1c38034dcf8 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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7a112ff8f4f..3857eaa6716 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -446,10 +446,25 @@ static float calculateFixedWingAirspeedTPAFactor(void){ const float airspeed = getAirspeedEstimate(); // in cm/s const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); - tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); + tpaFactor= constrainf(tpaFactor, 0.3f, 1.5f); // Reduced from 2.0 to 1.5 (max 50% gain increase) return tpaFactor; } +// Calculate I-term scaling factor (less aggressive than P/D/FF) +static float calculateFixedWingAirspeedITermFactor(void){ + const float airspeed = getAirspeedEstimate(); // in cm/s + 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+0.01f), (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; From b3ac0f511b44b9a212e99d2f4451e3eb1ff88f34 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 3 Jan 2026 00:34:50 -0600 Subject: [PATCH 3/6] Add defensive airspeed clamping to prevent division issues Clamp airspeed to 100-20000 cm/s (3.6-720 km/h) before using in power calculations to prevent: - Division by zero or near-zero values - NaN results from invalid airspeed readings - Overflow from extreme values The constrainf() output clamps are still in place as the final safeguard, but this prevents bad intermediate calculations. --- src/main/flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3857eaa6716..dc3c7e44bad 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -443,16 +443,16 @@ 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); - tpaFactor= constrainf(tpaFactor, 0.3f, 1.5f); // Reduced from 2.0 to 1.5 (max 50% gain increase) + float tpaFactor= powf(referenceAirspeed/airspeed, currentControlProfile->throttle.apa_pow/100.0f); + tpaFactor= constrainf(tpaFactor, 0.3f, 1.5f); return tpaFactor; } // Calculate I-term scaling factor (less aggressive than P/D/FF) static float calculateFixedWingAirspeedITermFactor(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 const float apa_pow = currentControlProfile->throttle.apa_pow; @@ -460,7 +460,7 @@ static float calculateFixedWingAirspeedITermFactor(void){ return 1.0f; } - float iTermFactor = powf(referenceAirspeed/(airspeed+0.01f), (apa_pow/100.0f) - 1.0f); + float iTermFactor = powf(referenceAirspeed/airspeed, (apa_pow/100.0f) - 1.0f); iTermFactor = constrainf(iTermFactor, 0.3f, 1.5f); return iTermFactor; } From 7897f7f57b3179cb9dc1cf978841b6c0b087282d Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 8 Jan 2026 18:47:37 -0600 Subject: [PATCH 4/6] Fix pitot validation to use virtual airspeed on failure When hardware pitot fails validation, getAirspeedEstimate() now returns GPS-based virtual airspeed instead of the corrupted pitot value. This ensures APA (Airspeed-based PID Attenuation) continues working correctly with valid airspeed data. Changes: - getAirspeedEstimate() falls back to virtual airspeed when pitotHardwareFailed - Faster failure detection: 0.2s (20 samples) vs 1s (100 samples) - Slower recovery: 2s of consecutive good readings required - Separate recovery counter prevents underflow with asymmetric thresholds Fixes issue where blocked pitot caused APA to use invalid airspeed, resulting in incorrect PID gain scaling. Co-Authored-By: Claude Opus 4.5 --- src/main/sensors/pitotmeter.c | 39 ++++++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index ce0f65df64a..b4b61f57669 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -68,8 +68,12 @@ pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0}; // Pitot sensor validation state static bool pitotHardwareFailed = false; static uint16_t pitotFailureCounter = 0; -#define PITOT_FAILURE_THRESHOLD 100 // 1 second at 100Hz (sustained failure required) -#define PITOT_RECOVERY_THRESHOLD 50 // 0.5 seconds of good readings to recover +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); @@ -314,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; } @@ -447,22 +460,28 @@ bool pitotValidForAirspeed(void) // Declare failure after sustained implausible readings if (pitotFailureCounter >= PITOT_FAILURE_THRESHOLD) { pitotHardwareFailed = true; + pitotRecoveryCounter = 0; // Start recovery tracking } - // Recovery: require sustained good readings to clear failure - if (pitotHardwareFailed && isPitotReadingPlausible()) { - if (pitotFailureCounter > 0) { - pitotFailureCounter--; - } - if (pitotFailureCounter <= (PITOT_FAILURE_THRESHOLD - PITOT_RECOVERY_THRESHOLD)) { - pitotHardwareFailed = false; // Sensor has recovered - pitotFailureCounter = 0; + // 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) From 933bbfc9ab7d6cfa72190d78683d0cfa0390ee0e Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 10 Jan 2026 14:08:10 -0600 Subject: [PATCH 5/6] Adjust TPA parameters for fixed-wing aircraft - Increase fw_tpa_time_constant default from 1500 to 2000ms - Raise airspeed TPA factor upper limit from 1.5 to 2.0 --- src/main/fc/settings.yaml | 2 +- src/main/flight/pid.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1c38034dcf8..1e1932531e5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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 dc3c7e44bad..0a2faa648ca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -446,7 +446,7 @@ static float calculateFixedWingAirspeedTPAFactor(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 float tpaFactor= powf(referenceAirspeed/airspeed, currentControlProfile->throttle.apa_pow/100.0f); - tpaFactor= constrainf(tpaFactor, 0.3f, 1.5f); + tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); return tpaFactor; } From 605d365d1edbfff569995ba2c5ce9b62faa1e14b Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sat, 10 Jan 2026 14:49:57 -0600 Subject: [PATCH 6/6] Fix Settings.md documentation to match settings.yaml Updated fw_tpa_time_constant default value from 1500 to 2000 in Settings.md to match the authoritative value in settings.yaml. The discrepancy was caused by the documentation being out of sync with the YAML source. Regenerated using: python3 src/utils/update_cli_docs.py --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3e512137c14..4e577bbedd5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -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 | ---