Resolves issue in which vehicle would get stuck at RTL_ALT_FINAL if
failsafe occured.
Also clear out pilot acceleration from loiter controller if failsafe
occurs during the final descent.
This fixes the issue in which a short RTL_LOIT_TIME could cause the
vehicle's heading to be caught between it's heading when it arrived at
home and the initial armed heading. With this fix it now waits above
home until the timer has run out AND the heading is within 2degrees of
the initial armed heading.
Note that an RTL command executed in AUTO mode will also disarm when it
lands and pilot's throttle is put to zero and no further commands will
be executed. This is normally not an issue because missions generally
end with an RTL (instead of having the RTL in the middle) and a work
around is available in that the LAND command could be used instead of
RTL.
Andrew Chapman
Randy Mackay
Andrew Chapman
Jonathan Challinger