drive_waypoint fix init seq
Merge branch 'drive-rewrite' of github.com:froj/nastya into drive-rewrite