S-term (for wings) (#13679)

* S-term for wings

* tabs

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

* pidData[axis].S

---------

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

src/main/blackbox/blackbox.c

@@ -1497,6 +1497,13 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d",              currentPidProfile->rateAccelLimit);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT, "%d",           currentPidProfile->pidSumLimit);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT_YAW, "%d",       currentPidProfile->pidSumLimitYaw);
+
+#ifdef USE_WING
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_ROLL,  "%d", currentPidProfile->pid[PID_ROLL].S);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_PITCH, "%d", currentPidProfile->pid[PID_PITCH].S);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_YAW,   "%d", currentPidProfile->pid[PID_YAW].S);
+#endif // #ifdef USE_WING
+
         // End of Betaflight controller parameters
 
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d",               rcControlsConfig()->deadband);

src/main/build/debug.c

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

src/main/build/debug.h

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

src/main/cli/settings.c

@@ -1163,6 +1163,12 @@ const clivalue_t valueTable[] = {
     { "d_yaw",                      VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
     { "f_yaw",                      VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, F_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].F) },
 
+#ifdef USE_WING
+    { PARAM_NAME_S_PITCH,           VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].S) },
+    { PARAM_NAME_S_ROLL,            VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ROLL].S) },
+    { PARAM_NAME_S_YAW,             VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, PID_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].S) },
+#endif // #ifdef USE_WING
+
     { PARAM_NAME_ANGLE_P_GAIN,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
     { PARAM_NAME_ANGLE_FEEDFORWARD,     VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].F) },
     { PARAM_NAME_ANGLE_FF_SMOOTHING_MS, VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_feedforward_smoothing_ms) },

src/main/fc/parameter_names.h

@@ -136,6 +136,9 @@
 #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
 #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"
 #define PARAM_NAME_ANGLE_LIMIT "angle_limit"
+#define PARAM_NAME_S_PITCH "s_pitch"
+#define PARAM_NAME_S_ROLL "s_roll"
+#define PARAM_NAME_S_YAW "s_yaw"
 #define PARAM_NAME_ANGLE_P_GAIN "angle_p_gain"
 #define PARAM_NAME_ANGLE_EARTH_REF "angle_earth_ref"
 

src/main/flight/pid.c

@@ -125,8 +125,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
             [PID_ROLL] =  PID_ROLL_DEFAULT,
             [PID_PITCH] = PID_PITCH_DEFAULT,
             [PID_YAW] =   PID_YAW_DEFAULT,
-            [PID_LEVEL] = { 50, 75, 75, 50 },
+            [PID_LEVEL] = { 50, 75, 75, 50, 0 },
-            [PID_MAG] =   { 40, 0, 0, 0 },
+            [PID_MAG] =   { 40, 0, 0, 0, 0 },
         },
         .pidSumLimit = PIDSUM_LIMIT,
         .pidSumLimitYaw = PIDSUM_LIMIT_YAW,
@@ -796,6 +796,22 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
 }
 #endif
 
+static float getSterm(int axis, const pidProfile_t *pidProfile)
+{
+#ifdef USE_WING
+    const float sTerm = getSetpointRate(axis) / getMaxRcRate(axis) * 1000.0f *
+        (float)pidProfile->pid[axis].S / 100.0f;
+
+    DEBUG_SET(DEBUG_S_TERM, axis, lrintf(sTerm));
+
+    return sTerm;
+#else
+    UNUSED(axis);
+    UNUSED(pidProfile);
+    return 0.0f;
+#endif
+}
+
 // 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)
@@ -1098,6 +1114,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
                 pidData[axis].P = 0;
                 pidData[axis].D = 0;
                 pidData[axis].F = 0;
+                pidData[axis].S = 0;
             }
         }
 #endif // USE_YAW_SPIN_RECOVERY
@@ -1133,8 +1150,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             }
         }
 
+        pidData[axis].S = getSterm(axis, pidProfile);
+
         // calculating the PID sum
-        const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
+        const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F + pidData[axis].S;
 #ifdef USE_INTEGRATED_YAW_CONTROL
         if (axis == FD_YAW && pidRuntime.useIntegratedYaw) {
             pidData[axis].Sum += pidSum * pidRuntime.dT * 100.0f;
@@ -1154,6 +1173,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
             pidData[axis].I = 0;
             pidData[axis].D = 0;
             pidData[axis].F = 0;
+            pidData[axis].S = 0;
 
             pidData[axis].Sum = 0;
         }

src/main/flight/pid.h

@@ -59,9 +59,9 @@
 #define ITERM_ACCELERATOR_GAIN_OFF 0
 #define ITERM_ACCELERATOR_GAIN_MAX 250
 
-#define PID_ROLL_DEFAULT  { 45, 80, 40, 120 }
+#define PID_ROLL_DEFAULT  { 45, 80, 40, 120, 0 }
-#define PID_PITCH_DEFAULT { 47, 84, 46, 125 }
+#define PID_PITCH_DEFAULT { 47, 84, 46, 125, 0 }
-#define PID_YAW_DEFAULT   { 45, 80,  0, 120 }
+#define PID_YAW_DEFAULT   { 45, 80,  0, 120, 0 }
 #define D_MIN_DEFAULT     { 30, 34, 0 }
 
 #define DTERM_LPF1_DYN_MIN_HZ_DEFAULT 75
@@ -107,6 +107,7 @@ typedef struct pidf_s {
     uint8_t I;
     uint8_t D;
     uint16_t F;
+    uint8_t S;
 } pidf_t;
 
 typedef enum {
@@ -267,6 +268,7 @@ typedef struct pidAxisData_s {
     float I;
     float D;
     float F;
+    float S;
 
     float Sum;
 } pidAxisData_t;

src/test/unit/pid_unittest.cc

@@ -114,10 +114,10 @@ void setDefaultTestSettings(void)
 {
     pgResetAll();
     pidProfile = pidProfilesMutable(1);
-    pidProfile->pid[PID_ROLL]  =  { 40, 40, 30, 65 };
+    pidProfile->pid[PID_ROLL]  =  { 40, 40, 30, 65, 0 };
-    pidProfile->pid[PID_PITCH] =  { 58, 50, 35, 60 };
+    pidProfile->pid[PID_PITCH] =  { 58, 50, 35, 60, 0 };
-    pidProfile->pid[PID_YAW]   =  { 70, 45, 20, 60 };
+    pidProfile->pid[PID_YAW]   =  { 70, 45, 20, 60, 0 };
-    pidProfile->pid[PID_LEVEL] =  { 50, 50, 75, 50 };
+    pidProfile->pid[PID_LEVEL] =  { 50, 50, 75, 50, 0 };
 
     // Compensate for the upscaling done without 'use_integrated_yaw'
     pidProfile->pid[PID_YAW].I = pidProfile->pid[PID_YAW].I / 2.5f;
@@ -183,6 +183,7 @@ void resetTest(void)
         pidData[axis].I = 0;
         pidData[axis].D = 0;
         pidData[axis].F = 0;
+        pidData[axis].S = 0;
         pidData[axis].Sum = 0;
         simulatedSetpointRate[axis] = 0;
         simulatedRcDeflection[axis] = 0;