You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
84 lines
2.2 KiB
84 lines
2.2 KiB
#include "Copter.h" |
|
|
|
// |
|
// failsafe support |
|
// Andrew Tridgell, December 2011 |
|
// |
|
// our failsafe strategy is to detect main loop lockup and disarm the motors |
|
// |
|
|
|
static bool failsafe_enabled = false; |
|
static uint16_t failsafe_last_ticks; |
|
static uint32_t failsafe_last_timestamp; |
|
static bool in_failsafe; |
|
|
|
// |
|
// failsafe_enable - enable failsafe |
|
// |
|
void Copter::failsafe_enable() |
|
{ |
|
failsafe_enabled = true; |
|
failsafe_last_timestamp = micros(); |
|
} |
|
|
|
// |
|
// failsafe_disable - used when we know we are going to delay the mainloop significantly |
|
// |
|
void Copter::failsafe_disable() |
|
{ |
|
failsafe_enabled = false; |
|
} |
|
|
|
// |
|
// failsafe_check - this function is called from the core timer interrupt at 1kHz. |
|
// |
|
void Copter::failsafe_check() |
|
{ |
|
uint32_t tnow = AP_HAL::micros(); |
|
|
|
const uint16_t ticks = scheduler.ticks(); |
|
if (ticks != failsafe_last_ticks) { |
|
// the main loop is running, all is OK |
|
failsafe_last_ticks = ticks; |
|
failsafe_last_timestamp = tnow; |
|
if (in_failsafe) { |
|
in_failsafe = false; |
|
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED); |
|
} |
|
return; |
|
} |
|
|
|
if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) { |
|
// motors are running but we have gone 2 second since the |
|
// main loop ran. That means we're in trouble and should |
|
// disarm the motors-> |
|
in_failsafe = true; |
|
// reduce motors to minimum (we do not immediately disarm because we want to log the failure) |
|
if (motors->armed()) { |
|
motors->output_min(); |
|
} |
|
// log an error |
|
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED); |
|
} |
|
|
|
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { |
|
// disarm motors every second |
|
failsafe_last_timestamp = tnow; |
|
if(motors->armed()) { |
|
motors->armed(false); |
|
motors->output(); |
|
} |
|
} |
|
} |
|
|
|
|
|
#if ADVANCED_FAILSAFE == ENABLED |
|
/* |
|
check for AFS failsafe check |
|
*/ |
|
void Copter::afs_fs_check(void) |
|
{ |
|
// perform AFS failsafe checks |
|
g2.afs.check(failsafe.last_heartbeat_ms, fence.get_breaches() != 0, last_radio_update_ms); |
|
} |
|
#endif
|
|
|