From 8dd58b41819b0a6609a7d5d03209d78ef4586f7c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 9 Nov 2018 21:25:58 +1100
Subject: [PATCH] HAL_ChibiOS: support reversible DShot motors

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 25 +++++++++++++++++++++----
 libraries/AP_HAL_ChibiOS/RCOutput.h   | 10 ++++++++++
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index a7a5472f29..ffc0304f29 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -917,14 +917,31 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
                 pwm = safe_pwm[chan+chan_offset];
             }
             
-            pwm = constrain_int16(pwm, _esc_pwm_min, _esc_pwm_max);
-            uint16_t value = 2000UL * uint32_t(pwm - _esc_pwm_min) / uint32_t(_esc_pwm_max - _esc_pwm_min);
-            //uint32_t value = (chan+1) * 3;
+            const uint16_t chan_mask = (1U<<chan);
+            if (pwm == 0) {
+                // no output
+                continue;
+            }
+
+            pwm = constrain_int16(pwm, 1000, 2000);
+            uint16_t value = 2 * (pwm - 1000);
+
+            if (chan_mask & (reversible_mask>>chan_offset)) {
+                // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed
+                if (value < 1000) {
+                    value = 2000 - value;
+                } else if (value > 1000) {
+                    value = value - 1000;
+                } else {
+                    // mid-throttle is off
+                    value = 0;
+                }
+            }
             if (value != 0) {
                 // dshot values are from 48 to 2047. Zero means off.
                 value += 47;
             }
-            uint16_t chan_mask = (1U<<chan);
+
             bool request_telemetry = (telem_request_mask & chan_mask)?true:false;
             uint16_t packet = create_dshot_packet(value, request_telemetry);
             if (request_telemetry) {
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 5958ad1d23..2a48db5c3e 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -146,6 +146,15 @@ public:
      */
     void set_safety_mask(uint16_t mask) { safety_mask = mask; }
 
+    /*
+     * mark the channels in chanmask as reversible. This is needed for some ESC types (such as DShot)
+     * so that output scaling can be performed correctly. The chanmask passed is added (ORed) into
+     * any existing mask.
+     */
+    void set_reversible_mask(uint16_t chanmask) override {
+        reversible_mask |= chanmask;
+    }
+    
 private:
     struct pwm_group {
         // only advanced timers can do high clocks needed for more than 400Hz
@@ -252,6 +261,7 @@ private:
     // mask of channels that are running in high speed
     uint16_t fast_channel_mask;
     uint16_t io_fast_channel_mask;
+    uint16_t reversible_mask;
 
     // min time to trigger next pulse to prevent overlap
     uint64_t min_pulse_trigger_us;