TPA delay in ms (for wings) (#13662)

* TPA delay in ms (for wings)

* Added comments for time constant

* ledvinap's suggestions

* tpaFactor local variable (based on ledvinap's suggestions)

* pt1FilterGainFromDelay with inlined calculations for cutoff (based on ledvinap's suggestions)

* Karatebrot extra review

* Pump PG for pidConfig_t

* Fixes based on review

* Moved tpaLpf to the end of pidRuntime_t

src/main/blackbox/blackbox.c

@@ -1664,6 +1664,10 @@ static bool blackboxWriteSysinfo(void)
 #endif
 #endif
 
+#ifdef USE_WING
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_DELAY_MS, "%d", currentPidProfile->tpa_delay_ms);
+#endif
+
         default:
             return true;
     }

src/main/build/debug.c

@@ -117,4 +117,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "MAG_CALIB",
     "MAG_TASK_RATE",
     "EZLANDING",
+    "TPA",
 };

src/main/build/debug.h

@@ -119,6 +119,7 @@ typedef enum {
     DEBUG_MAG_CALIB,
     DEBUG_MAG_TASK_RATE,
     DEBUG_EZLANDING,
+    DEBUG_TPA,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -1259,6 +1259,10 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_TPA_LOW_BREAKPOINT,      VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_breakpoint) },
     { 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) },
+#endif
+
     { PARAM_NAME_EZ_LANDING_THRESHOLD,      VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) },
     { PARAM_NAME_EZ_LANDING_LIMIT,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 75 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_limit) },
     { PARAM_NAME_EZ_LANDING_SPEED,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_speed) },

src/main/common/filter.c

@@ -30,6 +30,9 @@
 
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - 2nd order butterworth*/
 
+// PTn cutoff correction = 1 / sqrt(2^(1/n) - 1)
+#define CUTOFF_CORRECTION_PT2 1.553773974f
+#define CUTOFF_CORRECTION_PT3 1.961459177f
 
 // NULL filter
 
@@ -48,6 +51,17 @@ FAST_CODE_NOINLINE float pt1FilterGain(float f_cut, float dT)
     return omega / (omega + 1.0f);
 }
 
+// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
+float pt1FilterGainFromDelay(float delay, float dT)
+{
+    if (delay <= 0) {
+        return 1.0f; // gain = 1 means no filtering
+    }
+
+    const float cutoffHz = 1.0f / (2.0f * M_PIf * delay);
+    return pt1FilterGain(cutoffHz, dT);
+}
+
 void pt1FilterInit(pt1Filter_t *filter, float k)
 {
     filter->state = 0.0f;
@@ -70,13 +84,21 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
 
 FAST_CODE float pt2FilterGain(float f_cut, float dT)
 {
-    // PTn cutoff correction = 1 / sqrt(2^(1/n) - 1)
-    #define CUTOFF_CORRECTION_PT2 1.553773974f
-
     // shift f_cut to satisfy -3dB cutoff condition
     return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT2, dT);
 }
 
+// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
+float pt2FilterGainFromDelay(float delay, float dT)
+{
+    if (delay <= 0) {
+        return 1.0f; // gain = 1 means no filtering
+    }
+
+    const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT2);
+    return pt2FilterGain(cutoffHz, dT);
+}
+
 void pt2FilterInit(pt2Filter_t *filter, float k)
 {
     filter->state = 0.0f;
@@ -101,13 +123,21 @@ FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
 
 FAST_CODE float pt3FilterGain(float f_cut, float dT)
 {
-    // PTn cutoff correction = 1 / sqrt(2^(1/n) - 1)
-    #define CUTOFF_CORRECTION_PT3 1.961459177f
-
     // shift f_cut to satisfy -3dB cutoff condition
     return pt1FilterGain(f_cut * CUTOFF_CORRECTION_PT3, dT);
 }
 
+// Calculates filter gain based on delay (time constant of filter) - time it takes for filter response to reach 63.2% of a step input.
+float pt3FilterGainFromDelay(float delay, float dT)
+{
+    if (delay <= 0) {
+        return 1.0f; // gain = 1 means no filtering
+    }
+
+    const float cutoffHz = 1.0f / (M_PIf * delay * CUTOFF_CORRECTION_PT3);
+    return pt3FilterGain(cutoffHz, dT);
+}
+
 void pt3FilterInit(pt3Filter_t *filter, float k)
 {
     filter->state = 0.0f;

src/main/common/filter.h

@@ -98,16 +98,19 @@ typedef struct meanAccumulator_s {
 float nullFilterApply(filter_t *filter, float input);
 
 float pt1FilterGain(float f_cut, float dT);
+float pt1FilterGainFromDelay(float delay, float dT);
 void pt1FilterInit(pt1Filter_t *filter, float k);
 void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 
 float pt2FilterGain(float f_cut, float dT);
+float pt2FilterGainFromDelay(float delay, float dT);
 void pt2FilterInit(pt2Filter_t *filter, float k);
 void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
 float pt2FilterApply(pt2Filter_t *filter, float input);
 
 float pt3FilterGain(float f_cut, float dT);
+float pt3FilterGainFromDelay(float delay, float dT);
 void pt3FilterInit(pt3Filter_t *filter, float k);
 void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 float pt3FilterApply(pt3Filter_t *filter, float input);

src/main/fc/parameter_names.h

@@ -58,6 +58,7 @@
 #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_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/pid.c

@@ -88,7 +88,7 @@ FAST_DATA_ZERO_INIT float throttleBoost;
 pt1Filter_t throttleLpf;
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 4);
 
 #ifndef DEFAULT_PID_PROCESS_DENOM
 #define DEFAULT_PID_PROCESS_DENOM       1
@@ -230,6 +230,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .ez_landing_threshold = 25,
         .ez_landing_limit = 15,
         .ez_landing_speed = 50,
+        .tpa_delay_ms = 0,
     );
 
 #ifndef USE_D_MIN
@@ -292,7 +293,18 @@ void pidUpdateTpaFactor(float throttle)
     } else {
         tpaRate = pidRuntime.tpaLowMultiplier * (pidRuntime.tpaLowBreakpoint - throttle);
     }
-    pidRuntime.tpaFactor = 1.0f - tpaRate;
+
+    float tpaFactor = 1.0f - tpaRate;
+    DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000));
+
+#ifdef USE_WING
+    if (isFixedWing()) {
+        tpaFactor = pt2FilterApply(&pidRuntime.tpaLpf, tpaFactor);
+    }    
+#endif
+
+    DEBUG_SET(DEBUG_TPA, 1, lrintf(tpaFactor * 1000));
+    pidRuntime.tpaFactor = tpaFactor;
 }
 
 void pidUpdateAntiGravityThrottleFilter(float throttle)

src/main/flight/pid.h

@@ -245,6 +245,7 @@ typedef struct pidProfile_s {
     uint8_t ez_landing_threshold;           // Threshold stick position below which motor output is limited
     uint8_t ez_landing_limit;               // Maximum motor output when all sticks centred and throttle zero
     uint8_t ez_landing_speed;               // Speed below which motor output is limited
+    uint16_t tpa_delay_ms;                  // TPA delay for fixed wings using pt2 filter (time constant)
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -423,6 +424,10 @@ typedef struct pidRuntime_s {
     bool axisInAngleMode[3];
     float maxRcRateInv[2];
 #endif
+
+#ifdef USE_WING
+    pt2Filter_t tpaLpf;
+#endif
 } pidRuntime_t;
 
 extern pidRuntime_t pidRuntime;

src/main/flight/pid_init.c

@@ -257,6 +257,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 #endif
 
     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));
+#endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)