Line data Source code
1 : #include "AC_AttitudeControl_Multi.h"
2 : #include <AP_HAL/AP_HAL.h>
3 : #include <AP_Math/AP_Math.h>
4 : #include <AC_PID/AC_PID.h>
5 : #include <AP_Scheduler/AP_Scheduler.h>
6 :
7 : // table of user settable parameters
8 : const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
9 : // parameters from parent vehicle
10 : AP_NESTEDGROUPINFO(AC_AttitudeControl, 0),
11 :
12 : // @Param: RAT_RLL_P
13 : // @DisplayName: Roll axis rate controller P gain
14 : // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
15 : // @Units: s/rad
16 : // @Range: 0.01 0.5
17 : // @Increment: 0.005
18 : // @User: Standard
19 :
20 : // @Param: RAT_RLL_I
21 : // @DisplayName: Roll axis rate controller I gain
22 : // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
23 : // @Units: 1/rad
24 : // @Range: 0.01 2.0
25 : // @Increment: 0.01
26 : // @User: Standard
27 :
28 : // @Param: RAT_RLL_IMAX
29 : // @DisplayName: Roll axis rate controller I gain maximum
30 : // @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
31 : // @Range: 0 1
32 : // @Increment: 0.01
33 : // @User: Standard
34 :
35 : // @Param: RAT_RLL_D
36 : // @DisplayName: Roll axis rate controller D gain
37 : // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
38 : // @Units: s^2/rad
39 : // @Range: 0.0 0.05
40 : // @Increment: 0.001
41 : // @User: Standard
42 :
43 : // @Param: RAT_RLL_FF
44 : // @DisplayName: Roll axis rate controller feed forward
45 : // @Description: Roll axis rate controller feed forward
46 : // @Units: s/rad
47 : // @Range: 0 0.5
48 : // @Increment: 0.001
49 : // @User: Standard
50 :
51 : // @Param: RAT_RLL_FLTT
52 : // @DisplayName: Roll axis rate controller target frequency in Hz
53 : // @Description: Roll axis rate controller target frequency in Hz
54 : // @Range: 5 100
55 : // @Increment: 1
56 : // @Units: Hz
57 : // @User: Standard
58 :
59 : // @Param: RAT_RLL_FLTE
60 : // @DisplayName: Roll axis rate controller error frequency in Hz
61 : // @Description: Roll axis rate controller error frequency in Hz
62 : // @Range: 0 100
63 : // @Increment: 1
64 : // @Units: Hz
65 : // @User: Standard
66 :
67 : // @Param: RAT_RLL_FLTD
68 : // @DisplayName: Roll axis rate controller derivative frequency in Hz
69 : // @Description: Roll axis rate controller derivative frequency in Hz
70 : // @Range: 5 100
71 : // @Increment: 1
72 : // @Units: Hz
73 : // @User: Standard
74 :
75 : // @Param: RAT_RLL_SMAX
76 : // @DisplayName: Roll slew rate limit
77 : // @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.
78 : // @Range: 0 200
79 : // @Increment: 0.5
80 : // @User: Advanced
81 :
82 : // @Param: RAT_RLL_PDMX
83 : // @DisplayName: Roll axis rate controller PD sum maximum
84 : // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
85 : // @Range: 0 1
86 : // @Increment: 0.01
87 :
88 : // @Param: RAT_RLL_D_FF
89 : // @DisplayName: Roll Derivative FeedForward Gain
90 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
91 : // @Units: s^2/rad
92 : // @Range: 0 0.02
93 : // @Increment: 0.0001
94 : // @User: Advanced
95 :
96 : // @Param: RAT_RLL_NTF
97 : // @DisplayName: Roll Target notch filter index
98 : // @Description: Roll Target notch filter index, zero disables
99 : // @Range: 0 8
100 : // @User: Advanced
101 :
102 : // @Param: RAT_RLL_NEF
103 : // @DisplayName: Roll Error notch filter index
104 : // @Description: Roll Error notch filter index, zero disables
105 : // @Range: 0 8
106 : // @User: Advanced
107 :
108 : AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID),
109 :
110 : // @Param: RAT_PIT_P
111 : // @DisplayName: Pitch axis rate controller P gain
112 : // @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate output
113 : // @Units: s/rad
114 : // @Range: 0.01 0.50
115 : // @Increment: 0.005
116 : // @User: Standard
117 :
118 : // @Param: RAT_PIT_I
119 : // @DisplayName: Pitch axis rate controller I gain
120 : // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
121 : // @Units: 1/rad
122 : // @Range: 0.01 2.0
123 : // @Increment: 0.01
124 : // @User: Standard
125 :
126 : // @Param: RAT_PIT_IMAX
127 : // @DisplayName: Pitch axis rate controller I gain maximum
128 : // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
129 : // @Range: 0 1
130 : // @Increment: 0.01
131 : // @User: Standard
132 :
133 : // @Param: RAT_PIT_D
134 : // @DisplayName: Pitch axis rate controller D gain
135 : // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
136 : // @Units: s^2/rad
137 : // @Range: 0.0 0.05
138 : // @Increment: 0.001
139 : // @User: Standard
140 :
141 : // @Param: RAT_PIT_FF
142 : // @DisplayName: Pitch axis rate controller feed forward
143 : // @Description: Pitch axis rate controller feed forward
144 : // @Units: s/rad
145 : // @Range: 0 0.5
146 : // @Increment: 0.001
147 : // @User: Standard
148 :
149 : // @Param: RAT_PIT_FLTT
150 : // @DisplayName: Pitch axis rate controller target frequency in Hz
151 : // @Description: Pitch axis rate controller target frequency in Hz
152 : // @Range: 5 100
153 : // @Increment: 1
154 : // @Units: Hz
155 : // @User: Standard
156 :
157 : // @Param: RAT_PIT_FLTE
158 : // @DisplayName: Pitch axis rate controller error frequency in Hz
159 : // @Description: Pitch axis rate controller error frequency in Hz
160 : // @Range: 0 100
161 : // @Increment: 1
162 : // @Units: Hz
163 : // @User: Standard
164 :
165 : // @Param: RAT_PIT_FLTD
166 : // @DisplayName: Pitch axis rate controller derivative frequency in Hz
167 : // @Description: Pitch axis rate controller derivative frequency in Hz
168 : // @Range: 5 100
169 : // @Increment: 1
170 : // @Units: Hz
171 : // @User: Standard
172 :
173 : // @Param: RAT_PIT_SMAX
174 : // @DisplayName: Pitch slew rate limit
175 : // @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.
176 : // @Range: 0 200
177 : // @Increment: 0.5
178 : // @User: Advanced
179 :
180 : // @Param: RAT_PIT_PDMX
181 : // @DisplayName: Pitch axis rate controller PD sum maximum
182 : // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
183 : // @Range: 0 1
184 : // @Increment: 0.01
185 :
186 : // @Param: RAT_PIT_D_FF
187 : // @DisplayName: Pitch Derivative FeedForward Gain
188 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
189 : // @Units: s^2/rad
190 : // @Range: 0 0.02
191 : // @Increment: 0.0001
192 : // @User: Advanced
193 :
194 : // @Param: RAT_PIT_NTF
195 : // @DisplayName: Pitch Target notch filter index
196 : // @Description: Pitch Target notch filter index, zero disables
197 : // @Range: 0 8
198 : // @User: Advanced
199 :
200 : // @Param: RAT_PIT_NEF
201 : // @DisplayName: Pitch Error notch filter index
202 : // @Description: Pitch Error notch filter index, zero disables
203 : // @Range: 0 8
204 : // @User: Advanced
205 :
206 : AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID),
207 :
208 : // @Param: RAT_YAW_P
209 : // @DisplayName: Yaw axis rate controller P gain
210 : // @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
211 : // @Units: s/rad
212 : // @Range: 0.10 2.50
213 : // @Increment: 0.005
214 : // @User: Standard
215 :
216 : // @Param: RAT_YAW_I
217 : // @DisplayName: Yaw axis rate controller I gain
218 : // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
219 : // @Units: 1/rad
220 : // @Range: 0.010 1.0
221 : // @Increment: 0.01
222 : // @User: Standard
223 :
224 : // @Param: RAT_YAW_IMAX
225 : // @DisplayName: Yaw axis rate controller I gain maximum
226 : // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
227 : // @Range: 0 1
228 : // @Increment: 0.01
229 : // @User: Standard
230 :
231 : // @Param: RAT_YAW_D
232 : // @DisplayName: Yaw axis rate controller D gain
233 : // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
234 : // @Units: s^2/rad
235 : // @Range: 0.000 0.02
236 : // @Increment: 0.001
237 : // @User: Standard
238 :
239 : // @Param: RAT_YAW_FF
240 : // @DisplayName: Yaw axis rate controller feed forward
241 : // @Description: Yaw axis rate controller feed forward
242 : // @Units: s/rad
243 : // @Range: 0 0.5
244 : // @Increment: 0.001
245 : // @User: Standard
246 :
247 : // @Param: RAT_YAW_FLTT
248 : // @DisplayName: Yaw axis rate controller target frequency in Hz
249 : // @Description: Yaw axis rate controller target frequency in Hz
250 : // @Range: 1 50
251 : // @Increment: 1
252 : // @Units: Hz
253 : // @User: Standard
254 :
255 : // @Param: RAT_YAW_FLTE
256 : // @DisplayName: Yaw axis rate controller error frequency in Hz
257 : // @Description: Yaw axis rate controller error frequency in Hz
258 : // @Range: 0 20
259 : // @Increment: 1
260 : // @Units: Hz
261 : // @User: Standard
262 :
263 : // @Param: RAT_YAW_FLTD
264 : // @DisplayName: Yaw axis rate controller derivative frequency in Hz
265 : // @Description: Yaw axis rate controller derivative frequency in Hz
266 : // @Range: 5 50
267 : // @Increment: 1
268 : // @Units: Hz
269 : // @User: Standard
270 :
271 : // @Param: RAT_YAW_SMAX
272 : // @DisplayName: Yaw slew rate limit
273 : // @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.
274 : // @Range: 0 200
275 : // @Increment: 0.5
276 : // @User: Advanced
277 :
278 : // @Param: RAT_YAW_PDMX
279 : // @DisplayName: Yaw axis rate controller PD sum maximum
280 : // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
281 : // @Range: 0 1
282 : // @Increment: 0.01
283 :
284 : // @Param: RAT_YAW_D_FF
285 : // @DisplayName: Yaw Derivative FeedForward Gain
286 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
287 : // @Units: s^2/rad
288 : // @Range: 0 0.02
289 : // @Increment: 0.0001
290 : // @User: Advanced
291 :
292 : // @Param: RAT_YAW_NTF
293 : // @DisplayName: Yaw Target notch filter index
294 : // @Description: Yaw Target notch filter index, zero disables
295 : // @Range: 0 8
296 : // @User: Advanced
297 :
298 : // @Param: RAT_YAW_NEF
299 : // @DisplayName: Yaw Error notch filter index
300 : // @Description: Yaw Error notch filter index, zero disables
301 : // @Range: 0 8
302 : // @User: Advanced
303 :
304 : AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
305 :
306 : // @Param: THR_MIX_MIN
307 : // @DisplayName: Throttle Mix Minimum
308 : // @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
309 : // @Range: 0.1 0.25
310 : // @User: Advanced
311 : AP_GROUPINFO("THR_MIX_MIN", 4, AC_AttitudeControl_Multi, _thr_mix_min, AC_ATTITUDE_CONTROL_MIN_DEFAULT),
312 :
313 : // @Param: THR_MIX_MAX
314 : // @DisplayName: Throttle Mix Maximum
315 : // @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
316 : // @Range: 0.5 0.9
317 : // @User: Advanced
318 : AP_GROUPINFO("THR_MIX_MAX", 5, AC_AttitudeControl_Multi, _thr_mix_max, AC_ATTITUDE_CONTROL_MAX_DEFAULT),
319 :
320 : // @Param: THR_MIX_MAN
321 : // @DisplayName: Throttle Mix Manual
322 : // @Description: Throttle vs attitude control prioritisation used during manual flight (higher values mean we prioritise attitude control over throttle)
323 : // @Range: 0.1 0.9
324 : // @User: Advanced
325 : AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Multi, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT),
326 :
327 : // @Param: THR_G_BOOST
328 : // @DisplayName: Throttle-gain boost
329 : // @Description: Throttle-gain boost ratio. A value of 0 means no boosting is applied, a value of 1 means full boosting is applied. Describes the ratio increase that is applied to angle P and PD on pitch and roll.
330 : // @Range: 0 1
331 : // @User: Advanced
332 : AP_GROUPINFO("THR_G_BOOST", 7, AC_AttitudeControl_Multi, _throttle_gain_boost, 0.0f),
333 :
334 : AP_GROUPEND
335 : };
336 :
337 1119 : AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, AP_MotorsMulticopter& motors) :
338 : AC_AttitudeControl(ahrs, motors),
339 1119 : _motors_multi(motors)
340 : {
341 1119 : AP_Param::setup_object_defaults(this, var_info);
342 :
343 : #if AP_FILTER_ENABLED
344 1119 : set_notch_sample_rate(AP::scheduler().get_loop_rate_hz());
345 : #endif
346 1119 : }
347 :
348 : // Update Alt_Hold angle maximum
349 29709936 : void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in)
350 : {
351 : // calc maximum tilt angle based on throttle
352 29709936 : float thr_max = _motors_multi.get_throttle_thrust_max();
353 :
354 : // divide by zero check
355 29709936 : if (is_zero(thr_max)) {
356 10234719 : _althold_lean_angle_max_rad = 0.0f;
357 10234719 : return;
358 : }
359 :
360 19475217 : float althold_lean_angle_max = acosf(constrain_float(throttle_in / (AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
361 19475217 : _althold_lean_angle_max_rad = _althold_lean_angle_max_rad + (_dt_s / (_dt_s + _angle_limit_tc)) * (althold_lean_angle_max - _althold_lean_angle_max_rad);
362 : }
363 :
364 29709936 : void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
365 : {
366 29709936 : _throttle_in = throttle_in;
367 29709936 : update_althold_lean_angle_max(throttle_in);
368 29709936 : _motors.set_throttle_filter_cutoff(filter_cutoff);
369 29709936 : if (apply_angle_boost) {
370 : // Apply angle boost
371 29676984 : throttle_in = get_throttle_boosted(throttle_in);
372 : } else {
373 : // Clear angle_boost for logging purposes
374 32952 : _angle_boost = 0.0f;
375 : }
376 29709936 : _motors.set_throttle(throttle_in);
377 29709936 : _motors.set_throttle_avg_max(get_throttle_avg_max(MAX(throttle_in, _throttle_in)));
378 29709936 : }
379 :
380 1706804 : void AC_AttitudeControl_Multi::set_throttle_mix_max(float ratio)
381 : {
382 1706804 : ratio = constrain_float(ratio, 0.0f, 1.0f);
383 1706804 : _throttle_rpy_mix_desired = (1.0f - ratio) * _thr_mix_min + ratio * _thr_mix_max;
384 1706804 : }
385 :
386 : // returns a throttle including compensation for roll/pitch angle
387 : // throttle value should be 0 ~ 1
388 29676984 : float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
389 : {
390 29676984 : if (!_angle_boost_enabled) {
391 19111 : _angle_boost = 0;
392 19111 : return throttle_in;
393 : }
394 : // inverted_factor is 1 for tilt angles below 84 degrees
395 : // inverted_factor reduces from 1 to 0 for tilt angles between 84 and 90 degrees
396 :
397 29657873 : float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
398 29657873 : float inverted_factor = constrain_float(10.0f * cos_tilt, 0.0f, 1.0f);
399 29657873 : float cos_tilt_target = cosf(_thrust_angle_rad);
400 29657873 : float boost_factor = 1.0f / constrain_float(cos_tilt_target, 0.1f, 1.0f);
401 :
402 29657873 : float throttle_out = throttle_in * inverted_factor * boost_factor;
403 29657873 : _angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);
404 29657873 : return throttle_out;
405 : }
406 :
407 : // returns a throttle including compensation for roll/pitch angle
408 : // throttle value should be 0 ~ 1
409 29709936 : float AC_AttitudeControl_Multi::get_throttle_avg_max(float throttle_in)
410 : {
411 29709936 : throttle_in = constrain_float(throttle_in, 0.0f, 1.0f);
412 29709936 : return MAX(throttle_in, throttle_in * MAX(0.0f, 1.0f - _throttle_rpy_mix) + _motors.get_throttle_hover() * _throttle_rpy_mix);
413 : }
414 :
415 : // update_throttle_gain_boost - boost angle_p/pd each cycle on high throttle slew
416 30013508 : void AC_AttitudeControl_Multi::update_throttle_gain_boost()
417 : {
418 : // Boost PD and Angle P on very rapid throttle changes
419 30013508 : if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) {
420 252366 : const float pd_boost = constrain_float(_throttle_gain_boost + 1.0f, 1.0, 2.0);
421 252366 : set_PD_scale_mult(Vector3f{pd_boost, pd_boost, 1.0f});
422 :
423 252366 : const float angle_p_boost = constrain_float((_throttle_gain_boost + 1.0f) * (_throttle_gain_boost + 1.0f), 1.0, 4.0);
424 252366 : set_angle_P_scale_mult(Vector3f{angle_p_boost, angle_p_boost, 1.0f});
425 : }
426 30013508 : }
427 :
428 : // update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
429 30013508 : void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
430 : {
431 : // slew _throttle_rpy_mix to _throttle_rpy_mix_desired
432 30013508 : if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
433 : // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
434 123836 : _throttle_rpy_mix += MIN(2.0f * _dt_s, _throttle_rpy_mix_desired - _throttle_rpy_mix);
435 29889672 : } else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
436 : // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
437 98763 : _throttle_rpy_mix -= MIN(0.5f * _dt_s, _throttle_rpy_mix - _throttle_rpy_mix_desired);
438 :
439 : // if the mix is still higher than that being used, reset immediately
440 98763 : const float throttle_hover = _motors.get_throttle_hover();
441 98763 : const float throttle_in = _motors.get_throttle();
442 98763 : const float throttle_out = MAX(_motors.get_throttle_out(), throttle_in);
443 : float mix_used;
444 : // since throttle_out >= throttle_in at this point we don't need to check throttle_in < throttle_hover
445 98763 : if (throttle_out < throttle_hover) {
446 20099 : mix_used = (throttle_out - throttle_in) / (throttle_hover - throttle_in);
447 : } else {
448 78664 : mix_used = throttle_out / throttle_hover;
449 : }
450 :
451 98763 : _throttle_rpy_mix = MIN(_throttle_rpy_mix, MAX(mix_used, _throttle_rpy_mix_desired));
452 : }
453 30013508 : _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
454 30013508 : }
455 :
456 30013508 : void AC_AttitudeControl_Multi::rate_controller_run_dt(const Vector3f& gyro_rads, float dt)
457 : {
458 : // take a copy of the target so that it can't be changed from under us.
459 30013508 : Vector3f ang_vel_body = _ang_vel_body_rads;
460 :
461 : // boost angle_p/pd each cycle on high throttle slew
462 30013508 : update_throttle_gain_boost();
463 :
464 : // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
465 30013508 : update_throttle_rpy_mix();
466 :
467 30013508 : ang_vel_body += _sysid_ang_vel_body_rads;
468 :
469 30013508 : _rate_gyro_rads = gyro_rads;
470 30013508 : _rate_gyro_time_us = AP_HAL::micros64();
471 :
472 30013508 : _motors.set_roll(get_rate_roll_pid().update_all(ang_vel_body.x, gyro_rads.x, dt, _motors.limit.roll, _pd_scale.x, _i_scale.x) + _actuator_sysid.x);
473 30013508 : _motors.set_roll_ff(get_rate_roll_pid().get_ff());
474 :
475 30013508 : _motors.set_pitch(get_rate_pitch_pid().update_all(ang_vel_body.y, gyro_rads.y, dt, _motors.limit.pitch, _pd_scale.y, _i_scale.y) + _actuator_sysid.y);
476 30013508 : _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
477 :
478 30013508 : _motors.set_yaw(get_rate_yaw_pid().update_all(ang_vel_body.z, gyro_rads.z, dt, _motors.limit.yaw, _pd_scale.z, _i_scale.z) + _actuator_sysid.z);
479 30013508 : _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
480 :
481 30013508 : _pd_scale_used = _pd_scale;
482 30013508 : _i_scale_used = _i_scale;
483 30013508 : _angle_P_scale_used = _angle_P_scale;
484 30013508 : }
485 :
486 : // run the rate controller using the configured _dt and latest gyro_rads
487 29725288 : void AC_AttitudeControl_Multi::rate_controller_run()
488 : {
489 29725288 : Vector3f gyro_latest_rads = _ahrs.get_gyro_latest();
490 29725288 : rate_controller_run_dt(gyro_latest_rads, _dt_s);
491 29725288 : }
492 :
493 : // sanity check parameters. should be called once before takeoff
494 1119 : void AC_AttitudeControl_Multi::parameter_sanity_check()
495 : {
496 : // sanity check throttle mix parameters
497 1119 : if (_thr_mix_man < 0.1f || _thr_mix_man > AC_ATTITUDE_CONTROL_MAN_LIMIT) {
498 : // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0
499 : // which can be useful for very high powered copters with very low hover throttle
500 0 : _thr_mix_man.set_and_save(constrain_float(_thr_mix_man, 0.1, AC_ATTITUDE_CONTROL_MAN_LIMIT));
501 : }
502 1119 : if (_thr_mix_min < 0.1f || _thr_mix_min > AC_ATTITUDE_CONTROL_MIN_LIMIT) {
503 0 : _thr_mix_min.set_and_save(constrain_float(_thr_mix_min, 0.1, AC_ATTITUDE_CONTROL_MIN_LIMIT));
504 : }
505 1119 : if (_thr_mix_max < 0.5f || _thr_mix_max > AC_ATTITUDE_CONTROL_MAX) {
506 : // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0
507 : // which can be useful for very high powered copters with very low hover throttle
508 0 : _thr_mix_max.set_and_save(constrain_float(_thr_mix_max, 0.5, AC_ATTITUDE_CONTROL_MAX));
509 : }
510 1119 : if (_thr_mix_min > _thr_mix_max) {
511 0 : _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT);
512 0 : _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT);
513 : }
514 1119 : }
515 :
516 89868 : void AC_AttitudeControl_Multi::set_notch_sample_rate(float sample_rate)
517 : {
518 : #if AP_FILTER_ENABLED
519 89868 : _pid_rate_roll.set_notch_sample_rate(sample_rate);
520 89868 : _pid_rate_pitch.set_notch_sample_rate(sample_rate);
521 89868 : _pid_rate_yaw.set_notch_sample_rate(sample_rate);
522 : #endif
523 89868 : }
|
