_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);