AC_WPNav: record next destination

This allows AC_WPNav_OA to completely restore the path on deactivation

libraries/AC_WPNav/AC_WPNav.cpp

@@ -350,6 +350,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
 
     _this_leg_is_spline = false;
     _scurve_next_leg.init();
+    _next_destination.zero();       // clear next destination
     _flags.fast_waypoint = false;   // default waypoint back to slow
     _flags.reached_destination = false;
 
@@ -380,6 +381,9 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain
     // next destination provided so fast waypoint
     _flags.fast_waypoint = true;
 
+    // record next destination
+    _next_destination = destination;
+
     return true;
 }
 

libraries/AC_WPNav/AC_WPNav.h

@@ -273,6 +273,7 @@ protected:
     float       _wp_desired_speed_xy_cms;   // desired wp speed in cm/sec
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from ekf origin
     Vector3f    _destination;           // target destination in cm from ekf origin
+    Vector3f    _next_destination;      // next target destination in cm from ekf origin
     float       _track_scalar_dt;       // time compression multiplier to slow the progress along the track
     float       _offset_vel;            // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
     float       _offset_accel;          // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain