From 41fd7c726e5b0051bee4ea911788721f9e5a64d8 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 27 Apr 2024 08:44:31 +0100
Subject: [PATCH] AP_IOMCU: allow up to 16 channels of servo data to be sent to
 the iomcu

---
 libraries/AP_IOMCU/AP_IOMCU.cpp              |  6 +++---
 libraries/AP_IOMCU/AP_IOMCU.h                |  6 +++---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp |  2 +-
 libraries/AP_IOMCU/iofirmware/ioprotocol.h   | 14 ++++++++------
 libraries/AP_IOMCU/iofirmware/mixer.cpp      |  4 ++--
 5 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 73f283532f..83ebd6e01a 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -791,7 +791,7 @@ bool AP_IOMCU::modify_register(uint8_t page, uint8_t offset, uint16_t clearbits,
 
 void AP_IOMCU::write_channel(uint8_t chan, uint16_t pwm)
 {
-    if (chan >= IOMCU_MAX_CHANNELS) {
+    if (chan >= IOMCU_MAX_RC_CHANNELS) {    // could be SBUS out
         return;
     }
     if (chan >= pwm_out.num_channels) {
@@ -1112,7 +1112,7 @@ bool AP_IOMCU::check_crc(void)
 void AP_IOMCU::set_failsafe_pwm(uint16_t chmask, uint16_t period_us)
 {
     bool changed = false;
-    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+    for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
         if (chmask & (1U<<i)) {
             if (pwm_out.failsafe_pwm[i] != period_us) {
                 pwm_out.failsafe_pwm[i] = period_us;
@@ -1191,7 +1191,7 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan,
 #define MIX_UPDATE(a,b) do { if ((a) != (b)) { a = b; changed = true; }} while (0)
 
     // update mixing structure, checking for changes
-    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+    for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
         const SRV_Channel *c = SRV_Channels::srv_channel(i);
         if (!c) {
             continue;
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 253b1b9890..5199c5fb3d 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -243,9 +243,9 @@ private:
     // output pwm values
     struct {
         uint8_t num_channels;
-        uint16_t pwm[IOMCU_MAX_CHANNELS];
+        uint16_t pwm[IOMCU_MAX_RC_CHANNELS];
         uint16_t safety_mask;
-        uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
+        uint16_t failsafe_pwm[IOMCU_MAX_RC_CHANNELS];
         uint8_t failsafe_pwm_set;
         uint8_t failsafe_pwm_sent;
         uint16_t channel_mask;
@@ -253,7 +253,7 @@ private:
 
     // read back pwm values
     struct {
-        uint16_t pwm[IOMCU_MAX_CHANNELS];
+        uint16_t pwm[IOMCU_MAX_RC_CHANNELS];
     } pwm_in;
 
     // output rates
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 545dfad9aa..bb24b0dcad 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -1216,7 +1216,7 @@ void AP_IOMCU_FW::rcout_config_update(void)
  */
 void AP_IOMCU_FW::fill_failsafe_pwm(void)
 {
-    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+    for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
         if (reg_status.flag_safety_off) {
             reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i];
         } else {
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index 9e13fafc48..be00d92759 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -9,7 +9,9 @@
 
 // 22 is enough for the rc_input page in one transfer
 #define PKT_MAX_REGS 22
+// The number of channels that can be propagated - due to SBUS_OUT is higher than the physical channels
 #define IOMCU_MAX_RC_CHANNELS 16
+// The actual number of output channels
 #define IOMCU_MAX_CHANNELS 8
 #define IOMCU_MAX_TELEM_CHANNELS 4
 
@@ -155,17 +157,17 @@ struct page_rc_input {
   data for mixing on FMU failsafe
  */
 struct page_mixing {
-    uint16_t servo_min[IOMCU_MAX_CHANNELS];
-    uint16_t servo_max[IOMCU_MAX_CHANNELS];
-    uint16_t servo_trim[IOMCU_MAX_CHANNELS];
-    uint8_t servo_function[IOMCU_MAX_CHANNELS];
-    uint8_t servo_reversed[IOMCU_MAX_CHANNELS];
+    uint16_t servo_min[IOMCU_MAX_RC_CHANNELS];
+    uint16_t servo_max[IOMCU_MAX_RC_CHANNELS];
+    uint16_t servo_trim[IOMCU_MAX_RC_CHANNELS];
+    uint8_t servo_function[IOMCU_MAX_RC_CHANNELS];
+    uint8_t servo_reversed[IOMCU_MAX_RC_CHANNELS];
 
     // RC input arrays are in AETR order
     uint16_t rc_min[4];
     uint16_t rc_max[4];
     uint16_t rc_trim[4];
-    uint8_t rc_reversed[IOMCU_MAX_CHANNELS];
+    uint8_t rc_reversed[IOMCU_MAX_RC_CHANNELS];
     uint8_t rc_channel[4];
 
     // gain for elevon and vtail mixing, x1000
diff --git a/libraries/AP_IOMCU/iofirmware/mixer.cpp b/libraries/AP_IOMCU/iofirmware/mixer.cpp
index 9f270704b0..58f38d4f32 100644
--- a/libraries/AP_IOMCU/iofirmware/mixer.cpp
+++ b/libraries/AP_IOMCU/iofirmware/mixer.cpp
@@ -130,7 +130,7 @@ void AP_IOMCU_FW::run_mixer(void)
     // get RC input angles
     if (rc_input.flags_rc_ok) {
         for (uint8_t i=0;i<4; i++) {
-            if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_CHANNELS) {
+            if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_RC_CHANNELS) {
                 uint8_t chan = mixing.rc_channel[i]-1;
                 if (i == 2 && !mixing.throttle_is_angle) {
                     rcin[i] = mix_input_range(i, rc_input.pwm[chan]);
@@ -141,7 +141,7 @@ void AP_IOMCU_FW::run_mixer(void)
         }
     }
 
-    for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
+    for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
         SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i];
         uint16_t &pwm = reg_direct_pwm.pwm[i];