implement MAV_CMD_NAV_LOITER_TO_ALT and general mission cleanup

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -14,6 +14,9 @@ then
 	# FW uses L1 distance for acceptance radius 
 	#  set a smaller NAV_ACC_RAD for vertical acceptance distance
 	param set NAV_ACC_RAD 10
+	
+	param set MIS_LTRMIN_ALT 25
+	param set MIS_TAKEOFF_ALT 25
 
 	param set PE_VELNE_NOISE 0.3
 	param set PE_VELD_NOISE 0.35

src/modules/commander/commander.cpp

@@ -1064,23 +1064,23 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
+	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
-	case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
 	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
 	case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
-	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
 	case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
 	case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
+	case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
 		/* ignore commands that handled in low prio loop */

src/modules/mavlink/mavlink_mission.cpp

@@ -767,24 +767,83 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 int
 MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
 {
-	/* only support global waypoints for now */
-	switch (mavlink_mission_item->frame) {
-	case MAV_FRAME_GLOBAL:
-		mission_item->lat = (double)mavlink_mission_item->x;
-		mission_item->lon = (double)mavlink_mission_item->y;
-		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = false;
-		break;
+	if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
+		mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+
+		// only support global waypoints for now
 
-	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
 		mission_item->lat = (double)mavlink_mission_item->x;
 		mission_item->lon = (double)mavlink_mission_item->y;
 		mission_item->altitude = mavlink_mission_item->z;
-		mission_item->altitude_is_relative = true;
-		break;
 
-	case MAV_FRAME_MISSION:
-		// This is a mission item with no coordinate
+		if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) {
+			mission_item->altitude_is_relative = false;
+		} else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+			mission_item->altitude_is_relative = true;
+		}
+
+		mission_item->time_inside = 0.0f;
+
+		switch (mavlink_mission_item->command) {
+		case MAV_CMD_NAV_WAYPOINT:
+			mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+			mission_item->time_inside = mavlink_mission_item->param1;
+			mission_item->acceptance_radius = mavlink_mission_item->param2;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_UNLIM:
+			mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+			mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_TIME:
+			mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+			mission_item->time_inside = mavlink_mission_item->param1;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
+			mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
+			break;
+
+		case MAV_CMD_NAV_LAND:
+			mission_item->nav_cmd = NAV_CMD_LAND;
+			// TODO: abort alt param1
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_TAKEOFF:
+			mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+			mission_item->pitch_min = mavlink_mission_item->param1;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		case MAV_CMD_NAV_LOITER_TO_ALT:
+			mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false;
+			mission_item->loiter_radius = fabsf(mavlink_mission_item->param2);
+			mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+			mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false;
+			break;
+
+		case MAV_CMD_NAV_VTOL_TAKEOFF:
+		case MAV_CMD_NAV_VTOL_LAND:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
+			break;
+
+		default:
+			mission_item->nav_cmd = NAV_CMD_INVALID;
+			return MAV_MISSION_UNSUPPORTED;
+		}
+
+		mission_item->frame = mavlink_mission_item->frame;
+
+	} else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
+
+		// this is a mission item with no coordinates
+
 		mission_item->params[0] = mavlink_mission_item->param1;
 		mission_item->params[1] = mavlink_mission_item->param2;
 		mission_item->params[2] = mavlink_mission_item->param3;
@@ -792,50 +851,44 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->params[4] = mavlink_mission_item->x;
 		mission_item->params[5] = mavlink_mission_item->y;
 		mission_item->params[6] = mavlink_mission_item->z;
-		break;
-
-	default:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-	}
-
-	mission_item->frame = mavlink_mission_item->frame;
-
-	switch (mavlink_mission_item->command) {
-	case MAV_CMD_NAV_TAKEOFF:
-		mission_item->pitch_min = mavlink_mission_item->param1;
-		break;
-
-	case MAV_CMD_DO_JUMP:
-		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
-		mission_item->do_jump_current_count = 0;
-		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
-		break;
-
-	default:
-		mission_item->acceptance_radius = mavlink_mission_item->param2;
-		mission_item->time_inside = mavlink_mission_item->param1;
-		break;
-	}
 
