use closest waypoint in space
Merge branch 'drive-rewrite' of github.com:froj/nastya into drive-rewrite