Setpoint PID attenuation (for wings) (#13719)

* Setpoint PID attenuation

* Suggestions from ledvinap's review

src/main/blackbox/blackbox.c

@@ -1412,6 +1412,18 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_LIMIT, "%d",       currentPidProfile->ez_landing_limit);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_SPEED, "%d",       currentPidProfile->ez_landing_speed);
 
+#ifdef USE_WING
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_CENTER, "%d",        currentPidProfile->spa_center[FD_ROLL]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_WIDTH, "%d",         currentPidProfile->spa_width[FD_ROLL]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_MODE, "%d",          currentPidProfile->spa_mode[FD_ROLL]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_CENTER, "%d",       currentPidProfile->spa_center[FD_PITCH]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_WIDTH, "%d",        currentPidProfile->spa_width[FD_PITCH]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_MODE, "%d",         currentPidProfile->spa_mode[FD_PITCH]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_CENTER, "%d",         currentPidProfile->spa_center[FD_YAW]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_WIDTH, "%d",          currentPidProfile->spa_width[FD_YAW]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_MODE, "%d",           currentPidProfile->spa_mode[FD_YAW]);
+#endif
+
         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
                                                                             currentControlRateProfile->rcRates[PITCH],
                                                                             currentControlRateProfile->rcRates[YAW]);

src/main/build/debug.c

@@ -118,5 +118,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "MAG_TASK_RATE",
     "EZLANDING",
     "TPA",
-    "S_TERM",  
+    "S_TERM",
+    "SPA",
 };

src/main/build/debug.h

@@ -121,6 +121,7 @@ typedef enum {
     DEBUG_EZLANDING,
     DEBUG_TPA,
     DEBUG_S_TERM,
+    DEBUG_SPA,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -439,6 +439,10 @@ static const char * const lookupTableTpaMode[] = {
 };
 #endif
 
+static const char * const lookupTableSpaMode[] = {
+    "OFF", "I_FREEZE", "I", "PID", "PD_I_FREEZE"
+};
+
 #ifdef USE_LED_STRIP
 #ifdef USE_LED_STRIP_STATUS_MODE
 static const char * const lookupTableLEDProfile[] = {
@@ -629,6 +633,7 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_TPA_MODE
     LOOKUP_TABLE_ENTRY(lookupTableTpaMode),
 #endif
+    LOOKUP_TABLE_ENTRY(lookupTableSpaMode),
 #ifdef USE_LED_STRIP
     LOOKUP_TABLE_ENTRY(lookupTableLEDProfile),
     LOOKUP_TABLE_ENTRY(lookupTableLedstripColors),
@@ -1273,6 +1278,18 @@ const clivalue_t valueTable[] = {
     { 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) },
 
+#ifdef USE_WING
+    { PARAM_NAME_SPA_ROLL_CENTER,    VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_center[FD_ROLL]) },
+    { PARAM_NAME_SPA_ROLL_WIDTH,     VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_width[FD_ROLL]) },
+    { PARAM_NAME_SPA_ROLL_MODE,      VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_mode[FD_ROLL]) },
+    { PARAM_NAME_SPA_PITCH_CENTER,   VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_center[FD_PITCH]) },
+    { PARAM_NAME_SPA_PITCH_WIDTH,    VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_width[FD_PITCH]) },
+    { PARAM_NAME_SPA_PITCH_MODE,     VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_mode[FD_PITCH]) },
+    { PARAM_NAME_SPA_YAW_CENTER,     VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_center[FD_YAW]) },
+    { PARAM_NAME_SPA_YAW_WIDTH,      VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT16_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_width[FD_YAW]) },
+    { PARAM_NAME_SPA_YAW_MODE,       VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, spa_mode[FD_YAW]) },
+#endif
+
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },

src/main/cli/settings.h

@@ -123,6 +123,7 @@ typedef enum {
 #ifdef USE_TPA_MODE
     TABLE_TPA_MODE,
 #endif
+    TABLE_SPA_MODE,
 #ifdef USE_LED_STRIP
     TABLE_LED_PROFILE,
     TABLE_LEDSTRIP_COLOR,

src/main/common/maths.c

@@ -346,3 +346,26 @@ fix12_t  qConstruct(int16_t num, int16_t den)
 {
     return (num << 12) / den;
 }
+
+// Cubic polynomial blending function
+static float cubicBlend(const float t)
+{
+    return t * t * (3.0f - 2.0f * t);
+}
+
+// Smooth step-up transition function from 0 to 1
+float smoothStepUpTransition(const float x, const float center, const float width)
+{
+    const float half_width = width * 0.5f;
+    const float left_limit = center - half_width;
+    const float right_limit = center + half_width;
+
+    if (x < left_limit) {
+        return 0.0f;
+    } else if (x > right_limit) {
+        return 1.0f;
+    } else {
+        const float t = (x - left_limit) / width; // Normalize x within the range
+        return cubicBlend(t);
+    }
+}

src/main/common/maths.h

@@ -156,6 +156,8 @@ int16_t qPercent(fix12_t q);
 int16_t qMultiply(fix12_t q, int16_t input);
 fix12_t qConstruct(int16_t num, int16_t den);
 
+float smoothStepDownTransition(const float x, const float center, const float width);
+
 static inline int constrain(int amt, int low, int high)
 {
     if (amt < low)

src/main/fc/parameter_names.h

@@ -63,6 +63,15 @@
 #define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold"
 #define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit"
 #define PARAM_NAME_EZ_LANDING_SPEED "ez_landing_speed"
+#define PARAM_NAME_SPA_ROLL_CENTER "spa_roll_center"
+#define PARAM_NAME_SPA_ROLL_WIDTH "spa_roll_width"
+#define PARAM_NAME_SPA_ROLL_MODE "spa_roll_mode"
+#define PARAM_NAME_SPA_PITCH_CENTER "spa_pitch_center"
+#define PARAM_NAME_SPA_PITCH_WIDTH "spa_pitch_width"
+#define PARAM_NAME_SPA_PITCH_MODE "spa_pitch_mode"
+#define PARAM_NAME_SPA_YAW_CENTER "spa_yaw_center"
+#define PARAM_NAME_SPA_YAW_WIDTH "spa_yaw_width"
+#define PARAM_NAME_SPA_YAW_MODE "spa_yaw_mode"
 #define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
 #define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
 #define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm"

src/main/flight/pid.c

@@ -231,6 +231,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .ez_landing_limit = 15,
         .ez_landing_speed = 50,
         .tpa_delay_ms = 0,
+        .spa_center = { 0, 0, 0 },
+        .spa_width = { 0, 0, 0 },
+        .spa_mode = { 0, 0, 0 },
     );
 
 #ifndef USE_D_MIN
@@ -812,6 +815,47 @@ static float getSterm(int axis, const pidProfile_t *pidProfile)
 #endif
 }
 
+NOINLINE static void calculateSpaValues(const pidProfile_t *pidProfile)
+{
+#ifdef USE_WING
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        float currentRate = getSetpointRate(axis);
+        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 // #ifdef USE_WING ... #else
+}
+
+NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile)
+{
+#ifdef USE_WING
+    switch(pidProfile->spa_mode[axis]){
+        case SPA_MODE_PID:
+            pidData[axis].P *= pidRuntime.spa[axis];
+            pidData[axis].D *= pidRuntime.spa[axis];
+            pidData[axis].I *= pidRuntime.spa[axis];
+            break;
+        case SPA_MODE_I:
+            pidData[axis].I *= pidRuntime.spa[axis];
+            break;
+        case SPA_MODE_PD_I_FREEZE:
+            pidData[axis].P *= pidRuntime.spa[axis];
+            pidData[axis].D *= pidRuntime.spa[axis];
+            break;
+        case SPA_MODE_I_FREEZE:
+        case SPA_MODE_OFF:
+        default:
+            break;
+    }
+#else
+    UNUSED(axis);
+    UNUSED(pidProfile);
+#endif // #ifdef USE_WING ... #else
+}
+
 // 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)
@@ -819,6 +863,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
     static float previousGyroRateDterm[XYZ_AXIS_COUNT];
     static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
 
+    calculateSpaValues(pidProfile);
+
 #ifdef USE_TPA_MODE
     const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
 #else
@@ -1015,7 +1061,14 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
                 pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
             }
         }
-        const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
+
+        float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
+#ifdef USE_WING
+        if (pidProfile->spa_mode[axis] != SPA_MODE_OFF) {
+            // slowing down I-term change, or even making it zero if setpoint is high enough
+            iTermChange *= pidRuntime.spa[axis];
+        }
+#endif // #ifdef USE_WING
         pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
 
         // -----calculate D component
@@ -1151,6 +1204,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
         }
 
         pidData[axis].S = getSterm(axis, pidProfile);
+        applySpa(axis, pidProfile);
 
         // calculating the PID sum
         const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F + pidData[axis].S;

src/main/flight/pid.h

@@ -81,6 +81,14 @@ typedef enum {
     TPA_MODE_D
 } tpaMode_e;
 
+typedef enum {
+    SPA_MODE_OFF,
+    SPA_MODE_I_FREEZE,
+    SPA_MODE_I,
+    SPA_MODE_PID,
+    SPA_MODE_PD_I_FREEZE,
+} spaMode_e;
+
 typedef enum {
     PID_ROLL,
     PID_PITCH,
@@ -253,6 +261,9 @@ typedef struct pidProfile_s {
     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)
+    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
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -435,6 +446,7 @@ typedef struct pidRuntime_s {
 
 #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
 #endif
 } pidRuntime_t;
 

src/main/flight/pid_init.c

@@ -259,6 +259,9 @@ 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));
+    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)
+    }
 #endif
 }