AP_IOMCU: add support for disabled channels

ArduPilot · Sep 4, 2024 · ff2c64e · ff2c64e
1 parent 48624f1
commit ff2c64e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -1003,6 +1003,16 @@ AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const
     return AP_HAL::RCOutput::output_mode(reg_status.rcout_mode);
 }
 
+uint32_t AP_IOMCU::get_disabled_channels(uint32_t digital_mask) const
+{
+    uint32_t dig_out = reg_status.rcout_mask & (digital_mask & 0xFF);
+    if (dig_out > 0
+        && AP_HAL::RCOutput::is_dshot_protocol(AP_HAL::RCOutput::output_mode(reg_status.rcout_mode))) {
+        return ~dig_out & 0xFF;
+    }
+    return 0;
+}
+
 // setup channels
 void  AP_IOMCU::enable_ch(uint8_t ch)
 {

diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -116,6 +116,9 @@ class AP_IOMCU
     // get output mode
     AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const;
 
+    // approximation to disabled channel
+    uint32_t get_disabled_channels(uint32_t digital_mask) const;
+
     // MCUID
     uint32_t get_mcu_id() const { return config.mcuid; }