New ROI commands implementation

msg/vehicle_command.msg

@@ -38,6 +38,9 @@ uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing.
 uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 uint16 VEHICLE_CMD_DO_REPOSITION = 192
 uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193
+uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195	# Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude|
+uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint|
+uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197		# Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200		# Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_SET_ROI = 201				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203

msg/vehicle_roi.msg

@@ -1,7 +1,7 @@
 # Vehicle Region Of Interest (ROI)
 
 uint8 ROI_NONE = 0                         # No region of interest
-uint8 ROI_WPNEXT = 1                       # Point toward next MISSION
+uint8 ROI_WPNEXT = 1                       # Point toward next MISSION with optional offset
 uint8 ROI_WPINDEX = 2                      # Point toward given MISSION
 uint8 ROI_LOCATION = 3                     # Point toward fixed location
 uint8 ROI_TARGET = 4                       # Point toward target
@@ -15,3 +15,7 @@ uint32 target_seq   # target sequence to point to
 float64 lat			    # Latitude to point to
 float64 lon			    # Longitude to point to
 float32 alt			    # Altitude to point to
+
+float32 pitchOffset		# Additional pitch offset to next waypoint
+float32 rollOffset		# Additional roll offset to next waypoint
+float32 yawOffset		# Additional yaw offset to next waypoint

src/modules/commander/commander.cpp

@@ -1176,6 +1176,9 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
 	case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
 	case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
+    case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 

src/modules/mavlink/mavlink_mission.cpp

@@ -1362,6 +1362,13 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 			break;
 
+		case MAV_CMD_DO_SET_ROI_LOCATION:
+			mission_item->nav_cmd = NAV_CMD_DO_SET_ROI_LOCATION;
+			mission_item->params[4] = mavlink_mission_item->x;
+			mission_item->params[5] = mavlink_mission_item->y;
+			mission_item->params[6] = mavlink_mission_item->z;
+			break;
+
 		case MAV_CMD_NAV_VTOL_TAKEOFF:
 		case MAV_CMD_NAV_VTOL_LAND:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
@@ -1449,6 +1456,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_DO_VTOL_TRANSITION:
 		case MAV_CMD_NAV_DELAY:
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+		case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
+		case MAV_CMD_DO_SET_ROI_NONE:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
 

src/modules/navigator/mission_block.cpp

@@ -86,6 +86,9 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_DO_MOUNT_CONFIGURE:
 	case NAV_CMD_DO_MOUNT_CONTROL:
 	case NAV_CMD_DO_SET_ROI:
+	case NAV_CMD_DO_SET_ROI_LOCATION:
+	case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
+	case NAV_CMD_DO_SET_ROI_NONE:
 	case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 	case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case NAV_CMD_SET_CAMERA_MODE:

src/modules/navigator/mission_feasibility_checker.cpp

@@ -268,6 +268,9 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&

src/modules/navigator/navigation.h

@@ -74,6 +74,9 @@ enum NAV_CMD {
 	NAV_CMD_DO_SET_HOME = 179,
 	NAV_CMD_DO_SET_SERVO = 183,
 	NAV_CMD_DO_LAND_START = 189,
+	NAV_CMD_DO_SET_ROI_LOCATION = 195,
+	NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196,
+	NAV_CMD_DO_SET_ROI_NONE = 197,
 	NAV_CMD_DO_SET_ROI = 201,
 	NAV_CMD_DO_DIGICAM_CONTROL = 203,
 	NAV_CMD_DO_MOUNT_CONFIGURE = 204,

src/modules/navigator/navigator_main.cpp

@@ -525,9 +525,33 @@ Navigator::task_main()
 				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI
-				   || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI) {
+				   || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI
+				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION
+				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
+				   || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) {
 				_vroi = {};
-				_vroi.mode = cmd.param1;
+
+				switch (cmd.command) {
+				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
+				case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
+					_vroi.mode = cmd.param1;
+					break;
+
+				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
+					_vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION;
+					break;
+
+				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
+					_vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT;
+					_vroi.pitchOffset = cmd.param5;
+					_vroi.rollOffset = cmd.param6;
+					_vroi.yawOffset = cmd.param7;
+					break;
+
+				case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
+					_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
+					break;
+				}
 
 				switch (_vroi.mode) {
 				case vehicle_command_s::VEHICLE_ROI_NONE: