Add gyro yaw limit
This commit is contained in:
parent
7a66260713
commit
be8391e3db
|
@ -569,6 +569,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
|
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
|
||||||
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
|
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
|
||||||
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
|
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
|
||||||
|
{ "gyro_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, gyroRateLimitYaw) },
|
||||||
|
|
||||||
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
|
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
|
||||||
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
|
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
|
||||||
|
|
|
@ -114,7 +114,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
||||||
.crash_setpoint_threshold = 350, // degrees/second
|
.crash_setpoint_threshold = 350, // degrees/second
|
||||||
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
|
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
|
||||||
.horizon_tilt_effect = 75,
|
.horizon_tilt_effect = 75,
|
||||||
.horizon_tilt_expert_mode = false
|
.horizon_tilt_expert_mode = false,
|
||||||
|
.gyroRateLimitYaw = 1000
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -238,6 +239,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
||||||
|
|
||||||
static float Kp[3], Ki[3], Kd[3];
|
static float Kp[3], Ki[3], Kd[3];
|
||||||
static float maxVelocity[3];
|
static float maxVelocity[3];
|
||||||
|
static float gyroRateLimitYaw;
|
||||||
static float relaxFactor;
|
static float relaxFactor;
|
||||||
static float dtermSetpointWeight;
|
static float dtermSetpointWeight;
|
||||||
static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
||||||
|
@ -258,6 +260,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
||||||
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
|
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
|
||||||
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
|
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
|
||||||
}
|
}
|
||||||
|
gyroRateLimitYaw = pidProfile->gyroRateLimitYaw;
|
||||||
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
|
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
|
||||||
relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
|
relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
|
||||||
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
|
||||||
|
@ -419,7 +422,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
||||||
BEEP_OFF;
|
BEEP_OFF;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
|
float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
|
||||||
|
if (axis == FD_YAW) {
|
||||||
|
gyroRate = constrainf(gyroRate, -gyroRateLimitYaw, gyroRateLimitYaw);
|
||||||
|
}
|
||||||
|
|
||||||
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
|
||||||
// 2-DOF PID controller with optional filter on derivative term.
|
// 2-DOF PID controller with optional filter on derivative term.
|
||||||
|
|
|
@ -105,6 +105,7 @@ typedef struct pidProfile_s {
|
||||||
uint8_t crash_recovery_angle; // degrees
|
uint8_t crash_recovery_angle; // degrees
|
||||||
uint8_t crash_recovery_rate; // degree/second
|
uint8_t crash_recovery_rate; // degree/second
|
||||||
pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode
|
pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode
|
||||||
|
uint16_t gyroRateLimitYaw; // limits yaw gyroRate, so crashes don't cause huge throttle increase
|
||||||
} pidProfile_t;
|
} pidProfile_t;
|
||||||
|
|
||||||
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
|
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
|
||||||
|
|
Loading…
Reference in New Issue