fix camera_trigger publish and reduce time in capture interrupt routine

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 :

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<CameraCapture *>(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) {

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 <drivers/drv_hrt.h>
 #include <drivers/drv_input_capture.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/camera_trigger.h>
@@ -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);
 

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);
 	}
 }