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

Added setpoint check to crash detection

Browse source
This commit is contained in:
Martin Budden 2017-07-23 07:10:14 +01:00
parent 3fc3de9ebd
commit 1ee034c38a
3 changed files with 11 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -109,7 +109,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.crash_recovery_angle = 10, // 10 degrees
.crash_recovery_rate = 100, // 100 degrees/second
.crash_dthreshold = 50, // 50 degrees/second/second
.crash_gthreshold = 500, // 500 degrees/second
.crash_gthreshold = 200, // 200 degrees/second
.crash_setpoint_threshold = 400, // 400 degrees/second
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
.horizon_tilt_effect = 75,
.horizon_tilt_expert_mode = false
@ -241,6 +242,7 @@ static int32_t crashRecoveryAngleDeciDegrees;
static float crashRecoveryRate;
static float crashDtermThreshold;
static float crashGyroThreshold;
static float crashSetpointThreshold;
void pidInitConfig(const pidProfile_t *pidProfile) {
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -265,6 +267,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
crashRecoveryRate = pidProfile->crash_recovery_rate;
crashGyroThreshold = pidProfile->crash_gthreshold;
crashDtermThreshold = pidProfile->crash_dthreshold;
crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
}
void pidInit(const pidProfile_t *pidProfile)
@ -448,7 +451,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// if crash recovery is on and accelerometer enabled then check for a crash
if (pidProfile->crash_recovery && sensors(SENSOR_ACC)) {
if (motorMixRange >= 1.0f && ABS(delta) > crashDtermThreshold && ABS(errorRate) > crashGyroThreshold) {
if (motorMixRange >= 1.0f
&& ABS(delta) > crashDtermThreshold
&& ABS(errorRate) > crashGyroThreshold
&& ABS(getSetpointRate(axis)) < crashSetpointThreshold) {
inCrashRecoveryMode = true;
crashDetectedAtUs = currentTimeUs;
if (pidProfile->crash_recovery == PID_CRASH_RECOVERY_BEEP) {