Copter: fix takeoff drift if vehicle is not in origin

[chobitsfan]

replace #17132

@lthall suggest change the implementation of shift_wp_origin_to_current_pos() to

// get current and target locations
const Vector3f& curr_pos = _inav.get_position();

// shift origin and destination horizontally
_origin.x = curr_pos.x;
_origin.y = curr_pos.y;
_destination.x = curr_pos.x;
_destination.y = curr_pos.y;

// move pos controller to the current position
_pos_control.set_pos_target(curr_pos); 

It makes it just the same as shift_wp_origin_and_destination_to_current_pos_xy(). Maybe we should just use it
flight test log using ext nav
5 2021-4-29 AM 10-12-26.zip
4 2021-4-29 AM 10-10-10.zip

[rmackay9]

I wonder what happens if we have significant barometer drift...

[rmackay9]

I'm pretty sure we will see issues with the altitude changing due to baro drift but I think this is an improvement so merged!

[chobitsfan]

I wonder what happens if we have significant barometer drift...

Hi @rmackay9 Thank you. I will try to do more test and check this issue