TPA air speed speed estimation instead of TPA delay (for wings) (#13895)

* tpa speed speed estimation (for wings)

* Some renaming based on Karatebrot review

* Fix with currentPidProfile

* Ledvinap's review

* minor fixes

* Karatebrot and ledvinap review

* Review by haslinghuis

src/main/blackbox/blackbox.c

@@ -1409,6 +1409,7 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d",                     motorConfig()->maxthrottle);
         BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x",                     castFloatBytesToInt(1.0f));
         BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d",                  motorOutputLowInt, motorOutputHighInt);
+        BLACKBOX_PRINT_HEADER_LINE("motor_kv", "%d",                        motorConfig()->kv);
 #if defined(USE_ACC)
         BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u",                          acc.dev.acc_1G);
 #endif
@@ -1737,9 +1738,15 @@ static bool blackboxWriteSysinfo(void)
 #endif
 
 #ifdef USE_WING
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS,       "%d", currentPidProfile->tpa_delay_ms);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_TYPE, "%d", currentPidProfile->tpa_speed_type);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR0,   "%d", currentPidProfile->tpa_gravity_thr0);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_DELAY, "%d", currentPidProfile->tpa_speed_basic_delay);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_GRAVITY_THR100, "%d", currentPidProfile->tpa_gravity_thr100);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_GRAVITY, "%d", currentPidProfile->tpa_speed_basic_gravity);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH, "%d", currentPidProfile->tpa_speed_adv_prop_pitch);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_MASS, "%d", currentPidProfile->tpa_speed_adv_mass);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_DRAG_K, "%d", currentPidProfile->tpa_speed_adv_drag_k);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_THRUST, "%d", currentPidProfile->tpa_speed_adv_thrust);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, "%d", currentPidProfile->tpa_speed_max_voltage);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_PITCH_OFFSET, "%d", currentPidProfile->tpa_speed_pitch_offset);
 #endif
 
         default:

src/main/cli/settings.c

@@ -539,6 +539,12 @@ const char* const lookupTableTpaCurveType[] = {
 };
 #endif
 
+#ifdef USE_WING
+const char* const lookupTableTpaSpeedType[] = {
+    "BASIC", "ADVANCED",
+};
+#endif
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -665,6 +671,9 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_ADVANCED_TPA
     LOOKUP_TABLE_ENTRY(lookupTableTpaCurveType),
 #endif
+#ifdef USE_WING
+    LOOKUP_TABLE_ENTRY(lookupTableTpaSpeedType),
+#endif
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -1278,9 +1287,15 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_TPA_LOW_ALWAYS, VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_always) },
 
 #ifdef USE_WING
-    { PARAM_NAME_TPA_DELAY_MS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_delay_ms) },
+    { PARAM_NAME_TPA_SPEED_TYPE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_SPEED_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_type) },
-    { PARAM_NAME_TPA_GRAVITY_THR0, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr0) },
+    { PARAM_NAME_TPA_SPEED_BASIC_DELAY, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_basic_delay) },
-    { PARAM_NAME_TPA_GRAVITY_THR100, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_GRAVITY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_gravity_thr100) },
+    { PARAM_NAME_TPA_SPEED_BASIC_GRAVITY, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_basic_gravity) },
+    { PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_prop_pitch) },
+    { PARAM_NAME_TPA_SPEED_ADV_MASS, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_mass) },
+    { PARAM_NAME_TPA_SPEED_ADV_DRAG_K, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_drag_k) },
+    { PARAM_NAME_TPA_SPEED_ADV_THRUST, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 1, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_adv_thrust) },
+    { PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_max_voltage) },
+    { PARAM_NAME_TPA_SPEED_PITCH_OFFSET, VAR_INT16 | PROFILE_VALUE, .config.minmaxUnsigned = { INT16_MIN, INT16_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_speed_pitch_offset) },
 #endif // USE_WING
 
 #ifdef USE_ADVANCED_TPA

src/main/cli/settings.h

@@ -145,6 +145,9 @@ typedef enum {
 #endif
 #ifdef USE_ADVANCED_TPA
     TABLE_TPA_CURVE_TYPE,
+#endif
+#ifdef USE_WING
+    TABLE_TPA_SPEED_TYPE,
 #endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/main/fc/parameter_names.h

@@ -58,14 +58,20 @@
 #define PARAM_NAME_TPA_LOW_BREAKPOINT "tpa_low_breakpoint"
 #define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always"
 #define PARAM_NAME_TPA_MODE "tpa_mode"
-#define PARAM_NAME_TPA_DELAY_MS "tpa_delay_ms"
-#define PARAM_NAME_TPA_GRAVITY_THR0 "tpa_gravity_thr0"
-#define PARAM_NAME_TPA_GRAVITY_THR100 "tpa_gravity_thr100"
 #define PARAM_NAME_TPA_CURVE_TYPE "tpa_curve_type"
 #define PARAM_NAME_TPA_CURVE_STALL_THROTTLE "tpa_curve_stall_throttle"
 #define PARAM_NAME_TPA_CURVE_PID_THR0 "tpa_curve_pid_thr0"
 #define PARAM_NAME_TPA_CURVE_PID_THR100 "tpa_curve_pid_thr100"
 #define PARAM_NAME_TPA_CURVE_EXPO "tpa_curve_expo"
+#define PARAM_NAME_TPA_SPEED_TYPE "tpa_speed_type"
+#define PARAM_NAME_TPA_SPEED_BASIC_DELAY "tpa_speed_basic_delay"
+#define PARAM_NAME_TPA_SPEED_BASIC_GRAVITY "tpa_speed_basic_gravity"
+#define PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH "tpa_speed_adv_prop_pitch"
+#define PARAM_NAME_TPA_SPEED_ADV_MASS "tpa_speed_adv_mass"
+#define PARAM_NAME_TPA_SPEED_ADV_DRAG_K "tpa_speed_adv_drag_k"
+#define PARAM_NAME_TPA_SPEED_ADV_THRUST "tpa_speed_adv_thrust"
+#define PARAM_NAME_TPA_SPEED_MAX_VOLTAGE "tpa_speed_max_voltage"
+#define PARAM_NAME_TPA_SPEED_PITCH_OFFSET "tpa_speed_pitch_offset"
 #define PARAM_NAME_MIXER_TYPE "mixer_type"
 #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold"
 #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit"

src/main/flight/mixer.c

@@ -438,6 +438,15 @@ static void applyRpmLimiter(mixerRuntime_t *mixer)
 }
 #endif // USE_RPM_LIMIT
 
+#ifdef USE_WING
+static float motorOutputRms = 0.0f;
+
+float getMotorOutputRms(void)
+{
+    return motorOutputRms;
+}
+#endif // USE_WING
+
 static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
 {
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
@@ -473,6 +482,20 @@ static void applyMixToMotors(const float motorMix[MAX_SUPPORTED_MOTORS], motorMi
             motor[i] = motor_disarmed[i];
         }
     }
+
+#ifdef USE_WING
+    float motorSumSquares = 0.0f;
+    if (motorIsEnabled()) {
+        for (int i = 0; i < mixerRuntime.motorCount; i++) {
+            if (motorIsMotorEnabled(i)) {
+                const float motorOutput = scaleRangef(motorConvertToExternal(motor[i]), PWM_RANGE_MIN, PWM_RANGE_MAX, 0.0f, 1.0f);
+                motorSumSquares += motorOutput * motorOutput;
+            }
+        }
+    }
+    motorOutputRms = sqrtf(motorSumSquares / mixerRuntime.motorCount);
+#endif // USE_WING
+
     DEBUG_SET(DEBUG_EZLANDING, 1, throttle * 10000U);
     // DEBUG_EZLANDING 0 is the ezLanding factor 2 is the throttle limit
 }
@@ -497,6 +520,10 @@ static void applyMotorStop(void)
     for (int i = 0; i < mixerRuntime.motorCount; i++) {
         motor[i] = mixerRuntime.disarmMotorOutput;
     }
+
+#ifdef USE_WING
+    motorOutputRms = 0.0f;
+#endif // USE_WING
 }
 
 #ifdef USE_DYN_LPF

src/main/flight/mixer.h

@@ -148,4 +148,9 @@ bool isFixedWing(void);
 
 float getMotorOutputLow(void);
 float getMotorOutputHigh(void);
+
 bool crashFlipSuccessful(void);
+
+#ifdef USE_WING
+float getMotorOutputRms(void);
+#endif

src/main/flight/pid.c

@@ -231,9 +231,6 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .ez_landing_threshold = 25,
         .ez_landing_limit = 15,
         .ez_landing_speed = 50,
-        .tpa_delay_ms = 0,
-        .tpa_gravity_thr0 = 0,
-        .tpa_gravity_thr100 = 0,
         .spa_center = { 0, 0, 0 },
         .spa_width = { 0, 0, 0 },
         .spa_mode = { 0, 0, 0 },
@@ -245,6 +242,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .tpa_curve_pid_thr0 = 200,
         .tpa_curve_pid_thr100 = 70,
         .tpa_curve_expo = 20,
+        .tpa_speed_type = TPA_SPEED_BASIC,
+        .tpa_speed_basic_delay = 1000,
+        .tpa_speed_basic_gravity = 50,
+        .tpa_speed_adv_prop_pitch = 370,
+        .tpa_speed_adv_mass = 1000,
+        .tpa_speed_adv_drag_k = 1000,
+        .tpa_speed_adv_thrust = 2000,
+        .tpa_speed_max_voltage = 2520,
+        .tpa_speed_pitch_offset = 0,
     );
 }
 
@@ -288,21 +294,55 @@ void pidResetIterm(void)
 }
 
 #ifdef USE_WING
-static float getWingTpaArgument(float throttle)
+static float calcWingThrottle(void)
 {
-    const float pitchFactorAdjustment = scaleRangef(throttle, 0.0f, 1.0f, pidRuntime.tpaGravityThr0, pidRuntime.tpaGravityThr100);
+    float batteryThrottleFactor = 1.0f;
-    const float pitchAngleFactor = getSinPitchAngle() * pitchFactorAdjustment;
+    if (pidRuntime.tpaSpeed.maxVoltage > 0.0f) {
-    DEBUG_SET(DEBUG_TPA, 1, lrintf(pitchAngleFactor * 1000.0f));
+        batteryThrottleFactor = getBatteryVoltageLatest() / 100.0f / pidRuntime.tpaSpeed.maxVoltage;
+        batteryThrottleFactor = constrainf(batteryThrottleFactor, 0.0f, 1.0f);
+    }
 
-    float tpaArgument = throttle + pitchAngleFactor;
+    return getMotorOutputRms() * batteryThrottleFactor;
-    const float maxTpaArgument = MAX(1.0 + pidRuntime.tpaGravityThr100, pidRuntime.tpaGravityThr0);
+}
-    tpaArgument = tpaArgument / maxTpaArgument;
+
-    tpaArgument = pt2FilterApply(&pidRuntime.tpaLpf, tpaArgument);
+static float calcWingAcceleration(float throttle, float pitchAngleRadians)
-    DEBUG_SET(DEBUG_TPA, 2, lrintf(tpaArgument * 1000.0f));
+{
+    const tpaSpeedParams_t *tpa = &pidRuntime.tpaSpeed;
+
+    const float thrust = (throttle * throttle - throttle * tpa->speed * tpa->inversePropMaxSpeed) * tpa->twr * G_ACCELERATION;
+    const float drag = tpa->speed * tpa->speed * tpa->dragMassRatio;
+    const float gravity = G_ACCELERATION * sin_approx(pitchAngleRadians);
+
+    return thrust - drag + gravity;
+}
+
+static float calcWingTpaArgument(void)
+{
+    const float t = calcWingThrottle();
+    const float pitchRadians = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
+    const float rollRadians = DECIDEGREES_TO_RADIANS(attitude.values.roll);
+
+    DEBUG_SET(DEBUG_TPA, 1, lrintf(attitude.values.roll)); // decidegrees
+    DEBUG_SET(DEBUG_TPA, 2, lrintf(attitude.values.pitch)); // decidegrees
+    DEBUG_SET(DEBUG_TPA, 3, lrintf(t * 1000.0f)); // calculated throttle in the range of 0 - 1000
+
+    // pitchRadians is always -90 to 90 degrees. The bigger the ABS(pitch) the less portion of pitchOffset is needed.
+    // If ABS(roll) > 90 degrees - flying inverted, then negative portion of pitchOffset is needed.
+    // If ABS(roll) ~ 90 degrees - flying sideways, no pitchOffset is applied.
+    const float correctedPitchAnge = pitchRadians + cos_approx(pitchRadians) * cos_approx(rollRadians) * pidRuntime.tpaSpeed.pitchOffset;
+
+    const float a = calcWingAcceleration(t, correctedPitchAnge);
+
+    pidRuntime.tpaSpeed.speed += a * pidRuntime.dT;
+    pidRuntime.tpaSpeed.speed = MAX(0.0f, pidRuntime.tpaSpeed.speed);
+    const float tpaArgument = constrainf(pidRuntime.tpaSpeed.speed / pidRuntime.tpaSpeed.maxSpeed, 0.0f, 1.0f);
+
+    DEBUG_SET(DEBUG_TPA, 4, lrintf(pidRuntime.tpaSpeed.speed * 10.0f));
+    DEBUG_SET(DEBUG_TPA, 5, lrintf(tpaArgument * 1000.0f));
 
     return tpaArgument;
 }
-#endif // #ifndef USE_WING
+#endif // USE_WING
 
 float getTpaFactorClassic(float tpaArgument)
 {
@@ -328,7 +368,7 @@ void pidUpdateTpaFactor(float throttle)
     float tpaFactor;
 
 #ifdef USE_WING
-    const float tpaArgument = isFixedWing() ?  getWingTpaArgument(throttle) : throttle;
+    const float tpaArgument = isFixedWing() ?  calcWingTpaArgument() : throttle;
 #else
     const float tpaArgument = throttle;
 #endif
@@ -896,7 +936,7 @@ NOINLINE static void calculateSpaValues(const pidProfile_t *pidProfile)
         pidRuntime.spa[axis] = 1.0f - smoothStepUpTransition(
             fabsf(currentRate), pidProfile->spa_center[axis], pidProfile->spa_width[axis]);
         DEBUG_SET(DEBUG_SPA, axis, lrintf(pidRuntime.spa[axis] * 1000));
-    }    
+    }
 #else
     UNUSED(pidProfile);
 #endif // USE_WING
@@ -960,7 +1000,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
 
     const bool isExternalAngleModeRequest = FLIGHT_MODE(GPS_RESCUE_MODE)
 #ifdef USE_ALT_HOLD_MODE
-                || FLIGHT_MODE(ALT_HOLD_MODE) 
+                || FLIGHT_MODE(ALT_HOLD_MODE)
 #endif
                 ;
     levelMode_e levelMode;

src/main/flight/pid.h

@@ -86,6 +86,8 @@
 #define TPA_CURVE_PWL_SIZE 17
 #endif // USE_ADVANCED_TPA
 
