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
  28 #include "pg/pg.h"
  29
  30 #define MAX_PID_PROCESS_DENOM       16
  31 #define PID_CONTROLLER_BETAFLIGHT   1
  32 #define PID_MIXER_SCALING           1000.0f
  33 #define PID_SERVO_MIXER_SCALING     0.7f
  34 #define PIDSUM_LIMIT                500
  35 #define PIDSUM_LIMIT_YAW            400
  36 #define PIDSUM_LIMIT_MIN            100
  37 #define PIDSUM_LIMIT_MAX            1000
  38
  39 // 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
  40 #define PTERM_SCALE 0.032029f
  41 #define ITERM_SCALE 0.244381f
  42 #define DTERM_SCALE 0.000529f
  43
  44 // The constant scale factor to replace the Kd component of the feedforward calculation.
  45 // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
  46 #define FEEDFORWARD_SCALE 0.013754f
  47
  48 // Full iterm suppression in setpoint mode at high-passed setpoint rate > 40deg/sec
  49 #define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
  50 #define ITERM_RELAX_CUTOFF_DEFAULT 15
  51
  52 // Anti gravity I constant
  53 #define AG_KI 21.586988f;
  54
  55 #define ITERM_ACCELERATOR_GAIN_OFF 1000
  56 #define ITERM_ACCELERATOR_GAIN_MAX 30000
  57 typedef enum {
  58     PID_ROLL,
  59     PID_PITCH,
  60     PID_YAW,
  61     PID_LEVEL,
  62     PID_MAG,
  63     PID_ITEM_COUNT
  64 } pidIndex_e;
  65
  66 typedef enum {
  67     SUPEREXPO_YAW_OFF = 0,
  68     SUPEREXPO_YAW_ON,
  69     SUPEREXPO_YAW_ALWAYS
  70 } pidSuperExpoYaw_e;
  71
  72 typedef enum {
  73     PID_STABILISATION_OFF = 0,
  74     PID_STABILISATION_ON
  75 } pidStabilisationState_e;
  76
  77 typedef enum {
  78     PID_CRASH_RECOVERY_OFF = 0,
  79     PID_CRASH_RECOVERY_ON,
  80     PID_CRASH_RECOVERY_BEEP,
  81     PID_CRASH_RECOVERY_DISARM,
  82 } pidCrashRecovery_e;
  83
  84 typedef struct pidf_s {
  85     uint8_t P;
  86     uint8_t I;
  87     uint8_t D;
  88     uint16_t F;
  89 } pidf_t;
  90
  91 typedef enum {
  92     ANTI_GRAVITY_SMOOTH,
  93     ANTI_GRAVITY_STEP
  94 } antiGravityMode_e;
  95
  96 typedef enum {
  97     ITERM_RELAX_OFF,
  98     ITERM_RELAX_RP,
  99     ITERM_RELAX_RPY,
 100     ITERM_RELAX_RP_INC,
 101     ITERM_RELAX_RPY_INC,
 102     ITERM_RELAX_COUNT,
 103 } itermRelax_e;
 104
 105 typedef enum {
 106     ITERM_RELAX_GYRO,
 107     ITERM_RELAX_SETPOINT,
 108     ITERM_RELAX_TYPE_COUNT,
 109 } itermRelaxType_e;
 110
 111 typedef enum ffInterpolationType_e {
 112     FF_INTERPOLATE_OFF,
 113     FF_INTERPOLATE_ON,
 114     FF_INTERPOLATE_AVG2,
 115     FF_INTERPOLATE_AVG3,
 116     FF_INTERPOLATE_AVG4
 117 } ffInterpolationType_t;
 118
 119 #define MAX_PROFILE_NAME_LENGTH 8u
 120
 121 typedef struct pidProfile_s {
 122     uint16_t yaw_lowpass_hz;                // Additional yaw filter when yaw axis too noisy
 123     uint16_t dterm_lowpass_hz;              // Delta Filter in hz
 124     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
 125     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
 126
 127     pidf_t  pid[PID_ITEM_COUNT];
 128
 129     uint8_t dterm_filter_type;              // Filter selection for dterm
 130     uint8_t itermWindupPointPercent;        // iterm windup threshold, percent motor saturation
 131     uint16_t pidSumLimit;
 132     uint16_t pidSumLimitYaw;
 133     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
 134     uint8_t levelAngleLimit;                // Max angle in degrees in level mode
 135
 136     uint8_t horizon_tilt_effect;            // inclination factor for Horizon mode
 137     uint8_t horizon_tilt_expert_mode;       // OFF or ON
 138
 139     // Betaflight PID controller parameters
 140     uint8_t  antiGravityMode;             // type of anti gravity method
 141     uint16_t itermThrottleThreshold;        // max allowed throttle delta before iterm accelerated in ms
 142     uint16_t itermAcceleratorGain;          // Iterm Accelerator Gain when itermThrottlethreshold is hit
 143     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
 144     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
 145     uint16_t crash_dthreshold;              // dterm crash value
 146     uint16_t crash_gthreshold;              // gyro crash value
 147     uint16_t crash_setpoint_threshold;      // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
 148     uint16_t crash_time;                    // ms
 149     uint16_t crash_delay;                   // ms
 150     uint8_t crash_recovery_angle;           // degrees
 151     uint8_t crash_recovery_rate;            // degree/second
 152     uint8_t feedForwardTransition;          // Feed forward weight transition
 153     uint16_t crash_limit_yaw;               // limits yaw errorRate, so crashes don't cause huge throttle increase
 154     uint16_t itermLimit;
 155     uint16_t dterm_lowpass2_hz;             // Extra PT1 Filter on D in hz
 156     uint8_t crash_recovery;                 // off, on, on and beeps when it is in crash recovery mode
 157     uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
 158     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
 159     uint8_t iterm_rotation;                 // rotates iterm to translate world errors to local coordinate system
 160     uint8_t iterm_relax_type;               // Specifies type of relax algorithm
 161     uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
 162     uint8_t iterm_relax;                    // Enable iterm suppression during stick input
 163     uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
 164     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
 165     uint8_t acro_trainer_gain;              // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
 166     uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
 167     uint8_t abs_control_gain;               // How strongly should the absolute accumulated error be corrected for
 168     uint8_t abs_control_limit;              // Limit to the correction
 169     uint8_t abs_control_error_limit;        // Limit to the accumulated error
 170     uint8_t abs_control_cutoff;             // Cutoff frequency for path estimation in abs control
 171     uint8_t dterm_filter2_type;             // Filter selection for 2nd dterm
 172     uint16_t dyn_lpf_dterm_min_hz;
 173     uint16_t dyn_lpf_dterm_max_hz;
 174     uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
 175     uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
 176     uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
 177     uint8_t launchControlGain;              // Iterm gain used while launch control is active
 178     uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
 179     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
 180     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
 181     uint8_t thrustLinearization;            // Compensation factor for pid linearization
 182     uint8_t d_min[XYZ_AXIS_COUNT];          // Minimum D value on each axis
 183     uint8_t d_min_gain;                     // Gain factor for amount of gyro / setpoint activity required to boost D
 184     uint8_t d_min_advance;                  // Percentage multiplier for setpoint input to boost algorithm
 185     uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
 186     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
 187     uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
 188     uint8_t ff_boost;                       // amount of high-pass filtered FF to add to FF, 100 means 100% added
 189     char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
 190
 191     uint8_t idle_min_rpm;                   // minimum motor speed enforced by integrating p controller
 192     uint8_t idle_adjustment_speed;          // how quickly the integrating p controller tries to correct
 193     uint8_t idle_p;                         // kP
 194     uint8_t idle_pid_limit;                 // max P
 195     uint8_t idle_max_increase;              // max integrated correction
 196
 197     uint8_t ff_interpolate_sp;              // Calculate FF from interpolated setpoint
 198     uint8_t ff_max_rate_limit;              // Maximum setpoint rate percentage for FF
 199     uint8_t ff_spike_limit;                 // FF stick extrapolation lookahead period in ms
 200     uint8_t ff_smooth_factor;               // Amount of smoothing for interpolated FF steps
 201     uint8_t dyn_lpf_curve_expo;             // set the curve for dynamic dterm lowpass filter
 202     uint8_t level_race_mode;                // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
 203     uint8_t vbat_sag_compensation;          // Reduce motor output by this percentage of the maximum compensation amount
 204 } pidProfile_t;
 205
 206 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
 207
 208 typedef struct pidConfig_s {
 209     uint8_t pid_process_denom;              // Processing denominator for PID controller vs gyro sampling rate
 210     uint8_t runaway_takeoff_prevention;          // off, on - enables pidsum runaway disarm logic
 211     uint16_t runaway_takeoff_deactivate_delay;   // delay in ms for "in-flight" conditions before deactivation (successful flight)
 212     uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
 213 } pidConfig_t;
 214
 215 PG_DECLARE(pidConfig_t, pidConfig);
 216
 217 union rollAndPitchTrims_u;
 218 void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
 219
 220 typedef struct pidAxisData_s {
 221     float P;
 222     float I;
 223     float D;
 224     float F;
 225
 226     float Sum;
 227 } pidAxisData_t;
 228
 229 typedef union dtermLowpass_u {
 230     pt1Filter_t pt1Filter;
 231     biquadFilter_t biquadFilter;
 232 } dtermLowpass_t;
 233
 234 typedef struct pidCoefficient_s {
 235     float Kp;
 236     float Ki;
 237     float Kd;
 238     float Kf;
 239 } pidCoefficient_t;
 240
 241 typedef struct pidRuntime_s {
 242     float dT;
 243     float pidFrequency;
 244     bool pidStabilisationEnabled;
 245     float previousPidSetpoint[XYZ_AXIS_COUNT];
 246     filterApplyFnPtr dtermNotchApplyFn;
 247     biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
 248     filterApplyFnPtr dtermLowpassApplyFn;
 249     dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
 250     filterApplyFnPtr dtermLowpass2ApplyFn;
 251     dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
 252     filterApplyFnPtr ptermYawLowpassApplyFn;
 253     pt1Filter_t ptermYawLowpass;
 254     bool antiGravityEnabled;
 255     uint8_t antiGravityMode;
 256     pt1Filter_t antiGravityThrottleLpf;
 257     float antiGravityOsdCutoff;
 258     float antiGravityThrottleHpf;
 259     float ffBoostFactor;
 260     float ffSpikeLimitInverse;
 261     float itermAccelerator;
 262     uint16_t itermAcceleratorGain;
 263     float feedForwardTransition;
 264     pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
 265     float levelGain;
 266     float horizonGain;
 267     float horizonTransition;
 268     float horizonCutoffDegrees;
 269     float horizonFactorRatio;
 270     uint8_t horizonTiltExpertMode;
 271     float maxVelocity[XYZ_AXIS_COUNT];
 272     float itermWindupPointInv;
 273     bool inCrashRecoveryMode;
 274     timeUs_t crashDetectedAtUs;
 275     timeDelta_t crashTimeLimitUs;
 276     timeDelta_t crashTimeDelayUs;
 277     int32_t crashRecoveryAngleDeciDegrees;
 278     float crashRecoveryRate;
 279     float crashGyroThreshold;
 280     float crashDtermThreshold;
 281     float crashSetpointThreshold;
 282     float crashLimitYaw;
 283     float itermLimit;
 284     bool itermRotation;
 285     bool zeroThrottleItermReset;
 286     bool levelRaceMode;
 287
 288 #ifdef USE_ITERM_RELAX
 289     pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 290     uint8_t itermRelax;
 291     uint8_t itermRelaxType;
 292     uint8_t itermRelaxCutoff;
 293 #endif
 294
 295 #ifdef USE_ABSOLUTE_CONTROL
 296     float acCutoff;
 297     float acGain;
 298     float acLimit;
 299     float acErrorLimit;
 300     pt1Filter_t acLpf[XYZ_AXIS_COUNT];
 301     float oldSetpointCorrection[XYZ_AXIS_COUNT];
 302 #endif
 303
 304 #ifdef USE_D_MIN
 305     biquadFilter_t dMinRange[XYZ_AXIS_COUNT];
 306     pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT];
 307     float dMinPercent[XYZ_AXIS_COUNT];
 308     float dMinGyroGain;
 309     float dMinSetpointGain;
 310 #endif
 311
 312 #ifdef USE_AIRMODE_LPF
 313     pt1Filter_t airmodeThrottleLpf1;
 314     pt1Filter_t airmodeThrottleLpf2;
 315 #endif
 316
 317 #ifdef USE_RC_SMOOTHING_FILTER
 318     pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
 319     biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
 320     bool setpointDerivativeLpfInitialized;
 321     uint8_t rcSmoothingDebugAxis;
 322     uint8_t rcSmoothingFilterType;
 323 #endif // USE_RC_SMOOTHING_FILTER
 324
 325 #ifdef USE_ACRO_TRAINER
 326     float acroTrainerAngleLimit;
 327     float acroTrainerLookaheadTime;
 328     uint8_t acroTrainerDebugAxis;
 329     float acroTrainerGain;
 330     bool acroTrainerActive;
 331     int acroTrainerAxisState[2];  // only need roll and pitch
 332 #endif
 333
 334 #ifdef USE_DYN_LPF
 335     uint8_t dynLpfFilter;
 336     uint16_t dynLpfMin;
 337     uint16_t dynLpfMax;
 338     uint8_t dynLpfCurveExpo;
 339 #endif
 340
 341 #ifdef USE_LAUNCH_CONTROL
 342     uint8_t launchControlMode;
 343     uint8_t launchControlAngleLimit;
 344     float launchControlKi;
 345 #endif
 346
 347 #ifdef USE_INTEGRATED_YAW_CONTROL
 348     bool useIntegratedYaw;
 349     uint8_t integratedYawRelax;
 350 #endif
 351
 352 #ifdef USE_THRUST_LINEARIZATION
 353     float thrustLinearization;
 354     float throttleCompensateAmount;
 355 #endif
 356
 357 #ifdef USE_AIRMODE_LPF
 358     float airmodeThrottleOffsetLimit;
 359 #endif
 360
 361 #ifdef USE_INTERPOLATED_SP
 362     ffInterpolationType_t ffFromInterpolatedSetpoint;
 363     float ffSmoothFactor;
 364 #endif
 365 } pidRuntime_t;
 366
 367 extern pidRuntime_t pidRuntime;
 368
 369 extern const char pidNames[];
 370
 371 extern pidAxisData_t pidData[3];
 372
 373 extern uint32_t targetPidLooptime;
 374
 375 extern float throttleBoost;
 376 extern pt1Filter_t throttleLpf;
 377
 378 void pidResetIterm(void);
 379 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 380 void pidSetItermAccelerator(float newItermAccelerator);
 381 bool crashRecoveryModeActive(void);
 382 void pidAcroTrainerInit(void);
 383 void pidSetAcroTrainerState(bool newState);
 384 void pidUpdateAntiGravityThrottleFilter(float throttle);
 385 bool pidOsdAntiGravityActive(void);
 386 bool pidOsdAntiGravityMode(void);
 387 void pidSetAntiGravityState(bool newState);
 388 bool pidAntiGravityEnabled(void);
 389 #ifdef USE_THRUST_LINEARIZATION
 390 float pidApplyThrustLinearization(float motorValue);
 391 float pidCompensateThrustLinearization(float throttle);
 392 #endif
 393 #ifdef USE_AIRMODE_LPF
 394 void pidUpdateAirmodeLpf(float currentOffset);
 395 float pidGetAirmodeThrottleOffset();
 396 #endif
 397
 398 #ifdef UNIT_TEST
 399 #include "sensors/acceleration.h"
 400 extern float axisError[XYZ_AXIS_COUNT];
 401 void applyItermRelax(const int axis, const float iterm,
 402     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint);
 403 void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
 404 void rotateItermAndAxisError();
 405 float pidLevel(int axis, const pidProfile_t *pidProfile,
 406     const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
 407 float calcHorizonLevelStrength(void);
 408 #endif
 409 void dynLpfDTermUpdate(float throttle);
 410 void pidSetItermReset(bool enabled);
 411 float pidGetPreviousSetpoint(int axis);
 412 float pidGetDT();
 413 float pidGetPidFrequency();
 414 float pidGetFfBoostFactor();
 415 float pidGetFfSmoothFactor();
 416 float pidGetSpikeLimitInverse();
 417 float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);