Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : // Code to detect a crash or block
       4             : 
       5             : // crash_check - sets hold mode and optionally disarms motors if a crash or block has been detected
       6             : // crashes are detected by speed below crash_vel_min or turn rate below crash_turn_rate_min
       7             : //     with throttle demand above crash_thr_min for longer than crash_timeout
       8             : // called at 10Hz
       9      125720 : void Rover::crash_check()
      10             : {
      11             :     static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
      12      125720 :     bool crashed = false; //stores crash state
      13             : 
      14             :     // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode(if it is not a balance bot)
      15      125720 :     if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || ((!control_mode->is_autopilot_mode()) && (!is_balancebot()))) {
      16      120797 :         crash_counter = 0;
      17      120797 :         return;
      18             :     }
      19             : 
      20             :     // Crashed if pitch/roll > crash_angle
      21        4923 :     if ((g2.crash_angle != 0) && ((fabsf(ahrs.get_pitch_rad()) > radians(g2.crash_angle)) || (fabsf(ahrs.get_roll_rad()) > radians(g2.crash_angle)))) {
      22           0 :         crashed = true;
      23             :     }
      24             : 
      25             :     // TODO : Check if min vel can be calculated
      26             :     // min_vel = ( g2.crash_thr_min * g.speed_cruise) / g.throttle_cruise;
      27             : 
      28        4923 :     if (!is_balancebot() && g2.crash_thr_min > 0.0f && g2.crash_timeout > 0.0f) {
      29         120 :         if (!crashed && 
      30          60 :             ((g2.crash_vel_min > 0.0f && ahrs.groundspeed() >= g2.crash_vel_min) ||                      // Check velocity
      31          60 :             (g2.crash_turn_rate_min > 0.0f && fabsf(ahrs.get_gyro().z) >= radians(g2.crash_turn_rate_min)) ||  // Check turn rate
      32          60 :             (fabsf(g2.motors.get_throttle()) < g2.crash_thr_min))) {
      33          20 :             crash_counter = 0;
      34          20 :             return;
      35             :         }
      36             : 
      37             :         // we may be crashing
      38          40 :         crash_counter++;
      39             : 
      40             :         // check if crashing for crash_timeout seconds
      41          40 :         if (crash_counter >= (g2.crash_timeout * 10)) {
      42           2 :             crashed = true;
      43             :         }
      44             :     }
      45             : 
      46        4903 :     if (crashed) {
      47           2 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK,
      48             :                            LogErrorCode::CRASH_CHECK_CRASH);
      49             : 
      50           2 :         if (is_balancebot()) {
      51             :             // send message to gcs
      52           0 :             GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Crash: Disarming");
      53           0 :             arming.disarm(AP_Arming::Method::CRASH);
      54             :         } else {
      55             :             // send message to gcs
      56           2 :             GCS_SEND_TEXT(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
      57             :             // change mode to hold and disarm
      58           2 :             set_mode(mode_hold, ModeReason::CRASH_FAILSAFE);
      59           2 :             if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
      60           1 :                 arming.disarm(AP_Arming::Method::CRASH);
      61             :             }
      62             :         }
      63             :     }
      64             : }
      65             : 
      66             : // override AP_Vehicle::is_crashed() to also check control mode reason
      67             : // returns true if control_mode_reason is CRASH_FAILSAFE or
      68             : // parent method returns true
      69           0 : bool Rover::is_crashed() const
      70             : {
      71           0 :     return (control_mode_reason == ModeReason::CRASH_FAILSAFE) ||
      72           0 :         AP_Vehicle::is_crashed();
      73             : }