Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: move failsafe_trigger from system to failsafe

This commit is contained in:
Pierre Kancir 2017-06-20 19:26:45 +02:00 committed by Randy Mackay
parent 4270bac49e
commit 0017485ee7
2 changed files with 42 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
APMrover2/failsafe.cpp
View File

@ -49,6 +49,48 @@ void Rover::failsafe_check()
} }
} }
/*
called to set/unset a failsafe event.
*/
void Rover::failsafe_trigger(uint8_t failsafe_type, 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_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
}
failsafe.triggered &= failsafe.bits;
if (failsafe.triggered == 0 &&
failsafe.bits != 0 &&
millis() - failsafe.start_time > g.fs_timeout * 1000 &&
control_mode != RTL &&
control_mode != HOLD) {
failsafe.triggered = failsafe.bits;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
switch (g.fs_action) {
case 0:
break;
case 1:
set_mode(RTL);
break;
case 2:
set_mode(HOLD);
break;
}
}
}
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
/* /*
check for AFS failsafe check check for AFS failsafe check

42
APMrover2/system.cpp
View File

@ -368,48 +368,6 @@ bool Rover::mavlink_set_mode(uint8_t mode)
return false; return false;
} }
/*
called to set/unset a failsafe event.
*/
void Rover::failsafe_trigger(uint8_t failsafe_type, 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_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
}
failsafe.triggered &= failsafe.bits;
if (failsafe.triggered == 0 &&
failsafe.bits != 0 &&
millis() - failsafe.start_time > g.fs_timeout*1000 &&
control_mode != RTL &&
control_mode != HOLD) {
failsafe.triggered = failsafe.bits;
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
switch (g.fs_action) {
case 0:
break;
case 1:
set_mode(RTL);
break;
case 2:
set_mode(HOLD);
break;
}
}
}
void Rover::startup_INS_ground(void) void Rover::startup_INS_ground(void)
{ {
gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC"); gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC");