Improve trigger control for survey applications

ROMFS/px4fmu_common/init.d-posix/1030_plane

@@ -16,6 +16,8 @@ then
 	param set FW_LND_ANG 8
 	param set FW_THR_LND_MAX 0
 
+	param set FW_L1_PERIOD 15
+
 	param set FW_P_TC 0.5
 	param set FW_PR_FF 0.40
 	param set FW_PR_I 0.05

ROMFS/px4fmu_common/init.d-posix/1031_plane_cam

@@ -4,3 +4,11 @@
 #
 
 sh /etc/init.d-posix/1030_plane
+
+if [ $AUTOCNF = yes ]
+then
+	# Camera trigger interface is MAVLink
+	param set TRIG_INTERFACE 3
+	# Distance trigger mode enabled
+	param set TRIG_MODE 4
+fi

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -110,9 +110,10 @@ simulator_tcp_port=$((4560+px4_instance))
 udp_offboard_port_local=$((14580+px4_instance))
 udp_offboard_port_remote=$((14540+px4_instance))
 [ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps
+udp_onboard_payload_port_local=$((14280+px4_instance))
+udp_onboard_payload_port_remote=$((14030+px4_instance))
 udp_gcs_port_local=$((18570+px4_instance))
 
-
 if [ $AUTOCNF = yes ]
 then
 	param set SYS_AUTOSTART $REQUESTED_AUTOSTART
@@ -276,6 +277,9 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
 # API/Offboard link
 mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
 
+# Onboard link to camera
+mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
+
 # execute autostart post script if any
 [ -e "$autostart_file".post ] && sh "$autostart_file".post
 

msg/vehicle_command.msg

@@ -26,6 +26,7 @@ uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113		# Ascend/descend at rate.  Delay
 uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114		# Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_CONDITION_YAW = 115			# Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_CONDITION_LAST = 159		# NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_CONDITION_GATE = 4501		# Wait until passing a threshold |2D coord mode: 0: Orthogonal to planned route | Altitude mode: 0: Ignore altitude| Empty| Empty| Lat| Lon| Alt|
 uint16 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_JUMP = 177			# Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_CHANGE_SPEED = 178		# Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|

platforms/posix/cmake/sitl_target.cmake

@@ -62,7 +62,7 @@ set(viewers none jmavsim gazebo)
 set(debuggers none ide gdb lldb ddd valgrind callgrind)
 set(models none shell
 	if750a iris iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps solo typhoon_h480
-	plane plane_catapult
+	plane plane_cam plane_catapult
 	standard_vtol tailsitter tiltrotor
 	rover boat
 	uuv_hippocampus)

src/drivers/camera_trigger/camera_trigger.cpp

@@ -40,6 +40,7 @@
  * @author Mohammed Kabir <[email protected]>
  * @author Kelly Steich <[email protected]>
  * @author Andreas Bircher <[email protected]>
+ * @author Lorenz Meier <[email protected]>
  */
 
 #include <stdio.h>
@@ -161,8 +162,10 @@ class CameraTrigger : public px4::ScheduledWorkItem
 
 	float			_activation_time;
 	float			_interval;
+	float			_min_interval;
 	float 			_distance;
 	uint32_t 		_trigger_seq;
+	hrt_abstime		_last_trigger_timestamp;
 	bool			_trigger_enabled;
 	bool			_trigger_paused;
 	bool			_one_shot;
@@ -181,6 +184,7 @@ class CameraTrigger : public px4::ScheduledWorkItem
 	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),
+	_last_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_INTERVA");
 	_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,33 +365,33 @@ 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{};
-		_lpos_sub.copy(&local);
+	vehicle_local_position_s local{};
+	_lpos_sub.copy(&local);
 
-		if (local.xy_valid) {
+	if (local.xy_valid) {
 
-			// Initialize position if not done yet
-			matrix::Vector2f current_position(local.x, local.y);
+		// Initialize position if not done yet
+		matrix::Vector2f current_position(local.x, local.y);
 
-			if (!_valid_position) {
-				// First time valid position, take first shot
-				_last_shoot_position = current_position;
-				_valid_position = local.xy_valid;
-				shoot_once();
-			}
+		if (!_valid_position) {
+			// First time valid position, take first shot
+			_last_shoot_position = current_position;
+			_valid_position = local.xy_valid;
 
-			// Check that distance threshold is exceeded
-			if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
+			if (!_one_shot) {
 				shoot_once();
-				_last_shoot_position = current_position;
-
 			}
 		}
+
+		// Check that distance threshold is exceeded
+		if (matrix::Vector2f(_last_shoot_position - current_position).length() >= _distance) {
+			shoot_once();
+			_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
-		hrt_call_after(&_engagecall, 0,
-			       (hrt_callout)&CameraTrigger::engage, this);
+	// schedule trigger on and off calls
+	hrt_call_after(&_engagecall, 0,
+		       (hrt_callout)&CameraTrigger::engage, this);
 
-		hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
-			       (hrt_callout)&CameraTrigger::disengage, this);
-	}
+	hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
+		       (hrt_callout)&CameraTrigger::disengage, this);
 }
 
 bool
@@ -526,18 +532,27 @@ 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) {
-				// test shots are not logged or forwarded to GCS for geotagging
-				_test_shot = true;
-			}
+			if (now - _last_trigger_timestamp < _min_interval * 1000) {
+				// triggering too fast, abort
+				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
-			if (commandParamToInt((float)cmd.param5) == 1) {
-				// Schedule shot
-				_one_shot = true;
-			}
+			} else {
+				if (commandParamToInt(cmd.param7) == 1) {
+					// test shots are not logged or forwarded to GCS for geotagging
+					_test_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) {
 
@@ -571,14 +586,15 @@ CameraTrigger::Run()
 
 			/*
 			 * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
-			*/
+			 */
 
 			if (cmd.param1 > 0.0f) {
 				_distance = cmd.param1;
 				param_set_no_notification(_p_distance, &_distance);
 
 				_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->_last_trigger_timestamp > 0) && (now - trig->_last_trigger_timestamp < trig->_min_interval * 1000)) {
+		return;
+	}
+
 	// Trigger the camera
 	trig->_camera_interface->trigger(true);
+	// set last timestamp
+	trig->_last_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 = now;
 
 	timespec tv = {};
 	px4_clock_gettime(CLOCK_REALTIME, &tv);

src/drivers/camera_trigger/camera_trigger_params.c

@@ -37,6 +37,7 @@
  *
  * @author Mohammed Kabir <[email protected]>
  * @author Andreas Bircher <[email protected]>
+ * @author Lorenz Meier <[email protected]>
  */
 
 /**
@@ -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_INTERVA, 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);

src/modules/mavlink/mavlink_mission.cpp

@@ -1386,6 +1386,10 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
 			break;
 
+		case MAV_CMD_CONDITION_GATE:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			break;
+
 		case MAV_CMD_NAV_FENCE_RETURN_POINT:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
@@ -1469,6 +1473,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 		case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 		case MAV_CMD_DO_SET_ROI_NONE:
+		case MAV_CMD_CONDITION_DELAY:
+		case MAV_CMD_CONDITION_DISTANCE:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;