fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 21:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

New ROI commands implementation

Browse Source
This commit is contained in:
DonLakeFlyer
2018-01-25 15:27:55 +01:00
committed by Lorenz Meier
parent ae52f74e78
commit 342509b3ab
8 changed files with 55 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+3
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;
src/modules/mavlink/mavlink_mission.cpp
+9
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;
src/modules/navigator/mission_block.cpp
+3
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:
src/modules/navigator/mission_feasibility_checker.cpp
+3
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 &&
src/modules/navigator/navigation.h
+3
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,
src/modules/navigator/navigator_main.cpp
+26 -2
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: