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

PID1 TPA Implementation

This commit is contained in:
borisbstyle 2015-05-08 15:20:53 +02:00
parent 69292fc966
commit 7f69537f86
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,7 +55,7 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
uint8_t dynP8[3], dynI8[3], dynD8[3];
uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@ -700,13 +700,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
RateError = AngleRateTmp - (gyroData[axis] / 4);
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis]) >> 7;
PTerm = (RateError * redP8[axis]) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// 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.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * redI8[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
@ -724,7 +724,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * pidProfile->D8[axis]) >> 8;
DTerm = (deltaSum * redD8[axis]) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;

7
src/main/mw.c
View File

@ -100,7 +100,7 @@ int16_t headFreeModeHold;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
extern uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3];
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -218,6 +218,11 @@ void annexCode(void)
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction used in PID controller 1
redP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop2 / 100;
redI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop2 / 100;
redD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop2 / 100;
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
}