Mate you are opening a can of worms with that question. I am currently looking at ways to improve this. It is actually a very tricky problem because we never know what our required altitude will be from second to second when using a lidar for terrain following.
The simplified answer is that we compare the vertical error with the vertical stopping distance at maximum climb rate. We scale these to the horizontal error and stopping distance. We then apply 3 axis limit to the target velocities.
This basically means as your vertical error approaches the vertical stopping distance your horizontal velocity reduces to zero.
You need to read the code to get the details.
So all we do then is move the vertical target up and down based on the current terrain altitude.
I suspect all this is not very clear but it was a bit of a monster of a problem at the time. There are cases that don’t work well like when you have high vertical accelerations and slow climb rates resulting in very short stopping distances.
While I suspect I didn’t explain how it works I hope this give you a bit of an idea or a starting point when you look at the code.