rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Seperate float and int accumulation logic

This commit is contained in:
borisbstyle 2016-07-17 03:16:04 +02:00
parent 9cac1c9a8a
commit 19a814afbb
2 changed files with 11 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/config/config.c
View File

@ -220,8 +220,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 180;
pidProfile->yawItermIgnoreRate = 35;
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 50;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->dynamic_pid = 1;

29
src/main/flight/pid.c
View File

@ -76,20 +76,6 @@ void setTargetPidLooptime(uint8_t pidProcessDenom)
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
{
uint16_t dynamicKi;
uint16_t resetRate;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi;
}
void pidResetErrorGyroState(void)
{
int axis;
@ -195,9 +181,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat
}
// -----calculate I component.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
// Prevent strong Iterm accumulation during stick inputs
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -330,11 +318,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
// Prevent Accumulation
uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint16_t dynamicFactor = (1 << 8) - constrain(((ABS((int32_t)angleRate) << 6) / resetRate), 0, 1 << 8);
uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
int32_t rateErrorLimited = errorLimiter * RateError;
errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI;
errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings