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

Add Back Iterm limit for saturation scenarios

Browse source
This commit is contained in:
borisbstyle 2016-06-01 20:58:00 +02:00
parent 704c09cced
commit a3c1f6e168
4 changed files with 27 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,7 @@
#include "config/runtime_config.h"
extern uint8_t motorCount;
extern bool motorLimitReached;
uint32_t targetPidLooptime;
int16_t axisPID[3];
@ -60,9 +61,9 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3];
static int32_t errorGyroI[3], errorGyroILimit[3];
#ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3];
static float errorGyroIf[3], errorGyroIfLimit[3];;
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
@ -215,6 +216,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// 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
ITerm = errorGyroIf[axis];
@ -341,6 +348,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term