AP_L1_Control: fixed waypoint approach logic

this could cause the RTL approach to not break off onto the circle
correctly