AC_PID: Support changing update period

libraries/AC_PID/AC_HELI_PID.cpp

@@ -73,8 +73,8 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
 };
 
 /// Constructor for PID
-AC_HELI_PID::AC_HELI_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) :
+AC_HELI_PID::AC_HELI_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) :
-    AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz, dt)
+    AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz)
 {
     _last_requested_rate = 0;
 }
@@ -84,7 +84,7 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa
 
 void AC_HELI_PID::update_leaky_i(float leak_rate)
 {
-    if (!is_zero(_ki) && !is_zero(_dt)){
+    if (!is_zero(_ki)){
 
         // integrator does not leak down below Leak Min
         if (_integrator > _leak_min){

libraries/AC_PID/AC_HELI_PID.h

@@ -17,7 +17,7 @@ class AC_HELI_PID : public AC_PID {
 public:
 
     /// Constructor for PID
-    AC_HELI_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);
+    AC_HELI_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);
 
     CLASS_NO_COPY(AC_HELI_PID);
 

libraries/AC_PID/AC_PID.cpp

@@ -69,8 +69,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):
+               float initial_srmax, float initial_srtau)
-    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -96,13 +95,6 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
     _slew_limit_scale = 1;
 }
 
-// set_dt - set time step in seconds
-void AC_PID::set_dt(float dt)
-{
-    // set dt and calculate the input filter alpha
-    _dt = dt;
-}
-
 // filt_T_hz - set target filter hz
 void AC_PID::filt_T_hz(float hz)
 {
@@ -131,7 +123,7 @@ void AC_PID::slew_limit(float smax)
 //  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
-float AC_PID::update_all(float target, float measurement, bool limit)
+float AC_PID::update_all(float target, float measurement, float dt, bool limit)
 {
     // don't process inf or NaN
     if (!isfinite(target) || !isfinite(measurement)) {
@@ -146,24 +138,24 @@ float AC_PID::update_all(float target, float measurement, bool limit)
         _derivative = 0.0f;
     } else {
         float error_last = _error;
-        _target += get_filt_T_alpha() * (target - _target);
+        _target += get_filt_T_alpha(dt) * (target - _target);
-        _error += get_filt_E_alpha() * ((_target - measurement) - _error);
+        _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
 
         // calculate and filter derivative
-        if (_dt > 0.0f) {
+        if (is_positive(dt)) {
-            float derivative = (_error - error_last) / _dt;
+            float derivative = (_error - error_last) / dt;
-            _derivative += get_filt_D_alpha() * (derivative - _derivative);
+            _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
         }
     }
 
     // update I term
-    update_i(limit);
+    update_i(dt, limit);
 
     float P_out = (_error * _kp);
     float D_out = (_derivative * _kd);
 
     // calculate slew limit modifier for P+D
-    _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
+    _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt);
     _pid_info.slew_rate = _slew_limiter.get_slew_rate();
 
     P_out *= _pid_info.Dmod;
@@ -184,7 +176,7 @@ float AC_PID::update_all(float target, float measurement, bool limit)
 //  the integral is then updated based on the setting of the limit flag
 //  Target and Measured must be set manually for logging purposes.
 // todo: remove function when it is no longer used.
-float AC_PID::update_error(float error, bool limit)
+float AC_PID::update_error(float error, float dt, bool limit)
 {
     // don't process inf or NaN
     if (!isfinite(error)) {
@@ -200,23 +192,23 @@ float AC_PID::update_error(float error, bool limit)
         _derivative = 0.0f;
     } else {
         float error_last = _error;
-        _error += get_filt_E_alpha() * (error - _error);
+        _error += get_filt_E_alpha(dt) * (error - _error);
 
         // calculate and filter derivative
-        if (_dt > 0.0f) {
+        if (is_positive(dt)) {
-            float derivative = (_error - error_last) / _dt;
+            float derivative = (_error - error_last) / dt;
-            _derivative += get_filt_D_alpha() * (derivative - _derivative);
+            _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
         }
     }
 
     // update I term
-    update_i(limit);
+    update_i(dt, limit);
 
     float P_out = (_error * _kp);
     float D_out = (_derivative * _kd);
 
     // calculate slew limit modifier for P+D
-    _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
+    _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt);
     _pid_info.slew_rate = _slew_limiter.get_slew_rate();
 
     P_out *= _pid_info.Dmod;
@@ -233,12 +225,12 @@ float AC_PID::update_error(float error, bool limit)
 
 //  update_i - update the integral
 //  If the limit flag is set the integral is only allowed to shrink
-void AC_PID::update_i(bool limit)
+void AC_PID::update_i(float dt, bool limit)
 {
-    if (!is_zero(_ki) && is_positive(_dt)) {
+    if (!is_zero(_ki) && is_positive(dt)) {
         // Ensure that integrator can only be reduced if the output is saturated
         if (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) {
-            _integrator += ((float)_error * _ki) * _dt;
+            _integrator += ((float)_error * _ki) * dt;
             _integrator = constrain_float(_integrator, -_kimax, _kimax);
         }
     } else {
@@ -302,7 +294,7 @@ void AC_PID::save_gains()
 }
 
 /// Overload the function call operator to permit easy initialisation
-void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt)
+void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz)
 {
     _kp.set(p_val);
     _ki.set(i_val);
@@ -312,31 +304,24 @@ void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, flo
     _filt_T_hz.set(input_filt_T_hz);
     _filt_E_hz.set(input_filt_E_hz);
     _filt_D_hz.set(input_filt_D_hz);
-    _dt = dt;
 }
 
 // get_filt_T_alpha - get the target filter alpha
-float AC_PID::get_filt_T_alpha() const
+float AC_PID::get_filt_T_alpha(float dt) const
 {
-    return get_filt_alpha(_filt_T_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_T_hz);
 }
 
 // get_filt_E_alpha - get the error filter alpha
-float AC_PID::get_filt_E_alpha() const
+float AC_PID::get_filt_E_alpha(float dt) const
 {
-    return get_filt_alpha(_filt_E_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_E_hz);
 }
 
 // get_filt_D_alpha - get the derivative filter alpha
-float AC_PID::get_filt_D_alpha() const
+float AC_PID::get_filt_D_alpha(float dt) const
 {
-    return get_filt_alpha(_filt_D_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_D_hz);
-}
-
-// get_filt_alpha - calculate a filter alpha
-float AC_PID::get_filt_alpha(float filt_hz) const
-{
-    return calc_lowpass_alpha_dt(_dt, filt_hz);
 }
 
 void AC_PID::set_integrator(float target, float measurement, float integrator)
@@ -356,9 +341,11 @@ void AC_PID::set_integrator(float integrator)
     _pid_info.I = _integrator;
 }
 
-void AC_PID::relax_integrator(float integrator, float time_constant)
+void AC_PID::relax_integrator(float integrator, float dt, float time_constant)
 {
     integrator = constrain_float(integrator, -_kimax, _kimax);
-    _integrator = _integrator + (integrator - _integrator) * (_dt / (_dt + time_constant));
+    if (is_positive(dt)) {
+        _integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant));
+    }
     _pid_info.I = _integrator;
 }

