AC_WPNav: fix takeoff drift if vehicle is not in origin

[chobitsfan]

Both dbe1c55 and comments in shift_wp_origin_to_current_pos() states this function shifts the origin and destination so the origin starts at the current position. But its implementation seems incorrect. Everything was fine before, because it returns early if vehicle is not in origin

ardupilot/libraries/AC_WPNav/AC_WPNav.cpp

Lines 282 to 285 in d66101d

	// return immediately if vehicle is not at the origin
	if (_track_desired > 0.0f) {
	return;
	}

But _track_desired assignment is removed when #15896 merged, it will always be zero and it never return early. This may cause a horizontal drift when vehicle takeoff.
20 2021-4-8 am 09-41-18.zip

This PR fix this problem by implementing this function as its comments state. Test flights using motion capture system and ext nav.
25 2021-4-9 PM 03-43-38.zip
26 2021-4-9 PM 03-45-20.zip

related to #16478)

[rmackay9]

@chobitsfan, thanks so much for this!

[lthall]

This is just used during take off and can probably be done clearer like this:

// 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);

[lthall]

This has a little kink as we define the S-Curve based on the distance between the origin and destination at the initialization. This can change as our origin shifts. The fundamental problem behind this is we should not be using the navigation controller to take off.

To make it smooth we either need to recalculate the S-Curve on every loop, climb to a different height based on our origin height change, twitch at the start or twitch at the end. I think the twitch at the start or have the destination change height is the best trade off.

[chobitsfan]

Hi @lthall Thank you

The fundamental problem behind this is we should not be using the navigation controller to take off.

I am sorry but I did not get it. Did you mean we should not use wpnav or pos_control?

[lthall]

Hi @lthall Thank you

The fundamental problem behind this is we should not be using the navigation controller to take off.

I am sorry but I did not get it. Did you mean we should not use wpnav or pos_control?

Sorry that isn't relevant to your PR. My statement is referring to the fact we can't navigate on the ground and we should be running a different strategy to break contact with the ground and then initialize the position controller and navigation after that.

[chobitsfan]

replaced by #17294

Hi @lthall Would you mind taking a look at #17294? Thank you very much for helping