Support for waypoint speed
Support for waypoint formation (and fixes)