From 9e99f724e59f8001ba35a548432f1316e286df59 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 16 Jun 2021 10:27:23 +0100
Subject: [PATCH] AP_HAL_ChibiOS: add 1Hz update_channel_masks()

Send dshot commands in update function
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp        | 13 ------
 libraries/AP_HAL_ChibiOS/RCOutput.h          |  7 ++--
 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 42 +++++++++-----------
 3 files changed, 22 insertions(+), 40 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index d95150a32e..d3b71c6fee 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -480,18 +480,6 @@ void RCOutput::disable_ch(uint8_t chan)
     }
 }
 
-bool RCOutput::prepare_for_arming()
-{
-    // force all the ESCs to be active, in the future consider returning false
-    // if ESCs are not active that we require
-    _active_escs_mask = (en_mask << chan_offset);
-#ifdef DISABLE_DSHOT
-    return true;
-#else
-    return _dshot_command_queue.is_empty();
-#endif
-}
-
 void RCOutput::write(uint8_t chan, uint16_t period_us)
 {
     if (chan >= max_channels) {
@@ -1167,7 +1155,6 @@ void RCOutput::dshot_send_groups(uint32_t time_out_us)
         // send a dshot command
         if (!hal.util->get_soft_armed()
             && is_dshot_protocol(group.current_mode)
-            && group_escs_active(group) // only send when someone is listening
             && dshot_command_is_active(group)) {
             command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
         // actually do a dshot send
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index 3fa3d1705e..173667e2e2 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -215,12 +215,11 @@ public:
      */
     void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
 
-#endif
-
     /*
-      If not already done flush any dshot commands still pending
+     * Update channel masks at 1Hz allowing for actions such as dshot commands to be sent
      */
-    bool prepare_for_arming() override;
+    void update_channel_masks() override;
+#endif
 
     /*
       setup serial LED output for a given channel number, with
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
index ead7f5a265..6e3ed3a099 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -85,10 +85,6 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
 // chan is the servo channel to send the command to
 void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
 {
-    if (!_active_escs_mask && !priority) {
-        return;
-    }
-
     DshotCommandPacket pkt;
     pkt.command = command;
     pkt.chan = chan;
@@ -108,34 +104,34 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
 // The chanmask passed is added (ORed) into any existing mask.
 void RCOutput::set_reversed_mask(uint16_t chanmask) {
     _reversed_mask |= chanmask;
-
-    for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
-        if (chanmask & (1U<<i)) {
-            switch (_dshot_esc_type) {
-                case DSHOT_ESC_BLHELI:
-                    send_dshot_command(DSHOT_REVERSE, i, 0, 10, true);
-                    break;
-                default:
-                    break;
-            }
-        }
-    }
 }
 
 // Set the dshot outputs that should be reversible/3D
 // The chanmask passed is added (ORed) into any existing mask.
 void RCOutput::set_reversible_mask(uint16_t chanmask) {
     _reversible_mask |= chanmask;
+}
+
+// Update the dshot outputs that should be reversible/3D at 1Hz
+void RCOutput::update_channel_masks() {
+
+    // post arming dshot commands will not be accepted
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
 
     for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
-        if (chanmask & (1U<<i)) {
-            switch (_dshot_esc_type) {
-                case DSHOT_ESC_BLHELI:
+        switch (_dshot_esc_type) {
+            case DSHOT_ESC_BLHELI:
+                if (_reversible_mask & (1U<<i)) {
                     send_dshot_command(DSHOT_3D_ON, i, 0, 10, true);
-                    break;
-                default:
-                    break;
-            }
+                }
+                if (_reversed_mask & (1U<<i)) {
+                    send_dshot_command(DSHOT_REVERSE, i, 0, 10, true);
+                }
+                break;
+            default:
+                break;
         }
     }
 }