Navigator: VTOL: set heading of transition of a VTOL_TAKEOFF to the V…

…TOL_TAKEOFF waypoint (and enforce heading)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/navigator/mission.cpp

@@ -758,6 +758,26 @@ Mission::set_mission_items()
 				    !_navigator->get_land_detected()->landed) {
 
 					/* disable weathervane before front transition for allowing yaw to align */
+					pos_sp_triplet->current.allow_weather_vane = false;
+
+					/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
+					_mission_item.yaw = get_bearing_to_next_waypoint(
+								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+								    _mission_item.lat, _mission_item.lon);
+
+					_mission_item.force_heading = true;
+
+					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
+
+					/* set position setpoint to current while aligning */
+					_mission_item.lat = _navigator->get_global_position()->lat;
+					_mission_item.lon = _navigator->get_global_position()->lon;
+				}
+
+				/* heading is aligned now, prepare transition */
+				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
+				    _work_item_type == WORK_ITEM_TYPE_ALIGN &&
+				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
 				    !_navigator->get_land_detected()->landed) {
 
 					/* check if the vtol_takeoff waypoint is on top of us */