-	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
-	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
-	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
+		switch (mavlink_mission_item->command) {
+		case MAV_CMD_DO_JUMP:
+			mission_item->nav_cmd = NAV_CMD_DO_JUMP;
+			mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+			mission_item->do_jump_current_count = 0;
+			mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
+			break;
+
+		case MAV_CMD_DO_CHANGE_SPEED:
+		case MAV_CMD_DO_SET_SERVO:
+		case MAV_CMD_DO_DIGICAM_CONTROL:
+		case MAV_CMD_DO_MOUNT_CONFIGURE:
+		case MAV_CMD_DO_MOUNT_CONTROL:
+		case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case MAV_CMD_DO_VTOL_TRANSITION:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			break;
+
+		default:
+			mission_item->nav_cmd = NAV_CMD_INVALID;
+			return MAV_MISSION_UNSUPPORTED;
+		}
 
-	/* unsigned, so can't be negative, only check for overflow */
-	if (mavlink_mission_item->command >= NAV_CMD_INVALID) {
-		mission_item->nav_cmd = NAV_CMD_INVALID;
+		mission_item->frame = MAV_FRAME_MISSION;
 
 	} else {
-		mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}
 
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	// mission_item->index = mavlink_mission_item->seq;
-	mission_item->origin = ORIGIN_MAVLINK;
 
 	/* reset DO_JUMP count */
 	mission_item->do_jump_current_count = 0;
 
+	mission_item->origin = ORIGIN_MAVLINK;
+
 	return MAV_MISSION_ACCEPTED;
 }
 
@@ -857,32 +910,82 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		mavlink_mission_item->y = mission_item->params[5];
 		mavlink_mission_item->z = mission_item->params[6];
 
-	// default mappings for regular waypoints
+		switch (mavlink_mission_item->command) {
+		case NAV_CMD_DO_JUMP:
+			mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+			mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+			break;
+
+		case NAV_CMD_DO_CHANGE_SPEED:
+		case NAV_CMD_DO_SET_SERVO:
+		case NAV_CMD_DO_DIGICAM_CONTROL:
+		case NAV_CMD_DO_MOUNT_CONFIGURE:
+		case NAV_CMD_DO_MOUNT_CONTROL:
+		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
+		case NAV_CMD_DO_VTOL_TRANSITION:
+			break;
+
+		default:
+			return ERROR;
+		}
+
 	} else {
+		mavlink_mission_item->param1 = 0.0f;
+		mavlink_mission_item->param2 = 0.0f;
+		mavlink_mission_item->param3 = 0.0f;
+		mavlink_mission_item->param4 = 0.0f;
 		mavlink_mission_item->x = (float)mission_item->lat;
 		mavlink_mission_item->y = (float)mission_item->lon;
 		mavlink_mission_item->z = mission_item->altitude;
 
-		mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
-		mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
-	}
-
-	// specific item handling
-	switch (mission_item->nav_cmd) {
-	case NAV_CMD_TAKEOFF:
-	case NAV_CMD_VTOL_TAKEOFF:
-		mavlink_mission_item->param1 = mission_item->pitch_min;
-		break;
-
-	case NAV_CMD_DO_JUMP:
-		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
-		break;
+		if (mission_item->altitude_is_relative) {
+			mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+		} else {
+			mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+		}
 
-	default:
-		mavlink_mission_item->param2 = mission_item->acceptance_radius;
-		mavlink_mission_item->param1 = mission_item->time_inside;
-		break;
+		switch (mission_item->nav_cmd) {
+		case NAV_CMD_WAYPOINT:
+			mavlink_mission_item->param1 = mission_item->time_inside;
+			mavlink_mission_item->param2 = mission_item->acceptance_radius;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_LOITER_UNLIMITED:
+			mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_LOITER_TIME_LIMIT:
+			mavlink_mission_item->param1 = mission_item->time_inside;
+			mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
+			break;
+
+		case NAV_CMD_LAND:
+			// TODO: param1 abort alt
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_TAKEOFF:
+			mavlink_mission_item->param1 = mission_item->pitch_min;
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		case NAV_CMD_LOITER_TO_ALT:
+			mavlink_mission_item->param1 = mission_item->force_heading;
+			mavlink_mission_item->param2 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
+			mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
+			break;
+
+		case MAV_CMD_NAV_VTOL_TAKEOFF:
+		case MAV_CMD_NAV_VTOL_LAND:
+			mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+			break;
+
+		default:
+			return ERROR;
+		}
 	}
 
 	return OK;

src/modules/navigator/mission.cpp

@@ -348,7 +348,7 @@ Mission::set_mission_items()
 	/* make sure param is up to date */
 	updateParams();
 
