From fc51829b63dabecedcd08e86fee994a96bc569e0 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 13 Aug 2024 13:55:57 +0100
Subject: [PATCH] AP_HAL_ChibiOS: add iomcu support for reversible mask

check armed state on iomcu before sending dshot packets
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp        | 7 +++----
 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 6 ++++++
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index e87a415190..ed5d746c78 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1637,9 +1637,8 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
     bdshot_prepare_for_next_pulse(group);
 #endif
     bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
-#if !defined(IOMCU_FW)
     bool armed = hal.util->get_soft_armed();
-#endif
+
     memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
 
     for (uint8_t i=0; i<4; i++) {
@@ -1683,12 +1682,12 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
             if (value != 0) {
                 value += DSHOT_ZERO_THROTTLE;
             }
-#if !defined(IOMCU_FW)
+
             if (!armed) {
                 // when disarmed we always send a zero value
                 value = 0;
             }
-#endif
+
             // according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
             bool request_telemetry = telem_request_mask & chan_mask;
             uint16_t packet = create_dshot_packet(value, request_telemetry,
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
index dc6824256a..db7acbfd46 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -140,6 +140,12 @@ void RCOutput::set_reversed_mask(uint32_t chanmask) {
 // The mask uses servo channel numbering
 void RCOutput::set_reversible_mask(uint32_t chanmask) {
     _reversible_mask |= chanmask;
+#if HAL_WITH_IO_MCU
+    const uint32_t iomcu_mask = ((1U<<chan_offset)-1);
+    if (iomcu_dshot && (chanmask & iomcu_mask)) {
+        iomcu.set_reversible_mask(chanmask & iomcu_mask);
+    }
+#endif
 }
 
 // Update the dshot outputs that should be reversible/3D at 1Hz