diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 7bd1cd6715..9a66cd2abe 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -334,24 +334,32 @@ static struct { ch2_temp : 1500 }; -// A flag if GCS joystick control is in use -static bool rc_override_active = false; //////////////////////////////////////////////////////////////////////////////// // Failsafe //////////////////////////////////////////////////////////////////////////////// -// A tracking variable for type of failsafe active -// Used for failsafe based on loss of RC signal or GCS signal -static int16_t failsafe; -// 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 -static bool ch3_failsafe; +static struct { + // A flag if GCS joystick control is in use + bool rc_override_active; -// the time when the last HEARTBEAT message arrived from a GCS -static uint32_t last_heartbeat_ms; + // A tracking variable for type of failsafe active + // Used for failsafe based on loss of RC signal or GCS signal + int16_t state; + + // 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 + bool ch3_failsafe; + + // number of low ch3 values + uint8_t ch3_counter; + + // the time when the last HEARTBEAT message arrived from a GCS + uint32_t last_heartbeat_ms; + + // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal + uint32_t ch3_timer_ms; +} failsafe; -// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal -static uint32_t ch3_failsafe_timer = 0; //////////////////////////////////////////////////////////////////////////////// // LED output diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 82e868f9ab..1c0724b4cf 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -42,7 +42,7 @@ static bool stick_mixing_enabled(void) // we're in an auto mode. Check the stick mixing flag if (g.stick_mixing != STICK_MIXING_DISABLED && geofence_stickmixing() && - failsafe == FAILSAFE_NONE && + failsafe.state == FAILSAFE_NONE && (g.throttle_fs_enabled == 0 || channel_throttle->radio_in >= g.throttle_fs_value)) { // we're in an auto mode, and haven't triggered failsafe diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 131a2d3f45..42b3fe4873 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -28,7 +28,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) uint8_t system_status = MAV_STATE_ACTIVE; uint32_t custom_mode = control_mode; - if (failsafe != FAILSAFE_NONE) { + if (failsafe.state != FAILSAFE_NONE) { system_status = MAV_STATE_CRITICAL; } @@ -1911,7 +1911,7 @@ mission_failed: // a RC override message is consiered to be a 'heartbeat' from // the ground station for failsafe purposes - last_heartbeat_ms = millis(); + failsafe.last_heartbeat_ms = millis(); break; } @@ -1920,7 +1920,7 @@ mission_failed: // We keep track of the last time we received a heartbeat from // our GCS for failsafe purposes if (msg->sysid != g.sysid_my_gcs) break; - last_heartbeat_ms = millis(); + failsafe.last_heartbeat_ms = millis(); break; } diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 3846615ea2..257fb43a6f 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -10,7 +10,7 @@ static void read_control_switch() // If we get this value we do not want to change modes. if(switchPosition == 255) return; - if (ch3_failsafe) { + if (failsafe.ch3_failsafe) { // when we are in ch3_failsafe mode then RC input is not // working, and we need to ignore the mode switch channel return; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a7e18b6f3b..18abbb4667 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -18,10 +18,12 @@ // failsafe // ---------------------- -#define FAILSAFE_NONE 0 -#define FAILSAFE_SHORT 1 -#define FAILSAFE_LONG 2 -#define FAILSAFE_GCS 3 +enum failsafe_state { + FAILSAFE_NONE=0, + FAILSAFE_SHORT=1, + FAILSAFE_LONG=2, + FAILSAFE_GCS=3 +}; // active altitude sensor diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 2ef2b546f5..e542efe6a8 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -1,11 +1,11 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void failsafe_short_on_event(int16_t fstype) +static void failsafe_short_on_event(enum failsafe_state fstype) { // This is how to handle a short loss of control signal failsafe. - failsafe = fstype; - ch3_failsafe_timer = millis(); + failsafe.state = fstype; + failsafe.ch3_timer_ms = millis(); gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, ")); switch(control_mode) { @@ -35,13 +35,13 @@ static void failsafe_short_on_event(int16_t fstype) gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); } -static void failsafe_long_on_event(int16_t fstype) +static void failsafe_long_on_event(enum failsafe_state fstype) { // This is how to handle a long loss of control signal failsafe. gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); // If the GCS is locked up we allow control to revert to RC hal.rcin->clear_overrides(); - failsafe = fstype; + failsafe.state = fstype; switch(control_mode) { case MANUAL: @@ -74,7 +74,7 @@ static void failsafe_short_off_event() { // We're back in radio contact gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off")); - failsafe = FAILSAFE_NONE; + failsafe.state = FAILSAFE_NONE; // re-read the switch so we can return to our preferred mode // -------------------------------------------------------- diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 3985ead122..26a3583f07 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -2,7 +2,6 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling /* allow for runtime change of control channel ordering @@ -127,11 +126,11 @@ static void control_failsafe(uint16_t pwm) return; // Check for failsafe condition based on loss of GCS control - if (rc_override_active) { - if (millis() - last_heartbeat_ms > g.short_fs_timeout*1000) { - ch3_failsafe = true; + if (failsafe.rc_override_active) { + if (millis() - failsafe.last_heartbeat_ms > g.short_fs_timeout*1000) { + failsafe.ch3_failsafe = true; } else { - ch3_failsafe = false; + failsafe.ch3_failsafe = false; } //Check for failsafe and debounce funky reads @@ -139,26 +138,26 @@ static void control_failsafe(uint16_t pwm) if (pwm < (unsigned)g.throttle_fs_value) { // we detect a failsafe from radio // throttle has dropped below the mark - failsafeCounter++; - if (failsafeCounter == 9) { + failsafe.ch3_counter++; + if (failsafe.ch3_counter == 9) { gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm); - }else if(failsafeCounter == 10) { - ch3_failsafe = true; - }else if (failsafeCounter > 10) { - failsafeCounter = 11; + }else if(failsafe.ch3_counter == 10) { + failsafe.ch3_failsafe = true; + }else if (failsafe.ch3_counter > 10) { + failsafe.ch3_counter = 11; } - }else if(failsafeCounter > 0) { + }else if(failsafe.ch3_counter > 0) { // we are no longer in failsafe condition // but we need to recover quickly - failsafeCounter--; - if (failsafeCounter > 3) { - failsafeCounter = 3; + failsafe.ch3_counter--; + if (failsafe.ch3_counter > 3) { + failsafe.ch3_counter = 3; } - if (failsafeCounter == 1) { + if (failsafe.ch3_counter == 1) { gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm); - } else if(failsafeCounter == 0) { - ch3_failsafe = false; + } else if(failsafe.ch3_counter == 0) { + failsafe.ch3_failsafe = false; } } } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 064b921944..9d3ad31062 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -306,7 +306,7 @@ static void startup_ground(void) // reset last heartbeat time, so we don't trigger failsafe on slow // startup - last_heartbeat_ms = millis(); + failsafe.last_heartbeat_ms = millis(); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are @@ -411,27 +411,31 @@ static void check_long_failsafe() uint32_t tnow = millis(); // only act on changes // ------------------- - if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) { - if (rc_override_active && (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) { + if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) { + if (failsafe.rc_override_active && (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG); - } else if (!rc_override_active && failsafe == FAILSAFE_SHORT && - (tnow - ch3_failsafe_timer) > g.long_fs_timeout*1000) { + } else if (!failsafe.rc_override_active && + failsafe.state == FAILSAFE_SHORT && + (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_LONG); } else if (g.gcs_heartbeat_fs_enabled && - last_heartbeat_ms != 0 && - (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) { + failsafe.last_heartbeat_ms != 0 && + (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) { failsafe_long_on_event(FAILSAFE_GCS); } } else { // We do not change state but allow for user to change mode - if (failsafe == FAILSAFE_GCS && - (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) { - failsafe = FAILSAFE_NONE; - } else if (failsafe == FAILSAFE_LONG && rc_override_active && - (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) { - failsafe = FAILSAFE_NONE; - } else if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) { - failsafe = FAILSAFE_NONE; + if (failsafe.state == FAILSAFE_GCS && + (tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) { + failsafe.state = FAILSAFE_NONE; + } 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.state = FAILSAFE_NONE; } } } @@ -440,14 +444,14 @@ static void check_short_failsafe() { // only act on changes // ------------------- - if(failsafe == FAILSAFE_NONE) { - if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde + if(failsafe.state == FAILSAFE_NONE) { + if(failsafe.ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde failsafe_short_on_event(FAILSAFE_SHORT); } } - if(failsafe == FAILSAFE_SHORT) { - if(!ch3_failsafe) { + if(failsafe.state == FAILSAFE_SHORT) { + if(!failsafe.ch3_failsafe) { failsafe_short_off_event(); } }