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
  25 #include "common/axis.h"
  26 #include "common/chirp.h"
  27 #include "common/filter.h"
  28 #include "common/pwl.h"
  29 #include "common/time.h"
  30
  31 #include "pg/pg.h"
  32
  33 #define MAX_PID_PROCESS_DENOM       16
  34 #define PID_CONTROLLER_BETAFLIGHT   1
  35 #define PID_MIXER_SCALING           1000.0f
  36 #define PID_SERVO_MIXER_SCALING     0.7f
  37 #define PIDSUM_LIMIT                500
  38 #define PIDSUM_LIMIT_YAW            400
  39 #define PIDSUM_LIMIT_MIN            100
  40 #define PIDSUM_LIMIT_MAX            1000
  41
  42 #define PID_GAIN_MAX 250
  43 #define F_GAIN_MAX 1000
  44
  45 // 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
  46 #define PTERM_SCALE 0.032029f
  47 #define ITERM_SCALE 0.244381f
  48 #define DTERM_SCALE 0.000529f
  49
  50 // The constant scale factor to replace the Kd component of the feedforward calculation.
  51 // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
  52 #define FEEDFORWARD_SCALE 0.013754f
  53
  54 // Full iterm suppression in setpoint mode at high-passed setpoint rate > 40deg/sec
  55 #define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
  56 #define ITERM_RELAX_CUTOFF_DEFAULT 15
  57
  58 // Anti gravity I constant
  59 #define ANTIGRAVITY_KI 0.34f; // if AG gain is 6, about 6 times iTerm will be added
  60 #define ANTIGRAVITY_KP 0.0034f; // one fifth of the I gain on P by default
  61 #define ITERM_ACCELERATOR_GAIN_OFF 0
  62 #define ITERM_ACCELERATOR_GAIN_MAX 250
  63
  64 #define PID_ROLL_DEFAULT  { 45, 80, 30, 120, 0 }
  65 #define PID_PITCH_DEFAULT { 47, 84, 34, 125, 0 }
  66 #define PID_YAW_DEFAULT   { 45, 80,  0, 120, 0 }
  67 #define D_MAX_DEFAULT     { 40, 46, 0 }
  68
  69 #define DTERM_LPF1_DYN_MIN_HZ_DEFAULT 75
  70 #define DTERM_LPF1_DYN_MAX_HZ_DEFAULT 150
  71 #define DTERM_LPF2_HZ_DEFAULT 150
  72
  73 #define TPA_MAX 100
  74
  75 #ifdef USE_WING
  76 #define ANGLE_PITCH_OFFSET_MAX 450
  77 #define S_TERM_SCALE 0.01f
  78 #define TPA_LOW_RATE_MIN INT8_MIN
  79 #define TPA_GRAVITY_MAX 5000
  80 #define TPA_CURVE_STALL_THROTTLE_MAX 100
  81 #else
  82 #define TPA_LOW_RATE_MIN 0
  83 #endif
  84
  85 #ifdef USE_ADVANCED_TPA
  86 #define TPA_CURVE_PID_MAX 1000
  87 #define TPA_CURVE_EXPO_MIN -100
  88 #define TPA_CURVE_EXPO_MAX 100
  89 #define TPA_CURVE_PWL_SIZE 17
  90 #endif // USE_ADVANCED_TPA
  91
  92 #define G_ACCELERATION 9.80665f // gravitational acceleration in m/s^2
  93
  94 typedef enum {
  95     TPA_MODE_PD,
  96     TPA_MODE_D,
  97 #ifdef USE_WING
  98     TPA_MODE_PDS,
  99 #endif
 100 } tpaMode_e;
 101
 102 typedef enum {
 103     TERM_P,
 104     TERM_I,
 105     TERM_D,
 106     TERM_F,
 107     TERM_S,
 108 } term_e;
 109
 110 typedef enum {
 111     SPA_MODE_OFF,
 112     SPA_MODE_I_FREEZE,
 113     SPA_MODE_I,
 114     SPA_MODE_PID,
 115     SPA_MODE_PD_I_FREEZE,
 116 } spaMode_e;
 117
 118 typedef enum {
 119     PID_ROLL,
 120     PID_PITCH,
 121     PID_YAW,
 122     PID_LEVEL,
 123     PID_MAG,
 124     PID_ITEM_COUNT
 125 } pidIndex_e;
 126
 127 typedef enum {
 128     SUPEREXPO_YAW_OFF = 0,
 129     SUPEREXPO_YAW_ON,
 130     SUPEREXPO_YAW_ALWAYS
 131 } pidSuperExpoYaw_e;
 132
 133 typedef enum {
 134     PID_STABILISATION_OFF = 0,
 135     PID_STABILISATION_ON
 136 } pidStabilisationState_e;
 137
 138 typedef enum {
 139     PID_CRASH_RECOVERY_OFF = 0,
 140     PID_CRASH_RECOVERY_ON,
 141     PID_CRASH_RECOVERY_BEEP,
 142     PID_CRASH_RECOVERY_DISARM,
 143 } pidCrashRecovery_e;
 144
 145 typedef struct pidf_s {
 146     uint8_t P;
 147     uint8_t I;
 148     uint8_t D;
 149     uint16_t F;
 150     uint8_t S;
 151 } pidf_t;
 152
 153 typedef enum {
 154     ITERM_RELAX_OFF,
 155     ITERM_RELAX_RP,
 156     ITERM_RELAX_RPY,
 157     ITERM_RELAX_RP_INC,
 158     ITERM_RELAX_RPY_INC,
 159     ITERM_RELAX_COUNT,
 160 } itermRelax_e;
 161
 162 typedef enum {
 163     ITERM_RELAX_GYRO,
 164     ITERM_RELAX_SETPOINT,
 165     ITERM_RELAX_TYPE_COUNT,
 166 } itermRelaxType_e;
 167
 168 typedef enum feedforwardAveraging_e {
 169     FEEDFORWARD_AVERAGING_OFF,
 170     FEEDFORWARD_AVERAGING_2_POINT,
 171     FEEDFORWARD_AVERAGING_3_POINT,
 172     FEEDFORWARD_AVERAGING_4_POINT,
 173 } feedforwardAveraging_t;
 174
 175 typedef enum tpaCurveType_e {
 176     TPA_CURVE_CLASSIC,
 177     TPA_CURVE_HYPERBOLIC,
 178 } tpaCurveType_t;
 179
 180 typedef enum tpaSpeedType_e {
 181     TPA_SPEED_BASIC,
 182     TPA_SPEED_ADVANCED,
 183 } tpaSpeedType_t;
 184
 185 typedef enum {
 186     YAW_TYPE_RUDDER,
 187     YAW_TYPE_DIFF_THRUST,
 188 } yawType_e;
 189
 190 #define MAX_PROFILE_NAME_LENGTH 8u
 191
 192 typedef struct pidProfile_s {
 193     uint16_t yaw_lowpass_hz;                // Additional yaw filter when yaw axis too noisy
 194     uint16_t dterm_lpf1_static_hz;          // Static Dterm lowpass 1 filter cutoff value in hz
 195     uint16_t dterm_notch_hz;                // Biquad dterm notch hz
 196     uint16_t dterm_notch_cutoff;            // Biquad dterm notch low cutoff
 197
 198     pidf_t  pid[PID_ITEM_COUNT];
 199
 200     uint8_t dterm_lpf1_type;                // Filter type for dterm lowpass 1
 201     uint8_t itermWindup;                    // iterm windup threshold, percentage of pidSumLimit within which to limit iTerm
 202     uint16_t pidSumLimit;                   // pidSum limit value for pitch and roll
 203     uint16_t pidSumLimitYaw;                // pidSum limit value for yaw
 204     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
 205     uint8_t angle_limit;                    // Max angle in degrees in Angle mode
 206
 207     uint8_t horizon_limit_degrees;          // in Horizon mode, zero levelling when the quad's attitude exceeds this angle
 208     uint8_t horizon_ignore_sticks;          // 0 = default, meaning both stick and attitude attenuation; 1 = only attitude attenuation
 209
 210     // Betaflight PID controller parameters
 211     uint8_t anti_gravity_gain;              // AntiGravity Gain (was itermAcceleratorGain)
 212     uint16_t yawRateAccelLimit;             // yaw accel limiter for deg/sec/ms
 213     uint16_t rateAccelLimit;                // accel limiter roll/pitch deg/sec/ms
 214     uint16_t crash_dthreshold;              // dterm crash value
 215     uint16_t crash_gthreshold;              // gyro crash value
 216     uint16_t crash_setpoint_threshold;      // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
 217     uint16_t crash_time;                    // ms
 218     uint16_t crash_delay;                   // ms
 219     uint8_t crash_recovery_angle;           // degrees
 220     uint8_t crash_recovery_rate;            // degree/second
 221     uint16_t crash_limit_yaw;               // limits yaw errorRate, so crashes don't cause huge throttle increase
 222     uint16_t itermLimit;
 223     uint16_t dterm_lpf2_static_hz;          // Static Dterm lowpass 2 filter cutoff value in hz
 224     uint8_t crash_recovery;                 // off, on, on and beeps when it is in crash recovery mode
 225     uint8_t throttle_boost;                 // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
 226     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
 227     uint8_t iterm_rotation;                 // rotates iterm to translate world errors to local coordinate system
 228     uint8_t iterm_relax_type;               // Specifies type of relax algorithm
 229     uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
 230     uint8_t iterm_relax;                    // Enable iterm suppression during stick input
 231     uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
 232     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
 233     uint8_t acro_trainer_gain;              // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
 234     uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
 235     uint8_t abs_control_gain;               // How strongly should the absolute accumulated error be corrected for
 236     uint8_t abs_control_limit;              // Limit to the correction
 237     uint8_t abs_control_error_limit;        // Limit to the accumulated error
 238     uint8_t abs_control_cutoff;             // Cutoff frequency for path estimation in abs control
 239     uint8_t dterm_lpf2_type;                // Filter type for 2nd dterm lowpass
 240     uint16_t dterm_lpf1_dyn_min_hz;         // Dterm lowpass filter 1 min hz when in dynamic mode
 241     uint16_t dterm_lpf1_dyn_max_hz;         // Dterm lowpass filter 1 max hz when in dynamic mode
 242     uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
 243     uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
 244     uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
 245     uint8_t launchControlGain;              // Iterm gain used while launch control is active
 246     uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
 247     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
 248     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
 249     uint8_t thrustLinearization;            // Compensation factor for pid linearization
 250     uint8_t d_max[XYZ_AXIS_COUNT];          // Maximum D value on each axis
 251     uint8_t d_max_gain;                     // Gain factor for amount of gyro / setpoint activity required to boost D
 252     uint8_t d_max_advance;                  // Percentage multiplier for setpoint input to boost algorithm
 253     uint8_t motor_output_limit;             // Upper limit of the motor output (percent)
 254     int8_t auto_profile_cell_count;         // Cell count for this profile to be used with if auto PID profile switching is used
 255     uint8_t transient_throttle_limit;       // Maximum DC component of throttle change to mix into throttle to prevent airmode mirroring noise
 256     char profileName[MAX_PROFILE_NAME_LENGTH + 1]; // Descriptive name for profile
 257
 258     uint8_t dyn_idle_min_rpm;               // minimum motor speed enforced by the dynamic idle controller
 259     uint8_t dyn_idle_p_gain;                // P gain during active control of rpm
 260     uint8_t dyn_idle_i_gain;                // I gain during active control of rpm
 261     uint8_t dyn_idle_d_gain;                // D gain for corrections around rapid changes in rpm
 262     uint8_t dyn_idle_max_increase;          // limit on maximum possible increase in motor idle drive during active control
 263
 264     uint8_t feedforward_transition;         // Feedforward attenuation around centre sticks
 265     uint8_t feedforward_averaging;          // Number of packets to average when averaging is on
 266     uint8_t feedforward_smooth_factor;      // Amount of lowpass type smoothing for feedforward steps
 267     uint8_t feedforward_jitter_factor;      // Number of RC steps below which to attenuate feedforward
 268     uint8_t feedforward_boost;              // amount of setpoint acceleration to add to feedforward, 10 means 100% added
 269     uint8_t feedforward_max_rate_limit;     // Maximum setpoint rate percentage for feedforward
 270     uint8_t feedforward_yaw_hold_gain;          // Amount of sustained high-pass yaw setpoint to add to feedforward, zero disables
 271     uint8_t feedforward_yaw_hold_time ;     // Time constant of the sustained yaw hold element in ms to add to feed forward, higher values decay slower
 272
 273     uint8_t dterm_lpf1_dyn_expo;            // set the curve for dynamic dterm lowpass filter
 274     uint8_t level_race_mode;                // NFE race mode - when true pitch setpoint calculation is gyro based in level mode
 275     uint8_t vbat_sag_compensation;          // Reduce motor output by this percentage of the maximum compensation amount
 276
 277     uint8_t simplified_pids_mode;
 278     uint8_t simplified_master_multiplier;
 279     uint8_t simplified_roll_pitch_ratio;
 280     uint8_t simplified_i_gain;
 281     uint8_t simplified_d_gain;
 282     uint8_t simplified_pi_gain;
 283     uint8_t simplified_d_max_gain;
 284     uint8_t simplified_feedforward_gain;
 285     uint8_t simplified_dterm_filter;
 286     uint8_t simplified_dterm_filter_multiplier;
 287     uint8_t simplified_pitch_pi_gain;
 288
 289     uint8_t anti_gravity_cutoff_hz;
 290     uint8_t anti_gravity_p_gain;
 291     uint8_t tpa_mode;                       // Controls which PID terms TPA effects
 292     uint8_t tpa_rate;                       // Percent reduction in P or D at full throttle
 293     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
 294
 295     uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
 296     uint8_t angle_earth_ref;                // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
 297     uint16_t horizon_delay_ms;              // delay when Horizon Strength increases, 50 = 500ms time constant
 298     int8_t tpa_low_rate;                    // Percent reduction in P or D at zero throttle
 299     uint16_t tpa_low_breakpoint;            // Breakpoint where lower TPA is deactivated
 300     uint8_t tpa_low_always;                 // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time
 301
 302     uint8_t ez_landing_threshold;           // Threshold stick position below which motor output is limited
 303     uint8_t ez_landing_limit;               // Maximum motor output when all sticks centred and throttle zero
 304     uint8_t ez_landing_speed;               // Speed below which motor output is limited
 305     uint8_t landing_disarm_threshold;            // Accelerometer vector delta (jerk) threshold with disarms if exceeded
 306
 307     uint16_t spa_center[XYZ_AXIS_COUNT];    // RPY setpoint at which PIDs are reduced to 50% (setpoint PID attenuation)
 308     uint16_t spa_width[XYZ_AXIS_COUNT];     // Width of smooth transition around spa_center
 309     uint8_t spa_mode[XYZ_AXIS_COUNT];       // SPA mode for each axis
 310     uint8_t tpa_curve_type;                 // Classic type - for multirotor, hyperbolic - usually for wings
 311     uint8_t tpa_curve_stall_throttle;       // For wings: speed at which PIDs should be maxed out (stall speed)
 312     uint16_t tpa_curve_pid_thr0;            // For wings: PIDs multiplier at stall speed
 313     uint16_t tpa_curve_pid_thr100;          // For wings: PIDs multiplier at full speed
 314     int8_t tpa_curve_expo;                  // For wings: how fast PIDs do transition as speed grows
 315     uint8_t tpa_speed_type;             // For wings: relative air speed estimation model type
 316     uint16_t tpa_speed_basic_delay;     // For wings when tpa_speed_type = BASIC: delay of air speed estimation from throttle in milliseconds (time of reaching 50% of terminal speed in horizontal flight at full throttle)
 317     uint16_t tpa_speed_basic_gravity;   // For wings when tpa_speed_type = BASIC: gravity effect on air speed estimation in percents
 318     uint16_t tpa_speed_adv_prop_pitch;  // For wings when tpa_speed_type = ADVANCED: prop pitch in inches * 100
 319     uint16_t tpa_speed_adv_mass;        // For wings when tpa_speed_type = ADVANCED: craft mass in grams
 320     uint16_t tpa_speed_adv_drag_k;      // For wings when tpa_speed_type = ADVANCED: craft drag coefficient
 321     uint16_t tpa_speed_adv_thrust;      // For wings when tpa_speed_type = ADVANCED: stationary thrust in grams
 322     uint16_t tpa_speed_max_voltage;     // For wings: theoretical max voltage; used for throttle scailing with voltage for air speed estimation
 323     int16_t tpa_speed_pitch_offset;     // For wings: pitch offset in degrees*10 for craft speed estimation
 324     uint8_t yaw_type;                   // For wings: type of yaw (rudder or differential thrust)
 325     int16_t angle_pitch_offset;         // For wings: pitch offset for angle modes; in decidegrees; positive values tilting the wing down
 326
 327     uint8_t chirp_lag_freq_hz;              // leadlag1Filter cutoff/pole to shape the excitation signal
 328     uint8_t chirp_lead_freq_hz;             // leadlag1Filter cutoff/zero
 329     uint16_t chirp_amplitude_roll;          // amplitude roll in degree/second
 330     uint16_t chirp_amplitude_pitch;         // amplitude pitch in degree/second
 331     uint16_t chirp_amplitude_yaw;           // amplitude yaw in degree/second
 332     uint16_t chirp_frequency_start_deci_hz; // start frequency in units of 0.1 hz
 333     uint16_t chirp_frequency_end_deci_hz;   // end frequency in units of 0.1 hz
 334     uint8_t chirp_time_seconds;             // excitation time
 335 } pidProfile_t;
 336
 337 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
 338
 339 typedef struct pidConfig_s {
 340     uint8_t pid_process_denom;                   // Processing denominator for PID controller vs gyro sampling rate
 341     uint8_t runaway_takeoff_prevention;          // off, on - enables pidsum runaway disarm logic
 342     uint16_t runaway_takeoff_deactivate_delay;   // delay in ms for "in-flight" conditions before deactivation (successful flight)
 343     uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
 344 } pidConfig_t;
 345
 346 PG_DECLARE(pidConfig_t, pidConfig);
 347
 348 union rollAndPitchTrims_u;
 349 void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
 350
 351 typedef struct pidAxisData_s {
 352     float P;
 353     float I;
 354     float D;
 355     float F;
 356     float S;
 357
 358     float Sum;
 359 } pidAxisData_t;
 360
 361 typedef union dtermLowpass_u {
 362     pt1Filter_t pt1Filter;
 363     biquadFilter_t biquadFilter;
 364     pt2Filter_t pt2Filter;
 365     pt3Filter_t pt3Filter;
 366 } dtermLowpass_t;
 367
 368 typedef struct pidCoefficient_s {
 369     float Kp;
 370     float Ki;
 371     float Kd;
 372     float Kf;
 373 } pidCoefficient_t;
 374
 375 typedef struct tpaSpeedParams_s {
 376     float maxSpeed;
 377     float dragMassRatio;
 378     float inversePropMaxSpeed;
 379     float twr;
 380     float speed;
 381     float maxVoltage;
 382     float pitchOffset;
 383 } tpaSpeedParams_t;
 384
 385 typedef struct pidRuntime_s {
 386     float dT;
 387     float pidFrequency;
 388     bool pidStabilisationEnabled;
 389     float previousPidSetpoint[XYZ_AXIS_COUNT];
 390     filterApplyFnPtr dtermNotchApplyFn;
 391     biquadFilter_t dtermNotch[XYZ_AXIS_COUNT];
 392     filterApplyFnPtr dtermLowpassApplyFn;
 393     dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
 394     filterApplyFnPtr dtermLowpass2ApplyFn;
 395     dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
 396     filterApplyFnPtr ptermYawLowpassApplyFn;
 397     pt1Filter_t ptermYawLowpass;
 398     bool antiGravityEnabled;
 399     pt2Filter_t antiGravityLpf;
 400     float antiGravityOsdCutoff;
 401     float antiGravityThrottleD;
 402     float itermAccelerator;
 403     uint8_t antiGravityGain;
 404     float antiGravityPGain;
 405     pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
 406     float angleGain;
 407     float angleFeedforwardGain;
 408     float horizonGain;
 409     float horizonLimitSticks;
 410     float horizonLimitSticksInv;
 411     float horizonLimitDegrees;
 412     float horizonLimitDegreesInv;
 413     float horizonIgnoreSticks;
 414     float maxVelocity[XYZ_AXIS_COUNT];
 415     bool inCrashRecoveryMode;
 416     timeUs_t crashDetectedAtUs;
 417     timeDelta_t crashTimeLimitUs;
 418     timeDelta_t crashTimeDelayUs;
 419     int32_t crashRecoveryAngleDeciDegrees;
 420     float crashRecoveryRate;
 421     float crashGyroThreshold;
 422     float crashDtermThreshold;
 423     float crashSetpointThreshold;
 424     float crashLimitYaw;
 425     float itermLimit;
 426     float itermLimitYaw;
 427     bool itermRotation;
 428     bool zeroThrottleItermReset;
 429     bool levelRaceMode;
 430     float tpaFactor;
 431     float tpaBreakpoint;
 432     float tpaMultiplier;
 433     float tpaLowBreakpoint;
 434     float tpaLowMultiplier;
 435     bool tpaLowAlways;
 436     bool useEzDisarm;
 437     float landingDisarmThreshold;
 438
 439 #ifdef USE_ITERM_RELAX
 440     pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 441     uint8_t itermRelax;
 442     uint8_t itermRelaxType;
 443     uint8_t itermRelaxCutoff;
 444 #endif
 445
 446 #ifdef USE_ABSOLUTE_CONTROL
 447     float acCutoff;
 448     float acGain;
 449     float acLimit;
 450     float acErrorLimit;
 451     pt1Filter_t acLpf[XYZ_AXIS_COUNT];
 452     float oldSetpointCorrection[XYZ_AXIS_COUNT];
 453 #endif
 454
 455 #ifdef USE_D_MAX
 456     pt2Filter_t dMaxRange[XYZ_AXIS_COUNT];
 457     pt2Filter_t dMaxLowpass[XYZ_AXIS_COUNT];
 458     float dMaxPercent[XYZ_AXIS_COUNT];
 459     uint8_t dMax[XYZ_AXIS_COUNT];
 460     float dMaxGyroGain;
 461     float dMaxSetpointGain;
 462 #endif
 463
 464 #ifdef USE_AIRMODE_LPF
 465     pt1Filter_t airmodeThrottleLpf1;
 466     pt1Filter_t airmodeThrottleLpf2;
 467 #endif
 468
 469 #ifdef USE_ACRO_TRAINER
 470     float acroTrainerAngleLimit;
 471     float acroTrainerLookaheadTime;
 472     uint8_t acroTrainerDebugAxis;
 473     float acroTrainerGain;
 474     bool acroTrainerActive;
 475     int acroTrainerAxisState[RP_AXIS_COUNT];  // only need roll and pitch
 476 #endif
 477
 478 #ifdef USE_DYN_LPF
 479     uint8_t dynLpfFilter;
 480     uint16_t dynLpfMin;
 481     uint16_t dynLpfMax;
 482     uint8_t dynLpfCurveExpo;
 483 #endif
 484
 485 #ifdef USE_LAUNCH_CONTROL
 486     uint8_t launchControlMode;
 487     uint8_t launchControlAngleLimit;
 488     float launchControlKi;
 489 #endif
 490
 491 #ifdef USE_INTEGRATED_YAW_CONTROL
 492     bool useIntegratedYaw;
 493     uint8_t integratedYawRelax;
 494 #endif
 495
 496 #ifdef USE_THRUST_LINEARIZATION
 497     float thrustLinearization;
 498     float throttleCompensateAmount;
 499 #endif
 500
 501 #ifdef USE_AIRMODE_LPF
 502     float airmodeThrottleOffsetLimit;
 503 #endif
 504
 505 #ifdef USE_FEEDFORWARD
 506     feedforwardAveraging_t feedforwardAveraging;
 507     float feedforwardSmoothFactor;
 508     uint8_t feedforwardJitterFactor;
 509     float feedforwardJitterFactorInv;
 510     float feedforwardBoostFactor;
 511     float feedforwardTransition;
 512     float feedforwardTransitionInv;
 513     uint8_t feedforwardMaxRateLimit;
 514     float feedforwardYawHoldGain;
 515     float feedforwardYawHoldTime;
 516     bool feedforwardInterpolate; // Whether to interpolate an FF value for duplicate/identical data values
 517     pt3Filter_t angleFeedforwardPt3[XYZ_AXIS_COUNT];
 518 #endif
 519
 520 #ifdef USE_ACC
 521     pt3Filter_t attitudeFilter[RP_AXIS_COUNT];  // Only for ROLL and PITCH
 522     pt1Filter_t horizonSmoothingPt1;
 523     uint16_t horizonDelayMs;
 524     float angleYawSetpoint;
 525     float angleEarthRef;
 526     float angleTarget[RP_AXIS_COUNT];
 527     bool axisInAngleMode[3];
 528 #endif
 529
 530 #ifdef USE_WING
 531     float spa[XYZ_AXIS_COUNT]; // setpoint pid attenuation (0.0 to 1.0). 0 - full attenuation, 1 - no attenuation
 532     tpaSpeedParams_t tpaSpeed;
 533     float tpaFactorYaw;
 534     float tpaFactorSterm[XYZ_AXIS_COUNT];
 535 #endif // USE_WING
 536
 537 #ifdef USE_ADVANCED_TPA
 538     pwl_t tpaCurvePwl;
 539     float tpaCurvePwl_yValues[TPA_CURVE_PWL_SIZE];
 540     tpaCurveType_t tpaCurveType;
 541 #endif // USE_ADVANCED_TPA
 542
 543 #ifdef USE_CHIRP
 544     chirp_t chirp;
 545     phaseComp_t chirpFilter;
 546     float chirpLagFreqHz;
 547     float chirpLeadFreqHz;
 548     float chirpAmplitude[3];
 549     float chirpFrequencyStartHz;
 550     float chirpFrequencyEndHz;
 551     float chirpTimeSeconds;
 552 #endif // USE_CHIRP
 553 } pidRuntime_t;
 554
 555 extern pidRuntime_t pidRuntime;
 556
 557 extern const char pidNames[];
 558
 559 extern pidAxisData_t pidData[3];
 560
 561 extern uint32_t targetPidLooptime;
 562
 563 extern float throttleBoost;
 564 extern pt1Filter_t throttleLpf;
 565
 566 void resetPidProfile(pidProfile_t *profile);
 567
 568 void pidResetIterm(void);
 569 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 570 void pidSetItermAccelerator(float newItermAccelerator);
 571 bool crashRecoveryModeActive(void);
 572 void pidAcroTrainerInit(void);
 573 void pidSetAcroTrainerState(bool newState);
 574 void pidUpdateTpaFactor(float throttle);
 575 void pidUpdateAntiGravityThrottleFilter(float throttle);
 576 bool pidOsdAntiGravityActive(void);
 577 void pidSetAntiGravityState(bool newState);
 578 bool pidAntiGravityEnabled(void);
 579
 580 #ifdef USE_THRUST_LINEARIZATION
 581 float pidApplyThrustLinearization(float motorValue);
 582 float pidCompensateThrustLinearization(float throttle);
 583 #endif
 584
 585 #ifdef USE_AIRMODE_LPF
 586 void pidUpdateAirmodeLpf(float currentOffset);
 587 float pidGetAirmodeThrottleOffset(void);
 588 #endif
 589
 590 #ifdef UNIT_TEST
 591 #include "sensors/acceleration.h"
 592 extern float axisError[XYZ_AXIS_COUNT];
 593 void applyItermRelax(const int axis, const float iterm,
 594     const float gyroRate, float *itermErrorRate, float *currentPidSetpoint);
 595 void applyAbsoluteControl(const int axis, const float gyroRate, float *currentPidSetpoint, float *itermErrorRate);
 596 void rotateItermAndAxisError();
 597 float pidLevel(int axis, const pidProfile_t *pidProfile,
 598     const rollAndPitchTrims_t *angleTrim, float rawSetpoint, float horizonLevelStrength);
 599 float calcHorizonLevelStrength(void);
 600 #endif
 601
 602 void dynLpfDTermUpdate(float throttle);
 603 void pidSetItermReset(bool enabled);
 604 float pidGetPreviousSetpoint(int axis);
 605 float pidGetDT(void);
 606 float pidGetPidFrequency(void);
 607
 608 float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
 609 #ifdef USE_CHIRP
 610 bool  pidChirpIsFinished();
 611 #endif