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

Allow crash detection and yaw recovery when ACC not enabled

Browse source
This commit is contained in:
Martin Budden 2017-08-22 13:24:12 +01:00
parent 96c285f607
commit d031b038eb
1 changed files with 19 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -415,22 +415,30 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw); errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
} else { } else {
// on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash // 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 // 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; errorRate = currentPidSetpoint - gyroRate;
} }
}
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) && 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; inCrashRecoveryMode = false;
BEEP_OFF; BEEP_OFF;
} }
} else {
inCrashRecoveryMode = false;
BEEP_OFF;
}
}
} }
// --------low-level gyro-based PID based on 2DOF PID controller. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
@ -469,7 +477,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