From 96458d3184dd5203f4417b1bf1e871b0f70f7eca Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Apr 2017 17:07:19 -0700 Subject: [PATCH] Support for RTL and Delay mission commands --- msg/vehicle_command.msg | 3 ++- src/modules/commander/commander.cpp | 4 +++- src/modules/mavlink/mavlink_mission.cpp | 2 ++ src/modules/navigator/mission_block.cpp | 9 ++++++++- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ src/modules/navigator/navigation.h | 1 + src/modules/navigator/navigator_main.cpp | 10 ++++++++++ 7 files changed, 28 insertions(+), 3 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index c0e79051a2..08850f4d95 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -16,7 +16,8 @@ uint32 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location uint32 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| uint32 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92 # hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| -uint32 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| +uint32 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| uint32 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| uint32 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee7ff05b68..dbbc182808 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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: diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index ef655b24e6..faf2785109 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -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; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2020d6c8b0..01249e4311 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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; } diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 422f2ab49d..ca92078b01 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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 && diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 69c0384b45..21ccf1026f 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -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, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 95a43dfdbe..f53327cc9f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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 &&