Line data Source code
1 : /// @file AC_PID_2D.cpp
2 : /// @brief 2D PID controller with vector support, input filtering, integrator clamping, and EEPROM-backed gain storage.
3 :
4 : #include <AP_Math/AP_Math.h>
5 : #include "AC_PID_2D.h"
6 :
7 : #define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
8 :
9 : const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
10 : // @Param: P
11 : // @DisplayName: PID Proportional Gain
12 : // @Description: P Gain which produces an output value that is proportional to the current error value
13 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_2D, _kp, default_kp),
14 :
15 : // @Param: I
16 : // @DisplayName: PID Integral Gain
17 : // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
18 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_2D, _ki, default_ki),
19 :
20 : // @Param: IMAX
21 : // @DisplayName: PID Integral Maximum
22 : // @Description: The maximum/minimum value that the I term can output
23 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_2D, _kimax, default_kimax),
24 :
25 : // @Param: FLTE
26 : // @DisplayName: PID Error filter frequency in Hz
27 : // @Description: Low-pass filter frequency applied to the error (Hz)
28 : // @Units: Hz
29 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_2D, _filt_E_hz, default_filt_E_hz),
30 :
31 : // @Param: D
32 : // @DisplayName: PID Derivative Gain
33 : // @Description: D Gain which produces an output that is proportional to the rate of change of the error
34 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_2D, _kd, default_kd),
35 :
36 : // @Param: FLTD
37 : // @DisplayName: D term filter frequency in Hz
38 : // @Description: Low-pass filter frequency applied to the derivative (Hz)
39 : // @Units: Hzs
40 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_2D, _filt_D_hz, default_filt_D_hz),
41 :
42 : // @Param: FF
43 : // @DisplayName: PID Feed Forward Gain
44 : // @Description: FF Gain which produces an output that is proportional to the magnitude of the target
45 : AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_2D, _kff, default_kff),
46 :
47 : AP_GROUPEND
48 : };
49 :
50 : // Constructor
51 1570 : 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) :
52 1570 : default_kp(initial_kP),
53 1570 : default_ki(initial_kI),
54 1570 : default_kd(initial_kD),
55 1570 : default_kff(initial_kFF),
56 1570 : default_kimax(initial_imax),
57 1570 : default_filt_E_hz(initial_filt_E_hz),
58 1570 : default_filt_D_hz(initial_filt_D_hz)
59 : {
60 : // load parameter values from eeprom
61 1570 : AP_Param::setup_object_defaults(this, var_info);
62 :
63 : // reset input filter to first value received
64 1570 : _reset_filter = true;
65 1570 : }
66 :
67 : // Computes the 2D PID output from target and measurement vectors.
68 : // Applies filtering to error and derivative terms.
69 : // Integrator is updated only if it does not grow in the direction of the specified limit vector.
70 19442525 : Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit)
71 : {
72 : // Return zero if any component is NaN or infinite
73 58327575 : if (target.is_nan() || target.is_inf() ||
74 58327575 : measurement.is_nan() || measurement.is_inf()) {
75 0 : return Vector2f{};
76 : }
77 :
78 19442525 : _target = target;
79 :
80 : // reset input filter to value received
81 19442525 : if (_reset_filter) {
82 : // Reset filters to match the current inputs
83 2317957 : _reset_filter = false;
84 2317957 : _error = _target - measurement;
85 : // Reset the derivative to avoid transients
86 2317957 : _derivative.zero();
87 : } else {
88 17124568 : Vector2f error_last{_error};
89 : // Apply first-order low-pass filter to error
90 17124568 : _error += ((_target - measurement) - _error) * get_filt_E_alpha(dt);
91 :
92 : // Compute and low-pass filter the derivative
93 17124568 : if (is_positive(dt)) {
94 17113242 : const Vector2f derivative{(_error - error_last) / dt};
95 17113242 : _derivative += (derivative - _derivative) * get_filt_D_alpha(dt);
96 : }
97 : }
98 :
99 : // update I term
100 19442525 : update_i(dt, limit);
101 :
102 19442525 : _pid_info_x.target = _target.x;
103 19442525 : _pid_info_x.actual = measurement.x;
104 19442525 : _pid_info_x.error = _error.x;
105 19442525 : _pid_info_x.P = _error.x * _kp;
106 19442525 : _pid_info_x.I = _integrator.x;
107 19442525 : _pid_info_x.D = _derivative.x * _kd;
108 19442525 : _pid_info_x.FF = _target.x * _kff;
109 :
110 19442525 : _pid_info_y.target = _target.y;
111 19442525 : _pid_info_y.actual = measurement.y;
112 19442525 : _pid_info_y.error = _error.y;
113 19442525 : _pid_info_y.P = _error.y * _kp;
114 19442525 : _pid_info_y.I = _integrator.y;
115 19442525 : _pid_info_y.D = _derivative.y * _kd;
116 19442525 : _pid_info_y.FF = _target.y * _kff;
117 :
118 : // Return total control output: P + I + D + FF terms
119 19442525 : return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
120 : }
121 :
122 0 : Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit)
123 : {
124 0 : return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, dt, Vector2f{limit.x, limit.y});
125 : }
126 :
127 : // Updates the 2D integrator using the filtered error.
128 : // The integrator is only allowed to grow if it does not push further in the direction of the limit vector.
129 19442525 : void AC_PID_2D::update_i(float dt, const Vector2f &limit)
130 : {
131 19442525 : _pid_info_x.limit = false;
132 19442525 : _pid_info_y.limit = false;
133 :
134 19442525 : Vector2f delta_integrator = (_error * _ki) * dt;
135 19442525 : float integrator_length = _integrator.length();
136 19442525 : _integrator += delta_integrator;
137 : // Compute integrator delta and apply anti-windup by limiting growth in the direction of the limit vector
138 19442525 : if (is_positive(delta_integrator * limit) && _integrator.limit_length(integrator_length)) {
139 363303 : _pid_info_x.limit = true;
140 363303 : _pid_info_y.limit = true;
141 : }
142 :
143 : // Clamp integrator to maximum length (IMAX)
144 19442525 : _integrator.limit_length(_kimax);
145 19442525 : }
146 :
147 0 : Vector2f AC_PID_2D::get_p() const
148 : {
149 0 : return _error * _kp;
150 : }
151 :
152 178458 : const Vector2f& AC_PID_2D::get_i() const
153 : {
154 178458 : return _integrator;
155 : }
156 :
157 0 : Vector2f AC_PID_2D::get_d() const
158 : {
159 0 : return _derivative * _kd;
160 : }
161 :
162 : // Update FF terms in PID logs and return feedforward vector
163 0 : Vector2f AC_PID_2D::get_ff()
164 : {
165 0 : _pid_info_x.FF = _target.x * _kff;
166 0 : _pid_info_y.FF = _target.y * _kff;
167 0 : return _target * _kff;
168 : }
169 :
170 80 : void AC_PID_2D::reset_I()
171 : {
172 80 : _integrator.zero();
173 80 : }
174 :
175 : // Saves controller configuration from EEPROM, including gains and filter frequencies. (not used)
176 0 : void AC_PID_2D::save_gains()
177 : {
178 0 : _kp.save();
179 0 : _ki.save();
180 0 : _kd.save();
181 0 : _kff.save();
182 0 : _kimax.save();
183 0 : _filt_E_hz.save();
184 0 : _filt_D_hz.save();
185 0 : }
186 :
187 : // Returns alpha value for the error low-pass filter (based on filter frequency and dt)
188 17124568 : float AC_PID_2D::get_filt_E_alpha(float dt) const
189 : {
190 17124568 : return calc_lowpass_alpha_dt(dt, _filt_E_hz);
191 : }
192 :
193 : // Returns alpha value for the derivative low-pass filter (based on filter frequency and dt)
194 17113242 : float AC_PID_2D::get_filt_D_alpha(float dt) const
195 : {
196 17113242 : return calc_lowpass_alpha_dt(dt, _filt_D_hz);
197 : }
198 :
199 : // Compute error from target and measurement, then set integrator
200 0 : void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)
201 : {
202 0 : set_integrator(target - measurement, i);
203 0 : }
204 :
205 : // Convert from desired total output and error to integrator value
206 0 : void AC_PID_2D::set_integrator(const Vector2f& error, const Vector2f& i)
207 : {
208 0 : set_integrator(i - error * _kp);
209 0 : }
210 :
211 : // Set integrator directly and clamp to IMAX
212 3887333 : void AC_PID_2D::set_integrator(const Vector2f& i)
213 : {
214 3887333 : _integrator = i;
215 3887333 : _integrator.limit_length(_kimax);
216 3887333 : }
217 :
|
