Plane: added FS_LONG_TIMEOUT and FS_SHORT_TIMEOUT parameters

this is to address the issue raised here:

http://diydrones.com/forum/topics/ardupilot-circle-mode

ArduPlane/Parameters.h

@@ -191,6 +191,8 @@ public:
         k_param_rc_12,
         k_param_fs_batt_voltage,
         k_param_fs_batt_mah,
+        k_param_short_fs_timeout,
+        k_param_long_fs_timeout,
 
         //
         // 200: Feed-forward gains
@@ -313,6 +315,8 @@ public:
     // Failsafe
     AP_Int8 short_fs_action;
     AP_Int8 long_fs_action;
+    AP_Float short_fs_timeout;
+    AP_Float long_fs_timeout;
     AP_Int8 gcs_heartbeat_fs_enabled;
     AP_Float fs_batt_voltage;
     AP_Float fs_batt_mah;

ArduPlane/Parameters.pde

@@ -357,18 +357,36 @@ const AP_Param::Info var_info[] PROGMEM = {
 
     // @Param: FS_SHORT_ACTN
     // @DisplayName: Short failsafe action
-    // @Description: The action to take on a short (1.5 seconds) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause a change to CIRCLE mode. In AUTO mode you can choose whether it will RTL (ReturnToLaunch) or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for 20 seconds.
+    // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event in AUTO, GUIDED or LOITER modes. A short failsafe event in stabilization modes will always cause an immediate change to CIRCLE mode. In AUTO mode you can choose whether it will RTL (ReturnToLaunch) or continue with the mission. If FS_SHORT_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter CIRCLE mode, and then enter RTL if the failsafe condition persists for FS_LONG_TIMEOUT seconds.
     // @Values: 0:Continue,1:Circle/ReturnToLaunch
     // @User: Standard
     GSCALAR(short_fs_action,        "FS_SHORT_ACTN",  SHORT_FAILSAFE_ACTION),
 
+    // @Param: FS_SHORT_TIMEOUT
+    // @DisplayName: Short failsafe timeout
+    // @Description: The time in seconds that a failsafe condition has to persist before a short failsafe event will occor. This defaults to 1.5 seconds
+    // @Units: seconds
+    // @Range: 1 100
+    // @Increment: 0.5
+    // @User: Standard
+    GSCALAR(short_fs_timeout,        "FS_SHORT_TIMEOUT", 1.5f),
+
     // @Param: FS_LONG_ACTN
     // @DisplayName: Long failsafe action
-    // @Description: The action to take on a long (20 second) failsafe event in AUTO, GUIDED or LOITER modes. A long failsafe event in stabilization modes will always cause an RTL (ReturnToLaunch). In AUTO modes you can choose whether it will RTL or continue with the mission. If FS_LONG_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter RTL mode. Note that if FS_SHORT_ACTN is 1, then the aircraft will enter CIRCLE mode after 1.5 seconds of failsafe, and will always enter RTL after 20 seconds of failsafe, regardless of the FS_LONG_ACTN setting.
+    // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event in AUTO, GUIDED or LOITER modes. A long failsafe event in stabilization modes will always cause an RTL (ReturnToLaunch). In AUTO modes you can choose whether it will RTL or continue with the mission. If FS_LONG_ACTN is 0 then it will continue with the mission, if it is 1 then it will enter RTL mode. Note that if FS_SHORT_ACTN is 1, then the aircraft will enter CIRCLE mode after FS_SHORT_TIMEOUT seconds of failsafe, and will always enter RTL after FS_LONG_TIMEOUT seconds of failsafe, regardless of the FS_LONG_ACTN setting.
     // @Values: 0:Continue,1:ReturnToLaunch
     // @User: Standard
     GSCALAR(long_fs_action,         "FS_LONG_ACTN",   LONG_FAILSAFE_ACTION),
 
+    // @Param: FS_LONG_TIMEOUT
+    // @DisplayName: Long failsafe timeout
+    // @Description: The time in seconds that a failsafe condition has to persist before a long failsafe event will occor. This defaults to 20 seconds
+    // @Units: seconds
+    // @Range: 1 300
+    // @Increment: 0.5
+    // @User: Standard
+    GSCALAR(long_fs_timeout,        "FS_LONG_TIMEOUT", 20),
+
     // @Param: FS_BATT_VOLTAGE
     // @DisplayName: Failsafe battery voltage
     // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the plane will RTL

ArduPlane/defines.h

@@ -22,8 +22,6 @@
 #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

ArduPlane/radio.pde

@@ -140,7 +140,7 @@ static void control_failsafe(uint16_t pwm)
 
     // Check for failsafe condition based on loss of GCS control
     if (rc_override_active) {
-        if (millis() - last_heartbeat_ms > FAILSAFE_SHORT_TIME) {
+        if (millis() - last_heartbeat_ms > g.short_fs_timeout*1000) {
             ch3_failsafe = true;
         } else {
             ch3_failsafe = false;

ArduPlane/system.pde

@@ -402,28 +402,27 @@ static void check_long_failsafe()
     // only act on changes
     // -------------------
     if(failsafe != FAILSAFE_LONG  && failsafe != FAILSAFE_GCS) {
-        if (rc_override_active && tnow - last_heartbeat_ms > FAILSAFE_LONG_TIME) {
+        if (rc_override_active && (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) {
             failsafe_long_on_event(FAILSAFE_LONG);
-        }
+        } else if (!rc_override_active && failsafe == FAILSAFE_SHORT && 
-        if(!rc_override_active && failsafe == FAILSAFE_SHORT && 
+           (tnow - ch3_failsafe_timer) > g.long_fs_timeout*1000) {
-           (tnow - ch3_failsafe_timer) > FAILSAFE_LONG_TIME) {
             failsafe_long_on_event(FAILSAFE_LONG);
-        }
+        } else if (g.gcs_heartbeat_fs_enabled && 
-        if (g.gcs_heartbeat_fs_enabled && 
             last_heartbeat_ms != 0 &&
-            (tnow - last_heartbeat_ms) > FAILSAFE_LONG_TIME) {
+            (tnow - 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) < FAILSAFE_SHORT_TIME) 
+            (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
             failsafe = FAILSAFE_NONE;
-        if (failsafe == FAILSAFE_LONG && rc_override_active && 
+        } else if (failsafe == FAILSAFE_LONG && rc_override_active && 
-            (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME) 
+                   (tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
             failsafe = FAILSAFE_NONE;
-        if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) 
+        } else if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) {
             failsafe = FAILSAFE_NONE;
+        }
     }
 }