CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Rover/crash_check.cpp
Views: 1798
1
#include "Rover.h"

2


3
// Code to detect a crash or block

4
static const uint16_t CRASH_CHECK_TRIGGER_SEC = 2;   // 2 seconds blocked indicates a crash

5
static const float CRASH_CHECK_THROTTLE_MIN = 5.0f;  // vehicle must have a throttle greater that 5% to be considered crashed

6
static const float CRASH_CHECK_VEL_MIN = 0.08f;      // vehicle must have a velocity under 0.08 m/s or rad/s to be considered crashed

7


8
// crash_check - disarms motors if a crash or block has been detected

9
// crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running

10
// called at 10Hz

11
void Rover::crash_check()

12
{

13
    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed

14
    bool crashed = false; //stores crash state

15


16
    // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot)

17
    if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) {

18
        crash_counter = 0;

19
        return;

20
    }

21


22
    // Crashed if pitch/roll > crash_angle

23
    if ((g2.crash_angle != 0) && ((fabsf(ahrs.get_pitch()) > radians(g2.crash_angle)) || (fabsf(ahrs.get_roll()) > radians(g2.crash_angle)))) {

24
        crashed = true;

25
    }

26


27
    // TODO : Check if min vel can be calculated

28
    // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise;

29


30
    if (!is_balancebot()) {

31
        if (!crashed && ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN) ||        // Check velocity

32
            (fabsf(ahrs.get_gyro().z) >= CRASH_CHECK_VEL_MIN) ||  // Check turn speed

33
            (fabsf(g2.motors.get_throttle()) < CRASH_CHECK_THROTTLE_MIN))) {

34
            crash_counter = 0;

35
            return;

36
        }

37


38
        // we may be crashing

39
        crash_counter++;

40


41
        // check if crashing for 2 seconds

42
        if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {

43
            crashed = true;

44
        }

45
    }

46


47
    if (crashed) {

48
        LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK,

49
                           LogErrorCode::CRASH_CHECK_CRASH);

50


51
        if (is_balancebot()) {

52
            // send message to gcs

53
            gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Disarming");

54
            arming.disarm(AP_Arming::Method::CRASH);

55
        } else {

56
            // send message to gcs

57
            gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");

58
            // change mode to hold and disarm

59
            set_mode(mode_hold, ModeReason::CRASH_FAILSAFE);

60
            if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {

61
                arming.disarm(AP_Arming::Method::CRASH);

62
            }

63
        }

64
    }

65
}

66


67