i propose to remove this part of code. Does it calculate an inner product or something else (i can't understand the math behind it) ? But it seems to me, it will return TRUE even if the rotorcraft is not on the desired wp when it cross over the normal line to the path(from_wp to next_wp).  Flight plan may switch to the next wp, when there is a deviation from the path line (e.g. disturbance like wind) if the rotorcraft cross over this normal line? If this part will be removed , i propose to change the name of the function 'nav_approachong_from' since the aim will not be approaching from a waypoint, but it will be holding on a waypoint for a desired time. (e.g. 'nav_wp_successed')