Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: pos control updated during pivots

Browse Source
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

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