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

Support for RTL and Delay mission commands

Browse Source
This commit is contained in:
Don Gagne
2017-04-18 08:56:01 +02:00
committed by Lorenz Meier
parent 3721fb9d52
commit 96458d3184
7 changed files with 28 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+8 -1
View File
@@ -297,6 +297,11 @@ MissionBlock::is_mission_item_reached()
}
}
} else if (_mission_item.nav_cmd == NAV_CMD_DELAY) {
_waypoint_position_reached = true;
_waypoint_yaw_reached = true;
_time_wp_reached = now;
} else {
/* for normal mission items used their acceptance radius */
float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
@@ -500,7 +505,9 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
item->nav_cmd == NAV_CMD_DO_SET_ROI ||
item->nav_cmd == NAV_CMD_ROI ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION ||
item->nav_cmd == NAV_CMD_DELAY ||
item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
return false;
}
src/modules/navigator/mission_feasibility_checker.cpp
+2
View File
@@ -254,11 +254,13 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
missionitem.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
missionitem.nav_cmd != NAV_CMD_LAND &&
missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
missionitem.nav_cmd != NAV_CMD_DELAY &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
src/modules/navigator/navigation.h
+1
View File
@@ -69,6 +69,7 @@ enum NAV_CMD {
NAV_CMD_ROI = 80,
NAV_CMD_VTOL_TAKEOFF = 84,
NAV_CMD_VTOL_LAND = 85,
NAV_CMD_DELAY = 93,
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_CHANGE_SPEED = 178,
NAV_CMD_DO_SET_SERVO = 183,
src/modules/navigator/navigator_main.cpp
+10
View File
@@ -546,6 +546,16 @@ Navigator::task_main()
(void)orb_unadvertise(pub);
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
vehicle_command_s vcmd = {};
vcmd.target_system = get_vstatus()->system_id;
vcmd.target_component = get_vstatus()->component_id;
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(pub);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (get_mission_result()->valid &&