-	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
+	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
 	altitude_sp_foh_reset();
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -453,6 +453,7 @@ Mission::set_mission_items()
 
 		/* do takeoff before going to setpoint if needed and not already in takeoff */
 		if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+
 			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
 			/* use current mission item as next position item */
@@ -482,8 +483,9 @@ Mission::set_mission_items()
 					&& _navigator->get_vstatus()->is_rotary_wing
 					&& !_navigator->get_land_detected()->landed
 					&& has_next_position_item) {
+
 				/* check if the vtol_takeoff command is on top of us */
-				if(do_need_move_to_takeoff()){
+				if (do_need_move_to_takeoff()){
 					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
 				} else {
 					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
@@ -516,6 +518,7 @@ Mission::set_mission_items()
 		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 				&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
 				&& !_navigator->get_land_detected()->landed) {
+
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 			/* use current mission item as next position item */
 			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
@@ -537,6 +540,7 @@ Mission::set_mission_items()
 				&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
 				&& !_navigator->get_vstatus()->is_rotary_wing
 				&& !_navigator->get_land_detected()->landed) {
+
 			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 			_mission_item.autocontinue = true;
@@ -547,6 +551,7 @@ Mission::set_mission_items()
 		if (do_need_move_to_land() &&
 				(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
 				 _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 			/* use current mission item as next position item */
@@ -572,6 +577,7 @@ Mission::set_mission_items()
 		/* we just moved to the landing waypoint, now descend */
 		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
 				&& _navigator->get_vstatus()->is_rotary_wing) {
+
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
@@ -607,7 +613,7 @@ Mission::set_mission_items()
 
 	/*********************************** set setpoints and check next *********************************************/
 
-	/* set current position setpoint from mission item (is protected agains non-position items) */
+	/* set current position setpoint from mission item (is protected against non-position items) */
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
 	/* issue command if ready (will do nothing for position mission items) */
@@ -617,7 +623,9 @@ Mission::set_mission_items()
 	_work_item_type = new_work_item_type;
 
 	/* require takeoff after landing or idle */
-	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
+			|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+
 		_need_takeoff = true;
 	}
 
@@ -647,6 +655,7 @@ Mission::set_mission_items()
 
 	/* Save the distance between the current sp and the previous one */
 	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
+
 		_distance_current_previous = get_distance_to_next_waypoint(
 				pos_sp_triplet->current.lat,
 				pos_sp_triplet->current.lon,
@@ -661,6 +670,7 @@ bool
 Mission::do_need_takeoff()
 {
 	if (_navigator->get_vstatus()->is_rotary_wing) {
+
 		float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
 		/* force takeoff if landed (additional protection) */
@@ -674,13 +684,12 @@ Mission::do_need_takeoff()
 
 		/* check if current mission item is one that requires takeoff before */
 		if (_need_takeoff && (
-				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
 				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
+				_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
 
 			_need_takeoff = false;
 			return true;
@@ -693,7 +702,8 @@ Mission::do_need_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
-	if (_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
+	if (_navigator->get_vstatus()->is_rotary_wing
+		&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
 			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -757,7 +767,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 	/* calculate takeoff altitude */
 	float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
 
-	/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+	/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 	if (_navigator->get_land_detected()->landed) {
 		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
 
@@ -866,10 +876,14 @@ Mission::altitude_sp_foh_update()
 	}
 
 	/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
-	 * and the FW controller has a custom landing logic */
+	 * and the FW controller has a custom landing logic
+	 *
+	 * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
+	 * */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND
 			|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 			|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
 			|| !_navigator->is_planned_mission()) {
 		return;
 	}

src/modules/navigator/mission_block.cpp

@@ -75,6 +75,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_actuators{},
 	_actuator_pub(nullptr),
 	_cmd_pub(nullptr),
+	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
 	_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
@@ -98,13 +99,13 @@ MissionBlock::is_mission_item_reached()
 		case NAV_CMD_VTOL_LAND:
 			return _navigator->get_land_detected()->landed;
 
-		/* TODO: count turns */
-		/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
 		case NAV_CMD_IDLE: /* fall through */
 		case NAV_CMD_LOITER_UNLIMITED:
 			return false;
 
 		case NAV_CMD_DO_DIGICAM_CONTROL:
+		case NAV_CMD_DO_MOUNT_CONFIGURE:
+		case NAV_CMD_DO_MOUNT_CONTROL:
 		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 			return true;
 
@@ -115,13 +116,14 @@ MissionBlock::is_mission_item_reached()
 			 */
 			if (hrt_absolute_time() - _action_start > 500000 &&
 					!_navigator->get_vstatus()->in_transition_mode) {
+
 				_action_start = 0;
 				return true;
 			} else {
 				return false;
 			}
 
-		case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
+		case NAV_CMD_DO_CHANGE_SPEED:
 			// XXX not differentiating ground and airspeed yet
 			if (_mission_item.params[1] > 0.0f) {
 				_navigator->set_cruising_speed(_mission_item.params[1]);
@@ -152,14 +154,14 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator->get_home_position()->alt
-			              : _mission_item.altitude;
+					? _mission_item.altitude + _navigator->get_home_position()->alt
+					: _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator->get_global_position()->lat,
-							  _navigator->get_global_position()->lon,
-							  _navigator->get_global_position()->alt,
-				&dist_xy, &dist_z);
+					_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon,
+					_navigator->get_global_position()->alt,
+					&dist_xy, &dist_z);
 
 		if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
 			&& _navigator->get_vstatus()->is_rotary_wing) {
@@ -176,8 +178,7 @@ MissionBlock::is_mission_item_reached()
 			}
 		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
 			(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
-			_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-			_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
+			_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
 			/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
 			 * coordinates with a radius equal to the loiter_radius field. It is not flying
 			 * through the waypoint center.
@@ -186,7 +187,61 @@ MissionBlock::is_mission_item_reached()
 			 */
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
 				&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
 				_waypoint_position_reached = true;
+			} else {
+				_time_first_inside_orbit = 0;
+			}
+
+		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
+			(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
+
+
+			// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
+			//  first check if the altitude setpoint is the mission setpoint
+			struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+
+			if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
+				// check if the initial loiter has been accepted
+				dist = -1.0f;
+				dist_xy = -1.0f;
+				dist_z = -1.0f;
+
+				dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
+								_navigator->get_global_position()->lat,
+								_navigator->get_global_position()->lon,
+								_navigator->get_global_position()->alt,
+								&dist_xy, &dist_z);
+
+				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
+					// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
+					curr_sp->alt = altitude_amsl;
+					_navigator->set_position_setpoint_triplet_updated();
+				}
+
+			} else {
+				if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+					&& dist_z <= _navigator->get_default_acceptance_radius()) {
+
+					_waypoint_position_reached = true;
+
+					// set required yaw from bearing to the next mission item
+					if (_mission_item.force_heading) {
+						struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
+						
+						if (next_sp.valid) {
+							_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+											_navigator->get_global_position()->lon,
+											next_sp.lat, next_sp.lon);
+
+							_waypoint_yaw_reached = false;
+						} else {
+							_waypoint_yaw_reached = true;
+						}
+					}
+				}
 			}
 		} else {
 			/* for normal mission items used their acceptance radius */
@@ -213,8 +268,9 @@ MissionBlock::is_mission_item_reached()
 
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
-		/* TODO: removed takeoff, why? */
-		if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) {
+		if ((_navigator->get_vstatus()->is_rotary_wing
+			|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
+			&& PX4_ISFINITE(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
@@ -242,18 +298,29 @@ MissionBlock::is_mission_item_reached()
 
 		if (_time_first_inside_orbit == 0) {
 			_time_first_inside_orbit = now;
-
-			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
-			// 		(double)_mission_item.time_inside);
-			// }
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+
+			// exit xtrack location
+			if (_mission_item.loiter_exit_xtrack &&
+				(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				 _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
+
+				// reset lat/lon of loiter waypoint so vehicle exits on a tangent
+				struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
+				curr_sp->lat = _navigator->get_global_position()->lat;
+				curr_sp->lon = _navigator->get_global_position()->lon;
+			}
+
 			return true;
 		}
 	}
+
+	// all acceptance criteria must be met in the same iteration
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
 	return false;
 }
 
@@ -298,7 +365,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		PX4_WARN("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
 		memset(&actuators, 0, sizeof(actuators));
-		// params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6
+		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
 		actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
 		actuators.timestamp = hrt_absolute_time();
@@ -332,8 +399,9 @@ 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_REPEAT_SERVO ||
 		item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
+		item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE ||
+		item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
 		item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
 		item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
 		return false;
@@ -349,6 +417,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 
 	if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
 			&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
+
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
 										   _navigator->get_global_position()->lon,
@@ -365,7 +434,6 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		return;
 	}
 
-	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
 	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
@@ -397,8 +465,12 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		}
 		break;
 
+	case NAV_CMD_LOITER_TO_ALT:
+		// initially use current altitude, and switch to mission item altitude once in loiter position
+		sp->alt = math::max(_navigator->get_global_position()->alt, _navigator->get_home_position()->alt + _param_loiter_min_alt.get());
+
+		// no break
 	case NAV_CMD_LOITER_TIME_LIMIT:
-	case NAV_CMD_LOITER_TURN_COUNT:
 	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 		if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
@@ -410,6 +482,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 		break;
 	}
+
+	sp->valid = true;
 }
 
 void
@@ -472,7 +546,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
 
 	} else {
 
-		item->nav_cmd = NAV_CMD_FOLLOW_TARGET;
+		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
 
 		/* use current target position */
 
@@ -605,5 +679,3 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
 }
-
-

src/modules/navigator/mission_block.h

@@ -141,6 +141,7 @@ protected:
 	orb_advert_t    _actuator_pub;
 	orb_advert_t	_cmd_pub;
 
+	control::BlockParamFloat _param_loiter_min_alt;
 	control::BlockParamFloat _param_yaw_timeout;
 	control::BlockParamFloat _param_yaw_err;
 	control::BlockParamInt _param_vtol_wv_land;

src/modules/navigator/mission_feasibility_checker.cpp

@@ -238,17 +238,18 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
 			missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
 			missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
-			/* not yet supported: missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && */
 			missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
 			missionitem.nav_cmd != NAV_CMD_LAND &&
 			missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
-			missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
 			missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
-			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
+			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_DIGICAM_CONTROL &&
+			missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
+			missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
 			missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 			missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
@@ -407,13 +408,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 bool
 MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
-	if( cmd == NAV_CMD_WAYPOINT ||
-		cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	if (cmd == NAV_CMD_WAYPOINT ||
 		cmd == NAV_CMD_LOITER_UNLIMITED ||
-		cmd == NAV_CMD_TAKEOFF ||
+		cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 		cmd == NAV_CMD_LAND ||
-		cmd == NAV_CMD_PATHPLANNING) {
+		cmd == NAV_CMD_TAKEOFF ||
+		cmd == NAV_CMD_LOITER_TO_ALT ||
+		cmd == NAV_CMD_VTOL_TAKEOFF ||
+		cmd == NAV_CMD_VTOL_LAND) {
+
 		return true;
 	} else {
 		return false;

src/modules/navigator/navigation.h

@@ -54,21 +54,19 @@ enum NAV_CMD {
 	NAV_CMD_IDLE = 0,
 	NAV_CMD_WAYPOINT = 16,
 	NAV_CMD_LOITER_UNLIMITED = 17,
-	NAV_CMD_LOITER_TURN_COUNT = 18,
 	NAV_CMD_LOITER_TIME_LIMIT = 19,
-	NAV_CMD_RETURN_TO_LAUNCH = 20,
 	NAV_CMD_LAND = 21,
 	NAV_CMD_TAKEOFF = 22,
-	NAV_CMD_ROI = 80,
-	NAV_CMD_PATHPLANNING = 81,
+	NAV_CMD_LOITER_TO_ALT = 31,
+	NAV_CMD_DO_FOLLOW_REPOSITION = 33,
 	NAV_CMD_VTOL_TAKEOFF = 84,
 	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DO_JUMP = 177,
 	NAV_CMD_DO_CHANGE_SPEED = 178,
 	NAV_CMD_DO_SET_SERVO=183,
-	NAV_CMD_DO_REPEAT_SERVO=184,
-	NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder
 	NAV_CMD_DO_DIGICAM_CONTROL=203,
+	NAV_CMD_DO_MOUNT_CONFIGURE=204,
+	NAV_CMD_DO_MOUNT_CONTROL=205,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST=206,
 	NAV_CMD_DO_VTOL_TRANSITION=3000,
 	NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */
@@ -99,7 +97,8 @@ struct mission_item_s {
 	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
 	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	unsigned nav_cmd;		/**< navigation command					*/
+	bool loiter_exit_xtrack;	/**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
 	float acceptance_radius;	/**< default radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */