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

Setpoint PID attenuation (for wings) (#13719)

Browse source 
* Setpoint PID attenuation

* Suggestions from ledvinap's review
This commit is contained in:
Ivan Efimov 2024-06-25 16:38:43 -05:00 • committed by GitHub
parent 8eec0fe8d5
commit 2ceb8e0417
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
11 changed files with 137 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

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

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

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

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

1
src/main/cli/settings.h
View file

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

23
src/main/common/maths.c
View file

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

2
src/main/common/maths.h
View file

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

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

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

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

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

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

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

3
src/main/flight/pid_init.c
View file

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