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

---
 libraries/AP_IOMCU/AP_IOMCU.cpp | 69 ++++++++++++++++++++++++++++++++-
 libraries/AP_IOMCU/AP_IOMCU.h   | 17 ++++++++
 2 files changed, 85 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index c7cc7cd045..865f4c0646 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -34,7 +34,8 @@ enum ioevents {
     IOEVENT_SET_HEATER_TARGET,
     IOEVENT_SET_DEFAULT_RATE,
     IOEVENT_SET_SAFETY_MASK,
-    IOEVENT_MIXING
+    IOEVENT_MIXING,
+    IOEVENT_GPIO,
 };
 
 // max number of consecutve protocol failures we accept before raising
@@ -218,6 +219,16 @@ void AP_IOMCU::thread_main(void)
         }
         mask &= ~EVENT_MASK(IOEVENT_SET_SAFETY_MASK);
 
+        if (is_chibios_backend) {
+            if (mask & EVENT_MASK(IOEVENT_GPIO)) {
+                if (!write_registers(PAGE_GPIO, 0, sizeof(GPIO)/sizeof(uint16_t), (const uint16_t*)&GPIO)) {
+                    event_failed(mask);
+                    continue;
+                }
+            }
+            mask &= ~EVENT_MASK(IOEVENT_GPIO);
+        }
+
         // check for regular timed events
         uint32_t now = AP_HAL::millis();
         if (now - last_rc_read_ms > 20) {
@@ -1052,6 +1063,62 @@ void AP_IOMCU::check_iomcu_reset(void)
     last_rc_protocols = 0;
 }
 
+// Check if pin number is valid for GPIO
+bool AP_IOMCU::valid_GPIO_pin(uint8_t pin) const
+{
+    return convert_pin_number(pin);
+}
+
+// convert external pin numbers 101 to 108 to internal 0 to 7
+bool AP_IOMCU::convert_pin_number(uint8_t& pin) const
+{
+    if (pin < 101) {
+        return false;
+    }
+    pin -= 101;
+    if (pin > 7) {
+        return false;
+    }
+    return true;
+}
+
+// set GPIO mask of channels setup for output
+void AP_IOMCU::set_GPIO_mask(uint8_t mask)
+{
+    if (mask == GPIO.channel_mask) {
+        return;
+    }
+    GPIO.channel_mask = mask;
+    trigger_event(IOEVENT_GPIO);
+}
+
+// write to a output pin
+void AP_IOMCU::write_GPIO(uint8_t pin, bool value)
+{
+    if (!convert_pin_number(pin)) {
+        return;
+    }
+    if (value == ((GPIO.output_mask & (1U << pin)) != 0)) {
+        return;
+    }
+    if (value) {
+        GPIO.output_mask |= (1U << pin);
+    } else {
+        GPIO.output_mask &= ~(1U << pin);
+    }
+    trigger_event(IOEVENT_GPIO);
+}
+
+// toggle a output pin
+void AP_IOMCU::toggle_GPIO(uint8_t pin)
+{
+    if (!convert_pin_number(pin)) {
+        return;
+    }
+    GPIO.output_mask ^= (1U << pin);
+    trigger_event(IOEVENT_GPIO);
+}
+
 
 namespace AP {
     AP_IOMCU *iomcu(void) {
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index f243959129..4f89351a93 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -105,6 +105,21 @@ public:
     bool setup_mixing(RCMapper *rcmap, int8_t override_chan,
                       float mixing_gain, uint16_t manual_rc_mask);
 
+    // Check if pin number is valid for GPIO
+    bool valid_GPIO_pin(uint8_t pin) const;
+
+    // convert external pin numbers 101 to 108 to internal 0 to 7
+    bool convert_pin_number(uint8_t& pin) const;
+
+    // set GPIO mask of channels setup for output
+    void set_GPIO_mask(uint8_t mask);
+
+    // write to a output pin
+    void write_GPIO(uint8_t pin, bool value);
+
+    // toggle a output pin
+    void toggle_GPIO(uint8_t pin);
+
     // channel group masks
     const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };
 
@@ -202,6 +217,8 @@ private:
         bool brushed_enabled;
     } rate;
 
+    struct page_GPIO GPIO;
+
     // IMU heater duty cycle
     uint8_t heater_duty_cycle;