diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index b48dc4bafe..1ea206423f 100644 --- a/msg/vehicle_command.msg +++ b/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 diff --git a/msg/vehicle_roi.msg b/msg/vehicle_roi.msg index ed2fabe8b9..d1e32ddd98 100644 --- a/msg/vehicle_roi.msg +++ b/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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 780efbcc3e..dddc2c2a82 100644 --- a/src/modules/commander/commander.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index f25f5fd0b8..5f2d5b4a0f 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index ff87359161..5ed462b059 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/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: diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index c44b78d082..4cd29ea7ab 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/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 && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index b6d947b36e..9f43d995b2 100644 --- a/src/modules/navigator/navigation.h +++ b/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, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3aad446c6f..1a8b76dca0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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: