From 99fb5a994e458ba5c62bfd62151010f7c9b12b5e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 4 Jul 2024 17:37:24 +1000 Subject: [PATCH] AC_WPNav: correct calculation of predict-accel when zeroing pilot desired accel --- libraries/AC_WPNav/AC_Loiter.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 02e8085877..81bc70dc80 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -33,7 +33,9 @@ public: Vector2f get_pilot_desired_acceleration() const { return Vector2f{_desired_accel.x, _desired_accel.y}; } /// clear pilot desired acceleration - void clear_pilot_desired_acceleration() { _desired_accel.zero(); } + void clear_pilot_desired_acceleration() { + set_pilot_desired_acceleration(0, 0); + } /// get vector to stopping point based on a horizontal position and velocity void get_stopping_point_xy(Vector2f& stopping_point) const;