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

AR_WPNav: allow target position on track to stop advancing

This commit is contained in:
Randy Mackay 2024-03-15 14:12:17 +09:00 committed by Andrew Tridgell
parent 158f7944fd
commit 381fb42023
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/AR_WPNav/AR_WPNav.cpp
View File

@ -426,7 +426,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
float track_velocity = curr_vel_NED.xy().dot(track_direction); float track_velocity = curr_vel_NED.xy().dot(track_direction);
// set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation. // set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation.
const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f; const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f;
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, time_scaler_dt_max); track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.0f, time_scaler_dt_max);
} }
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f; float track_scaler_tc = 1.0f;