From 762bb3e6e848da77830ac81ab4085238b012ec00 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sun, 13 Jul 2014 03:01:03 -0700 Subject: [PATCH] AC_AttitudeControl: Limit _pos_target.z to below alt_max before computing error --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 756dbf0ce9..84523807eb 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -246,6 +246,12 @@ void AC_PosControl::pos_to_rate_z() _limit.pos_up = 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 _pos_error.z = _pos_target.z - curr_alt; @@ -261,12 +267,6 @@ void AC_PosControl::pos_to_rate_z() _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 if (_p_alt_pos.kP() != 0) { linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());