From 6c92ea8ee834be87f8f56c4f12953c5b4ecc5869 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 18 Feb 2015 22:55:05 +0100 Subject: [PATCH 1/4] Harakiri PID fix Change errorGyroI and errorAngleI from int32 to float --- src/main/flight/pid.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fccab9ab56..0abc12eff9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -60,6 +60,7 @@ uint8_t dynP8[3], dynI8[3], dynD8[3]; static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static int32_t errorAngleI[2] = { 0, 0 }; +static float errorAngleIf[2] = { 0.0f, 0.0f }; static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); @@ -73,6 +74,9 @@ void resetErrorAngle(void) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; + + errorAngleIf[ROLL] = 0; + errorAngleIf[PITCH] = 0; } void resetErrorGyro(void) @@ -554,19 +558,19 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); - errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); - ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; + errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); + ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; } if (!FLIGHT_MODE(ANGLE_MODE)) { if (ABS((int16_t)gyroData[axis]) > 2560) { - errorGyroI[axis] = 0.0f; + errorGyroIf[axis] = 0.0f; } else { error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; - 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)) { PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); From 6548c90ca80e4c4e527fcab6bda9f5426445bc90 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 08:30:41 +0100 Subject: [PATCH 2/4] Align Harakiri PID with Crashpilot1000 updates --- src/main/flight/pid.c | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0abc12eff9..c63d858894 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -75,8 +75,8 @@ void resetErrorAngle(void) errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - errorAngleIf[ROLL] = 0; - errorAngleIf[PITCH] = 0; + errorAngleIf[ROLL] = 0.0f; + errorAngleIf[PITCH] = 0.0f; } void resetErrorGyro(void) @@ -85,9 +85,9 @@ void resetErrorGyro(void) errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; - errorGyroIf[ROLL] = 0; - errorGyroIf[PITCH] = 0; - errorGyroIf[YAW] = 0; + errorGyroIf[ROLL] = 0.0f; + errorGyroIf[PITCH] = 0.0f; + errorGyroIf[YAW] = 0.0f; } const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; @@ -523,13 +523,14 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) UNUSED(rxConfig); float delta, RCfactor, rcCommandAxis, MainDptCut; - float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0; - static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; + float PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; + static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; + float gyroDataQuant[2] = { 0.0f, 0.0f }; float tmp0flt; int32_t tmp0; uint8_t axis; - float ACCDeltaTimeINS = 0; - float FLOATcycleTime = 0; + float ACCDeltaTimeINS = 0.0f; + float FLOATcycleTime = 0.0f; // 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 @@ -542,6 +543,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } for (axis = 0; axis < 2; axis++) { + tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea + gyroDataQuant[axis] = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS @@ -566,7 +569,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) if (ABS((int16_t)gyroData[axis]) > 2560) { errorGyroIf[axis] = 0.0f; } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis]; + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant[axis]; errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } @@ -584,10 +587,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = ITermACC; } - PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; - delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + PTerm -= gyroDataQuant[axis] * dynP8[axis] * 0.003f; + delta = (gyroDataQuant[axis] - lastGyro[axis]) / ACCDeltaTimeINS; - lastGyro[axis] = gyroData[axis]; + lastGyro[axis] = gyroDataQuant[axis]; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; From cd94377651c8f298ae575ea825e5b3107d3e22e1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 09:00:20 +0100 Subject: [PATCH 3/4] Latest Crashpilot1000 update --- src/main/flight/pid.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c63d858894..42cfe7f9cb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -522,10 +522,9 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { UNUSED(rxConfig); - float delta, RCfactor, rcCommandAxis, MainDptCut; + float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant; float PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; - float gyroDataQuant[2] = { 0.0f, 0.0f }; float tmp0flt; int32_t tmp0; uint8_t axis; @@ -544,7 +543,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) for (axis = 0; axis < 2; axis++) { tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroDataQuant[axis] = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight + gyroDataQuant = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS @@ -569,7 +568,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) if (ABS((int16_t)gyroData[axis]) > 2560) { errorGyroIf[axis] = 0.0f; } else { - error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant[axis]; + error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant; errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f); } @@ -587,10 +586,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = ITermACC; } - PTerm -= gyroDataQuant[axis] * dynP8[axis] * 0.003f; - delta = (gyroDataQuant[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + PTerm -= gyroDataQuant * dynP8[axis] * 0.003f; + delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS; - lastGyro[axis] = gyroDataQuant[axis]; + lastGyro[axis] = gyroDataQuant; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; From 257c7e092e3a3554a59323e09c4b4453269c1900 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 19 Feb 2015 21:28:26 +0100 Subject: [PATCH 4/4] Harakiri PID controller variables cleanup Flight tested --- src/main/flight/pid.c | 43 ++++++++++++++++++++----------------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 42cfe7f9cb..d123250484 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -523,13 +523,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) UNUSED(rxConfig); float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant; - float PTerm = 0.0f, ITerm = 0.0f, DTerm = 0.0f, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO = 0.0f, error = 0.0f, prop = 0.0f; + float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f; static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f }; - float tmp0flt; - int32_t tmp0; uint8_t axis; - float ACCDeltaTimeINS = 0.0f; - float FLOATcycleTime = 0.0f; + float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; // 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 @@ -542,8 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } for (axis = 0; axis < 2; axis++) { - tmp0 = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea - gyroDataQuant = (float)tmp0 * 3.2f; // but delivers more accuracy and also reduces jittery flight + 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 if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { #ifdef GPS @@ -558,8 +555,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } #endif PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; - tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; - PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); + float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f; + PTermACC = constrain(PTermACC, -limitf, +limitf); errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f); ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f; } @@ -602,37 +599,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) #endif } - tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter - tmp0flt /= 3000.0f; + Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter + Mwii3msTimescale /= 3000.0f; 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; - tmp0 = lrintf(gyroData[FD_YAW] * 0.25f); - PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80; - if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { + int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f); + PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; + if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { errorGyroI[FD_YAW] = 0; } else { - error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; - errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp + error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } } else { - tmp0 = ((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 + int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; + 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; } 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); 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 - tmp0 = 300; - if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW]; - PTerm = constrain(PTerm, -tmp0, tmp0); + int32_t limit = 300; + if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW]; + PTerm = constrain(PTerm, -limit, limit); } } axisPID[FD_YAW] = PTerm + ITerm;