mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +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
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) },
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue