This fix is for avoiding yaw overshoot and bounce back for some
configurations The hardcoded limit in the mixer and PID controllers 3-5 would be removed by default and will be configurable by CLI variables: yaw_jump_prevention_limit, global setting (original fixed value was 100) yaw_p_limit, per profile setting (fixed value was 300)
This commit is contained in:
parent
bad0b1b04d
commit
2fd79a7aa2
|
@ -180,6 +180,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->I8[PIDVEL] = 45;
|
||||
pidProfile->D8[PIDVEL] = 1;
|
||||
|
||||
pidProfile->yaw_p_limit = 0;
|
||||
|
||||
pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully
|
||||
pidProfile->I_f[ROLL] = 0.6f;
|
||||
pidProfile->D_f[ROLL] = 0.06f;
|
||||
|
@ -317,6 +319,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
|
|||
void resetMixerConfig(mixerConfig_t *mixerConfig) {
|
||||
mixerConfig->pid_at_min_throttle = 1;
|
||||
mixerConfig->yaw_direction = 1;
|
||||
mixerConfig->yaw_jump_prevention_limit = 0;
|
||||
#ifdef USE_SERVOS
|
||||
mixerConfig->tri_unarmed_servo = 1;
|
||||
mixerConfig->servo_lowpass_freq = 400;
|
||||
|
|
|
@ -564,9 +564,9 @@ void mixTable(void)
|
|||
{
|
||||
uint32_t i;
|
||||
|
||||
if (motorCount > 3) {
|
||||
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit) {
|
||||
// prevent "yaw jump" during yaw correction
|
||||
axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW]));
|
||||
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
|
||||
}
|
||||
|
||||
// motors for non-servo mixes
|
||||
|
|
|
@ -66,6 +66,7 @@ typedef struct mixer_t {
|
|||
typedef struct mixerConfig_s {
|
||||
uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle
|
||||
int8_t yaw_direction;
|
||||
int8_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100)
|
||||
#ifdef USE_SERVOS
|
||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
||||
|
|
|
@ -297,7 +297,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
}
|
||||
}
|
||||
|
||||
#define GYRO_P_MAX 300
|
||||
#define GYRO_I_MAX 256
|
||||
|
||||
static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination,
|
||||
|
@ -394,9 +393,8 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
|||
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
|
||||
|
||||
// Constrain YAW by D value if not servo driven in that case servolimits apply
|
||||
if(motorCount > 3) {
|
||||
int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW];
|
||||
PTerm = constrain(PTerm, -limit, +limit);
|
||||
if(motorCount >= 4 && pidProfile->yaw_p_limit) {
|
||||
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||
}
|
||||
|
||||
ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
|
||||
|
@ -505,9 +503,8 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
|
|||
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6;
|
||||
|
||||
// Constrain YAW by D value if not servo driven in that case servolimits apply
|
||||
if(motorCount > 3) {
|
||||
int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW];
|
||||
PTerm = constrain(PTerm, -limit, +limit);
|
||||
if(motorCount >= 4 && pidProfile->yaw_p_limit) {
|
||||
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||
}
|
||||
|
||||
ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
|
||||
|
@ -635,10 +632,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
|
|||
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
|
||||
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
|
||||
|
||||
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
|
||||
int32_t limit = 300;
|
||||
if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW];
|
||||
PTerm = constrain(PTerm, -limit, limit);
|
||||
if(motorCount >= 4 && pidProfile->yaw_p_limit) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
|
||||
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||
}
|
||||
}
|
||||
axisPID[FD_YAW] = PTerm + ITerm;
|
||||
|
|
|
@ -47,6 +47,7 @@ typedef struct pidProfile_s {
|
|||
float A_level;
|
||||
float H_level;
|
||||
uint8_t H_sensitivity;
|
||||
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
|
||||
} pidProfile_t;
|
||||
|
||||
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
|
||||
|
|
|
@ -387,6 +387,7 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
{ "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.pid_at_min_throttle, 0, 1 },
|
||||
{ "yaw_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_direction, -1, 1 },
|
||||
{ "yaw_jump_prevention_limit", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, 0, 250 },
|
||||
#ifdef USE_SERVOS
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 },
|
||||
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400},
|
||||
|
@ -470,6 +471,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
|
||||
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
|
||||
|
||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, 0, 500 },
|
||||
|
||||
#ifdef BLACKBOX
|
||||
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },
|
||||
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },
|
||||
|
|
Loading…
Reference in New Issue