From 91424c1f0347de641e5d9ae88ef7ac1184ba1a88 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 7 Dec 2021 19:09:41 +1100
Subject: [PATCH] AP_Periph: enable DShot ESC telemetry

---
 Tools/AP_Periph/Parameters.cpp | 15 +++++++++++++++
 Tools/AP_Periph/Parameters.h   |  4 ++++
 Tools/AP_Periph/rc_out.cpp     |  6 ++++++
 3 files changed, 25 insertions(+)

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index e9c6ea2918..7c11d51c36 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -29,6 +29,10 @@ extern const AP_HAL::HAL &hal;
 #define AP_PERIPH_MSP_PORT_DEFAULT 1
 #endif
 
+#ifndef AP_PERIPH_ESC_TELEM_PORT_DEFAULT
+#define AP_PERIPH_ESC_TELEM_PORT_DEFAULT -1
+#endif
+
 #ifndef HAL_DEFAULT_MAV_SYSTEM_ID
 #define MAV_SYSTEM_ID 3
 #else
@@ -302,6 +306,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @User: Advanced
     // @RebootRequired: True
     GSCALAR(esc_pwm_type, "ESC_PWM_TYPE",     0),
+
+#if HAL_WITH_ESC_TELEM
+    // @Param: ESC_TELEM_PORT
+    // @DisplayName: ESC Telemetry Serial Port
+    // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to ESC Telemetry
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GSCALAR(esc_telem_port, "ESC_TELEM_PORT", AP_PERIPH_ESC_TELEM_PORT_DEFAULT),
+#endif
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_MSP
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 690c312b5e..a5b9937ac0 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -50,6 +50,7 @@ public:
         k_param_serial_manager,
         k_param_gps_mb_only_can_port,
         k_param_scripting,
+        k_param_esc_telem_port,
     };
 
     AP_Int16 format_version;
@@ -105,6 +106,9 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
     AP_Int8 esc_pwm_type;
+#if HAL_WITH_ESC_TELEM
+    AP_Int8 esc_telem_port;
+#endif
 #endif
 
     AP_Int8 debug;
diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
index c001f97b73..46f42ac246 100644
--- a/Tools/AP_Periph/rc_out.cpp
+++ b/Tools/AP_Periph/rc_out.cpp
@@ -31,6 +31,12 @@ void AP_Periph_FW::rcout_init()
     // start up with safety enabled. This disables the pwm output until we receive an packet from the rempte system
     hal.rcout->force_safety_on();
 
+    if (g.esc_telem_port >= 0) {
+        serial_manager.set_protocol_and_baud(g.esc_telem_port, AP_SerialManager::SerialProtocol_ESCTelemetry, 115200);
+    }
+
+    SRV_Channels::init();
+
 #if HAL_PWM_COUNT > 0
     for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
         servo_channels.set_default_function(i, SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i));