mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-13 03:13:57 -04:00
ArduCopter: failsafe added to shutdown motors if mainloop fails
This commit is contained in:
parent
fa0963d592
commit
c0ad98be31
@ -911,6 +911,8 @@ static byte medium_loopCounter;
|
||||
static byte slow_loopCounter;
|
||||
// Counters for branching at 1 hz
|
||||
static byte counter_one_herz;
|
||||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
static uint16_t mainLoop_count;
|
||||
// used to track the elapsed time between GPS reads
|
||||
static uint32_t nav_loopTimer;
|
||||
// Delta Time in milliseconds for navigation computations, updated with every good GPS read
|
||||
@ -1002,6 +1004,9 @@ void loop()
|
||||
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
|
||||
fast_loopTimer = timer;
|
||||
|
||||
// for mainloop failure monitoring
|
||||
mainLoop_count++;
|
||||
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
fast_loop();////
|
||||
|
59
ArduCopter/failsafe.pde
Normal file
59
ArduCopter/failsafe.pde
Normal file
@ -0,0 +1,59 @@
|
||||
// -*- 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;
|
||||
}
|
||||
|
||||
//
|
||||
// failsafe_check - this function is called from the core timer interrupt at 1kHz.
|
||||
//
|
||||
void failsafe_check(uint32_t tnow)
|
||||
{
|
||||
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;
|
||||
in_failsafe = false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (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;
|
||||
}
|
||||
|
||||
if (in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
|
||||
// disarm motors every second
|
||||
failsafe_last_timestamp = tnow;
|
||||
if( motors.armed() ) {
|
||||
motors.armed(false);
|
||||
motors.output();
|
||||
}
|
||||
}
|
||||
}
|
@ -106,6 +106,9 @@ static void init_arm_motors()
|
||||
// which calibrates the IMU
|
||||
static bool did_ground_start = false;
|
||||
|
||||
// disable failsafe because initialising everything takes a while
|
||||
failsafe_disable();
|
||||
|
||||
//Serial.printf("\nARM\n");
|
||||
#if HIL_MODE != HIL_MODE_DISABLED || defined(DESKTOP_BUILD)
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
|
||||
@ -118,7 +121,6 @@ static void init_arm_motors()
|
||||
if (gcs3.initialised) {
|
||||
Serial3.set_blocking_writes(false);
|
||||
}
|
||||
motors.armed(true);
|
||||
|
||||
#if COPTER_LEDS == ENABLED
|
||||
if ( bitRead(g.copter_leds_mode, 3) ) {
|
||||
@ -164,6 +166,12 @@ static void init_arm_motors()
|
||||
#if SECONDARY_DMP_ENABLED == ENABLED
|
||||
ahrs2.set_fast_gains(false);
|
||||
#endif
|
||||
|
||||
// finally actually arm the motors
|
||||
motors.armed(true);
|
||||
|
||||
// reenable failsafe
|
||||
failsafe_enable();
|
||||
}
|
||||
|
||||
|
||||
|
@ -206,6 +206,12 @@ static void init_ardupilot()
|
||||
|
||||
timer_scheduler.init( &isr_registry );
|
||||
|
||||
/*
|
||||
* setup the 'main loop is dead' check. Note that this relies on
|
||||
* the RC library being initialised.
|
||||
*/
|
||||
timer_scheduler.set_failsafe(failsafe_check);
|
||||
|
||||
// initialise the analog port reader
|
||||
AP_AnalogSource_Arduino::init_timer(&timer_scheduler);
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user