From 1c6606cc58a3bdf6068a68673dedfb51954fd1c3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Aug 2014 18:13:59 +0900 Subject: [PATCH] AC_WPNav: resolve twitch when passing spline waypoints The target positions target velocity was being reset to zero as we passed through a spline waypoint. --- libraries/AC_WPNav/AC_WPNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index b7a7b2cfbc..16f46d23db 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -695,7 +695,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V // before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination _spline_origin_vel = (_destination - _origin); _spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment? - _spline_vel_scaler = 0.0f; // To-Do: this should be set based on speed at end of prev straight segment? + _spline_vel_scaler = _pos_control.get_vel_target().length(); // start velocity target from current target velocity }else{ // previous segment is splined, vehicle will fly through origin // we can use the previous segment's destination velocity as this segment's origin velocity @@ -707,7 +707,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V }else{ _spline_time = 0.0f; } - _spline_vel_scaler = 0.0f; + // Note: we leave _spline_vel_scaler as it was from end of previous segment } }