diff --git a/libraries/AC_PID/AC_P_1D.cpp b/libraries/AC_PID/AC_P_1D.cpp index 513c3af016..164c0a4772 100644 --- a/libraries/AC_PID/AC_P_1D.cpp +++ b/libraries/AC_PID/AC_P_1D.cpp @@ -24,24 +24,17 @@ AC_P_1D::AC_P_1D(float initial_p, float dt) : // update_all - set target and measured inputs to P controller and calculate outputs // target and measurement are filtered -// if measurement is further than error_min or error_max (see set_limits method) -// the target is moved closer to the measurement and limit_min or limit_max will be set true -float AC_P_1D::update_all(float &target, float measurement, bool &limit_min, bool &limit_max) +float AC_P_1D::update_all(float &target, float measurement) { - limit_min = false; - limit_max = false; - // calculate distance _error _error = target - measurement; if (is_negative(_error_min) && (_error < _error_min)) { _error = _error_min; target = measurement + _error; - limit_min = true; } else if (is_positive(_error_max) && (_error > _error_max)) { _error = _error_max; target = measurement + _error; - limit_max = true; } // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded diff --git a/libraries/AC_PID/AC_P_1D.h b/libraries/AC_PID/AC_P_1D.h index 046d5e02be..5285ebce79 100644 --- a/libraries/AC_PID/AC_P_1D.h +++ b/libraries/AC_PID/AC_P_1D.h @@ -21,9 +21,7 @@ public: // update_all - set target and measured inputs to P controller and calculate outputs // target and measurement are filtered - // if measurement is further than error_min or error_max (see set_limits method) - // the target is moved closer to the measurement and limit_min or limit_max will be set true - float update_all(float &target, float measurement, bool &limit_min, bool &limit_max) WARN_IF_UNUSED; + float update_all(float &target, float measurement) WARN_IF_UNUSED; // set_limits - sets the maximum error to limit output and first and second derivative of output void set_limits(float output_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f); diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp index a37e2ee9c1..f7478100fc 100644 --- a/libraries/AC_PID/AC_P_2D.cpp +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -23,11 +23,8 @@ AC_P_2D::AC_P_2D(float initial_p, float dt) : } // update_all - set target and measured inputs to P controller and calculate outputs -// limit is set true if the target has been moved to limit the maximum position error -Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit) +Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) { - limit = false; - // calculate distance _error _error = (Vector2p{target_x, target_y} - measurement.topostype()).tofloat(); @@ -36,7 +33,6 @@ Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vec if (is_positive(_error_max) && _error.limit_length(_error_max)) { target_x = measurement.x + _error.x; target_y = measurement.y + _error.y; - limit = true; } // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h index d547a07da1..17b5803e2e 100644 --- a/libraries/AC_PID/AC_P_2D.h +++ b/libraries/AC_PID/AC_P_2D.h @@ -20,12 +20,12 @@ public: void set_dt(float dt) { _dt = dt; } // set target and measured inputs to P controller and calculate outputs - Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED; + Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) WARN_IF_UNUSED; // set target and measured inputs to P controller and calculate outputs // measurement is provided as 3-axis vector but only x and y are used - Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED { - return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}, limit); + Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector3f &measurement) WARN_IF_UNUSED { + return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}); } // set_limits - sets the maximum error to limit output and first and second derivative of output