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 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
                        )

This issue was also pointed out here: Auterion/px4-ros2-interface-lib#50

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 more thoroughly compliant with MAVLink's VTOL_TRANSITION_HEADING

Test coverage

• It is used in cpp with the ros2 uxrce-dds interface here: https://github.com/JacopoPan/aerial-autonomy-stack
• A basic Python-based SITL example is in https://github.com/JacopoPan/px4-fw-aerobatics/tree/main
• It works well in SITL on top of 1.14, 1.15, and 1.16
• I tested in hardware applying the change to 1.14.3 on a 6X autopilot and on multiple quadplanes (triggering the takeoff both from QGC missions as well as directly with cpp/ros2/xrce-dds vehicle_commands)

[hamishwillee]

@sfuhrer I've assigned to you as "in your area of interest".

[sfuhrer]

That overall doesn't look like a bad idea, and thanks for the clean write up!
vtol_takeff.cpp is not used as part of a mission, but is intended to be used as a standalone command. You can set the transition altitude and the loiter point it should go to after the transition, and it does the transition already in direction of this point. I guess you then do not pass along a loiter point, or what's the motivation behind setting the transition direction independently?

[hamishwillee]

@JacopoPan If this goes in, it will require documentation.

I am assuming that this will work in both mission and via a mavlink command, and if so, the places to update would be:

• https://docs.px4.io/main/en/flight_modes_vtol/mission.html#mission-commands
• https://docs.px4.io/main/en/flight_modes_vtol/mission.html#mission-takeoff
• https://docs.px4.io/main/en/msg_docs/VehicleCommand.html#vehiclecommand-uorb-message (uORB topic docs should be updated in this PR.

@sfuhrer I think we're missing a VTOL takeoff page in https://docs.px4.io/main/en/frames_vtol/ - right? Can you help with that. I would expect that to note behaviour of MAV_CMD_NAV_VTOL_TAKEOFF when sent as a command.

My understanding is that VTOL always take off in MC mode and then transition. Currently they will ignore param4 (yaw angle) but will respect the other parameters. After this change, they will respect param4 and yaw to face the specified direction after taking off (to what height). Is that right?

[JacopoPan]

That overall doesn't look like a bad idea, and thanks for the clean write up! vtol_takeff.cpp is not used as part of a mission, but is intended to be used as a standalone command. You can set the transition altitude and the loiter point it should go to after the transition, and it does the transition already in direction of this point. I guess you then do not pass along a loiter point, or what's the motivation behind setting the transition direction independently?

Yes, my use case was indeed to climb in quad mode to a given altitude, transition towards a specified heading and then send a new loiter point (with a different altitude and specified radius) as soon as the transition is completed.

The motivation was being able to do parallel transitions but with different target loiters for multiple vehicles as well as being slightly more compliant to the definition of https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_TAKEOFF

It has been a while since I implemented this (originally on 1.14), so I'll also run a few more tests in the next few days

[JacopoPan]

I have done a few SITL experiments.

The current implementation of VehicleCommand 84 specifies altitude (of the transition) and lat/lon of the loiter point sent immediately after transition.
The transition yaw is computed between the takeoff point and the center of the loiter.

From what I see in simulation, radius and altitude of the loiter are NAV_LOITER_RAD and VTO_LOITER_ALT.
(This is not ideal, for example, in the case NAV_LOITER_RAD is greater than the distance between the takeoff point and the center of the loiter: the vehicle does a u-turn just after transition.)

The proposed modification should not affect any of the behaviors currently possible but would add the ability to explicitly specify a transition direction.

[sfuhrer]

Thanks, looks clean and should not interfere with anything existing.

@hamishwillee it doesn't affect Mission, only the VTOL_Takeoff mode. That one doesn't have a button to be triggered in QGC, only in proprietary ground stations or over companion computer.

[sfuhrer]

@JacopoPan there is a CI failure - can you run make format and push the change?

[hamishwillee]

@hamishwillee it doesn't affect Mission, only the VTOL_Takeoff mode. That one doesn't have a button to be triggered in QGC, only in proprietary ground stations or over companion computer.

@sfuhrer Thanks, but just to be clear,

1. it still needs to be documented since I can set it via MAVLink or any mavlink SDK - and if we don't document it then it will never get a UI.
2. I think the right place would be a VTOL takeoff page which could explain all the options. Alternatively it could be in https://docs.px4.io/main/en/flight_modes_vtol/ as a comment "Takeoff mode is like X, Y, Z, but you can also use MAV_CMD_NAV_VTOL_TAKEOFF
3. Either way, it would be good to confirm some general things about VTOL takeoff - such as if it ignores a normal takeoff command? Will it take off in FW mode? And so on.

[JacopoPan]

@JacopoPan there is a CI failure - can you run make format and push the change?

done, sorry about that

[JacopoPan]

@hamishwillee it doesn't affect Mission, only the VTOL_Takeoff mode. That one doesn't have a button to be triggered in QGC, only in proprietary ground stations or over companion computer.

@sfuhrer Thanks, but just to be clear,

1. it still needs to be documented since I can set it via MAVLink or any mavlink SDK - and if we don't document it then it will never get a UI.

2. I think the right place would be a  VTOL takeoff page which could explain all the options. Alternatively it could be in https://docs.px4.io/main/en/flight_modes_vtol/ as a comment "Takeoff mode is like X, Y, Z, but you can also use [MAV_CMD_NAV_VTOL_TAKEOFF](https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_VTOL_TAKEOFF)

3. Either way, it would be good to confirm some general things about VTOL takeoff - such as if it ignores a normal takeoff command? Will it take off in FW mode? And so on.

If the will/desire is to add docs through this same PR, I can certainly start a draft

[hamishwillee]

If the will/desire is to add docs through this same PR, I can certainly start a draft

@JacopoPan IMO the only thing that you really should update in this PR would be the docs in https://github.com/PX4/PX4-Autopilot/blob/main/msg/versioned/VehicleCommand.msg - since I think that indicates the parameter is unused. At least check it.

Any other docs don't have to be in this PR but it should happen in a relatively timely manner. They would also include an update to the release note

Thanks!