FW Nav Auto Landing fixes

[breadoven]

Closes #9916. Also fixes 2 other issues:

1. Landing detection disarm doesn't trigger during Failsafe RTH - fix resulted in another Auto land State being added NAV_STATE_FW_LANDING_FINISHED.
2. WP mission Landing not triggered correctly in RTH Landing state for multi missions > 1

WP RTH Landing state code also cleaned up.

Tests OK in HITL.

[b14ckyy]

Awesome, will test the fixes tomorrow. Thanks a lot.

[Jetrell]

Ran some quick tests this afternoon.. It now appears to shut off the motor and disarm after a mission LAND WP.
Sorry, I ran out of time to test the multi-mission fix.

Has anyone else noticed their planes will sometimes linger at nav_rth_altitude when loitering around the safehome for a wind direction check. Instead of descending to nav_rth_home_altitude ? Or Landing Approach altitude. I happen to have them both at 40meters.
I'm not sure if its related to nav_rth_use_linear_descent being enabled. I can't seem to pin down the reason.

@b14ckyy nav_fw_loiter_radius = 2500 . I only mention it because I've seen it happen rather constantly. While it doesn't occur when there is no landing safehome.

[b14ckyy]

@Jetrell I had that once when my Loiter radius was accidently set to 50cm instead of 50m. It took sometimes a few minutes until the plane actually considered the loiter to be entered to switch into wind check or start descent. After the loiter radius was not too small anymore I never had a problem again.

I was not able to test yesterday. Started to rain all afternoon.

[breadoven]

It fails to detect you've arrived at the RTH home point because it simply looks to see if you're within 1.1 * nav_fw_loiter_radius from the home point to trigger arrival. So if nav_fw_loiter_radius has been set much smaller than the plane can actually achieve it never arrives, made worse by the fact the plane starts to go into a loiter quite some distance (around 4 * nav_fw_loiter_radius ) before the home point itself is reached.

I think there's perhaps a better way of doing this which just looks to see if the plane has started to loiter around the home/hold point. This might mean it arrives too early but since this would only affect RTH which ends up in some kind of loiter at the home point regardless it shouldn't make any difference. I'll test and see.

[DzikuVx]

@breadoven please resolve conflicts. And shall we merge or wait?

[b14ckyy]

when solved and it fixes the disarm issue, imho its ready to merge. Optimizations and enhancements can be done in the future. The disarm bug is more important to be fixed.

[breadoven]

I tested this in HITL pretty thoroughly and it worked as expected so I don't see why it shouldn't work in real flight just the same given it's just logic changes which HITL should simulate exactly. Should be good to merge on that basis and also the testing by @Jetrell.