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_config.h"
  25 #include "debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "drivers/sensor.h"
  32 #include "drivers/gyro_sync.h"
  33
  34 #include "drivers/accgyro.h"
  35 #include "sensors/sensors.h"
  36 #include "sensors/gyro.h"
  37 #include "sensors/acceleration.h"
  38
  39 #include "rx/rx.h"
  40
  41 #include "io/rc_controls.h"
  42 #include "io/gps.h"
  43
  44 #include "flight/pid.h"
  45 #include "flight/imu.h"
  46 #include "flight/navigation.h"
  47
  48
  49 #include "config/runtime_config.h"
  50
  51 extern uint8_t motorCount;
  52 extern float dT;
  53 extern bool motorLimitReached;
  54
  55 int16_t axisPID[3];
  56
  57 #ifdef BLACKBOX
  58 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  59 #endif
  60
  61 #define DELTA_MAX_SAMPLES 12
  62
  63 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
  64 uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
  65
  66 static int32_t errorGyroI[3], errorGyroILimit[3];
  67 static float errorGyroIf[3], errorGyroIfLimit[3];
  68 static int32_t errorAngleI[2];
  69 static float errorAngleIf[2];
  70 static bool lowThrottlePidReduction;
  71
  72 static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
  73         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
  74
  75 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
  76         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
  77
  78 pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
  79
  80 void pidResetErrorAngle(void)
  81 {
  82     errorAngleI[ROLL] = 0;
  83     errorAngleI[PITCH] = 0;
  84
  85     errorAngleIf[ROLL] = 0.0f;
  86     errorAngleIf[PITCH] = 0.0f;
  87 }
  88
  89 void pidResetErrorGyroState(uint8_t resetOption)
  90 {
  91     if (resetOption >= RESET_ITERM) {
  92         int axis;
  93         for (axis = 0; axis < 3; axis++) {
  94             errorGyroI[axis] = 0;
  95             errorGyroIf[axis] = 0.0f;
  96         }
  97     }
  98
  99     if (resetOption == RESET_ITERM_AND_REDUCE_PID) {
 100         lowThrottlePidReduction = true;
 101     } else {
 102         lowThrottlePidReduction = false;
 103     }
 104 }
 105
 106 void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
 107     float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
 108     static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
 109     static float antiWindUpIncrement = 0;
 110
 111     if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime;  // Calculate increment for 500ms period
 112
 113     if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) {   /* scaling should not happen in level modes */
 114         /* Reset Iterm on high stick inputs. No scaling necessary here */
 115         iTermScaler[axis] = 0.0f;
 116         errorGyroI[axis] = 0;
 117         errorGyroIf[axis] = 0.0f;
 118     } else {
 119         /* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms  */
 120         if (iTermScaler[axis] < 1) {
 121             iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
 122             if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) {
 123                 errorGyroI[axis] *= iTermScaler[axis];
 124             } else {
 125                 errorGyroIf[axis] *= iTermScaler[axis];
 126             }
 127         }
 128     }
 129 }
 130
 131 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 132
 133 static biquad_t deltaBiQuadState[3];
 134 static bool deltaStateIsSet;
 135
 136 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
 137         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 138 {
 139     float RateError, AngleRate, gyroRate;
 140     float ITerm,PTerm,DTerm;
 141     static float lastErrorForDelta[3];
 142     static float previousDelta[3][DELTA_MAX_SAMPLES];
 143     float delta, deltaSum;
 144     int axis, deltaCount;
 145     float horizonLevelStrength = 1;
 146
 147     float dT = (float)targetLooptime * 0.000001f;
 148
 149     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
 150         for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
 151         deltaStateIsSet = true;
 152     }
 153
 154     if (FLIGHT_MODE(HORIZON_MODE)) {
 155         // Figure out the raw stick positions
 156         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
 157         const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
 158         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
 159         // Progressively turn off the horizon self level strength as the stick is banged over
 160         horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection
 161         if(pidProfile->H_sensitivity == 0){
 162             horizonLevelStrength = 0;
 163         } else {
 164             horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1);
 165         }
 166     }
 167
 168     // ----------PID controller----------
 169     for (axis = 0; axis < 3; axis++) {
 170         uint8_t rate = controlRateConfig->rates[axis];
 171
 172         if (axis == FD_YAW) {
 173             // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
 174             AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f;
 175          } else {
 176              // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
 177              AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max roll/pitch rate
 178              if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 179                 // calculate error angle and limit the angle to the max inclination
 180 #ifdef GPS
 181                 const float errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 182                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
 183 #else
 184                 const float errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
 185                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
 186 #endif
 187                 if (FLIGHT_MODE(ANGLE_MODE)) {
 188                     // ANGLE mode - control is angle based, so control loop is needed
 189                     AngleRate = errorAngle * pidProfile->A_level;
 190                 } else {
 191                     // HORIZON mode - direct sticks control is applied to rate PID
 192                     // mix up angle error to desired AngleRate to add a little auto-level feel
 193                     AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength;
 194                 }
 195             }
 196         }
 197
 198         gyroRate = gyroADC[axis] * gyro.scale; // gyro output scaled to dps
 199
 200         // --------low-level gyro-based PID. ----------
 201         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
 202         // -----calculate scaled error.AngleRates
 203         // multiplication of rcCommand corresponds to changing the sticks scaling here
 204         RateError = AngleRate - gyroRate;
 205
 206         // -----calculate P component
 207         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 208
 209         // -----calculate I component.
 210         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 211
 212         if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
 213             scaleItermToRcInput(axis, pidProfile);
 214             if (antiWindupProtection || motorLimitReached) {
 215                 errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
 216             } else {
 217                 errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
 218             }
 219         }
 220
 221         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 222         // I coefficient (I8) moved before integration to make limiting independent from PID settings
 223         ITerm = errorGyroIf[axis];
 224
 225         //-----calculate D-term
 226         if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
 227             delta = RateError - lastErrorForDelta[axis];
 228             lastErrorForDelta[axis] = RateError;
 229         } else {
 230             delta = -(gyroRate - lastErrorForDelta[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
 231             lastErrorForDelta[axis] = gyroRate;
 232         }
 233
 234         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
 235         // would be scaled by different dt each time. Division by dT fixes that.
 236         delta *= (1.0f / dT);
 237
 238         if (deltaStateIsSet) {
 239             delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
 240         } else {
 241             // Apply moving average
 242             deltaSum = 0;
 243             for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
 244             previousDelta[axis][0] = delta;
 245             for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
 246             delta = (deltaSum / pidProfile->dterm_average_count);
 247         }
 248
 249         DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 250
 251         // -----calculate total PID output
 252         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
 253
 254         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 255
 256 #ifdef BLACKBOX
 257         axisPID_P[axis] = PTerm;
 258         axisPID_I[axis] = ITerm;
 259         axisPID_D[axis] = DTerm;
 260 #endif
 261     }
 262 }
 263
 264 static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
 265             rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 266 {
 267     UNUSED(rxConfig);
 268
 269     int axis, deltaCount, prop = 0;
 270     int32_t rc, error, errorAngle, delta, gyroError;
 271     int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
 272     static int16_t lastErrorForDelta[2];
 273     static int32_t previousDelta[2][DELTA_MAX_SAMPLES];
 274
 275     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
 276         for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
 277         deltaStateIsSet = true;
 278     }
 279
 280     if (FLIGHT_MODE(HORIZON_MODE)) {
 281         prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
 282     }
 283
 284     // PITCH & ROLL
 285     for (axis = 0; axis < 2; axis++) {
 286
 287         rc = rcCommand[axis] << 1;
 288
 289         gyroError = gyroADC[axis] / 4;
 290
 291         error = rc - gyroError;
 292         errorGyroI[axis]  = constrain(errorGyroI[axis] + error, -16000, +16000);   // WindUp   16 bits is ok here
 293
 294         if (ABS(gyroADC[axis]) > (640 * 4)) {
 295             errorGyroI[axis] = 0;
 296         }
 297
 298         // Anti windup protection
 299         if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
 300             scaleItermToRcInput(axis, pidProfile);
 301             if (antiWindupProtection || motorLimitReached) {
 302                 errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
 303             } else {
 304                 errorGyroILimit[axis] = ABS(errorGyroI[axis]);
 305             }
 306         }
 307
 308         ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6;   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
 309
 310         PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6;
 311
 312         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {   // axis relying on ACC
 313             // 50 degrees max inclination
 314 #ifdef GPS
 315             errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 316                 +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 317 #else
 318             errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
 319                 +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 320 #endif
 321
 322             errorAngleI[axis]  = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);                                                // WindUp     //16 bits is ok here
 323
 324             PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7;   // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result
 325
 326             int16_t limit = pidProfile->D8[PIDLEVEL] * 5;
 327             PTermACC = constrain(PTermACC, -limit, +limit);
 328
 329             ITermACC = ((int32_t)errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;  // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
 330
 331             ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
 332             PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
 333         }
 334
 335         PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6;   // 32 bits is needed for calculation
 336
 337         //-----calculate D-term based on the configured approach (delta from measurement or deltafromError)
 338         if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
 339             delta = error - lastErrorForDelta[axis];
 340             lastErrorForDelta[axis] = error;
 341         } else {                                       /* Delta from measurement */
 342             delta = -(gyroError - lastErrorForDelta[axis]);
 343             lastErrorForDelta[axis] = gyroError;
 344         }
 345
 346         if (deltaStateIsSet) {
 347             DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3;  // Keep same scaling as unfiltered delta
 348         } else {
 349             // Apply moving average
 350             DTerm = 0;
 351             for (deltaCount = pidProfile->dterm_average_count-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
 352             previousDelta[axis][0] = delta;
 353             for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) DTerm += previousDelta[axis][deltaCount];
 354             delta = (DTerm / pidProfile->dterm_average_count) * 3;  // Keep same original scaling
 355         }
 356
 357         DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
 358
 359         axisPID[axis] = PTerm + ITerm + DTerm;
 360
 361         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 362
 363
 364 #ifdef BLACKBOX
 365         axisPID_P[axis] = PTerm;
 366         axisPID_I[axis] = ITerm;
 367         axisPID_D[axis] = DTerm;
 368 #endif
 369     }
 370
 371     //YAW
 372     rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30)  >> 5;
 373 #ifdef ALIENWII32
 374     error = rc - gyroADC[FD_YAW];
 375 #else
 376     error = rc - (gyroADC[FD_YAW] / 4);
 377 #endif
 378     errorGyroI[FD_YAW]  += (int32_t)error * pidProfile->I8[FD_YAW];
 379     errorGyroI[FD_YAW]  = constrain(errorGyroI[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
 380     if (ABS(rc) > 50) errorGyroI[FD_YAW] = 0;
 381
 382     PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
 383
 384     // Constrain YAW by D value if not servo driven in that case servolimits apply
 385     if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
 386         PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 387     }
 388
 389     ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
 390
 391     axisPID[FD_YAW] =  PTerm + ITerm;
 392
 393     if (lowThrottlePidReduction) axisPID[FD_YAW] /= 4;
 394
 395 #ifdef BLACKBOX
 396     axisPID_P[FD_YAW] = PTerm;
 397     axisPID_I[FD_YAW] = ITerm;
 398     axisPID_D[FD_YAW] = 0;
 399 #endif
 400 }
 401
 402 static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
 403         rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 404 {
 405     UNUSED(rxConfig);
 406
 407     int axis, deltaCount;
 408     int32_t PTerm, ITerm, DTerm, delta, deltaSum;
 409     static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
 410     static int32_t previousDelta[3][DELTA_MAX_SAMPLES];
 411     int32_t AngleRateTmp, RateError, gyroRate;
 412
 413     int8_t horizonLevelStrength = 100;
 414
 415     if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
 416         for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
 417         deltaStateIsSet = true;
 418     }
 419
 420     if (FLIGHT_MODE(HORIZON_MODE)) {
 421         // Figure out the raw stick positions
 422         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
 423         const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
 424         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
 425         // Progressively turn off the horizon self level strength as the stick is banged over
 426         horizonLevelStrength = (500 - mostDeflectedPos) / 5;  // 100 at centre stick, 0 = max stick deflection
 427         // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
 428         // For more rate mode increase D and slower flips and rolls will be possible
 429         horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
 430     }
 431
 432     // ----------PID controller----------
 433     for (axis = 0; axis < 3; axis++) {
 434         uint8_t rate = controlRateConfig->rates[axis];
 435
 436         // -----Get the desired angle rate depending on flight mode
 437         if (axis == FD_YAW) {
 438             // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
 439             AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[YAW]) >> 5;
 440         } else {
 441             AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
 442             if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 443                 // calculate error angle and limit the angle to max configured inclination
 444 #ifdef GPS
 445                 const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 446                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 447 #else
 448                 const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
 449                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 450 #endif
 451                 if (FLIGHT_MODE(ANGLE_MODE)) {
 452                     // ANGLE mode - control is angle based, so control loop is needed
 453                     AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
 454                 } else {
 455                     // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
 456                     // horizonLevelStrength is scaled to the stick input
 457                     AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
 458                 }
 459             }
 460         }
 461
 462         // --------low-level gyro-based PID. ----------
 463         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
 464         // -----calculate scaled error.AngleRates
 465         // multiplication of rcCommand corresponds to changing the sticks scaling here
 466         gyroRate = gyroADC[axis] / 4;
 467         RateError = AngleRateTmp - gyroRate;
 468
 469         // -----calculate P component
 470         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
 471
 472         // -----calculate I component
 473         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
 474         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
 475         // Time correction (to avoid different I scaling for different builds based on average cycle time)
 476         // is normalized to cycle time = 2048.
 477         errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
 478
 479         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 480         // I coefficient (I8) moved before integration to make limiting independent from PID settings
 481         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 482
 483         if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
 484             scaleItermToRcInput(axis, pidProfile);
 485             if (antiWindupProtection || motorLimitReached) {
 486                 errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
 487             } else {
 488                 errorGyroILimit[axis] = ABS(errorGyroI[axis]);
 489             }
 490         }
 491
 492         ITerm = errorGyroI[axis] >> 13;
 493
 494         //-----calculate D-term
 495         if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
 496             delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
 497             lastErrorForDelta[axis] = RateError;
 498         } else {
 499             delta = -(gyroRate - lastErrorForDelta[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
 500             lastErrorForDelta[axis] = gyroRate;
 501         }
 502
 503         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
 504         // would be scaled by different dt each time. Division by dT fixes that.
 505         delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
 506
 507         if (deltaStateIsSet) {
 508             delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3;  // Keep same scaling as unfiltered delta
 509         } else {
 510             // Apply moving average
 511             deltaSum = 0;
 512             for (deltaCount = pidProfile->dterm_average_count -1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
 513             previousDelta[axis][0] = delta;
 514             for (deltaCount = 0; deltaCount < pidProfile->dterm_average_count; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
 515             delta = (deltaSum / pidProfile->dterm_average_count) * 3;  // Keep same original scaling
 516         }
 517
 518         DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 519
 520         // -----calculate total PID output
 521         axisPID[axis] = PTerm + ITerm + DTerm;
 522
 523         if (lowThrottlePidReduction) axisPID[axis] /= 4;
 524
 525
 526 #ifdef BLACKBOX
 527         axisPID_P[axis] = PTerm;
 528         axisPID_I[axis] = ITerm;
 529         axisPID_D[axis] = DTerm;
 530 #endif
 531     }
 532 }
 533
 534 void pidSetController(pidControllerType_e type)
 535 {
 536     switch (type) {
 537         default:
 538         case PID_CONTROLLER_MWREWRITE:
 539             pid_controller = pidMultiWiiRewrite;
 540             break;
 541         case PID_CONTROLLER_LUX_FLOAT:
 542             pid_controller = pidLuxFloat;
 543             break;
 544         case PID_CONTROLLER_MW23:
 545             pid_controller = pidMultiWii23;
 546     }
 547 }
 548