Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: P 1D, P 2D: remove unused limit flags

Browse Source
This commit is contained in:
Iampete1 2021-11-21 12:29:20 +00:00 committed by Randy Mackay
parent be6598708e
commit 6162775dd0
4 changed files with 6 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_PID/AC_P_1D.cpp
View File

@ -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 // update_all - set target and measured inputs to P controller and calculate outputs
// target and measurement are filtered // target and measurement are filtered
// if measurement is further than error_min or error_max (see set_limits method) float AC_P_1D::update_all(float &target, float measurement)
// 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)
{ {
limit_min = false;
limit_max = false;
// calculate distance _error // calculate distance _error
_error = target - measurement; _error = target - measurement;
if (is_negative(_error_min) && (_error < _error_min)) { if (is_negative(_error_min) && (_error < _error_min)) {
_error = _error_min; _error = _error_min;
target = measurement + _error; target = measurement + _error;
limit_min = true;
} else if (is_positive(_error_max) && (_error > _error_max)) { } else if (is_positive(_error_max) && (_error > _error_max)) {
_error = _error_max; _error = _error_max;
target = measurement + _error; 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 // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded

4
libraries/AC_PID/AC_P_1D.h
View File

@ -21,9 +21,7 @@ public:
// update_all - set target and measured inputs to P controller and calculate outputs // update_all - set target and measured inputs to P controller and calculate outputs
// target and measurement are filtered // target and measurement are filtered
// if measurement is further than error_min or error_max (see set_limits method) float update_all(float &target, float measurement) WARN_IF_UNUSED;
// 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;
// set_limits - sets the maximum error to limit output and first and second derivative of output // 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); void set_limits(float output_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);

6
libraries/AC_PID/AC_P_2D.cpp
View File

@ -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 // 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)
Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit)
{ {
limit = false;
// calculate distance _error // calculate distance _error
_error = (Vector2p{target_x, target_y} - measurement.topostype()).tofloat(); _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)) { if (is_positive(_error_max) && _error.limit_length(_error_max)) {
target_x = measurement.x + _error.x; target_x = measurement.x + _error.x;
target_y = measurement.y + _error.y; target_y = measurement.y + _error.y;
limit = true;
} }
// MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded

6
libraries/AC_PID/AC_P_2D.h
View File

@ -20,12 +20,12 @@ public:
void set_dt(float dt) { _dt = dt; } void set_dt(float dt) { _dt = dt; }
// set target and measured inputs to P controller and calculate outputs // 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 // 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 // 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 { 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}, limit); 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 // set_limits - sets the maximum error to limit output and first and second derivative of output