libraries/AC_PID/AC_PID.h

@@ -23,18 +23,15 @@ public:
 
     // Constructor for 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=0, float initial_srtau=1.0);
+           float initial_srmax=0, float initial_srtau=1.0);
 
     CLASS_NO_COPY(AC_PID);
 
-    // set_dt - set time step in seconds
-    void set_dt(float dt);
-
     //  update_all - set target and measured inputs to PID controller and calculate outputs
     //  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
-    float update_all(float target, float measurement, bool limit = false);
+    float update_all(float target, float measurement, float dt, bool limit = false);
 
     //  update_error - set error input to PID controller and calculate outputs
     //  target is set to zero and error is set and filtered
@@ -42,11 +39,11 @@ public:
     //  the integral is then updated based on the setting of the limit flag
     //  Target and Measured must be set manually for logging purposes.
     // todo: remove function when it is no longer used.
-    float update_error(float error, bool limit = false);
+    float update_error(float error, float dt, bool limit = false);
 
     //  update_i - update the integral
     //  if the limit flag is set the integral is only allowed to shrink
-    void update_i(bool limit);
+    void update_i(float dt, bool limit);
 
     // get_pid - get results from pid controller
     float get_pid() const;
@@ -71,7 +68,7 @@ public:
     void save_gains();
 
     /// operator function call for easy initialisation
-    void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt);
+    void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz);
 
     // get accessors
     AP_Float &kP() { return _kp; }
@@ -85,10 +82,9 @@ public:
     AP_Float &slew_limit() { return _slew_rate_max; }
 
     float imax() const { return _kimax.get(); }
