Add input capture module

Copy camera capture module as input capture
F

boards/px4/fmu-v5/default.px4board

@@ -22,6 +22,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_INPUT_CAPTURE=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y

msg/CMakeLists.txt

@@ -111,6 +111,7 @@ set(msg_files
 	HeaterStatus.msg
 	HomePosition.msg
 	HoverThrustEstimate.msg
+	InputCapture.msg
 	InputRc.msg
 	InternalCombustionEngineStatus.msg
 	IridiumsbdStatus.msg

msg/InputCapture.msg

@@ -0,0 +1,5 @@
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # the timestamp of the raw data (microseconds)
+uint32 seq		# Capture sequence number
+
+uint32 ORB_QUEUE_LENGTH = 2

src/drivers/input_capture/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+set(PARAM_PREFIX PWM_MAIN)
+
+if(CONFIG_BOARD_IO)
+	set(PARAM_PREFIX PWM_AUX)
+endif()
+
+px4_add_module(
+	MODULE drivers__input_capture
+	MAIN input_capture
+	COMPILE_FLAGS
+		-DPARAM_PREFIX="${PARAM_PREFIX}"
+	SRCS
+		input_capture.cpp
+	DEPENDS
+		arch_io_pins
+)

src/drivers/input_capture/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_INPUT_CAPTURE
+	bool "input_capture"
+	default n
+	---help---
+		Enable support for input_capture

src/drivers/input_capture/input_capture.cpp

@@ -0,0 +1,476 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2021 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file input_capture.cpp
+ *
+ * Online and offline geotagging from camera feedback
+ *
+ * @author Mohammed Kabir <kabir@uasys.io>
+ * @author Jaeyoung Lim <jalim@ethz.ch>
+ */
+
+#include "input_capture.hpp"
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
+
+#include <board_config.h>
+
+#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
+
+using namespace time_literals;
+
+namespace camera_capture
+{
+InputCapture *g_input_capture{nullptr};
+}
+
+struct work_s InputCapture::_work_publisher;
+
+InputCapture::InputCapture() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	memset(&_work_publisher, 0, sizeof(_work_publisher));
+
+	// Capture Parameters
+	_p_strobe_delay = param_find("CAM_CAP_DELAY");
+	param_get(_p_strobe_delay, &_strobe_delay);
+
+	_p_camera_capture_mode = param_find("CAM_CAP_MODE");
+	param_get(_p_camera_capture_mode, &_camera_capture_mode);
+
+	_p_camera_capture_edge = param_find("CAM_CAP_EDGE");
+	param_get(_p_camera_capture_edge, &_camera_capture_edge);
+
+	// get the capture channel from function configuration params
+	_capture_channel = -1;
+
+	for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+		param_t function_handle = param_find(param_name);
+		int32_t function;
+
+		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+			if (function == 2032) { // Camera_Capture
+				_capture_channel = i;
+			}
+		}
+	}
+
+	_trigger_pub.advertise();
+}
+
+InputCapture::~InputCapture()
+{
+	camera_capture::g_input_capture = nullptr;
+}
+
+void
+InputCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+{
+	// Maximum acceptable rate is 5kHz
+	if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
+		++_trigger_rate_exceeded_counter;
+
+		if (_trigger_rate_exceeded_counter > 100) {
+
+			// Trigger rate too high, stop future interrupts
+			up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
+			_trigger_rate_failure.store(true);
+		}
+
+	} else if (_trigger_rate_exceeded_counter > 0) {
+		--_trigger_rate_exceeded_counter;
+	}
+
+	_trigger.chan_index = chan_index;
+	_trigger.hrt_edge_time = edge_time;
+	_trigger.edge_state = edge_state;
+	_trigger.overflow = overflow;
+
+	work_queue(HPWORK, &_work_publisher, (worker_t)&InputCapture::publish_trigger_trampoline, this, 0);
+}
+
+int
+InputCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
+{
+	InputCapture *dev = static_cast<InputCapture *>(arg);
+
+	dev->_trigger.chan_index = 0;
+	dev->_trigger.hrt_edge_time = hrt_absolute_time();
+	dev->_trigger.edge_state = 0;
+	dev->_trigger.overflow = 0;
+
+	work_queue(HPWORK, &_work_publisher, (worker_t)&InputCapture::publish_trigger_trampoline, dev, 0);
+
+	return PX4_OK;
+}
+
+void
+InputCapture::publish_trigger_trampoline(void *arg)
+{
+	InputCapture *dev = static_cast<InputCapture *>(arg);
+
+	dev->publish_trigger();
+}
+
+void
+InputCapture::publish_trigger()
+{
+	bool publish = false;
+
+	if (_trigger_rate_failure.load()) {
+		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Input capture disabled\t");
+		events::send(events::ID("input_capture_trigger_rate_exceeded"),
+			     events::Log::Error, "Hardware fault: Input capture disabled");
+		_trigger_rate_failure.store(false);
+	}
+
+	camera_trigger_s trigger{};
+
+	// MODES 1 and 2 are not fully tested
+	if (_camera_capture_mode == 0 || _gpio_capture) {
+		trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+		trigger.seq = _capture_seq++;
+		_last_trig_time = trigger.timestamp;
+
+		publish = true;
+
+	} else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high)
+		if (_trigger.edge_state == 1) {
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+		} else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) {
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+			trigger.seq = _capture_seq++;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+			_last_trig_time = trigger.timestamp;
+			publish = true;
+			_capture_seq++;
+		}
+
+	} else { // Get timestamp of mid-exposure (active low)
+		if (_trigger.edge_state == 0) {
+			_last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay);
+
+		} else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) {
+			trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2);
+			trigger.seq = _capture_seq++;
+			_last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time;
+			_last_trig_time = trigger.timestamp;
+			publish = true;
+		}
+
+	}
+
+	trigger.feedback = true;
+	_capture_overflows = _trigger.overflow;
+
+	if (!publish) {
+		return;
+	}
+
+	pps_capture_s pps_capture;
+
+	if (_pps_capture_sub.update(&pps_capture)) {
+		_pps_hrt_timestamp = pps_capture.timestamp;
+		_pps_rtc_timestamp = pps_capture.rtc_timestamp;
+	}
+
+
+	if (_pps_hrt_timestamp > 0) {
+		// Last PPS RTC time + elapsed time to the camera capture interrupt
+		trigger.timestamp_utc = _pps_rtc_timestamp + (trigger.timestamp - _pps_hrt_timestamp);
+
+	} else {
+		// No PPS capture received, use RTC clock as fallback
+		timespec tv{};
+		px4_clock_gettime(CLOCK_REALTIME, &tv);
+		trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp);
+	}
+
+	_trigger_pub.publish(trigger);
+}
+
+void
+InputCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+				 uint32_t overflow)
+{
+	camera_capture::g_input_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
+}
+
+void
+InputCapture::Run()
+{
+	// Command handling
+	vehicle_command_s cmd{};
+
+	if (_command_sub.update(&cmd)) {
+
+		// TODO : this should eventuallly be a capture control command
+		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
+
+			// Enable/disable signal capture
+			if (commandParamToInt(cmd.param1) == 1) {
+				set_capture_control(true);
+
+			} else if (commandParamToInt(cmd.param1) == 0) {
+				set_capture_control(false);
+
+			}
+
+			// Reset capture sequence
+			if (commandParamToInt(cmd.param2) == 1) {
+				reset_statistics(true);
+
+			}
+
+			// Acknowledge the command
+			vehicle_command_ack_s command_ack{};
+
+			command_ack.timestamp = hrt_absolute_time();
+			command_ack.command = cmd.command;
+			command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			command_ack.target_system = cmd.source_system;
+			command_ack.target_component = cmd.source_component;
+
+			_command_ack_pub.publish(command_ack);
+		}
+	}
+}
+
+void
+InputCapture::set_capture_control(bool enabled)
+{
+// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin. It's used if no channel is configured
+#if defined(BOARD_CAPTURE_GPIO)
+
+	if (_capture_channel == -1) {
+		px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &InputCapture::gpio_interrupt_routine, this);
+		_capture_enabled = enabled;
+		_gpio_capture = true;
+		reset_statistics(false);
+	}
+
+#endif
+
+	if (!_gpio_capture) {
+		if (_capture_channel == -1) {
+			PX4_WARN("No capture channel configured");
+			_capture_enabled = false;
+
+		} else {
+			capture_callback_t callback = nullptr;
+			void *context = nullptr;
+
+			if (enabled) {
+				callback = &InputCapture::capture_trampoline;
+				context = this;
+			}
+
+			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 " (%i)", _capture_channel, ret);
+				_capture_enabled = false;
+			}
+
+			reset_statistics(false);
+		}
+	}
+
+}
+
+void
+InputCapture::reset_statistics(bool reset_seq)
+{
+	if (reset_seq) {
+		_capture_seq = 0;
+	}
+
+	_last_trig_begin_time = 0;
+	_last_exposure_time = 0;
+	_last_trig_time = 0;
+	_capture_overflows = 0;
+}
+
+int
+InputCapture::start()
+{
+	if (!_gpio_capture && _capture_channel != -1) {
+		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;
+		}
+	}
+
+	// run every 100 ms (10 Hz)
+	ScheduleOnInterval(100000, 10000);
+
+	return PX4_OK;
+}
+
+void
+InputCapture::stop()
+{
+	ScheduleClear();
+
+	work_cancel(HPWORK, &_work_publisher);
+
+	if (camera_capture::g_input_capture != nullptr) {
+		delete (camera_capture::g_input_capture);
+	}
+}
+
+void
+InputCapture::status()
+{
+	PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO");
+	PX4_INFO("Frame sequence : %" PRIu32, _capture_seq);
+
+	if (_last_trig_time != 0) {
+		PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time,
+			 (int)(hrt_elapsed_time(&_last_trig_time) / 1000));
+
+	} else {
+		PX4_INFO("No trigger yet");
+	}
+
+	if (_camera_capture_mode != 0) {
+		PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0);
+	}
+
+	PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows);
+
+	if (_gpio_capture) {
+		PX4_INFO("Using board GPIO pin");
+
+	} else if (_capture_channel == -1) {
+		PX4_INFO("No Capture channel configured");
+
+	} else {
+		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);
+		}
+	}
+}
+
+static int usage()
+{
+	PX4_INFO("usage: camera_capture {start|stop|on|off|reset|status}\n");
+	return 1;
+}
+
+extern "C" __EXPORT int input_capture_main(int argc, char *argv[]);
+
+int input_capture_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		return usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (camera_capture::g_input_capture != nullptr) {
+			PX4_WARN("already running");
+			return 0;
+		}
+
+		camera_capture::g_input_capture = new InputCapture();
+
+		if (camera_capture::g_input_capture == nullptr) {
+			PX4_WARN("alloc failed");
+			return 1;
+		}
+
+		if (!camera_capture::g_input_capture->start()) {
+			return 0;
+
+		} else {
+			return 1;
+		}
+
+	}
+
+	if (camera_capture::g_input_capture == nullptr) {
+		PX4_WARN("not running");
+		return 1;
+
+	} else if (!strcmp(argv[1], "stop")) {
+		camera_capture::g_input_capture->stop();
+
+	} else if (!strcmp(argv[1], "status")) {
+		camera_capture::g_input_capture->status();
+
+	} else if (!strcmp(argv[1], "on")) {
+		camera_capture::g_input_capture->set_capture_control(true);
+
+	} else if (!strcmp(argv[1], "off")) {
+		camera_capture::g_input_capture->set_capture_control(false);
+
+	} else if (!strcmp(argv[1], "reset")) {
+		camera_capture::g_input_capture->set_capture_control(false);
+		camera_capture::g_input_capture->reset_statistics(true);
+
+	} else {
+		return usage();
+	}
+
+	return 0;
+}

