Add maximum height above ground (HAGL) check to all Navigator modes

[bresch]

Solved Problem

The HAGL navigator failsafe I added in #23501 should be used in all navigator modes.

Solution

Centralize the check instead of adding it to each mode separately.

Changelog Entry

For release notes:

Add maximum height above ground (HAGL) check to all Navigator modes
New parameter: -
Documentation: -

Test coverage

• SITL tests

[sfuhrer]

Agree that we should aim for a generic solution and not have them custom for RTL/Takeoff. Some comments from testing in SITL:

• the reporting of the HAGL failsafe is a bit rough. E.g. it says it will switch to Hold when it already is in Hold, and it doesn't apply the timeout
  
• doesn't seem to work with MC Orbit
• RTL wih OF only: why does it 3 times switch into RTL before then going to Land?
  

[sfuhrer]

Looks good to me now, the extension to not have the delay when switching to Hold seems correct. For my other comments:

doesn't seem to work with MC Orbit

Okay that is expected and not easy to change, as the Orbit doesn't use the triplet at all

RTL wih OF only: why does it 3 times switch into RTL before then going to Land?

That was actually the GCS sending the command 3 times.