From 35b76528a3cde907c5cbd94e51ac1171b085e393 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Nov 2023 11:03:48 +0900 Subject: [PATCH] AR_WPNav: pos control updated during pivots --- libraries/AR_WPNav/AR_WPNav.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index afb6f4956c..1ab6c817d8 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -505,6 +505,10 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_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 ¤t_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)