Line data Source code
1 : #include "AC_AttitudeControl_Sub.h"
2 : #include <AP_HAL/AP_HAL.h>
3 : #include <AP_Math/AP_Math.h>
4 :
5 : // table of user settable parameters
6 : const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
7 : // parameters from parent vehicle
8 : AP_NESTEDGROUPINFO(AC_AttitudeControl, 0),
9 :
10 : // @Param: RAT_RLL_P
11 : // @DisplayName: Roll axis rate controller P gain
12 : // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
13 : // @Range: 0.0 0.30
14 : // @Increment: 0.005
15 : // @User: Standard
16 :
17 : // @Param: RAT_RLL_I
18 : // @DisplayName: Roll axis rate controller I gain
19 : // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
20 : // @Range: 0.0 0.5
21 : // @Increment: 0.01
22 : // @User: Standard
23 :
24 : // @Param: RAT_RLL_IMAX
25 : // @DisplayName: Roll axis rate controller I gain maximum
26 : // @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
27 : // @Range: 0 1
28 : // @Increment: 0.01
29 : // @User: Standard
30 :
31 : // @Param: RAT_RLL_D
32 : // @DisplayName: Roll axis rate controller D gain
33 : // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
34 : // @Range: 0.0 0.02
35 : // @Increment: 0.001
36 : // @User: Standard
37 :
38 : // @Param: RAT_RLL_FF
39 : // @DisplayName: Roll axis rate controller feed forward
40 : // @Description: Roll axis rate controller feed forward
41 : // @Range: 0 0.5
42 : // @Increment: 0.001
43 : // @User: Standard
44 :
45 : // @Param: RAT_RLL_FLTT
46 : // @DisplayName: Roll axis rate controller input frequency in Hz
47 : // @Description: Roll axis rate controller input frequency in Hz
48 : // @Range: 1 100
49 : // @Increment: 1
50 : // @Units: Hz
51 : // @User: Standard
52 :
53 : // @Param: RAT_RLL_FLTE
54 : // @DisplayName: Roll axis rate controller input frequency in Hz
55 : // @Description: Roll axis rate controller input frequency in Hz
56 : // @Range: 1 100
57 : // @Increment: 1
58 : // @Units: Hz
59 : // @User: Standard
60 :
61 : // @Param: RAT_RLL_FLTD
62 : // @DisplayName: Roll axis rate controller input frequency in Hz
63 : // @Description: Roll axis rate controller input frequency in Hz
64 : // @Range: 1 100
65 : // @Increment: 1
66 : // @Units: Hz
67 : // @User: Standard
68 :
69 : // @Param: RAT_RLL_SMAX
70 : // @DisplayName: Roll slew rate limit
71 : // @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.
72 : // @Range: 0 200
73 : // @Increment: 0.5
74 : // @User: Advanced
75 :
76 : // @Param: RAT_RLL_PDMX
77 : // @DisplayName: Roll axis rate controller PD sum maximum
78 : // @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
79 : // @Range: 0 1
80 : // @Increment: 0.01
81 :
82 : // @Param: RAT_RLL_D_FF
83 : // @DisplayName: Roll Derivative FeedForward Gain
84 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
85 : // @Range: 0 0.02
86 : // @Increment: 0.0001
87 : // @User: Advanced
88 :
89 : // @Param: RAT_RLL_NTF
90 : // @DisplayName: Roll Target notch filter index
91 : // @Description: Roll Target notch filter index, zero disables
92 : // @Range: 0 8
93 : // @User: Advanced
94 :
95 : // @Param: RAT_RLL_NEF
96 : // @DisplayName: Roll Error notch filter index
97 : // @Description: Roll Error notch filter index, zero disables
98 : // @Range: 0 8
99 : // @User: Advanced
100 :
101 : AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Sub, AC_PID),
102 :
103 : // @Param: RAT_PIT_P
104 : // @DisplayName: Pitch axis rate controller P gain
105 : // @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
106 : // @Range: 0.0 0.30
107 : // @Increment: 0.005
108 : // @User: Standard
109 :
110 : // @Param: RAT_PIT_I
111 : // @DisplayName: Pitch axis rate controller I gain
112 : // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
113 : // @Range: 0.0 0.5
114 : // @Increment: 0.01
115 : // @User: Standard
116 :
117 : // @Param: RAT_PIT_IMAX
118 : // @DisplayName: Pitch axis rate controller I gain maximum
119 : // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
120 : // @Range: 0 1
121 : // @Increment: 0.01
122 : // @User: Standard
123 :
124 : // @Param: RAT_PIT_D
125 : // @DisplayName: Pitch axis rate controller D gain
126 : // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
127 : // @Range: 0.0 0.02
128 : // @Increment: 0.001
129 : // @User: Standard
130 :
131 : // @Param: RAT_PIT_FF
132 : // @DisplayName: Pitch axis rate controller feed forward
133 : // @Description: Pitch axis rate controller feed forward
134 : // @Range: 0 0.5
135 : // @Increment: 0.001
136 : // @User: Standard
137 :
138 : // @Param: RAT_PIT_FLTT
139 : // @DisplayName: Pitch axis rate controller input frequency in Hz
140 : // @Description: Pitch axis rate controller input frequency in Hz
141 : // @Range: 1 100
142 : // @Increment: 1
143 : // @Units: Hz
144 : // @User: Standard
145 :
146 : // @Param: RAT_PIT_FLTE
147 : // @DisplayName: Pitch axis rate controller input frequency in Hz
148 : // @Description: Pitch axis rate controller input frequency in Hz
149 : // @Range: 1 100
150 : // @Increment: 1
151 : // @Units: Hz
152 : // @User: Standard
153 :
154 : // @Param: RAT_PIT_FLTD
155 : // @DisplayName: Pitch axis rate controller input frequency in Hz
156 : // @Description: Pitch axis rate controller input frequency in Hz
157 : // @Range: 1 100
158 : // @Increment: 1
159 : // @Units: Hz
160 : // @User: Standard
161 :
162 : // @Param: RAT_PIT_SMAX
163 : // @DisplayName: Pitch slew rate limit
164 : // @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.
165 : // @Range: 0 200
166 : // @Increment: 0.5
167 : // @User: Advanced
168 :
169 : // @Param: RAT_PIT_PDMX
170 : // @DisplayName: Pitch axis rate controller PD sum maximum
171 : // @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
172 : // @Range: 0 1
173 : // @Increment: 0.01
174 :
175 : // @Param: RAT_PIT_D_FF
176 : // @DisplayName: Pitch Derivative FeedForward Gain
177 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
178 : // @Range: 0 0.02
179 : // @Increment: 0.0001
180 : // @User: Advanced
181 :
182 : // @Param: RAT_PIT_NTF
183 : // @DisplayName: Pitch Target notch filter index
184 : // @Description: Pitch Target notch filter index, zero disables
185 : // @Range: 0 8
186 : // @User: Advanced
187 :
188 : // @Param: RAT_PIT_NEF
189 : // @DisplayName: Pitch Error notch filter index
190 : // @Description: Pitch Error notch filter index, zero disables
191 : // @Range: 0 8
192 : // @User: Advanced
193 :
194 : AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Sub, AC_PID),
195 :
196 : // @Param: RAT_YAW_P
197 : // @DisplayName: Yaw axis rate controller P gain
198 : // @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
199 : // @Range: 0.0 0.50
200 : // @Increment: 0.005
201 : // @User: Standard
202 :
203 : // @Param: RAT_YAW_I
204 : // @DisplayName: Yaw axis rate controller I gain
205 : // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
206 : // @Range: 0.0 0.05
207 : // @Increment: 0.01
208 : // @User: Standard
209 :
210 : // @Param: RAT_YAW_IMAX
211 : // @DisplayName: Yaw axis rate controller I gain maximum
212 : // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
213 : // @Range: 0 1
214 : // @Increment: 0.01
215 : // @User: Standard
216 :
217 : // @Param: RAT_YAW_D
218 : // @DisplayName: Yaw axis rate controller D gain
219 : // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
220 : // @Range: 0.000 0.02
221 : // @Increment: 0.001
222 : // @User: Standard
223 :
224 : // @Param: RAT_YAW_FF
225 : // @DisplayName: Yaw axis rate controller feed forward
226 : // @Description: Yaw axis rate controller feed forward
227 : // @Range: 0 0.5
228 : // @Increment: 0.001
229 : // @User: Standard
230 :
231 : // @Param: RAT_YAW_FLTT
232 : // @DisplayName: Yaw axis rate controller input frequency in Hz
233 : // @Description: Yaw axis rate controller input frequency in Hz
234 : // @Range: 1 100
235 : // @Increment: 1
236 : // @Units: Hz
237 : // @User: Standard
238 :
239 : // @Param: RAT_YAW_FLTE
240 : // @DisplayName: Yaw axis rate controller input frequency in Hz
241 : // @Description: Yaw axis rate controller input frequency in Hz
242 : // @Range: 1 100
243 : // @Increment: 1
244 : // @Units: Hz
245 : // @User: Standard
246 :
247 : // @Param: RAT_YAW_FLTD
248 : // @DisplayName: Yaw axis rate controller input frequency in Hz
249 : // @Description: Yaw axis rate controller input frequency in Hz
250 : // @Range: 1 100
251 : // @Increment: 1
252 : // @Units: Hz
253 : // @User: Standard
254 :
255 : // @Param: RAT_YAW_SMAX
256 : // @DisplayName: Yaw slew rate limit
257 : // @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.
258 : // @Range: 0 200
259 : // @Increment: 0.5
260 : // @User: Advanced
261 :
262 : // @Param: RAT_YAW_PDMX
263 : // @DisplayName: Yaw axis rate controller PD sum maximum
264 : // @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
265 : // @Range: 0 1
266 : // @Increment: 0.01
267 :
268 : // @Param: RAT_YAW_D_FF
269 : // @DisplayName: Yaw Derivative FeedForward Gain
270 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
271 : // @Range: 0 0.02
272 : // @Increment: 0.0001
273 : // @User: Advanced
274 :
275 : // @Param: RAT_YAW_NTF
276 : // @DisplayName: Yaw Target notch filter index
277 : // @Description: Yaw Target notch filter index, zero disables
278 : // @Range: 0 8
279 : // @User: Advanced
280 :
281 : // @Param: RAT_YAW_NEF
282 : // @DisplayName: Yaw Error notch filter index
283 : // @Description: Yaw Error notch filter index, zero disables
284 : // @Range: 0 8
285 : // @User: Advanced
286 :
287 : AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Sub, AC_PID),
288 :
289 : // @Param: THR_MIX_MIN
290 : // @DisplayName: Throttle Mix Minimum
291 : // @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
292 : // @Range: 0.1 0.25
293 : // @User: Advanced
294 : AP_GROUPINFO("THR_MIX_MIN", 4, AC_AttitudeControl_Sub, _thr_mix_min, AC_ATTITUDE_CONTROL_MIN_DEFAULT),
295 :
296 : // @Param: THR_MIX_MAX
297 : // @DisplayName: Throttle Mix Maximum
298 : // @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
299 : // @Range: 0.5 0.9
300 : // @User: Advanced
301 : AP_GROUPINFO("THR_MIX_MAX", 5, AC_AttitudeControl_Sub, _thr_mix_max, AC_ATTITUDE_CONTROL_MAX_DEFAULT),
302 :
303 : // @Param: THR_MIX_MAN
304 : // @DisplayName: Throttle Mix Manual
305 : // @Description: Throttle vs attitude control prioritisation used during manual flight (higher values mean we prioritise attitude control over throttle)
306 : // @Range: 0.5 0.9
307 : // @User: Advanced
308 : AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Sub, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT),
309 :
310 : // @Param: RAT_RLL_FILT
311 : // @DisplayName: Roll axis rate controller input frequency in Hz
312 : // @Description: Roll axis rate controller input frequency in Hz
313 : // @Range: 1 100
314 : // @Increment: 1
315 : // @Units: Hz
316 : // @User: Standard
317 :
318 : // @Param: RAT_PIT_FILT
319 : // @DisplayName: Pitch axis rate controller input frequency in Hz
320 : // @Description: Pitch axis rate controller input frequency in Hz
321 : // @Range: 1 100
322 : // @Increment: 1
323 : // @Units: Hz
324 : // @User: Standard
325 :
326 : // @Param: RAT_YAW_FILT
327 : // @DisplayName: Yaw axis rate controller input frequency in Hz
328 : // @Description: Yaw axis rate controller input frequency in Hz
329 : // @Range: 1 100
330 : // @Increment: 1
331 : // @Units: Hz
332 : // @User: Standard
333 :
334 : AP_GROUPEND
335 : };
336 :
337 83 : AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, AP_MotorsMulticopter& motors) :
338 : AC_AttitudeControl(ahrs, motors),
339 83 : _motors_multi(motors)
340 : {
341 83 : AP_Param::setup_object_defaults(this, var_info);
342 :
343 : // Sub-specific defaults for parent class
344 83 : _p_angle_roll.kP().set_default(AC_ATC_SUB_ANGLE_P);
345 83 : _p_angle_pitch.kP().set_default(AC_ATC_SUB_ANGLE_P);
346 83 : _p_angle_yaw.kP().set_default(AC_ATC_SUB_ANGLE_P);
347 :
348 83 : _accel_yaw_max_degss.set_default(AC_ATC_SUB_ACCEL_Y_MAX_DEGSS);
349 83 : }
350 :
351 : // Update Alt_Hold angle maximum
352 2040664 : void AC_AttitudeControl_Sub::update_althold_lean_angle_max(float throttle_in)
353 : {
354 : // calc maximum tilt angle based on throttle
355 2040664 : float thr_max = _motors_multi.get_throttle_thrust_max();
356 :
357 : // divide by zero check
358 2040664 : if (is_zero(thr_max)) {
359 1012389 : _althold_lean_angle_max_rad = 0.0f;
360 1012389 : return;
361 : }
362 :
363 1028275 : float althold_lean_angle_max = acosf(constrain_float(throttle_in/(AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f));
364 1028275 : _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);
365 : }
366 :
367 2040664 : void AC_AttitudeControl_Sub::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
368 : {
369 2040664 : _throttle_in = throttle_in;
370 2040664 : update_althold_lean_angle_max(throttle_in);
371 2040664 : _motors.set_throttle_filter_cutoff(filter_cutoff);
372 2040664 : _motors.set_throttle(throttle_in);
373 2040664 : _motors.set_throttle_avg_max(get_throttle_avg_max(MAX(throttle_in, _throttle_in)));
374 2040664 : }
375 :
376 : // returns a throttle including compensation for roll/pitch angle
377 : // throttle value should be 0 ~ 1
378 0 : float AC_AttitudeControl_Sub::get_throttle_boosted(float throttle_in)
379 : {
380 0 : if (!_angle_boost_enabled) {
381 0 : _angle_boost = 0;
382 0 : return throttle_in;
383 : }
384 : // inverted_factor is 1 for tilt angles below 60 degrees
385 : // inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees
386 :
387 0 : float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
388 0 : float inverted_factor = constrain_float(2.0f*cos_tilt, 0.0f, 1.0f);
389 0 : float boost_factor = 1.0f/constrain_float(cos_tilt, 0.5f, 1.0f);
390 :
391 0 : float throttle_out = throttle_in*inverted_factor*boost_factor;
392 0 : _angle_boost = constrain_float(throttle_out - throttle_in,-1.0f,1.0f);
393 0 : return throttle_out;
394 : }
395 :
396 : // Returns a throttle value that accounts for the priority of attitude control over throttle.
397 : // This allows graceful reduction of control authority as thrust approaches its minimum.
398 : // returns a throttle including compensation for roll/pitch angle
399 : // throttle value should be 0 ~ 1
400 2040664 : float AC_AttitudeControl_Sub::get_throttle_avg_max(float throttle_in)
401 : {
402 2040664 : throttle_in = constrain_float(throttle_in, 0.0f, 1.0f);
403 2040664 : return MAX(throttle_in, throttle_in*MAX(0.0f,1.0f-_throttle_rpy_mix)+_motors.get_throttle_hover()*_throttle_rpy_mix);
404 : }
405 :
406 : // Slews the throttle-to-attitude mix ratio (_throttle_rpy_mix) toward the requested value (_throttle_rpy_mix_desired).
407 : // Increases rapidly and decreases more slowly to ensure stability during transitions.
408 1134713 : void AC_AttitudeControl_Sub::update_throttle_rpy_mix()
409 : {
410 : // slew _throttle_rpy_mix to _throttle_rpy_mix_desired
411 1134713 : if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
412 : // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
413 0 : _throttle_rpy_mix += MIN(2.0f*_dt_s, _throttle_rpy_mix_desired-_throttle_rpy_mix);
414 1134713 : } else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
415 : // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
416 0 : _throttle_rpy_mix -= MIN(0.5f*_dt_s, _throttle_rpy_mix-_throttle_rpy_mix_desired);
417 : }
418 1134713 : _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
419 1134713 : }
420 :
421 1134713 : void AC_AttitudeControl_Sub::rate_controller_run()
422 : {
423 : // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
424 1134713 : update_throttle_rpy_mix();
425 :
426 1134713 : _rate_gyro_rads = _ahrs.get_gyro_latest();
427 1134713 : _rate_gyro_time_us = AP_HAL::micros64();
428 :
429 1134713 : _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body_rads.x, _rate_gyro_rads.x, _dt_s, _motors.limit.roll));
430 1134713 : _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body_rads.y, _rate_gyro_rads.y, _dt_s, _motors.limit.pitch));
431 1134713 : _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body_rads.z, _rate_gyro_rads.z, _dt_s, _motors.limit.yaw));
432 1134713 : }
433 :
434 : // sanity check parameters. should be called once before takeoff
435 0 : void AC_AttitudeControl_Sub::parameter_sanity_check()
436 : {
437 : // sanity check throttle mix parameters
438 0 : if (_thr_mix_man < 0.1f || _thr_mix_man > AC_ATTITUDE_CONTROL_MAN_LIMIT) {
439 : // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0
440 : // which can be useful for very high powered copters with very low hover throttle
441 0 : _thr_mix_man.set_and_save(constrain_float(_thr_mix_man, 0.1, AC_ATTITUDE_CONTROL_MAN_LIMIT));
442 : }
443 0 : if (_thr_mix_min < 0.1f || _thr_mix_min > AC_ATTITUDE_CONTROL_MIN_LIMIT) {
444 0 : _thr_mix_min.set_and_save(constrain_float(_thr_mix_min, 0.1, AC_ATTITUDE_CONTROL_MIN_LIMIT));
445 : }
446 0 : if (_thr_mix_max < 0.5f || _thr_mix_max > AC_ATTITUDE_CONTROL_MAX) {
447 : // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0
448 : // which can be useful for very high powered copters with very low hover throttle
449 0 : _thr_mix_max.set_and_save(constrain_float(_thr_mix_max, 0.5, AC_ATTITUDE_CONTROL_MAX));
450 : }
451 0 : if (_thr_mix_min > _thr_mix_max) {
452 0 : _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT);
453 0 : _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT);
454 : }
455 0 : }
456 :
457 : // Sets desired roll, pitch, and yaw angles (in centidegrees), with yaw slewing.
458 : // Slews toward target yaw at a fixed rate (in centidegrees/s) until the error is within 5 degrees.
459 : // Used to enforce consistent heading changes without large instantaneous yaw errors.
460 163731 : void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float slew_yaw_rate_cds)
461 : {
462 : // Convert from centidegrees on public interface to radians
463 163731 : const float euler_yaw_angle = wrap_PI(cd_to_rad(euler_yaw_angle_cd));
464 :
465 163731 : const float current_yaw = AP::ahrs().get_yaw_rad();
466 :
467 : // Compute angle error
468 163731 : const float yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
469 :
470 163731 : int direction = 0;
471 163731 : if (yaw_error < 0){
472 16389 : direction = -1;
473 : } else {
474 147342 : direction = 1;
475 : }
476 :
477 163731 : slew_yaw_rate_cds *= direction;
478 :
479 163731 : if (fabsf(yaw_error) > MAX_YAW_ERROR) {
480 : // rotate the rov with desired yaw rate towards the target yaw
481 141860 : input_euler_angle_roll_pitch_euler_rate_yaw_cd(euler_roll_angle_cd, euler_pitch_angle_cd, slew_yaw_rate_cds);
482 : } else {
483 : // holds the rov's angles
484 21871 : input_euler_angle_roll_pitch_yaw_cd(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, true);
485 : }
486 163731 : }
487 :
488 5409 : void AC_AttitudeControl_Sub::set_notch_sample_rate(float sample_rate)
489 : {
490 : #if AP_FILTER_ENABLED
491 5409 : _pid_rate_roll.set_notch_sample_rate(sample_rate);
492 5409 : _pid_rate_pitch.set_notch_sample_rate(sample_rate);
493 5409 : _pid_rate_yaw.set_notch_sample_rate(sample_rate);
494 : #endif
495 5409 : }
|
