diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ef197e6838..7c527763c3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -93,7 +93,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint #endif // USE_ACRO_TRAINER -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4); void resetPidProfile(pidProfile_t *pidProfile) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3583023230..8835743843 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -86,17 +86,17 @@ typedef enum typedef struct pidProfile_s { - pid8_t pid[PID_ITEM_COUNT]; - - uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy - uint16_t dterm_lowpass_hz; // Delta Filter in hz + uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy + uint16_t dterm_lowpass_hz; // Delta Filter in hz uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff + + pid8_t pid[PID_ITEM_COUNT]; + uint8_t dterm_filter_type; // Filter selection for dterm uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation uint16_t pidSumLimit; uint16_t pidSumLimitYaw; - uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. uint8_t levelAngleLimit; // Max angle in degrees in level mode @@ -106,8 +106,7 @@ typedef struct pidProfile_s { // Betaflight PID controller parameters uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit - uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect - uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) + uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value @@ -117,24 +116,26 @@ typedef struct pidProfile_s { uint16_t crash_delay; // ms uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second - pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode + uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage + uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t itermLimit; - uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz + uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz + uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz. uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. - uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation - uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation - itermRelax_e iterm_relax; // Enable iterm suppression during stick input + uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation + uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation + uint8_t iterm_relax; // Enable iterm suppression during stick input uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees - uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch uint8_t acro_trainer_gain; // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit - uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for - uint8_t abs_control_limit; // Limit to the correction - uint8_t abs_control_error_limit; // Limit to the accumulated error + uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot + uint8_t abs_control_gain; // How strongly should the absolute accumulated error be corrected for + uint8_t abs_control_limit; // Limit to the correction + uint8_t abs_control_error_limit; // Limit to the accumulated error } pidProfile_t; #ifndef USE_OSD_SLAVE