ardupilot/ArduCopter/failsafe.pde

71 lines
2.0 KiB
Plaintext
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// failsafe support
// Andrew Tridgell, December 2011
//
// our failsafe strategy is to detect main loop lockup and disarm the motors
//
static bool failsafe_enabled = true;
static uint16_t failsafe_last_mainLoop_count;
static uint32_t failsafe_last_timestamp;
static bool in_failsafe;
//
// failsafe_enable - enable failsafe
//
void failsafe_enable()
{
failsafe_enabled = true;
failsafe_last_timestamp = micros();
}
//
// failsafe_disable - used when we know we are going to delay the mainloop significantly
//
void failsafe_disable()
{
failsafe_enabled = false;
}
2012-11-10 01:55:51 -04:00
//
// failsafe_check - this function is called from the core timer interrupt at 1kHz.
//
2013-09-30 07:37:21 -03:00
void failsafe_check()
{
2013-09-28 03:31:01 -03:00
uint32_t tnow = hal.scheduler->micros();
if (mainLoop_count != failsafe_last_mainLoop_count) {
// the main loop is running, all is OK
failsafe_last_mainLoop_count = mainLoop_count;
failsafe_last_timestamp = tnow;
2014-10-30 01:23:09 -03:00
if (in_failsafe) {
in_failsafe = false;
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_RESOLVED);
}
return;
}
2014-10-30 01:23:09 -03:00
if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
2012-11-10 01:55:51 -04:00
// 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
2014-10-30 01:23:09 -03:00
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;
2012-11-10 01:55:51 -04:00
if(motors.armed()) {
motors.armed(false);
motors.output();
}
}
2013-09-28 03:31:01 -03:00
}