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

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());