mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 03:50:02 +03:00
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
This commit is contained in:
ctzsnooze
parent
4fa4d4851e
commit
6aaaf727ff
13 changed files with 92 additions and 160 deletions
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue