From ade01f41d987fdd00e1891fe7d34f0531fdd4d38 Mon Sep 17 00:00:00 2001 From: Iampete1 <iampete@hotmail.co.uk> Date: Mon, 20 Sep 2021 14:44:46 +0100 Subject: [PATCH] AP_HAL_ChibiOS: support digital write via IOMCU --- .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h | 3 ++ libraries/AP_HAL_ChibiOS/GPIO.cpp | 51 +++++++++++++++++++ libraries/AP_HAL_ChibiOS/GPIO.h | 14 +++++ 3 files changed, 68 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index 4579705520..df07446561 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -4,6 +4,9 @@ namespace ChibiOS { class AnalogIn; class AnalogSource; class DigitalSource; +#if HAL_WITH_IO_MCU + class IOMCU_DigitalSource; +#endif class DSP; class GPIO; class I2CBus; diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index a5cbf1c0c5..bcf2d1d017 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -29,6 +29,12 @@ using namespace ChibiOS; +#if HAL_WITH_IO_MCU +#include <AP_IOMCU/AP_IOMCU.h> +extern AP_IOMCU iomcu; +#endif + + // GPIO pin table from hwdef.dat static struct gpio_entry { uint8_t pin_num; @@ -73,6 +79,13 @@ void GPIO::init() uint8_t chan_offset = 0; #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { + uint8_t GPIO_mask = 0; + for (uint8_t i=0; i<8; i++) { + if (SRV_Channels::is_GPIO(i)) { + GPIO_mask |= 1U << i; + } + } + iomcu.set_GPIO_mask(GPIO_mask); chan_offset = 8; } #endif @@ -204,6 +217,12 @@ 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) { @@ -220,6 +239,12 @@ void GPIO::write(uint8_t pin, uint8_t value) void GPIO::toggle(uint8_t pin) { +#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); @@ -229,6 +254,11 @@ void GPIO::toggle(uint8_t pin) /* Alternative interface: */ AP_HAL::DigitalSource* GPIO::channel(uint16_t pin) { +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { + return new IOMCU_DigitalSource(pin); + } +#endif struct gpio_entry *g = gpio_by_pin_num(pin); if (!g) { return nullptr; @@ -352,6 +382,22 @@ void DigitalSource::toggle() palToggleLine(line); } +#if HAL_WITH_IO_MCU +IOMCU_DigitalSource::IOMCU_DigitalSource(uint8_t _pin) : + pin(_pin) +{} + +void IOMCU_DigitalSource::write(uint8_t value) +{ + iomcu.write_GPIO(pin, value); +} + +void IOMCU_DigitalSource::toggle() +{ + iomcu.toggle_GPIO(pin); +} +#endif // HAL_WITH_IO_MCU + static void pal_interrupt_cb(void *arg) { if (arg != nullptr) { @@ -443,6 +489,11 @@ 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 HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) { + return true; + } +#endif return gpio_by_pin_num(pin) != nullptr; } diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h index 1b9949ee8a..29f3596e47 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.h +++ b/libraries/AP_HAL_ChibiOS/GPIO.h @@ -108,3 +108,17 @@ public: private: ioline_t line; }; + +#if HAL_WITH_IO_MCU +class ChibiOS::IOMCU_DigitalSource : public AP_HAL::DigitalSource { +public: + IOMCU_DigitalSource(uint8_t _pin); + void write(uint8_t value) override; + void toggle() override; + // IOMCU GPIO is write only + void mode(uint8_t output) override {}; + uint8_t read() override { return 0; } +private: + uint8_t pin; +}; +#endif