TPA mode PDS + Wing setpoint attenuation (for wings) (#14010)

* TPA mode PDS + Wing setpoint attenuation

* removed brackets by haslinghuis :)

* Ledvinap's review

---------

Co-authored-by: limonspb <gendalf44@yandex.ru>

src/main/build/debug.c

@@ -121,4 +121,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     [DEBUG_SPA] = "SPA",
     [DEBUG_TASK] = "TASK",
     [DEBUG_GIMBAL] = "GIMBAL",
+    [DEBUG_WING_SETPOINT] = "WING_SETPOINT",
 };

src/main/build/debug.h

@@ -123,6 +123,7 @@ typedef enum {
     DEBUG_SPA,
     DEBUG_TASK,
     DEBUG_GIMBAL,
+    DEBUG_WING_SETPOINT,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -440,7 +440,10 @@ static const char * const lookupTableLaunchControlMode[] = {
 #endif
 
 static const char * const lookupTableTpaMode[] = {
-    "PD", "D"
+    "PD", "D",
+#ifdef USE_WING    
+    "PDS",
+#endif
 };
 
 static const char * const lookupTableSpaMode[] = {

src/main/flight/pid.c

@@ -118,6 +118,12 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
+#ifdef USE_ACC
+#define IS_AXIS_IN_ANGLE_MODE(i) (pidRuntime.axisInAngleMode[(i)])
+#else
+#define IS_AXIS_IN_ANGLE_MODE(i) false
+#endif // USE_ACC
+
 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 11);
 
 void resetPidProfile(pidProfile_t *pidProfile)
@@ -256,6 +262,21 @@ void resetPidProfile(pidProfile_t *pidProfile)
     );
 }
 
+static bool isTpaActive(tpaMode_e tpaMode, term_e term) {
+    switch (tpaMode) {
+    case TPA_MODE_PD:
+        return term == TERM_P || term == TERM_D;
+    case TPA_MODE_D:
+        return term == TERM_D;
+#ifdef USE_WING
+    case TPA_MODE_PDS:
+        return term == TERM_P || term == TERM_D || term == TERM_S;
+#endif
+    default:
+        return false;
+    }
+}
+
 void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
 {
     for (int i = 0; i < PID_PROFILE_COUNT; i++) {
@@ -343,8 +364,50 @@ static float calcWingTpaArgument(void)
 
     return tpaArgument;
 }
+
+static void updateStermTpaFactor(int axis, float tpaFactor)
+{
+    float tpaFactorSterm = tpaFactor;
+    if (pidRuntime.tpaCurveType == TPA_CURVE_HYPERBOLIC) {
+        const float maxSterm = tpaFactorSterm * (float)currentPidProfile->pid[axis].S * S_TERM_SCALE;
+        if (maxSterm > 1.0f) {
+            tpaFactorSterm *=  1.0f / maxSterm;
+        }
+    }
+    pidRuntime.tpaFactorSterm[axis] = tpaFactorSterm;
+}
+
+static void updateStermTpaFactors(void) {
+    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+        float tpaFactor = pidRuntime.tpaFactor;
+        if (i == FD_YAW && currentPidProfile->yaw_type == YAW_TYPE_DIFF_THRUST) {
+            tpaFactor = pidRuntime.tpaFactorYaw;
+        }
+        updateStermTpaFactor(i, tpaFactor);
+    }
+}
 #endif // USE_WING
 
+static float wingAdjustSetpoint(float currentPidSetpoint, int axis)
+{
+#ifdef USE_WING
+    float adjustedSetpoint = currentPidSetpoint;
+    if (!IS_AXIS_IN_ANGLE_MODE(axis)) {
+        const bool skipYaw = axis == FD_YAW && currentPidProfile->yaw_type == YAW_TYPE_DIFF_THRUST;
+        if (pidRuntime.tpaFactorSterm[axis] > 0.0f && pidRuntime.tpaFactor > 0.0f && !skipYaw) {
+            adjustedSetpoint = currentPidSetpoint * pidRuntime.tpaFactorSterm[axis] / pidRuntime.tpaFactor;
+        }
+    }
+
+    DEBUG_SET(DEBUG_WING_SETPOINT, 2 * axis, lrintf(currentPidSetpoint));
+    DEBUG_SET(DEBUG_WING_SETPOINT, 2 * axis + 1, lrintf(adjustedSetpoint));
+    return adjustedSetpoint;
+#else
+    UNUSED(axis);
+    return currentPidSetpoint;
+#endif // USE_WING
+}
+
 float getTpaFactorClassic(float tpaArgument)
 {
     static bool isTpaLowFaded = false;
@@ -399,6 +462,7 @@ void pidUpdateTpaFactor(float throttle)
         pidRuntime.tpaFactorYaw = pidRuntime.tpaFactor;
         break;
     }
+    updateStermTpaFactors();
 #endif // USE_WING
 }
 
@@ -930,18 +994,48 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
 }
 #endif
 
-static float getSterm(int axis, const pidProfile_t *pidProfile)
+static float getTpaFactor(const pidProfile_t *pidProfile, int axis, term_e term)
+{
+    float tpaFactor = pidRuntime.tpaFactor;
+
+#ifdef USE_WING
+    if (axis == FD_YAW) {
+        tpaFactor = pidRuntime.tpaFactorYaw;
+    }
+#else
+    UNUSED(axis);
+#endif
+
+    const bool tpaActive = isTpaActive(pidProfile->tpa_mode, term);
+    switch (term) {
+    case TERM_P:
+        return tpaActive ? tpaFactor : 1.0f;
+    case TERM_D:
+        return tpaFactor;
+#ifdef USE_WING
+    case TERM_S:
+        return tpaActive ? pidRuntime.tpaFactorSterm[axis] : 1.0f;
+#endif
+    default:
+        return 1.0f;
+    }
+}
+
+static float getSterm(int axis, const pidProfile_t *pidProfile, float setpoint)
 {
 #ifdef USE_WING
-    const float sTerm = getSetpointRate(axis) / getMaxRcRate(axis) * 1000.0f *
+    float sTerm = setpoint / getMaxRcRate(axis) * 1000.0f *
-        (float)pidProfile->pid[axis].S / 100.0f;
+        (float)pidProfile->pid[axis].S * S_TERM_SCALE;
 
-    DEBUG_SET(DEBUG_S_TERM, axis, lrintf(sTerm));
+    DEBUG_SET(DEBUG_S_TERM, 2 * axis, lrintf(sTerm));
+    sTerm *= getTpaFactor(pidProfile, axis, TERM_S);
+    DEBUG_SET(DEBUG_S_TERM, 2 * axis + 1, lrintf(sTerm));
 
     return sTerm;
 #else
     UNUSED(axis);
     UNUSED(pidProfile);
+    UNUSED(setpoint);
     return 0.0f;
 #endif
 }
@@ -993,28 +1087,6 @@ NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile)
 #endif // USE_WING
 }
 
-static float getTpaFactor(const pidProfile_t *pidProfile, int axis, term_e term)
-{
-    float tpaFactor = pidRuntime.tpaFactor;
-
-#ifdef USE_WING
-    if (axis == FD_YAW) {
-        tpaFactor = pidRuntime.tpaFactorYaw;
-    }
-#else
-    UNUSED(axis);
-#endif
-
-    switch (term) {
-    case TERM_P:
-        return (pidProfile->tpa_mode == TPA_MODE_PD) ? tpaFactor : 1.0f;
-    case TERM_D:
-        return tpaFactor;
-    default:
-        return 1.0f;
-    }
-}
-
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
 void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@@ -1145,6 +1217,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         }
 #endif
 
+        const float currentPidSetpointBeforeWingAdjust = currentPidSetpoint;
+        currentPidSetpoint = wingAdjustSetpoint(currentPidSetpoint, axis);
+
 #ifdef USE_ACRO_TRAINER
         if ((axis != FD_YAW) && pidRuntime.acroTrainerActive && !pidRuntime.inCrashRecoveryMode && !launchControlActive) {
             currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
@@ -1367,7 +1442,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             }
         }
 
-        pidData[axis].S = getSterm(axis, pidProfile);
+        pidData[axis].S = getSterm(axis, pidProfile, currentPidSetpointBeforeWingAdjust);
         applySpa(axis, pidProfile);
 
         // calculating the PID sum

src/main/flight/pid.h

@@ -73,6 +73,7 @@
 
 #ifdef USE_WING
 #define ANGLE_PITCH_OFFSET_MAX 450
+#define S_TERM_SCALE 0.01f
 #define TPA_LOW_RATE_MIN INT8_MIN
 #define TPA_GRAVITY_MAX 5000
 #define TPA_CURVE_STALL_THROTTLE_MAX 100
@@ -91,7 +92,10 @@
 
 typedef enum {
     TPA_MODE_PD,
-    TPA_MODE_D
+    TPA_MODE_D,
+#ifdef USE_WING
+    TPA_MODE_PDS,
+#endif
 } tpaMode_e;
 
 typedef enum {
@@ -517,6 +521,7 @@ typedef struct pidRuntime_s {
     float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation
     tpaSpeedParams_t tpaSpeed;
     float tpaFactorYaw;
+    float tpaFactorSterm[XYZ_AXIS_COUNT];
 #endif // USE_WING
 
 #ifdef USE_ADVANCED_TPA