From 48cac0db34e5ef97e561178f128407414804857e Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Tue, 13 Aug 2024 13:59:08 +0100
Subject: [PATCH] AP_IOMCU: add iomcu support for reversible mask

---
 libraries/AP_IOMCU/AP_IOMCU.cpp              | 7 +++++++
 libraries/AP_IOMCU/AP_IOMCU.h                | 3 +++
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 6 +++++-
 libraries/AP_IOMCU/iofirmware/iofirmware.h   | 1 +
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   | 1 +
 5 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index a228c977ae077e..49a231b52aad4e 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -990,6 +990,13 @@ void AP_IOMCU::set_bidir_dshot_mask(uint16_t mask)
     trigger_event(IOEVENT_SET_OUTPUT_MODE);
 }
 
+// set reversible mask
+void AP_IOMCU::set_reversible_mask(uint16_t mask)
+{
+    mode_out.reversible_mask = mask;
+    trigger_event(IOEVENT_SET_OUTPUT_MODE);
+}
+
 AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const
 {
     mask = reg_status.rcout_mask;
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 5092bf26fa0ddf..107b2248ed70df 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -110,6 +110,9 @@ class AP_IOMCU
     // set bi-directional mask
     void set_bidir_dshot_mask(uint16_t mask);
 
+    // set reversible mask
+    void set_reversible_mask(uint16_t mask);
+
     // get output mode
     AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const;
 
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index f76b1e8ce91353..0926f743ee5e08 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -920,6 +920,7 @@ bool AP_IOMCU_FW::handle_code_write()
             mode_out.mode = rx_io_packet.regs[1];
             mode_out.bdmask = rx_io_packet.regs[2];
             mode_out.esc_type = rx_io_packet.regs[3];
+            mode_out.reversible_mask = rx_io_packet.regs[4];
             break;
 
         case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
@@ -1175,6 +1176,7 @@ void AP_IOMCU_FW::rcout_config_update(void)
     // see if there is anything to do, we only support setting the mode for a particular channel once
     if ((last_output_mode_mask & mode_out.mask) == mode_out.mask
         && (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask
+        && (last_output_reversible_mask & mode_out.reversible_mask) == mode_out.reversible_mask
         && last_output_esc_type == mode_out.esc_type) {
         return;
     }
@@ -1187,13 +1189,15 @@ void AP_IOMCU_FW::rcout_config_update(void)
 #endif
 #ifdef HAL_WITH_BIDIR_DSHOT
         hal.rcout->set_bidir_dshot_mask(mode_out.bdmask);
-        hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
 #endif
+        hal.rcout->set_reversible_mask(mode_out.reversible_mask);
+        hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
         hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
         // enabling dshot changes the memory allocation
         reg_status.freemem = hal.util->available_memory();
         last_output_mode_mask |= mode_out.mask;
         last_output_bdmask |= mode_out.bdmask;
+        last_output_reversible_mask |= mode_out.reversible_mask;
         last_output_esc_type = mode_out.esc_type;
         break;
     case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 6069b3cf2add1b..51dbe418bfd03d 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -120,6 +120,7 @@ class AP_IOMCU_FW {
     uint16_t last_output_mode_mask;
     uint16_t last_output_bdmask;
     uint16_t last_output_esc_type;
+    uint16_t last_output_reversible_mask;
 
     // MIXER values
     struct page_mixing mixing;
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index c6a804ad45c3c2..f13580ec47444b 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -200,6 +200,7 @@ struct page_mode_out {
     uint16_t mode;
     uint16_t bdmask;
     uint16_t esc_type;
+    uint16_t reversible_mask;
 };
 
 struct page_dshot {