Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

Revert crash_time timeout handling changes

Browse source
This commit is contained in:
Bruce Luckcuck 2019-09-12 19:52:56 -04:00
parent 381b38ca52
commit 26c5e6aae2
1 changed files with 6 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -156,7 +156,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.rateAccelLimit = 0, .rateAccelLimit = 0,
.itermThrottleThreshold = 250, .itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000, .itermAcceleratorGain = 5000,
.crash_time = 1000, // ms .crash_time = 500, // ms
.crash_delay = 0, // ms .crash_delay = 0, // ms
.crash_recovery_angle = 10, // degrees .crash_recovery_angle = 10, // degrees
.crash_recovery_rate = 100, // degrees/second .crash_recovery_rate = 100, // degrees/second
@ -873,17 +873,7 @@ static void handleCrashRecovery(
const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim, const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate) const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
{ {
// check to see if we're still in the startup delay
if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) { if (inCrashRecoveryMode && cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeDelayUs) {
// check to see if the crash_time limit has been exceeded, if so then
// exit crash recovery regardless of the current attitude or recovery status
if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs) {
inCrashRecoveryMode = false;
BEEP_OFF;
return;
}
if (crash_recovery == PID_CRASH_RECOVERY_BEEP) { if (crash_recovery == PID_CRASH_RECOVERY_BEEP) {
BEEP_ON; BEEP_ON;
} }
@ -901,11 +891,11 @@ static void handleCrashRecovery(
// reset iterm, since accumulated error before crash is now meaningless // reset iterm, since accumulated error before crash is now meaningless
// and iterm windup during crash recovery can be extreme, especially on yaw axis // and iterm windup during crash recovery can be extreme, especially on yaw axis
pidData[axis].I = 0.0f; pidData[axis].I = 0.0f;
if (getMotorMixRange() < 1.0f if (cmpTimeUs(currentTimeUs, crashDetectedAtUs) > crashTimeLimitUs
&& fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate || (getMotorMixRange() < 1.0f
&& fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate && fabsf(gyro.gyroADCf[FD_ROLL]) < crashRecoveryRate
&& fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate) { && fabsf(gyro.gyroADCf[FD_PITCH]) < crashRecoveryRate
&& fabsf(gyro.gyroADCf[FD_YAW]) < crashRecoveryRate)) {
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
// check aircraft nearly level // check aircraft nearly level
if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees if (ABS(attitude.raw[FD_ROLL] - angleTrim->raw[FD_ROLL]) < crashRecoveryAngleDeciDegrees