diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0286439ef4..2864427a92 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -318,11 +318,6 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, _jerk_max_z_cmsss(POSCONTROL_JERK_Z * 100.0) { AP_Param::setup_object_defaults(this, var_info); - - // initialise flags - _limit.pos_xy = true; - _limit.pos_up = true; - _limit.pos_down = true; } @@ -620,7 +615,7 @@ void AC_PosControl::update_xy_controller() // Position Controller const Vector3f &curr_pos = _inav.get_position(); - Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _limit.pos_xy); + Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos); // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise vel_target *= ahrsControlScaleXY; @@ -958,7 +953,7 @@ void AC_PosControl::update_z_controller() // calculate the target velocity correction float pos_target_zf = _pos_target.z; - _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt, _limit.pos_down, _limit.pos_up); + _vel_target.z = _p_pos_z.update_all(pos_target_zf, curr_alt); _vel_target.z *= AP::ahrs().getControlScaleZ(); _pos_target.z = pos_target_zf; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index b3b8142441..93b08190b2 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -390,13 +390,6 @@ protected: uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep } _flags; - // limit flags structure - struct poscontrol_limit_flags { - bool pos_xy; // true if we have hit a horizontal position limit - bool pos_up; // true if we have hit a vertical position limit while going up - bool pos_down; // true if we have hit a vertical position limit while going down - } _limit; - /// init_xy - initialise the position controller to the current position, velocity and acceleration. /// This function is private and contains all the shared xy axis initialisation functions void init_xy();