src/main/flight/pid.h

   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 #pragma once
  19
  20 #define PID_MAX_I 256
  21 #define PID_MAX_D 512
  22 #define PID_MAX_TOTAL_PID 1000
  23
  24 #define GYRO_I_MAX 256                      // Gyro I limiter
  25 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
  26 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
  27
  28 typedef enum {
  29     PIDROLL,
  30     PIDPITCH,
  31     PIDYAW,
  32     PIDALT,
  33     PIDPOS,
  34     PIDPOSR,
  35     PIDNAVR,
  36     PIDLEVEL,
  37     PIDMAG,
  38     PIDVEL,
  39     PID_ITEM_COUNT
  40 } pidIndex_e;
  41
  42 typedef enum {
  43     PID_CONTROLLER_MW23 = 0,
  44     PID_CONTROLLER_MWREWRITE,
  45     PID_CONTROLLER_LUX_FLOAT,
  46     PID_COUNT
  47 } pidControllerType_e;
  48
  49 typedef enum {
  50     PID_DELTA_FROM_MEASUREMENT = 0,
  51     PID_DELTA_FROM_ERROR
  52 } pidDeltaMethod_e;
  53
  54 typedef enum {
  55     HORIZON_TILT_MODE_SAFE = 0,
  56     HORIZON_TILT_MODE_EXPERT
  57 } horizonTiltMode_e;
  58
  59 typedef struct pidProfile_s {
  60     uint8_t  P8[PID_ITEM_COUNT];
  61     uint8_t  I8[PID_ITEM_COUNT];
  62     uint8_t  D8[PID_ITEM_COUNT];
  63     uint8_t  pidController;
  64     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
  65     uint16_t dterm_lpf_hz;                  // Delta Filter in hz
  66     uint16_t yaw_lpf_hz;                    // additional yaw filter when yaw axis too noisy
  67     uint8_t  deltaMethod;
  68     uint8_t dterm_filter_type;              // Filter selection for dterm
  69     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
  70     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
  71
  72     uint8_t horizon_tilt_effect;            // inclination factor for Horizon mode
  73     uint8_t horizon_tilt_mode;              // SAFE or EXPERT
  74
  75 } pidProfile_t;
  76
  77 PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
  78
  79 struct controlRateConfig_s;
  80 union rollAndPitchTrims_u;
  81 struct rxConfig_s;
  82 typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig,
  83         uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);            // pid controller function prototype
  84
  85 extern int16_t axisPID[FD_INDEX_COUNT];
  86 extern int32_t axisPID_P[FD_INDEX_COUNT], axisPID_I[FD_INDEX_COUNT], axisPID_D[FD_INDEX_COUNT];
  87 extern uint32_t targetPidLooptime;
  88
  89 float pidScaleITermToRcInput(int axis);
  90 void pidFilterIsSetCheck(const pidProfile_t *pidProfile);
  91
  92 void pidInitFilters(const pidProfile_t *pidProfile);
  93 void pidSetController(pidControllerType_e type);
  94 void pidSetTargetLooptime(uint32_t pidLooptime);
  95 void pidResetITermAngle(void);
  96 void pidResetITerm(void);
  97
  98 int calcHorizonLevelStrength(uint16_t rxConfigMidrc, int horizonTiltEffect,
  99         uint8_t horizonTiltMode, int horizonSensitivity);