Plane: RTL with rally point does not track terrain

[tridge]

when using RTL with a rally point with high points in the terrain between current location and the rally point, with TERRAIN_FOLLOW=1 the plane sometimes does not climb over the terrain as it should. As yet it is unclear under what conditions it fails.

Reproduced in 4.4.1 stable in SITL with the following log:
http://uav.tridgell.net/tmp/00000013.BIN

[LanoAstur]

Start RTL about 12.8km from the rally point, with an altitude of 600m relative to the take-off point. The rally point has an altitude of 60m relative to the take-off point.

At about 6.5km from the RTL point there is a crest of about 480m (approx.) relative to the take-off point. Despite having TERRAIN_LOOKAHD=3000 the aircraft reaches a height of 390m relative to the take-off point. It ends up impacting against the hillside.

I will have repeated this simulation if not hundreds of times, then dozens of times with the same result (despite making a few parameter changes).

As you can see the TECS.dh is absolutely stable until the impact occurs.

https://drive.google.com/file/d/1mR49H5uNLJC1Li3lOswvuhu6G7S22_oT/view?usp=sharing

https://drive.google.com/file/d/1s6gyq6rWoUXxI_Wrg9Nuab76yP2PRNus/view?usp=sharing

[tridge]

@LanoAstur many thanks for reporting this! autotest that reproduces the bug and a fix is here:
#25165