Rework trigger to operate on work queue and timers without jitter

src/drivers/camera_trigger/camera_trigger.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
- *   Author: Mohammed Kabir <mhkabir98@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -47,6 +46,7 @@
 #include <stdbool.h>
 #include <nuttx/clock.h>
 #include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
@@ -60,6 +60,8 @@
 #include <mavlink/mavlink_log.h>
 #include <board_config.h>
 
+#define TRIGGER_PIN_DEFAULT 1
+
 extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
 
 class CameraTrigger
@@ -75,6 +77,11 @@ public:
 	 */
 	~CameraTrigger();
 
+	/**
+	 * Set the trigger on / off
+	 */
+	void		control(bool on);
+
 	/**
 	 * Start the task.
 	 */
@@ -90,37 +97,32 @@ public:
 	 */
 	void		info();
 
-	int 		pin;
+	int			_pin;
 
 private:
 
-	struct hrt_call		_pollcall;
-	struct hrt_call		_firecall;
+	struct hrt_call		_engagecall;
+	struct hrt_call		_disengagecall;
+	static struct work_s	_work;
 
 	int 			_gpio_fd;
 
 	int 			_polarity;
 	float 			_activation_time;
-	float  			_integration_time;
-	float  			_transfer_time;
+	float  			_interval;
 	uint32_t 		_trigger_seq;
 	bool	 		_trigger_enabled;
 
-	int			_sensor_sub;
 	int			_vcommand_sub;
 
 	orb_advert_t		_trigger_pub;
 
-	struct camera_trigger_s		_trigger;
-	struct sensor_combined_s	_sensor;
-	struct vehicle_command_s	_command;
-
 	param_t polarity;
 	param_t activation_time;
-	param_t integration_time ;
+	param_t interval;
 	param_t transfer_time ;
 
-	int32_t _gpios[6] = {
+	static constexpr uint32_t _gpios[6] = {
 		GPIO_GPIO0_OUTPUT,
 		GPIO_GPIO1_OUTPUT,
 		GPIO_GPIO2_OUTPUT,
@@ -130,9 +132,9 @@ private:
 	};
 
 	/**
-	 * Topic poller to check for fire info.
+	 * Vehicle command handler
 	 */
-	static void	poll(void *arg);
+	static void	cycle_trampoline(void *arg);
 	/**
 	 * Fires trigger
 	 */
@@ -144,6 +146,9 @@ private:
 
 };
 
+struct work_s CameraTrigger::_work;
+constexpr uint32_t CameraTrigger::_gpios[6];
+
 namespace camera_trigger
 {
 
@@ -151,38 +156,26 @@ CameraTrigger	*g_camera_trigger;
 }
 
 CameraTrigger::CameraTrigger() :
-	pin(1),
-	_pollcall {},
-	  _firecall {},
+	_pin(TRIGGER_PIN_DEFAULT),
+	_engagecall {},
+	_disengagecall {},
 	_gpio_fd(-1),
 	_polarity(0),
-	  _activation_time(0.0f),
-	  _integration_time(0.0f),
-	  _transfer_time(0.0f),
+	_activation_time(0.5f /* ms */),
+	_interval(100.0f /* ms */),
 	_trigger_seq(0),
 	_trigger_enabled(false),
-	  _sensor_sub(-1),
 	_vcommand_sub(-1),
-	  _trigger_pub(nullptr),
-	  _trigger {},
-	  _sensor {},
-	  _command {},
-_gpios {} {
-
-	memset(&_pollcall, 0, sizeof(_pollcall));
-	memset(&_firecall, 0, sizeof(_firecall));
-
-	memset(&_trigger, 0, sizeof(_trigger));
-	memset(&_sensor, 0, sizeof(_sensor));
-	memset(&_command, 0, sizeof(_command));
-
-	memset(&_gpios, 0, sizeof(_gpios));
+	_trigger_pub(nullptr)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(&_engagecall, 0, sizeof(_engagecall));
+	memset(&_disengagecall, 0, sizeof(_disengagecall));
 
 	// Parameters
 	polarity = param_find("TRIG_POLARITY");
+	interval = param_find("TRIG_INTERVAL");
 	activation_time = param_find("TRIG_ACT_TIME");
-	integration_time = param_find("TRIG_INT_TIME");
-	transfer_time = param_find("TRIG_TRANS_TIME");
 }
 
 CameraTrigger::~CameraTrigger()
@@ -191,39 +184,53 @@ CameraTrigger::~CameraTrigger()
 }
 
 void
-CameraTrigger::start()
+CameraTrigger::control(bool on)
 {
+	// always execute, even if already on
+	// to reset timings if necessary
 
-	_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
+	// schedule trigger on and off calls
+	hrt_call_every(&_engagecall, 500, (_interval * 1000),
+			       (hrt_callout)&CameraTrigger::engage, this);
+
+	// schedule trigger on and off calls
+	hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
+			       (hrt_callout)&CameraTrigger::disengage, this);
+
+	_trigger_enabled = on;
+}
+
+void
+CameraTrigger::start()
+{
 
 	param_get(polarity, &_polarity);
 	param_get(activation_time, &_activation_time);
-	param_get(integration_time, &_integration_time);
-	param_get(transfer_time, &_transfer_time);
+	param_get(interval, &_interval);
 
-	stm32_configgpio(_gpios[pin - 1]);
+	stm32_configgpio(_gpios[_pin - 1]);
 
 	if (_polarity == 0) {
-		stm32_gpiowrite(_gpios[pin - 1], 1);	 	// GPIO pin pull high
+		stm32_gpiowrite(_gpios[_pin - 1], 1);	 	// GPIO pin pull high
 
 	} else if (_polarity == 1) {
-		stm32_gpiowrite(_gpios[pin - 1], 0);	 	// GPIO pin pull low
+		stm32_gpiowrite(_gpios[_pin - 1], 0);	 	// GPIO pin pull low
 
 	} else {
 		warnx(" invalid trigger polarity setting. stopping.");
 		stop();
 	}
 
-	poll(this);	// Trampoline call
-
+	// start to monitor at high rate for trigger enable command
+	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
 }
 
 void
 CameraTrigger::stop()
 {
-	hrt_cancel(&_firecall);
-	hrt_cancel(&_pollcall);
+	work_cancel(LPWORK, &_work);
+	hrt_cancel(&_engagecall);
+	hrt_cancel(&_disengagecall);
 
 	if (camera_trigger::g_camera_trigger != nullptr) {
 		delete(camera_trigger::g_camera_trigger);
@@ -231,62 +238,49 @@ CameraTrigger::stop()
 }
 
 void
-CameraTrigger::poll(void *arg)
+CameraTrigger::cycle_trampoline(void *arg)
 {
 
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
+	if (trig->_vcommand_sub < 0) {
+		trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
+	}
+
 	bool updated;
 	orb_check(trig->_vcommand_sub, &updated);
 
-	if (updated) {
+	// while the trigger is inactive it has to be ready
+	// to become active instantaneously
+	int poll_interval_usec = 5000;
 
-		orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command);
+	if (updated) {
 
-		if (trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
-			if (trig->_command.param1 < 1) {
-				if (trig->_trigger_enabled) {
-					trig->_trigger_enabled = false ;
-				}
+		struct vehicle_command_s cmd;
 
-			} else if (trig->_command.param1 >= 1) {
-				if (!trig->_trigger_enabled) {
-					trig->_trigger_enabled = true ;
-				}
-			}
+		orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
 
+		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
 			// Set trigger rate from command
-			if (trig->_command.param2 > 0) {
-				trig->_integration_time = trig->_command.param2;
-				param_set(trig->integration_time, &(trig->_integration_time));
+			if (cmd.param2 > 0) {
+				trig->_interval = cmd.param2;
+				param_set(trig->interval, &(trig->_interval));
 			}
-		}
-	}
-
-	if (!trig->_trigger_enabled)	{
-		hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig);
-		return;
-
-	} else {
-		engage(trig);
-		hrt_call_after(&trig->_firecall, trig->_activation_time * 1000, (hrt_callout)&CameraTrigger::disengage, trig);
 
-		orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor);
+			if (cmd.param1 < 1.0f) {
+				trig->control(false);
 
-		trig->_trigger.timestamp = trig->_sensor.timestamp;	// get IMU timestamp
-		trig->_trigger.seq = trig->_trigger_seq++;
-
-		if (trig->_trigger_pub != nullptr) {
-			orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger);
-
-		} else {
-			trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger);
+			} else if (cmd.param1 >= 1.0f) {
+				trig->control(true);
+				// while the trigger is active there is no
+				// need to poll at a very high rate
+				poll_interval_usec = 100000;
+			}
 		}
-
-		hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time) * 1000,
-			       (hrt_callout)&CameraTrigger::poll, trig);
 	}
 
+	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
+		camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
 }
 
 void
@@ -295,16 +289,23 @@ CameraTrigger::engage(void *arg)
 
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
-	stm32_configgpio(trig->_gpios[trig->pin - 1]);
+	stm32_configgpio(trig->_gpios[trig->_pin - 1]);
+
+	struct camera_trigger_s	trigger;
+
+	/* set timestamp the instant before the trigger goes off */
+	trigger.timestamp = hrt_absolute_time();
 
 	if (trig->_polarity == 0) {	// ACTIVE_LOW
-		stm32_gpiowrite(trig->_gpios[trig->pin - 1], 0);
+		stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 0);
 
 	} else if (trig->_polarity == 1) {	// ACTIVE_HIGH
-		stm32_gpiowrite(trig->_gpios[trig->pin - 1], 1);
+		stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 1);
 	}
 
+	trigger.seq = trig->_trigger_seq++;
 
+	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
 }
 
 void
@@ -313,13 +314,13 @@ CameraTrigger::disengage(void *arg)
 
 	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
 
-	stm32_configgpio(trig->_gpios[trig->pin - 1]);
+	stm32_configgpio(trig->_gpios[trig->_pin - 1]);
 
 	if (trig->_polarity == 0) {		// ACTIVE_LOW
-		stm32_gpiowrite(trig->_gpios[trig->pin - 1], 1);
+		stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 1);
 
 	} else if (trig->_polarity == 1) {	// ACTIVE_HIGH
-		stm32_gpiowrite(trig->_gpios[trig->pin - 1], 0);
+		stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 0);
 	}
 
 }
@@ -327,16 +328,15 @@ CameraTrigger::disengage(void *arg)
 void
 CameraTrigger::info()
 {
-	warnx("Trigger state : %s", _trigger_enabled ? "enabled" : "disabled");
-	warnx("Trigger pin : %i", pin);
-	warnx("Trigger polarity : %s", _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
-	warnx("Shutter integration time : %.2f", (double)_integration_time);
+	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
+	warnx("pin : %i, polarity : %s", _pin, _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
+	warnx("interval : %.2f", (double)_interval);
 }
 
 static void usage()
 {
 	errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n"
-	     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
+	     "\t-p <n>\tUse specified AUX OUT pin number (default: %d)", TRIGGER_PIN_DEFAULT
 	    );
 }
 
@@ -360,11 +360,11 @@ int camera_trigger_main(int argc, char *argv[])
 
 		if (argc > 3) {
 
-			camera_trigger::g_camera_trigger->pin = (int)argv[3];
+			camera_trigger::g_camera_trigger->_pin = (int)argv[3];
 
 			if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) {
 				warnx("starting trigger on pin : %li ", atoi(argv[3]));
-				camera_trigger::g_camera_trigger->pin = atoi(argv[3]);
+				camera_trigger::g_camera_trigger->_pin = atoi(argv[3]);
 
 			} else {
 				usage();
@@ -378,14 +378,19 @@ int camera_trigger_main(int argc, char *argv[])
 
 	if (camera_trigger::g_camera_trigger == nullptr) {
 		errx(1, "not running");
-	}
 
-	else if (!strcmp(argv[1], "stop")) {
+	} else if (!strcmp(argv[1], "stop")) {
 		camera_trigger::g_camera_trigger->stop();
 
 	} else if (!strcmp(argv[1], "info")) {
 		camera_trigger::g_camera_trigger->info();
 
+	} else if (!strcmp(argv[1], "enable")) {
+		camera_trigger::g_camera_trigger->control(true);
+
+	} else if (!strcmp(argv[1], "disable")) {
+		camera_trigger::g_camera_trigger->control(false);
+
 	} else {
 		usage();
 	}

src/drivers/camera_trigger/camera_trigger_params.c

@@ -51,19 +51,19 @@
  * @max 500.0
  * @group Camera trigger
  */
-PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 300.0f);
+PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 1.0f);
 
 /**
- * Camera trigger transfer time
+ * Camera trigger interval
  *
- * This parameter sets the time the image transfer takes (PointGrey mode_0)
+ * This parameter sets the time between two consecutive trigger events
  *
  * @unit milliseconds
  * @min 15.0
  * @max 33.0
  * @group Camera trigger
  */
-PARAM_DEFINE_FLOAT(TRIG_TRANS_TIME, 15.0f);
+PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 15.0f);
 
 /**
  * Camera trigger polarity
@@ -79,10 +79,20 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
 /**
  * Camera trigger activation time
  *
- * This parameter sets the time the trigger needs to pulled high or low to start light 
- * integration.
+ * This parameter sets the time the trigger needs to pulled high or low.
  *
  * @unit milliseconds
  * @group Camera trigger
  */
-PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 5.0f);
+PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f);
+
+/**
+ * Camera trigger mode
+ *
+ * 0 disables the trigger, 1 sets it to enabled on command, 2 always on
+ *
+ * @min 0
+ * @max 2
+ * @group Camera trigger
+ */
+PARAM_DEFINE_INT32(TRIG_MODE, 0);