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:
@@ -160,6 +159,19 @@ MissionBlock::is_mission_item_reached_or_completed()
 			return false;
 		}
 
+	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: {
+			const float gimbal_command_elasped_time_s = (now - _gimbal_command_time) * 1E-6f;
+			if (gimbal_command_elasped_time_s > _gimbal_wait_time) {
+				PX4_DEBUG("Waited %.2f seconds for the gimbal to reach the desired orientation, resuming mission!", (double) _gimbal_wait_time);
+				return true;
+
+			}
+
+			// We are still waiting the desired delay for the gimbal
+			return false;
+		}
+
+
 	case NAV_CMD_DO_GRIPPER: {
 			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
 
@@ -595,6 +607,13 @@ MissionBlock::issue_command(const mission_item_s &item)
 			}
 		}
 
+		// Register the Gimbal control command (NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW) timestamp so to wait
+		// for the gimbal to reach the desired position
+		if (item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW)
+		{
+			_gimbal_command_time = hrt_absolute_time();
+		}
+
 		_navigator->publish_vehicle_cmd(&vcmd);
 	}
 }
@@ -619,10 +638,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

@@ -133,6 +133,19 @@ public:
 		_payload_deploy_timeout_s = timeout_s;
 	}
 
+	/**
+	 * @brief Set the gimbal wait time
+	 *
+	 * Accessed in Navigator to set the appropriate time to wait for the gimbal to reach
+	 * the desired orientation before resuming the mission
+	 *
+	 * @param wait_time Wait time in seconds
+	 */
+	void set_gimbal_wait_time(const float wait_time)
+	{
+		_gimbal_wait_time = wait_time;
+	}
+
 	/**
 	 * Copies position from setpoint if valid, otherwise copies current position
 	 */
@@ -252,6 +265,10 @@ protected:
 	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
 
+	/* Gimbal commands time variables are used in case of NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW to wait for the gimbal movement */
+	hrt_abstime _gimbal_command_time{0};		// Last gimbal command timestamp
+	float _gimbal_wait_time{0.0f};			// Time to wait for the gimbal to reach the desired configuration
+
 private:
 	void updateMaxHaglFailsafe();
 

src/modules/navigator/mission_params.c

@@ -155,3 +155,16 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30);
+
+/**
+ * Time in seconds allowed for the gimbal to reach its commanded attitude before the mission resumes.
+ *
+ * Time allocated for a gimbal command to execute within a mission before resuming.
+ * This ensures the gimbal has reached the commanded orientation before beginning to take pictures.
+ *
+ * @unit s
+ * @min 0
+ * @decimal 1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_GIM_WAIT_T, 0.0f);

src/modules/navigator/navigator.h

@@ -424,10 +424,11 @@ private:
 		_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,
+		(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_GIM_WAIT_T>)  _param_mis_gimbal_wait_t
 	)
 };

src/modules/navigator/navigator_main.cpp

@@ -117,6 +117,7 @@ Navigator::Navigator() :
 
 	// 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());
+	_mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get());
 
 	_adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(),
 			_param_nav_traff_a_ver.get(),
@@ -150,6 +151,8 @@ void Navigator::params_update()
 	}
 
 	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
+	_mission.set_gimbal_wait_time(_param_mis_gimbal_wait_t.get());
+
 }
 
 void Navigator::run()