Terrain following fix

[joe-bfd]

Solved Problem

When using jerk limited/acceleration based input position controllers I found that terrain following was not functional. Terrain following z position setpoints were ignored.

Solution

Only set position setpoint to current position if z position setpoint is not already set

[dagar]

Trivial whitespace thing is making astyle angry. https://github.com/PX4/PX4-Autopilot/actions/runs/5674219445/job/15377357389?pr=21885

[joe-bfd]

Updated. Will check the output a little later to make sure It's all green lit

[joe-bfd]

Hey guys, any update on this PR? I can rebase it onto latest master if we want

[dagar]

It wasn't exactly clear to me why this is the right fix, could we do it more explicitly so it's obvious?

Are you interested in terrain hold or the full terrain following? Could it apply to only the current default manual position control mode or is it actually needed across all altitude and position control modes?

Ping me on slack if you want to discuss directly.

[julianoes]

Is this still work in progress or can we close it?

[bresch]

The issue with that patch is that FlightTaskAltitude is now producing the altitude setpoint in all cases, meaning that locking won't be as smooth as when done using the smooth vel variant.
For this, we might need to rewrite the terrain following/hold logic in FlightTaskAltitudeSmoothVel. It might be a good opportunity to review it and extract it in a class.

[AlexKlimaj]

@bresch maybe we want to make _terrain_hold a public variable of FlightTaskManualAltitude, then FlightTaskManualAltitudeSmoothVel can check before it does _position_setpoint(2) = _smoothing.getCurrentPosition();

[bresch]

replaced by #22294