AP_HAL_PX4: implement attach_interrupt

This commit is contained in:
Peter Barker 2018-07-04 19:13:00 +10:00 committed by Randy Mackay
parent 74263f8412
commit 4767666a2d
2 changed files with 159 additions and 2 deletions

View File

@ -9,6 +9,8 @@
#include <fcntl.h>
#include <unistd.h>
#include <GCS_MAVLink/GCS.h>
/* PX4 headers */
#include <drivers/drv_led.h>
#include <drivers/drv_tone_alarm.h>
@ -249,8 +251,150 @@ AP_HAL::DigitalSource* PX4GPIO::channel(uint16_t n) {
}
/* Interrupt interface: */
bool PX4GPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode)
AP_HAL::GPIO::irq_handler_fn_t PX4GPIO::pin_handlers[6] = {};
int PX4GPIO::irq_handler_gpio0(int irq, void *context)
{
irq_handler(0);
return 0;
}
int PX4GPIO::irq_handler_gpio1(int irq, void *context)
{
irq_handler(1);
return 0;
}
int PX4GPIO::irq_handler_gpio2(int irq, void *context)
{
irq_handler(2);
return 0;
}
int PX4GPIO::irq_handler_gpio3(int irq, void *context)
{
irq_handler(3);
return 0;
}
int PX4GPIO::irq_handler_gpio4(int irq, void *context)
{
irq_handler(4);
return 0;
}
int PX4GPIO::irq_handler_gpio5(int irq, void *context)
{
irq_handler(5);
return 0;
}
void PX4GPIO::irq_handler(uint8_t handler)
{
const uint32_t now = AP_HAL::micros();
AP_HAL::GPIO::irq_handler_fn_t &fn = pin_handlers[handler];
if (!fn) {
return;
}
const uint8_t pin = handler + 50;
const bool pin_state = hal.gpio->read(pin);
fn(pin, pin_state, now);
}
uint32_t PX4GPIO::pin_to_gpio(uint8_t pin)
{
#ifdef GPIO_GPIO0_INPUT
switch (pin) {
case 50:
return GPIO_GPIO0_INPUT;
case 51:
return GPIO_GPIO1_INPUT;
case 52:
return GPIO_GPIO2_INPUT;
case 53:
return GPIO_GPIO3_INPUT;
case 54:
return GPIO_GPIO4_INPUT;
case 55:
return GPIO_GPIO5_INPUT;
default:
return 0;
}
#endif // GPIO_GPIO5_INPUT
return 0;
}
bool PX4GPIO::attach_interrupt(uint8_t pin,
irq_handler_fn_t fn,
INTERRUPT_TRIGGER_TYPE mode)
{
const uint32_t gpio = pin_to_gpio(pin);
if (gpio == 0) {
// failed to map from pin to GPIO INPUT
return false;
}
if (pin < 50) {
return false;
}
const uint8_t handler_offset = pin - 50;
if (handler_offset > ARRAY_SIZE(pin_handlers)) {
return false;
}
// bounce attempt if there is already a handler:
if (fn && pin_handlers[handler_offset]) {
return false;
}
// attach a distinct interrupt handler for each different GPIO
int (*handler)(int irq, void *context) = nullptr;
switch (gpio) {
#ifdef GPIO_GPIO0_INPUT
case GPIO_GPIO0_INPUT:
handler = irq_handler_gpio0;
break;
case GPIO_GPIO1_INPUT:
handler = irq_handler_gpio1;
break;
case GPIO_GPIO2_INPUT:
handler = irq_handler_gpio2;
break;
case GPIO_GPIO3_INPUT:
handler = irq_handler_gpio3;
break;
case GPIO_GPIO4_INPUT:
handler = irq_handler_gpio4;
break;
case GPIO_GPIO5_INPUT:
handler = irq_handler_gpio5;
break;
#endif // GPIO_GPIO5_INPUT
default:
break;
}
if (handler == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "No handler for %u", gpio);
return false;
}
if (fn) {
stm32_gpiosetevent(gpio,
(mode == INTERRUPT_RISING)||(mode==INTERRUPT_BOTH),
(mode == INTERRUPT_FALLING)||(mode==INTERRUPT_BOTH),
false, // event
handler);
} else {
// removing existing handler
stm32_gpiosetevent(gpio,
false, // rising
false, // falling
false, // event
nullptr);
}
pin_handlers[handler_offset] = fn;
return true;
}

View File

@ -45,7 +45,9 @@ public:
AP_HAL::DigitalSource* channel(uint16_t n) override;
/* Interrupt interface: */
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override;
bool attach_interrupt(uint8_t interrupt_num,
irq_handler_fn_t fn,
INTERRUPT_TRIGGER_TYPE mode) override;
/* return true if USB cable is connected */
bool usb_connected(void) override;
@ -58,6 +60,17 @@ private:
int _gpio_fmu_fd = -1;
int _gpio_io_fd = -1;
bool _usb_connected = false;
static int irq_handler_gpio0(int irq, void *context);
static int irq_handler_gpio1(int irq, void *context);
static int irq_handler_gpio2(int irq, void *context);
static int irq_handler_gpio3(int irq, void *context);
static int irq_handler_gpio4(int irq, void *context);
static int irq_handler_gpio5(int irq, void *context);
static void irq_handler(uint8_t pin);
static uint32_t pin_to_gpio(uint8_t pin);
static AP_HAL::GPIO::irq_handler_fn_t pin_handlers[];
};
class PX4::PX4DigitalSource : public AP_HAL::DigitalSource {