2012-04-30 04:17:14 -03:00
|
|
|
/*
|
|
|
|
failsafe support
|
|
|
|
Andrew Tridgell, December 2011
|
|
|
|
*/
|
|
|
|
|
2015-05-13 00:16:45 -03:00
|
|
|
#include "Rover.h"
|
|
|
|
|
2019-08-20 22:21:49 -03:00
|
|
|
#include <stdio.h>
|
|
|
|
|
2012-04-30 04:17:14 -03:00
|
|
|
/*
|
2020-02-11 21:47:21 -04:00
|
|
|
our failsafe strategy is to detect main loop lockup and disarm.
|
2012-04-30 04:17:14 -03:00
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
this failsafe_check function is called from the core timer interrupt
|
|
|
|
at 1kHz.
|
|
|
|
*/
|
2015-05-12 02:03:23 -03:00
|
|
|
void Rover::failsafe_check()
|
2012-04-30 04:17:14 -03:00
|
|
|
{
|
2017-11-13 01:58:44 -04:00
|
|
|
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();
|
2012-04-30 04:17:14 -03:00
|
|
|
|
2017-11-13 01:58:44 -04:00
|
|
|
const uint16_t ticks = scheduler.ticks();
|
|
|
|
if (ticks != last_ticks) {
|
2012-04-30 04:17:14 -03:00
|
|
|
// the main loop is running, all is OK
|
2017-11-13 01:58:44 -04:00
|
|
|
last_ticks = ticks;
|
2012-04-30 04:17:14 -03:00
|
|
|
last_timestamp = tnow;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (tnow - last_timestamp > 200000) {
|
|
|
|
// we have gone at least 0.2 seconds since the main loop
|
2016-12-20 09:33:16 -04:00
|
|
|
// ran. That means we're in trouble, or perhaps are in
|
2017-08-22 06:26:13 -03:00
|
|
|
// an initialisation routine or log erase. disarm the motors
|
2019-02-11 04:10:43 -04:00
|
|
|
// To-Do: log error
|
2017-08-22 06:26:13 -03:00
|
|
|
if (arming.is_armed()) {
|
|
|
|
// disarm motors
|
2020-02-21 09:09:57 -04:00
|
|
|
arming.disarm(AP_Arming::Method::CPUFAILSAFE);
|
2012-12-18 07:44:12 -04:00
|
|
|
}
|
2012-04-30 04:17:14 -03:00
|
|
|
}
|
|
|
|
}
|
2015-05-12 04:00:25 -03:00
|
|
|
|
2017-06-20 14:26:45 -03:00
|
|
|
/*
|
|
|
|
called to set/unset a failsafe event.
|
|
|
|
*/
|
2020-02-01 11:29:53 -04:00
|
|
|
void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool on)
|
2017-06-20 14:26:45 -03:00
|
|
|
{
|
|
|
|
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
|
2020-02-01 11:29:53 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "%s Failsafe Cleared", type_str);
|
2017-06-20 14:26:45 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
failsafe.triggered &= failsafe.bits;
|
|
|
|
|
2019-10-04 04:07:46 -03:00
|
|
|
if ((failsafe.triggered == 0) &&
|
|
|
|
(failsafe.bits != 0) &&
|
|
|
|
(millis() - failsafe.start_time > g.fs_timeout * 1000) &&
|
|
|
|
(control_mode != &mode_rtl) &&
|
2019-09-29 23:32:07 -03:00
|
|
|
((control_mode != &mode_hold || (g2.fs_options & (uint32_t)Failsafe_Options::Failsafe_Option_Active_In_Hold)))) {
|
2017-06-20 14:26:45 -03:00
|
|
|
failsafe.triggered = failsafe.bits;
|
2020-02-01 11:29:53 -04:00
|
|
|
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();
|
|
|
|
|
2018-11-13 04:40:40 -04:00
|
|
|
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
|
2023-01-29 18:49:35 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
|
2018-11-13 04:40:40 -04:00
|
|
|
} else {
|
2023-09-21 11:22:37 -03:00
|
|
|
switch ((FailsafeAction)g.fs_action.get()) {
|
|
|
|
case FailsafeAction::None:
|
2017-06-20 14:26:45 -03:00
|
|
|
break;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::SmartRTL:
|
2024-07-01 13:04:03 -03:00
|
|
|
if (set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
|
2023-09-21 11:22:37 -03:00
|
|
|
break;
|
2018-01-18 18:19:20 -04:00
|
|
|
}
|
2023-09-21 11:22:37 -03:00
|
|
|
FALLTHROUGH;
|
|
|
|
case FailsafeAction::RTL:
|
2024-07-01 13:04:03 -03:00
|
|
|
if (set_mode(mode_rtl, ModeReason::FAILSAFE)) {
|
2023-09-21 11:22:37 -03:00
|
|
|
break;
|
2018-01-18 14:19:50 -04:00
|
|
|
}
|
2023-09-21 11:22:37 -03:00
|
|
|
FALLTHROUGH;
|
|
|
|
case FailsafeAction::Hold:
|
2024-07-01 13:04:03 -03:00
|
|
|
set_mode(mode_hold, ModeReason::FAILSAFE);
|
2018-01-18 14:19:50 -04:00
|
|
|
break;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::SmartRTL_Hold:
|
2019-10-17 00:48:47 -03:00
|
|
|
if (!set_mode(mode_smartrtl, ModeReason::FAILSAFE)) {
|
|
|
|
set_mode(mode_hold, ModeReason::FAILSAFE);
|
2018-01-18 14:19:50 -04:00
|
|
|
}
|
|
|
|
break;
|
2023-09-21 11:23:01 -03:00
|
|
|
case FailsafeAction::Terminate:
|
|
|
|
arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE);
|
|
|
|
break;
|
2018-11-13 04:40:40 -04:00
|
|
|
}
|
2017-06-20 14:26:45 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-03-01 23:30:40 -04:00
|
|
|
void Rover::handle_battery_failsafe(const char* type_str, const int8_t action)
|
|
|
|
{
|
2023-09-21 11:22:37 -03:00
|
|
|
switch ((FailsafeAction)action) {
|
|
|
|
case FailsafeAction::None:
|
2018-03-01 23:30:40 -04:00
|
|
|
break;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::SmartRTL:
|
2019-10-17 00:48:47 -03:00
|
|
|
if (set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
|
2018-03-01 23:30:40 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::RTL:
|
2019-10-17 00:48:47 -03:00
|
|
|
if (set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE)) {
|
2018-03-01 23:30:40 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::Hold:
|
2019-10-17 00:48:47 -03:00
|
|
|
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
|
2018-03-01 23:30:40 -04:00
|
|
|
break;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::SmartRTL_Hold:
|
2019-10-17 00:48:47 -03:00
|
|
|
if (!set_mode(mode_smartrtl, ModeReason::BATTERY_FAILSAFE)) {
|
|
|
|
set_mode(mode_hold, ModeReason::BATTERY_FAILSAFE);
|
2018-03-01 23:30:40 -04:00
|
|
|
}
|
|
|
|
break;
|
2023-09-21 11:22:37 -03:00
|
|
|
case FailsafeAction::Terminate:
|
2018-03-01 23:30:40 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
|
|
|
char battery_type_str[17];
|
|
|
|
snprintf(battery_type_str, 17, "%s battery", type_str);
|
2018-05-05 05:28:48 -03:00
|
|
|
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-10-20 22:25:53 -03:00
|
|
|
g2.afs.check(failsafe.last_valid_rc_ms);
|
2017-01-30 10:21:55 -04:00
|
|
|
}
|
|
|
|
#endif
|