Add airspeed TPA support

docs/Settings.md

@@ -302,6 +302,16 @@ ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't hav
 
 ---
 
+### airspeed_tpa
+
+Use airspeed instead of throttle position for TPA if airspeed is available. use throttle as {tpa_breakpoint + (airspeed - fw_reference_airspeed)/fw_reference_airspeed * (tpa_breakpoint - ThrottleIdleValue(default:1150))} for TPA calculation
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### align_board_pitch
 
 Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc

src/main/fc/controlrate_profile.c

@@ -46,7 +46,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
                 .dynPID = SETTING_TPA_RATE_DEFAULT,
                 .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT,
                 .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT,
-                .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT
+                .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT,
+                .airspeed_tpa = SETTING_AIRSPEED_TPA_DEFAULT,
             },
 
             .stabilized = {

src/main/fc/controlrate_profile_config_struct.h

@@ -32,6 +32,7 @@ typedef struct controlRateConfig_s {
         bool dynPID_on_YAW;
         uint16_t pa_breakpoint;                // Breakpoint where TPA is activated
         uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
+        bool airspeed_tpa;                     // Use airspeed instead of throttle position for TPA calculation
     } throttle;
 
     struct {

src/main/fc/settings.yaml

@@ -1343,6 +1343,11 @@ groups:
         field: throttle.dynPID
         min: 0
         max: 100
+      - name: airspeed_tpa
+        description: "Use airspeed instead of throttle position for TPA if airspeed is available. use throttle as {tpa_breakpoint + (airspeed - fw_reference_airspeed)/fw_reference_airspeed * (tpa_breakpoint - ThrottleIdleValue(default:1150))} for TPA calculation"
+        type: bool
+        field: throttle.airspeed_tpa
+        default_value: OFF
       - name: tpa_breakpoint
         description: "See tpa_rate."
         default_value: 1500

src/main/flight/pid.c

@@ -469,15 +469,15 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
     return tpaFactor;
 }
 
-static float calculateMultirotorTPAFactor(void)
+static float calculateMultirotorTPAFactor(uint16_t throttle)
 {
     float tpaFactor;
 
     // TPA should be updated only when TPA is actually set
-    if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) {
+    if (currentControlRateProfile->throttle.dynPID == 0 || throttle < currentControlRateProfile->throttle.pa_breakpoint) {
         tpaFactor = 1.0f;
-    } else if (rcCommand[THROTTLE] < getMaxThrottle()) {
-        tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
+    } else if (throttle < getMaxThrottle()) {
+        tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (throttle - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
     } else {
         tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
     }
@@ -493,20 +493,24 @@ void schedulePidGainsUpdate(void)
 void updatePIDCoefficients(void)
 {
     STATIC_FASTRAM uint16_t prevThrottle = 0;
+    STATIC_FASTRAM uint16_t tpaThrottle = 0;
 
-    // Check if throttle changed. Different logic for fixed wing vs multirotor
-    if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
-        uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
-        if (filteredThrottle != prevThrottle) {
-            prevThrottle = filteredThrottle;
-            pidGainsUpdateRequired = true;
-        }
+    if (usedPidControllerType == PID_TYPE_PIFF && pitotValidForAirspeed() && currentControlRateProfile->throttle.airspeed_tpa) {
+        // Use airspeed instead of throttle for TPA calculation
+        const float airspeed = getAirspeedEstimate(); // in cm/s
+        const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s
+        tpaThrottle = currentControlRateProfile->throttle.pa_breakpoint + (uint16_t)((airspeed - referenceAirspeed) / referenceAirspeed * (currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()));
+        tpaThrottle = constrain(tpaThrottle, getThrottleIdleValue(), tpaThrottle);
+    }
+    else if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
+        tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
     }
     else {
-        if (rcCommand[THROTTLE] != prevThrottle) {
-            prevThrottle = rcCommand[THROTTLE];
-            pidGainsUpdateRequired = true;
-        }
+        tpaThrottle = rcCommand[THROTTLE];
+    }
+    if (tpaThrottle != prevThrottle) {
+        prevThrottle = tpaThrottle;
+        pidGainsUpdateRequired = true;
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -528,8 +532,7 @@ void updatePIDCoefficients(void)
         return;
     }
 
-    const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
-
+    const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(prevThrottle);
     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
     for (int axis = 0; axis < 3; axis++) {

src/main/sensors/pitotmeter.c

@@ -305,4 +305,13 @@ bool pitotIsHealthy(void)
     return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS;
 }
 
+bool pitotValidForAirspeed(void)
+{
+    bool ret = false;
+    ret = pitotIsHealthy() && pitotIsCalibrationComplete();
+    if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) {
+        ret = ret && STATE(GPS_FIX);
+    }
+    return ret;
+}
 #endif /* PITOT */

src/main/sensors/pitotmeter.h

@@ -69,5 +69,6 @@ void pitotStartCalibration(void);
 void pitotUpdate(void);
 float getAirspeedEstimate(void);
 bool pitotIsHealthy(void);
+bool pitotValidForAirspeed(void);
 
 #endif