Use VehicleCommand heading for VTOL transition

Author: JacopoPan

src/modules/navigator/navigator_main.cpp

@@ -652,6 +652,12 @@ void Navigator::run()
 
 				_vtol_takeoff.setTransitionAltitudeAbsolute(cmd.param7);
 
+				float epsilon = 1e-6f;
+				if (std::fabs(cmd.param2 - 3.0f) < epsilon) { // Specified transition direction
+					PX4_WARN("[Navigator::run] setTransitionDirecton from cmd.param4");
+					_vtol_takeoff.setTransitionDirection(cmd.param4);
+				}
+
 				// after the transition the vehicle will establish on a loiter at this position
 				_vtol_takeoff.setLoiterLocation(matrix::Vector2d(cmd.param5, cmd.param6));
 

src/modules/navigator/vtol_takeoff.cpp

@@ -71,8 +71,13 @@ VtolTakeoff::on_active()
 				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-				_mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat,
-							    _mission_item.lon, _loiter_location(0), _loiter_location(1)));
+				if (std::isnan(_transition_direction_deg)) {
+					_mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat,
+							_navigator->get_home_position()->lon, _loiter_location(0), _loiter_location(1)));
+				} else {
+					PX4_WARN("[VtolTakeoff] Transition direction from Command");
+					_mission_item.yaw = wrap_pi(math::radians(_transition_direction_deg));
+				}
 				_mission_item.force_heading = true;
 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 				pos_sp_triplet->current.cruising_speed = -1.f;

src/modules/navigator/vtol_takeoff.h

@@ -55,6 +55,7 @@ class VtolTakeoff : public MissionBlock, public ModuleParams
 	void on_active() override;
 
 	void setTransitionAltitudeAbsolute(const float alt_amsl) {_transition_alt_amsl = alt_amsl; }
+	void setTransitionDirection(const float tran_bear) {_transition_direction_deg = tran_bear; }
 
 	void setLoiterLocation(matrix::Vector2d loiter_location) { _loiter_location = loiter_location; }
 	void setLoiterHeight(const float height_m) { _loiter_height = height_m; }
@@ -73,6 +74,7 @@ class VtolTakeoff : public MissionBlock, public ModuleParams
 	float _takeoff_alt_msl{0.f};
 	matrix::Vector2d _loiter_location;
 	float _loiter_height{0};
+	float _transition_direction_deg{NAN};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::VTO_LOITER_ALT>) _param_loiter_alt