AC_PID: Add error variable with accessor and clean up

libraries/AC_PID/AC_PID.cpp

@@ -70,10 +70,7 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
 // Constructor
 AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz,
                float dt, float initial_srmax, float initial_srtau):
-    _dt(dt),
-    _integrator(0.0f),
-    _error(0.0f),
-    _derivative(0.0f)
+    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);

libraries/AC_PID/AC_PID_2D.cpp

@@ -73,7 +73,7 @@ AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float
 //  target and error are filtered
 //  the derivative is then calculated and filtered
 //  the integral is then updated based on the setting of the limit flag
-Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, bool limit)
+Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit)
 {
     // don't process inf or NaN
     if (target.is_nan() || target.is_inf() ||
@@ -121,24 +121,49 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme
     return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
 }
 
-Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, bool limit)
+Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit)
 {
-    return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, limit);
+    return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, Vector2f{limit.x, limit.y});
 }
 
 //  update_i - update the integral
-//  If the limit flag is set the integral is only allowed to shrink
-void AC_PID_2D::update_i(bool limit)
+//  If the limit is set the integral is only allowed to reduce in the direction of the limit
+void AC_PID_2D::update_i( const Vector2f &limit)
 {
-    float integrator_length_orig = _kimax;
-    if (limit) {
-        integrator_length_orig = MIN(integrator_length_orig, _integrator.length());
+    Vector2f limit_direction = limit;
+    Vector2f delta_integrator = (_error * _ki) * _dt;
+    if (!is_zero(limit_direction.length_squared())) {
+        // zero delta_vel if it will increase the velocity error
+        limit_direction.normalize();
+        if (is_positive(delta_integrator * limit)) {
+            delta_integrator.zero();
         }
-    _integrator += (_error * _ki) * _dt;
-    const float integrator_length_new = _integrator.length();
-    if (integrator_length_new > integrator_length_orig) {
-        _integrator *= (integrator_length_orig / integrator_length_new);
     }
+
+    _integrator += delta_integrator;
+    _integrator.limit_length(_kimax);
+}
+
+Vector2f AC_PID_2D::get_p() const
+{
+    return _error * _kp;
+}
+
+Vector2f AC_PID_2D::get_i() const
+{
+    return _integrator;
+}
+
+Vector2f AC_PID_2D::get_d() const
+{
+    return _derivative * _kd;
+}
+
+Vector2f AC_PID_2D::get_ff()
+{
+    _pid_info_x.FF = _target.x * _kff;
+    _pid_info_y.FF = _target.y * _kff;
+    return _target * _kff;
 }
 
 // save_gains - save gains to eeprom

libraries/AC_PID/AC_PID_2D.h

@@ -24,18 +24,19 @@ public:
     // target and error are filtered
     // the derivative is then calculated and filtered
     // the integral is then updated based on the setting of the limit flag
-    Vector2f update_all(const Vector2f &target, const Vector2f &measurement, bool limit = false);
-    Vector2f update_all(const Vector3f &target, const Vector3f &measurement, bool limit = false);
+    Vector2f update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit);
+    Vector2f update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit);
 
     // update the integral
     // if the limit flag is set the integral is only allowed to shrink
-    void update_i(bool limit);
+    void update_i(const Vector2f &limit);
 
     // get results from pid controller
     Vector2f get_p() const;
     Vector2f get_i() const;
     Vector2f get_d() const;
     Vector2f get_ff();
+    const Vector2f& get_error() const { return _error; }
 
     // reset the integrator
     void reset_I() { _integrator.zero(); };

libraries/AC_PID/AC_PID_Basic.h

@@ -34,6 +34,7 @@ public:
     float get_i() const WARN_IF_UNUSED { return _integrator; }
     float get_d() const WARN_IF_UNUSED { return _derivative * _kd; }
     float get_ff() const WARN_IF_UNUSED { return _target * _kff; }
+    float get_error() const WARN_IF_UNUSED { return _error; }
 
     // reset the integrator
     void reset_I() { _integrator = 0.0f; }

libraries/AC_PID/AC_P_1D.cpp

@@ -20,12 +20,11 @@ AC_P_1D::AC_P_1D(float initial_p, float dt) :
     AP_Param::setup_object_defaults(this, var_info);
 
     _kp = initial_p;
