move to Land WP's altitude instead of staying at current altitude

[ThomasRigi]

@dagar @sfuhrer @RomanBapst here's the PR from our slack conversation about moving to the altitude defined in the Land waypoint. (https://px4.slack.com/archives/C0V533X4N/p1639150436168000)

Describe problem solved by this pull request
For MC and VTOL drones that enforce landing in MC mode the altitude of the Land WP is ignored, which is confusing when planning a mission. For example, in the attached mission I'd expect the drone to fly diagonally from WP No. 5 at 25m to the Land WP at 5m, and then descend vertically for the last descent.
short vtol mission ZH.zip
Without this PR, the drone flies towards the landing spot at 25m altitude and then descends vertically which is not my intent when planning a mission like this.

Additionally, if the takeoff spot is higher than the landing spot with the current implementation the drone descends too slowly for too long as the slow land speed is by default set relative to home. This PR removes this false slow land speed problem.

Describe your solution
Don't ignore the VTOL&Land and Land waypoints' altitude.

Test data / coverage
SITL gazebo standard VTOL with slight variations of the above mission:

VTOL in MC mode, rel alt, normal land WP: https://logs.px4.io/plot_app?log=9ab242e3-5af7-4edf-a36c-1dd01fc9e41d
mission: VTOL takeoff, 2WP at 25m rel, BT, 1 WP at 25m rel, Land WP at 5m rel.

VTOL in MC mode, abs alt, normal land WP: https://logs.px4.io/plot_app?log=04398968-2b24-4b80-9874-e6531522c8dd
mission: VTOL takeoff, 2WP at 25m rel, BT, 1 WP at 25m rel, Land WP at 533m AMSL.

VTOL in FW mode, rel alt, normal land WP: https://logs.px4.io/plot_app?log=54320484-ebfe-4348-a8ff-6307db4cc888
mission: VTOL takeoff, 3WP at 25m rel, Land WP at 5m rel.

VTOL in FW mode, abs alt, normal land WP: https://logs.px4.io/plot_app?log=38905b9a-6431-41b8-9b7a-d8d8b1fb06bb
mission: VTOL takeoff, 3WP at 25m rel, Land WP at 533m AMSL.

VTOL in FW mode, rel alt, VTOL&Land WP: https://logs.px4.io/plot_app?log=a3cac434-e8ad-4ba8-b52b-e9945e8d06a5
mission: VTOL takeoff, 3WP at 25m rel, VTOL&Land WP at 5m rel.

VTOL in FW mode, abs alt, VTOL&Land WP: https://logs.px4.io/plot_app?log=a49b6308-d13c-425e-a3ba-4c0380d39a47
mission: VTOL takeoff, 3WP at 25m rel, VTOL&Land WP at 533m AMSL.

SITL gazebo Iris:
MC, rel alt: https://logs.px4.io/plot_app?log=6d029bcd-835f-4d7e-959d-38e6e62a1b06
mission: 2 normal WPs at 25m rel, Land WP at 5m rel.

MC, abs alt: https://logs.px4.io/plot_app?log=63924e11-7c38-408b-a296-e3fc14ed95b6
mission: 2 normal WPs at 25m rel, Land WP at 533m AMSL.

Additional context

• To my understanding this PR does not affect FW planes. I assume that NAV_CMD_VTOL_LAND is reserved for VTOLs and do_need_move_to_land() always returns false for FW planes, so the deleted blocks would never be activated on FW planes .

• This PR can cause hard landings if you don't plan your Land altitude correctly. If the Land WP is below ground level the drone will hit the ground at normal descent speed, same as if you had put a normal WP below ground - which is something you shouldn't do in the first place.
  

• You can get around my problems by setting an additional normal WP at the desired Land position at the desired lower altitude, but I don't like having to put these "useless" waypoints.

[ThomasRigi]

Answering a comment from @sfuhrer on slack:

The vehicle should itself be able to detect proximity to the ground (distance sensor or offline map) and decelerate to a slower descent speed once below some threshold

We could probably handle this inside FlightTaskAuto::_updateTrajConstraints() using FlightTask::_evaluateDistanceToGround() or similar. I'd be in favour of only limiting the down speed if _dist_to_bottom is valid because otherwise you end up with a too slow descent again if you want to land somewhere below home.

[sfuhrer]

So with that change we the Alt field of the Land WP would mean: "Fly to altitude X and then start descending vertically until landed". Looking at the mavlink docs this is against the mavlink specs of CMD_NAV_LAND and VTOL_LAND if I get it correctly.

