Line data Source code
1 : #include "AC_CustomControl_config.h"
2 :
3 : #if AP_COPTER_CUSTOMCONTROL_PID_ENABLED
4 :
5 : #include "AC_CustomControl_PID.h"
6 : #include "AC_AttitudeControl/AC_AttitudeControl_Multi.h"
7 :
8 : // table of user settable parameters
9 : const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = {
10 : // @Param: ANG_RLL_P
11 : // @DisplayName: Roll axis angle controller P gain
12 : // @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
13 : // @Range: 3.000 12.000
14 : // @Range{Sub}: 0.0 12.000
15 : // @User: Standard
16 : AP_SUBGROUPINFO(_p_angle_roll2, "ANG_RLL_", 1, AC_CustomControl_PID, AC_P),
17 :
18 : // @Param: ANG_PIT_P
19 : // @DisplayName: Pitch axis angle controller P gain
20 : // @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
21 : // @Range: 3.000 12.000
22 : // @Range{Sub}: 0.0 12.000
23 : // @User: Standard
24 : AP_SUBGROUPINFO(_p_angle_pitch2, "ANG_PIT_", 2, AC_CustomControl_PID, AC_P),
25 :
26 : // @Param: ANG_YAW_P
27 : // @DisplayName: Yaw axis angle controller P gain
28 : // @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
29 : // @Range: 3.000 12.000
30 : // @Range{Sub}: 0.0 6.000
31 : // @User: Standard
32 : AP_SUBGROUPINFO(_p_angle_yaw2, "ANG_YAW_", 3, AC_CustomControl_PID, AC_P),
33 :
34 :
35 : // @Param: RAT_RLL_P
36 : // @DisplayName: Roll axis rate controller P gain
37 : // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
38 : // @Range: 0.01 0.5
39 : // @Increment: 0.005
40 : // @User: Standard
41 :
42 : // @Param: RAT_RLL_I
43 : // @DisplayName: Roll axis rate controller I gain
44 : // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
45 : // @Range: 0.01 2.0
46 : // @Increment: 0.01
47 : // @User: Standard
48 :
49 : // @Param: RAT_RLL_IMAX
50 : // @DisplayName: Roll axis rate controller I gain maximum
51 : // @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
52 : // @Range: 0 1
53 : // @Increment: 0.01
54 : // @User: Standard
55 :
56 : // @Param: RAT_RLL_D
57 : // @DisplayName: Roll axis rate controller D gain
58 : // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
59 : // @Range: 0.0 0.05
60 : // @Increment: 0.001
61 : // @User: Standard
62 :
63 : // @Param: RAT_RLL_FF
64 : // @DisplayName: Roll axis rate controller feed forward
65 : // @Description: Roll axis rate controller feed forward
66 : // @Range: 0 0.5
67 : // @Increment: 0.001
68 : // @User: Standard
69 :
70 : // @Param: RAT_RLL_FLTT
71 : // @DisplayName: Roll axis rate controller target frequency in Hz
72 : // @Description: Roll axis rate controller target frequency in Hz
73 : // @Range: 5 100
74 : // @Increment: 1
75 : // @Units: Hz
76 : // @User: Standard
77 :
78 : // @Param: RAT_RLL_FLTE
79 : // @DisplayName: Roll axis rate controller error frequency in Hz
80 : // @Description: Roll axis rate controller error frequency in Hz
81 : // @Range: 0 100
82 : // @Increment: 1
83 : // @Units: Hz
84 : // @User: Standard
85 :
86 : // @Param: RAT_RLL_FLTD
87 : // @DisplayName: Roll axis rate controller derivative frequency in Hz
88 : // @Description: Roll axis rate controller derivative frequency in Hz
89 : // @Range: 5 100
90 : // @Increment: 1
91 : // @Units: Hz
92 : // @User: Standard
93 :
94 : // @Param: RAT_RLL_SMAX
95 : // @DisplayName: Roll slew rate limit
96 : // @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.
97 : // @Range: 0 200
98 : // @Increment: 0.5
99 : // @User: Advanced
100 :
101 : // @Param: RAT_RLL_PDMX
102 : // @DisplayName: Roll axis rate controller PD sum maximum
103 : // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
104 : // @Range: 0 1
105 : // @Increment: 0.01
106 : // @User: Advanced
107 :
108 : // @Param: RAT_RLL_D_FF
109 : // @DisplayName: Roll Derivative FeedForward Gain
110 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
111 : // @Range: 0 0.02
112 : // @Increment: 0.0001
113 : // @User: Advanced
114 :
115 : // @Param: RAT_RLL_NTF
116 : // @DisplayName: Roll Target notch filter index
117 : // @Description: Roll Target notch filter index, zero disables
118 : // @Range: 0 8
119 : // @User: Advanced
120 :
121 : // @Param: RAT_RLL_NEF
122 : // @DisplayName: Roll Error notch filter index
123 : // @Description: Roll Error notch filter index, zero disables
124 : // @Range: 0 8
125 : // @User: Advanced
126 :
127 : AP_SUBGROUPINFO(_pid_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, AC_PID),
128 :
129 : // @Param: RAT_PIT_P
130 : // @DisplayName: Pitch axis rate controller P gain
131 : // @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
132 : // @Range: 0.01 0.50
133 : // @Increment: 0.005
134 : // @User: Standard
135 :
136 : // @Param: RAT_PIT_I
137 : // @DisplayName: Pitch axis rate controller I gain
138 : // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
139 : // @Range: 0.01 2.0
140 : // @Increment: 0.01
141 : // @User: Standard
142 :
143 : // @Param: RAT_PIT_IMAX
144 : // @DisplayName: Pitch axis rate controller I gain maximum
145 : // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
146 : // @Range: 0 1
147 : // @Increment: 0.01
148 : // @User: Standard
149 :
150 : // @Param: RAT_PIT_D
151 : // @DisplayName: Pitch axis rate controller D gain
152 : // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
153 : // @Range: 0.0 0.05
154 : // @Increment: 0.001
155 : // @User: Standard
156 :
157 : // @Param: RAT_PIT_FF
158 : // @DisplayName: Pitch axis rate controller feed forward
159 : // @Description: Pitch axis rate controller feed forward
160 : // @Range: 0 0.5
161 : // @Increment: 0.001
162 : // @User: Standard
163 :
164 : // @Param: RAT_PIT_FLTT
165 : // @DisplayName: Pitch axis rate controller target frequency in Hz
166 : // @Description: Pitch axis rate controller target frequency in Hz
167 : // @Range: 5 100
168 : // @Increment: 1
169 : // @Units: Hz
170 : // @User: Standard
171 :
172 : // @Param: RAT_PIT_FLTE
173 : // @DisplayName: Pitch axis rate controller error frequency in Hz
174 : // @Description: Pitch axis rate controller error frequency in Hz
175 : // @Range: 0 100
176 : // @Increment: 1
177 : // @Units: Hz
178 : // @User: Standard
179 :
180 : // @Param: RAT_PIT_FLTD
181 : // @DisplayName: Pitch axis rate controller derivative frequency in Hz
182 : // @Description: Pitch axis rate controller derivative frequency in Hz
183 : // @Range: 5 100
184 : // @Increment: 1
185 : // @Units: Hz
186 : // @User: Standard
187 :
188 : // @Param: RAT_PIT_SMAX
189 : // @DisplayName: Pitch slew rate limit
190 : // @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.
191 : // @Range: 0 200
192 : // @Increment: 0.5
193 : // @User: Advanced
194 :
195 : // @Param: RAT_PIT_PDMX
196 : // @DisplayName: Pitch axis rate controller PD sum maximum
197 : // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
198 : // @Range: 0 1
199 : // @Increment: 0.01
200 : // @User: Advanced
201 :
202 : // @Param: RAT_PIT_D_FF
203 : // @DisplayName: Pitch Derivative FeedForward Gain
204 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
205 : // @Range: 0 0.02
206 : // @Increment: 0.0001
207 : // @User: Advanced
208 :
209 : // @Param: RAT_PIT_NTF
210 : // @DisplayName: Pitch Target notch filter index
211 : // @Description: Pitch Target notch filter index, zero disables
212 : // @Range: 0 8
213 : // @User: Advanced
214 :
215 : // @Param: RAT_PIT_NEF
216 : // @DisplayName: Pitch Error notch filter index
217 : // @Description: Pitch Error notch filter index, zero disables
218 : // @Range: 0 8
219 : // @User: Advanced
220 :
221 : AP_SUBGROUPINFO(_pid_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, AC_PID),
222 :
223 :
224 : // @Param: RAT_YAW_P
225 : // @DisplayName: Yaw axis rate controller P gain
226 : // @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
227 : // @Range: 0.10 2.50
228 : // @Increment: 0.005
229 : // @User: Standard
230 :
231 : // @Param: RAT_YAW_I
232 : // @DisplayName: Yaw axis rate controller I gain
233 : // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
234 : // @Range: 0.010 1.0
235 : // @Increment: 0.01
236 : // @User: Standard
237 :
238 : // @Param: RAT_YAW_IMAX
239 : // @DisplayName: Yaw axis rate controller I gain maximum
240 : // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
241 : // @Range: 0 1
242 : // @Increment: 0.01
243 : // @User: Standard
244 :
245 : // @Param: RAT_YAW_D
246 : // @DisplayName: Yaw axis rate controller D gain
247 : // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
248 : // @Range: 0.000 0.02
249 : // @Increment: 0.001
250 : // @User: Standard
251 :
252 : // @Param: RAT_YAW_FF
253 : // @DisplayName: Yaw axis rate controller feed forward
254 : // @Description: Yaw axis rate controller feed forward
255 : // @Range: 0 0.5
256 : // @Increment: 0.001
257 : // @User: Standard
258 :
259 : // @Param: RAT_YAW_FLTT
260 : // @DisplayName: Yaw axis rate controller target frequency in Hz
261 : // @Description: Yaw axis rate controller target frequency in Hz
262 : // @Range: 1 50
263 : // @Increment: 1
264 : // @Units: Hz
265 : // @User: Standard
266 :
267 : // @Param: RAT_YAW_FLTE
268 : // @DisplayName: Yaw axis rate controller error frequency in Hz
269 : // @Description: Yaw axis rate controller error frequency in Hz
270 : // @Range: 0 20
271 : // @Increment: 1
272 : // @Units: Hz
273 : // @User: Standard
274 :
275 : // @Param: RAT_YAW_FLTD
276 : // @DisplayName: Yaw axis rate controller derivative frequency in Hz
277 : // @Description: Yaw axis rate controller derivative frequency in Hz
278 : // @Range: 5 50
279 : // @Increment: 1
280 : // @Units: Hz
281 : // @User: Standard
282 :
283 : // @Param: RAT_YAW_SMAX
284 : // @DisplayName: Yaw slew rate limit
285 : // @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.
286 : // @Range: 0 200
287 : // @Increment: 0.5
288 : // @User: Advanced
289 :
290 : // @Param: RAT_YAW_PDMX
291 : // @DisplayName: Yaw axis rate controller PD sum maximum
292 : // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
293 : // @Range: 0 1
294 : // @Increment: 0.01
295 : // @User: Advanced
296 :
297 : // @Param: RAT_YAW_D_FF
298 : // @DisplayName: Yaw Derivative FeedForward Gain
299 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
300 : // @Range: 0 0.02
301 : // @Increment: 0.0001
302 : // @User: Advanced
303 :
304 : // @Param: RAT_YAW_NTF
305 : // @DisplayName: Yaw Target notch filter index
306 : // @Description: Yaw Target notch filter index, zero disables
307 : // @Range: 0 8
308 : // @User: Advanced
309 :
310 : // @Param: RAT_YAW_NEF
311 : // @DisplayName: Yaw Error notch filter index
312 : // @Description: Yaw Error notch filter index, zero disables
313 : // @Range: 0 8
314 : // @User: Advanced
315 :
316 : AP_SUBGROUPINFO(_pid_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID),
317 :
318 : AP_GROUPEND
319 : };
320 :
321 2 : AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) :
322 : AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt),
323 4 : _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
324 2 : _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
325 2 : _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
326 2 : _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f),
327 2 : _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f),
328 4 : _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f)
329 : {
330 2 : _dt = dt;
331 2 : AP_Param::setup_object_defaults(this, var_info);
332 2 : }
333 :
334 8400 : Vector3f AC_CustomControl_PID::update()
335 : {
336 : // reset controller based on spool state
337 8400 : switch (_motors->get_spool_state()) {
338 0 : case AP_Motors::SpoolState::SHUT_DOWN:
339 : case AP_Motors::SpoolState::GROUND_IDLE:
340 : // We are still at the ground. Reset custom controller to avoid
341 : // build up, ex: integrator
342 0 : reset();
343 0 : break;
344 :
345 8400 : case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
346 : case AP_Motors::SpoolState::SPOOLING_UP:
347 : case AP_Motors::SpoolState::SPOOLING_DOWN:
348 : // we are off the ground
349 8400 : break;
350 : }
351 :
352 : // run custom controller after here
353 8400 : Quaternion attitude_body, attitude_target;
354 8400 : _ahrs->get_quat_body_to_ned(attitude_body);
355 :
356 8400 : attitude_target = _att_control->get_attitude_target_quat();
357 : // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
358 8400 : Vector3f attitude_error;
359 : float _thrust_angle_rad, _thrust_error_angle_rad;
360 8400 : _att_control->thrust_heading_rotation_angles(attitude_target, attitude_body, attitude_error, _thrust_angle_rad, _thrust_error_angle_rad);
361 :
362 : // recalculate ang vel feedforward from attitude target model
363 : // rotation from the target frame to the body frame
364 8400 : Quaternion rotation_target_to_body = attitude_body.inverse() * attitude_target;
365 : // target angle velocity vector in the body frame
366 8400 : Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel();
367 :
368 : // run attitude controller
369 8400 : Vector3f target_rate;
370 8400 : target_rate[0] = _p_angle_roll2.kP() * attitude_error.x + ang_vel_body_feedforward[0];
371 8400 : target_rate[1] = _p_angle_pitch2.kP() * attitude_error.y + ang_vel_body_feedforward[1];
372 8400 : target_rate[2] = _p_angle_yaw2.kP() * attitude_error.z + ang_vel_body_feedforward[2];
373 :
374 : // run rate controller
375 8400 : Vector3f gyro_latest = _ahrs->get_gyro_latest();
376 8400 : Vector3f motor_out;
377 8400 : motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], _dt, false);
378 8400 : motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], _dt, false);
379 8400 : motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], _dt, false);
380 :
381 8400 : return motor_out;
382 : }
383 :
384 : // This example uses exact same controller architecture as ArduCopter attitude controller without all the safe guard against saturation.
385 : // The gains are scaled 0.9 times to better detect switch over response.
386 : // Note that integrator are not reset correctly as it is done in reset_main_att_controller inside AC_CustomControl.cpp
387 : // This is done intentionally to demonstrate switch over performance of two exact controller with different reset handling.
388 1 : void AC_CustomControl_PID::reset(void)
389 : {
390 1 : _pid_atti_rate_roll.reset_I();
391 1 : _pid_atti_rate_pitch.reset_I();
392 1 : _pid_atti_rate_yaw.reset_I();
393 1 : _pid_atti_rate_roll.reset_filter();
394 1 : _pid_atti_rate_pitch.reset_filter();
395 1 : _pid_atti_rate_yaw.reset_filter();
396 1 : }
397 :
398 :
399 117 : void AC_CustomControl_PID::set_notch_sample_rate(float sample_rate)
400 : {
401 : #if AP_FILTER_ENABLED
402 117 : _pid_atti_rate_roll.set_notch_sample_rate(sample_rate);
403 117 : _pid_atti_rate_pitch.set_notch_sample_rate(sample_rate);
404 117 : _pid_atti_rate_yaw.set_notch_sample_rate(sample_rate);
405 : #endif
406 117 : }
407 :
408 : #endif // AP_COPTER_CUSTOMCONTROL_PID_ENABLED
|
