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

AC_AttitudeControl: Limit _pos_target.z to below alt_max before computing error

This commit is contained in:
Jonathan Challinger 2014-07-13 03:01:03 -07:00 committed by Randy Mackay
parent 28fedba4d8
commit 762bb3e6e8
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -246,6 +246,12 @@ void AC_PosControl::pos_to_rate_z()
_limit.pos_up = false; _limit.pos_up = false;
_limit.pos_down = false; _limit.pos_down = false;
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// calculate altitude error // calculate altitude error
_pos_error.z = _pos_target.z - curr_alt; _pos_error.z = _pos_target.z - curr_alt;
@ -261,12 +267,6 @@ void AC_PosControl::pos_to_rate_z()
_limit.pos_down = true; _limit.pos_down = true;
} }
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// check kP to avoid division by zero // check kP to avoid division by zero
if (_p_alt_pos.kP() != 0) { if (_p_alt_pos.kP() != 0) {
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP()); linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());