Your interpretation is to use it rather as an "approach altitude" (already present for VTOL_LAND, though the intent there is to use it as transition altitude). While I agree that this interpretation would be more useful that specifying the groundlevel at the land site (it's not by coincidence that it so far wasn't used on the px4 side), for me personally I'm not sure if it's worth to change the mavlink specs around it (add field for it), and certainly not to diverge from them.

What's the main goal you're trying to achieve? Reducing the amount of hover time of VTOLs but not flying close to the landing spot in fixed-wing mode? While you're diagonal landing approach ofc does the trick in reducing the flight time, I'm not so convinced concerning safety. If the landing altitude (your definition) is only set a couple of m too low, then the vehicle would not only hit the ground with a high vz speed but also a considerable horizontal speed. And your comparison to normal WP doesn't really hold for me here: normal WP are maybe around 30-100m, whereas here it would be down to something like 5-10m to really reduce the flight time (assuming the VTOL transitions at 20-30m).

You can get around my problems by setting an additional normal WP at the desired Land position at the desired lower altitude, but I don't like having to put these "useless" waypoints.

Beside already possible without mavlink/PX4/QGC changes, this has the extra advantage of more predictability though in my eyes.

Additionally, if the takeoff spot is higher than the landing spot with the current implementation the drone descends too slowly for too long as the slow land speed is by default set relative to home. This PR removes this false slow land speed problem.

I would solve this differently, something like

We could probably handle this inside FlightTaskAuto::_updateTrajConstraints() using FlightTask::_evaluateDistanceToGround() or similar. I'd be in favour of only limiting the down speed if _dist_to_bottom is valid because otherwise you end up with a too slow descent again if you want to land somewhere below home.

We would need to give this some extra thoughts though (purely thinking about the "start descend slower from this altitude on" - do we also need some better logic for distance-sensor-less vehicles? Or should we use the "Land Altitude" for that actually (start decelerating 10m above the set altitude)?

[ThomasRigi]

Thanks for your response. You raise some valid points, and I have to admit that I haven't read the mavlink specifications with enough attention. I agree that it's not worth to diverge from mavlink, and that it would be additional work to have the mavlink item changed.

Basically I have three problems with the current way:

• as you said we want to reduce MC hover time
• we want to limit the slow descent phase
• we're working a more advanced 3D geofence on the companion computer, one that follows the altitude of your specified waypoints. So if the altitude of the Land WP is ignored we need to handle it as a special case and need to be really careful when planning the missions.

There's the work-around of adding an additional "useless" WP just at the landing spot at the correct altitude. So the reason of this PR is that I don't like to have to add "useless" WPs in every mission and ensure they haven't been forgotten by mistake:

Beside already possible without mavlink/PX4/QGC changes, this has the extra advantage of more predictability though in my eyes.
For me it's rather unpredictable now that it uses current altitude to go to the Land WP. What I agree though is that if you set the additional WP it's 100% clear what will happen. I guess that's what you meant with being more predictable.

I know that my approach has its own set of drawbacks. If they're outweighing the advantages for the larger part of the community I'm completely ok with closing this PR. - The slow land descent phase still needs improvement though! If my vision of using the Land altitude as the starting altitude does not go through then I'm in favour of using it to start the slow landing as you propose: start the slow descend at Land WP's altitude + MPC_LAND_ALT1. (unless there is a ground distance sensor available)

[sfuhrer]

The slow land descent phase still needs improvement though! If my vision of using the Land altitude as the starting altitude does not go through then I'm in favour of using it to start the slow landing as you propose: start the slow descend at Land WP's altitude + MPC_LAND_ALT1. (unless there is a ground distance sensor available)

Yes - what do other's think about that, e.g. @MaEtUgR @bresch ?

What I think is important is to pushing for removing the Altitude field of the Land WP from the QGC planning if we can't come up with consuming it on the flight controller side. It's not crucially important as the last years have proven where it was like that, but certainly results in confusion if it's there and not used.

For me it's rather unpredictable now that it uses current altitude to go to the Land WP. What I agree though is that if you set the additional WP it's 100% clear what will happen. I guess that's what you meant with being more predictable.

I personally disagree here, for most applications (thinking more in MC than VTOL now) it's completely fine and expected to stay on the current altitude and start to descend only upon reaching the Land waypoint's lat/lon. But I may also be biased from using it that way for several years now.