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 #include "rx/rx.h"
  18
  19 #pragma once
  20
  21 #define GYRO_I_MAX 256                      // Gyro I limiter
  22 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
  23 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
  24 #define YAW_JUMP_PREVENTION_LIMIT_LOW 80
  25 #define YAW_JUMP_PREVENTION_LIMIT_HIGH 400
  26
  27 #define DYNAMIC_PTERM_STICK_THRESHOLD 400
  28
  29
  30 // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
  31 #define PTERM_SCALE 0.032029f
  32 #define ITERM_SCALE 0.244381f
  33 #define DTERM_SCALE 0.000529f
  34
  35 typedef enum {
  36     PIDROLL,
  37     PIDPITCH,
  38     PIDYAW,
  39     PIDALT,
  40     PIDPOS,
  41     PIDPOSR,
  42     PIDNAVR,
  43     PIDLEVEL,
  44     PIDMAG,
  45     PIDVEL,
  46     PID_ITEM_COUNT
  47 } pidIndex_e;
  48
  49 typedef enum {
  50     PID_CONTROLLER_LEGACY = 0,           // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future
  51     PID_CONTROLLER_BETAFLIGHT,           // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future
  52     PID_COUNT
  53 } pidControllerType_e;
  54
  55 typedef enum {
  56     DELTA_FROM_ERROR = 0,
  57     DELTA_FROM_MEASUREMENT
  58 } pidDeltaType_e;
  59
  60 typedef enum {
  61     SUPEREXPO_YAW_OFF = 0,
  62     SUPEREXPO_YAW_ON,
  63     SUPEREXPO_YAW_ALWAYS
  64 } pidSuperExpoYaw_e;
  65
  66 typedef enum {
  67     NEGATIVE_ERROR = 0,
  68     POSITIVE_ERROR
  69 } pidErrorPolarity_e;
  70
  71 typedef enum {
  72     PID_STABILISATION_OFF = 0,
  73     PID_STABILISATION_ON
  74 } pidStabilisationState_e;
  75
  76 typedef struct pidProfile_s {
  77     uint8_t pidController;                  // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat)
  78
  79     uint8_t P8[PID_ITEM_COUNT];
  80     uint8_t I8[PID_ITEM_COUNT];
  81     uint8_t D8[PID_ITEM_COUNT];
  82
  83     uint8_t dterm_filter_type;              // Filter selection for dterm
  84     uint16_t dterm_lpf_hz;                  // Delta Filter in hz
  85     uint16_t yaw_lpf_hz;                    // Additional yaw filter when yaw axis too noisy
  86     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
  87     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
  88     uint8_t deltaMethod;                    // Alternative delta Calculation
  89     uint16_t rollPitchItermIgnoreRate;      // Experimental threshold for resetting iterm for pitch and roll on certain rates
  90     uint16_t yawItermIgnoreRate;            // Experimental threshold for resetting iterm for yaw on certain rates
  91     uint16_t yaw_p_limit;
  92     uint8_t dterm_average_count;            // Configurable delta count for dterm
  93     uint8_t vbatPidCompensation;            // Scale PIDsum to battery voltage
  94     uint8_t zeroThrottleStabilisation;      // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active.
  95
  96     // Betaflight PID controller parameters
  97     uint8_t toleranceBand;                  // Error tolerance area where toleranceBandReduction is applied under certain circumstances
  98     uint8_t toleranceBandReduction;         // Lowest possible P and D reduction in percentage
  99     uint8_t zeroCrossAllowanceCount;        // Amount of bouncebacks within tolerance band allowed before reduction kicks in
 100     uint8_t itermThrottleGain;              // Throttle coupling to iterm. Quick throttle changes will bump iterm
 101     uint8_t ptermSetpointWeight;            // Setpoint weight for Pterm (lower means more PV tracking)
 102     uint8_t dtermSetpointWeight;            // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative)
 103     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
 104     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
 105
 106 #ifdef GTUNE
 107     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune
 108     uint8_t  gtune_hilimP[3];               // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
 109     uint8_t  gtune_pwr;                     // [0..10] Strength of adjustment
 110     uint16_t gtune_settle_time;             // [200..1000] Settle time in ms
 111     uint8_t  gtune_average_cycles;          // [8..128] Number of looptime cycles used for gyro average calculation
 112 #endif
 113 } pidProfile_t;
 114
 115 struct controlRateConfig_s;
 116 union rollAndPitchTrims_u;
 117 struct rxConfig_s;
 118 typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
 119         const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);            // pid controller function prototype
 120
 121 extern int16_t axisPID[XYZ_AXIS_COUNT];
 122 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 123 bool airmodeWasActivated;
 124 extern uint32_t targetPidLooptime;
 125
 126 void pidSetController(pidControllerType_e type);
 127 void pidResetErrorGyroState(void);
 128 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 129 void setTargetPidLooptime(uint8_t pidProcessDenom);
 130