From 9c15dcb2065b27a604eb67ce6870c5191cdb464e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 27 Jun 2023 13:11:07 +0900 Subject: [PATCH] AR_PosControl: limit and zero velocity I-term zero in forward-back direction limit in lateral direction --- libraries/APM_Control/AR_PosControl.cpp | 15 +++++++++++++-- libraries/APM_Control/AR_PosControl.h | 1 - 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index b4eb88e6a5..6e64514014 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -157,12 +157,23 @@ void AR_PosControl::update(float dt) _vel_target.y = vel_3d_cms.y * 0.01; } + // calculate limit vector based on steering limits + Vector2f steering_limit_vec; + if (_atc.steering_limit_left()) { + steering_limit_vec = AP::ahrs().body_to_earth2D(Vector2f{0, _reversed ? 1.0f : -1.0f}); + } else if (_atc.steering_limit_right()) { + steering_limit_vec = AP::ahrs().body_to_earth2D(Vector2f{0, _reversed ? -1.0f : 1.0f}); + } + // calculate desired acceleration - // To-Do: fixup _limit_vel used below - _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), dt, _limit_vel); + _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), dt, steering_limit_vec); if (_accel_desired_valid) { _accel_target += _accel_desired; } + // velocity controller I-term zeroed in forward-back direction + const Vector2f lat_vec_ef = AP::ahrs().body_to_earth2D(Vector2f{0, 1}); + const Vector2f vel_i = _pid_vel.get_i().projected(lat_vec_ef); + _pid_vel.set_integrator(vel_i); // convert desired acceleration to desired forward-back speed, desired lateral speed and desired turn rate diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h index c02cd8b13f..d06d852fc4 100644 --- a/libraries/APM_Control/AR_PosControl.h +++ b/libraries/APM_Control/AR_PosControl.h @@ -98,7 +98,6 @@ private: float _lat_accel_max; // lateral acceleration maximum in m/s/s float _jerk_max; // maximum jerk in m/s/s/s (used for both forward and lateral input shaping) float _turn_radius; // vehicle turn radius in meters - Vector2f _limit_vel; // To-Do: explain what this is // position and velocity targets Vector2p _pos_target; // position target as an offset (in meters) from the EKF origin