Merge pull request #11197 from iNavFlight/master

Add airspeed TPA support (backport from #11042)  via merge from master

Author: sensei-hacker

docs/Settings.md

@@ -432,6 +432,16 @@ 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;
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 120 | 0 | 200 |
+
+---
+
 ### applied_defaults
 
 Internal (configurator) hint. Should not be changed manually
@@ -6442,13 +6452,23 @@ Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should b
 
 ---
 
+### tpa_pitch_compensation
+
+Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 8 | 0 | 20 |
+
+---
+
 ### tpa_rate
 
-Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.
+Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0 | 0 | 100 |
+| 0 | 0 | 200 |
 
 ---
 

src/main/fc/control_profile.c

@@ -45,7 +45,9 @@ void pgResetFn_controlProfiles(controlConfig_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,
+                .apa_pow = SETTING_APA_POW_DEFAULT,
+                .tpa_pitch_compensation = SETTING_TPA_PITCH_COMPENSATION_DEFAULT
             },
 
             .stabilized = {

src/main/fc/control_profile_config_struct.h

@@ -32,6 +32,8 @@ typedef struct controlConfig_s {
         bool dynPID_on_YAW;
         uint16_t pa_breakpoint;                // Breakpoint where TPA is activated
         uint16_t fixedWingTauMs;               // Time constant of airplane TPA PT1-filter
+        uint16_t apa_pow;                     // Use airspeed instead of throttle position for TPA calculation,0 to disable
+        uint8_t tpa_pitch_compensation;      // Pitch angle based throttle compensation for fixed wing
     } throttle;
 
     struct {

src/main/fc/settings.yaml

@@ -1379,18 +1379,31 @@ groups:
         field: throttle.rcExpo8
         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;"
+        type: uint16_t
+        field: throttle.apa_pow
+        default_value: 120
+        min: 0
+        max: 200
       - name: tpa_rate
-        description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate."
+        description: "Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details."
         default_value: 0
         field: throttle.dynPID
         min: 0
-        max: 100
+        max: 200
       - name: tpa_breakpoint
         description: "See tpa_rate."
         default_value: 1500
         field: throttle.pa_breakpoint
         min: PWM_RANGE_MIN
         max: PWM_RANGE_MAX
+      - name: tpa_pitch_compensation
+        description: "Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down."
+        default_value: 8
+        field: throttle.tpa_pitch_compensation
+        min: 0
+        max: 20
       - name: tpa_on_yaw
         description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors."
         type: bool

src/main/flight/pid.c

@@ -126,7 +126,7 @@ static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
 static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
 
 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
-STATIC_FASTRAM bool pidGainsUpdateRequired;
+STATIC_FASTRAM bool pidGainsUpdateRequired= true;
 FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
 
 #ifdef USE_BLACKBOX
@@ -442,6 +442,14 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate)
     return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS);
 }
 
+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);
+    return tpaFactor;
+}
+
 static float calculateFixedWingTPAFactor(uint16_t throttle)
 {
     float tpaFactor;
@@ -452,16 +460,15 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
         if (throttle > getThrottleIdleValue()) {
             // Calculate TPA according to throttle
             tpaFactor = 0.5f + ((float)(currentControlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
-
-            // Limit to [0.5; 2] range
-            tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
         }
         else {
             tpaFactor = 2.0f;
         }
 
         // Attenuate TPA curve according to configured amount
         tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlProfile->throttle.dynPID / 100.0f);
+        // Limit to [0.5; 2] range
+        tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f);
     }
     else {
         tpaFactor = 1.0f;
@@ -470,45 +477,48 @@ 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 (currentControlProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlProfile->throttle.pa_breakpoint) {
+    if (currentControlProfile->throttle.dynPID == 0 || throttle < currentControlProfile->throttle.pa_breakpoint) {
         tpaFactor = 1.0f;
-    } else if (rcCommand[THROTTLE] < getMaxThrottle()) {
-        tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f;
+    } else if (throttle < getMaxThrottle()) {
+        tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (throttle - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f;
     } else {
-        tpaFactor = (100 - currentControlProfile->throttle.dynPID) / 100.0f;
+        tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f;
     }
 
     return tpaFactor;
 }
 
+static float calculateTPAThtrottle(void)
+{
+    uint16_t tpaThrottle = 0;
+    static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } };
+
+    if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering
+        fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } };
+        float groundCos = vectorDotProduct(&vForward, &vDown);
+        int16_t throttleAdjustment =  currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //when 1deg pitch up, increase throttle by pitch(deg)_to_throttle. cos(89 deg)*90/(pi/2)=0.99995,cos(80 deg)*90/(pi/2)=9.9493,
+        uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000);
+        tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, constrain(throttleAdjusted, 1000, 2000));
+    }
+    else {
+        tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering
+    }
+    return tpaThrottle;
+}
+
 void schedulePidGainsUpdate(void)
 {
     pidGainsUpdateRequired = true;
 }
 
 void updatePIDCoefficients(void)
 {
-    STATIC_FASTRAM uint16_t prevThrottle = 0;
-
-    // Check if throttle changed. Different logic for fixed wing vs multirotor
-    if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) {
-        uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
-        if (filteredThrottle != prevThrottle) {
-            prevThrottle = filteredThrottle;
-            pidGainsUpdateRequired = true;
-        }
-    }
-    else {
-        if (rcCommand[THROTTLE] != prevThrottle) {
-            prevThrottle = rcCommand[THROTTLE];
-            pidGainsUpdateRequired = true;
-        }
-    }
+    STATIC_FASTRAM float tpaFactorprev=-1.0f;
 
 #ifdef USE_ANTIGRAVITY
     if (usedPidControllerType == PID_TYPE_PID) {
@@ -523,14 +533,29 @@ void updatePIDCoefficients(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
     }
+    
+    float tpaFactor=1.0f;
+    if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation
+        if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){
+            tpaFactor = calculateFixedWingAirspeedTPAFactor();
+        }else{
+            tpaFactor = calculateFixedWingTPAFactor(calculateTPAThtrottle());
+        }
+    } else {
+        tpaFactor = calculateMultirotorTPAFactor(calculateTPAThtrottle());
+    }
+    if (tpaFactor != tpaFactorprev) {
+        pidGainsUpdateRequired = true;
+    }
+    tpaFactorprev = tpaFactor;
 
+    
     // If nothing changed - don't waste time recalculating coefficients
     if (!pidGainsUpdateRequired) {
         return;
     }
 
-    const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
-
+    
     // 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

@@ -312,4 +312,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