Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Cleanup and optimize new anti windup // Simplify relaxFactor to Dterm

Browse source
This commit is contained in:
borisbstyle 2017-01-29 02:15:57 +01:00
parent 9dfb3e45ee
commit ad892400e5
4 changed files with 11 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/fc/cli.c
View file

@ -707,8 +707,7 @@ const clivalue_t valueTable[] = {
{ "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } },
{ "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {30, 100 } },
{ "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } },

3
src/main/fc/config.c
View file

@ -169,8 +169,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->pidSumLimit = PIDSUM_LIMIT;
pidProfile->yaw_lpf_hz = 0; pidProfile->yaw_lpf_hz = 0;
pidProfile->itermWindupPointPercent = 75; pidProfile->itermWindupPointPercent = 50;
pidProfile->itermNoiseThreshold = 0;
pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_filter_type = FILTER_BIQUAD;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->dterm_notch_hz = 260; pidProfile->dterm_notch_hz = 260;

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

@ -148,7 +148,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermNoiseThresholdDps; static float levelGain, horizonGain, horizonTransition, ITermWindupPoint;
void pidInitConfig(const pidProfile_t *pidProfile) { void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -164,7 +164,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f;
} }
static float calcHorizonLevelStrength(void) { static float calcHorizonLevelStrength(void) {
@ -215,6 +214,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
static float previousRateError[2]; static float previousRateError[2];
static float previousSetpoint[3]; static float previousSetpoint[3];
const float motorMixRange = getMotorMixRange();
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
@ -243,14 +244,12 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component // -----calculate I component
float ITerm = previousGyroIf[axis]; float ITerm = previousGyroIf[axis];
const float motorMixRangeAbs = ABS(getMotorMixRange()); if (motorMixRange < 1.0f) {
if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) {
// Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
// Reduce strong Iterm accumulation during higher stick inputs
float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator;
// gradually scale back integration when above windup point (default is 75%) // gradually scale back integration when above windup point
if (motorMixRangeAbs > ITermWindupPoint) { if (motorMixRange > ITermWindupPoint) {
ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint); ITermDelta *= (1.0f - motorMixRange) * ITermWindupPoint;
} }
ITerm += ITermDelta; ITerm += ITermDelta;
// also limit maximum integrator value to prevent windup // also limit maximum integrator value to prevent windup
@ -267,10 +266,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
dynC = c[axis]; dynC = c[axis];
if (currentPidSetpoint > 0) { if (currentPidSetpoint > 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis]) if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis])
dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); dynC *= (1.0f - rcDeflection) * relaxFactor[axis];
} else if (currentPidSetpoint < 0) { } else if (currentPidSetpoint < 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis]) if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis])
dynC = dynC * sq(rcDeflection) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]); dynC *= (1.0f - rcDeflection) * relaxFactor[axis];
} }
} }
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y

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

@ -67,7 +67,6 @@ typedef struct pidProfile_s {
uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation
uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold
uint16_t yaw_p_limit; uint16_t yaw_p_limit;
float pidSumLimit; float pidSumLimit;
uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t dterm_average_count; // Configurable delta count for dterm