diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 29149bb1b2e43..1864a14609f57 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -52,7 +52,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _leash_up_z(POSCONTROL_LEASH_LENGTH_MIN), _roll_target(0.0f), _pitch_target(0.0f), - _alt_max(0.0f), _distance_to_target(0.0f), _accel_target_jerk_limited(0.0f,0.0f), _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ) @@ -166,12 +165,6 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d _pos_target.z += climb_rate_cms * dt; } - // 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; - } - // do not use z-axis desired velocity feed forward // vel_desired set to desired climb rate for reporting and land-detector _flags.use_desvel_ff_z = false; @@ -212,14 +205,6 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) { _pos_target.z += _vel_desired.z * dt; } - - // 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; - // decelerate feed forward to zero - _vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit); - } } /// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 3cfb8f9fbb217..ac7faeff236db 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -76,11 +76,6 @@ class AC_PosControl /// z position controller /// - /// set_alt_max - sets maximum altitude above the ekf origin in cm - /// only enforced when set_alt_target_from_climb_rate is used - /// set to zero to disable limit - void set_alt_max(float alt) { _alt_max = alt; } - /// set_speed_z - sets maximum climb and descent rates /// speed_down can be positive or negative but will always be interpreted as a descent speed /// leash length will be recalculated the next time update_z_controller() is called @@ -417,7 +412,6 @@ class AC_PosControl Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required? Vector3f _accel_feedforward; // feedforward acceleration in cm/s/s Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set - float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence float _distance_to_target; // distance to position target - for reporting only LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error