Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00
Code Issues Wiki Activity

S-term (for wings) (#13679)

Browse source 
* S-term for wings

* tabs

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

* pidData[axis].S

---------

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
This commit is contained in:
Ivan Efimov 2024-06-20 06:01:13 -04:00 • committed by GitHub
parent 00ca8232d5
commit e2e1a68ba0
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
8 changed files with 51 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/blackbox/blackbox.c
View file

@ -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);

1
src/main/build/debug.c
View file

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

1
src/main/build/debug.h
View file

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

6
src/main/cli/settings.c
View file

@ -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) },

3
src/main/fc/parameter_names.h
View file

@ -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"

26
src/main/flight/pid.c
View file

@ -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;
}

8
src/main/flight/pid.h
View file

@ -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;

9
src/test/unit/pid_unittest.cc
View file

@ -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;