aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AR_WPNav: pivot turns stop before turning

Browse Source
This commit is contained in:
Randy Mackay 2022-01-04 20:37:35 +09:00
parent e0a628bb8c
commit 729dfee01c
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -499,13 +499,12 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
{ {
// handle pivot turns // handle pivot turns
if (_pivot.active()) { if (_pivot.active()) {
_cross_track_error = calc_crosstrack_error(current_loc);
_desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd();
_desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt);
_desired_lat_accel = 0.0f;
// decelerate to zero // decelerate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd();
_desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0;
_desired_lat_accel = 0.0f;
_cross_track_error = calc_crosstrack_error(current_loc);
return; return;
} }