update to remembering current nav waypoint

Additional testing found that, without a do_nav_wp() call inside the
set_mode() to AUTO, the navigation and controls weren't re-initializing
correctly.