Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: allow target position on track to stop advancing

This commit is contained in:
Randy Mackay 2024-03-15 14:10:41 +09:00 committed by Andrew Tridgell
parent d3125fe8aa
commit 158f7944fd
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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