-    _lim_D_Out = 10.0f;     // maximum first differential of output
 }
 
 // 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_error method)
+// 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)
 {
@@ -33,33 +32,65 @@ float AC_P_1D::update_all(float &target, float measurement, bool &limit_min, boo
     limit_max = false;
 
     // calculate distance _error
-    float error = target - measurement;
+    _error = target - measurement;
 
-    if (error < _lim_err_min) {
-        error = _lim_err_min;
-        target = measurement + error;
+    if (is_negative(_error_min) && (_error < _error_min)) {
+        _error = _error_min;
+        target = measurement + _error;
         limit_min = true;
-    } else if (error > _lim_err_max) {
-        error = _lim_err_max;
-        target = measurement + error;
+    } else if (is_positive(_error_max) && (_error > _error_max)) {
+        _error = _error_max;
+        target = measurement + _error;
         limit_max = true;
     }
 
-    // ToDo: Replace sqrt_controller with optimal acceleration and jerk limited curve
     // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded
-    return sqrt_controller(error, _kp, _lim_D_Out, _dt);
+    return sqrt_controller(_error, _kp, _D1_max, _dt);
 }
 
-// set limits on error, output and output from D term
-// in normal use the lim_err_min and lim_out_min will be negative
+// set_limits - sets the maximum error to limit output and first and second derivative of output
 // when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk
-void AC_P_1D::set_limits_error(float lim_err_min, float lim_err_max, float lim_out_min, float lim_out_max, float lim_D_Out, float lim_D2_Out)
+void AC_P_1D::set_limits(float output_min, float output_max, float D_Out_max, float D2_Out_max)
 {
-    _lim_D_Out = lim_D_Out;
-    if (is_positive(lim_D2_Out)) {
+    _D1_max = 0.0f;
+    _error_min = 0.0f;
+    _error_max = 0.0f;
+
+    if (is_positive(D_Out_max)) {
+        _D1_max = D_Out_max;
+    }
+
+    if (is_positive(D2_Out_max) && is_positive(_kp)) {
         // limit the first derivative so as not to exceed the second derivative
-        _lim_D_Out = MIN(_lim_D_Out, lim_D2_Out / _kp);
+        _D1_max = MIN(_D1_max, D2_Out_max / _kp);
+    }
+
+    if (is_negative(output_min) && is_positive(_kp)) {
+        _error_min = inv_sqrt_controller(output_min, _kp, _D1_max);
+    }
+
+    if (is_positive(output_max) && is_positive(_kp)) {
+        _error_max = inv_sqrt_controller(output_max, _kp, _D1_max);
+    }
+}
+
+// set_error_limits - reduce maximum position error to error_max
+// to be called after setting limits
+void AC_P_1D::set_error_limits(float error_min, float error_max)
+{
+    if (is_negative(error_min)) {
+        if (!is_zero(_error_min)) {
+            _error_min = MAX(_error_min, error_min);
+        } else {
+            _error_min = error_min;
+        }
+    }
+
+    if (is_positive(error_max)) {
+        if (!is_zero(_error_max)) {
+            _error_max = MIN(_error_max, error_max);
+        } else {
+            _error_max = error_max;
+        }
     }
-    _lim_err_min = MAX(inv_sqrt_controller(lim_out_min, _kp, _lim_D_Out), lim_err_min);
-    _lim_err_max = MAX(inv_sqrt_controller(lim_out_max, _kp, _lim_D_Out), lim_err_max);
 }

libraries/AC_PID/AC_P_1D.h

@@ -19,19 +19,32 @@ 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_error method)
+    // 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;
 
+    // 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);
+
+    // set_error_limits - reduce maximum position error to error_max
+    // to be called after setting limits
+    void set_error_limits(float error_min, float error_max);
+
+    // get_error_min - return minimum position error
+    float get_error_min() const { return _error_min; }
+
+    // get_error_max - return maximum position error
+    float get_error_max() const { return _error_max; }
+
     // save gain to eeprom
     void save_gains() { _kp.save(); }
 
-    // set limits on error, output and output from D term
-    void set_limits_error(float error_min, float error_max, float output_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
-
     // accessors
     AP_Float &kP() WARN_IF_UNUSED { return _kp; }
     const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
+    float get_error() const { return _error; }
+
+    // set accessors
     void kP(float v) { _kp.set(v); }
 
     // parameter var table
@@ -44,7 +57,8 @@ private:
 
     // internal variables
     float _dt;          // time step in seconds
-    float _lim_err_min; // error limit in negative direction
-    float _lim_err_max; // error limit in positive direction
-    float _lim_D_Out;   // maximum first differential of output
+    float _error;       // time step in seconds
+    float _error_min; // error limit in negative direction
+    float _error_max; // error limit in positive direction
+    float _D1_max;      // maximum first derivative of output
 };

libraries/AC_PID/AC_P_2D.cpp

@@ -23,19 +23,55 @@ AC_P_2D::AC_P_2D(float initial_p, float dt) :
 }
 
 //  set target and measured inputs to P controller and calculate outputs
-Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max)
+Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit)
 {
+    limit = false;
+
     // calculate distance _error
-    Vector2f error = Vector2f(target_x, target_y) - measurement;
+    _error = Vector2f{target_x, target_y} - measurement;
 
     // Constrain _error and target position
     // Constrain the maximum length of _vel_target to the maximum position correction velocity
-    if (error.limit_length(error_max)) {
-        target_x = measurement.x + error.x;
-        target_y = measurement.y + error.y;
+    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
-    // return sqrt_controller(Vector2f(_error.x, _error.y), _kp, MIN(_D_max, _D2_max / _kp), _dt);
-    return sqrt_controller(error, _kp, D2_max, _dt);
+    return sqrt_controller(_error, _kp, _D1_max, _dt);
+}
+
+// set_limits - sets the maximum error to limit output and first and second derivative of output
+// when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk
+void AC_P_2D::set_limits(float output_max, float D_Out_max, float D2_Out_max)
+{
+    _D1_max = 0.0f;
+    _error_max = 0.0f;
+
+    if (is_positive(D_Out_max)) {
+        _D1_max = D_Out_max;
+    }
+
+    if (is_positive(D2_Out_max) && is_positive(_kp)) {
+        // limit the first derivative so as not to exceed the second derivative
+        _D1_max = MIN(_D1_max, D2_Out_max / _kp);
+    }
+
+    if (is_positive(output_max) && is_positive(_kp)) {
+        _error_max = inv_sqrt_controller(output_max, _kp, _D1_max);
+    }
+}
+
+// set_error_max - reduce maximum position error to error_max
+// to be called after setting limits
+void AC_P_2D::set_error_max(float error_max)
+{
+    if (is_positive(error_max)) {
+        if (!is_zero(_error_max) ) {
+            _error_max = MIN(_error_max, error_max);
+        } else {
+            _error_max = error_max;
+        }
+    }
 }

libraries/AC_PID/AC_P_2D.h

@@ -18,28 +18,46 @@ public:
     void set_dt(float dt) { _dt = dt; }
 
     // set target and measured inputs to P controller and calculate outputs
-    Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max) WARN_IF_UNUSED;
+    Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit) 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(float &target_x, float &target_y, const Vector3f &measurement, float error_max, float D2_max) WARN_IF_UNUSED {
-        return update_all(target_x, target_y, Vector2f(measurement.x, measurement.y), error_max, D2_max);
+    Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED {
+        return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}, limit);
     }
 
+    // set_limits - sets the maximum error to limit output and first and second derivative of output
+    void set_limits(float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
+
+    // set_error_max - reduce maximum position error to error_max
+    // to be called after setting limits
+    void set_error_max(float error_max);
+
+    // get_error_max - return maximum position error
+    float get_error_max() { return _error_max; }
+
+    // save gain to eeprom
+    void save_gains() { _kp.save(); }
+
     // get accessors
     AP_Float &kP() WARN_IF_UNUSED { return _kp; }
     const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
+    const Vector2f& get_error() const { return _error; }
 
-    // set accessor
+    // set accessors
     void kP(float v) { _kp.set(v); }
 
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
+
     // parameters
     AP_Float    _kp;
 
     // internal variables
     float _dt;          // time step in seconds
+    Vector2f _error;    // time step in seconds
+    float _error_max; // error limit in positive direction
+    float _D1_max;      // maximum first derivative of output
 };