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