[Fixed-Wing] PIDFF Attenuation using the AirSpeed

docs/Settings.md

@@ -5522,6 +5522,36 @@ Can be used in ANGLE and HORIZON mode and will automatically boost throttle when
 
 ---
 
+### tpa_airspeed_attenuation
+
+Calculate PIDFF Attenuation based on AirSpeed (0 is disabled) ~ 1500 is a recommended value. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 3000 |
+
+---
+
+### tpa_airspeed_max
+
+Maximum AirSpeed demanded to calculate the PIDFF Attenuation. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2200 | 500 | 10000 |
+
+---
+
+### tpa_airspeed_min
+
+Should be set to 20% higher than level flight stall speed to calculate the PIDFF Attenuation based on AirSpeed. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 900 | 500 | 10000 |
+
+---
+
 ### tpa_breakpoint
 
 See tpa_rate.

src/main/fc/settings.yaml

@@ -2197,6 +2197,27 @@ groups:
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
+      - name: tpa_airspeed_min
+        description: "Should be set to 20% higher than level flight stall speed to calculate the PIDFF Attenuation based on AirSpeed. [cm/s]"
+        default_value: 900
+        field: TPA_Speed_Min
+        condition: USE_PITOT
+        min: 500
+        max: 10000
+      - name: tpa_airspeed_max
+        description: "Maximum AirSpeed demanded to calculate the PIDFF Attenuation. [cm/s]"
+        default_value: 2200
+        field: TPA_Speed_Max
+        condition: USE_PITOT
+        min: 500
+        max: 10000
+      - name: tpa_airspeed_attenuation
+        description: "Calculate PIDFF Attenuation based on AirSpeed (0 is disabled) ~ 1500 is a recommended value. [cm/s]"
+        default_value: 0
+        field: TPA_Scaling_Speed
+        condition: USE_PITOT
+        min: 0
+        max: 3000
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -311,6 +311,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
         .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
 #endif
+
+#ifdef USE_PITOT
+        .TPA_Scaling_Speed = SETTING_TPA_AIRSPEED_ATTENUATION_DEFAULT,
+        .TPA_Speed_Min = SETTING_TPA_AIRSPEED_MIN_DEFAULT,
+        .TPA_Speed_Max = SETTING_TPA_AIRSPEED_MAX_DEFAULT,
+#endif
 );
 
 FUNCTION_COMPILE_FOR_SIZE
@@ -438,8 +444,42 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate)
     return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS);
 }
 
+#ifdef USE_PITOT
+
+static float Get_PID_AirSpeed_Scaler(const float ScalingSpeed)
+{
+    float AirSpeedValue = CENTIMETERS_TO_METERS(pitotCalculateAirSpeed()); //in m/s
+    float AirSpeed_Scaler = 0.0f;
+    if (AirSpeedValue > 0.0001f)
+    {
+      AirSpeed_Scaler = ScalingSpeed / AirSpeedValue;
+    }
+    else
+    {
+      AirSpeed_Scaler = 2.0f;
+    }
+    float AirSpeed_TPA_Scale_Min = MIN(0.5f, (0.5f * CENTIMETERS_TO_METERS(pidProfile()->TPA_Speed_Min)) / ScalingSpeed);
+    float AirSpeed_TPA_Scale_Max = MAX(2.0f, (1.5f * CENTIMETERS_TO_METERS(pidProfile()->TPA_Speed_Max)) / ScalingSpeed);
+    AirSpeed_Scaler = constrainf(AirSpeed_Scaler, AirSpeed_TPA_Scale_Min, AirSpeed_TPA_Scale_Max);
+    return AirSpeed_Scaler;
+}
+
+#endif
+
 static float calculateFixedWingTPAFactor(uint16_t throttle)
 {
+
+#ifdef USE_PITOT
+
+  const float ParseScalingSpeed = CENTIMETERS_TO_METERS(pidProfile()->TPA_Scaling_Speed);
+
+  if (ParseScalingSpeed > 0 && pitotIsHealthy())
+  {
+    return Get_PID_AirSpeed_Scaler(ParseScalingSpeed);
+  }
+
+#endif
+
     float tpaFactor;
 
     // tpa_rate is amount of curve TPA applied to PIDs

src/main/flight/pid.h

@@ -164,6 +164,12 @@ typedef struct pidProfile_s {
     float smithPredictorDelay;
     uint16_t smithPredictorFilterHz;
 #endif
+
+#ifdef USE_PITOT
+    float TPA_Scaling_Speed;
+    uint16_t TPA_Speed_Min;
+    uint16_t TPA_Speed_Max;
+#endif
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {