Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: stop I term build-up when motors at max

Browse Source
This commit is contained in:
Randy Mackay 2014-04-22 22:33:48 +09:00
parent 05c59c757c
commit e565ee6d33
1 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -589,9 +589,7 @@ void AC_PosControl::rate_to_accel_xy(float dt)
{
const Vector3f &vel_curr = _inav.get_velocity(); // current velocity in cm/s
float accel_total; // total acceleration in cm/s/s
// reset accel limit flag
_limit.accel_xy = false;
float lat_i, lon_i;
// reset last velocity if this controller has just been engaged or dt is zero
if (dt == 0.0) {
@ -611,10 +609,21 @@ void AC_PosControl::rate_to_accel_xy(float dt)
_vel_error.x = _vel_target.x - vel_curr.x;
_vel_error.y = _vel_target.y - vel_curr.y;
// get current i term
lat_i = _pid_rate_lat.get_integrator();
lon_i = _pid_rate_lon.get_integrator();
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lat_i>0&&_vel_error.x<0)||(lat_i<0&&_vel_error.x>0))) {
lat_i = _pid_rate_lat.get_i(_vel_error.x, dt);
}
if ((!_limit.accel_xy && !_motors.limit.throttle_upper) || ((lon_i>0&&_vel_error.y<0)||(lon_i<0&&_vel_error.y>0))) {
lon_i = _pid_rate_lat.get_i(_vel_error.y, dt);
}
// combine feed forward accel with PID output from velocity error
// To-Do: check accel limit flag before adding I term
_accel_target.x += _pid_rate_lat.get_pid(_vel_error.x, dt);
_accel_target.y += _pid_rate_lon.get_pid(_vel_error.y, dt);
_accel_target.x += _pid_rate_lat.get_p(_vel_error.x) + lat_i + _pid_rate_lat.get_d(_vel_error.x, dt);
_accel_target.y += _pid_rate_lon.get_p(_vel_error.y) + lon_i + _pid_rate_lon.get_d(_vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
// To-Do: move this check down to the accel_to_lean_angle method?
@ -623,6 +632,9 @@ void AC_PosControl::rate_to_accel_xy(float dt)
_accel_target.x = POSCONTROL_ACCEL_XY_MAX * _accel_target.x/accel_total;
_accel_target.y = POSCONTROL_ACCEL_XY_MAX * _accel_target.y/accel_total;
_limit.accel_xy = true; // unused
} else {
// reset accel limit flag
_limit.accel_xy = false;
}
}