-    float get_filt_alpha(float filt_hz) const;
+    float get_filt_T_alpha(float dt) const;
-    float get_filt_T_alpha() const;
+    float get_filt_E_alpha(float dt) const;
-    float get_filt_E_alpha() const;
+    float get_filt_D_alpha(float dt) const;
-    float get_filt_D_alpha() const;
 
     // set accessors
     void kP(const float v) { _kp.set(v); }
@@ -109,7 +105,7 @@ public:
     void set_integrator(float target, float measurement, float i);
     void set_integrator(float error, float i);
     void set_integrator(float i);
-    void relax_integrator(float integrator, float time_constant);
+    void relax_integrator(float integrator, float dt, float time_constant);
 
     // set slew limiter scale factor
     void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; }
@@ -149,7 +145,6 @@ protected:
     } _flags;
 
     // internal variables
-    float _dt;                // timestep in seconds
     float _integrator;        // integrator value
     float _target;            // target value to enable filtering
     float _error;             // error value to enable filtering

libraries/AC_PID/AC_PID_2D.cpp

@@ -50,8 +50,7 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
 };
 
 // Constructor
-AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
+AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
-    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -72,7 +71,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 if it does not increase in the direction of the limit vector
-Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit)
+Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit)
 {
     // don't process inf or NaN
     if (target.is_nan() || target.is_inf() ||
@@ -89,17 +88,17 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme
         _derivative.zero();
     } else {
         Vector2f error_last{_error};
-        _error += ((_target - measurement) - _error) * get_filt_E_alpha();
+        _error += ((_target - measurement) - _error) * get_filt_E_alpha(dt);
 
         // calculate and filter derivative
-        if (_dt > 0.0f) {
+        if (is_positive(dt)) {
-            const Vector2f derivative{(_error - error_last) / _dt};
+            const Vector2f derivative{(_error - error_last) / dt};
-            _derivative += (derivative - _derivative) * get_filt_D_alpha();
+            _derivative += (derivative - _derivative) * get_filt_D_alpha(dt);
         }
     }
 
     // update I term
-    update_i(limit);
+    update_i(dt, limit);
 
     _pid_info_x.target = _target.x;
     _pid_info_x.actual = measurement.x;
@@ -120,19 +119,19 @@ 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, const Vector3f &limit)
+Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit)
 {
-    return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, Vector2f{limit.x, limit.y});
+    return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, dt, Vector2f{limit.x, limit.y});
 }
 
 //  update_i - update the integral
 //  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)
+void AC_PID_2D::update_i(float dt, const Vector2f &limit)
 {
     _pid_info_x.limit = false;
     _pid_info_y.limit = false;
 
-    Vector2f delta_integrator = (_error * _ki) * _dt;
+    Vector2f delta_integrator = (_error * _ki) * dt;
     float integrator_length = _integrator.length();
     _integrator += delta_integrator;
     // do not let integrator increase in length if delta_integrator is in the direction of limit
@@ -186,15 +185,15 @@ void AC_PID_2D::save_gains()
 }
 
 // get the target filter alpha
-float AC_PID_2D::get_filt_E_alpha() const
+float AC_PID_2D::get_filt_E_alpha(float dt) const
 {
-    return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_E_hz);
 }
 
 // get the derivative filter alpha
-float AC_PID_2D::get_filt_D_alpha() const
+float AC_PID_2D::get_filt_D_alpha(float dt) const
 {
-    return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_D_hz);
 }
 
 void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)

libraries/AC_PID/AC_PID_2D.h

@@ -15,23 +15,20 @@ class AC_PID_2D {
 public:
 
     // Constructor for PID
-    AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt);
+    AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_hz, float initial_filt_d_hz);
 
     CLASS_NO_COPY(AC_PID_2D);
 
-    // set time step in seconds
-    void set_dt(float dt) { _dt = dt; }
-
     // update_all - set target and measured inputs to PID controller and calculate outputs
     // target and error are filtered
     // the derivative is then calculated and filtered
     // the integral is then updated if it does not increase in the direction of the limit vector
-    Vector2f update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit);
+    Vector2f update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit);
-    Vector2f update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit);
+    Vector2f update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit);
 
     // update the integral
     // if the limit flag is set the integral is only allowed to shrink
-    void update_i(const Vector2f &limit);
+    void update_i(float dt, const Vector2f &limit);
 
     // get results from pid controller
     Vector2f get_p() const;
@@ -57,8 +54,8 @@ public:
     AP_Float &filt_E_hz() { return _filt_E_hz; }
     AP_Float &filt_D_hz() { return _filt_D_hz; }
     float imax() const { return _kimax.get(); }
