From a3e79c8263b2d3c930485b0709dfa71651d514ec Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 23 May 2022 11:42:48 +0100
Subject: [PATCH] AP_HAL_ChibiOS: add BLHeli_S ESC type and use it to control
 bitwidths

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp        | 22 ++++++++++++++++++++
 libraries/AP_HAL_ChibiOS/RCOutput.h          |  2 +-
 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp |  1 +
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index c3f0071502..47b8abdc93 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -467,6 +467,28 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
     _dshot_period_us = 1000000UL / drate;
 }
 
+#ifndef DISABLE_DSHOT
+/*
+ Set/get the dshot esc_type
+ */
+void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
+{
+    _dshot_esc_type = dshot_esc_type;
+    switch (_dshot_esc_type) {
+        case DSHOT_ESC_BLHELI_S:
+            DSHOT_BIT_WIDTH_TICKS = DSHOT_BIT_WIDTH_TICKS_S;
+            DSHOT_BIT_0_TICKS = DSHOT_BIT_0_TICKS_S;
+            DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_S;
+            break;
+        default:
+            DSHOT_BIT_WIDTH_TICKS = DSHOT_BIT_WIDTH_TICKS_DEFAULT;
+            DSHOT_BIT_0_TICKS = DSHOT_BIT_0_TICKS_DEFAULT;
+            DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_DEFAULT;
+            break;
+    }
+}
+#endif
+
 /*
   find pwm_group and index in group given a channel number
  */
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index d99f460f41..aa3929202b 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -171,7 +171,7 @@ public:
     /*
       Set/get the dshot esc_type
      */
-    void set_dshot_esc_type(DshotEscType dshot_esc_type) override { _dshot_esc_type = dshot_esc_type; }
+    void set_dshot_esc_type(DshotEscType dshot_esc_type) override;
 
     DshotEscType get_dshot_esc_type() const override { return _dshot_esc_type; }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
index 9183a4ca2f..4945f8759c 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -146,6 +146,7 @@ void RCOutput::update_channel_masks() {
     for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
         switch (_dshot_esc_type) {
             case DSHOT_ESC_BLHELI:
+            case DSHOT_ESC_BLHELI_S:
                 if (_reversible_mask & (1U<<i)) {
                     send_dshot_command(DSHOT_3D_ON, i + chan_offset, 0, 10, true);
                 }