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.

libraries/AC_WPNav/AC_WPNav.cpp

@@ -414,6 +414,33 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
 }
 
+/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
+///     used to reset the position just before takeoff
+///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
+void AC_WPNav::shift_wp_origin_to_current_pos()
+{
+    // return immediately if vehicle is not at the origin
+    if (_track_desired > 0.0f) {
+        return;
+    }
+
+    // get current and target locations
+    const Vector3f curr_pos = _inav.get_position();
+    const Vector3f pos_target = _pos_control.get_pos_target();
+
+    // calculate difference between current position and target
+    Vector3f pos_diff = curr_pos - pos_target;
+
+    // shift origin and destination
+    _origin += pos_diff;
+    _destination += pos_diff;
+
+    // move pos controller target and disable feed forward
+    _pos_control.set_pos_target(curr_pos);
+    _pos_control.freeze_ff_xy();
+    _pos_control.freeze_ff_z();
+}
+
 /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
 void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
 {

libraries/AC_WPNav/AC_WPNav.h

@@ -135,6 +135,11 @@ public:
     /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
     void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
 
+    /// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
+    ///     used to reset the position just before takeoff
+    ///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
+    void shift_wp_origin_to_current_pos();
+
     /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
     ///		results placed in stopping_position vector
     void get_wp_stopping_point_xy(Vector3f& stopping_point) const;