From 54e53ed71cf1caea6a7764d2f9bd4eb8609bbe3c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 15 Jul 2021 08:39:37 +1000
Subject: [PATCH] HAL_ChibiOS: use is_GPIO() instead of BRD_PWM_COUNT

---
 libraries/AP_HAL_ChibiOS/GPIO.cpp     | 15 +++++++++++++--
 libraries/AP_HAL_ChibiOS/RCOutput.cpp |  7 +++++--
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index 639a55dcac..7481b8f2e8 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -19,9 +19,13 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include "hwdef/common/stm32_util.h"
 #include <AP_InternalError/AP_InternalError.h>
+#ifndef HAL_BOOTLOADER_BUILD
+#include <SRV_Channel/SRV_Channel.h>
+#endif
 #ifndef HAL_NO_UARTDRIVER
 #include <GCS_MAVLink/GCS.h>
 #endif
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 
 using namespace ChibiOS;
 
@@ -65,14 +69,21 @@ GPIO::GPIO()
 
 void GPIO::init()
 {
+#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) && !defined(HAL_BOOTLOADER_BUILD)
+    uint8_t chan_offset = 0;
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled()) {
+        chan_offset = 8;
+    }
+#endif
     // auto-disable pins being used for PWM output based on BRD_PWM_COUNT parameter
-    uint8_t pwm_count = AP_BoardConfig::get_pwm_count();
     for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
         struct gpio_entry *g = &_gpio_tab[i];
         if (g->pwm_num != 0) {
-            g->enabled = g->pwm_num > pwm_count;
+            g->enabled = SRV_Channels::is_GPIO(g->pwm_num+chan_offset);
         }
     }
+#endif // HAL_BOOTLOADER_BUILD
 #ifdef HAL_PIN_ALT_CONFIG
     setup_alt_config();
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 7dade31c13..17d4572b54 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -27,7 +27,9 @@
 #ifndef HAL_NO_UARTDRIVER
 #include <GCS_MAVLink/GCS.h>
 #endif
+
 #if HAL_USE_PWM == TRUE
+#include <SRV_Channel/SRV_Channel.h>
 
 using namespace ChibiOS;
 
@@ -59,7 +61,6 @@ static const eventmask_t EVT_PWM_SEND_NEXT  = EVENT_MASK(14);
  */
 void RCOutput::init()
 {
-    uint8_t pwm_count = AP_BoardConfig::get_pwm_count();
     for (auto &group : pwm_group_list) {
         const uint8_t i = &group - pwm_group_list;
         //Start Pwm groups
@@ -67,10 +68,12 @@ void RCOutput::init()
         group.dshot_event_mask = EVENT_MASK(i);
 
         for (uint8_t j = 0; j < 4; j++ ) {
+#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
             uint8_t chan = group.chan[j];
-            if (chan >= pwm_count) {
+            if (SRV_Channels::is_GPIO(chan+chan_offset)) {
                 group.chan[j] = CHAN_DISABLED;
             }
+#endif
             if (group.chan[j] != CHAN_DISABLED) {
                 num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1);
                 group.ch_mask |= (1U<<group.chan[j]);