Fixed bug with lookahead point calculation; implemented telemetry reporting of next waypoint