src/main/flight/pid.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include <platform.h>
  23
  24 #include "build/build_config.h"
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "fc/fc_core.h"
  32 #include "fc/fc_rc.h"
  33
  34 #include "fc/rc_controls.h"
  35 #include "fc/runtime_config.h"
  36
  37 #include "flight/pid.h"
  38 #include "flight/imu.h"
  39 #include "flight/mixer.h"
  40 #include "flight/navigation.h"
  41
  42 #include "sensors/gyro.h"
  43 #include "sensors/acceleration.h"
  44
  45 uint32_t targetPidLooptime;
  46 static bool pidStabilisationEnabled;
  47
  48 float axisPIDf[3];
  49
  50 #ifdef BLACKBOX
  51 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  52 #endif
  53
  54 static float previousGyroIf[3];
  55
  56 static float dT;
  57
  58 void pidSetTargetLooptime(uint32_t pidLooptime)
  59 {
  60     targetPidLooptime = pidLooptime;
  61     dT = (float)targetPidLooptime * 0.000001f;
  62 }
  63
  64 void pidResetErrorGyroState(void)
  65 {
  66     for (int axis = 0; axis < 3; axis++) {
  67         previousGyroIf[axis] = 0.0f;
  68     }
  69 }
  70
  71 static float itermAccelerator = 1.0f;
  72
  73 void pidSetItermAccelerator(float newItermAccelerator) {
  74     itermAccelerator = newItermAccelerator;
  75 }
  76
  77 void pidStabilisationState(pidStabilisationState_e pidControllerState)
  78 {
  79     pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
  80 }
  81
  82 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
  83
  84 static filterApplyFnPtr dtermNotchFilterApplyFn;
  85 static void *dtermFilterNotch[2];
  86 static filterApplyFnPtr dtermLpfApplyFn;
  87 static void *dtermFilterLpf[2];
  88 static filterApplyFnPtr ptermYawFilterApplyFn;
  89 static void *ptermYawFilter;
  90
  91 void pidInitFilters(const pidProfile_t *pidProfile)
  92 {
  93     static biquadFilter_t biquadFilterNotch[2];
  94     static pt1Filter_t pt1Filter[2];
  95     static biquadFilter_t biquadFilter[2];
  96     static firFilterDenoise_t denoisingFilter[2];
  97     static pt1Filter_t pt1FilterYaw;
  98
  99     BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
 100
 101     if (pidProfile->dterm_notch_hz == 0) {
 102         dtermNotchFilterApplyFn = nullFilterApply;
 103     } else {
 104         dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
 105         const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
 106         for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
 107             dtermFilterNotch[axis] = &biquadFilterNotch[axis];
 108             biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
 109         }
 110     }
 111
 112     if (pidProfile->dterm_lpf_hz == 0) {
 113         dtermLpfApplyFn = nullFilterApply;
 114     } else {
 115         switch (pidProfile->dterm_filter_type) {
 116         default:
 117             dtermLpfApplyFn = nullFilterApply;
 118             break;
 119         case FILTER_PT1:
 120             dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply;
 121             for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
 122                 dtermFilterLpf[axis] = &pt1Filter[axis];
 123                 pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT);
 124             }
 125             break;
 126         case FILTER_BIQUAD:
 127             dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
 128             for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
 129                 dtermFilterLpf[axis] = &biquadFilter[axis];
 130                 biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
 131             }
 132             break;
 133         case FILTER_FIR:
 134             dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
 135             for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
 136                 dtermFilterLpf[axis] = &denoisingFilter[axis];
 137                 firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
 138             }
 139             break;
 140         }
 141     }
 142
 143     if (pidProfile->yaw_lpf_hz == 0) {
 144         ptermYawFilterApplyFn = nullFilterApply;
 145     } else {
 146         ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
 147         ptermYawFilter = &pt1FilterYaw;
 148         pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, dT);
 149     }
 150 }
 151
 152 static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
 153 static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
 154
 155 void pidInitConfig(const pidProfile_t *pidProfile) {
 156     for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 157         Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
 158         Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
 159         Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
 160         c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
 161         relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
 162     }
 163     levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
 164     horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
 165     horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
 166     maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
 167     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
 168     ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
 169     ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
 170     itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
 171 }
 172
 173 static float calcHorizonLevelStrength(void) {
 174     float horizonLevelStrength = 0.0f;
 175     if (horizonTransition > 0.0f) {
 176         const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
 177         // Progressively turn off the horizon self level strength as the stick is banged over
 178         horizonLevelStrength = constrainf(1 - mostDeflectedPos * horizonTransition, 0, 1);
 179     }
 180     return horizonLevelStrength;
 181 }
 182
 183 static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
 184     // calculate error angle and limit the angle to the max inclination
 185     float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
 186 #ifdef GPS
 187     errorAngle += GPS_angle[axis];
 188 #endif
 189     errorAngle = constrainf(errorAngle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
 190     errorAngle = (errorAngle - ((attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f));
 191     if(FLIGHT_MODE(ANGLE_MODE)) {
 192         // ANGLE mode - control is angle based, so control loop is needed
 193         currentPidSetpoint = errorAngle * levelGain;
 194     } else {
 195         // HORIZON mode - direct sticks control is applied to rate PID
 196         // mix up angle error to desired AngleRate to add a little auto-level feel
 197         const float horizonLevelStrength = calcHorizonLevelStrength();
 198         currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength);
 199     }
 200     return currentPidSetpoint;
 201 }
 202
 203 static float accelerationLimit(int axis, float currentPidSetpoint) {
 204     static float previousSetpoint[3];
 205     const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];
 206
 207     if(ABS(currentVelocity) > maxVelocity[axis])
 208         currentPidSetpoint = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity[axis] : previousSetpoint[axis] - maxVelocity[axis];
 209
 210     previousSetpoint[axis] = currentPidSetpoint;
 211     return currentPidSetpoint;
 212 }
 213
 214 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 215 // Based on 2DOF reference design (matlab)
 216 void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim)
 217 {
 218     static float previousRateError[2];
 219     const float tpaFactor = getThrottlePIDAttenuation();
 220     const float motorMixRange = getMotorMixRange();
 221
 222     // Dynamic ki component to gradually scale back integration when above windup point
 223     float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
 224
 225     // ----------PID controller----------
 226     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 227         float currentPidSetpoint = getSetpointRate(axis);
 228
 229         if(maxVelocity[axis])
 230             currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
 231
 232         // Yaw control is GYRO based, direct sticks control is applied to rate PID
 233         if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
 234             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
 235         }
 236
 237         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
 238
 239         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
 240         //  ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error).  ----------
 241
 242         // -----calculate error rate
 243         const float errorRate = currentPidSetpoint - gyroRate;       // r - y
 244
 245         // -----calculate P component and add Dynamic Part based on stick input
 246         float PTerm = Kp[axis] * errorRate * tpaFactor;
 247         if (axis == FD_YAW) {
 248             PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
 249         }
 250
 251         // -----calculate I component
 252         float ITerm = previousGyroIf[axis];
 253         if (motorMixRange < 1.0f) {
 254             // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
 255             // Iterm will only be accelerated below steady rate threshold
 256             if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
 257                 dynKi *= itermAccelerator;
 258             float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
 259             ITerm += ITermDelta;
 260             previousGyroIf[axis] = ITerm;
 261         }
 262
 263         // -----calculate D component (Yaw D not yet supported)
 264         float DTerm = 0.0;
 265         if (axis != FD_YAW) {
 266             float dynC = c[axis];
 267             if (pidProfile->setpointRelaxRatio < 100) {
 268                 dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f);
 269             }
 270             const float rD = dynC * currentPidSetpoint - gyroRate;    // cr - y
 271             // Divide rate change by dT to get differential (ie dr/dt)
 272             const float delta = (rD - previousRateError[axis]) / dT;
 273             previousRateError[axis] = rD;
 274
 275             DTerm = Kd[axis] * delta * tpaFactor;
 276             DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
 277
 278             // apply filters
 279             DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
 280             DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
 281
 282         }
 283
 284         // -----calculate total PID output
 285         axisPIDf[axis] = PTerm + ITerm + DTerm;
 286         // Disable PID control at zero throttle
 287         if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
 288
 289 #ifdef BLACKBOX
 290         axisPID_P[axis] = PTerm;
 291         axisPID_I[axis] = ITerm;
 292         axisPID_D[axis] = DTerm;
 293 #endif
 294     }
 295 }