From dc58d0406c540ed9b11e9c71d6e55b75f143fde2 Mon Sep 17 00:00:00 2001
From: Thomas Watson <twatson52@icloud.com>
Date: Thu, 27 Jun 2024 18:20:37 -0500
Subject: [PATCH] AP_Periph: add ESC_RATE parameter to RC_OUT peripherals

Like other vehicles (which use RC_SPEED), ESC_RATE is used to set the
PWM output rate for outputs whose functions are set to MotorN so that
ESCs can be driven at a fast speed (400Hz, same default as aerial
vehicles) while servos still run at normal speed (50Hz, controlled by
OUT_RATE).
---
 Tools/AP_Periph/Parameters.cpp | 9 +++++++++
 Tools/AP_Periph/Parameters.h   | 2 ++
 Tools/AP_Periph/rc_out.cpp     | 3 +++
 3 files changed, 14 insertions(+)

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 61e42c29f3..79eed58f74 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -407,6 +407,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
     GOBJECT(servo_channels, "OUT",     SRV_Channels),
 
+    // @Param: ESC_RATE
+    // @DisplayName: ESC Update Rate
+    // @Description: Rate in Hz that ESC PWM outputs (function is MotorN) will update at
+    // @Units: Hz
+    // @Range: 50 400
+    // @Increment: 1
+    // @User: Advanced
+    GSCALAR(esc_rate, "ESC_RATE", 400), // effective Copter and QuadPlane default after clamping
+
     // @Param: ESC_PWM_TYPE
     // @DisplayName: Output PWM type
     // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index d68e38f7b4..03730a5ff4 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -95,6 +95,7 @@ public:
         k_param_rangefinder_port1,
         k_param_options,
         k_param_rpm_msg_rate,
+        k_param_esc_rate,
     };
 
     AP_Int16 format_version;
@@ -174,6 +175,7 @@ public:
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
+    AP_Int16 esc_rate;
     AP_Int8 esc_pwm_type;
     AP_Int16 esc_command_timeout_ms;
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
index 6f8459db37..4b9f5e97d6 100644
--- a/Tools/AP_Periph/rc_out.cpp
+++ b/Tools/AP_Periph/rc_out.cpp
@@ -67,6 +67,9 @@ void AP_Periph_FW::rcout_init()
     hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
 #endif
 
+    // run PWM ESCs at configured rate
+    hal.rcout->set_freq(esc_mask, g.esc_rate.get());
+
     // setup ESCs with the desired PWM type, allowing for DShot
     SRV_Channels::init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get());