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

AR_PosControl: limit and zero velocity I-term 

zero in forward-back direction
limit in lateral direction
This commit is contained in:
Randy Mackay 2023-06-27 13:11:07 +09:00
parent 6f52eaf9a7
commit 9c15dcb206
2 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/APM_Control/AR_PosControl.cpp
View File

@ -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

1
libraries/APM_Control/AR_PosControl.h
View File

@ -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