PositionSmoothing: fix corner altitude bug

[MaEtUgR]

Solved Problem

We got multiple reports that the altitude profile between two waypoints is not a diagonal line but changes altitude much quicker when leaving the previous waypoint and getting much flatter after that.

During a round corner the L1 distance calculation
was only done in 2D and the z-axis coordinate
was directly coming from the next waypoint.
This lead to an unpredictable altitude profile
between two waypoints. Sometimes almost all
altitude difference was already covered during
the turn instead of going diagonally.

Fixes #{Github issue ID}

Solution

• Make the entire L1 distance and intersection calculation 3D
• Refactor ...

Changelog Entry

Bugfix: Multicopter altitude change sometimes not linear between two waypoints

Test coverage

Simulation testing procedure:

1. Command to test: PX4_SIM_SPEED_FACTOR=100 make px4_sitl jmavsim
2. Load plan file for jMAVsim location through QGC: rounded_altitude_change.plan

I still need to verify the 2D only case with no valid altitude works as expected.

Context

Before: https://logs.px4.io/3d?log=45c8f43e-32e7-4c7f-ab30-975e1c8aa32b

After: https://logs.px4.io/3d?log=17dd1c3e-bd5e-46b0-abad-3bba6c517e2f

[MaEtUgR]

This fix was just real-world tested and worked like expected.

[tstastny]

@bresch ready for final pass :)

[saengphet]

Hi @MaEtUgR This bug occurs only in main, v1.14, or also occurs in v1.13.3 too? thank you.

[MaEtUgR]

occurs only in main, v1.14, or also occurs in v1.13.3 too? thank you.

@saengphet I actually just minutes ago tried to find this out but it's not so clear to me yet. The part I changed does the same thing since rounded turns were introduced but possibly it was not a problem before other changes. I'd need to dig more. 1.14 is for sure the same and I'll backport my fix once it's merged.

[bresch]

Cool, that's a pretty straightforward fix: 2D -> 3D

[bresch]

Made a SITL test and had a fly-away
https://logs.px4.io/plot_app?log=0d5aae66-628f-4e0f-8a28-426e2b85bcdb

[MaEtUgR]

had a fly-away

Is it reproducible? Can you share the plan file that produces it?

[MaEtUgR]

Same plan works fine for me. On to debugging 👀
https://logs.px4.io/plot_app?log=e1242eb4-e62c-412f-9fb0-0da1cc9ea859

[bresch]

I was able to repeat the issue on main, so it's unrelated to this PR

[tstastny]

I was able to repeat the issue on main, so it's unrelated to this PR

oof - alrighty - can you make an issue so we track this newfound bug @bresch ?

[bresch]

Just for completeness, the "flyaway" i had in simulation was due to a landing approach that I did set for a VTOL plane simulation earlier.

[MaEtUgR]

I checked the 2D case which is the one where the altitude is not valid:

PX4-Autopilot/src/lib/motion_planning/PositionSmoothing.cpp

Line 215 in 19b681c

else if (xy_target_valid) {

Following findings: I don't see how it can ever be the path taken if not by my test code because QGC requires every waypoint to have an altitude, navigator puts altitudes to the waypoints and the flight task puts at least the current altitude to every waypoint. In case there was a valid code path my change would do the same calculation as before iff the z coordinate is really the same altitude for the current position and waypoint. I expect this case to get obsolete in the next refactor.