Support rally landing points

[sfuhrer]

This PR is a rebased and slightly modified version of #7743 of @bkueng to enable landings at Rally Points during an RTL.

Describe problem solved by the proposed pull request
When a RTL is triggered, the vehicle will land at the closest Rally point or home it that's closer. If a mission landing is planned (applies mainly for VTOL or FW), then the DO_LAND_START marker replaces Home, meaning that it will check if that one is closer than any rally point, fly to it and then continue the landing procedure.

Test data / coverage
SITL tested.

Additional context
This is a first, basic, implementation of Rally points. There is especially some discussion and follow up work needed on when it should land at the closest Rally point, or really come all the way back to home. E.g. if the RTL was triggered by the user or due to RC loss, the expected behavior is that it comes to home. Battery empty RTL's make sense at the closes Rally point.

Also, if flying a VTOL and a malfunction of the FW system is detected (either automatically or by the user) and it thus is in hover mode, landing at the closes Rally point is crucial as probably the distance to home is too large.

EDIT:
I've made some changes:

• nothing changes for RTL_TYPE 0,1,2 (so all that were there before this PR)
• safe landing points are now only considered if RTL_TYPE=3 ("Return via direct way to whatever is closest: home, mission lading or safe point"
• if DO_LAND_START marker is available, then the distance to this one is compared to home/safe landing points if RTL_TYPE=3

[bkueng]

Thanks for bringing this back.
I think we should just merge this and then work out more elaborate behaviors.

@hamishwillee fyi

[julianoes]

Jenkins is unhappy:

../../src/modules/navigator/rtl.cpp: In member function 'virtual void RTL::on_activation()':
../../src/modules/navigator/rtl.cpp:132:45: error: 'closest_safe_point.mission_safe_point_s::lat' may be used uninitialized in this function [-Werror=maybe-uninitialized]
    _destination.lat = closest_safe_point.lat;

[hamishwillee]

Assuming this goes in, is there any further work to be done to make it work with QGC - ie will upload magically work for PX4 in the same way as for ArduPilot? [Note, I think the upload probably already works, it's just that the points aren't used]

And just to confirm, the handling is currently always to RTL to closest "safe point" (rally point or home)?

[sfuhrer]

@bkueng I added a new commit to allow for mission landings if the RTL chooses home to land at. Would be cool if you could review that as well.

[dagar]

I'm wondering if the the closest safe landing point should be updated in RTL::on_inactive() on change. This would be close to how mission is handled now and avoids having to read from the sdcard when switching modes. The position controller will be in limbo waiting for a position setpoint triplet.

[hamishwillee]

-4: RTL_SAFEPOINT_LAND vehicle returns to closest safe point or home, and if it's home
then it uses the planned mission landing (like if RTL_TYPE=1)

What does this mean? A planned mission landing is by definition wherever the mission says it should be, not home.

Also, this appears to be for MC. What happens for FW and VTOL for these settings?

[LorenzMeier]

The review comment commits should not be in upstream later - make sure to squash all commits on merging or do an interactive rebase.

Commits should represent blocks of functionality, not the history of what you did.

[mrivi]

Flight tested the PR. Behaved as expected. Here's a log https://review.px4.io/plot_app?log=ccd0d34d-f426-444d-ac9f-55526c1be1a9

[bkueng]

Good to merge once CI passes.

[hamishwillee]

@mrivi Can you answer the questions #12696 (comment) and #12696 (comment) - specifically, which parameters are still relative to home and which are relevant to the rally point.
I would assume this would work best if all the parameters are relative to the rally point, including its altitude.

Or to put it another way, how do you know it worked as expected when there is no definition of the expectation?

[hamishwillee]

Also, do these apply to all vehicles, or just MC?

[sfuhrer]

Also, do these apply to all vehicles, or just MC?

Yes, also applies to VTOL and FW. With RTL_TYPE=3 (aka the new "find closest" landing point in RTL), it will chose whether home, land (resp. the DO_LAND_START waypoint if set) or a safe landing point is closest. If it's the DO_LAND_START one it will do the mission landing starting at this point, otherwise a "normal" RTL-landing (with that I mean transitioning to MC above landing point for VTOL and descend vertically, and loiter for a FW). Let me know if that makes it clear or if you need some further help (I guess you want to update the user guide?)

[sfuhrer]

The current behaviour for return landing is all in terms of home position. If you're using a rally point, are the return altitude and other parameters in the cone relative to the target rally point.?

The return altitude is relative to whatever altitude homer or the safe lading point is set to (resp. to the altitude of the wp at the DO_LAND_START marker).
@mrivi did you check the cone feature for RTL? It should make no difference if on landing at home/mission landing/rally point.

[sfuhrer]

@sfuhrer @bresch Re

could you please check that the RTL cone landing feature still works as expected?

as per #12696 (comment), please let me know what "as expected" means for all the parameters.

So the cone RTL is not yet described anywhere? From what I understand by looking at the implementation is that the user can set the RTL cone angle. The vehicle will then return on the altitude set by the cone, which is a function of horizontal distance to landing point (the alt will though at least be RTL_DESCEND_ALT). @bresch @MaEtUgR please correct me if I'm wrong.

[hamishwillee]

Thanks @sfuhrer . Yes, that is exactly the clarification I needed - all altitudes are relative to the destination, not home.

Docs for the cone are here: http://docs.px4.io/master/en/flight_modes/return.html#return_altitude

Note, I find it a little odd that RTL_TYPE=0 doesn't use rally points as well.

[hamishwillee]

I updated RTL param docs in #13302. Please review.