1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +03:00

Merge pull request #3909 from martinbudden/bf_gyro_yaw_limit

Fix "yaw spin to the moon" after crash
This commit is contained in:
Martin Budden 2017-08-24 08:35:31 +01:00 committed by GitHub
commit 66785f425f
4 changed files with 52 additions and 25 deletions

View file

@ -563,12 +563,14 @@ const clivalue_t valueTable[] = {
{ "crash_delay", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_delay) }, { "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_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_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) }, { "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) }, { "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) }, { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
{ "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) },
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
{ "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) },

View file

@ -114,7 +114,9 @@ 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,
.crash_limit_yaw = 200,
.itermLimit = 100
); );
} }
@ -241,7 +243,7 @@ static float maxVelocity[3];
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;
static float ITermWindupPoint, ITermWindupPointInv; static float ITermWindupPointInv;
static uint8_t horizonTiltExpertMode; static uint8_t horizonTiltExpertMode;
static timeDelta_t crashTimeLimitUs; static timeDelta_t crashTimeLimitUs;
static timeDelta_t crashTimeDelayUs; static timeDelta_t crashTimeDelayUs;
@ -250,6 +252,8 @@ static float crashRecoveryRate;
static float crashDtermThreshold; static float crashDtermThreshold;
static float crashGyroThreshold; static float crashGyroThreshold;
static float crashSetpointThreshold; static float crashSetpointThreshold;
static float crashLimitYaw;
static float itermLimit;
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
{ {
@ -268,7 +272,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f; horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint); ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
crashTimeLimitUs = pidProfile->crash_time * 1000; crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000; crashTimeDelayUs = pidProfile->crash_delay * 1000;
@ -277,6 +281,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
crashGyroThreshold = pidProfile->crash_gthreshold; crashGyroThreshold = pidProfile->crash_gthreshold;
crashDtermThreshold = pidProfile->crash_dthreshold; crashDtermThreshold = pidProfile->crash_dthreshold;
crashSetpointThreshold = pidProfile->crash_setpoint_threshold; crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
crashLimitYaw = pidProfile->crash_limit_yaw;
itermLimit = pidProfile->itermLimit;
} }
void pidInit(const pidProfile_t *pidProfile) void pidInit(const pidProfile_t *pidProfile)
@ -391,43 +397,60 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
float currentPidSetpoint = getSetpointRate(axis); float currentPidSetpoint = getSetpointRate(axis);
if (maxVelocity[axis]) { if (maxVelocity[axis]) {
currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint); currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
} }
// Yaw control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint); currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
} }
if (inCrashRecoveryMode && axis != FD_YAW && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { // -----calculate error rate
// self-level - errorAngle is deviation from horizontal 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) { if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {
BEEP_ON; 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
if (sensors(SENSOR_ACC)) {
// errorAngle is deviation from horizontal
const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; const float errorAngle = -(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
currentPidSetpoint = errorAngle * levelGain; currentPidSetpoint = errorAngle * levelGain;
errorRate = currentPidSetpoint - gyroRate;
}
}
// reset ITerm, since accumulated error before crash is now meaningless
// and ITerm windup during crash recovery can be extreme, especially on yaw axis
axisPID_I[axis] = 0.0f;
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
|| (motorMixRange < 1.0f || (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_ROLL]) < crashRecoveryRate
&& ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate) && ABS(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
) { && ABS(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) {
if (sensors(SENSOR_ACC)) {
// check aircraft nearly level
if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees
&& ABS(attitude.raw[FD_PITCH] - angleTrim->raw[FD_PITCH]) < crashRecoveryAngleDeciDegrees) {
inCrashRecoveryMode = false;
BEEP_OFF;
}
} else {
inCrashRecoveryMode = false; inCrashRecoveryMode = false;
BEEP_OFF; BEEP_OFF;
} }
} }
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec }
// --------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.
// b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error). // 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 // -----calculate P component and add Dynamic Part based on stick input
axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor; axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
if (axis == FD_YAW) { if (axis == FD_YAW) {
@ -436,7 +459,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component // -----calculate I component
const float ITerm = axisPID_I[axis]; const float ITerm = axisPID_I[axis];
const float ITermNew = ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator; const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated // Only increase ITerm if output is not saturated
@ -460,7 +483,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
previousRateError[axis] = rD; previousRateError[axis] = rD;
// if crash recovery is on and accelerometer enabled then check for a crash // if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery && sensors(SENSOR_ACC) && ARMING_FLAG(ARMED)) { if (pidProfile->crash_recovery && ARMING_FLAG(ARMED)) {
if (motorMixRange >= 1.0f && inCrashRecoveryMode == false if (motorMixRange >= 1.0f && inCrashRecoveryMode == false
&& ABS(delta) > crashDtermThreshold && ABS(delta) > crashDtermThreshold
&& ABS(errorRate) > crashGyroThreshold && ABS(errorRate) > crashGyroThreshold

View file

@ -105,6 +105,8 @@ 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 crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
uint16_t itermLimit;
} pidProfile_t; } pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

View file

@ -77,7 +77,7 @@
#define USE_RX_CX10 #define USE_RX_CX10
#define USE_RX_H8_3D #define USE_RX_H8_3D
//#define USE_RX_INAV // Temporary disabled to make some room in flash //#define USE_RX_INAV // Temporary disabled to make some room in flash
#define USE_RX_SYMA //#define USE_RX_SYMA
#define USE_RX_V202 #define USE_RX_V202
//#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5 //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5
//#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5C //#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5C