From b4803697d2ae2eb8e40f59613b4eef882b46066c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 07:56:26 +0100 Subject: [PATCH 1/7] Initial port of Harakiri PID controller some of the settings are hardcoded --- src/main/flight/flight.c | 120 +++++++++++++++++++++++++++++++++++++++ src/main/io/serial_cli.c | 2 +- 2 files changed, 121 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index cf2e66c3ed..35cb64b3a8 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -43,6 +43,7 @@ extern uint16_t cycleTime; extern uint8_t motorCount; +extern uint32_t currentTime; int16_t heading, magHold; int16_t axisPID[3]; @@ -506,6 +507,122 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } +int32_t SpecialIntegerRoundUp(float val) // If neg value just represents a change in direction rounding to next higher number is "more" negative +{ + if (val > 0) return val + 0.5f; + else if (val < 0) return val - 0.5f; + else return 0; +} + +#define RCconstPI 0.159154943092f // 0.5f / M_PI; +#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. +#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) + +static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, +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 tmp0flt; + static uint32_t previousTime; + int32_t tmp0, PTermYW; + uint8_t axis; + static int32_t errorGyroI_YW = 0; + float ACCDeltaTimeINS = 0; + float FLOATcycleTime = 0; + +// 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 + FLOATcycleTime = (float)constrain(currentTime - previousTime, 1, 100000); // 1us - 100ms + previousTime = currentTime; + ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now + RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element + + if(FLIGHT_MODE(HORIZON_MODE)) prop = (float)MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 450) / 450.0f; + for (axis = 0; axis < 2; axis++) { + rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers + if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { + error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; + 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; + } + if (!FLIGHT_MODE(ANGLE_MODE)) { + if (ABS((int16_t)gyroData[axis]) > 2560) errorGyroI[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); + } + ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f; + if (FLIGHT_MODE(HORIZON_MODE)) { + PTerm = PTermACC + prop * (rcCommandAxis - PTermACC); + ITerm = ITermACC + prop * (ITermGYRO - ITermACC); + } + else { + PTerm = rcCommandAxis; + ITerm = ITermGYRO; + } + } + else { + PTerm = PTermACC; + ITerm = ITermACC; + } + PTerm -= gyroData[axis] * dynP8[axis] * 0.003f; + delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS; + lastGyro[axis] = gyroData[axis]; + lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); + DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; + axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. + + #ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } + + tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter + tmp0flt /= 3000.0f; + if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded for now + PTermYW = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; + tmp0 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); + axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTermYW / 80; + if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) + errorGyroI_YW = 0; + else { + error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; + errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp + axisPID[FD_YAW] += (errorGyroI_YW / 125 * pidProfile->I8[FD_YAW]) >> 6; + } + } + else { + tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; + error = tmp0 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better + if (ABS(tmp0) > 50) errorGyroI_YW = 0; + else errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + axisPID[FD_YAW] = constrain(errorGyroI_YW >> 13, -GYRO_I_MAX, +GYRO_I_MAX); + PTermYW = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; + if(motorCount > 3) { // 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]; + PTermYW = constrain(PTermYW, -tmp0, tmp0); + } + axisPID[FD_YAW] += PTermYW; + } + axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. + +#ifdef BLACKBOX + axisPID_P[FD_YAW] = PTerm; + axisPID_I[FD_YAW] = ITerm; + axisPID_D[FD_YAW] = DTerm; +#endif +} + static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -613,6 +730,9 @@ void setPIDController(int type) break; case 4: pid_controller = pidMultiWiiHybrid; + break; + case 5: + pid_controller = pidHarakiri; } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 47737b7a73..2ec7e39f4b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -370,7 +370,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 4 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, From ef749e822ae9923cd71c74be595ff62c4bd25076 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 08:45:37 +0100 Subject: [PATCH 2/7] Harakiri PID controller code cleanup and documentation --- docs/PID tuning.md | 16 ++++++++++++++++ src/main/flight/flight.c | 31 +++++++++++++++---------------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 0ebfa00e3d..ab2414d3c5 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -121,3 +121,19 @@ PID Controller 4 is an hybrid version of two MultiWii PID controllers. Roll and This PID controller was initialy implemented for testing purposes but is also performing quite well. For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This will provide best performance on very small multicopters with brushed motors. + +### PID controller 5, "Harakiri" + +PID Controller 5 is an port of the PID controller from the Harakiri firmware. + +The algorithm is leveraging more floating point math. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: + + OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. + MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) + +The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. + +Yaw authority is also quite good. Only the P value is used in the yaw algorithm. + + + diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 35cb64b3a8..e6467dc3da 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -528,9 +528,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0}; float tmp0flt; static uint32_t previousTime; - int32_t tmp0, PTermYW; + int32_t tmp0; uint8_t axis; - static int32_t errorGyroI_YW = 0; float ACCDeltaTimeINS = 0; float FLOATcycleTime = 0; @@ -579,7 +578,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. - #ifdef BLACKBOX +#ifdef BLACKBOX axisPID_P[axis] = PTerm; axisPID_I[axis] = ITerm; axisPID_D[axis] = -DTerm; @@ -589,37 +588,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter tmp0flt /= 3000.0f; if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded for now - PTermYW = ((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 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); - axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTermYW / 80; + axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTerm / 80; if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) - errorGyroI_YW = 0; + errorGyroI[FD_YAW] = 0; else { error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0; - errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp - axisPID[FD_YAW] += (errorGyroI_YW / 125 * pidProfile->I8[FD_YAW]) >> 6; + errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp + axisPID[FD_YAW] += (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 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better - if (ABS(tmp0) > 50) errorGyroI_YW = 0; - else errorGyroI_YW = constrain(errorGyroI_YW + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); - axisPID[FD_YAW] = constrain(errorGyroI_YW >> 13, -GYRO_I_MAX, +GYRO_I_MAX); - PTermYW = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; + if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0; + else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); + axisPID[FD_YAW] = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); + PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; if(motorCount > 3) { // 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]; - PTermYW = constrain(PTermYW, -tmp0, tmp0); + PTerm = constrain(PTerm, -tmp0, tmp0); } - axisPID[FD_YAW] += PTermYW; + axisPID[FD_YAW] += PTerm; } axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = ITerm; - axisPID_D[FD_YAW] = DTerm; + axisPID_I[FD_YAW] = 0; + axisPID_D[FD_YAW] = 0; #endif } From a5c3c266866aa4e66781f4906c94f28b5088854f Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 09:23:50 +0100 Subject: [PATCH 3/7] Fix problem for non GPS targets and add code for autotuning --- src/main/flight/flight.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index e6467dc3da..460376bcde 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -544,7 +544,17 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) for (axis = 0; axis < 2; axis++) { rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { +#ifdef GPS error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#else + error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +#endif + +#ifdef AUTOTUNE + if (shouldAutotune()) { + error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error))); + } +#endif PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f; tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f; PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt); From db8d539cbb5a617657e351b4050255490ed88c68 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 10:42:41 +0100 Subject: [PATCH 4/7] Another documentation upddate for the Harakiri PID controller --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index ab2414d3c5..9d8a4ffcdb 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -126,7 +126,7 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller is also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) From 124ae1f590516c1dfb72bc474400b3bc3f6beebf Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 14:13:04 +0100 Subject: [PATCH 5/7] Finalize Blackbox yaw output for Harakiri PID controller Minor code updates and cleanup Documentation update --- docs/PID tuning.md | 4 ++-- src/main/flight/flight.c | 37 +++++++++++++------------------------ 2 files changed, 15 insertions(+), 26 deletions(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 9d8a4ffcdb..3d2900bbdd 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -126,14 +126,14 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This PID Controller 5 is an port of the PID controller from the Harakiri firmware. -The algorithm is leveraging more floating point math. This PID controller is also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: +The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don’t need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri: OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly. -Yaw authority is also quite good. Only the P value is used in the yaw algorithm. +Yaw authority is also quite good. diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 460376bcde..97334700c9 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -43,7 +43,6 @@ extern uint16_t cycleTime; extern uint8_t motorCount; -extern uint32_t currentTime; int16_t heading, magHold; int16_t axisPID[3]; @@ -507,16 +506,9 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } -int32_t SpecialIntegerRoundUp(float val) // If neg value just represents a change in direction rounding to next higher number is "more" negative -{ - if (val > 0) return val + 0.5f; - else if (val < 0) return val - 0.5f; - else return 0; -} - #define RCconstPI 0.159154943092f // 0.5f / M_PI; -#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. #define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) +#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -527,7 +519,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) 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 tmp0flt; - static uint32_t previousTime; int32_t tmp0; uint8_t axis; float ACCDeltaTimeINS = 0; @@ -535,8 +526,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) // 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 - FLOATcycleTime = (float)constrain(currentTime - previousTime, 1, 100000); // 1us - 100ms - previousTime = currentTime; + FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element @@ -586,7 +576,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) lastGyro[axis] = gyroData[axis]; lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]); DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f; - axisPID[axis] = SpecialIntegerRoundUp(PTerm + ITerm - DTerm); // Round up result. + axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result. #ifdef BLACKBOX axisPID_P[axis] = PTerm; @@ -597,37 +587,36 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter tmp0flt /= 3000.0f; - if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardrcoded 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; - tmp0 = SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); - axisPID[FD_YAW] = rcCommand[FD_YAW] - tmp0 * PTerm / 80; - if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) - errorGyroI[FD_YAW] = 0; + tmp0 = lrintf(gyroData[FD_YAW] * 0.25f); + PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80; + if ((ABS(tmp0) > 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 - axisPID[FD_YAW] += (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; + 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 - SpecialIntegerRoundUp(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better + error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better if (ABS(tmp0) > 50) errorGyroI[FD_YAW] = 0; else errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454); - axisPID[FD_YAW] = 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; if(motorCount > 3) { // 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); } - axisPID[FD_YAW] += PTerm; } - axisPID[FD_YAW] = SpecialIntegerRoundUp(axisPID[FD_YAW]); // Round up result. + axisPID[FD_YAW] = PTerm + ITerm; + axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. #ifdef BLACKBOX axisPID_P[FD_YAW] = PTerm; - axisPID_I[FD_YAW] = 0; + axisPID_I[FD_YAW] = ITerm; axisPID_D[FD_YAW] = 0; #endif } From 56176abe66e2f0b242942a0e128dd29c69d6458c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 27 Jan 2015 14:42:49 +0100 Subject: [PATCH 6/7] Fix potential overflow. --- src/main/flight/flight.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 97334700c9..2f72ee7873 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -585,7 +585,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) #endif } - tmp0flt = (uint16_t)FLOATcycleTime & (uint16_t)0xFFFC; // Filter last 2 bit jitter + tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter tmp0flt /= 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; From 25676e944a61fe51aec13560b9b9dbe141f3180e Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 28 Jan 2015 15:21:24 +0100 Subject: [PATCH 7/7] Update PID controller documentation --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 3d2900bbdd..fd17c1c9b8 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -31,7 +31,7 @@ strength of the correction to be backed off in order to avoid overshooting the t ## PID controllers -Cleanflight has 5 built-in PID controllers which each have different flight behavior. Each controller requires +Cleanflight has 6 built-in PID controllers which each have different flight behavior. Each controller requires different PID settings for best performance, so if you tune your craft using one PID controller, those settings will likely not work well on any of the other controllers.