Navigator: add optional delay after gimbal mission items

src/modules/navigator/mission_block.cpp

@@ -81,7 +81,6 @@ MissionBlock::is_mission_item_reached_or_completed()
 	case NAV_CMD_DO_CONTROL_VIDEO:
 	case NAV_CMD_DO_MOUNT_CONFIGURE:
 	case NAV_CMD_DO_MOUNT_CONTROL:
-	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
 	case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 	case NAV_CMD_DO_SET_ROI:
 	case NAV_CMD_DO_SET_ROI_LOCATION:
@@ -142,40 +141,14 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 		break;
 
-	case NAV_CMD_DO_WINCH: {
-			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) *
-					1E-6f; // TODO: Add proper microseconds_to_seconds function
-
-			if (_payload_deploy_ack_successful) {
-				PX4_DEBUG("Winch Deploy Ack received! Resuming mission");
-				return true;
-
-			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
-				PX4_DEBUG("Winch Deploy Timed out, resuming mission!");
-				return true;
-
-			}
-
-			// We are still waiting for the acknowledgement / execution of deploy
-			return false;
+	case NAV_CMD_DO_WINCH:
+	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+	case NAV_CMD_DO_GRIPPER:
+		if (now > _timestamp_command_timeout + (_command_timeout * 1_s)) {
+			return true;
 		}
 
-	case NAV_CMD_DO_GRIPPER: {
-			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
-
-			if (_payload_deploy_ack_successful) {
-				PX4_DEBUG("Gripper Deploy Ack received! Resuming mission");
-				return true;
-
-			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
-				PX4_DEBUG("Gripper Deploy Timed out, resuming mission!");
-				return true;
-
-			}
-
-			// We are still waiting for the acknowledgement / execution of deploy
-			return false;
-		}
+		return false; // Still waiting
 
 	default:
 		/* do nothing, this is a 3D waypoint */
@@ -445,7 +418,7 @@ MissionBlock::is_mission_item_reached_or_completed()
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
-		    (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
+		    (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) {
 			time_inside_reached = true;
 		}
 
@@ -549,53 +522,37 @@ MissionBlock::issue_command(const mission_item_s &item)
 		return;
 	}
 
-	if (item.nav_cmd == NAV_CMD_DO_WINCH ||
-	    item.nav_cmd == NAV_CMD_DO_GRIPPER) {
-		// Initiate Payload Deployment
-		vehicle_command_s vcmd = {};
-		vcmd.command = item.nav_cmd;
-		vcmd.param1 = item.params[0];
-		vcmd.param2 = item.params[1];
-		vcmd.param3 = item.params[2];
-		vcmd.param4 = item.params[3];
-		vcmd.param5 = static_cast<double>(item.params[4]);
-		vcmd.param6 = static_cast<double>(item.params[5]);
-		_navigator->publish_vehicle_cmd(&vcmd);
-
-		// Reset payload deploy flag & data to get ready to receive deployment ack result
-		_payload_deploy_ack_successful = false;
-		_payload_deployed_time = hrt_absolute_time();
-
-	} else {
+	// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
+	if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
+		_navigator->acquire_gimbal_control();
+	}
 
-		// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
-		if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
-			_navigator->acquire_gimbal_control();
+	// Mission item's NAV_CMD enums directly map to the according vehicle command
+	// So set the raw value directly (MAV_FRAME_MISSION mission item)
+	vehicle_command_s vcmd = {};
+	vcmd.command = item.nav_cmd;
+	vcmd.param1 = item.params[0];
+	vcmd.param2 = item.params[1];
+	vcmd.param3 = item.params[2];
+	vcmd.param4 = item.params[3];
+	vcmd.param5 = static_cast<double>(item.params[4]);
+	vcmd.param6 = static_cast<double>(item.params[5]);
+	vcmd.param7 = item.params[6];
+
+	if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
+		// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
+		vcmd.param5 = item.lat;
+		vcmd.param6 = item.lon;
+
+		if (item.altitude_is_relative) {
+			vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
 		}
+	}
 
-		// Mission item's NAV_CMD enums directly map to the according vehicle command
-		// So set the raw value directly (MAV_FRAME_MISSION mission item)
-		vehicle_command_s vcmd = {};
-		vcmd.command = item.nav_cmd;
-		vcmd.param1 = item.params[0];
-		vcmd.param2 = item.params[1];
-		vcmd.param3 = item.params[2];
-		vcmd.param4 = item.params[3];
-		vcmd.param5 = static_cast<double>(item.params[4]);
-		vcmd.param6 = static_cast<double>(item.params[5]);
-		vcmd.param7 = item.params[6];
-
-		if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
-			// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
-			vcmd.param5 = item.lat;
-			vcmd.param6 = item.lon;
-
-			if (item.altitude_is_relative) {
-				vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
-			}
-		}
+	_navigator->publish_vehicle_cmd(&vcmd);
 
-		_navigator->publish_vehicle_cmd(&vcmd);
+	if (item_has_timeout(item)) {
+		_timestamp_command_timeout = hrt_absolute_time();
 	}
 }
 
@@ -619,10 +576,13 @@ MissionBlock::get_time_inside(const mission_item_s &item) const
 // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER
 // or so, and have a function named 'item_is_conditional'
 // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands
+// A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW
 bool
 MissionBlock::item_has_timeout(const mission_item_s &item)
 {
-	return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER;
+	return item.nav_cmd == NAV_CMD_DO_WINCH ||
+	       item.nav_cmd == NAV_CMD_DO_GRIPPER ||
+	       item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
 }
 
 bool

src/modules/navigator/mission_block.h

@@ -108,30 +108,11 @@ class MissionBlock : public NavigatorMode
 	static bool item_contains_marker(const mission_item_s &item);
 
 	/**
-	 * @brief Set the payload deployment successful flag object
+	 * Set the item_has_timeout() command timeout
 	 *
-	 * Function is accessed in Navigator (navigator_main.cpp) to flag when a successful
-	 * payload deployment ack command has been received. Which in turn allows the mission item
-	 * to continue to the next in the 'is_mission_item_reached_or_completed' function below
+	 * @param timeout Timeout in seconds
 	 */
-	void set_payload_depolyment_successful_flag(bool payload_deploy_result)
-	{
-		_payload_deploy_ack_successful = payload_deploy_result;
-	}
-
-	/**
-	 * @brief Set the payload deployment timeout
-	 *
-	 * Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful
-	 * payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will
-	 * continue into the next item automatically.
-	 *
-	 * @param timeout_s Timeout in seconds
-	 */
-	void set_payload_deployment_timeout(const float timeout_s)
-	{
-		_payload_deploy_timeout_s = timeout_s;
-	}
+	void set_command_timeout(const float timeout) { _command_timeout = timeout; }
 
 	/**
 	 * Copies position from setpoint if valid, otherwise copies current position
@@ -247,12 +228,10 @@ class MissionBlock : public NavigatorMode
 
 	hrt_abstime _time_wp_reached{0};
 
-	/* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */
-	bool _payload_deploy_ack_successful{false};	// Flag to keep track of whether we received an acknowledgement for a successful payload deployment
-	hrt_abstime _payload_deployed_time{0};		// Last payload deployment start time to handle timeouts
-	float _payload_deploy_timeout_s{0.0f};		// Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
+	// Mission items that have a timeout to allow the payload e.g. gripper, winch, gimbal executing the command see item_has_timeout()
+	hrt_abstime _timestamp_command_timeout{0}; ///< Timestamp when the current item_has_timeout() command was started
+	float _command_timeout{0.f}; ///< Time in seconds any item_has_timeout() command should be waited for before continuing the mission
 
 private:
 	void updateMaxHaglFailsafe();
-
 };

src/modules/navigator/mission_params.c

@@ -132,17 +132,6 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
 
-/**
- * Timeout for a successful payload deployment acknowledgement
- *
- * @unit s
- * @min 0
- * @decimal 1
- * @increment 1
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f);
-
 /**
  * Landing abort min altitude
  *
@@ -155,3 +144,21 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30);
+
+/**
+ * Timeout to allow the payload to execute the mission command
+ *
+ * Ensure:
+ *   gripper: NAV_CMD_DO_GRIPPER
+ *     has released before continuing mission.
+ *   winch: CMD_DO_WINCH
+ *     has delivered before continuing mission.
+ *   gimbal: CMD_DO_GIMBAL_MANAGER_PITCHYAW
+ *     has reached the commanded orientation before beginning to take pictures.
+ *
+ * @unit s
+ * @min 0
+ * @decimal 1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_COMMAND_TOUT, 0.f);

src/modules/navigator/navigator.h

@@ -424,10 +424,10 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 		_param_nav_min_gnd_dist,	/**< minimum distance to ground (Mission and RTL)*/
 
 		// non-navigator parameters: Mission (MIS_*)
-		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
-		(ParamFloat<px4::params::MIS_YAW_TMT>)     _param_mis_yaw_tmt,
-		(ParamFloat<px4::params::MIS_YAW_ERR>)     _param_mis_yaw_err,
-		(ParamFloat<px4::params::MIS_PD_TO>)       _param_mis_payload_delivery_timeout,
-		(ParamInt<px4::params::MIS_LND_ABRT_ALT>)  _param_mis_lnd_abrt_alt
+		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>)    _param_mis_takeoff_alt,
+		(ParamFloat<px4::params::MIS_YAW_TMT>)        _param_mis_yaw_tmt,
+		(ParamFloat<px4::params::MIS_YAW_ERR>)        _param_mis_yaw_err,
+		(ParamInt<px4::params::MIS_LND_ABRT_ALT>)     _param_mis_lnd_abrt_alt,
+		(ParamFloat<px4::params::MIS_COMMAND_TOUT>) _param_mis_command_tout
 	)
 };

src/modules/navigator/navigator_main.cpp

@@ -115,13 +115,6 @@ Navigator::Navigator() :
 	_distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF;
 	_distance_sensor_mode_change_request_pub.update();
 
-	// Update the timeout used in mission_block (which can't hold it's own parameters)
-	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
-
-	_adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(),
-			_param_nav_traff_a_ver.get(),
-			_param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get());
-
 	reset_triplets();
 }
 
@@ -149,7 +142,10 @@ void Navigator::params_update()
 		param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor);
 	}
 
-	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
+	_mission.set_command_timeout(_param_mis_command_tout.get());
+	_adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(),
+			_param_nav_traff_a_ver.get(),
+			_param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get());
 }
 
 void Navigator::run()
@@ -1267,9 +1263,7 @@ void Navigator::run_fake_traffic()
 
 void Navigator::check_traffic()
 {
-
 	if (_traffic_sub.updated()) {
-
 		_traffic_sub.copy(&_adsb_conflict._transponder_report);
 
 		uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
@@ -1288,7 +1282,6 @@ void Navigator::check_traffic()
 	}
 
 	_adsb_conflict.remove_expired_conflicts();
-
 }
 
 bool Navigator::abort_landing()