From 12d5fbbc86ce6c0070a7a23e8bb8be804470e198 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 20 Sep 2021 14:43:32 +0100
Subject: [PATCH] AP_IOMCU: iofirmware: support digital write

---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 28 ++++++++++++++++++++
 libraries/AP_IOMCU/iofirmware/iofirmware.h   |  5 ++++
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   |  6 +++++
 3 files changed, 39 insertions(+)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 5c74394275..4bf32f98c8 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -257,6 +257,7 @@ void AP_IOMCU_FW::update()
         if (dsm_bind_state) {
             dsm_bind_step();
         }
+        GPIO_write();
     }
 }
 
@@ -616,6 +617,24 @@ bool AP_IOMCU_FW::handle_code_write()
         break;
     }
 
+    case PAGE_GPIO:
+        if (rx_io_packet.count != 1) {
+            return false;
+        }
+        memcpy(&GPIO, &rx_io_packet.regs[0] + rx_io_packet.offset, sizeof(GPIO));
+        if (GPIO.channel_mask != last_GPIO_channel_mask) {
+            for (uint8_t i=0; i<8; i++) {
+                if ((GPIO.channel_mask & (1U << i)) != 0) {
+                    hal.rcout->disable_ch(i);
+                    hal.gpio->pinMode(101+i, HAL_GPIO_OUTPUT);
+                } else {
+                    hal.rcout->enable_ch(i);
+                }
+            }
+            last_GPIO_channel_mask = GPIO.channel_mask;
+        }
+        break;
+
     default:
         break;
     }
@@ -749,4 +768,13 @@ void AP_IOMCU_FW::fill_failsafe_pwm(void)
     }
 }
 
+void AP_IOMCU_FW::GPIO_write()
+{
+    for (uint8_t i=0; i<8; i++) {
+        if ((GPIO.channel_mask & (1U << i)) != 0) {
+            hal.gpio->write(101+i, (GPIO.output_mask & (1U << i)) != 0);
+        }
+    }
+}
+
 AP_HAL_MAIN();
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index a4690d1144..8aef68165d 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -105,6 +105,11 @@ public:
     // MIXER values
     struct page_mixing mixing;
 
+    // GPIO masks
+    struct page_GPIO GPIO;
+    uint8_t last_GPIO_channel_mask;
+    void GPIO_write();
+
     // true when override channel active
     bool override_active;
 
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index 5d7819fc74..2835c00fe3 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -55,6 +55,7 @@ enum iopage {
     PAGE_DIRECT_PWM = 54,
     PAGE_FAILSAFE_PWM = 55,
     PAGE_MIXING = 200,
+    PAGE_GPIO = 201,
 };
 
 // setup page registers
@@ -160,3 +161,8 @@ struct page_mixing {
 
     uint8_t pad; // pad to even size
 };
+
+struct page_GPIO {
+    uint8_t channel_mask;
+    uint8_t output_mask;
+};