Navigation enhancements

- Refactor activateWaypoint into another file.  Called select_waypoint_simple(new, prev) which calculates a path between two waypoints
- Move the calculation of the path into the main thread using a flag