Browse Source

AC_PosControl: update _pos_error if it is being limited

This fixes a bug that causes 10hz throttle noise.
mission-4.1.18
jschall 11 years ago committed by Randy Mackay
parent
commit
024855014f
  1. 2
      libraries/AC_AttitudeControl/AC_PosControl.cpp

2
libraries/AC_AttitudeControl/AC_PosControl.cpp

@ -213,10 +213,12 @@ void AC_PosControl::pos_to_rate_z() @@ -213,10 +213,12 @@ void AC_PosControl::pos_to_rate_z()
// do not let target altitude get too far from current altitude
if (_pos_error.z > _leash_up_z) {
_pos_target.z = curr_alt + _leash_up_z;
_pos_error.z = _leash_up_z;
_limit.pos_up = true;
}
if (_pos_error.z < -_leash_down_z) {
_pos_target.z = curr_alt - _leash_down_z;
_pos_error.z = -_leash_down_z;
_limit.pos_down = true;
}

Loading…
Cancel
Save