Support for RTL and Delay mission commands

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| 

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:

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;
 

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

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

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,

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