AR_WPNav: pos control updated during pivots

ArduPilot · Nov 24, 2023 · 52c66ef · 52c66ef
1 parent 515cabb
commit 52c66ef
Showing 1 changed file with 8 additions and 7 deletions.
diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -505,21 +505,22 @@ 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
         _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;
-        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)