mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Merge branch 'master' into development
This commit is contained in:
commit
e7bd930ff5
4 changed files with 5 additions and 2 deletions
|
@ -169,6 +169,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
pidProfile->D8[PIDVEL] = 75;
|
pidProfile->D8[PIDVEL] = 75;
|
||||||
|
|
||||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||||
|
pidProfile->pidSumLimit = PIDSUM_LIMIT;
|
||||||
pidProfile->yaw_lpf_hz = 0;
|
pidProfile->yaw_lpf_hz = 0;
|
||||||
pidProfile->rollPitchItermIgnoreRate = 130;
|
pidProfile->rollPitchItermIgnoreRate = 130;
|
||||||
pidProfile->yawItermIgnoreRate = 32;
|
pidProfile->yawItermIgnoreRate = 32;
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
||||||
#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
|
#define YAW_JUMP_PREVENTION_LIMIT_LOW 80
|
||||||
#define YAW_JUMP_PREVENTION_LIMIT_HIGH 400
|
#define YAW_JUMP_PREVENTION_LIMIT_HIGH 400
|
||||||
|
#define PIDSUM_LIMIT 700
|
||||||
|
|
||||||
#define DYNAMIC_PTERM_STICK_THRESHOLD 400
|
#define DYNAMIC_PTERM_STICK_THRESHOLD 400
|
||||||
|
|
||||||
|
@ -83,6 +84,7 @@ typedef struct pidProfile_s {
|
||||||
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
|
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
|
||||||
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
|
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
|
||||||
uint16_t yaw_p_limit;
|
uint16_t yaw_p_limit;
|
||||||
|
uint16_t pidSumLimit;
|
||||||
uint8_t dterm_average_count; // Configurable delta count for dterm
|
uint8_t dterm_average_count; // Configurable delta count for dterm
|
||||||
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
|
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 pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
|
||||||
|
|
|
@ -220,7 +220,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
||||||
DTerm = Kd[axis] * delta * tpaFactor;
|
DTerm = Kd[axis] * delta * tpaFactor;
|
||||||
|
|
||||||
// -----calculate total PID output
|
// -----calculate total PID output
|
||||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -800, 800);
|
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
|
||||||
} else {
|
} else {
|
||||||
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
|
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
|
||||||
|
|
||||||
|
|
|
@ -815,7 +815,7 @@ const clivalue_t valueTable[] = {
|
||||||
|
|
||||||
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } },
|
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } },
|
||||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
||||||
|
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 100, 1000 } },
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue