Line data    Source code

       1             : /*
       2             :   failsafe support
       3             :   Andrew Tridgell, December 2011
       4             :  */
       5             : 
       6             : #include "Rover.h"
       7             : 
       8             : #include <stdio.h>
       9             : 
      10             : /*
      11             :   our failsafe strategy is to detect main loop lockup and disarm.
      12             :  */
      13             : 
      14             : /*
      15             :   this failsafe_check function is called from the core timer interrupt
      16             :   at 1kHz.
      17             :  */
      18    15369390 : void Rover::failsafe_check()
      19             : {
      20             :     static uint16_t last_ticks;
      21             :     static uint32_t last_timestamp;
      22    15369390 :     const uint32_t tnow = AP_HAL::micros();
      23             : 
      24    15369390 :     const uint16_t ticks = scheduler.ticks();
      25    15369390 :     if (ticks != last_ticks) {
      26             :         // the main loop is running, all is OK
      27      629048 :         last_ticks = ticks;
      28      629048 :         last_timestamp = tnow;
      29      629048 :         return;
      30             :     }
      31             : 
      32    14740342 :     if (tnow - last_timestamp > 200000) {
      33             :         // we have gone at least 0.2 seconds since the main loop
      34             :         // ran. That means we're in trouble, or perhaps are in
      35             :         // an initialisation routine or log erase. disarm the motors
      36             :         // To-Do: log error
      37      215675 :         if (arming.is_armed()) {
      38             :             // disarm motors
      39           0 :             arming.disarm(AP_Arming::Method::CPUFAILSAFE);
      40             :         }
      41             :     }
      42             : }
      43             : 
      44             : /*
      45             :   called to set/unset a failsafe event.
      46             :  */
      47      618038 : void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on)
      48             : {
      49      618038 :     uint8_t old_bits = failsafe.bits;
      50      618038 :     if (on) {
      51        2758 :         failsafe.bits |= failsafe_type;
      52             :     } else {
      53      615280 :         failsafe.bits &= ~failsafe_type;
      54             :     }
      55      618038 :     if (old_bits == 0 && failsafe.bits != 0) {
      56             :         // a failsafe event has started
      57          13 :         failsafe.start_time = millis();
      58             :     }
      59      618038 :     if (failsafe.triggered != 0 && failsafe.bits == 0) {
      60             :         // a failsafe event has ended
      61           8 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Failsafe Cleared", type_str);
      62             :     }
      63             : 
      64      618038 :     failsafe.triggered &= failsafe.bits;
      65             : 
      66      618038 :     if (failsafe_type == FAILSAFE_EVENT_GCS) {
      67        2010 :         AP_Notify::flags.failsafe_gcs = on;
      68             :     }
      69             : 
      70     1852127 :     if ((failsafe.triggered == 0) &&
      71      616051 :         (failsafe.bits != 0) &&
      72         771 :         (millis() - failsafe.start_time > g.fs_timeout * 1000) &&
      73     1234335 :         (control_mode != &mode_rtl) &&
      74         246 :         ((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {
      75           8 :         failsafe.triggered = failsafe.bits;
      76           8 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s Failsafe", type_str);
      77             : 
      78             :         // clear rc overrides
      79           8 :         RC_Channels::clear_overrides();
      80             : 
      81           9 :         if ((control_mode == &mode_auto) &&
      82           1 :             ((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
      83           1 :              (failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
      84             :             // continue with mission in auto mode
      85           1 :             GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
      86             :         } else {
      87           7 :             switch ((FailsafeAction)g.fs_action.get()) {
      88           1 :             case FailsafeAction::None:
      89           1 :                 break;
      90           0 :             case FailsafeAction::SmartRTL:
      91           0 :                 if (set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
      92           0 :                     break;
      93             :                 }
      94             :                 FALLTHROUGH;
      95             :             case FailsafeAction::RTL:
      96           4 :                 if (set_mode(mode_rtl, ModeReason::FAILSAFE)) {
      97           4 :                     break;
      98             :                 }
      99             :                 FALLTHROUGH;
     100             :             case FailsafeAction::Hold:
     101           0 :                 set_mode(mode_hold, ModeReason::FAILSAFE);
     102           0 :                 break;
     103           0 :             case FailsafeAction::SmartRTL_Hold:
     104           0 :                 if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
     105           0 :                     set_mode(mode_hold, ModeReason::FAILSAFE);
     106             :                 }
     107           0 :                 break;
     108           2 :             case FailsafeAction::Loiter_Hold:
     109           2 :                 if (!set_mode(mode_loiter, ModeReason::FAILSAFE)) {
     110           0 :                     set_mode(mode_hold, ModeReason::FAILSAFE);
     111             :                 }
     112           2 :                 break;
     113           0 :             case FailsafeAction::Terminate:
     114           0 :                 arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
     115           0 :                 break;
     116             :             }
     117             :         }
     118             :     }
     119      618038 : }
     120             : 
     121           0 : void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
     122             : {
     123           0 :         switch ((FailsafeAction)action) {
     124           0 :             case FailsafeAction::None:
     125           0 :                 break;
     126           0 :             case FailsafeAction::SmartRTL:
     127           0 :                 if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
     128           0 :                     break;
     129             :                 }
     130             :                 FALLTHROUGH;
     131             :             case FailsafeAction::RTL:
     132           0 :                 if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
     133           0 :                     break;
     134             :                 }
     135             :                 FALLTHROUGH;
     136             :             case FailsafeAction::Hold:
     137           0 :                 set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
     138           0 :                 break;
     139           0 :             case FailsafeAction::Loiter_Hold:
     140           0 :                 if (!set_mode(mode_loiter, ModeReason::BATTERY_FAILSAFE)) {
     141           0 :                     set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
     142             :                 }
     143           0 :                 break;
     144           0 :             case FailsafeAction::SmartRTL_Hold:
     145           0 :                 if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
     146           0 :                     set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
     147             :                 }
     148           0 :                 break;
     149           0 :             case FailsafeAction::Terminate:
     150             : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
     151             :                 char battery_type_str[17];
     152             :                 snprintf(battery_type_str, 17, "%s battery", type_str);
     153             :                 g2.afs.gcs_terminate(true, battery_type_str);
     154             : #else
     155           0 :                 arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
     156             : #endif // AP_ROVER_ADVANCED_FAILSAFE_ENABLED
     157           0 :                 break;
     158             :         }
     159           0 : }
     160             : 
     161             : #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
     162             : /*
     163             :    check for AFS failsafe check
     164             :  */
     165             : void Rover::afs_fs_check(void)
     166             : {
     167             :     // perform AFS failsafe checks
     168             :     g2.afs.check(failsafe.last_valid_rc_ms);
     169             : }
     170             : #endif