Survey - fix of survey tracking problem on steep slopes

[Claudio-Chies]

Solved Problem

There was an existing problem that surveys on steep terrain had bad tracking behaviour, whereby the multicopter went far offtrack before continuing to the next waypoint.

Solution

I changed the state-logic to use 3D coordinates instead of 2d. whereby the main contributer to the problem was that the elevation of the closest point between previous and current waypoint was set to use the elevation of the current waypoint
_closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2));

Changelog Entry

For release notes:

Bugfix of the survey offtrack behaviour

Test coverage

Tested in SITL

Context

Before:

After:

[MaEtUgR]

Thanks for looking into this and coming up with a fix! 🙏
The 3D case not working nicely sounds familiar 😬
#22331

[MaEtUgR]

Thanks for the propsed fix! I think this generally makes sense, the 2D calculations are not general enough. I only found some details to improve.

[MaEtUgR]

Thanks, all fixed 👍
I ran a quick simulation test with a pretty degenerate mission. It could be better but I don't see any significant devitation from the line path.
https://logs.px4.io/plot_app?log=58b8f0bf-104e-4a43-8a13-75f928adb827