From 8e17b8d6d439d5d3206db13a59a3c9823bd06e39 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 24 Dec 2011 15:34:31 -0700 Subject: [PATCH] Correct a bug in throttle failsafe feature --- ArduPlane/control_modes.pde | 6 ++++++ ArduPlane/events.pde | 7 ++++--- ArduPlane/system.pde | 11 ++++------- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index c5b31df963..1c545559ef 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -2,7 +2,12 @@ static void read_control_switch() { + byte switchPosition = readSwitch(); + + // If switchPosition = 255 this indicates that the mode control channel input was out of range + // If we get this value we do not want to change modes. + if(switchPosition == 255) return; // we look for changes in the switch position. If the // RST_SWITCH_CH parameter is set, then it is a switch that can be @@ -33,6 +38,7 @@ static void read_control_switch() static byte readSwitch(void){ uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1); + if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; if (pulsewidth > 1490 && pulsewidth <= 1620) return 3; diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index e4ea0b4f0c..de38c848ca 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -1,10 +1,10 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void failsafe_short_on_event() +static void failsafe_short_on_event(int fstype) { // This is how to handle a short loss of control signal failsafe. - failsafe = FAILSAFE_SHORT; + failsafe = fstype; ch3_failsafe_timer = millis(); gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, ")); switch(control_mode) @@ -32,11 +32,12 @@ static void failsafe_short_on_event() gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); } -static void failsafe_long_on_event() +static void failsafe_long_on_event(int fstype) { // This is how to handle a long loss of control signal failsafe. gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC + failsafe = fstype; switch(control_mode) { case MANUAL: diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 0dfbaf87aa..11af9abf41 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -430,16 +430,13 @@ static void check_long_failsafe() // ------------------- if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){ if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_LONG; - failsafe_long_on_event(); + failsafe_long_on_event(FAILSAFE_LONG); } if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_LONG; - failsafe_long_on_event(); + failsafe_long_on_event(FAILSAFE_LONG); } if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { - failsafe = FAILSAFE_GCS; - failsafe_long_on_event(); + failsafe_long_on_event(FAILSAFE_GCS); } } else { // We do not change state but allow for user to change mode @@ -455,7 +452,7 @@ static void check_short_failsafe() // ------------------- if(failsafe == FAILSAFE_NONE){ if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde - failsafe_short_on_event(); + failsafe_short_on_event(FAILSAFE_SHORT); } }