From 158f7944fd5176398f1157dbb073d9ea902dec2c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 15 Mar 2024 14:10:41 +0900 Subject: [PATCH] AC_WPNav: allow target position on track to stop advancing --- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index be5c7ac978..318bccc1a5 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -482,7 +482,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) const float track_error = _pos_control.get_pos_error_cm().dot(track_direction); const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction); // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation. - track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); + track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.0f, 1.0f); } // Use vel_scaler_dt to slow down the trajectory time