From 011a7f26f4a93d915cddc868b3de601677488c23 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Tue, 4 Sep 2018 14:17:29 +0200 Subject: [PATCH] fix camera_trigger publish and reduce time in capture interrupt routine --- src/drivers/camera_capture/CMakeLists.txt | 5 +- src/drivers/camera_capture/camera_capture.cpp | 81 +++++++++++++++---- src/drivers/camera_capture/camera_capture.hpp | 20 ++++- src/drivers/stm32/drv_input_capture.c | 2 +- 4 files changed, 86 insertions(+), 22 deletions(-) diff --git a/src/drivers/camera_capture/CMakeLists.txt b/src/drivers/camera_capture/CMakeLists.txt index c07905b6e22..44016de5860 100644 --- a/src/drivers/camera_capture/CMakeLists.txt +++ b/src/drivers/camera_capture/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2018 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -36,8 +36,5 @@ px4_add_module( COMPILE_FLAGS SRCS camera_capture.cpp - DEPENDS - platforms__common - EXTERNAL ) # 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 bf2f5756593..7a9e0edd8af 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -53,6 +53,7 @@ CameraCapture::CameraCapture() : _trigger_pub(nullptr), _command_ack_pub(nullptr), _command_sub(-1), + _trig_buffer(nullptr), _camera_capture_feedback(false), _camera_capture_mode(0), _camera_capture_edge(0), @@ -77,14 +78,15 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - if (_camera_capture_feedback) { - struct camera_trigger_s trigger = {}; - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); - } } CameraCapture::~CameraCapture() { + /* free any existing reports */ + if (_trig_buffer != nullptr) { + delete _trig_buffer; + } + camera_capture::g_camera_capture = nullptr; } @@ -93,31 +95,66 @@ CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { + struct _trig_s trigger; + + trigger.chan_index = chan_index; + trigger.edge_time = edge_time; + trigger.edge_state = edge_state; + trigger.overflow = overflow; + + /* post message to the ring */ + _trig_buffer->put(&trigger); + + work_queue(LPWORK, &_work, (worker_t)&CameraCapture::publish_trigger_trampoline, this, 0); +} + +void +CameraCapture::publish_trigger_trampoline(void *arg) +{ + CameraCapture *dev = reinterpret_cast(arg); + + dev->publish_trigger(); +} + +void +CameraCapture::publish_trigger() +{ + struct _trig_s trig; + + _trig_buffer->get(&trig); + if (_last_fall_time > 0) { + struct camera_trigger_s trigger {}; if (_camera_capture_mode == 0) { - trigger.timestamp = edge_time; + trigger.timestamp = trig.edge_time; } else { - trigger.timestamp = edge_time - ((edge_time - _last_fall_time) / 2); // Get timestamp of mid-exposure + trigger.timestamp = trig.edge_time - ((trig.edge_time - _last_fall_time) / 2); // Get timestamp of mid-exposure } trigger.seq = _capture_seq++; trigger.feedback = true; if (_camera_capture_feedback) { - orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); + if (_trigger_pub == nullptr) { + + _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); + + } else { + + orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); + } } - _last_exposure_time = edge_time - _last_fall_time; + _last_exposure_time = trig.edge_time - _last_fall_time; } // Timestamp and compensate for strobe delay - _last_fall_time = edge_time - uint64_t(1000 * _strobe_delay); - - _capture_overflows = overflow; + _last_fall_time = trig.edge_time - uint64_t(1000 * _strobe_delay); + _capture_overflows = trig.overflow; } void @@ -231,12 +268,21 @@ CameraCapture::reset_statistics(bool reset_seq) _capture_overflows = 0; } -void +int CameraCapture::start() { + /* allocate basic report buffers */ + _trig_buffer = new ringbuffer::RingBuffer(2, sizeof(_trig_s)); + + if (_trig_buffer == nullptr) { + return PX4_ERROR; + } + // start to monitor at low rates for capture control commands work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this, USEC2TICK(1)); // TODO : is this low rate??! + + return PX4_OK; } void @@ -288,8 +334,13 @@ int camera_capture_main(int argc, char *argv[]) return 1; } - camera_capture::g_camera_capture->start(); - return 0; + if (!camera_capture::g_camera_capture->start()) { + return 0; + + } else { + return 1; + } + } if (camera_capture::g_camera_capture == nullptr) { diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index ad66a5b9835..812baaf5d83 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,7 @@ #include #include +#include #include #include @@ -78,7 +79,7 @@ public: /** * Start the task. */ - void start(); + int start(); /** * Stop the task. @@ -97,6 +98,8 @@ public: void reset_statistics(bool reset_seq); + void publish_trigger(); + static struct work_s _work; @@ -111,6 +114,16 @@ private: // Subscribers int _command_sub; + // Trigger Buffer + struct _trig_s { + uint32_t chan_index; + hrt_abstime edge_time; + uint32_t edge_state; + uint32_t overflow; + }; + + ringbuffer::RingBuffer *_trig_buffer; + // Parameters param_t _p_strobe_delay; float _strobe_delay; @@ -131,6 +144,9 @@ private: void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + // Signal capture publish + static void publish_trigger_trampoline(void *arg); + // Low-rate command handling loop static void cycle_trampoline(void *arg); diff --git a/src/drivers/stm32/drv_input_capture.c b/src/drivers/stm32/drv_input_capture.c index cd748817aad..c7505087b1b 100644 --- a/src/drivers/stm32/drv_input_capture.c +++ b/src/drivers/stm32/drv_input_capture.c @@ -126,7 +126,7 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, if (channel_handlers[chan_index].callback) { channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index, channel_stats[chan_index].last_time, - channel_stats[chan_index].last_edge, channel_stats[chan_index].overflows); + channel_stats[chan_index].last_edge, overflow); } }