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 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, 0);
  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
 130 static float dT;
 131
 132 #ifdef UNIT_TEST
 133 STATIC_UNIT_TESTED void pidResetDt(void)
 134 {
 135     dT = 0.0f;
 136 }
 137 #endif
 138 float getdT(void)
 139 {
 140     if (!dT) dT = (float)targetPidLooptime * 0.000001f;
 141
 142     return dT;
 143 }
 144
 145 void pidSetTargetLooptime(uint32_t pidLooptime)
 146 {
 147     targetPidLooptime = pidLooptime;
 148 }
 149
 150 void pidInitFilters(const pidProfile_t *pidProfile)
 151 {
 152     int axis;
 153
 154     if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
 155         for (axis = 0; axis < 3; axis++) biquadFilterInitNotch(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
 156         dtermNotchInitialised = true;
 157     }
 158
 159     if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
 160         if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) {
 161             for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
 162             dtermBiquadLpfInitialised = true;
 163         }
 164     }
 165 }
 166
 167 void pidResetITerm(void)
 168 {
 169     for (int axis = 0; axis < 3; axis++) {
 170         lastITerm[axis] = 0;
 171         lastITermf[axis] = 0.0f;
 172     }
 173 }
 174
 175 void pidSetController(pidControllerType_e type)
 176 {
 177     switch (type) {
 178         default:
 179         case PID_CONTROLLER_LUX_FLOAT:
 180             pid_controller = pidLuxFloat;
 181             break;
 182 #ifdef USE_PID_MWREWRITE
 183         case PID_CONTROLLER_MWREWRITE:
 184             pid_controller = pidMultiWiiRewrite;
 185             break;
 186 #endif
 187 #ifdef USE_PID_MW23
 188         case PID_CONTROLLER_MW23:
 189             pid_controller = pidMultiWii23;
 190             break;
 191 #endif
 192     }
 193 }
 194
 195 // calculates strength of horizon leveling; 0 = none, 100 = most leveling
 196 int calcHorizonLevelStrength(uint16_t rxConfigMidrc, int horizonTiltEffect,
 197         uint8_t horizonTiltMode, int horizonSensitivity)
 198 {
 199     // get raw stick positions (-500 to 500):
 200     const int32_t stickPosAil = getRcStickDeflection(FD_ROLL, rxConfigMidrc);
 201     const int32_t stickPosEle = getRcStickDeflection(FD_PITCH, rxConfigMidrc);
 202
 203     // 0 at center stick, 500 at max stick deflection:
 204     const int32_t mostDeflectedPos = MAX(ABS(stickPosAil), ABS(stickPosEle));
 205
 206     // start with 100 at center stick, 0 at max stick deflection:
 207     int horizonLevelStrength = (500 - mostDeflectedPos) / 5;
 208
 209     // 0 at level, 900 at vertical, 1800 at inverted (degrees * 10)
 210     const int currentInclination = MAX(ABS(attitude.values.roll),
 211                                                 ABS(attitude.values.pitch));
 212
 213     // horizonTiltMode:  SAFE = leveling always active when sticks centered,
 214     // EXPERT = leveling can be totally off when inverted
 215     if (horizonTiltMode == HORIZON_TILT_MODE_EXPERT) {
 216         if (horizonTiltEffect < 175) {
 217             // horizonTiltEffect 0 to 125 => 2700 to 900
 218             //  (represents where leveling goes to zero):
 219             const int cutoffDeciDegrees = (175-horizonTiltEffect) * 18;
 220             // inclinationLevelRatio (0 to 100) is smaller (less leveling)
 221             //  for larger inclinations; 0 at cutoffDeciDegrees value:
 222             const int inclinationLevelRatio = constrain(
 223                               (((cutoffDeciDegrees-currentInclination)*10) /
 224                                            (cutoffDeciDegrees/10)), 0, 100);
 225             // apply configured horizon sensitivity:
 226             if (horizonSensitivity <= 0) {       // zero means no leveling
 227                 horizonLevelStrength = 0;
 228             } else {
 229                 // when stick is near center (horizonLevelStrength ~= 100)
 230                 //  H_sensitivity value has little effect,
 231                 // when stick is deflected (horizonLevelStrength near 0)
 232                 //  H_sensitivity value has more effect:
 233                 horizonLevelStrength = (horizonLevelStrength - 100) *
 234                         100 / horizonSensitivity + 100;
 235             }
 236             // apply inclination ratio, which may lower leveling
 237             //  to zero regardless of stick position:
 238             horizonLevelStrength = horizonLevelStrength * inclinationLevelRatio / 100;
 239         }
 240         else
 241           horizonLevelStrength = 0;
 242     } else {  // horizon_tilt_mode = SAFE (leveling always active when sticks centered)
 243         int sensitFact;
 244         if (horizonTiltEffect > 0) {
 245             // ratio of 100 to 0 (larger means more leveling):
 246             const int factorRatio = 100 - horizonTiltEffect;
 247             // inclinationLevelRatio (0 to 100) is smaller (less leveling)
 248             //  for larger inclinations, goes to 100 at inclination level:
 249             int inclinationLevelRatio =
 250                   ((1800-currentInclination)/18 * (100-factorRatio)) / 100 + factorRatio;
 251             // apply ratio to configured horizon sensitivity:
 252             sensitFact = horizonSensitivity * inclinationLevelRatio / 100;
 253         }
 254         else   // horizonTiltEffect=0 for "old" functionality
 255             sensitFact = horizonSensitivity;
 256
 257         if (sensitFact <= 0) {           // zero means no leveling
 258             horizonLevelStrength = 0;
 259         } else {
 260             // when stick is near center (horizonLevelStrength ~= 100)
 261             //  sensitFact value has little effect,
 262             // when stick is deflected (horizonLevelStrength near 0)
 263             //  sensitFact value has more effect:
 264             horizonLevelStrength =  (horizonLevelStrength - 100) * 100 / sensitFact + 100;
 265         }
 266     }
 267     return constrain(horizonLevelStrength, 0, 100);
 268 }