Add hysteresis to max height above ground failsafe logic

Add hysteresis to max height above ground failsafe logic #31931