From 2ceb8e041730dedd10f986e7f68c7777a06e44b3 Mon Sep 17 00:00:00 2001 From: Ivan Efimov Date: Tue, 25 Jun 2024 16:38:43 -0500 Subject: [PATCH] Setpoint PID attenuation (for wings) (#13719) * Setpoint PID attenuation * Suggestions from ledvinap's review --- src/main/blackbox/blackbox.c | 12 ++++++++ src/main/build/debug.c | 3 +- src/main/build/debug.h | 1 + src/main/cli/settings.c | 17 +++++++++++ src/main/cli/settings.h | 1 + src/main/common/maths.c | 23 ++++++++++++++ src/main/common/maths.h | 2 ++ src/main/fc/parameter_names.h | 9 ++++++ src/main/flight/pid.c | 56 ++++++++++++++++++++++++++++++++++- src/main/flight/pid.h | 12 ++++++++ src/main/flight/pid_init.c | 3 ++ 11 files changed, 137 insertions(+), 2 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2f93f7f1e4..0aaa77c5c6 100644 --- a/src/main/blackbox/blackbox.c +++ b/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]); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index b523888442..b1f8f61df7 100644 --- a/src/main/build/debug.c +++ b/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", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 311b79c007..6c44485d28 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -121,6 +121,7 @@ typedef enum { DEBUG_EZLANDING, DEBUG_TPA, DEBUG_S_TERM, + DEBUG_SPA, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index d73ccd4e23..4d5cd72b43 100644 --- a/src/main/cli/settings.c +++ b/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) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index fef52dea48..dc45e46398 100644 --- a/src/main/cli/settings.h +++ b/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, diff --git a/src/main/common/maths.c b/src/main/common/maths.c index c0327454fd..93c62d660f 100644 --- a/src/main/common/maths.c +++ b/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); + } +} diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 26941a685f..bf6b7039dd 100644 --- a/src/main/common/maths.h +++ b/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) diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 4689a2c502..3f3ac448c5 100644 --- a/src/main/fc/parameter_names.h +++ b/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" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 365d032ecf..704af23bb5 100644 --- a/src/main/flight/pid.c +++ b/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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 49dcb868d2..0fb26825b8 100644 --- a/src/main/flight/pid.h +++ b/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; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 9ff1007831..0fe69925bd 100644 --- a/src/main/flight/pid_init.c +++ b/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 }