Browse Source

ArduPlane failsafes: remove rc_override_active

- rc_override_active is never set anywhere in the ArduPlane code; it's only used for Copter and Rover. Removing it significantly simplifies the failsafe code.
- modified code has been tested in SITL. Heartbeat and RC failures in AUTO, CRUISE, and RTL modes (covering the three cases in the failsafe check functions) have been simulated with FS_LONG_ACTN = 0, 1, and 2, FS_SHORT_ACTN = 0, 1, and 2, and FS_GCS_ENABL = 0, 1, and 2. In all cases the results are identical to those with the original code.
mission-4.1.18
Evan Slatyer 11 years ago committed by Andrew Tridgell
parent
commit
c4093b159f
  1. 2
      ArduPlane/ArduPlane.pde
  2. 13
      ArduPlane/radio.pde
  3. 10
      ArduPlane/system.pde

2
ArduPlane/ArduPlane.pde

@ -374,8 +374,6 @@ static struct {
// Failsafe // Failsafe
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static struct { static struct {
// A flag if GCS joystick control is in use
uint8_t rc_override_active:1;
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost // RC receiver should be set up to output a low throttle value when signal is lost

13
ArduPlane/radio.pde

@ -193,18 +193,7 @@ static void control_failsafe(uint16_t pwm)
if(g.throttle_fs_enabled == 0) if(g.throttle_fs_enabled == 0)
return; return;
// Check for failsafe condition based on loss of GCS control if (g.throttle_fs_enabled) {
if (failsafe.rc_override_active) {
if (millis() - failsafe.last_heartbeat_ms > g.short_fs_timeout*1000) {
failsafe.ch3_failsafe = true;
AP_Notify::flags.failsafe_radio = true;
} else {
failsafe.ch3_failsafe = false;
AP_Notify::flags.failsafe_radio = false;
}
//Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) {
if (rc_failsafe_active()) { if (rc_failsafe_active()) {
// we detect a failsafe from radio // we detect a failsafe from radio
// throttle has dropped below the mark // throttle has dropped below the mark

10
ArduPlane/system.pde

@ -449,10 +449,7 @@ static void check_long_failsafe()
// only act on changes // only act on changes
// ------------------- // -------------------
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) { if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
if (failsafe.rc_override_active && (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { if (failsafe.state == FAILSAFE_SHORT &&
failsafe_long_on_event(FAILSAFE_LONG);
} else if (!failsafe.rc_override_active &&
failsafe.state == FAILSAFE_SHORT &&
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_LONG); failsafe_long_on_event(FAILSAFE_LONG);
} else if (g.gcs_heartbeat_fs_enabled != GCS_FAILSAFE_OFF && } else if (g.gcs_heartbeat_fs_enabled != GCS_FAILSAFE_OFF &&
@ -470,11 +467,6 @@ static void check_long_failsafe()
(tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) { (tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe.state = FAILSAFE_NONE; failsafe.state = FAILSAFE_NONE;
} else if (failsafe.state == FAILSAFE_LONG && } else if (failsafe.state == FAILSAFE_LONG &&
failsafe.rc_override_active &&
(tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe.state = FAILSAFE_NONE;
} else if (failsafe.state == FAILSAFE_LONG &&
!failsafe.rc_override_active &&
!failsafe.ch3_failsafe) { !failsafe.ch3_failsafe) {
failsafe.state = FAILSAFE_NONE; failsafe.state = FAILSAFE_NONE;
} }

Loading…
Cancel
Save