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

Merge pull request #526 from MJ666/Harakiri_PID

Browse source 
Fix for Harakiri PID drift.
This commit is contained in:
Dominic Clifton 2015-02-19 21:57:19 +00:00
commit 242a7e65a8
1 changed files with 38 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -60,6 +60,7 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 }; static int32_t errorAngleI[2] = { 0, 0 };
static float errorAngleIf[2] = { 0.0f, 0.0f };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -73,6 +74,9 @@ void resetErrorAngle(void)
{ {
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0; errorAngleI[PITCH] = 0;
errorAngleIf[ROLL] = 0.0f;
errorAngleIf[PITCH] = 0.0f;
} }
void resetErrorGyro(void) void resetErrorGyro(void)
@ -81,9 +85,9 @@ void resetErrorGyro(void)
errorGyroI[PITCH] = 0; errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0; errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0; errorGyroIf[ROLL] = 0.0f;
errorGyroIf[PITCH] = 0; errorGyroIf[PITCH] = 0.0f;
errorGyroIf[YAW] = 0; errorGyroIf[YAW] = 0.0f;
} }
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
@ -518,14 +522,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
float delta, RCfactor, rcCommandAxis, MainDptCut; float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0; float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
float tmp0flt;
int32_t tmp0;
uint8_t axis; uint8_t axis;
float ACCDeltaTimeINS = 0; float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
float FLOATcycleTime = 0;
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D // MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
@ -538,6 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
} }
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS #ifdef GPS
@ -552,21 +555,21 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
} }
#endif #endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); PTermACC = constrain(PTermACC, -limitf, +limitf);
errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
} }
if (!FLIGHT_MODE(ANGLE_MODE)) { if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroData[axis]) > 2560) { if (ABS((int16_t)gyroData[axis]) > 2560) {
errorGyroI[axis] = 0.0f; errorGyroIf[axis] = 0.0f;
} else { } else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant;
errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
} }
ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f; ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f;
if (FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
@ -580,10 +583,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
ITerm = ITermACC; ITerm = ITermACC;
} }
PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroData[axis]; lastGyro[axis] = gyroDataQuant;
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
@ -596,37 +599,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
#endif #endif
} }
tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
tmp0flt /= 3000.0f; Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
tmp0 = lrintf(gyroData[FD_YAW] * 0.25f); int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80; PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0; errorGyroI[FD_YAW] = 0;
} else { } else {
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
} }
} else { } else {
tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp0) > 50) { if (ABS(tmp) > 50) {
errorGyroI[FD_YAW] = 0; errorGyroI[FD_YAW] = 0;
} else { } else {
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454);
} }
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
tmp0 = 300; int32_t limit = 300;
if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -tmp0, tmp0); PTerm = constrain(PTerm, -limit, limit);
} }
} }
axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = PTerm + ITerm;