From bf2b0f890fa22e3fd5ff685fb59ac5669550189d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 1 Dec 2023 17:10:34 +0900 Subject: [PATCH] AC_WPNav: record next destination This allows AC_WPNav_OA to completely restore the path on deactivation --- libraries/AC_WPNav/AC_WPNav.cpp | 4 ++++ libraries/AC_WPNav/AC_WPNav.h | 1 + 2 files changed, 5 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 116e85911c..03200a463e 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/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; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index e4563f7ce6..c060498263 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/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