From 6aaaf727ff86f66f863ef4a314e02f973c9c34ce Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 20 Jun 2022 08:55:31 +1000 Subject: [PATCH] update antigravity for 4.4, no boost on yaw Update to anti-gravity, including removal of the old Step mode, ability to adjust the P contribution (thanks @Limon), PT2 smoothed derivative model, inherent limiting of P boost during extremely fast stick travels to minimise P oscillations, less I during the middle of a throttle up, no I boost on yaw, add hz to cutoff labels No antigravity on yaw, fix longstanding typo h --- src/main/blackbox/blackbox.c | 7 +- src/main/cli/settings.c | 14 ++-- src/main/cli/settings.h | 1 - src/main/cms/cms_menu_imu.c | 12 ++-- src/main/fc/parameter_names.h | 4 +- src/main/fc/rc.c | 35 ---------- src/main/flight/pid.c | 103 ++++++++++++++-------------- src/main/flight/pid.h | 30 ++++---- src/main/flight/pid_init.c | 23 ++----- src/main/msp/msp.c | 14 ++-- src/main/msp/msp_box.c | 2 +- src/main/target/ALIENWHOOP/config.c | 3 +- src/test/unit/pid_unittest.cc | 4 +- 13 files changed, 92 insertions(+), 160 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5cf7f4be05..00bdbe4a21 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1379,9 +1379,10 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_AT_MIN_THROTTLE, "%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_MODE, "%d", currentPidProfile->antiGravityMode); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_THRESHOLD, "%d", currentPidProfile->itermThrottleThreshold); - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_GAIN, "%d", currentPidProfile->itermAcceleratorGain); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_GAIN, "%d", currentPidProfile->anti_gravity_gain); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_CUTOFF_HZ, "%d", currentPidProfile->anti_gravity_cutoff_hz); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_P_GAIN, "%d", currentPidProfile->anti_gravity_p_gain); + #ifdef USE_ABSOLUTE_CONTROL BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ABS_CONTROL_GAIN, "%d", currentPidProfile->abs_control_gain); #endif diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9e25b8c0c3..ef6a7b43f6 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -312,11 +312,6 @@ static const char * const lookupTableDtermLowpassType[] = { "PT3", }; -static const char * const lookupTableAntiGravityMode[] = { - "SMOOTH", - "STEP", -}; - static const char * const lookupTableFailsafe[] = { "AUTO-LAND", "DROP", "GPS-RESCUE" }; @@ -571,7 +566,6 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol), LOOKUP_TABLE_ENTRY(lookupTableLowpassType), LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType), - LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode), LOOKUP_TABLE_ENTRY(lookupTableFailsafe), LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode), LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery), @@ -1083,10 +1077,10 @@ const clivalue_t valueTable[] = { #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) { PARAM_NAME_VBAT_SAG_COMPENSATION, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, vbat_sag_compensation) }, #endif - { PARAM_NAME_PID_AT_MIN_THROTTLE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, - { PARAM_NAME_ANTI_GRAVITY_MODE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) }, - { PARAM_NAME_ANTI_GRAVITY_THRESHOLD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, - { PARAM_NAME_ANTI_GRAVITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, + { PARAM_NAME_PID_AT_MIN_THROTTLE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, + { PARAM_NAME_ANTI_GRAVITY_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, anti_gravity_gain) }, + { PARAM_NAME_ANTI_GRAVITY_CUTOFF_HZ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 2, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, anti_gravity_cutoff_hz) }, + { PARAM_NAME_ANTI_GRAVITY_P_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, anti_gravity_p_gain) }, { PARAM_NAME_ACC_LIMIT_YAW, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { PARAM_NAME_ACC_LIMIT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 10, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 3bf0e292a8..c0a5e07a56 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -66,7 +66,6 @@ typedef enum { TABLE_MOTOR_PWM_PROTOCOL, TABLE_GYRO_LPF_TYPE, TABLE_DTERM_LPF_TYPE, - TABLE_ANTI_GRAVITY_MODE, TABLE_FAILSAFE, TABLE_FAILSAFE_SWITCH_MODE, TABLE_CRASH_RECOVERY, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index bdd0e3569b..ee9af3a6e2 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -503,8 +503,7 @@ static uint8_t cmsx_horizonTransition; static uint8_t cmsx_levelAngleLimit; static uint8_t cmsx_throttleBoost; static uint8_t cmsx_thrustLinearization; -static uint16_t cmsx_itermAcceleratorGain; -static uint16_t cmsx_itermThrottleThreshold; +static uint16_t cmsx_antiGravityGain; static uint8_t cmsx_motorOutputLimit; static int8_t cmsx_autoProfileCellCount; #ifdef USE_D_MIN @@ -544,8 +543,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp) cmsx_horizonTransition = pidProfile->pid[PID_LEVEL].D; cmsx_levelAngleLimit = pidProfile->levelAngleLimit; - cmsx_itermAcceleratorGain = pidProfile->itermAcceleratorGain; - cmsx_itermThrottleThreshold = pidProfile->itermThrottleThreshold; + cmsx_antiGravityGain = pidProfile->anti_gravity_gain; cmsx_throttleBoost = pidProfile->throttle_boost; cmsx_thrustLinearization = pidProfile->thrustLinearization; @@ -594,8 +592,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry pidProfile->pid[PID_LEVEL].D = cmsx_horizonTransition; pidProfile->levelAngleLimit = cmsx_levelAngleLimit; - pidProfile->itermAcceleratorGain = cmsx_itermAcceleratorGain; - pidProfile->itermThrottleThreshold = cmsx_itermThrottleThreshold; + pidProfile->anti_gravity_gain = cmsx_antiGravityGain; pidProfile->throttle_boost = cmsx_throttleBoost; pidProfile->thrustLinearization = cmsx_thrustLinearization; @@ -647,8 +644,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } }, { "ANGLE LIMIT", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_levelAngleLimit, 10, 90, 1 } }, - { "AG GAIN", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermAcceleratorGain, ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 10 } }, - { "AG THR", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_itermThrottleThreshold, 20, 1000, 1 } }, + { "AG GAIN", OME_UINT8, NULL, &(OSD_UINT16_t) { &cmsx_antiGravityGain, ITERM_ACCELERATOR_GAIN_OFF, ITERM_ACCELERATOR_GAIN_MAX, 10 } }, #ifdef USE_THROTTLE_BOOST { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_throttleBoost, 0, 100, 1 } }, #endif diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index f351895ca1..4ae140b6bd 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -68,9 +68,9 @@ #define PARAM_NAME_DTERM_NOTCH_CUTOFF "dterm_notch_cutoff" #define PARAM_NAME_VBAT_SAG_COMPENSATION "vbat_sag_compensation" #define PARAM_NAME_PID_AT_MIN_THROTTLE "pid_at_min_throttle" -#define PARAM_NAME_ANTI_GRAVITY_MODE "anti_gravity_mode" -#define PARAM_NAME_ANTI_GRAVITY_THRESHOLD "anti_gravity_threshold" #define PARAM_NAME_ANTI_GRAVITY_GAIN "anti_gravity_gain" +#define PARAM_NAME_ANTI_GRAVITY_CUTOFF_HZ "anti_gravity_cutoff_hz" +#define PARAM_NAME_ANTI_GRAVITY_P_GAIN "anti_gravity_p_gain" #define PARAM_NAME_ACC_LIMIT_YAW "acc_limit_yaw" #define PARAM_NAME_ACC_LIMIT "acc_limit" #define PARAM_NAME_ITERM_RELAX "iterm_relax" diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 3589de9c21..7ddd563cd0 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -265,34 +265,6 @@ static void scaleRawSetpointToFpvCamAngle(void) rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, -SETPOINT_RATE_LIMIT * 1.0f, SETPOINT_RATE_LIMIT * 1.0f); } -#define THROTTLE_BUFFER_MAX 20 -#define THROTTLE_DELTA_MS 100 - -static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) -{ - static int index; - static int16_t rcCommandThrottlePrevious[THROTTLE_BUFFER_MAX]; - - const int rxRefreshRateMs = rxRefreshRate / 1000; - const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); - const int16_t throttleVelocityThreshold = (featureIsEnabled(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; - - rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; - if (index >= indexMax) { - index = 0; - } - - const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index]; - - if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) { - if (ABS(rcCommandSpeed) > throttleVelocityThreshold) { - pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain)); - } else { - pidSetItermAccelerator(0.0f); - } - } -} - void updateRcRefreshRate(timeUs_t currentTimeUs) { static timeUs_t lastRxTimeUs; @@ -570,13 +542,6 @@ FAST_CODE void processRcCommand(void) { if (isRxDataNew) { newRxDataForFF = true; - } - - if (isRxDataNew && pidAntiGravityEnabled()) { - checkForThrottleErrorResetState(currentRxRefreshRate); - } - - if (isRxDataNew) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { #ifdef USE_FEEDFORWARD diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b6a3c5baca..7714b521a4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -117,7 +117,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4); void resetPidProfile(pidProfile_t *pidProfile) { @@ -140,8 +140,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .feedforward_transition = 0, .yawRateAccelLimit = 0, .rateAccelLimit = 0, - .itermThrottleThreshold = 250, - .itermAcceleratorGain = 3500, + .anti_gravity_gain = 80, .crash_time = 500, // ms .crash_delay = 0, // ms .crash_recovery_angle = 10, // degrees @@ -168,7 +167,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .abs_control_limit = 90, .abs_control_error_limit = 20, .abs_control_cutoff = 11, - .antiGravityMode = ANTI_GRAVITY_SMOOTH, .dterm_lpf1_static_hz = DTERM_LPF1_DYN_MIN_HZ_DEFAULT, // NOTE: dynamic lpf is enabled by default so this setting is actually // overridden and the static lowpass 1 is disabled. We can't set this @@ -218,6 +216,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .simplified_pitch_pi_gain = SIMPLIFIED_TUNING_DEFAULT, .simplified_dterm_filter = true, .simplified_dterm_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT, + .anti_gravity_cutoff_hz = 5, + .anti_gravity_p_gain = 100, ); #ifndef USE_D_MIN @@ -303,25 +303,23 @@ void pidUpdateTpaFactor(float throttle) void pidUpdateAntiGravityThrottleFilter(float throttle) { - if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) { - // calculate a boost factor for P in the same way as for I when throttle changes quickly - const float antiGravityThrottleLpf = pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle); - // focus P boost on low throttle range only - if (throttle < 0.5f) { - pidRuntime.antiGravityPBoost = 0.5f - throttle; - } else { - pidRuntime.antiGravityPBoost = 0.0f; - } - // use lowpass to identify start of a throttle up, use this to reduce boost at start by half - if (antiGravityThrottleLpf < throttle) { - pidRuntime.antiGravityPBoost *= 0.5f; - } - // high-passed throttle focuses boost on faster throttle changes - pidRuntime.antiGravityThrottleHpf = fabsf(throttle - antiGravityThrottleLpf); - pidRuntime.antiGravityPBoost = pidRuntime.antiGravityPBoost * pidRuntime.antiGravityThrottleHpf; - // smooth the P boost at 3hz to remove the jagged edges and prolong the effect after throttle stops - pidRuntime.antiGravityPBoost = pt1FilterApply(&pidRuntime.antiGravitySmoothLpf, pidRuntime.antiGravityPBoost); + static float previousThrottle = 0.0f; + const float throttleInv = 1.0f - throttle; + float throttleDerivative = fabsf(throttle - previousThrottle) * pidRuntime.pidFrequency; + DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(throttleDerivative * 100)); + throttleDerivative *= throttleInv * throttleInv; + // generally focus on the low throttle period + if (throttle > previousThrottle) { + throttleDerivative *= throttleInv * 0.5f; + // when increasing throttle, focus even more on the low throttle range } + previousThrottle = throttle; + throttleDerivative = pt2FilterApply(&pidRuntime.antiGravityLpf, throttleDerivative); + // lower cutoff suppresses peaks relative to troughs and prolongs the effects + // PT2 smoothing of throttle derivative. + // 6 is a typical value for the peak boost factor with default cutoff of 6Hz + DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(throttleDerivative * 100)); + pidRuntime.antiGravityThrottleD = throttleDerivative; } #ifdef USE_ACRO_TRAINER @@ -874,32 +872,27 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim UNUSED(currentTimeUs); #endif - - // Dynamic i component, - if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) { - // traditional itermAccelerator factor for iTerm - pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain; - DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.itermAccelerator * 1000)); - // users AG Gain changes P boost - pidRuntime.antiGravityPBoost *= pidRuntime.itermAcceleratorGain; - // add some percentage of that slower, longer acting P boost factor to prolong AG effect on iTerm - pidRuntime.itermAccelerator += pidRuntime.antiGravityPBoost * 0.05f; - // set the final P boost amount - pidRuntime.antiGravityPBoost *= 0.02f; + // Anti Gravity + if (pidRuntime.antiGravityEnabled) { + pidRuntime.antiGravityThrottleD *= pidRuntime.antiGravityGain; + // used later to increase pTerm + pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleD * ANTIGRAVITY_KI; } else { - pidRuntime.antiGravityPBoost = 0.0f; + pidRuntime.antiGravityThrottleD = 0.0f; + pidRuntime.itermAccelerator = 0.0f; } - DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000)); + DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf((1 + (pidRuntime.itermAccelerator / pidRuntime.pidCoefficient[FD_PITCH].Ki)) * 1000)); + // amount of antigravity added relative to user's pitch iTerm coefficient + pidRuntime.itermAccelerator *= pidRuntime.dT; + // used later to increase iTerm - const float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI; - - // gradually scale back integration when above windup point + // iTerm windup (attenuation of iTerm if motorMix range is large) float dynCi = pidRuntime.dT; if (pidRuntime.itermWindupPointInv > 1.0f) { dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f); } - // Precalculate gyro deta for D-term here, this allows loop unrolling + // Precalculate gyro delta for D-term here, this allows loop unrolling float gyroRateDterm[XYZ_AXIS_COUNT]; for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { gyroRateDterm[axis] = gyro.gyroADCf[axis]; @@ -1010,8 +1003,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } // -----calculate I component - float Ki; - float axisDynCi; + float Ki = pidRuntime.pidCoefficient[axis].Ki; + float axisDynCi = pidRuntime.dT; #ifdef USE_LAUNCH_CONTROL // if launch control is active override the iterm gains and apply iterm windup protection to all axes if (launchControlActive) { @@ -1020,11 +1013,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } else #endif { - Ki = pidRuntime.pidCoefficient[axis].Ki; - axisDynCi = (axis == FD_YAW) ? dynCi : pidRuntime.dT; // only apply windup protection to yaw + if (axis == FD_YAW) { + axisDynCi = dynCi; // only apply windup protection to yaw + pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm + } } - pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + agGain) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit); + pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + pidRuntime.itermAccelerator) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit); // -----calculate pidSetpointDelta float pidSetpointDelta = 0; @@ -1149,18 +1144,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } } #endif - // calculating the PID sum - // P boost at the end of throttle chop - // attenuate effect if turning more than 50 deg/s, half at 100 deg/s - float agBoostAttenuator = fabsf(currentPidSetpoint) / 50.0f; - agBoostAttenuator = MAX(agBoostAttenuator, 1.0f); - const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator); + // Add P boost from antiGravity when sticks are close to zero if (axis != FD_YAW) { - pidData[axis].P *= agBoost; - DEBUG_SET(DEBUG_ANTI_GRAVITY, axis - FD_ROLL + 2, lrintf(agBoost * 1000)); + float agSetpointAttenuator = fabsf(currentPidSetpoint) / 50.0f; + agSetpointAttenuator = MAX(agSetpointAttenuator, 1.0f); + // attenuate effect if turning more than 50 deg/s, half at 100 deg/s + const float antiGravityPBoost = 1.0f + (pidRuntime.antiGravityThrottleD / agSetpointAttenuator) * pidRuntime.antiGravityPGain; + pidData[axis].P *= antiGravityPBoost; + if (axis == FD_PITCH) { + DEBUG_SET(DEBUG_ANTI_GRAVITY, 3, lrintf(antiGravityPBoost * 1000)); + } } + // calculating the PID sum const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; #ifdef USE_INTEGRATED_YAW_CONTROL if (axis == FD_YAW && pidRuntime.useIntegratedYaw) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3bf7f092fa..ca1a936023 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -54,10 +54,11 @@ #define ITERM_RELAX_CUTOFF_DEFAULT 15 // Anti gravity I constant -#define AG_KI 21.586988f; - +#define ANTIGRAVITY_KI 0.34f; // if AG gain is 6, about 6 times iTerm will be added +#define ANTIGRAVITY_KP 0.0034f; // one fifth of the I gain on P by default #define ITERM_ACCELERATOR_GAIN_OFF 0 -#define ITERM_ACCELERATOR_GAIN_MAX 30000 +#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 } @@ -101,11 +102,6 @@ typedef struct pidf_s { uint16_t F; } pidf_t; -typedef enum { - ANTI_GRAVITY_SMOOTH, - ANTI_GRAVITY_STEP -} antiGravityMode_e; - typedef enum { ITERM_RELAX_OFF, ITERM_RELAX_RP, @@ -149,9 +145,7 @@ typedef struct pidProfile_s { uint8_t horizon_tilt_expert_mode; // OFF or ON // Betaflight PID controller parameters - uint8_t antiGravityMode; // type of anti gravity method - uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms - uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit + uint8_t anti_gravity_gain; // AntiGravity Gain (was itermAcceleratorGain) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value @@ -226,6 +220,9 @@ typedef struct pidProfile_s { uint8_t simplified_dterm_filter; uint8_t simplified_dterm_filter_multiplier; uint8_t simplified_pitch_pi_gain; + + uint8_t anti_gravity_cutoff_hz; + uint8_t anti_gravity_p_gain; } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -279,14 +276,12 @@ typedef struct pidRuntime_s { filterApplyFnPtr ptermYawLowpassApplyFn; pt1Filter_t ptermYawLowpass; bool antiGravityEnabled; - uint8_t antiGravityMode; - pt1Filter_t antiGravityThrottleLpf; - pt1Filter_t antiGravitySmoothLpf; + pt2Filter_t antiGravityLpf; float antiGravityOsdCutoff; - float antiGravityThrottleHpf; - float antiGravityPBoost; + float antiGravityThrottleD; float itermAccelerator; - uint16_t itermAcceleratorGain; + uint8_t antiGravityGain; + float antiGravityPGain; pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT]; float levelGain; float horizonGain; @@ -419,7 +414,6 @@ void pidSetAcroTrainerState(bool newState); void pidUpdateTpaFactor(float throttle); void pidUpdateAntiGravityThrottleFilter(float throttle); bool pidOsdAntiGravityActive(void); -bool pidOsdAntiGravityMode(void); void pidSetAntiGravityState(bool newState); bool pidAntiGravityEnabled(void); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index b3170f5d23..658cb9f7b1 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -52,8 +52,6 @@ #define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f #endif -#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff -#define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3 // The anti gravity P smoothing filter cutoff #define ATTITUDE_CUTOFF_HZ 250 static void pidSetTargetLooptime(uint32_t pidLooptime) @@ -237,15 +235,14 @@ void pidInitFilters(const pidProfile_t *pidProfile) } #endif - pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT)); - pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT)); - #ifdef USE_ACC const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT); for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only pt3FilterInit(&pidRuntime.attitudeFilter[axis], k); } #endif + + pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT)); } void pidInit(const pidProfile_t *pidProfile) @@ -307,7 +304,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f; pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint); } - pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain; + pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain; pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000; pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000; pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10; @@ -321,17 +318,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) throttleBoost = pidProfile->throttle_boost * 0.1f; #endif pidRuntime.itermRotation = pidProfile->iterm_rotation; - pidRuntime.antiGravityMode = pidProfile->antiGravityMode; - // Calculate the anti-gravity value that will trigger the OSD display. - // For classic AG it's either 1.0 for off and > 1.0 for on. - // For the new AG it's a continuous floating value so we want to trigger the OSD - // display when it exceeds 25% of its possible range. This gives a useful indication - // of AG activity without excessive display. - pidRuntime.antiGravityOsdCutoff = 0.0f; - if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) { - pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f; - } + // Calculate the anti-gravity value that will trigger the OSD display when its strength exceeds 25% of max. + // This gives a useful indication of AG activity without excessive display. + pidRuntime.antiGravityOsdCutoff = (pidRuntime.antiGravityGain / 10.0f) * 0.25f; + pidRuntime.antiGravityPGain = ((float)(pidProfile->anti_gravity_p_gain) / 100.0f) * ANTIGRAVITY_KP; #if defined(USE_ITERM_RELAX) pidRuntime.itermRelax = pidProfile->iterm_relax; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 25caec362e..0f1a3e1bfb 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1884,8 +1884,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, currentPidProfile->yawRateAccelLimit); sbufWriteU8(dst, currentPidProfile->levelAngleLimit); sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity - sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); - sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); + sbufWriteU16(dst, 0); // was currentPidProfile->itermThrottleThreshold + sbufWriteU16(dst, currentPidProfile->anti_gravity_gain); sbufWriteU16(dst, 0); // was currentPidProfile->dtermSetpointWeight sbufWriteU8(dst, currentPidProfile->iterm_rotation); sbufWriteU8(dst, 0); // was currentPidProfile->smart_feedforward @@ -1914,8 +1914,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, currentPidProfile->pid[PID_ROLL].F); sbufWriteU16(dst, currentPidProfile->pid[PID_PITCH].F); sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F); - - sbufWriteU8(dst, currentPidProfile->antiGravityMode); + sbufWriteU8(dst, 0); // was currentPidProfile->antiGravityMode #if defined(USE_D_MIN) sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]); sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]); @@ -2975,8 +2974,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); // was pidProfile.levelSensitivity } if (sbufBytesRemaining(src) >= 4) { - currentPidProfile->itermThrottleThreshold = sbufReadU16(src); - currentPidProfile->itermAcceleratorGain = sbufReadU16(src); + sbufReadU16(src); // was currentPidProfile->itermAcceleratorGain + currentPidProfile->anti_gravity_gain = sbufReadU16(src); } if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); // was currentPidProfile->dtermSetpointWeight @@ -3011,8 +3010,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, currentPidProfile->pid[PID_ROLL].F = sbufReadU16(src); currentPidProfile->pid[PID_PITCH].F = sbufReadU16(src); currentPidProfile->pid[PID_YAW].F = sbufReadU16(src); - - currentPidProfile->antiGravityMode = sbufReadU8(src); + sbufReadU8(src); // was currentPidProfile->antiGravityMode } if (sbufBytesRemaining(src) >= 7) { // Added in MSP API 1.41 diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index 8a95798a5e..165558675b 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -192,7 +192,7 @@ void initActiveBoxIds(void) bool acceleratorGainsEnabled = false; for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) { - if (pidProfiles(i)->itermAcceleratorGain != ITERM_ACCELERATOR_GAIN_OFF) { + if (pidProfiles(i)->anti_gravity_gain != ITERM_ACCELERATOR_GAIN_OFF) { acceleratorGainsEnabled = true; } } diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index dd20b6af9c..71f720b998 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -134,8 +134,7 @@ void targetConfiguration(void) pidProfile->feedforward_transition = 0; /* Anti-Gravity */ - pidProfile->itermThrottleThreshold = 500; - pidProfile->itermAcceleratorGain = 5000; + pidProfile->anti_gravity_gain = 35; pidProfile->levelAngleLimit = 65; } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index caf14fe9dc..f267eddfd2 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -143,9 +143,7 @@ void setDefaultTestSettings(void) { pidProfile->feedforward_transition = 100; pidProfile->yawRateAccelLimit = 100; pidProfile->rateAccelLimit = 0; - pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH; - pidProfile->itermThrottleThreshold = 250; - pidProfile->itermAcceleratorGain = 1000; + pidProfile->anti_gravity_gain = 10; pidProfile->crash_time = 500; pidProfile->crash_delay = 0; pidProfile->crash_recovery_angle = 10;