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 #include "flight/gtune.h"
  48
  49 #include "config/runtime_config.h"
  50
  51 extern uint8_t motorCount;
  52 uint32_t targetPidLooptime;
  53
  54 int16_t axisPID[3];
  55
  56 #ifdef BLACKBOX
  57 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  58 #endif
  59
  60 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
  61 uint8_t PIDweight[3];
  62
  63 static int32_t errorGyroI[3];
  64 #ifndef SKIP_PID_LUXFLOAT
  65 static float errorGyroIf[3];
  66 #endif
  67
  68 static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
  69         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
  70
  71 pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
  72
  73 void setTargetPidLooptime(uint8_t pidProcessDenom) {
  74         targetPidLooptime = targetLooptime * pidProcessDenom;
  75 }
  76
  77 float calculateExpoPlus(int axis, const rxConfig_t *rxConfig) {
  78     float propFactor;
  79     float superExpoFactor;
  80
  81     if (axis == YAW && !rxConfig->superExpoYawMode) {
  82         propFactor = 1.0f;
  83     } else {
  84         float rcFactor = (ABS(rcCommand[axis]) / 500.0f);
  85
  86         superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
  87         propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * rcFactor * rcFactor * rcFactor), 0.0f, 1.0f);
  88     }
  89
  90     return propFactor;
  91 }
  92
  93 uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
  94     uint16_t dynamicKp;
  95
  96     uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);
  97
  98     dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;
  99
 100     return dynamicKp;
 101 }
 102
 103 uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
 104     uint16_t dynamicKi;
 105     uint16_t resetRate;
 106
 107     resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
 108
 109     uint32_t dynamicFactor = (1 << 8) - constrain((ABS(gyroADC[axis]) << 6) / resetRate, 0, 1 << 8);
 110
 111     dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
 112
 113     return dynamicKi;
 114 }
 115
 116 void pidResetErrorGyroState(void)
 117 {
 118     int axis;
 119
 120     for (axis = 0; axis < 3; axis++) {
 121         errorGyroI[axis] = 0;
 122 #ifndef SKIP_PID_LUXFLOAT
 123         errorGyroIf[axis] = 0.0f;
 124 #endif
 125     }
 126 }
 127
 128 float getdT (void) {
 129     static float dT;
 130     if (!dT) dT = (float)targetPidLooptime * 0.000001f;
 131
 132     return dT;
 133 }
 134
 135 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 136
 137 static filterStatePt1_t deltaFilterState[3];
 138 static filterStatePt1_t yawFilterState;
 139
 140 #ifndef SKIP_PID_LUXFLOAT
 141 static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
 142         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
 143 {
 144     float RateError, AngleRate, gyroRate;
 145     float ITerm,PTerm,DTerm;
 146     static float lastRate[3];
 147     float delta;
 148     int axis;
 149     float horizonLevelStrength = 1;
 150
 151     float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
 152
 153     // Scaling factors for Pids to match rewrite and use same defaults
 154     static const float luxPTermScale = 1.0f / 128;
 155     static const float luxITermScale = 1000000.0f / 0x1000000;
 156     static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508;
 157
 158     if (FLIGHT_MODE(HORIZON_MODE)) {
 159         // Figure out the raw stick positions
 160         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
 161         const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
 162         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
 163         // Progressively turn off the horizon self level strength as the stick is banged over
 164         horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500;  // 1 at centre stick, 0 = max stick deflection
 165         if(pidProfile->D8[PIDLEVEL] == 0){
 166             horizonLevelStrength = 0;
 167         } else {
 168             horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1);
 169         }
 170     }
 171
 172     // ----------PID controller----------
 173     for (axis = 0; axis < 3; axis++) {
 174         uint8_t rate = controlRateConfig->rates[axis];
 175
 176         if (axis == FD_YAW) {
 177             // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
 178             AngleRate = (float)((rate + 47) * rcCommand[YAW]) / 32.0f;
 179          } else {
 180              // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
 181              AngleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
 182              if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 183                 // calculate error angle and limit the angle to the max inclination
 184 #ifdef GPS
 185                 const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 186                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
 187 #else
 188                 const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
 189                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
 190 #endif
 191                 if (FLIGHT_MODE(ANGLE_MODE)) {
 192                     // ANGLE mode - control is angle based, so control loop is needed
 193                     AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
 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                     AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
 198                 }
 199             }
 200         }
 201
 202         gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale
 203
 204         // --------low-level gyro-based PID. ----------
 205         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
 206         // -----calculate scaled error.AngleRates
 207         // multiplication of rcCommand corresponds to changing the sticks scaling here
 208         RateError = AngleRate - gyroRate;
 209
 210         uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
 211
 212         // -----calculate P component
 213         if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
 214             PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
 215         } else {
 216             PTerm = luxPTermScale * RateError * kP * tpaFactor;
 217         }
 218
 219         // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
 220         if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
 221             PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 222         }
 223
 224         // -----calculate I component.
 225         uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
 226
 227         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f);
 228
 229         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 230         // I coefficient (I8) moved before integration to make limiting independent from PID settings
 231         ITerm = errorGyroIf[axis];
 232
 233         //-----calculate D-term
 234         if (axis == YAW) {
 235             if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
 236
 237             axisPID[axis] = lrintf(PTerm + ITerm);
 238
 239             if (motorCount >= 4) {
 240                 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);
 241
 242                 // prevent "yaw jump" during yaw correction
 243                 axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
 244             }
 245         } else {
 246             delta = -(gyroRate - lastRate[axis]);
 247             lastRate[axis] = gyroRate;
 248
 249             // Divide delta by targetLooptime to get differential (ie dr/dt)
 250             delta *= (1.0f / getdT());
 251
 252             // Filter delta
 253             if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
 254
 255             DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
 256
 257             // -----calculate total PID output
 258             axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
 259         }
 260
 261 #ifdef GTUNE
 262         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 263             calculate_Gtune(axis);
 264         }
 265 #endif
 266
 267 #ifdef BLACKBOX
 268         axisPID_P[axis] = PTerm;
 269         axisPID_I[axis] = ITerm;
 270         axisPID_D[axis] = DTerm;
 271 #endif
 272     }
 273 }
 274 #endif
 275
 276 static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
 277         const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
 278 {
 279     int axis;
 280     int32_t PTerm, ITerm, DTerm, delta;
 281     static int32_t lastRate[3];
 282     int32_t AngleRateTmp, RateError, gyroRate;
 283
 284     int8_t horizonLevelStrength = 100;
 285
 286     if (FLIGHT_MODE(HORIZON_MODE)) {
 287         // Figure out the raw stick positions
 288         const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
 289         const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
 290         const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
 291         // Progressively turn off the horizon self level strength as the stick is banged over
 292         horizonLevelStrength = (500 - mostDeflectedPos) / 5;  // 100 at centre stick, 0 = max stick deflection
 293         // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
 294         // For more rate mode increase D and slower flips and rolls will be possible
 295         horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
 296     }
 297
 298     // ----------PID controller----------
 299     for (axis = 0; axis < 3; axis++) {
 300         uint8_t rate = controlRateConfig->rates[axis];
 301
 302         // -----Get the desired angle rate depending on flight mode
 303         if (axis == FD_YAW) {
 304             // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
 305             AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5;
 306         } else {
 307             AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
 308             if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 309                 // calculate error angle and limit the angle to max configured inclination
 310 #ifdef GPS
 311                 const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 312                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 313 #else
 314                 const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
 315                     +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 316 #endif
 317                 if (FLIGHT_MODE(ANGLE_MODE)) {
 318                     // ANGLE mode - control is angle based, so control loop is needed
 319                     AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
 320                 } else {
 321                     // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
 322                     // horizonLevelStrength is scaled to the stick input
 323                     AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
 324                 }
 325             }
 326         }
 327
 328         // --------low-level gyro-based PID. ----------
 329         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
 330         // -----calculate scaled error.AngleRates
 331         // multiplication of rcCommand corresponds to changing the sticks scaling here
 332         gyroRate = gyroADC[axis] / 4;
 333         RateError = AngleRateTmp - gyroRate;
 334
 335         uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
 336
 337         // -----calculate P component
 338         if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
 339             PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
 340         } else {
 341             PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
 342         }
 343
 344         // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
 345         if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
 346             PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 347         }
 348
 349         // -----calculate I component
 350         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
 351         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
 352         // Time correction (to avoid different I scaling for different builds based on average cycle time)
 353         // is normalized to cycle time = 2048.
 354         uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
 355
 356         errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
 357
 358         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 359         // I coefficient (I8) moved before integration to make limiting independent from PID settings
 360         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 361
 362         ITerm = errorGyroI[axis] >> 13;
 363
 364         //-----calculate D-term
 365         if (axis == YAW) {
 366             if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
 367
 368             axisPID[axis] = PTerm + ITerm;
 369
 370             if (motorCount >= 4) {
 371                 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);
 372
 373                 // prevent "yaw jump" during yaw correction
 374                 axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
 375             }
 376         } else {
 377             delta = -(gyroRate - lastRate[axis]);
 378             lastRate[axis] = gyroRate;
 379
 380             // Divide delta by targetLooptime to get differential (ie dr/dt)
 381             delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
 382
 383             // Filter delta
 384             if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
 385
 386             DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 387
 388             // -----calculate total PID output
 389             axisPID[axis] = PTerm + ITerm + DTerm;
 390         }
 391
 392
 393 #ifdef GTUNE
 394         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 395              calculate_Gtune(axis);
 396         }
 397 #endif
 398
 399 #ifdef BLACKBOX
 400         axisPID_P[axis] = PTerm;
 401         axisPID_I[axis] = ITerm;
 402         axisPID_D[axis] = DTerm;
 403 #endif
 404     }
 405 }
 406
 407 void pidSetController(pidControllerType_e type)
 408 {
 409     switch (type) {
 410         default:
 411         case PID_CONTROLLER_MWREWRITE:
 412             pid_controller = pidMultiWiiRewrite;
 413             break;
 414 #ifndef SKIP_PID_LUXFLOAT
 415         case PID_CONTROLLER_LUX_FLOAT:
 416             pid_controller = pidLuxFloat;
 417 #endif
 418     }
 419 }
 420