Line data    Source code

       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        1551 : void Copter::failsafe_enable()
      19             : {
      20        1551 :     failsafe_enabled = true;
      21        1551 :     failsafe_last_timestamp = micros();
      22        1551 : }
      23             : 
      24             : //
      25             : // failsafe_disable - used when we know we are going to delay the mainloop significantly
      26             : //
      27         553 : void Copter::failsafe_disable()
      28             : {
      29         553 :     failsafe_enabled = false;
      30         553 : }
      31             : 
      32             : //
      33             : //  failsafe_check - this function is called from the core timer interrupt at 1kHz.
      34             : //
      35    94293383 : void Copter::failsafe_check()
      36             : {
      37    94293383 :     uint32_t tnow = AP_HAL::micros();
      38             : 
      39    94293383 :     const uint16_t ticks = scheduler.ticks();
      40    94293383 :     if (ticks != failsafe_last_ticks) {
      41             :         // the main loop is running, all is OK
      42    30412448 :         failsafe_last_ticks = ticks;
      43    30412448 :         failsafe_last_timestamp = tnow;
      44    30412448 :         if (in_failsafe) {
      45           0 :             in_failsafe = false;
      46           0 :             LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED);
      47             :         }
      48    30412448 :         return;
      49             :     }
      50             : 
      51    63880935 :     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           0 :         in_failsafe = true;
      56             :         // reduce motors to minimum (we do not immediately disarm because we want to log the failure)
      57           0 :         if (motors->armed()) {
      58           0 :             motors->output_min();
      59             :         }
      60             : 
      61           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED);
      62             :     }
      63             : 
      64    63880935 :     if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
      65             :         // disarm motors every second
      66           0 :         failsafe_last_timestamp = tnow;
      67           0 :         if(motors->armed()) {
      68           0 :             motors->armed(false);
      69           0 :             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