From 11ab5e3f3d5ac640fadf0a6279d1f3c72839282b Mon Sep 17 00:00:00 2001
From: James O'Shannessy <12959316+joshanne@users.noreply.github.com>
Date: Thu, 1 Dec 2022 21:31:22 +1100
Subject: [PATCH] AP_Periph: Adds a parameter to allow for user configurable
 timeout

Timeout defaults to 200ms. Timeout can be disabled by setting parameter to 0. Timeout should be configured to be able to handle at small number of missed packets.
---
 Tools/AP_Periph/Parameters.cpp | 8 ++++++++
 Tools/AP_Periph/Parameters.h   | 2 ++
 Tools/AP_Periph/rc_out.cpp     | 5 +++--
 3 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index a7eb3962b8..21700de273 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -373,6 +373,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GSCALAR(esc_pwm_type, "ESC_PWM_TYPE",     0),
 
+    // @Param: ESC_CMD_TIMO
+    // @DisplayName: ESC Command Timeout
+    // @Description: This is the duration (ms) with which to hold the last driven ESC command before timing out and zeroing the ESC outputs. To disable zeroing of outputs in event of CAN loss, use 0. Use values greater than the expected duration between two CAN frames to ensure Periph is not starved of ESC Raw Commands.
+    // @Range: 0 10000
+    // @Units: ms
+    // @User: Advanced
+    GSCALAR(esc_command_timeout_ms, "ESC_CMD_TIMO",     200),
+
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     // @Param: ESC_TELEM_PORT
     // @DisplayName: ESC Telemetry Serial Port
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 5c9de5d642..d0a2f30b25 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -62,6 +62,7 @@ public:
         k_param_esc_telem_rate,
         k_param_can_slcan_cport,
         k_param_temperature_sensor,
+        k_param_esc_command_timeout_ms,
     };
 
     AP_Int16 format_version;
@@ -122,6 +123,7 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
     AP_Int8 esc_pwm_type;
+    AP_Int16 esc_command_timeout_ms;
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     AP_Int8 esc_telem_port;
 #endif
diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
index 346a67849c..ff2e6fc9fe 100644
--- a/Tools/AP_Periph/rc_out.cpp
+++ b/Tools/AP_Periph/rc_out.cpp
@@ -127,8 +127,9 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
 void AP_Periph_FW::rcout_update()
 {
     uint32_t now_ms = AP_HAL::millis();
-    const uint16_t esc_timeout_ms = 100U;
-    const bool has_esc_rawcommand_timed_out = (now_ms - last_esc_raw_command_ms) >= esc_timeout_ms;
+
+    const uint16_t esc_timeout_ms = g.esc_command_timeout_ms >= 0 ? g.esc_command_timeout_ms : 0; // Don't allow negative timeouts!
+    const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms);
     if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) {
         // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs
         int16_t esc_output[last_esc_num_channels] {};