-    float get_filt_E_alpha() const;
+    float get_filt_E_alpha(float dt) const;
-    float get_filt_D_alpha() const;
+    float get_filt_D_alpha(float dt) const;
 
     // set accessors
     void kP(float v) { _kp.set(v); }
@@ -93,7 +90,6 @@ protected:
     AP_Float _filt_D_hz;         // PID derivative filter frequency in Hz
 
     // internal variables
-    float       _dt;            // timestep in seconds
     Vector2f    _target;        // target value to enable filtering
     Vector2f    _error;         // error value to enable filtering
     Vector2f    _derivative;    // last derivative from low-pass filter

libraries/AC_PID/AC_PID_Basic.cpp

@@ -52,8 +52,7 @@ const AP_Param::GroupInfo AC_PID_Basic::var_info[] = {
 };
 
 // Constructor
-AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
+AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
-    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -70,16 +69,16 @@ AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, fl
     _reset_filter = true;
 }
 
-float AC_PID_Basic::update_all(float target, float measurement, bool limit)
+float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit)
 {
-    return update_all(target, measurement, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
+    return update_all(target, measurement, dt, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
 }
 
 //  update_all - set target and measured inputs to PID controller and calculate outputs
 //  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
-float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg, bool limit_pos)
+float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos)
 {
     // don't process inf or NaN
     if (!isfinite(target) || isnan(target) ||
@@ -97,17 +96,17 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg,
         _derivative = 0.0f;
     } else {
         float error_last = _error;
-        _error += get_filt_E_alpha() * ((_target - measurement) - _error);
+        _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
 
         // calculate and filter derivative
-        if (is_positive(_dt)) {
+        if (is_positive(dt)) {
-            float derivative = (_error - error_last) / _dt;
+            float derivative = (_error - error_last) / dt;
-            _derivative += get_filt_D_alpha() * (derivative - _derivative);
+            _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
         }
     }
 
     // update I term
-    update_i(limit_neg, limit_pos);
+    update_i(dt, limit_neg, limit_pos);
 
     const float P_out = _error * _kp;
     const float D_out = _derivative * _kd;
@@ -126,12 +125,12 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg,
 //  update_i - update the integral
 //  if limit_neg is true, the integral can only increase
 //  if limit_pos is true, the integral can only decrease
-void AC_PID_Basic::update_i(bool limit_neg, bool limit_pos)
+void AC_PID_Basic::update_i(float dt, bool limit_neg, bool limit_pos)
 {
     if (!is_zero(_ki)) {
         // Ensure that integrator can only be reduced if the output is saturated
         if (!((limit_neg && is_negative(_error)) || (limit_pos && is_positive(_error)))) {
-            _integrator += ((float)_error * _ki) * _dt;
+            _integrator += ((float)_error * _ki) * dt;
             _integrator = constrain_float(_integrator, -_kimax, _kimax);
         }
     } else {
@@ -158,15 +157,15 @@ void AC_PID_Basic::save_gains()
 }
 
 // get_filt_T_alpha - get the target filter alpha
-float AC_PID_Basic::get_filt_E_alpha() const
+float AC_PID_Basic::get_filt_E_alpha(float dt) const
 {
-    return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_E_hz);
 }
 
 // get_filt_D_alpha - get the derivative filter alpha
-float AC_PID_Basic::get_filt_D_alpha() const
+float AC_PID_Basic::get_filt_D_alpha(float dt) const
 {
-    return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
+    return calc_lowpass_alpha_dt(dt, _filt_D_hz);
 }
 
 void AC_PID_Basic::set_integrator(float target, float measurement, float i)

libraries/AC_PID/AC_PID_Basic.h

@@ -13,21 +13,18 @@ class AC_PID_Basic {
 public:
 
     // Constructor for PID
-    AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
+    AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz);
-
-    // set time step in seconds
-    void set_dt(float dt) { _dt = dt; }
 
     // set target and measured inputs to PID controller and calculate outputs
     // 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
-    float update_all(float target, float measurement, bool limit = false) WARN_IF_UNUSED;
+    float update_all(float target, float measurement, float dt, bool limit = false) WARN_IF_UNUSED;
-    float update_all(float target, float measurement, bool limit_neg, bool limit_pos) WARN_IF_UNUSED;
+    float update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) WARN_IF_UNUSED;
 
     // update the integral
     // if the limit flags are set the integral is only allowed to shrink
-    void update_i(bool limit_neg, bool limit_pos);
+    void update_i(float dt, bool limit_neg, bool limit_pos);
 
     // get results from pid controller
     float get_p() const WARN_IF_UNUSED { return _error * _kp; }
@@ -53,8 +50,8 @@ public:
     AP_Float &filt_E_hz() WARN_IF_UNUSED { return _filt_E_hz; }
     AP_Float &filt_D_hz() WARN_IF_UNUSED { return _filt_D_hz; }
     float imax() const WARN_IF_UNUSED { return _kimax.get(); }
-    float get_filt_E_alpha() const WARN_IF_UNUSED;
+    float get_filt_E_alpha(float dt) const WARN_IF_UNUSED;
-    float get_filt_D_alpha() const WARN_IF_UNUSED;
+    float get_filt_D_alpha(float dt) const WARN_IF_UNUSED;
 
     // set accessors
     void kP(float v) { _kp.set(v); }
@@ -87,7 +84,6 @@ protected:
     AP_Float _filt_D_hz;         // PID derivative filter frequency in Hz
 
     // internal variables
-    float _dt;          // timestep in seconds
     float _target;      // target value to enable filtering
     float _error;       // error value to enable filtering
     float _derivative;  // last derivative for low-pass filter

libraries/AC_PID/AC_P_1D.cpp

@@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_1D::var_info[] = {
 };
 
 // Constructor
-AC_P_1D::AC_P_1D(float initial_p, float dt) :
+AC_P_1D::AC_P_1D(float initial_p)
-    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -38,7 +37,7 @@ float AC_P_1D::update_all(float &target, float measurement)
     }
 
     // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded
-    return sqrt_controller(_error, _kp, _D1_max, _dt);
+    return sqrt_controller(_error, _kp, _D1_max, 0.0);
 }
 
 // set_limits - sets the maximum error to limit output and first and second derivative of output

libraries/AC_PID/AC_P_1D.h

@@ -12,13 +12,10 @@ class AC_P_1D {
 public:
 
     // constructor
-    AC_P_1D(float initial_p, float dt);
+    AC_P_1D(float initial_p);
 
     CLASS_NO_COPY(AC_P_1D);
 
-    // set time step in seconds
-    void set_dt(float dt) { _dt = dt; }
-
     // update_all - set target and measured inputs to P controller and calculate outputs
     // target and measurement are filtered
     float update_all(float &target, float measurement) WARN_IF_UNUSED;
@@ -56,7 +53,6 @@ private:
     AP_Float _kp;
 
     // internal variables
-    float _dt;          // time step in seconds
     float _error;       // time step in seconds
     float _error_min; // error limit in negative direction
     float _error_max; // error limit in positive direction

libraries/AC_PID/AC_P_2D.cpp

@@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_2D::var_info[] = {
 };
 
 // Constructor
-AC_P_2D::AC_P_2D(float initial_p, float dt) :
+AC_P_2D::AC_P_2D(float initial_p)
-    _dt(dt)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -36,7 +35,7 @@ Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vec
     }
 
     // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded
-    return sqrt_controller(_error, _kp, _D1_max, _dt);
+    return sqrt_controller(_error, _kp, _D1_max, 0.0);
 }
 
 // set_limits - sets the maximum error to limit output and first and second derivative of output

libraries/AC_PID/AC_P_2D.h

@@ -12,13 +12,10 @@ class AC_P_2D {
 public:
 
     // constructor
-    AC_P_2D(float initial_p, float dt);
+    AC_P_2D(float initial_p);
 
     CLASS_NO_COPY(AC_P_2D);
 
-    // set time step in seconds
-    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) WARN_IF_UNUSED;
 
@@ -58,7 +55,6 @@ private:
     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

libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp

@@ -70,8 +70,8 @@ void setup()
 void loop()
 {
     // setup (unfortunately must be done here as we cannot create a global AC_PID object)
-    AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER, TEST_DT);
+    AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER);
-    AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER, TEST_DT);
+    AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER);
 
     // display PID gains
     hal.console->printf("P %f  I %f  D %f  imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax());
@@ -91,7 +91,7 @@ void loop()
         rc().read_input(); // poll the radio for new values
         const uint16_t radio_in = c->get_radio_in();
         const int16_t error = radio_in - radio_trim;
-        pid.update_error(error);
+        pid.update_error(error, TEST_DT);
         const float control_P = pid.get_p();
         const float control_I = pid.get_i();
         const float control_D = pid.get_d();