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:
commit
66785f425f
4 changed files with 52 additions and 25 deletions
|
@ -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) },
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue