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 <string.h>
  21 #include <math.h>
  22
  23 #include <platform.h>
  24
  25 #include "build/build_config.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "config/parameter_group.h"
  32 #include "config/parameter_group_ids.h"
  33 #include "config/config_reset.h"
  34 #include "config/profile.h"
  35
  36 #include "drivers/sensor.h"
  37 #include "drivers/accgyro.h"
  38 #include "drivers/gyro_sync.h"
  39
  40 #include "sensors/sensors.h"
  41 #include "sensors/acceleration.h"
  42
  43 #include "rx/rx.h"
  44
  45 #include "fc/rc_controls.h"
  46 #include "fc/rate_profile.h"
  47
  48 #include "flight/pid.h"
  49 #include "flight/imu.h"
  50
  51 uint32_t targetPidLooptime;
  52
  53 int16_t axisPID[3];
  54
  55 #ifdef BLACKBOX
  56 int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  57 #endif
  58
  59 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
  60 uint8_t PIDweight[3];
  61
  62 int32_t lastITerm[3], ITermLimit[3];
  63 float lastITermf[3], ITermLimitf[3];
  64
  65 pt1Filter_t deltaFilter[3];
  66 pt1Filter_t yawFilter;
  67 biquadFilter_t dtermFilterLpf[3];
  68 biquadFilter_t dtermFilterNotch[3];
  69 static bool dtermNotchInitialised, dtermBiquadLpfInitialised;
  70
  71
  72 void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
  73         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
  74 #ifdef USE_PID_MWREWRITE
  75 void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
  76         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
  77 #endif
  78 #ifdef USE_PID_MW23
  79 void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
  80         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
  81 #endif
  82
  83 pidControllerFuncPtr pid_controller = pidLuxFloat;
  84
  85 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);
  86
  87 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
  88     .pidController = PID_CONTROLLER_LUX_FLOAT,
  89     .P8[PIDROLL] = 40,
  90     .I8[PIDROLL] = 30,
  91     .D8[PIDROLL] = 23,
  92     .P8[PIDPITCH] = 40,
  93     .I8[PIDPITCH] = 30,
  94     .D8[PIDPITCH] = 23,
  95     .P8[PIDYAW] = 85,
  96     .I8[PIDYAW] = 45,
  97     .D8[PIDYAW] = 0,
  98     .P8[PIDALT] = 50,
  99     .I8[PIDALT] = 0,
 100     .D8[PIDALT] = 0,
 101     .P8[PIDPOS] = 15,   // POSHOLD_P * 100
 102     .I8[PIDPOS] = 0,    // POSHOLD_I * 100
 103     .D8[PIDPOS] = 0,
 104     .P8[PIDPOSR] = 34,  // POSHOLD_RATE_P * 10
 105     .I8[PIDPOSR] = 14,  // POSHOLD_RATE_I * 100
 106     .D8[PIDPOSR] = 53,  // POSHOLD_RATE_D * 1000
 107     .P8[PIDNAVR] = 25,  // NAV_P * 10
 108     .I8[PIDNAVR] = 33,  // NAV_I * 100
 109     .D8[PIDNAVR] = 83,  // NAV_D * 1000
 110     .P8[PIDLEVEL] = 20,
 111     .I8[PIDLEVEL] = 10,
 112     .D8[PIDLEVEL] = 75,
 113     .P8[PIDMAG] = 40,
 114     .P8[PIDVEL] = 120,
 115     .I8[PIDVEL] = 45,
 116     .D8[PIDVEL] = 1,
 117
 118     .yaw_p_limit = YAW_P_LIMIT_MAX,
 119     .yaw_lpf_hz = 0,
 120     .dterm_filter_type = FILTER_BIQUAD,
 121     .dterm_lpf_hz = 100,    // filtering ON by default
 122     .dterm_notch_hz = 260,
 123     .dterm_notch_cutoff = 160,
 124     .deltaMethod = PID_DELTA_FROM_MEASUREMENT,
 125     .horizon_tilt_effect = 75,
 126     .horizon_tilt_mode = HORIZON_TILT_MODE_SAFE,
 127 );
 128
 129 void pidSetTargetLooptime(uint32_t pidLooptime)
 130 {
 131     targetPidLooptime = pidLooptime;
 132 }
 133
 134 void pidInitFilters(const pidProfile_t *pidProfile)
 135 {
 136     int axis;
 137
 138     if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
 139         float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
 140         for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
 141         dtermNotchInitialised = true;
 142     }
 143
 144     if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
 145         if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) {
 146             for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
 147             dtermBiquadLpfInitialised = true;
 148         }
 149     }
 150 }
 151
 152 void pidResetITerm(void)
 153 {
 154     for (int axis = 0; axis < 3; axis++) {
 155         lastITerm[axis] = 0;
 156         lastITermf[axis] = 0.0f;
 157     }
 158 }
 159
 160 void pidSetController(pidControllerType_e type)
 161 {
 162     switch (type) {
 163         default:
 164         case PID_CONTROLLER_LUX_FLOAT:
 165             pid_controller = pidLuxFloat;
 166             break;
 167 #ifdef USE_PID_MWREWRITE
 168         case PID_CONTROLLER_MWREWRITE:
 169             pid_controller = pidMultiWiiRewrite;
 170             break;
 171 #endif
 172 #ifdef USE_PID_MW23
 173         case PID_CONTROLLER_MW23:
 174             pid_controller = pidMultiWii23;
 175             break;
 176 #endif
 177     }
 178 }
 179
 180 // calculates strength of horizon leveling; 0 = none, 100 = most leveling
 181 int calcHorizonLevelStrength(uint16_t rxConfigMidrc, int horizonTiltEffect,
 182         uint8_t horizonTiltMode, int horizonSensitivity)
 183 {
 184     // get raw stick positions (-500 to 500):
 185     const int32_t stickPosAil = getRcStickDeflection(FD_ROLL, rxConfigMidrc);
 186     const int32_t stickPosEle = getRcStickDeflection(FD_PITCH, rxConfigMidrc);
 187
 188     // 0 at center stick, 500 at max stick deflection:
 189     const int32_t mostDeflectedPos = MAX(ABS(stickPosAil), ABS(stickPosEle));
 190
 191     // start with 100 at center stick, 0 at max stick deflection:
 192     int horizonLevelStrength = (500 - mostDeflectedPos) / 5;
 193
 194     // 0 at level, 900 at vertical, 1800 at inverted (degrees * 10)
 195     const int currentInclination = MAX(ABS(attitude.values.roll),
 196                                                 ABS(attitude.values.pitch));
 197
 198     // horizonTiltMode:  SAFE = leveling always active when sticks centered,
 199     // EXPERT = leveling can be totally off when inverted
 200     if (horizonTiltMode == HORIZON_TILT_MODE_EXPERT) {
 201         if (horizonTiltEffect < 175) {
 202             // horizonTiltEffect 0 to 125 => 2700 to 900
 203             //  (represents where leveling goes to zero):
 204             const int cutoffDeciDegrees = (175-horizonTiltEffect) * 18;
 205             // inclinationLevelRatio (0 to 100) is smaller (less leveling)
 206             //  for larger inclinations; 0 at cutoffDeciDegrees value:
 207             const int inclinationLevelRatio = constrain(
 208                               (((cutoffDeciDegrees-currentInclination)*10) /
 209                                            (cutoffDeciDegrees/10)), 0, 100);
 210             // apply configured horizon sensitivity:
 211             if (horizonSensitivity <= 0) {       // zero means no leveling
 212                 horizonLevelStrength = 0;
 213             } else {
 214                 // when stick is near center (horizonLevelStrength ~= 100)
 215                 //  H_sensitivity value has little effect,
 216                 // when stick is deflected (horizonLevelStrength near 0)
 217                 //  H_sensitivity value has more effect:
 218                 horizonLevelStrength = (horizonLevelStrength - 100) *
 219                         100 / horizonSensitivity + 100;
 220             }
 221             // apply inclination ratio, which may lower leveling
 222             //  to zero regardless of stick position:
 223             horizonLevelStrength = horizonLevelStrength * inclinationLevelRatio / 100;
 224         }
 225         else
 226           horizonLevelStrength = 0;
 227     } else {  // horizon_tilt_mode = SAFE (leveling always active when sticks centered)
 228         int sensitFact;
 229         if (horizonTiltEffect > 0) {
 230             // ratio of 100 to 0 (larger means more leveling):
 231             const int factorRatio = 100 - horizonTiltEffect;
 232             // inclinationLevelRatio (0 to 100) is smaller (less leveling)
 233             //  for larger inclinations, goes to 100 at inclination level:
 234             int inclinationLevelRatio =
 235                   ((1800-currentInclination)/18 * (100-factorRatio)) / 100 + factorRatio;
 236             // apply ratio to configured horizon sensitivity:
 237             sensitFact = horizonSensitivity * inclinationLevelRatio / 100;
 238         }
 239         else   // horizonTiltEffect=0 for "old" functionality
 240             sensitFact = horizonSensitivity;
 241
 242         if (sensitFact <= 0) {           // zero means no leveling
 243             horizonLevelStrength = 0;
 244         } else {
 245             // when stick is near center (horizonLevelStrength ~= 100)
 246             //  sensitFact value has little effect,
 247             // when stick is deflected (horizonLevelStrength near 0)
 248             //  sensitFact value has more effect:
 249             horizonLevelStrength =  (horizonLevelStrength - 100) * 100 / sensitFact + 100;
 250         }
 251     }
 252     return constrain(horizonLevelStrength, 0, 100);
 253 }