src/main/flight/pid.h

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #pragma once
  22
  23 #include <stdbool.h>
  24 #include "common/time.h"
  25 #include "common/filter.h"
  26 #include "common/axis.h"
  27 #include "pg/pg.h"
  28
  29 #define MAX_PID_PROCESS_DENOM       16
  30 #define PID_CONTROLLER_BETAFLIGHT   1
  31 #define PID_MIXER_SCALING           1000.0f
  32 #define PID_SERVO_MIXER_SCALING     0.7f
  33 #define PIDSUM_LIMIT                500
  34 #define PIDSUM_LIMIT_YAW            400
  35 #define PIDSUM_LIMIT_MIN            100
  36 #define PIDSUM_LIMIT_MAX            1000
  37
  38 // 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
  39 #define PTERM_SCALE 0.032029f
  40 #define ITERM_SCALE 0.244381f
  41 #define DTERM_SCALE 0.000529f
  42
  43 // The constant scale factor to replace the Kd component of the feedforward calculation.
  44 // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
  45 #define FEEDFORWARD_SCALE 0.013754f
  46
  47 // Full iterm suppression in setpoint mode at high-passed setpoint rate > 40deg/sec
  48 #define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
  49 #define ITERM_RELAX_CUTOFF_DEFAULT 20
  50
  51 typedef enum {
  52     PID_ROLL,
  53     PID_PITCH,
  54     PID_YAW,
  55     PID_LEVEL,
  56     PID_MAG,
  57     PID_ITEM_COUNT
  58 } pidIndex_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     PID_STABILISATION_OFF = 0,
  68     PID_STABILISATION_ON
  69 } pidStabilisationState_e;
  70
  71 typedef enum {
  72     PID_CRASH_RECOVERY_OFF = 0,
  73     PID_CRASH_RECOVERY_ON,
  74     PID_CRASH_RECOVERY_BEEP,
  75     PID_CRASH_RECOVERY_DISARM,
  76 } pidCrashRecovery_e;
  77
  78 typedef struct pidf_s {
  79     uint8_t P;
  80     uint8_t I;
  81     uint8_t D;
  82     uint16_t F;
  83 } pidf_t;
  84
  85 typedef enum {
  86     ANTI_GRAVITY_SMOOTH,
  87     ANTI_GRAVITY_STEP
  88 } antiGravityMode_e;
  89
  90 typedef enum {
  91     ITERM_RELAX_OFF,
  92     ITERM_RELAX_RP,
  93     ITERM_RELAX_RPY,
  94     ITERM_RELAX_RP_INC,
  95     ITERM_RELAX_RPY_INC
  96 } itermRelax_e;
  97
  98 typedef enum {
  99     ITERM_RELAX_GYRO,
 100     ITERM_RELAX_SETPOINT
 101 } itermRelaxType_e;
 102
 103 #define MAX_PROFILE_NAME_LENGTH 8u
 104
 105 typedef struct pidProfile_s {
 106     uint16_t yaw_lowpass_hz;                // Additional yaw filter when yaw axis too noisy
 107     uint16_t dterm_lowpass_hz;              // Delta Filter in hz
 108     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
 109     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
 110
 111     pidf_t  pid[PID_ITEM_COUNT];
 112
 113     uint8_t dterm_filter_type;              // Filter selection for dterm
 114     uint8_t itermWindupPointPercent;        // iterm windup threshold, percent motor saturation
 115     uint16_t pidSumLimit;
 116     uint16_t pidSumLimitYaw;
 117     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
 118     uint8_t levelAngleLimit;                // Max angle in degrees in level mode
 119
 120     uint8_t horizon_tilt_effect;            // inclination factor for Horizon mode
 121     uint8_t horizon_tilt_expert_mode;       // OFF or ON
 122
 123     // Betaflight PID controller parameters
 124     uint8_t  antiGravityMode;             // type of anti gravity method
 125     uint16_t itermThrottleThreshold;        // max allowed throttle delta before iterm accelerated in ms
 126     uint16_t itermAcceleratorGain;          // Iterm Accelerator Gain when itermThrottlethreshold is hit
 127     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
 128     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
 129     uint16_t crash_dthreshold;              // dterm crash value
 130     uint16_t crash_gthreshold;              // gyro crash value
 131     uint16_t crash_setpoint_threshold;      // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
 132     uint16_t crash_time;                    // ms
 133     uint16_t crash_delay;                   // ms
 134     uint8_t crash_recovery_angle;           // degrees
 135     uint8_t crash_recovery_rate;            // degree/second
 136     uint8_t vbatPidCompensation;            // Scale PIDsum to battery voltage
 137     uint8_t feedForwardTransition;          // Feed forward weight transition
 138     uint16_t crash_limit_yaw;               // limits yaw errorRate, so crashes don't cause huge throttle increase
 139     uint16_t itermLimit;
 140     uint16_t dterm_lowpass2_hz;             // Extra PT1 Filter on D in hz
 141     uint8_t crash_recovery;                 // off, on, on and beeps when it is in crash recovery mode
 142     uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
 143     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
 144     uint8_t iterm_rotation;                 // rotates iterm to translate world errors to local coordinate system
 145     uint8_t iterm_relax_type;               // Specifies type of relax algorithm
 146     uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
 147     uint8_t iterm_relax;                    // Enable iterm suppression during stick input
 148     uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
 149     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
 150     uint8_t acro_trainer_gain;              // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
 151     uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
 152     uint8_t abs_control_gain;               // How strongly should the absolute accumulated error be corrected for
 153     uint8_t abs_control_limit;              // Limit to the correction
 154     uint8_t abs_control_error_limit;        // Limit to the accumulated error
 155     uint8_t abs_control_cutoff;             // Cutoff frequency for path estimation in abs control
 156     uint8_t dterm_filter2_type;             // Filter selection for 2nd dterm
 157     uint16_t dyn_lpf_dterm_min_hz;
 158     uint16_t dyn_lpf_dterm_max_hz;
 159     uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
 160     uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
 161     uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
 162     uint8_t launchControlGain;              // Iterm gain used while launch control is active
 163     uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
 164     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
 165     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
 166     uint8_t thrustLinearization;            // Compensation factor for pid linearization
 167     uint8_t d_min[XYZ_AXIS_COUNT];          // Minimum D value on each axis
 168     uint8_t d_min_gain;                     // Gain factor for amount of gyro / setpoint activity required to boost D
 169     uint8_t d_min_advance;                  // Percentage multiplier for setpoint input to boost algorithm
 170     uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
 171     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
 172     uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
 173     char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
 174
 175     uint8_t idle_hz;                        // minimum motor speed enforced by integrating p controller
 176     uint8_t idle_throttle;                  // added to throttle, replaces dshot_idle_value
 177     uint8_t idle_adjustment_speed;          // how quickly the integrating p controller tries to correct
 178     uint8_t idle_p;                         // kP
 179     uint8_t idle_pid_limit;                 // max P
 180     uint8_t idle_max_increase;              // max integrated correction
 181
 182
 183 } pidProfile_t;
 184
 185 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
 186
 187 typedef struct pidConfig_s {
 188     uint8_t pid_process_denom;              // Processing denominator for PID controller vs gyro sampling rate
 189     uint8_t runaway_takeoff_prevention;          // off, on - enables pidsum runaway disarm logic
 190     uint16_t runaway_takeoff_deactivate_delay;   // delay in ms for "in-flight" conditions before deactivation (successful flight)
 191     uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
 192 } pidConfig_t;
 193
 194 PG_DECLARE(pidConfig_t, pidConfig);
 195
 196 union rollAndPitchTrims_u;
 197 void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
 198
 199 typedef struct pidAxisData_s {
 200     float P;
 201     float I;
 202     float D;
 203     float F;
 204
 205     float Sum;
 206 } pidAxisData_t;
 207
 208 extern const char pidNames[];
 209
 210 extern pidAxisData_t pidData[3];
 211
 212 extern uint32_t targetPidLooptime;
 213
 214 extern float throttleBoost;
 215 extern pt1Filter_t throttleLpf;
 216
 217 void pidResetIterm(void);
 218 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 219 void pidSetItermAccelerator(float newItermAccelerator);
 220 void pidInitFilters(const pidProfile_t *pidProfile);
 221 void pidInitConfig(const pidProfile_t *pidProfile);
 222 void pidInit(const pidProfile_t *pidProfile);
 223 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 224 bool crashRecoveryModeActive(void);
 225 void pidAcroTrainerInit(void);
 226 void pidSetAcroTrainerState(bool newState);
 227 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
 228 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
 229 void pidUpdateAntiGravityThrottleFilter(float throttle);
 230 bool pidOsdAntiGravityActive(void);
 231 bool pidOsdAntiGravityMode(void);
 232 void pidSetAntiGravityState(bool newState);
 233 bool pidAntiGravityEnabled(void);
 234 #ifdef USE_THRUST_LINEARIZATION
 235 float pidApplyThrustLinearization(float motorValue);
 236 float pidCompensateThrustLinearization(float throttle);
 237 #endif
 238 #ifdef USE_AIRMODE_LPF
 239 void pidUpdateAirmodeLpf(float currentOffset);
 240 float pidGetAirmodeThrottleOffset();
 241 #endif
 242
 243 #ifdef UNIT_TEST
 244 #include "sensors/acceleration.h"
 245 extern float axisError[XYZ_AXIS_COUNT];
 246 void applyItermRelax(const int axis, const float iterm,
 247     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint);
 248 void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
 249 void rotateItermAndAxisError();
 250 float pidLevel(int axis, const pidProfile_t *pidProfile,
 251     const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
 252 float calcHorizonLevelStrength(void);
 253 #endif
 254 void dynLpfDTermUpdate(float throttle);
 255 void pidSetItermReset(bool enabled);
 256 float pidGetPreviousSetpoint(int axis);
 257
 258
 259 extern float dT;
 260 extern float pidFrequency;
 261