Fixed errors in waypoint control math.