Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Use crash detection to limit yaw

Browse source
This commit is contained in:
Martin Budden 2017-08-22 08:41:32 +01:00
parent be8391e3db
commit cd757c8a89
3 changed files with 24 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/settings.c
View file

@ -563,13 +563,13 @@ const clivalue_t valueTable[] = {
{ "crash_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) },
{ "crash_recovery_angle", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_angle) },
{ "crash_recovery_rate", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery_rate) },
{ "crash_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_limit_yaw) },
{ "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "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_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) },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },

33
src/main/flight/pid.c
View file

@ -115,7 +115,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
.horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false,
.gyroRateLimitYaw = 1000
.crash_limit_yaw = 200
);
}
@ -239,7 +239,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static float Kp[3], Ki[3], Kd[3];
static float maxVelocity[3];
static float gyroRateLimitYaw;
static float relaxFactor;
static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
@ -252,6 +251,7 @@ static float crashRecoveryRate;
static float crashDtermThreshold;
static float crashGyroThreshold;
static float crashSetpointThreshold;
static float crashLimitYaw;
void pidInitConfig(const pidProfile_t *pidProfile)
{
@ -260,7 +260,6 @@ void pidInitConfig(const pidProfile_t *pidProfile)
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
}
gyroRateLimitYaw = pidProfile->gyroRateLimitYaw;
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
@ -280,6 +279,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
crashGyroThreshold = pidProfile->crash_gthreshold;
crashDtermThreshold = pidProfile->crash_dthreshold;
crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
crashLimitYaw = pidProfile->crash_limit_yaw;
}
void pidInit(const pidProfile_t *pidProfile)
@ -394,46 +394,49 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
if (inCrashRecoveryMode && axis != FD_YAW && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
// self-level - errorAngle is deviation from horizontal
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {
BEEP_ON;
}
if (axis == FD_YAW) {
// on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate
errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
} else {
// on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
// errorAngle is deviation from horizontal
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
currentPidSetpoint = errorAngle * levelGain;
errorRate = currentPidSetpoint - gyroRate;
}
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (motorMixRange < 1.0f
&& ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
&& ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees
&& ABS(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
&& ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate)
&& ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
&& ABS(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)
) {
inCrashRecoveryMode = false;
BEEP_OFF;
}
}
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. ----------
// 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
// -----calculate error rate
const float errorRate = currentPidSetpoint - gyroRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input
axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
if (axis == FD_YAW) {

2
src/main/flight/pid.h
View file

@ -105,7 +105,7 @@ typedef struct pidProfile_s {
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
uint16_t gyroRateLimitYaw; // limits yaw gyroRate, so crashes don't cause huge throttle increase
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);