From 4d8f523172818ec1e8909cbd41695fd5f5c0cb86 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 28 Jul 2024 17:32:50 +0100 Subject: [PATCH 1/4] AP_IOMCU: add GPIO mask getter and GPIO virtual read --- libraries/AP_IOMCU/AP_IOMCU.cpp | 17 +++++++++++++++++ libraries/AP_IOMCU/AP_IOMCU.h | 8 ++++++++ 2 files changed, 25 insertions(+) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 3507db08fad70..a228c977ae077 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -1369,6 +1369,12 @@ void AP_IOMCU::set_GPIO_mask(uint8_t mask) trigger_event(IOEVENT_GPIO); } +// Get GPIO mask of channels setup for output +uint8_t AP_IOMCU::get_GPIO_mask() const +{ + return GPIO.channel_mask; +} + // write to a output pin void AP_IOMCU::write_GPIO(uint8_t pin, bool value) { @@ -1386,6 +1392,17 @@ void AP_IOMCU::write_GPIO(uint8_t pin, bool value) trigger_event(IOEVENT_GPIO); } +// Read the last output value send to the GPIO pin +// This is not a real read of the actual pin +// This allows callers to check for state change +uint8_t AP_IOMCU::read_virtual_GPIO(uint8_t pin) const +{ + if (!convert_pin_number(pin)) { + return 0; + } + return (GPIO.output_mask & (1U << pin)) != 0; +} + // toggle a output pin void AP_IOMCU::toggle_GPIO(uint8_t pin) { diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 5199c5fb3dbdb..5092bf26fa0dd 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -157,9 +157,17 @@ class AP_IOMCU // set GPIO mask of channels setup for output void set_GPIO_mask(uint8_t mask); + // Get GPIO mask of channels setup for output + uint8_t get_GPIO_mask() const; + // write to a output pin void write_GPIO(uint8_t pin, bool value); + // Read the last output value send to the GPIO pin + // This is not a real read of the actual pin + // This allows callers to check for state change + uint8_t read_virtual_GPIO(uint8_t pin) const; + // toggle a output pin void toggle_GPIO(uint8_t pin); From b0eacc40adb924d5229d7157a093352ef4b9afea Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 28 Jul 2024 17:33:51 +0100 Subject: [PATCH 2/4] AP_HAL_ChibiOS: RCOut banner: check for GPIO on IOMCU --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 0fcc96e04c4c0..da2b34fa3621b 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1263,9 +1263,13 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) if (iomcu_enabled) { uint8_t iomcu_mask; const output_mode iomcu_mode = iomcu.get_output_mode(iomcu_mask); + const uint8_t gpio_mask = iomcu.get_GPIO_mask(); for (uint8_t i = 0; i < chan_offset; i++ ) { - if (iomcu_mask & 1U< Date: Sun, 28 Jul 2024 17:37:47 +0100 Subject: [PATCH 3/4] AP_HAL_ChibiOS: GPIO: Check IOMCU after local pins for speed --- libraries/AP_HAL_ChibiOS/GPIO.cpp | 37 +++++++++++++++++-------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 15812c9b33fbd..a210420951b25 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -246,12 +246,6 @@ uint8_t GPIO::read(uint8_t pin) void GPIO::write(uint8_t pin, uint8_t value) { -#if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { - iomcu.write_GPIO(pin, value); - return; - } -#endif struct gpio_entry *g = gpio_by_pin_num(pin); if (g) { if (g->is_input) { @@ -263,36 +257,42 @@ void GPIO::write(uint8_t pin, uint8_t value) } else { palSetLine(g->pal_line); } + return; } +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { + iomcu.write_GPIO(pin, value); + } +#endif } void GPIO::toggle(uint8_t pin) { + struct gpio_entry *g = gpio_by_pin_num(pin); + if (g) { + palToggleLine(g->pal_line); + return; + } #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { iomcu.toggle_GPIO(pin); - return; } #endif - struct gpio_entry *g = gpio_by_pin_num(pin); - if (g) { - palToggleLine(g->pal_line); - } } /* Alternative interface: */ AP_HAL::DigitalSource* GPIO::channel(uint16_t pin) { + struct gpio_entry *g = gpio_by_pin_num(pin); + if (g != nullptr) { + return NEW_NOTHROW DigitalSource(g->pal_line); + } #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { return NEW_NOTHROW IOMCU_DigitalSource(pin); } #endif - struct gpio_entry *g = gpio_by_pin_num(pin); - if (!g) { - return nullptr; - } - return NEW_NOTHROW DigitalSource(g->pal_line); + return nullptr; } extern const AP_HAL::HAL& hal; @@ -526,12 +526,15 @@ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_u // check if a pin number is valid bool GPIO::valid_pin(uint8_t pin) const { + if (gpio_by_pin_num(pin) != nullptr) { + return true; + } #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { return true; } #endif - return gpio_by_pin_num(pin) != nullptr; + return false; } // return servo channel associated with GPIO pin. Returns true on success and fills in servo_ch argument From 8683eae46b3c80ee1bc31bbc298f46c0908f435c Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 28 Jul 2024 17:38:26 +0100 Subject: [PATCH 4/4] AP_HAL_ChibiOS: GPIO: read: support virtual read of IOMCU pins --- libraries/AP_HAL_ChibiOS/GPIO.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index a210420951b25..d2218a21ab2a8 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -241,6 +241,11 @@ uint8_t GPIO::read(uint8_t pin) if (g) { return palReadLine(g->pal_line); } +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { + return iomcu.read_virtual_GPIO(pin); + } +#endif return 0; }