Camera trigger: Handle trigger distance handling better

src/drivers/camera_trigger/camera_trigger.cpp

@@ -40,6 +40,7 @@
  * @author Mohammed Kabir <kabir@uasys.io>
  * @author Kelly Steich <kelly.steich@wingtra.com>
  * @author Andreas Bircher <andreas@wingtra.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <stdio.h>
@@ -161,8 +162,10 @@ private:
 
 	float			_activation_time;
 	float			_interval;
+	float			_min_interval;
 	float 			_distance;
 	uint32_t 		_trigger_seq;
+	hrt_abstime		_trigger_timestamp;
 	bool			_trigger_enabled;
 	bool			_trigger_paused;
 	bool			_one_shot;
@@ -181,6 +184,7 @@ private:
 	param_t			_p_mode;
 	param_t			_p_activation_time;
 	param_t			_p_interval;
+	param_t			_p_min_interval;
 	param_t			_p_distance;
 	param_t			_p_interface;
 	param_t 		_p_cam_cap_fback;
@@ -242,8 +246,10 @@ CameraTrigger::CameraTrigger() :
 	_keepalivecall_down {},
 	_activation_time(0.5f /* ms */),
 	_interval(100.0f /* ms */),
+	_min_interval(1.0f /* ms */),
 	_distance(25.0f /* m */),
 	_trigger_seq(0),
+	_trigger_timestamp(0),
 	_trigger_enabled(false),
 	_trigger_paused(false),
 	_one_shot(false),
@@ -266,6 +272,7 @@ CameraTrigger::CameraTrigger() :
 
 	// Parameters
 	_p_interval = param_find("TRIG_INTERVAL");
+	_p_min_interval = param_find("TRIG_MIN_TIMING");
 	_p_distance = param_find("TRIG_DISTANCE");
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
@@ -274,6 +281,7 @@ CameraTrigger::CameraTrigger() :
 
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
+	param_get(_p_min_interval, &_min_interval);
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, (int32_t *)&_trigger_mode);
 	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
@@ -357,32 +365,32 @@ CameraTrigger::update_intervalometer()
 void
 CameraTrigger::update_distance()
 {
-	if (_turning_on) {
+	if (_turning_on || !_trigger_enabled || _trigger_paused) {
 		return;
 	}
 
-	if (_trigger_enabled) {
+	vehicle_local_position_s local{};
-		vehicle_local_position_s local{};
+	_lpos_sub.copy(&local);
-		_lpos_sub.copy(&local);
 
-		if (local.xy_valid) {
+	if (local.xy_valid) {
 
-			// Initialize position if not done yet
+		// Initialize position if not done yet
-			matrix::Vector2f current_position(local.x, local.y);
+		matrix::Vector2f current_position(local.x, local.y);
 
-			if (!_valid_position) {
+		if (!_valid_position) {
-				// First time valid position, take first shot
+			// First time valid position, take first shot
-				_last_shoot_position = current_position;
+			_last_shoot_position = current_position;
-				_valid_position = local.xy_valid;
+			_valid_position = local.xy_valid;
+
+			if (!_one_shot) {
 				shoot_once();
 			}
+		}
 
-			// Check that distance threshold is exceeded
+		// Check that distance threshold is exceeded
-			if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
+		if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
-				shoot_once();
+			shoot_once();
-				_last_shoot_position = current_position;
+			_last_shoot_position = current_position;
-
-			}
 		}
 	}
 }
@@ -419,14 +427,12 @@ CameraTrigger::toggle_power()
 void
 CameraTrigger::shoot_once()
 {
-	if (!_trigger_paused) {
+	// schedule trigger on and off calls
-		// schedule trigger on and off calls
+	hrt_call_after(&_engagecall, 0,
-		hrt_call_after(&_engagecall, 0,
+		       (hrt_callout)&CameraTrigger::engage, this);
-			       (hrt_callout)&CameraTrigger::engage, this);
 
-		hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
+	hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
-			       (hrt_callout)&CameraTrigger::disengage, this);
+		       (hrt_callout)&CameraTrigger::disengage, this);
-	}
 }
 
 bool
@@ -526,19 +532,28 @@ CameraTrigger::Run()
 		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
 
 			need_ack = true;
+			hrt_abstime now = hrt_absolute_time();
 
-			if (commandParamToInt(cmd.param7) == 1) {
+			if (now - _trigger_timestamp < _min_interval * 1000) {
-				// test shots are not logged or forwarded to GCS for geotagging
+				// triggering too fast, abort
-				_test_shot = true;
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+			} else {
+				if (commandParamToInt(cmd.param7) == 1) {
+					// test shots are not logged or forwarded to GCS for geotagging
+					_test_shot = true;
+
+				}
+
+				if (commandParamToInt((float)cmd.param5) == 1) {
+					// Schedule shot
+					_one_shot = true;
+
+				}
+
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
-			if (commandParamToInt((float)cmd.param5) == 1) {
-				// Schedule shot
-				_one_shot = true;
-			}
-
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
-
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
 
 			need_ack = true;
@@ -571,7 +586,7 @@ CameraTrigger::Run()
 
 			/*
 			 * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
-			*/
+			 */
 
 			if (cmd.param1 > 0.0f) {
 				_distance = cmd.param1;
@@ -579,6 +594,7 @@ CameraTrigger::Run()
 
 				_trigger_enabled = true;
 				_trigger_paused = false;
+				_valid_position = false;
 
 			} else if (commandParamToInt(cmd.param1) == 0) {
 				_trigger_paused = true;
@@ -677,6 +693,9 @@ CameraTrigger::Run()
 		}
 	}
 
+	// order matters - one_shot has to be handled LAST
+	// as the other trigger handlers will back off from it
+
 	// run every loop iteration and trigger if needed
 	if (_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
 	    _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
@@ -718,8 +737,16 @@ CameraTrigger::engage(void *arg)
 {
 	CameraTrigger *trig = static_cast<CameraTrigger *>(arg);
 
+	hrt_abstime now = hrt_absolute_time();
+
+	if ((trig->_trigger_timestamp > 0) && (now - trig->_trigger_timestamp < trig->_min_interval * 1000)) {
+		return;
+	}
+
 	// Trigger the camera
 	trig->_camera_interface->trigger(true);
+	// set last timestamp
+	trig->_trigger_timestamp = now;
 
 	if (trig->_test_shot) {
 		// do not send messages or increment frame count for test shots
@@ -732,7 +759,7 @@ CameraTrigger::engage(void *arg)
 	struct camera_trigger_s	trigger = {};
 
 	// Set timestamp the instant after the trigger goes off
-	trigger.timestamp = hrt_absolute_time();
+	trigger.timestamp = trig->_trigger_timestamp;
 
 	timespec tv = {};
 	px4_clock_gettime(CLOCK_REALTIME, &tv);

src/drivers/camera_trigger/camera_trigger_params.c

@@ -37,6 +37,7 @@
  *
  * @author Mohammed Kabir <kabir@uasys.io>
  * @author Andreas Bircher <andreas@wingtra.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 /**
@@ -50,7 +51,6 @@
 * @value 4 Generic PWM (IR trigger, servo)
 *
 * @reboot_required true
-*
 * @group Camera trigger
 */
 PARAM_DEFINE_INT32(TRIG_INTERFACE, 4);
@@ -64,10 +64,26 @@ PARAM_DEFINE_INT32(TRIG_INTERFACE, 4);
  * @min 4.0
  * @max 10000.0
  * @decimal 1
+ * @reboot_required true
  * @group Camera trigger
  */
 PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f);
 
+/**
+ * Minimum camera trigger interval
+ *
+ * This parameter sets the minimum time between two consecutive trigger events
+ * the specific camera setup is supporting.
+ *
+ * @unit ms
+ * @min 1.0
+ * @max 10000.0
+ * @decimal 1
+ * @reboot_required true
+ * @group Camera trigger
+ */
+PARAM_DEFINE_FLOAT(TRIG_MIN_TIMING, 1.0f);
+
 /**
  * Camera trigger polarity
  *
@@ -77,6 +93,7 @@ PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 40.0f);
  * @value 1 Active high
  * @min 0
  * @max 1
+ * @reboot_required true
  * @group Camera trigger
  */
 PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
@@ -90,6 +107,7 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
  * @min 0.1
  * @max 3000
  * @decimal 1
+ * @reboot_required true
  * @group Camera trigger
  */
 PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 40.0f);
@@ -136,6 +154,7 @@ PARAM_DEFINE_INT32(TRIG_PINS, 56);
  * @min 0
  * @increment 1
  * @decimal 1
+ * @reboot_required true
  * @group Camera trigger
  */
 PARAM_DEFINE_FLOAT(TRIG_DISTANCE, 25.0f);