plane: Allow current waypoint to be set correctly outside Auto mode.

Patch for issue https://github.com/diydrones/ardupilot/issues/620