Plane: move failsafe variables into a structure

this makes the logic a bit easier to follow

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
+static struct {
-// Used for failsafe based on loss of RC signal or GCS signal
+    // A flag if GCS joystick control is in use
-static int16_t failsafe;
+    bool rc_override_active;
-// 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;
 
-// the time when the last HEARTBEAT message arrived from a GCS
+    // A tracking variable for type of failsafe active
-static uint32_t last_heartbeat_ms;
+    // 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

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

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;
     }
 

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;

ArduPlane/defines.h

@@ -18,10 +18,12 @@
 
 // failsafe
 // ----------------------
-#define FAILSAFE_NONE   0
+enum failsafe_state {
-#define FAILSAFE_SHORT  1
+    FAILSAFE_NONE=0,
-#define FAILSAFE_LONG   2
+    FAILSAFE_SHORT=1,
-#define FAILSAFE_GCS    3
+    FAILSAFE_LONG=2,
+    FAILSAFE_GCS=3
+};
 
 
 // active altitude sensor

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;
+    failsafe.state = fstype;
-    ch3_failsafe_timer = millis();
+    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
     // --------------------------------------------------------

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 (failsafe.rc_override_active) {
-        if (millis() - last_heartbeat_ms > g.short_fs_timeout*1000) {
+        if (millis() - failsafe.last_heartbeat_ms > g.short_fs_timeout*1000) {
-            ch3_failsafe = true;
+            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++;
+            failsafe.ch3_counter++;
-            if (failsafeCounter == 9) {
+            if (failsafe.ch3_counter == 9) {
                 gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm);
-            }else if(failsafeCounter == 10) {
+            }else if(failsafe.ch3_counter == 10) {
-                ch3_failsafe = true;
+                failsafe.ch3_failsafe = true;
-            }else if (failsafeCounter > 10) {
+            }else if (failsafe.ch3_counter > 10) {
-                failsafeCounter = 11;
+                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--;
+            failsafe.ch3_counter--;
-            if (failsafeCounter > 3) {
+            if (failsafe.ch3_counter > 3) {
-                failsafeCounter = 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) {
+            } else if(failsafe.ch3_counter == 0) {
-                ch3_failsafe = false;
+                failsafe.ch3_failsafe = false;
             }
         }
     }

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(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
-        if (rc_override_active && (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) {
+        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 && 
+        } else if (!failsafe.rc_override_active && 
-           (tnow - ch3_failsafe_timer) > g.long_fs_timeout*1000) {
+                   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 &&
+            failsafe.last_heartbeat_ms != 0 &&
-            (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) {
+            (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 && 
+        if (failsafe.state == FAILSAFE_GCS && 
-            (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
+            (tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
-            failsafe = FAILSAFE_NONE;
+            failsafe.state = FAILSAFE_NONE;
-        } else if (failsafe == FAILSAFE_LONG && rc_override_active && 
+        } else if (failsafe.state == FAILSAFE_LONG && 
-                   (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
+                   failsafe.rc_override_active && 
-            failsafe = FAILSAFE_NONE;
+                   (tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
-        } else if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) {
+            failsafe.state = FAILSAFE_NONE;
-            failsafe = 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(failsafe.state == FAILSAFE_NONE) {
-        if(ch3_failsafe) {                                              // The condition is checked and the flag ch3_failsafe is set in radio.pde
+        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(failsafe.state == FAILSAFE_SHORT) {
-        if(!ch3_failsafe) {
+        if(!failsafe.ch3_failsafe) {
             failsafe_short_off_event();
         }
     }