AP_IOMCU: iofirmware: support digital write

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();

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;
 

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;
+};