diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 528862dcd0..fa875fe20d 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -106,11 +106,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint16_t capture = _REG32(timer->base, chan->ccr_offset); channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = _REG32(timer->base, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF; diff --git a/src/drivers/camera_capture/CMakeLists.txt b/src/drivers/camera_capture/CMakeLists.txt index 44016de586..542e1b0843 100644 --- a/src/drivers/camera_capture/CMakeLists.txt +++ b/src/drivers/camera_capture/CMakeLists.txt @@ -36,5 +36,7 @@ px4_add_module( COMPILE_FLAGS SRCS camera_capture.cpp + DEPENDS + arch_io_pins ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 1fa1709213..ae4db87fe7 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -43,6 +43,8 @@ #define commandParamToInt(n) static_cast(n >= 0 ? n + 0.5f : n - 0.5f) +static constexpr int capture_channel = 5; ///< FMU output 6 + namespace camera_capture { CameraCapture *g_camera_capture{nullptr}; @@ -216,63 +218,27 @@ CameraCapture::set_capture_control(bool enabled) #else - int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); - - if (fd < 0) { - PX4_ERR("open fail"); - return; - } - - input_capture_config_t conf; - conf.channel = 5; // FMU chan 6 - conf.filter = 0; - - if (_camera_capture_mode == 0) { - conf.edge = _camera_capture_edge ? Rising : Falling; - - } else { - conf.edge = Both; - } - - conf.callback = nullptr; - conf.context = nullptr; + capture_callback_t callback = nullptr; + void *context = nullptr; if (enabled) { - - conf.callback = &CameraCapture::capture_trampoline; - conf.context = this; - - unsigned int capture_count = 0; - - if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { - PX4_INFO("Not in a capture mode"); - - unsigned long mode = PWM_SERVO_MODE_4PWM2CAP; - - if (::ioctl(fd, PWM_SERVO_SET_MODE, mode) == 0) { - PX4_INFO("Mode changed to 4PWM2CAP"); - - } else { - PX4_ERR("Mode NOT changed to 4PWM2CAP!"); - goto err_out; - } - } + callback = &CameraCapture::capture_trampoline; + context = this; } - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { + int ret = up_input_capture_set_callback(capture_channel, callback, context); + + if (ret == 0) { _capture_enabled = enabled; _gpio_capture = false; } else { - PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", conf.channel); + PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", capture_channel, ret); _capture_enabled = false; - goto err_out; } reset_statistics(false); -err_out: - ::close(fd); #endif } @@ -292,6 +258,22 @@ CameraCapture::reset_statistics(bool reset_seq) int CameraCapture::start() { +#if !defined(BOARD_CAPTURE_GPIO) + input_capture_edge edge = Both; + + if (_camera_capture_mode == 0) { + edge = _camera_capture_edge ? Rising : Falling; + } + + int ret = up_input_capture_set(capture_channel, edge, 0, nullptr, nullptr); + + if (ret != 0) { + PX4_ERR("up_input_capture_set failed (%i)", ret); + return ret; + } + +#endif + // run every 100 ms (10 Hz) ScheduleOnInterval(100000, 10000); @@ -329,6 +311,26 @@ CameraCapture::status() } PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows); + +#if !defined(BOARD_CAPTURE_GPIO) + input_capture_stats_t stats; + int ret = up_input_capture_get_stats(capture_channel, &stats, false); + + if (ret != 0) { + PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", capture_channel, ret); + + } else { + PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32 + " overflows: %" PRIu32 " latency: %" PRIu16, + capture_channel, + stats.edges, + stats.last_time, + stats.last_edge, + stats.overflows, + stats.latency); + } + +#endif } static int usage() diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 2bdb31761e..8f77fd0e43 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -54,9 +54,6 @@ #include #include -#define PX4FMU_DEVICE_PATH "/dev/px4fmu" - - class CameraCapture : public px4::ScheduledWorkItem { public: diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index 80d080310e..0aeb916020 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -56,7 +56,7 @@ CameraInterfaceGPIO::~CameraInterfaceGPIO() unsigned channel = 0; while (_allocated_channels != 0) { - if (((1 << channel) & _allocated_channels)) { + if (((1u << channel) & _allocated_channels)) { io_timer_unallocate_channel(channel); _allocated_channels &= ~(1u << channel); } diff --git a/src/drivers/drv_input_capture.h b/src/drivers/drv_input_capture.h index dc79e14612..78f069f716 100644 --- a/src/drivers/drv_input_capture.h +++ b/src/drivers/drv_input_capture.h @@ -83,11 +83,11 @@ typedef struct input_capture_element_t { } input_capture_element_t; typedef struct input_capture_stats_t { - uint32_t chan_in_edges_out; + uint32_t edges; uint32_t overflows; uint32_t last_edge; hrt_abstime last_time; - uint16_t latnecy; + uint16_t latency; } input_capture_stats_t; /** diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 33805a2dd9..a0cb7aba1d 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -1850,16 +1850,10 @@ int PWMOut::test(const char *dev) { int fd; unsigned servo_count = 0; - unsigned capture_count = 0; unsigned pwm_value = 1000; int direction = 1; int ret; int rv = -1; - uint32_t rate_limit = 0; - struct input_capture_t { - bool valid; - input_capture_config_t chan; - } capture_conf[INPUT_CAPTURE_MAX_CHANNELS]; fd = ::open(dev, O_RDWR); @@ -1883,39 +1877,7 @@ int PWMOut::test(const char *dev) goto err_out; } - if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { - PX4_INFO("Not in a capture mode"); - } - - PX4_INFO("Testing %u servos and %u input captures", servo_count, capture_count); - memset(capture_conf, 0, sizeof(capture_conf)); - - if (capture_count != 0) { - for (unsigned i = 0; i < capture_count; i++) { - // Map to channel number - capture_conf[i].chan.channel = i + servo_count; - - /* Save handler */ - if (::ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) { - PX4_ERR("Unable to get capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel); - goto err_out; - - } else { - input_capture_config_t conf = capture_conf[i].chan; - conf.callback = &PWMOut::capture_trampoline; - conf.context = _objects[0].load(); // TODO PWMOut::get_instance(); - - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { - capture_conf[i].valid = true; - - } else { - PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel); - goto err_out; - } - } - - } - } + PX4_INFO("Testing %u servos", servo_count); struct pollfd fds; @@ -1972,31 +1934,6 @@ int PWMOut::test(const char *dev) } } - if (capture_count != 0 && (++rate_limit % 500 == 0)) { - for (unsigned i = 0; i < capture_count; i++) { - if (capture_conf[i].valid) { - input_capture_stats_t stats; - stats.chan_in_edges_out = capture_conf[i].chan.channel; - - if (::ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) { - PX4_ERR("Unable to get stats for chan %" PRIu8 "\n", capture_conf[i].chan.channel); - goto err_out; - - } else { - fprintf(stdout, "FMU: Status chan:%" PRIu8 " edges: %" PRIu32 " last time:%" PRIu64 " last state:%" PRIu32 - " overflows:%" PRIu32 " lantency:%" PRIu16 "\n", - capture_conf[i].chan.channel, - stats.chan_in_edges_out, - stats.last_time, - stats.last_edge, - stats.overflows, - stats.latnecy); - } - } - } - - } - /* Check if user wants to quit */ char c; ret = ::poll(&fds, 1, 0); @@ -2012,19 +1949,6 @@ int PWMOut::test(const char *dev) } } - if (capture_count != 0) { - for (unsigned i = 0; i < capture_count; i++) { - // Map to channel number - if (capture_conf[i].valid) { - /* Save handler */ - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) { - PX4_ERR("Unable to set capture callback for chan %" PRIu8 "\n", capture_conf[i].chan.channel); - goto err_out; - } - } - } - } - rv = 0; err_out: