ardupilot/Rover/failsafe.cpp

155 lines
4.9 KiB
C++
Raw Normal View History

/*
failsafe support
Andrew Tridgell, December 2011
*/
2015-05-13 00:16:45 -03:00
#include "Rover.h"
#include <stdio.h>
/*
our failsafe strategy is to detect main loop lockup and disarm.
*/
/*
this failsafe_check function is called from the core timer interrupt
at 1kHz.
*/
void Rover::failsafe_check()
{
static uint16_t last_ticks;
2015-05-12 04:00:25 -03:00
static uint32_t last_timestamp;
2017-01-31 06:12:26 -04:00
const uint32_t tnow = AP_HAL::micros();
const uint16_t ticks = scheduler.ticks();
if (ticks != last_ticks) {
// the main loop is running, all is OK
last_ticks = ticks;
last_timestamp = tnow;
return;
}
if (tnow - last_timestamp > 200000) {
// we have gone at least 0.2 seconds since the main loop
// ran. That means we're in trouble, or perhaps are in
// an initialisation routine or log erase. disarm the motors
2019-02-11 04:10:43 -04:00
// To-Do: log error
if (arming.is_armed()) {
// disarm motors
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::CPUFAILSAFE);
}
}
}
2015-05-12 04:00:25 -03:00
/*
called to set/unset a failsafe event.
*/
void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on)
{
uint8_t old_bits = failsafe.bits;
if (on) {
failsafe.bits |= failsafe_type;
} else {
failsafe.bits &= ~failsafe_type;
}
if (old_bits == 0 && failsafe.bits != 0) {
// a failsafe event has started
failsafe.start_time = millis();
}
if (failsafe.triggered != 0 && failsafe.bits == 0) {
// a failsafe event has ended
gcs().send_text(MAV_SEVERITY_INFO, "%s Failsafe Cleared", type_str);
}
failsafe.triggered &= failsafe.bits;
if ((failsafe.triggered == 0) &&
(failsafe.bits != 0) &&
(millis() - failsafe.start_time > g.fs_timeout * 1000) &&
(control_mode != &mode_rtl) &&
((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {
failsafe.triggered = failsafe.bits;
gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe", type_str);
2018-10-31 02:33:29 -03:00
// clear rc overrides
RC_Channels::clear_overrides();
if ((control_mode == &mode_auto) &&
((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
(failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
// continue with mission in auto mode
} else {
switch (g.fs_action) {
case Failsafe_Action_None:
break;
case Failsafe_Action_RTL:
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
break;
case Failsafe_Action_Hold:
set_mode(mode_hold, ModeReason::FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
if (!set_mode(mode_rtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
}
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
set_mode(mode_hold, ModeReason::FAILSAFE);
}
break;
}
}
}
}
2018-03-01 23:30:40 -04:00
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
{
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
break;
case Failsafe_Action_SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
2018-03-01 23:30:40 -04:00
break;
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
2018-03-01 23:30:40 -04:00
break;
}
FALLTHROUGH;
case Failsafe_Action_Hold:
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
2018-03-01 23:30:40 -04:00
break;
case Failsafe_Action_SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
2018-03-01 23:30:40 -04:00
}
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str);
2018-03-01 23:30:40 -04:00
#else
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
2018-03-01 23:30:40 -04:00
#endif // ADVANCED_FAILSAFE == ENABLED
break;
}
}
2017-01-30 10:21:55 -04:00
#if ADVANCED_FAILSAFE == ENABLED
/*
check for AFS failsafe check
*/
void Rover::afs_fs_check(void)
{
// perform AFS failsafe checks
2022-03-04 12:41:00 -04:00
g2.afs.check(fence.get_breaches() != 0, failsafe.last_valid_rc_ms);
2017-01-30 10:21:55 -04:00
}
#endif