Sub: Rework pilot input failsafe, add enable and timeout params

ArduSub/Parameters.cpp

@@ -165,6 +165,21 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(failsafe_terrain, "FS_TERRAIN_ENAB", FS_TERRAIN_DISARM),
 
+    // @Param: FS_PILOT_INPUT
+    // @DisplayName: Pilot input failsafe action
+    // @Description: Controls what action to take if no pilot input has been received after the timeout period specified by the FS_PILOT_TIMEOUT parameter
+    // @Values: 0:Disabled, 1:Warn Only, 2:Disarm
+    // @User: Standard
+    GSCALAR(failsafe_pilot_input, "FS_PILOT_INPUT", FS_PILOT_INPUT_DISARM),
+
+    // @Param: FS_PILOT_TIMEOUT
+    // @DisplayName: Timeout for activation of pilot input failsafe
+    // @Description: Controls the maximum interval between received pilot inputs before the failsafe action is triggered
+    // @Units: Seconds
+    // @Range 0.1 3.0
+    // @User: Standard
+    GSCALAR(failsafe_pilot_input_timeout, "FS_PILOT_TIMEOUT", 1.0f),
+
     // @Param: XTRACK_ANG_LIM
     // @DisplayName: Crosstrack correction angle limit
     // @Description: Maximum allowed angle (in degrees) between current track and desired heading during waypoint navigation

ArduSub/Parameters.h

@@ -170,6 +170,8 @@ public:
         k_param_failsafe_battery_enabled,
         k_param_fs_batt_mah,
         k_param_fs_batt_voltage,
+        k_param_failsafe_pilot_input,
+        k_param_failsafe_pilot_input_timeout,
 
 
         // Misc Sub settings
@@ -233,6 +235,8 @@ public:
     AP_Int32        failsafe_pressure_max;
     AP_Int8         failsafe_temperature_max;
     AP_Int8         failsafe_terrain;
+    AP_Int8         failsafe_pilot_input;       // pilot input failsafe behavior
+    AP_Float        failsafe_pilot_input_timeout;
 
     AP_Int8         xtrack_angle_limit;
 

ArduSub/defines.h

@@ -303,6 +303,11 @@ enum RTLState {
 #define FS_TERRAIN_HOLD         1
 #define FS_TERRAIN_SURFACE      2
 
+// Pilot input failsafe actions
+#define FS_PILOT_INPUT_DISABLED    0
+#define FS_PILOT_INPUT_WARN_ONLY   1
+#define FS_PILOT_INPUT_DISARM      2
+
 // Amount of time to attempt recovery of valid rangefinder data before
 // initiating terrain failsafe action
 #define FS_TERRAIN_RECOVER_TIMEOUT_MS 10000

ArduSub/failsafe.cpp

@@ -168,21 +168,29 @@ void Sub::failsafe_battery_check(void)
 void Sub::failsafe_pilot_input_check()
 {
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
-    uint32_t tnow = AP_HAL::millis();
+    if (g.failsafe_pilot_input == FS_PILOT_INPUT_DISABLED) {
-
+        failsafe.pilot_input = false;
-    // Require at least 0.5 Hz update
-    if (tnow > failsafe.last_pilot_input_ms + 2000) {
-        if (!failsafe.pilot_input) {
-            failsafe.pilot_input = true;
-            set_neutral_controls();
-            init_disarm_motors();
-            Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
-            gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
-        }
         return;
     }
 
-    failsafe.pilot_input = false;
+    if (AP_HAL::millis() < failsafe.last_pilot_input_ms + g.failsafe_pilot_input_timeout * 1000.0f) {
+        failsafe.pilot_input = false; // We've received an update from the pilot within the timeout period
+        return;
+    }
+
+    if (failsafe.pilot_input) {
+        return; // only act once
+    }
+
+    failsafe.pilot_input = true;
+
+    Log_Write_Error(ERROR_SUBSYSTEM_INPUT, ERROR_CODE_FAILSAFE_OCCURRED);
+    gcs_send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
+
+    if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
+        set_neutral_controls();
+        init_disarm_motors();
+    }
 #endif
 }