The modified statements are intended to
1. Set a new Z position setpoint that is equivalent to the current distance
to ground setpoint,
and
2. Set a new distance to ground setpoint that is equivalent to the current Z
position setpoint.
They are only called in terrain hold mode, when activating/deactivating the
holding (typically when coming to a stop and when starting to move again).
The setpoints take the current control error into account, but because the
control error is added, not subtracted, the result is that the new setpoint
is 2 times the control error off from the old setpoint, instead of being
at the same spot as the old setpoint.
This commit temporarily breaks direct horizontal thrust setpoint execution
which is used by FlightTaskManualAltitude. This is necessary to allow for
PositionControl cleanup namely calculating the whole velocity PID in one
Vector3f formula.
Having this in a separate commit is useful since it reduces indentation
of a bigger code block.
Morten Fyhn Amundsen