implement MAV_CMD_DO_LAND_START

msg/vehicle_command.msg

@@ -32,6 +32,7 @@ uint32 VEHICLE_CMD_DO_REPEAT_RELAY = 182		# Cycle a relay on and off for a desir
 uint32 VEHICLE_CMD_DO_SET_SERVO = 183			# Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_REPEAT_SERVO = 184		# Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| 
 uint32 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185		# Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| 
+uint32 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty|  */ 
 uint32 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */ 
 uint32 VEHICLE_CMD_DO_REPOSITION = 192
 uint32 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193

src/modules/commander/commander.cpp

@@ -1175,6 +1175,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
+	case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
 	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 	case vehicle_command_s::VEHICLE_CMD_LOGGING_START:

src/modules/mavlink/mavlink_mission.cpp

@@ -982,6 +982,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_DO_CHANGE_SPEED:
 		case MAV_CMD_DO_SET_SERVO:
+		case MAV_CMD_DO_LAND_START:
 		case MAV_CMD_DO_DIGICAM_CONTROL:
 		case MAV_CMD_DO_MOUNT_CONFIGURE:
 		case MAV_CMD_DO_MOUNT_CONTROL:
@@ -1045,6 +1046,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 		case NAV_CMD_DO_CHANGE_SPEED:
 		case NAV_CMD_DO_SET_SERVO:
+		case NAV_CMD_DO_LAND_START:
 		case NAV_CMD_DO_DIGICAM_CONTROL:
 		case NAV_CMD_IMAGE_START_CAPTURE:
 		case NAV_CMD_IMAGE_STOP_CAPTURE:

src/modules/navigator/mission.cpp

@@ -270,6 +270,33 @@ bool Mission::set_current_offboard_mission_index(unsigned index)
 	return false;
 }
 
+unsigned
+Mission::find_offboard_land_start()
+{
+	/* find the first MAV_CMD_DO_LAND_START and return the index
+	 *  return -1 if not found
+	 *
+	 * TODO: implement full spec and find closest landing point geographically
+	 */
+
+	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+
+	for (size_t i = 0; i < _offboard_mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return -1;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
 void
 Mission::update_onboard_mission()
 {

src/modules/navigator/mission.h

@@ -94,6 +94,8 @@ public:
 	
 	bool set_current_offboard_mission_index(unsigned index);
 
+	unsigned find_offboard_land_start();
+
 private:
 	/**
 	 * Update onboard mission topic

src/modules/navigator/mission_block.cpp

@@ -103,6 +103,7 @@ MissionBlock::is_mission_item_reached()
 		case NAV_CMD_LOITER_UNLIMITED:
 			return false;
 
+		case NAV_CMD_DO_LAND_START:
 		case NAV_CMD_DO_DIGICAM_CONTROL:
 		case NAV_CMD_IMAGE_START_CAPTURE:
 		case NAV_CMD_IMAGE_STOP_CAPTURE:
@@ -432,6 +433,11 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		return;
 	}
 
+	// NAV_CMD_DO_LAND_START is only a marker
+	if (item->nav_cmd == NAV_CMD_DO_LAND_START) {
+		return;
+	}
+
 	if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
 		PX4_INFO("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
@@ -470,6 +476,7 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
 	if (item->nav_cmd == NAV_CMD_DO_JUMP ||
 		item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED ||
 		item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
+		item->nav_cmd == NAV_CMD_DO_LAND_START ||
 		item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
 		item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE ||
 		item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE ||

src/modules/navigator/mission_feasibility_checker.cpp

@@ -250,8 +250,9 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
 			missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
 			missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
-			missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
 			missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
+			missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
+			missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
 			missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
 			missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
 			missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&

src/modules/navigator/navigation.h

@@ -69,6 +69,7 @@ enum NAV_CMD {
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,
+	NAV_CMD_DO_LAND_START=189,
 	NAV_CMD_DO_SET_ROI=201,
 	NAV_CMD_DO_DIGICAM_CONTROL=203,
 	NAV_CMD_DO_MOUNT_CONFIGURE=204,

src/modules/navigator/navigator_main.cpp

@@ -492,6 +492,24 @@ Navigator::task_main()
 				rep->current.valid = true;
 				rep->next.valid = false;
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
+
+				/* find NAV_CMD_DO_LAND_START in the mission and
+				 * use MAV_CMD_MISSION_START to start the mission there
+				 */
+				unsigned land_start = _mission.find_offboard_land_start();
+				if (land_start != -1) {
+					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_MISSION_START;
+					vcmd.param1 = land_start;
+					vcmd.param2 = 0;
+
+					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 &&