AC_WPNav: add shift_wp_origin_to_current_pos for takeoff

This shifts the origin to the vehicle's current position and should be
called just before take-off to ensure there are no sudden roll or pitch
moves on takeoff.
Copter: shift pos targets to current location before takeoff