Use VehicleCommand specified heading for VTOL transition

[JacopoPan]

Solved Problem

I wanted to use VEHICLE_CMD_NAV_VTOL_TAKEOFF's param2 to specify heading as in the MAVLINK Common Message Set in this example here but, in my understanding, PX4's vtol_takeoff.cpp defaults to the direction of the next waypoint in the mission

self.send_vehicle_command(
                        84, # VEHICLE_CMD_NAV_VTOL_TAKEOFF
                        param2=3.0, # takeoff mode 3: specified (custom PX4 only)
                        param4=yaw, # unused heading https://docs.px4.io/main/en/flight_modes_vtol/mission.html
                        # MAV_CMD_NAV_VTOL_TAKEOFF.param2 is ignored, heading to the next wp is used for the transition heading.
                        # Custom PX4 fix in navigator_main.cpp and vtol_takeoff.cpp circumvents this
                        param5=self.home_lat,
                        param6=self.home_lon,
                        param7=self.home_alt + alt,
                        conf=0
                        )

Solution

I added some naive logic to use the heading specified in param4 if and only if param2 is set to 3, it could be extended to be fully compliant with VTOL_TRANSITION_HEADING

Test coverage

• A complete Python-based SITL example is in https://github.com/JacopoPan/px4-fw-aerobatics/tree/main
• I tested successfully in hardware applying the change to 1.14.3 on a 6X autopilot and sending the vehicle command with uDDS/ROS2 in C++ (will update with the flight logs soon)