aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: check Z-axis accel imax can always overpower hover throttle

Browse Source 
This removes the possibility of the vehicle constantly climbing if the hover throttle becomes a very large value
This commit is contained in:
Leonard Hall 2016-09-05 13:49:56 +09:00 committed by Randy Mackay
parent aafd1512e4
commit fdcdcb0033
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -473,6 +473,11 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
// get i term
i = _pid_accel_z.get_integrator();
// ensure imax is always large enough to overpower hover throttle
if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
_pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
}
// update i term as long as we haven't breached the limits or the I term will certainly reduce
// To-Do: should this be replaced with limits check from attitude_controller?
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {