mission: add heading towards ROI mode for multicopters

src/modules/navigator/mission.cpp

@@ -1018,6 +1018,12 @@ Mission::heading_sp_update()
 			point_to_latlon[0] = _navigator->get_home_position()->lat;
 			point_to_latlon[1] = _navigator->get_home_position()->lon;
 
+		} else if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI
+			   && _navigator->get_vroi()->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
+			/* target location is ROI */
+			point_to_latlon[0] = _navigator->get_vroi()->lat;
+			point_to_latlon[1] = _navigator->get_vroi()->lon;
+
 		} else {
 			/* target location is next (current) waypoint */
 			point_to_latlon[0] = pos_sp_triplet->current.lat;

src/modules/navigator/mission.h

@@ -61,6 +61,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_roi.h>
 #include <uORB/uORB.h>
 
 class Navigator;
@@ -86,7 +87,8 @@ public:
 		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
 		MISSION_YAWMODE_FRONT_TO_HOME = 2,
 		MISSION_YAWMODE_BACK_TO_HOME = 3,
-		MISSION_YAWMODE_MAX = 4
+		MISSION_YAWMODE_TO_ROI = 4,
+		MISSION_YAWMODE_MAX = 5
 	};
 
 	bool set_current_offboard_mission_index(unsigned index);

src/modules/navigator/mission_params.c

@@ -125,6 +125,7 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
  * @value 1 Heading towards waypoint
  * @value 2 Heading towards home
  * @value 3 Heading away from home
+ * @value 4 Heading towards ROI
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_YAWMODE, 1);

src/modules/navigator/navigator.h

@@ -135,6 +135,7 @@ public:
 	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
 	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
 	struct vehicle_status_s *get_vstatus() { return &_vstatus; }
+	struct vehicle_roi_s *get_vroi() { return &_vroi; }
 
 	bool home_position_valid() { return (_home_pos.timestamp > 0); }
 
@@ -237,6 +238,7 @@ private:
 	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
+	int		_vehicle_roi_sub{-1};		/**< vehicle ROI subscription */
 
 	orb_advert_t	_geofence_result_pub{nullptr};
 	orb_advert_t	_mission_result_pub{nullptr};
@@ -257,6 +259,7 @@ private:
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
+	vehicle_roi_s					_vroi{};		/**< vehicle ROI */
 
 	int		_mission_instance_count{-1};	/**< instance count for the current mission */
 
@@ -304,6 +307,7 @@ private:
 	void		sensor_combined_update();
 	void		vehicle_land_detected_update();
 	void		vehicle_status_update();
+	void		vehicle_roi_update();
 
 	/**
 	 * Shim for calling task_main from task_create.

src/modules/navigator/navigator_main.cpp

@@ -213,6 +213,12 @@ Navigator::params_update()
 	}
 }
 
+void
+Navigator::vehicle_roi_update()
+{
+	orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &_vroi);
+}
+
 void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
@@ -246,6 +252,7 @@ Navigator::task_main()
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -257,6 +264,7 @@ Navigator::task_main()
 	home_position_update(true);
 	fw_pos_ctrl_status_update(true);
 	params_update();
+	vehicle_roi_update();
 
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1] = {};
@@ -357,6 +365,13 @@ Navigator::task_main()
 			home_position_update();
 		}
 
+		/* ROI updated */
+		orb_check(_vehicle_roi_sub, &updated);
+
+		if (updated) {
+			vehicle_roi_update();
+		}
+
 		/* vehicle_command updated */
 		orb_check(_vehicle_command_sub, &updated);