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 #define PID_GAIN_MAX 250
  40 #define F_GAIN_MAX 1000
  41 #define D_MIN_GAIN_MAX 250
  42
  43 // 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
  44 #define PTERM_SCALE 0.032029f
  45 #define ITERM_SCALE 0.244381f
  46 #define DTERM_SCALE 0.000529f
  47
  48 // The constant scale factor to replace the Kd component of the feedforward calculation.
  49 // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
  50 #define FEEDFORWARD_SCALE 0.013754f
  51
  52 // Full iterm suppression in setpoint mode at high-passed setpoint rate > 40deg/sec
  53 #define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
  54 #define ITERM_RELAX_CUTOFF_DEFAULT 15
  55
  56 // Anti gravity I constant
  57 #define ANTIGRAVITY_KI 0.34f; // if AG gain is 6, about 6 times iTerm will be added
  58 #define ANTIGRAVITY_KP 0.0034f; // one fifth of the I gain on P by default
  59 #define ITERM_ACCELERATOR_GAIN_OFF 0
  60 #define ITERM_ACCELERATOR_GAIN_MAX 250
  61
  62 #define PID_ROLL_DEFAULT  { 45, 80, 40, 120 }
  63 #define PID_PITCH_DEFAULT { 47, 84, 46, 125 }
  64 #define PID_YAW_DEFAULT   { 45, 80,  0, 120 }
  65 #define D_MIN_DEFAULT     { 30, 34, 0 }
  66
  67 #define DTERM_LPF1_DYN_MIN_HZ_DEFAULT 75
  68 #define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
  69 #define DTERM_LPF2_HZ_DEFAULT 150
  70
  71 #define TPA_MAX 100
  72
  73 typedef enum {
  74     TPA_MODE_PD,
  75     TPA_MODE_D
  76 } tpaMode_e;
  77
  78 typedef enum {
  79     PID_ROLL,
  80     PID_PITCH,
  81     PID_YAW,
  82     PID_LEVEL,
  83     PID_MAG,
  84     PID_ITEM_COUNT
  85 } pidIndex_e;
  86
  87 typedef enum {
  88     SUPEREXPO_YAW_OFF = 0,
  89     SUPEREXPO_YAW_ON,
  90     SUPEREXPO_YAW_ALWAYS
  91 } pidSuperExpoYaw_e;
  92
  93 typedef enum {
  94     PID_STABILISATION_OFF = 0,
  95     PID_STABILISATION_ON
  96 } pidStabilisationState_e;
  97
  98 typedef enum {
  99     PID_CRASH_RECOVERY_OFF = 0,
 100     PID_CRASH_RECOVERY_ON,
 101     PID_CRASH_RECOVERY_BEEP,
 102     PID_CRASH_RECOVERY_DISARM,
 103 } pidCrashRecovery_e;
 104
 105 typedef struct pidf_s {
 106     uint8_t P;
 107     uint8_t I;
 108     uint8_t D;
 109     uint16_t F;
 110 } pidf_t;
 111
 112 typedef enum {
 113     ITERM_RELAX_OFF,
 114     ITERM_RELAX_RP,
 115     ITERM_RELAX_RPY,
 116     ITERM_RELAX_RP_INC,
 117     ITERM_RELAX_RPY_INC,
 118     ITERM_RELAX_COUNT,
 119 } itermRelax_e;
 120
 121 typedef enum {
 122     ITERM_RELAX_GYRO,
 123     ITERM_RELAX_SETPOINT,
 124     ITERM_RELAX_TYPE_COUNT,
 125 } itermRelaxType_e;
 126
 127 typedef enum feedforwardAveraging_e {
 128     FEEDFORWARD_AVERAGING_OFF,
 129     FEEDFORWARD_AVERAGING_2_POINT,
 130     FEEDFORWARD_AVERAGING_3_POINT,
 131     FEEDFORWARD_AVERAGING_4_POINT,
 132 } feedforwardAveraging_t;
 133
 134 #define MAX_PROFILE_NAME_LENGTH 8u
 135
 136 typedef struct pidProfile_s {
 137     uint16_t yaw_lowpass_hz;                // Additional yaw filter when yaw axis too noisy
 138     uint16_t dterm_lpf1_static_hz;          // Static Dterm lowpass 1 filter cutoff value in hz
 139     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
 140     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
 141
 142     pidf_t  pid[PID_ITEM_COUNT];
 143
 144     uint8_t dterm_lpf1_type;                // Filter type for dterm lowpass 1
 145     uint8_t itermWindupPointPercent;        // iterm windup threshold, percent motor saturation
 146     uint16_t pidSumLimit;
 147     uint16_t pidSumLimitYaw;
 148     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
 149     uint8_t angle_limit;                    // Max angle in degrees in Angle mode
 150
 151     uint8_t horizon_limit_degrees;          // in Horizon mode, zero levelling when the quad's attitude exceeds this angle
 152     uint8_t horizon_ignore_sticks;          // 0 = default, meaning both stick and attitude attenuation; 1 = only attitude attenuation
 153
 154     // Betaflight PID controller parameters
 155     uint8_t anti_gravity_gain;              // AntiGravity Gain (was itermAcceleratorGain)
 156     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
 157     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
 158     uint16_t crash_dthreshold;              // dterm crash value
 159     uint16_t crash_gthreshold;              // gyro crash value
 160     uint16_t crash_setpoint_threshold;      // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
 161     uint16_t crash_time;                    // ms
 162     uint16_t crash_delay;                   // ms
 163     uint8_t crash_recovery_angle;           // degrees
 164     uint8_t crash_recovery_rate;            // degree/second
 165     uint16_t crash_limit_yaw;               // limits yaw errorRate, so crashes don't cause huge throttle increase
 166     uint16_t itermLimit;
 167     uint16_t dterm_lpf2_static_hz;          // Static Dterm lowpass 2 filter cutoff value in hz
 168     uint8_t crash_recovery;                 // off, on, on and beeps when it is in crash recovery mode
 169     uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
 170     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
 171     uint8_t iterm_rotation;                 // rotates iterm to translate world errors to local coordinate system
 172     uint8_t iterm_relax_type;               // Specifies type of relax algorithm
 173     uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
 174     uint8_t iterm_relax;                    // Enable iterm suppression during stick input
 175     uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
 176     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
 177     uint8_t acro_trainer_gain;              // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
 178     uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
 179     uint8_t abs_control_gain;               // How strongly should the absolute accumulated error be corrected for
 180     uint8_t abs_control_limit;              // Limit to the correction
 181     uint8_t abs_control_error_limit;        // Limit to the accumulated error
 182     uint8_t abs_control_cutoff;             // Cutoff frequency for path estimation in abs control
 183     uint8_t dterm_lpf2_type;                // Filter type for 2nd dterm lowpass
 184     uint16_t dterm_lpf1_dyn_min_hz;         // Dterm lowpass filter 1 min hz when in dynamic mode
 185     uint16_t dterm_lpf1_dyn_max_hz;         // Dterm lowpass filter 1 max hz when in dynamic mode
 186     uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
 187     uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
 188     uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
 189     uint8_t launchControlGain;              // Iterm gain used while launch control is active
 190     uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
 191     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
 192     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
 193     uint8_t thrustLinearization;            // Compensation factor for pid linearization
 194     uint8_t d_min[XYZ_AXIS_COUNT];          // Minimum D value on each axis
 195     uint8_t d_min_gain;                     // Gain factor for amount of gyro / setpoint activity required to boost D
 196     uint8_t d_min_advance;                  // Percentage multiplier for setpoint input to boost algorithm
 197     uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
 198     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
 199     uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
 200     char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
 201
 202     uint8_t dyn_idle_min_rpm;               // minimum motor speed enforced by the dynamic idle controller
 203     uint8_t dyn_idle_p_gain;                // P gain during active control of rpm
 204     uint8_t dyn_idle_i_gain;                // I gain during active control of rpm
 205     uint8_t dyn_idle_d_gain;                // D gain for corrections around rapid changes in rpm
 206     uint8_t dyn_idle_max_increase;          // limit on maximum possible increase in motor idle drive during active control
 207     uint8_t dyn_idle_start_increase;        // limit on maximum possible increase in motor idle drive with airmode not activated
 208
 209     uint8_t feedforward_transition;         // Feedforward attenuation around centre sticks
 210     uint8_t feedforward_averaging;          // Number of packets to average when averaging is on
 211     uint8_t feedforward_smooth_factor;      // Amount of lowpass type smoothing for feedforward steps
 212     uint8_t feedforward_jitter_factor;      // Number of RC steps below which to attenuate feedforward
 213     uint8_t feedforward_boost;              // amount of setpoint acceleration to add to feedforward, 10 means 100% added
 214     uint8_t feedforward_max_rate_limit;     // Maximum setpoint rate percentage for feedforward
 215
 216     uint8_t dterm_lpf1_dyn_expo;            // set the curve for dynamic dterm lowpass filter
 217     uint8_t level_race_mode;                // NFE race mode - when true pitch setpoint calculation is gyro based in level mode
 218     uint8_t vbat_sag_compensation;          // Reduce motor output by this percentage of the maximum compensation amount
 219
 220     uint8_t simplified_pids_mode;
 221     uint8_t simplified_master_multiplier;
 222     uint8_t simplified_roll_pitch_ratio;
 223     uint8_t simplified_i_gain;
 224     uint8_t simplified_d_gain;
 225     uint8_t simplified_pi_gain;
 226     uint8_t simplified_dmin_ratio;
 227     uint8_t simplified_feedforward_gain;
 228     uint8_t simplified_dterm_filter;
 229     uint8_t simplified_dterm_filter_multiplier;
 230     uint8_t simplified_pitch_pi_gain;
 231
 232     uint8_t anti_gravity_cutoff_hz;
 233     uint8_t anti_gravity_p_gain;
 234     uint8_t tpa_mode;                       // Controls which PID terms TPA effects
 235     uint8_t tpa_rate;                       // Percent reduction in P or D at full throttle
 236     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
 237     uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
 238     uint8_t angle_earth_ref;         // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
 239     uint16_t horizon_delay_ms;           // delay when Horizon Strength increases, 50 = 500ms time constant
 240     uint8_t tpa_low_rate;                 // Percent reduction in P or D at zero throttle
 241     uint16_t tpa_low_breakpoint;          // Breakpoint where lower TPA is deactivated
 242     uint8_t tpa_low_always;      // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time
 243 } pidProfile_t;
 244
 245 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
 246
 247 typedef struct pidConfig_s {
 248     uint8_t pid_process_denom;                   // Processing denominator for PID controller vs gyro sampling rate
 249     uint8_t runaway_takeoff_prevention;          // off, on - enables pidsum runaway disarm logic
 250     uint16_t runaway_takeoff_deactivate_delay;   // delay in ms for "in-flight" conditions before deactivation (successful flight)
 251     uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
 252 } pidConfig_t;
 253
 254 PG_DECLARE(pidConfig_t, pidConfig);
 255
 256 union rollAndPitchTrims_u;
 257 void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
 258
 259 typedef struct pidAxisData_s {
 260     float P;
 261     float I;
 262     float D;
 263     float F;
 264
 265     float Sum;
 266 } pidAxisData_t;
 267
 268 typedef union dtermLowpass_u {
 269     pt1Filter_t pt1Filter;
 270     biquadFilter_t biquadFilter;
 271     pt2Filter_t pt2Filter;
 272     pt3Filter_t pt3Filter;
 273 } dtermLowpass_t;
 274
 275 typedef struct pidCoefficient_s {
 276     float Kp;
 277     float Ki;
 278     float Kd;
 279     float Kf;
 280 } pidCoefficient_t;
 281
 282 typedef struct pidRuntime_s {
 283     float dT;
 284     float pidFrequency;
 285     bool pidStabilisationEnabled;
 286     float previousPidSetpoint[XYZ_AXIS_COUNT];
 287     filterApplyFnPtr dtermNotchApplyFn;
 288     biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
 289     filterApplyFnPtr dtermLowpassApplyFn;
 290     dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
 291     filterApplyFnPtr dtermLowpass2ApplyFn;
 292     dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
 293     filterApplyFnPtr ptermYawLowpassApplyFn;
 294     pt1Filter_t ptermYawLowpass;
 295     bool antiGravityEnabled;
 296     pt2Filter_t antiGravityLpf;
 297     float antiGravityOsdCutoff;
 298     float antiGravityThrottleD;
 299     float itermAccelerator;
 300     uint8_t antiGravityGain;
 301     float antiGravityPGain;
 302     pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
 303     float angleGain;
 304     float angleFeedforwardGain;
 305     float horizonGain;
 306     float horizonLimitSticks;
 307     float horizonLimitSticksInv;
 308     float horizonLimitDegrees;
 309     float horizonLimitDegreesInv;
 310     float horizonIgnoreSticks;
 311     float maxVelocity[XYZ_AXIS_COUNT];
 312     float itermWindupPointInv;
 313     bool inCrashRecoveryMode;
 314     timeUs_t crashDetectedAtUs;
 315     timeDelta_t crashTimeLimitUs;
 316     timeDelta_t crashTimeDelayUs;
 317     int32_t crashRecoveryAngleDeciDegrees;
 318     float crashRecoveryRate;
 319     float crashGyroThreshold;
 320     float crashDtermThreshold;
 321     float crashSetpointThreshold;
 322     float crashLimitYaw;
 323     float itermLimit;
 324     bool itermRotation;
 325     bool zeroThrottleItermReset;
 326     bool levelRaceMode;
 327     float tpaFactor;
 328     float tpaBreakpoint;
 329     float tpaMultiplier;
 330     float tpaLowBreakpoint;
 331     float tpaLowMultiplier;
 332     bool tpaLowAlways;
 333
 334 #ifdef USE_ITERM_RELAX
 335     pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 336     uint8_t itermRelax;
 337     uint8_t itermRelaxType;
 338     uint8_t itermRelaxCutoff;
 339 #endif
 340
 341 #ifdef USE_ABSOLUTE_CONTROL
 342     float acCutoff;
 343     float acGain;
 344     float acLimit;
 345     float acErrorLimit;
 346     pt1Filter_t acLpf[XYZ_AXIS_COUNT];
 347     float oldSetpointCorrection[XYZ_AXIS_COUNT];
 348 #endif
 349
 350 #ifdef USE_D_MIN
 351     pt2Filter_t dMinRange[XYZ_AXIS_COUNT];
 352     pt2Filter_t dMinLowpass[XYZ_AXIS_COUNT];
 353     float dMinPercent[XYZ_AXIS_COUNT];
 354     float dMinGyroGain;
 355     float dMinSetpointGain;
 356 #endif
 357
 358 #ifdef USE_AIRMODE_LPF
 359     pt1Filter_t airmodeThrottleLpf1;
 360     pt1Filter_t airmodeThrottleLpf2;
 361 #endif
 362
 363 #ifdef USE_ACRO_TRAINER
 364     float acroTrainerAngleLimit;
 365     float acroTrainerLookaheadTime;
 366     uint8_t acroTrainerDebugAxis;
 367     float acroTrainerGain;
 368     bool acroTrainerActive;
 369     int acroTrainerAxisState[2];  // only need roll and pitch
 370 #endif
 371
 372 #ifdef USE_DYN_LPF
 373     uint8_t dynLpfFilter;
 374     uint16_t dynLpfMin;
 375     uint16_t dynLpfMax;
 376     uint8_t dynLpfCurveExpo;
 377 #endif
 378
 379 #ifdef USE_LAUNCH_CONTROL
 380     uint8_t launchControlMode;
 381     uint8_t launchControlAngleLimit;
 382     float launchControlKi;
 383 #endif
 384
 385 #ifdef USE_INTEGRATED_YAW_CONTROL
 386     bool useIntegratedYaw;
 387     uint8_t integratedYawRelax;
 388 #endif
 389
 390 #ifdef USE_THRUST_LINEARIZATION
 391     float thrustLinearization;
 392     float throttleCompensateAmount;
 393 #endif
 394
 395 #ifdef USE_AIRMODE_LPF
 396     float airmodeThrottleOffsetLimit;
 397 #endif
 398
 399 #ifdef USE_FEEDFORWARD
 400     feedforwardAveraging_t feedforwardAveraging;
 401     float feedforwardSmoothFactor;
 402     uint8_t feedforwardJitterFactor;
 403     float feedforwardJitterFactorInv;
 404     float feedforwardBoostFactor;
 405     float feedforwardTransition;
 406     float feedforwardTransitionInv;
 407     uint8_t feedforwardMaxRateLimit;
 408     pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
 409 #endif
 410
 411 #ifdef USE_ACC
 412     pt3Filter_t attitudeFilter[2];  // Only for ROLL and PITCH
 413     pt1Filter_t horizonSmoothingPt1;
 414     uint16_t horizonDelayMs;
 415     float angleYawSetpoint;
 416     float angleEarthRef;
 417     float angleTarget[2];
 418     bool axisInAngleMode[3];
 419     float maxRcRateInv[2];
 420 #endif
 421 } pidRuntime_t;
 422
 423 extern pidRuntime_t pidRuntime;
 424
 425 extern const char pidNames[];
 426
 427 extern pidAxisData_t pidData[3];
 428
 429 extern uint32_t targetPidLooptime;
 430
 431 extern float throttleBoost;
 432 extern pt1Filter_t throttleLpf;
 433
 434 void resetPidProfile(pidProfile_t *profile);
 435
 436 void pidResetIterm(void);
 437 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 438 void pidSetItermAccelerator(float newItermAccelerator);
 439 bool crashRecoveryModeActive(void);
 440 void pidAcroTrainerInit(void);
 441 void pidSetAcroTrainerState(bool newState);
 442 void pidUpdateTpaFactor(float throttle);
 443 void pidUpdateAntiGravityThrottleFilter(float throttle);
 444 bool pidOsdAntiGravityActive(void);
 445 void pidSetAntiGravityState(bool newState);
 446 bool pidAntiGravityEnabled(void);
 447
 448 #ifdef USE_THRUST_LINEARIZATION
 449 float pidApplyThrustLinearization(float motorValue);
 450 float pidCompensateThrustLinearization(float throttle);
 451 #endif
 452
 453 #ifdef USE_AIRMODE_LPF
 454 void pidUpdateAirmodeLpf(float currentOffset);
 455 float pidGetAirmodeThrottleOffset();
 456 #endif
 457
 458 #ifdef UNIT_TEST
 459 #include "sensors/acceleration.h"
 460 extern float axisError[XYZ_AXIS_COUNT];
 461 void applyItermRelax(const int axis, const float iterm,
 462     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint);
 463 void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
 464 void rotateItermAndAxisError();
 465 float pidLevel(int axis, const pidProfile_t *pidProfile,
 466     const rollAndPitchTrims_t *angleTrim, float rawSetpoint, float horizonLevelStrength);
 467 float calcHorizonLevelStrength(void);
 468 #endif
 469
 470 void dynLpfDTermUpdate(float throttle);
 471 void pidSetItermReset(bool enabled);
 472 float pidGetPreviousSetpoint(int axis);
 473 float pidGetDT();
 474 float pidGetPidFrequency();
 475
 476 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);