fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

New ROI commands implementation

Browse Source
This commit is contained in:
DonLakeFlyer 2018-01-22 11:49:36 -08:00 committed by Lorenz Meier
parent ae52f74e78
commit 342509b3ab
8 changed files with 55 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_command.msg
View File

@ -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

6
msg/vehicle_roi.msg
View File

@ -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

3
src/modules/commander/commander.cpp
View File

@ -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;

9
src/modules/mavlink/mavlink_mission.cpp
View File

@ -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;

3
src/modules/navigator/mission_block.cpp
View File

@ -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:

3
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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 &&

3
src/modules/navigator/navigation.h
View File

@ -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,

28
src/modules/navigator/navigator_main.cpp
View File

@ -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: