From 8a0093eae842b168e4e98ee9d31c97876ced4cbe Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 19 Apr 2022 18:16:25 +0100
Subject: [PATCH] AP_HAL_ChibiOS: determine presence of disabled channels based
 on digital channels in a group

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 27 +++++++++++++++++++++++++++
 libraries/AP_HAL_ChibiOS/RCOutput.h   |  5 +++++
 2 files changed, 32 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 45f0c62cf5..3177ba126a 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -505,6 +505,33 @@ RCOutput::pwm_group *RCOutput::find_chan(uint8_t chan, uint8_t &group_idx)
     return nullptr;
 }
 
+/*
+ * return mask of channels that must be disabled because they share a group with a digital channel
+ */
+uint16_t RCOutput::get_disabled_channels(uint16_t digital_mask)
+{
+    uint16_t dmask = (digital_mask >> chan_offset);
+    uint16_t disabled_chan_mask = 0;
+    for (auto &group : pwm_group_list) {
+        bool digital_group = false;
+        for (uint8_t j = 0; j < 4; j++) {
+            if ((1U << group.chan[j]) & dmask) {
+                digital_group = true;
+            }
+        }
+        if (digital_group) {
+            for (uint8_t j = 0; j < 4; j++) {
+                if (!((1U << group.chan[j]) & dmask)) {
+                    disabled_chan_mask |= (1U << group.chan[j]);
+                }
+            }
+        }
+    }
+
+    disabled_chan_mask <<= chan_offset;
+    return disabled_chan_mask;
+}
+
 uint16_t RCOutput::get_freq(uint8_t chan)
 {
 #if HAL_WITH_IO_MCU
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index a899b35bca..d99f460f41 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -70,6 +70,11 @@ public:
     void set_output_mode(uint16_t mask, const enum output_mode mode) override;
     bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override;
 
+    /*
+     * return mask of channels that must be disabled because they share a group with a digital channel
+     */
+    uint16_t get_disabled_channels(uint16_t digital_mask) override;
+
     float scale_esc_to_unity(uint16_t pwm) override {
         return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0;
     }