From 2b05cbf9165e42e8c6244ef2aff1a831e02b5bf3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 391 ++----------------------------- src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 279 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 511 insertions(+), 376 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c6..68364d7b7 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f91875e2..068012ad5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,32 +47,25 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[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]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// 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]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +87,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +97,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,361 +120,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; - } - previousSetpoint[axis] = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----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. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // 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 - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} - void pidSetController(pidControllerType_e type) { switch (type) { @@ -493,4 +133,3 @@ void pidSetController(pidControllerType_e type) #endif } } - diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c12404ec5..56a89e938 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -118,6 +118,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 000000000..db07f11bb --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,279 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// 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]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + } + previousSetpoint[axis] = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 000000000..0424f5461 --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----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. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // 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 + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +