Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 07:45:29 +03:00
Code Issues Wiki Activity

Add ITerm anti-windup based on motor output saturation

Browse source 
Added noise threshold for PID ITerm accumulation

Removed ITerm setpoint scaler. Added CLI and MSP settings

Made default ITerm noise threshold zero. Added CLI setting.

Removed itermWindupPointPercent from MSP
This commit is contained in:
Martin Budden 2017-01-26 18:15:29 +00:00 • committed by borisbstyle
parent 990c13b7ea
commit 9dfb3e45ee
8 changed files with 39 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -34,6 +34,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "sensors/gyro.h"
@ -146,7 +147,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, 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;
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -161,6 +163,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f;
}
static float calcHorizonLevelStrength(void) {
@ -238,15 +242,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
// -----calculate I component
// Reduce strong Iterm accumulation during higher stick inputs
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f);
float ITerm = previousGyroIf[axis];
ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator;
// limit maximum integrator value to prevent WindUp
ITerm = constrainf(ITerm, -250.0f, 250.0f);
previousGyroIf[axis] = ITerm;
const float motorMixRangeAbs = ABS(getMotorMixRange());
if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) {
// 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;
// gradually scale back integration when above windup point (default is 75%)
if (motorMixRangeAbs > ITermWindupPoint) {
ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint);
}
ITerm += ITermDelta;
// also limit maximum integrator value to prevent windup
ITerm = constrainf(ITerm, -250.0f, 250.0f);
previousGyroIf[axis] = ITerm;
}
// -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0;