Line data Source code
1 : /// @file AC_PID_Basic.cpp
2 : /// @brief Lightweight PID controller with error and derivative filtering, integrator limit, and EEPROM gain storage.
3 :
4 : #include <AP_Math/AP_Math.h>
5 : #include <AP_InternalError/AP_InternalError.h>
6 : #include "AC_PID_Basic.h"
7 :
8 : #define AC_PID_Basic_FILT_E_HZ_MIN 0.01f // minimum input filter frequency
9 : #define AC_PID_Basic_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
10 :
11 : const AP_Param::GroupInfo AC_PID_Basic::var_info[] = {
12 : // @Param: P
13 : // @DisplayName: PID Proportional Gain
14 : // @Description: P Gain which produces an output value that is proportional to the current error value
15 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_Basic, _kp, default_kp),
16 :
17 : // @Param: I
18 : // @DisplayName: PID Integral Gain
19 : // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
20 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_Basic, _ki, default_ki),
21 :
22 : // @Param: IMAX
23 : // @DisplayName: PID Integral Maximum
24 : // @Description: The maximum/minimum value that the I term can output
25 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_Basic, _kimax, default_kimax),
26 :
27 : // @Param: FLTE
28 : // @DisplayName: PID Error filter frequency in Hz
29 : // @Description: Low-pass filter frequency applied to the error (Hz)
30 : // @Units: Hz
31 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_Basic, _filt_E_hz, default_filt_E_hz),
32 :
33 : // @Param: D
34 : // @DisplayName: PID Derivative Gain
35 : // @Description: D Gain which produces an output that is proportional to the rate of change of the error
36 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_Basic, _kd, default_kd),
37 :
38 : // @Param: FLTD
39 : // @DisplayName: D term filter frequency in Hz
40 : // @Description: Low-pass filter frequency applied to the derivative (Hz)
41 : // @Units: Hz
42 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_Basic, _filt_D_hz, default_filt_D_hz),
43 :
44 : // @Param: FF
45 : // @DisplayName: PID Feed Forward Gain
46 : // @Description: FF Gain which produces an output that is proportional to the magnitude of the target
47 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_Basic, _kff, default_kff),
48 :
49 : AP_GROUPEND
50 : };
51 :
52 : // Constructor
53 1368 : 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) :
54 1368 : default_kp(initial_p),
55 1368 : default_ki(initial_i),
56 1368 : default_kd(initial_d),
57 1368 : default_kff(initial_ff),
58 1368 : default_kimax(initial_imax),
59 1368 : default_filt_E_hz(initial_filt_E_hz),
60 1368 : default_filt_D_hz(initial_filt_D_hz)
61 : {
62 : // load parameter values from eeprom
63 1368 : AP_Param::setup_object_defaults(this, var_info);
64 :
65 : // reset input filter to first value received
66 1368 : _reset_filter = true;
67 1368 : }
68 :
69 14401 : float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit)
70 : {
71 14401 : return update_all(target, measurement, dt, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
72 : }
73 :
74 : // Computes the PID output using a target and measurement input.
75 : // Applies filters to the error and derivative, then updates the integrator.
76 : // If `limit` is true, the integrator is allowed to shrink but not grow.
77 25581503 : float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos)
78 : {
79 : // Return zero if inputs are invalid (NaN or infinite)
80 51163006 : if (!isfinite(target) || isnan(target) ||
81 51163006 : !isfinite(measurement) || isnan(measurement)) {
82 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
83 0 : return 0.0f;
84 : }
85 :
86 25581503 : _target = target;
87 :
88 : // Reset filter state to match current inputs (on first run or after reset)
89 25581503 : if (_reset_filter) {
90 : // Reset filters to match the current inputs
91 4121993 : _reset_filter = false;
92 4121993 : _error = _target - measurement;
93 4121993 : _derivative = 0.0f;
94 : } else {
95 21459510 : float error_last = _error;
96 21459510 : _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
97 :
98 : // Compute and low-pass filter the error derivative (D term)
99 21459510 : if (is_positive(dt)) {
100 21459510 : float derivative = (_error - error_last) / dt;
101 21459510 : _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
102 : }
103 : }
104 :
105 : // update I term
106 25581503 : update_i(dt, limit_neg, limit_pos);
107 :
108 25581503 : const float P_out = _error * _kp;
109 25581503 : const float D_out = _derivative * _kd;
110 :
111 25581503 : _pid_info.target = _target;
112 25581503 : _pid_info.actual = measurement;
113 25581503 : _pid_info.error = _error;
114 25581503 : _pid_info.P = _error * _kp;
115 25581503 : _pid_info.I = _integrator;
116 25581503 : _pid_info.D = _derivative * _kd;
117 25581503 : _pid_info.FF = _target * _kff;
118 :
119 25581503 : return P_out + _integrator + D_out + _target * _kff;
120 : }
121 :
122 : // Updates the integrator using current error and dt.
123 : // If `limit_neg` is true, integrator may only increase.
124 : // If `limit_pos` is true, integrator may only decrease.
125 25581503 : void AC_PID_Basic::update_i(float dt, bool limit_neg, bool limit_pos)
126 : {
127 25581503 : if (!is_zero(_ki)) {
128 : // Ensure that integrator can only be reduced if the output is saturated
129 14401 : if (!((limit_neg && is_negative(_error)) || (limit_pos && is_positive(_error)))) {
130 8442 : _integrator += ((float)_error * _ki) * dt;
131 8442 : _integrator = constrain_float(_integrator, -_kimax, _kimax);
132 : }
133 : } else {
134 25567102 : _integrator = 0.0f;
135 : }
136 25581503 : }
137 :
138 0 : void AC_PID_Basic::reset_I()
139 : {
140 0 : _integrator = 0.0;
141 0 : }
142 :
143 : // Saves controller configuration from EEPROM, including gains and filter frequencies. (not used)
144 0 : void AC_PID_Basic::save_gains()
145 : {
146 0 : _kp.save();
147 0 : _ki.save();
148 0 : _kd.save();
149 0 : _kff.save();
150 0 : _kimax.save();
151 0 : _filt_E_hz.save();
152 0 : _filt_D_hz.save();
153 0 : }
154 :
155 : // Returns alpha value for the error low-pass filter (based on filter frequency and dt)
156 21459510 : float AC_PID_Basic::get_filt_E_alpha(float dt) const
157 : {
158 21459510 : return calc_lowpass_alpha_dt(dt, _filt_E_hz);
159 : }
160 :
161 : // Returns alpha value for the derivative low-pass filter (based on filter frequency and dt)
162 21459510 : float AC_PID_Basic::get_filt_D_alpha(float dt) const
163 : {
164 21459510 : return calc_lowpass_alpha_dt(dt, _filt_D_hz);
165 : }
166 :
167 : // Sets integrator based on target, measurement, and desired total PID output.
168 0 : void AC_PID_Basic::set_integrator(float target, float measurement, float i)
169 : {
170 0 : set_integrator(target - measurement, i);
171 0 : }
172 :
173 : // Sets integrator using error and desired total output.
174 0 : void AC_PID_Basic::set_integrator(float error, float i)
175 : {
176 0 : set_integrator(i - error * _kp);
177 0 : }
178 :
179 : // Sets the integrator directly.
180 6184170 : void AC_PID_Basic::set_integrator(float i)
181 : {
182 6184170 : _integrator = constrain_float(i, -_kimax, _kimax);
183 6184170 : }
|
