Skip to content

Commit

Permalink
fix: conditional CRASH_CHECK_TRIGGER_SEC value
Browse files Browse the repository at this point in the history
  • Loading branch information
Sztosik committed Jul 12, 2024
1 parent fcff383 commit cd5f3ec
Showing 1 changed file with 8 additions and 2 deletions.
10 changes: 8 additions & 2 deletions ArduCopter/crash_check.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,13 @@
#include "Copter.h"

// Code to detect a crash main ArduCopter code
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
#if PARACHUTE == ENABLED
// CRASH_CHECK_TRIGGER_SEC should be grater than CHUTE_TIMEOUT, to prevent disarming without deploying parachute
#define CRASH_CHECK_TRIGGER_SEC parachute.get_chute_timeout() + 1.0f
#else
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
#endif

#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond target angle is signal we are out of control
#define CRASH_CHECK_ANGLE_MIN_DEG 15.0f // vehicle must be leaning at least 15deg to trigger crash check
#define CRASH_CHECK_SPEED_MAX 10.0f // vehicle must be moving at less than 10m/s to trigger crash check
Expand Down Expand Up @@ -86,7 +92,7 @@ void Copter::crash_check()
// we may be crashing
crash_counter++;

// check if crashing for 2 seconds
// check if crashing for time defined by CRASH_CHECK_TRIGGER_SEC seconds
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
// send message to gcs
Expand Down

0 comments on commit cd5f3ec

Please sign in to comment.