align towards next waypoint before MC to FW transition

src/modules/navigator/mission.cpp

@@ -412,6 +412,8 @@ Mission::set_mission_items()
 
 	/*********************************** handle mission item *********************************************/
 
+	bool do_issue_command = true;
+
 	/* handle position mission items */
 	if (item_contains_position(&_mission_item)) {
 
@@ -466,18 +468,50 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
-		/* set current position setpoint from mission item */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-
 	/* handle non-position mission items such as commands */
 	} else {
 
-		// XXX: before issuing command, check if we need to align for transition first
+		/* turn towards next waypoint before MC to FW transition */
-		issue_command(&_mission_item);
+		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+				&& _work_item_type != WORK_ITEM_TYPE_ALIGN
+				&& _navigator->get_vstatus()->is_rotary_wing
+				&& !_navigator->get_vstatus()->condition_landed
+				&& has_next_position_item) {
+
+			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+
+			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.lat = _navigator->get_global_position()->lat;
+			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.altitude = _navigator->get_global_position()->alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
+			_mission_item.yaw = get_bearing_to_next_waypoint(
+				_navigator->get_global_position()->lat,
+				_navigator->get_global_position()->lon,
+				mission_item_next_position.lat,
+				mission_item_next_position.lon);
+
+			do_issue_command = false;
+		}
+
+		/* yaw is aligned now */
+		if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
+			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+		}
 
 	}
 
-	/*********************************** clean up and set next *********************************************/
+	/*********************************** set setpoints and check next *********************************************/
+
+	/* set current position setpoint from mission item (is protected agains 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) */
+	if (do_issue_command) {
+		issue_command(&_mission_item);
+	}
 
 	/* set current work item type */
 	_work_item_type = new_work_item_type;
@@ -513,7 +547,8 @@ 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,
+		_distance_current_previous = get_distance_to_next_waypoint(
+				pos_sp_triplet->current.lat,
 				pos_sp_triplet->current.lon,
 				pos_sp_triplet->previous.lat,
 				pos_sp_triplet->previous.lon);
@@ -588,8 +623,9 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 void
 Mission::heading_sp_update()
 {
-	if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
+	/* we don't want to be yawing during takeoff or align */
-		/* we don't want to be yawing during takeoff */
+	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			|| _work_item_type == WORK_ITEM_TYPE_ALIGN) {
 		return;
 	}
 
@@ -601,11 +637,6 @@ Mission::heading_sp_update()
 		return;
 	}
 
-	/* Don't change heading for takeoff waypoints, the ground may be near */
-	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-		return;
-	}
-
 	/* set yaw angle for the waypoint iff a loiter time has been specified */
 	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
 		_mission_item.yaw = _on_arrival_yaw;

src/modules/navigator/mission_block.cpp

@@ -105,10 +105,10 @@ MissionBlock::is_mission_item_reached()
 		case NAV_CMD_LOITER_UNLIMITED:
 			return false;
 
-		case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
+		case NAV_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
-		case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
+		case NAV_CMD_DO_VTOL_TRANSITION:
 			// XXX: we should wait on command ACK or status change instead
-			// currently we just wait so the command be processed
+			// currently we just wait so the command can be processed
 			if (hrt_absolute_time() - _action_start > 1000) {
 				return true;
 
@@ -185,6 +185,7 @@ MissionBlock::is_mission_item_reached()
 
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
+			PX4_WARN("yaw err: %.3f", (float)yaw_err);
 
 			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
@@ -251,7 +252,7 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		return;
 	}
 
-	warnx("forwarding command.\n");
+	warnx("forwarding command %d\n", item->nav_cmd);
 	struct vehicle_command_s cmd = {};
 	mission_item_to_vehicle_command(item, &cmd);
 	_action_start = hrt_absolute_time();
@@ -268,8 +269,8 @@ bool
 MissionBlock::item_contains_position(const struct mission_item_s *item)
 {
 	// XXX: maybe extend that check onto item properties
-	if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL ||
+	if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
-			item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
+			item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
 		return false;
 	}