diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9b39237324..2f93f7f1e4 100644 --- a/src/main/blackbox/blackbox.c +++ b/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); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 66c94e1366..b523888442 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -118,4 +118,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "MAG_TASK_RATE", "EZLANDING", "TPA", + "S_TERM", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 2feb381ad6..311b79c007 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4d71995280..a4a0896ee3 100644 --- a/src/main/cli/settings.c +++ b/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) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 898b6f9395..4689a2c502 100644 --- a/src/main/fc/parameter_names.h +++ b/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" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 116c9a2549..3660acd408 100644 --- a/src/main/flight/pid.c +++ b/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_MAG] = { 40, 0, 0, 0 }, + [PID_LEVEL] = { 50, 75, 75, 50, 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; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e20725e754..87fd886694 100644 --- a/src/main/flight/pid.h +++ b/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_PITCH_DEFAULT { 47, 84, 46, 125 } -#define PID_YAW_DEFAULT { 45, 80, 0, 120 } +#define PID_ROLL_DEFAULT { 45, 80, 40, 120, 0 } +#define PID_PITCH_DEFAULT { 47, 84, 46, 125, 0 } +#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; diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 05942b6729..943b6e4028 100644 --- a/src/test/unit/pid_unittest.cc +++ b/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_PITCH] = { 58, 50, 35, 60 }; - pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 }; - pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 50 }; + pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65, 0 }; + pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60, 0 }; + pidProfile->pid[PID_YAW] = { 70, 45, 20, 60, 0 }; + 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;