ArduCopter/failsafe.cpp

   1 #include "Copter.h"
   2
   3 //
   4 //  failsafe support
   5 //  Andrew Tridgell, December 2011
   6 //
   7 //  our failsafe strategy is to detect main loop lockup and disarm the motors
   8 //
   9
  10 static bool failsafe_enabled;
  11 static uint16_t failsafe_last_ticks;
  12 static uint32_t failsafe_last_timestamp;
  13 static bool in_failsafe;
  14
  15 //
  16 // failsafe_enable - enable failsafe
  17 //
  18 void Copter::failsafe_enable()
  19 {
  20     failsafe_enabled = true;
  21     failsafe_last_timestamp = micros();
  22 }
  23
  24 //
  25 // failsafe_disable - used when we know we are going to delay the mainloop significantly
  26 //
  27 void Copter::failsafe_disable()
  28 {
  29     failsafe_enabled = false;
  30 }
  31
  32 //
  33 //  failsafe_check - this function is called from the core timer interrupt at 1kHz.
  34 //
  35 void Copter::failsafe_check()
  36 {
  37     uint32_t tnow = AP_HAL::micros();
  38
  39     const uint16_t ticks = scheduler.ticks();
  40     if (ticks != failsafe_last_ticks) {
  41         // the main loop is running, all is OK
  42         failsafe_last_ticks = ticks;
  43         failsafe_last_timestamp = tnow;
  44         if (in_failsafe) {
  45             in_failsafe = false;
  46             LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED);
  47         }
  48         return;
  49     }
  50
  51     if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
  52         // motors are running but we have gone 2 second since the
  53         // main loop ran. That means we're in trouble and should
  54         // disarm the motors->
  55         in_failsafe = true;
  56         // reduce motors to minimum (we do not immediately disarm because we want to log the failure)
  57         if (motors->armed()) {
  58             motors->output_min();
  59         }
  60
  61         LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED);
  62     }
  63
  64     if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
  65         // disarm motors every second
  66         failsafe_last_timestamp = tnow;
  67         if(motors->armed()) {
  68             motors->armed(false);
  69             motors->output();
  70         }
  71     }
  72 }
  73
  74
  75 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
  76 /*
  77   check for AFS failsafe check
  78 */
  79 void Copter::afs_fs_check(void)
  80 {
  81     // perform AFS failsafe checks
  82     g2.afs.check(last_radio_update_ms);
  83 }
  84 #endif