Rover: new failsafe logic

this obeys FS_TIMEOUT and FS_ACTION

APMrover2/APMrover2.pde

@@ -293,16 +293,14 @@ 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
+// Used for failsafe based on loss of RC signal or GCS signal. See 
-static int16_t 	failsafe;					
+// FAILSAFE_EVENT_*
-// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
+static struct {
-// RC receiver should be set up to output a low throttle value when signal is lost
+    uint8_t bits;
-static bool 	ch3_failsafe;
+    uint32_t rc_override_timer;
-
+    uint32_t start_time;
-// A timer used to track how long since we have received the last GCS heartbeat or rc override message
+    uint8_t triggered;
-static uint32_t rc_override_fs_timer = 0;
+} 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
@@ -667,6 +665,7 @@ static void medium_loop()
 		// This case deals with the GPS
 		//-------------------------------
 		case 0:
+            failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
 			medium_loopCounter++;
             update_GPS();
             
@@ -744,7 +743,6 @@ static void slow_loop()
 	switch (slow_loopCounter){
 		case 0:
 			slow_loopCounter++;
-			check_long_failsafe();
 			superslow_loopCounter++;
 			if(superslow_loopCounter >=200) {				//	200 = Execute every minute
 				#if HIL_MODE != HIL_MODE_ATTITUDE

APMrover2/GCS_Mavlink.pde

@@ -25,7 +25,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.triggered != 0) {
         system_status = MAV_STATE_CRITICAL;
     }
 
@@ -1631,7 +1631,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         hal.rcin->set_overrides(v, 8);
 
-        rc_override_fs_timer = millis();
+        failsafe.rc_override_timer = millis();
+        failsafe_trigger(FAILSAFE_EVENT_RC, false);
         break;
     }
 
@@ -1639,7 +1640,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         {
             // 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 = rc_override_fs_timer = millis();
+            last_heartbeat_ms = failsafe.rc_override_timer = millis();
+            failsafe_trigger(FAILSAFE_EVENT_GCS, false);
 			pmTest1++;
             break;
         }

APMrover2/Parameters.pde

@@ -241,16 +241,16 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Param: FS_ACTION
     // @DisplayName: Failsafe Action
     // @Description: What to do on a failsafe event
-    // @Values: 0:Nothing,1:RTL,2:STOP
+    // @Values: 0:Nothing,1:RTL,2:HOLD
     // @User: Standard
-	GSCALAR(fs_action,    "FS_ACTION",     0),
+	GSCALAR(fs_action,    "FS_ACTION",     1),
 
     // @Param: FS_TIMEOUT
     // @DisplayName: Failsafe timeout
     // @Description: How long a failsafe event need to happen for before we trigger the failsafe action
 	// @Units: seconds
     // @User: Standard
-	GSCALAR(fs_timeout,    "FS_TIMEOUT",     10),
+	GSCALAR(fs_timeout,    "FS_TIMEOUT",     5),
 
     // @Param: FS_THR_ENABLE
     // @DisplayName: Throttle Failsafe Enable

APMrover2/Steering.pde

@@ -157,11 +157,21 @@ static void set_servos(void)
         // do a direct pass through of radio values
         g.channel_steer.radio_out       = hal.rcin->read(CH_STEER);
         g.channel_throttle.radio_out    = hal.rcin->read(CH_THROTTLE);
+        if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+            // suppress throttle if in failsafe and manual
+            g.channel_throttle.radio_out = g.channel_throttle.radio_trim;
+        }
 	} else {       
         g.channel_steer.calc_pwm();
 		g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, 
                                                        g.throttle_min.get(), 
                                                        g.throttle_max.get());
+
+        if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
+            // suppress throttle if in failsafe
+            g.channel_throttle.servo_out = 0;
+        }
+
         // convert 0 to 100% into PWM
         g.channel_throttle.calc_pwm();
 

APMrover2/defines.h

@@ -14,16 +14,6 @@
 #define DEBUG 0
 #define SERVO_MAX 4500	// This value represents 45 degrees and is just an arbitrary representation of servo max travel.
 
-// failsafe
-// ----------------------
-#define FAILSAFE_NONE	0
-#define FAILSAFE_SHORT	1
-#define FAILSAFE_LONG	2
-#define FAILSAFE_GCS	3
-#define	FAILSAFE_SHORT_TIME 1500	// Miliiseconds
-#define	FAILSAFE_LONG_TIME  20000	// Miliiseconds
-
-
 // active altitude sensor
 // ----------------------
 #define SONAR 0
