Copter: RTL ensures heading is back to initial heading

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.