src/drivers/input_capture/input_capture.hpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file input_capture.hpp
+ *
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_input_capture.h>
+#include <drivers/drv_pwm_output.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/camera_trigger.h>
+#include <uORB/topics/pps_capture.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+class InputCapture : public px4::ScheduledWorkItem
+{
+public:
+	/**
+	 * Constructor
+	 */
+	InputCapture();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~InputCapture();
+
+	/**
+	 * Start the task.
+	 */
+	int			start();
+
+	/**
+	 * Stop the task.
+	 */
+	void			stop();
+
+	void 			status();
+
+	// Low-rate command handling loop
+	void			Run() override;
+
+	static void		capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+			uint32_t overflow);
+
+	void 			set_capture_control(bool enabled);
+
+	void			reset_statistics(bool reset_seq);
+
+	void			publish_trigger();
+
+
+	static struct work_s	_work_publisher;
+
+private:
+	int _capture_channel = 5; ///< by default, use FMU output 6
+
+	// Publishers
+	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<camera_trigger_s>		_trigger_pub{ORB_ID(camera_trigger)};
+
+	// Subscribers
+	uORB::Subscription				_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription				_pps_capture_sub{ORB_ID(pps_capture)};
+
+	// Trigger Buffer
+	struct _trig_s {
+		uint32_t chan_index;
+		hrt_abstime hrt_edge_time;
+		uint32_t edge_state;
+		uint32_t overflow;
+	} _trigger{};
+
+	bool			_capture_enabled{false};
+	bool			_gpio_capture{false};
+
+	// Parameters
+	param_t 		_p_strobe_delay{PARAM_INVALID};
+	float			_strobe_delay{0.0f};
+	param_t			_p_camera_capture_mode{PARAM_INVALID};
+	int32_t			_camera_capture_mode{0};
+	param_t			_p_camera_capture_edge{PARAM_INVALID};
+	int32_t			_camera_capture_edge{0};
+
+	// Signal capture statistics
+	uint32_t		_capture_seq{0};
+	hrt_abstime		_last_trig_begin_time{0};
+	hrt_abstime		_last_exposure_time{0};
+	hrt_abstime		_last_trig_time{0};
+	uint32_t 		_capture_overflows{0};
+
+	hrt_abstime	_pps_hrt_timestamp{0};
+	uint64_t		_pps_rtc_timestamp{0};
+	uint8_t			_trigger_rate_exceeded_counter{0};
+	px4::atomic<bool> _trigger_rate_failure{false};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	// Signal capture callback
+	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
+
+	// GPIO interrupt routine
+	static int		gpio_interrupt_routine(int irq, void *context, void *arg);
+
+	// Signal capture publish
+	static void		publish_trigger_trampoline(void *arg);
+
+};

src/drivers/input_capture/input_capture_params.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+// /**
+//  * @file camera_capture_params.c
+//  * Camera capture parameters
+//  *
+//  * @author Mohammed Kabir <kabir@uasys.io>
+//  */
+// /**
+//  * Camera strobe delay
+//  *
+//  * This parameter sets the delay between image integration start and strobe firing
+//  *
+//  * @unit ms
+//  * @min 0.0
+//  * @max 100.0
+//  * @decimal 1
+//  * @group Camera Capture
+//  */
+// PARAM_DEFINE_FLOAT(CAM_CAP_DELAY, 0.0f);
+
+// /**
+//  * Camera capture feedback
+//  *
+//  * Enables camera capture feedback
+//  *
+//  * @boolean
+//  * @group Camera Control
+//  * @reboot_required true
+//  */
+// PARAM_DEFINE_INT32(CAM_CAP_FBACK, 0);
+
+// /**
+//  * Camera capture timestamping mode
+//  *
+//  * Change time measurement
+//  *
+//  * @value 0 Get absolute timestamp
+//  * @value 1 Get timestamp of mid exposure (active high)
+//  * @value 2 Get timestamp of mid exposure (active low)
+//  *
+//  * @group Camera Control
+//  * @reboot_required true
+//  */
+// PARAM_DEFINE_INT32(CAM_CAP_MODE, 0);
+
+// /**
+//  * Camera capture edge
+//  *
+//  * @value 0 Falling edge
+//  * @value 1 Rising edge
+//  *
+//  * @group Camera Control
+//  * @reboot_required true
+//  */
+// PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);