@@ -73,6 +63,11 @@ enum mode {
     INITIALISING=16
 };
 
+// types of failsafe events
+#define FAILSAFE_EVENT_THROTTLE (1<<0)
+#define FAILSAFE_EVENT_GCS      (1<<1)
+#define FAILSAFE_EVENT_RC       (1<<2)
+
 // Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library
 #define CMD_BLANK 0 // there is no command stored in the mem location requested
 #define NO_COMMAND 0

APMrover2/events.pde

@@ -1,33 +1,6 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 
-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, "));
-    hal.rcin->clear_overrides();
-	failsafe = fstype;
-	switch(control_mode)
-	{
-		case MANUAL: 
-		case LEARNING:
-		case STEERING:
-			set_mode(RTL);
-			break;
-
-		case AUTO: 
-		case GUIDED: 
-            set_mode(RTL);
-			break;
-			
-		case RTL: 
-        case HOLD: 
-		default:
-			break;
-	}
-    gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
-}
-
 static void update_events(void)	// Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
 {
 	if(event_repeat == 0 || (millis() - event_timer) < event_delay)

APMrover2/radio.pde

@@ -1,10 +1,5 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-//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 rover circling
-
-
 static void init_rc_in()
 {
 	// set rc channel ranges
@@ -117,41 +112,9 @@ static void control_failsafe(uint16_t pwm)
 
 	// Check for failsafe condition based on loss of GCS control
 	if (rc_override_active) {
-		if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) {
+        failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
-			ch3_failsafe = true;
-		} else {
-			ch3_failsafe = false;
-		}
-
-	//Check for failsafe and debounce funky reads
 	} else if (g.fs_throttle_enabled) {
-		if (pwm < (unsigned)g.fs_throttle_value){
+        failsafe_trigger(FAILSAFE_EVENT_THROTTLE, pwm < (uint16_t)g.fs_throttle_value);
-			// we detect a failsafe from radio
-			// throttle has dropped below the mark
-			failsafeCounter++;
-			if (failsafeCounter == 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(failsafeCounter > 0){
-			// we are no longer in failsafe condition
-			// but we need to recover quickly
-			failsafeCounter--;
-			if (failsafeCounter > 3){
-				failsafeCounter = 3;
-			}
-			if (failsafeCounter == 1){
-				gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
-			}else if(failsafeCounter == 0) {
-				ch3_failsafe = false;
-			}else if (failsafeCounter <0){
-				failsafeCounter = -1;
-			}
-		}
 	}
 }
 

APMrover2/system.pde

@@ -337,26 +337,46 @@ static void set_mode(enum mode mode)
 		Log_Write_Mode(control_mode);
 }
 
-static void check_long_failsafe()
+/*
+  called to set/unset a failsafe event. 
+ */
+static void failsafe_trigger(uint8_t failsafe_type, bool on)
 {
-	// only act on changes
+    uint8_t old_bits = failsafe.bits;
-	// -------------------
+    if (on) {
-	if(failsafe != FAILSAFE_LONG  && failsafe != FAILSAFE_GCS){
+        failsafe.bits |= failsafe_type;
-		if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
+    } else {
-			failsafe_long_on_event(FAILSAFE_LONG);
+        failsafe.bits &= ~failsafe_type;
-		}
+    }
-		if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
+    if (old_bits == 0 && failsafe.bits != 0) {
-			failsafe_long_on_event(FAILSAFE_LONG);
+        // a failsafe event has started
-		}
+        failsafe.start_time = millis();
-		if (g.fs_gcs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
+    }
-			failsafe_long_on_event(FAILSAFE_GCS);
+    if (failsafe.triggered != 0 && failsafe.bits == 0) {
-		}
+        // a failsafe event has ended
-	} else {
+        gcs_send_text_fmt(PSTR("Failsafe ended"));
-		// We do not change state but allow for user to change mode
+    }
-		if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
+
-		if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
+    failsafe.triggered &= failsafe.bits;
-		if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE;
+
-	}
+    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(PSTR("Failsafe trigger 0x%x"), (unsigned)failsafe.triggered);
+        switch (g.fs_action) {
+        case 0:
+            break;
+        case 1:
+            set_mode(RTL);
+            break;
+        case 2:
+            set_mode(HOLD);
+            break;
+        }
+    }
 }
 
 static void startup_INS_ground(bool force_accel_level)