Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : // Code by Jon Challinger
17 : // Modified by Paul Riseborough to implement a three loop autopilot
18 : // topology
19 : //
20 : #include <AP_HAL/AP_HAL.h>
21 : #include "AP_YawController.h"
22 : #include <AP_AHRS/AP_AHRS.h>
23 : #include <GCS_MAVLink/GCS.h>
24 : #include <AP_Scheduler/AP_Scheduler.h>
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 : const AP_Param::GroupInfo AP_YawController::var_info[] = {
29 :
30 : // @Param: 2SRV_SLIP
31 : // @DisplayName: Sideslip control gain
32 : // @Description: Gain from lateral acceleration to demanded yaw rate for aircraft with enough fuselage area to detect lateral acceleration and sideslips. Do not enable for flying wings and gliders. Actively coordinates flight more than just yaw damping. Set after YAW2SRV_DAMP and YAW2SRV_INT are tuned.
33 : // @Range: 0 4
34 : // @Increment: 0.25
35 : // @User: Advanced
36 : AP_GROUPINFO("2SRV_SLIP", 0, AP_YawController, _K_A, 0),
37 :
38 : // @Param: 2SRV_INT
39 : // @DisplayName: Sideslip control integrator
40 : // @Description: Integral gain from lateral acceleration error. Effectively trims rudder to eliminate long-term sideslip.
41 : // @Range: 0 2
42 : // @Increment: 0.25
43 : // @User: Advanced
44 : AP_GROUPINFO("2SRV_INT", 1, AP_YawController, _K_I, 0),
45 :
46 : // @Param: 2SRV_DAMP
47 : // @DisplayName: Yaw damping
48 : // @Description: Gain from yaw rate to rudder. Most effective at yaw damping and should be tuned after KFF_RDDRMIX. Also disables YAW2SRV_INT if set to 0.
49 : // @Range: 0 2
50 : // @Increment: 0.25
51 : // @User: Advanced
52 : AP_GROUPINFO("2SRV_DAMP", 2, AP_YawController, _K_D, 0),
53 :
54 : // @Param: 2SRV_RLL
55 : // @DisplayName: Yaw coordination gain
56 : // @Description: Gain to the yaw rate required to keep it consistent with the turn rate in a coordinated turn. Corrects for yaw tendencies after the turn is established. Increase yaw into the turn by raising. Increase yaw out of the turn by decreasing. Values outside of 0.9-1.1 range indicate airspeed calibration problems.
57 : // @Range: 0.8 1.2
58 : // @Increment: 0.05
59 : // @User: Advanced
60 : AP_GROUPINFO("2SRV_RLL", 3, AP_YawController, _K_FF, 1),
61 :
62 : /*
63 : Note: index 4 should not be used - it was used for an incorrect
64 : AP_Int8 version of the IMAX in the 2.74 release
65 : */
66 :
67 : // @Param: 2SRV_IMAX
68 : // @DisplayName: Integrator limit
69 : // @Description: Limit of yaw integrator gain in centi-degrees of servo travel. Servos are assumed to have +/- 4500 centi-degrees of travel, so a value of 1500 allows trim of up to 1/3 of servo travel range.
70 : // @Range: 0 4500
71 : // @Increment: 1
72 : // @User: Advanced
73 : AP_GROUPINFO("2SRV_IMAX", 5, AP_YawController, _imax, 1500),
74 :
75 : // @Param: _RATE_ENABLE
76 : // @DisplayName: Yaw rate enable
77 : // @Description: Enable yaw rate controller for aerobatic flight
78 : // @Values: 0:Disable,1:Enable
79 : // @User: Advanced
80 : AP_GROUPINFO_FLAGS("_RATE_ENABLE", 6, AP_YawController, _rate_enable, 0, AP_PARAM_FLAG_ENABLE),
81 :
82 : // @Param: _RATE_P
83 : // @DisplayName: Yaw axis rate controller P gain
84 : // @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
85 : // @Range: 0.08 0.35
86 : // @Increment: 0.005
87 : // @User: Standard
88 :
89 : // @Param: _RATE_I
90 : // @DisplayName: Yaw axis rate controller I gain
91 : // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
92 : // @Range: 0.01 0.6
93 : // @Increment: 0.01
94 : // @User: Standard
95 :
96 : // @Param: _RATE_IMAX
97 : // @DisplayName: Yaw axis rate controller I gain maximum
98 : // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
99 : // @Range: 0 1
100 : // @Increment: 0.01
101 : // @User: Standard
102 :
103 : // @Param: _RATE_D
104 : // @DisplayName: Yaw axis rate controller D gain
105 : // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
106 : // @Range: 0.001 0.03
107 : // @Increment: 0.001
108 : // @User: Standard
109 :
110 : // @Param: _RATE_FF
111 : // @DisplayName: Yaw axis rate controller feed forward
112 : // @Description: Yaw axis rate controller feed forward
113 : // @Range: 0 3.0
114 : // @Increment: 0.001
115 : // @User: Standard
116 :
117 : // @Param: _RATE_FLTT
118 : // @DisplayName: Yaw axis rate controller target frequency in Hz
119 : // @Description: Yaw axis rate controller target frequency in Hz
120 : // @Range: 2 50
121 : // @Increment: 1
122 : // @Units: Hz
123 : // @User: Standard
124 :
125 : // @Param: _RATE_FLTE
126 : // @DisplayName: Yaw axis rate controller error frequency in Hz
127 : // @Description: Yaw axis rate controller error frequency in Hz
128 : // @Range: 2 50
129 : // @Increment: 1
130 : // @Units: Hz
131 : // @User: Standard
132 :
133 : // @Param: _RATE_FLTD
134 : // @DisplayName: Yaw axis rate controller derivative frequency in Hz
135 : // @Description: Yaw axis rate controller derivative frequency in Hz
136 : // @Range: 0 50
137 : // @Increment: 1
138 : // @Units: Hz
139 : // @User: Standard
140 :
141 : // @Param: _RATE_SMAX
142 : // @DisplayName: Yaw slew rate limit
143 : // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
144 : // @Range: 0 200
145 : // @Increment: 0.5
146 : // @User: Advanced
147 :
148 : // @Param: _RATE_PDMX
149 : // @DisplayName: Yaw axis rate controller PD sum maximum
150 : // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
151 : // @Range: 0 1
152 : // @Increment: 0.01
153 :
154 : // @Param: _RATE_D_FF
155 : // @DisplayName: Yaw Derivative FeedForward Gain
156 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
157 : // @Range: 0 0.03
158 : // @Increment: 0.001
159 : // @User: Advanced
160 :
161 : // @Param: _RATE_NTF
162 : // @DisplayName: Yaw Target notch filter index
163 : // @Description: Yaw Target notch filter index, zero disables
164 : // @Range: 0 8
165 : // @User: Advanced
166 :
167 : // @Param: _RATE_NEF
168 : // @DisplayName: Yaw Error notch filter index
169 : // @Description: Yaw Error notch filter index, zero disables
170 : // @Range: 0 8
171 : // @User: Advanced
172 :
173 : AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_YawController, AC_PID),
174 :
175 : AP_GROUPEND
176 : };
177 :
178 692 : AP_YawController::AP_YawController(const AP_FixedWing &parms)
179 692 : : aparm(parms)
180 : {
181 692 : AP_Param::setup_object_defaults(this, var_info);
182 692 : _pid_info.target = 0;
183 692 : _pid_info.FF = 0;
184 692 : _pid_info.P = 0;
185 692 : rate_pid.set_slew_limit_scale(45);
186 692 : }
187 :
188 6292312 : int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
189 : {
190 6292312 : uint32_t tnow = AP_HAL::millis();
191 6292312 : uint32_t dt = tnow - _last_t;
192 6292312 : if (_last_t == 0 || dt > 1000) {
193 468 : dt = 0;
194 468 : _pid_info.I = 0;
195 : }
196 6292312 : _last_t = tnow;
197 :
198 :
199 6292312 : int16_t aspd_min = aparm.airspeed_min;
200 6292312 : if (aspd_min < 1) {
201 0 : aspd_min = 1;
202 : }
203 :
204 6292312 : float delta_time = (float) dt * 0.001f;
205 :
206 : // Calculate yaw rate required to keep up with a constant height coordinated turn
207 : float aspeed;
208 : float rate_offset;
209 6292312 : float bank_angle = AP::ahrs().get_roll_rad();
210 : // limit bank angle between +- 80 deg if right way up
211 6292312 : if (fabsf(bank_angle) < 1.5707964f) {
212 6244383 : bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
213 : }
214 6292312 : const AP_AHRS &_ahrs = AP::ahrs();
215 6292312 : if (!_ahrs.airspeed_EAS(aspeed)) {
216 : // If no airspeed available use average of min and max
217 31021 : aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max));
218 : }
219 6292312 : rate_offset = (GRAVITY_MSS / MAX(aspeed, float(aspd_min))) * sinf(bank_angle) * _K_FF;
220 :
221 : // Get body rate vector (radians/sec)
222 6292312 : float omega_z = _ahrs.get_gyro().z;
223 :
224 : // Get the accln vector (m/s^2)
225 6292312 : float accel_y = AP::ins().get_accel().y;
226 :
227 : // subtract current bias estimate from EKF
228 6292312 : const Vector3f &abias = _ahrs.get_accel_bias();
229 6292312 : accel_y -= abias.y;
230 :
231 : // Subtract the steady turn component of rate from the measured rate
232 : // to calculate the rate relative to the turn requirement in degrees/sec
233 6292312 : float rate_hp_in = degrees(omega_z - rate_offset);
234 :
235 : // Apply a high-pass filter to the rate to washout any steady state error
236 : // due to bias errors in rate_offset
237 : // Use a cut-off frequency of omega = 0.2 rad/sec
238 : // Could make this adjustable by replacing 0.9960080 with (1 - omega * dt)
239 6292312 : float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;
240 6292312 : _last_rate_hp_out = rate_hp_out;
241 6292312 : _last_rate_hp_in = rate_hp_in;
242 :
243 : //Calculate input to integrator
244 6292312 : float integ_in = - _K_I * (_K_A * accel_y + rate_hp_out);
245 :
246 : // Apply integrator, but clamp input to prevent control saturation and freeze integrator below min FBW speed
247 : // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
248 : // Don't integrate if _K_D is zero as integrator will keep winding up
249 6292312 : if (!disable_integrator && _K_D > 0) {
250 : //only integrate if airspeed above min value
251 0 : if (aspeed > float(aspd_min)) {
252 : // prevent the integrator from increasing if surface defln demand is above the upper limit
253 0 : if (_last_out < -45) {
254 0 : _integrator += MAX(integ_in * delta_time, 0);
255 0 : } else if (_last_out > 45) {
256 : // prevent the integrator from decreasing if surface defln demand is below the lower limit
257 0 : _integrator += MIN(integ_in * delta_time, 0);
258 : } else {
259 0 : _integrator += integ_in * delta_time;
260 : }
261 : }
262 : } else {
263 6292312 : _integrator = 0;
264 : }
265 :
266 6292312 : if (_K_D < 0.0001f) {
267 : // yaw damping is disabled, and the integrator is scaled by damping, so return 0
268 6292312 : return 0;
269 : }
270 :
271 : // Scale the integration limit
272 0 : float intLimScaled = _imax * 0.01f / (_K_D * scaler * scaler);
273 :
274 : // Constrain the integrator state
275 0 : _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
276 :
277 : // Protect against increases to _K_D during in-flight tuning from creating large control transients
278 : // due to stored integrator values
279 0 : if (_K_D > _K_D_last && _K_D > 0) {
280 0 : _integrator = _K_D_last/_K_D * _integrator;
281 : }
282 0 : _K_D_last = _K_D;
283 :
284 : // Calculate demanded rudder deflection, +Ve deflection yaws nose right
285 : // Save to last value before application of limiter so that integrator limiting
286 : // can detect exceedance next frame
287 : // Scale using inverse dynamic pressure (1/V^2)
288 0 : _pid_info.I = _K_D * _integrator * scaler * scaler;
289 0 : _pid_info.D = _K_D * (-rate_hp_out) * scaler * scaler;
290 0 : _last_out = _pid_info.I + _pid_info.D;
291 :
292 : // Convert to centi-degrees and constrain
293 0 : return constrain_float(_last_out * 100, -4500, 4500);
294 : }
295 :
296 : // get actuator output for direct rate control
297 : // desired_rate is in deg/sec. scaler is the surface speed scaler
298 6655 : float AP_YawController::get_rate_out(float desired_rate, float scaler, bool disable_integrator)
299 : {
300 6655 : const AP_AHRS &_ahrs = AP::ahrs();
301 :
302 6655 : const float dt = AP::scheduler().get_loop_period_s();
303 6655 : const float eas2tas = _ahrs.get_EAS2TAS();
304 6655 : bool limit_I = fabsf(_last_out) >= 45 || disable_integrator;
305 6655 : float rate_z = _ahrs.get_gyro().z;
306 : float aspeed;
307 6655 : float old_I = rate_pid.get_i();
308 :
309 6655 : if (!_ahrs.airspeed_EAS(aspeed)) {
310 0 : aspeed = 0;
311 : }
312 6655 : bool underspeed = aspeed <= float(aparm.airspeed_min);
313 6655 : if (underspeed) {
314 0 : limit_I = true;
315 : }
316 :
317 : // the PID elements are scaled by sq(scaler). To use an
318 : // unmodified AC_PID object we scale the inputs (target and measurement)
319 : //
320 : // note that we run AC_PID in radians so that the normal scaling
321 : // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
322 6655 : rate_pid.update_all(radians(desired_rate) * scaler * scaler, rate_z * scaler * scaler, dt, limit_I);
323 :
324 6655 : if (underspeed) {
325 : // when underspeed we lock the integrator
326 0 : rate_pid.set_integrator(old_I);
327 : }
328 :
329 : // FF and DFF should be scaled by scaler/eas2tas, but since we have scaled
330 : // the AC_PID target above by scaler*scaler we need to instead
331 : // divide by scaler*eas2tas to get the right scaling
332 6655 : const float ff = degrees(rate_pid.get_ff_component() / (scaler * eas2tas));
333 6655 : const float dff = degrees(rate_pid.get_dff_component() / (scaler * eas2tas));
334 :
335 6655 : if (disable_integrator) {
336 0 : rate_pid.reset_I();
337 : }
338 :
339 : // convert AC_PID info object to same scale as old controller
340 6655 : _pid_info = rate_pid.get_pid_info();
341 6655 : auto &pinfo = _pid_info;
342 :
343 6655 : const float deg_scale = degrees(1);
344 6655 : pinfo.FF = ff;
345 6655 : pinfo.P *= deg_scale;
346 6655 : pinfo.I *= deg_scale;
347 6655 : pinfo.D *= deg_scale;
348 6655 : pinfo.DFF = dff;
349 6655 : pinfo.limit = limit_I;
350 :
351 : // fix the logged target and actual values to not have the scalers applied
352 6655 : pinfo.target = desired_rate;
353 6655 : pinfo.actual = degrees(rate_z);
354 :
355 : // sum components
356 6655 : float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
357 :
358 : // remember the last output to trigger the I limit
359 6655 : _last_out = out;
360 :
361 6655 : if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {
362 : // fake up an angular error based on a notional time constant of 0.5s
363 0 : const float angle_err_deg = desired_rate * gains.tau;
364 : // let autotune have a go at the values
365 0 : autotune->update(pinfo, scaler, angle_err_deg);
366 : }
367 :
368 : // output is scaled to notional centidegrees of deflection
369 6655 : return constrain_float(out * 100, -4500, 4500);
370 : }
371 :
372 2633847 : void AP_YawController::reset_I()
373 : {
374 2633847 : _pid_info.I = 0;
375 2633847 : rate_pid.reset_I();
376 2633847 : _integrator = 0;
377 2633847 : }
378 :
379 6297553 : void AP_YawController::reset_rate_PID()
380 : {
381 6297553 : rate_pid.reset_I();
382 6297553 : rate_pid.reset_filter();
383 6297553 : }
384 :
385 : /*
386 : start an autotune
387 : */
388 4 : void AP_YawController::autotune_start(void)
389 : {
390 4 : if (autotune == nullptr && rate_control_enabled()) {
391 : // the autotuner needs a time constant. Use an assumed tconst of 0.5
392 0 : gains.tau.set(0.5);
393 0 : gains.rmax_pos.set(90);
394 :
395 0 : autotune = NEW_NOTHROW AP_AutoTune(gains, AP_AutoTune::AUTOTUNE_YAW, aparm, rate_pid);
396 0 : if (autotune == nullptr) {
397 0 : if (!failed_autotune_alloc) {
398 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed yaw allocation");
399 : }
400 0 : failed_autotune_alloc = true;
401 : }
402 : }
403 4 : if (autotune != nullptr) {
404 0 : autotune->start();
405 : }
406 4 : }
407 :
408 : /*
409 : restore autotune gains
410 : */
411 1924 : void AP_YawController::autotune_restore(void)
412 : {
413 1924 : if (autotune != nullptr) {
414 0 : autotune->stop();
415 : }
416 1924 : }
|
