diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 7499fa955e2f82969552a8805d303362b502fbfe..763d56dd2030924dfec85fef2f39d9daa3441452 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -20,6 +20,7 @@ px4_add_board( barometer # all available barometer drivers batt_smbus camera_trigger + camera_capture differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 1d4d460df61c570889a73b73a741f8599b8f0f1a..074673cd68348b53b35ae2ac80e13d5c27333877 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -50,6 +50,7 @@ CameraCapture *g_camera_capture; CameraCapture::CameraCapture() : _capture_enabled(false), + _gpio_capture(false), _trigger_pub(nullptr), _command_ack_pub(nullptr), _command_sub(-1), @@ -100,6 +101,22 @@ CameraCapture::capture_callback(uint32_t chan_index, work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, this, 0); } +int +CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) +{ + CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg); + + dev->_trigger.chan_index = 0; + dev->_trigger.edge_time = hrt_absolute_time(); + dev->_trigger.edge_state = 0; + dev->_trigger.overflow = 0; + + work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0); + + return PX4_OK; + +} + void CameraCapture::publish_trigger_trampoline(void *arg) { @@ -116,7 +133,7 @@ CameraCapture::publish_trigger() struct camera_trigger_s trigger {}; // MODES 1 and 2 are not fully tested - if (_camera_capture_mode == 0) { + if (_camera_capture_mode == 0 || _gpio_capture) { trigger.timestamp = _trigger.edge_time - uint64_t(1000 * _strobe_delay); trigger.seq = _capture_seq++; _last_trig_time = trigger.timestamp; @@ -248,6 +265,8 @@ CameraCapture::cycle() void CameraCapture::set_capture_control(bool enabled) { +#if !defined CONFIG_ARCH_BOARD_AV_X_V1 + int fd = -1; fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -295,6 +314,7 @@ CameraCapture::set_capture_control(bool enabled) if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { _capture_enabled = enabled; + _gpio_capture = false; } else { PX4_ERR("Unable to set capture callback for chan %u\n", conf.channel); @@ -302,9 +322,21 @@ CameraCapture::set_capture_control(bool enabled) goto err_out; } +#else + + px4_arch_gpiosetevent(GPIO_TRIG_AVX, true, false, true, &CameraCapture::gpio_interrupt_routine, this); + _capture_enabled = enabled; + _gpio_capture = true; + +#endif + reset_statistics(false); + +#if !defined CONFIG_ARCH_BOARD_AV_X_V1 err_out: ::close(fd); +#endif + return; } diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index a21fa1f14be7795751be52dfaf9ad195f411c34c..06dff1e1030aed0fc81bdcbbdae1e0bb6dffbe5a 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -66,6 +66,10 @@ #define PX4FMU_DEVICE_PATH "/dev/px4fmu" +// For AV-X board +#define GPIO_TRIG_AVX /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14) + + class CameraCapture { public: @@ -110,6 +114,7 @@ public: private: bool _capture_enabled; + bool _gpio_capture; // Publishers orb_advert_t _trigger_pub; @@ -149,6 +154,9 @@ private: void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + // GPIO interrupt routine (for AV_X board) + static int gpio_interrupt_routine(int irq, void *context, void *arg); + // Signal capture publish static void publish_trigger_trampoline(void *arg);