+#define G_ACCELERATION 9.80665f // gravitational acceleration in m/s^2
+
 typedef enum {
     TPA_MODE_PD,
     TPA_MODE_D
@@ -161,6 +163,11 @@ typedef enum tpaCurveType_e {
     TPA_CURVE_HYPERBOLIC,
 } tpaCurveType_t;
 
+typedef enum tpaSpeedType_e {
+    TPA_SPEED_BASIC,
+    TPA_SPEED_ADVANCED,
+} tpaSpeedType_t;
+
 #define MAX_PROFILE_NAME_LENGTH 8u
 
 typedef struct pidProfile_s {
@@ -243,7 +250,7 @@ typedef struct pidProfile_s {
     uint8_t feedforward_max_rate_limit;     // Maximum setpoint rate percentage for feedforward
     uint8_t feedforward_yaw_hold_gain;          // Amount of sustained high-pass yaw setpoint to add to feedforward, zero disables
     uint8_t feedforward_yaw_hold_time ;     // Time constant of the sustained yaw hold element in ms to add to feed forward, higher values decay slower
-    
+
     uint8_t dterm_lpf1_dyn_expo;            // set the curve for dynamic dterm lowpass filter
     uint8_t level_race_mode;                // NFE race mode - when true pitch setpoint calculation is gyro based in level mode
     uint8_t vbat_sag_compensation;          // Reduce motor output by this percentage of the maximum compensation amount
@@ -278,17 +285,23 @@ typedef struct pidProfile_s {
     uint8_t ez_landing_speed;               // Speed below which motor output is limited
     uint8_t landing_disarm_threshold;            // Accelerometer vector delta (jerk) threshold with disarms if exceeded
 
-    uint16_t tpa_delay_ms;                  // TPA delay for fixed wings using pt2 filter (time constant)
     uint16_t spa_center[XYZ_AXIS_COUNT];    // RPY setpoint at which PIDs are reduced to 50% (setpoint PID attenuation)
     uint16_t spa_width[XYZ_AXIS_COUNT];     // Width of smooth transition around spa_center
     uint8_t spa_mode[XYZ_AXIS_COUNT];       // SPA mode for each axis
-    uint16_t tpa_gravity_thr0;              // For wings: gravity force addition to tpa argument in % when zero throttle
-    uint16_t tpa_gravity_thr100;            // For wings: gravity force addition to tpa argument in % when full throttle
     uint8_t tpa_curve_type;                 // Classic type - for multirotor, hyperbolic - usually for wings
-    uint8_t tpa_curve_stall_throttle;        // For wings: speed at which PIDs should be maxed out (stall speed)
+    uint8_t tpa_curve_stall_throttle;       // For wings: speed at which PIDs should be maxed out (stall speed)
     uint16_t tpa_curve_pid_thr0;            // For wings: PIDs multiplier at stall speed
     uint16_t tpa_curve_pid_thr100;          // For wings: PIDs multiplier at full speed
     int8_t tpa_curve_expo;                  // For wings: how fast PIDs do transition as speed grows
+    uint8_t tpa_speed_type;             // For wings: relative air speed estimation model type
+    uint16_t tpa_speed_basic_delay;     // For wings when tpa_speed_type = BASIC: delay of air speed estimation from throttle in milliseconds (time of reaching 50% of terminal speed in horizontal flight at full throttle)
+    uint16_t tpa_speed_basic_gravity;   // For wings when tpa_speed_type = BASIC: gravity effect on air speed estimation in percents
+    uint16_t tpa_speed_adv_prop_pitch;  // For wings when tpa_speed_type = ADVANCED: prop pitch in inches * 100
+    uint16_t tpa_speed_adv_mass;        // For wings when tpa_speed_type = ADVANCED: craft mass in grams
+    uint16_t tpa_speed_adv_drag_k;      // For wings when tpa_speed_type = ADVANCED: craft drag coefficient
+    uint16_t tpa_speed_adv_thrust;      // For wings when tpa_speed_type = ADVANCED: stationary thrust in grams
+    uint16_t tpa_speed_max_voltage;     // For wings: theoretical max voltage; used for throttle scailing with voltage for air speed estimation
+    int16_t tpa_speed_pitch_offset;     // For wings: pitch offset in degrees*10 for craft speed estimation
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -329,6 +342,16 @@ typedef struct pidCoefficient_s {
     float Kf;
 } pidCoefficient_t;
 
+typedef struct tpaSpeedParams_s {
+    float maxSpeed;
+    float dragMassRatio;
+    float inversePropMaxSpeed;
+    float twr;
+    float speed;
+    float maxVoltage;
+    float pitchOffset;
+} tpaSpeedParams_t;
+
 typedef struct pidRuntime_s {
     float dT;
     float pidFrequency;
@@ -460,7 +483,7 @@ typedef struct pidRuntime_s {
     uint8_t feedforwardMaxRateLimit;
     float feedforwardYawHoldGain;
     float feedforwardYawHoldTime;
-    bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values 
+    bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values
     pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
 #endif
 
@@ -475,10 +498,8 @@ typedef struct pidRuntime_s {
 #endif
 
 #ifdef USE_WING
-    pt2Filter_t tpaLpf;
     float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation
-    float tpaGravityThr0;
+    tpaSpeedParams_t tpaSpeed;
-    float tpaGravityThr100;
 #endif // USE_WING
 
 #ifdef USE_ADVANCED_TPA

src/main/flight/pid_init.c

@@ -40,6 +40,8 @@
 #include "flight/pid.h"
 #include "flight/rpm_filter.h"
 
+#include "pg/motor.h"
+
 #include "rx/rx.h"
 
 #include "sensors/gyro.h"
@@ -66,6 +68,66 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
 #endif
 }
 
+#ifdef USE_WING
+void tpaSpeedBasicInit(const pidProfile_t *pidProfile)
+{
+    // basic model assumes prop pitch speed is inf
+    const float gravityFactor = pidProfile->tpa_speed_basic_gravity / 100.0f;
+    const float delaySec = pidProfile->tpa_speed_basic_delay / 1000.0f;
+
+    pidRuntime.tpaSpeed.twr = 1.0f / (gravityFactor * gravityFactor);
+    const float massDragRatio = (2.0f / logf(3.0f)) * (2.0f / logf(3.0f)) * pidRuntime.tpaSpeed.twr * G_ACCELERATION * delaySec * delaySec;
+    pidRuntime.tpaSpeed.dragMassRatio = 1.0f / massDragRatio;
+    pidRuntime.tpaSpeed.maxSpeed = sqrtf(massDragRatio * pidRuntime.tpaSpeed.twr * G_ACCELERATION + G_ACCELERATION);
+    pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f;
+}
+
+void tpaSpeedAdvancedInit(const pidProfile_t *pidProfile)
+{
+    // Advanced model uses prop pitch speed, and is quite limited when craft speed is far above prop pitch speed.
+    pidRuntime.tpaSpeed.twr = (float)pidProfile->tpa_speed_adv_thrust / (float)pidProfile->tpa_speed_adv_mass;
+    const float mass = pidProfile->tpa_speed_adv_mass / 1000.0f;
+    const float dragK = pidProfile->tpa_speed_adv_drag_k / 10000.0f;
+    const float propPitch = pidProfile->tpa_speed_adv_prop_pitch / 100.0f;
+    pidRuntime.tpaSpeed.dragMassRatio = dragK / mass;
+    const float propMaxSpeed = (2.54f / 100.0f / 60.0f) * propPitch * motorConfig()->kv * pidRuntime.tpaSpeed.maxVoltage;
+    if (propMaxSpeed <= 0.0f) { // assuming propMaxSpeed is inf
+        pidRuntime.tpaSpeed.inversePropMaxSpeed = 0.0f;
+    } else {
+        pidRuntime.tpaSpeed.inversePropMaxSpeed = 1.0f / propMaxSpeed;
+    }
+
+    const float maxFallSpeed = sqrtf(mass * G_ACCELERATION / dragK);
+
+    const float a = dragK;
+    const float b = mass * pidRuntime.tpaSpeed.twr * G_ACCELERATION * pidRuntime.tpaSpeed.inversePropMaxSpeed;
+    const float c = -mass * (pidRuntime.tpaSpeed.twr + 1) * G_ACCELERATION;
+
+    const float maxDiveSpeed = (-b + sqrtf(b*b - 4.0f * a * c)) / (2.0f * a);
+
+    pidRuntime.tpaSpeed.maxSpeed = MAX(maxFallSpeed, maxDiveSpeed);
+    UNUSED(pidProfile);
+}
+
+void tpaSpeedInit(const pidProfile_t *pidProfile)
+{
+    pidRuntime.tpaSpeed.speed = 0.0f;
+    pidRuntime.tpaSpeed.maxVoltage = pidProfile->tpa_speed_max_voltage / 100.0f;
+    pidRuntime.tpaSpeed.pitchOffset = pidProfile->tpa_speed_pitch_offset * M_PIf / 10.0f / 180.0f;
+
+    switch (pidProfile->tpa_speed_type) {
+    case TPA_SPEED_BASIC:
+        tpaSpeedBasicInit(pidProfile);
+        break;
+    case TPA_SPEED_ADVANCED:
+        tpaSpeedAdvancedInit(pidProfile);
+        break;
+    default:
+        break;
+    }
+}
+#endif // USE_WING
+
 void pidInitFilters(const pidProfile_t *pidProfile)
 {
     STATIC_ASSERT(FD_YAW == 2, FD_YAW_incorrect); // ensure yaw axis is 2
@@ -257,9 +319,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 
     pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
 #ifdef USE_WING
-    pt2FilterInit(&pidRuntime.tpaLpf, pt2FilterGainFromDelay(pidProfile->tpa_delay_ms / 1000.0f, pidRuntime.dT));
-    pidRuntime.tpaGravityThr0 = pidProfile->tpa_gravity_thr0 / 100.0f;
-    pidRuntime.tpaGravityThr100 = pidProfile->tpa_gravity_thr100 / 100.0f;
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         pidRuntime.spa[axis] = 1.0f; // 1.0 = no PID attenuation in runtime. 0 - full attenuation (no PIDs)
     }
@@ -496,6 +555,9 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     pidRuntime.useEzDisarm = pidProfile->landing_disarm_threshold > 0;
     pidRuntime.landingDisarmThreshold = pidProfile->landing_disarm_threshold * 10.0f;
 
+#ifdef USE_WING
+    tpaSpeedInit(pidProfile);
+#endif
 }
 
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)