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

AR_WPNav: pos control updated during pivots

This commit is contained in:
Randy Mackay 2023-11-24 11:03:48 +09:00
parent d88dfa4428
commit 03c50db07e
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -505,6 +505,10 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
{
_cross_track_error = calc_crosstrack_error(current_loc);
// update position controller
_pos_control.set_reversed(_reversed);
_pos_control.update(dt);
// handle pivot turns
if (_pivot.active()) {
// decelerate to zero
@ -512,14 +516,11 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float 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;
return;
} else {
_desired_speed_limited = _pos_control.get_desired_speed();
_desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
_desired_lat_accel = _pos_control.get_desired_lat_accel();
}
_pos_control.set_reversed(_reversed);
_pos_control.update(dt);
_desired_speed_limited = _pos_control.get_desired_speed();
_desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
_desired_lat_accel = _pos_control.get_desired_lat_accel();
}
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)