fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:23: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/commander/commander.cpp
+3 -1
View File
@@ -1069,6 +1069,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
break;
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to RTL which ends the mission */
if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1183,7 +1184,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
/* ignore commands that handled in low prio loop */
case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
/* ignore commands that handled in low prio loop */
break;
default:
src/modules/mavlink/mavlink_mission.cpp
+2
View File
@@ -1035,6 +1035,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case NAV_CMD_ROI:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_VTOL_TRANSITION:
case MAV_CMD_NAV_DELAY:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
break;
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 &&