From f3db67b322b96a7f903941a07fddc3c48614914a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 19 Apr 2022 13:24:01 +0900
Subject: [PATCH] AP_HAL: add GPIO::pin_to_servo_channel method

---
 libraries/AP_HAL/GPIO.h           |  4 ++++
 libraries/AP_HAL_ChibiOS/GPIO.cpp | 31 +++++++++++++++++++++++++++++++
 libraries/AP_HAL_ChibiOS/GPIO.h   |  4 ++++
 3 files changed, 39 insertions(+)

diff --git a/libraries/AP_HAL/GPIO.h b/libraries/AP_HAL/GPIO.h
index 8298aaac4f..5bbd446848 100644
--- a/libraries/AP_HAL/GPIO.h
+++ b/libraries/AP_HAL/GPIO.h
@@ -55,6 +55,10 @@ public:
     virtual void    toggle(uint8_t pin) = 0;
     virtual bool    valid_pin(uint8_t pin) const { return true; }
 
+    // return servo channel associated with GPIO pin.  Returns true on success and fills in servo_ch argument
+    // servo_ch uses zero-based indexing
+    virtual bool    pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const { return false; }
+
     // allow for save and restore of pin settings
     virtual bool    get_mode(uint8_t pin, uint32_t &mode) { return false; }
     virtual void    set_mode(uint8_t pin, uint32_t mode) {}
diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index ebde6139df..c0203c7fcd 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -511,6 +511,37 @@ bool GPIO::valid_pin(uint8_t pin) const
     return gpio_by_pin_num(pin) != nullptr;
 }
 
+// return servo channel associated with GPIO pin.  Returns true on success and fills in servo_ch argument
+// servo_ch uses zero-based indexing
+bool GPIO::pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const
+{
+    uint8_t fmu_chan_offset = 0;
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled()) {
+        // check if this is one of the main pins
+        uint8_t main_servo_ch = pin;
+        if (iomcu.convert_pin_number(main_servo_ch)) {
+            servo_ch = main_servo_ch;
+            return true;
+        }
+        // with IOMCU the local (FMU) channels start at 8
+        fmu_chan_offset = 8;
+    }
+#endif
+
+    // search _gpio_tab for matching pin
+    for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
+        if (_gpio_tab[i].pin_num == pin) {
+            if (_gpio_tab[i].pwm_num == 0) {
+                return false;
+            }
+            servo_ch = _gpio_tab[i].pwm_num-1+fmu_chan_offset;
+            return true;
+        }
+    }
+    return false;
+}
+
 #if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) || defined(STM32L4)
 
 // allow for save and restore of pin settings
diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h
index 9ad48d14dc..21aa51ea82 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.h
+++ b/libraries/AP_HAL_ChibiOS/GPIO.h
@@ -82,6 +82,10 @@ public:
     // check if a pin number is valid
     bool valid_pin(uint8_t pin) const override;
 
+    // return servo channel associated with GPIO pin.  Returns true on success and fills in servo_ch argument
+    // servo_ch uses zero-based indexing
+    bool pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const override;
+
     /*
       resolve an ioline to take account of alternative configurations
      */