Line data Source code
1 : #include "AC_AttitudeControl_Heli.h"
2 : #include <AP_HAL/AP_HAL.h>
3 : #include <AP_Scheduler/AP_Scheduler.h>
4 :
5 : // table of user settable parameters
6 : const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
7 : // parameters from parent vehicle
8 : AP_NESTEDGROUPINFO(AC_AttitudeControl, 0),
9 :
10 : // @Param: HOVR_ROL_TRM
11 : // @DisplayName: Hover Roll Trim
12 : // @Description: Trim the hover roll angle to counter tail rotor thrust in a hover
13 : // @Units: cdeg
14 : // @Increment: 10
15 : // @Range: 0 1000
16 : // @User: Advanced
17 : AP_GROUPINFO("HOVR_ROL_TRM", 1, AC_AttitudeControl_Heli, _hover_roll_trim_cd, AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT),
18 :
19 : // @Param: RAT_RLL_P
20 : // @DisplayName: Roll axis rate controller P gain
21 : // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
22 : // @Units: s/rad
23 : // @Range: 0.0 0.35
24 : // @Increment: 0.005
25 : // @User: Standard
26 :
27 : // @Param: RAT_RLL_I
28 : // @DisplayName: Roll axis rate controller I gain
29 : // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
30 : // @Units: 1/rad
31 : // @Range: 0.0 0.6
32 : // @Increment: 0.01
33 : // @User: Standard
34 :
35 : // @Param: RAT_RLL_IMAX
36 : // @DisplayName: Roll axis rate controller I gain maximum
37 : // @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
38 : // @Range: 0 1
39 : // @Increment: 0.01
40 : // @User: Standard
41 :
42 : // @Param: RAT_RLL_ILMI
43 : // @DisplayName: Roll axis rate controller I-term leak minimum
44 : // @Description: Point below which I-term will not leak down
45 : // @Range: 0 1
46 : // @User: Advanced
47 :
48 : // @Param: RAT_RLL_D
49 : // @DisplayName: Roll axis rate controller D gain
50 : // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
51 : // @Units: s^2/rad
52 : // @Range: 0.0 0.03
53 : // @Increment: 0.001
54 : // @User: Standard
55 :
56 : // @Param: RAT_RLL_FF
57 : // @DisplayName: Roll axis rate controller feed forward
58 : // @Description: Roll axis rate controller feed forward
59 : // @Units: s/rad
60 : // @Range: 0.05 0.5
61 : // @Increment: 0.001
62 : // @User: Standard
63 :
64 : // @Param: RAT_RLL_FLTT
65 : // @DisplayName: Roll axis rate controller target frequency in Hz
66 : // @Description: Roll axis rate controller target frequency in Hz
67 : // @Range: 5 50
68 : // @Increment: 1
69 : // @Units: Hz
70 : // @User: Standard
71 :
72 : // @Param: RAT_RLL_FLTE
73 : // @DisplayName: Roll axis rate controller error frequency in Hz
74 : // @Description: Roll axis rate controller error frequency in Hz
75 : // @Range: 5 50
76 : // @Increment: 1
77 : // @Units: Hz
78 : // @User: Standard
79 :
80 : // @Param: RAT_RLL_FLTD
81 : // @DisplayName: Roll axis rate controller derivative frequency in Hz
82 : // @Description: Roll axis rate controller derivative frequency in Hz
83 : // @Range: 0 50
84 : // @Increment: 1
85 : // @Units: Hz
86 : // @User: Standard
87 :
88 : // @Param: RAT_RLL_SMAX
89 : // @DisplayName: Roll slew rate limit
90 : // @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.
91 : // @Range: 0 200
92 : // @Increment: 0.5
93 : // @User: Advanced
94 :
95 : // @Param: RAT_RLL_D_FF
96 : // @DisplayName: Roll Derivative FeedForward Gain
97 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
98 : // @Units: s^2/rad
99 : // @Range: 0 0.02
100 : // @Increment: 0.0001
101 : // @User: Advanced
102 :
103 : // @Param: RAT_RLL_NTF
104 : // @DisplayName: Roll Target notch filter index
105 : // @Description: Roll Target notch filter index, zero disables
106 : // @Range: 0 8
107 : // @User: Advanced
108 :
109 : // @Param: RAT_RLL_NEF
110 : // @DisplayName: Roll Error notch filter index
111 : // @Description: Roll Error notch filter index, zero disables
112 : // @Range: 0 8
113 : // @User: Advanced
114 :
115 : AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID),
116 :
117 : // @Param: RAT_PIT_P
118 : // @DisplayName: Pitch axis rate controller P gain
119 : // @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
120 : // @Units: s/rad
121 : // @Range: 0.0 0.35
122 : // @Increment: 0.005
123 : // @User: Standard
124 :
125 : // @Param: RAT_PIT_I
126 : // @DisplayName: Pitch axis rate controller I gain
127 : // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
128 : // @Units: 1/rad
129 : // @Range: 0.0 0.6
130 : // @Increment: 0.01
131 : // @User: Standard
132 :
133 : // @Param: RAT_PIT_IMAX
134 : // @DisplayName: Pitch axis rate controller I gain maximum
135 : // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
136 : // @Range: 0 1
137 : // @Increment: 0.01
138 : // @User: Standard
139 :
140 : // @Param: RAT_PIT_ILMI
141 : // @DisplayName: Pitch axis rate controller I-term leak minimum
142 : // @Description: Point below which I-term will not leak down
143 : // @Range: 0 1
144 : // @User: Advanced
145 :
146 : // @Param: RAT_PIT_D
147 : // @DisplayName: Pitch axis rate controller D gain
148 : // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
149 : // @Units: s^2/rad
150 : // @Range: 0.0 0.03
151 : // @Increment: 0.001
152 : // @User: Standard
153 :
154 : // @Param: RAT_PIT_FF
155 : // @DisplayName: Pitch axis rate controller feed forward
156 : // @Description: Pitch axis rate controller feed forward
157 : // @Units: s/rad
158 : // @Range: 0.05 0.5
159 : // @Increment: 0.001
160 : // @User: Standard
161 :
162 : // @Param: RAT_PIT_FLTT
163 : // @DisplayName: Pitch axis rate controller target frequency in Hz
164 : // @Description: Pitch axis rate controller target frequency in Hz
165 : // @Range: 5 50
166 : // @Increment: 1
167 : // @Units: Hz
168 : // @User: Standard
169 :
170 : // @Param: RAT_PIT_FLTE
171 : // @DisplayName: Pitch axis rate controller error frequency in Hz
172 : // @Description: Pitch axis rate controller error frequency in Hz
173 : // @Range: 5 50
174 : // @Increment: 1
175 : // @Units: Hz
176 : // @User: Standard
177 :
178 : // @Param: RAT_PIT_FLTD
179 : // @DisplayName: Pitch axis rate controller derivative frequency in Hz
180 : // @Description: Pitch axis rate controller derivative frequency in Hz
181 : // @Range: 0 50
182 : // @Increment: 1
183 : // @Units: Hz
184 : // @User: Standard
185 :
186 : // @Param: RAT_PIT_SMAX
187 : // @DisplayName: Pitch slew rate limit
188 : // @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.
189 : // @Range: 0 200
190 : // @Increment: 0.5
191 : // @User: Advanced
192 :
193 : // @Param: RAT_PIT_D_FF
194 : // @DisplayName: Pitch Derivative FeedForward Gain
195 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
196 : // @Units: s^2/rad
197 : // @Range: 0 0.02
198 : // @Increment: 0.0001
199 : // @User: Advanced
200 :
201 : // @Param: RAT_PIT_NTF
202 : // @DisplayName: Pitch Target notch filter index
203 : // @Description: Pitch Target notch filter index, zero disables
204 : // @Range: 0 8
205 : // @User: Advanced
206 :
207 : // @Param: RAT_PIT_NEF
208 : // @DisplayName: Pitch Error notch filter index
209 : // @Description: Pitch Error notch filter index, zero disables
210 : // @Range: 0 8
211 : // @User: Advanced
212 :
213 : AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID),
214 :
215 : // @Param: RAT_YAW_P
216 : // @DisplayName: Yaw axis rate controller P gain
217 : // @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
218 : // @Units: s/rad
219 : // @Range: 0.180 0.60
220 : // @Increment: 0.005
221 : // @User: Standard
222 :
223 : // @Param: RAT_YAW_I
224 : // @DisplayName: Yaw axis rate controller I gain
225 : // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
226 : // @Units: 1/rad
227 : // @Range: 0.01 0.2
228 : // @Increment: 0.01
229 : // @User: Standard
230 :
231 : // @Param: RAT_YAW_IMAX
232 : // @DisplayName: Yaw axis rate controller I gain maximum
233 : // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
234 : // @Range: 0 1
235 : // @Increment: 0.01
236 : // @User: Standard
237 :
238 : // @Param: RAT_YAW_ILMI
239 : // @DisplayName: Yaw axis rate controller I-term leak minimum
240 : // @Description: Point below which I-term will not leak down
241 : // @Range: 0 1
242 : // @User: Advanced
243 :
244 : // @Param: RAT_YAW_D
245 : // @DisplayName: Yaw axis rate controller D gain
246 : // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
247 : // @Units: s^2/rad
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 : // @Units: s/rad
256 : // @Range: 0 0.5
257 : // @Increment: 0.001
258 : // @User: Standard
259 :
260 : // @Param: RAT_YAW_FLTT
261 : // @DisplayName: Yaw axis rate controller target frequency in Hz
262 : // @Description: Yaw axis rate controller target frequency in Hz
263 : // @Range: 5 50
264 : // @Increment: 1
265 : // @Units: Hz
266 : // @User: Standard
267 :
268 : // @Param: RAT_YAW_FLTE
269 : // @DisplayName: Yaw axis rate controller error frequency in Hz
270 : // @Description: Yaw axis rate controller error frequency in Hz
271 : // @Range: 5 50
272 : // @Increment: 1
273 : // @Units: Hz
274 : // @User: Standard
275 :
276 : // @Param: RAT_YAW_FLTD
277 : // @DisplayName: Yaw axis rate controller derivative frequency in Hz
278 : // @Description: Yaw axis rate controller derivative frequency in Hz
279 : // @Range: 0 50
280 : // @Increment: 1
281 : // @Units: Hz
282 : // @User: Standard
283 :
284 : // @Param: RAT_YAW_SMAX
285 : // @DisplayName: Yaw slew rate limit
286 : // @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.
287 : // @Range: 0 200
288 : // @Increment: 0.5
289 : // @User: Advanced
290 :
291 : // @Param: RAT_YAW_D_FF
292 : // @DisplayName: Yaw Derivative FeedForward Gain
293 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
294 : // @Units: s^2/rad
295 : // @Range: 0 0.02
296 : // @Increment: 0.0001
297 : // @User: Advanced
298 :
299 : // @Param: RAT_YAW_NTF
300 : // @DisplayName: Yaw Target notch filter index
301 : // @Description: Yaw Target notch filter index, zero disables
302 : // @Range: 0 8
303 : // @User: Advanced
304 :
305 : // @Param: RAT_YAW_NEF
306 : // @DisplayName: Yaw Error notch filter index
307 : // @Description: Yaw Error notch filter index, zero disables
308 : // @Range: 0 8
309 : // @User: Advanced
310 :
311 : AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
312 :
313 : // @Param: PIRO_COMP
314 : // @DisplayName: Piro Comp Enable
315 : // @Description: Pirouette compensation enabled
316 : // @Values: 0:Disabled,1:Enabled
317 : // @User: Advanced
318 : AP_GROUPINFO("PIRO_COMP", 5, AC_AttitudeControl_Heli, _piro_comp_enabled, 0),
319 :
320 : AP_GROUPEND
321 : };
322 :
323 83 : AC_AttitudeControl_Heli::AC_AttitudeControl_Heli(AP_AHRS_View &ahrs, AP_MotorsHeli& motors) :
324 83 : AC_AttitudeControl(ahrs, motors)
325 : {
326 83 : AP_Param::setup_object_defaults(this, var_info);
327 :
328 : // initialise flags
329 83 : _flags_heli.leaky_i = true;
330 : #if AP_FILTER_ENABLED
331 83 : set_notch_sample_rate(AP::scheduler().get_loop_rate_hz());
332 : #endif
333 83 : }
334 :
335 : //
336 : // rate controller (body-frame) methods
337 : //
338 :
339 : // rate_controller_run - run lowest level rate controller and send outputs to the motors
340 : // should be called at 100hz or more
341 3233979 : void AC_AttitudeControl_Heli::rate_controller_run()
342 : {
343 3233979 : _ang_vel_body_rads += _sysid_ang_vel_body_rads;
344 :
345 3233979 : _rate_gyro_rads = _ahrs.get_gyro_latest();
346 3233979 : _rate_gyro_time_us = AP_HAL::micros64();
347 :
348 : // call rate controllers and send output to motors object
349 3233979 : rate_bf_to_motor_roll_pitch(_rate_gyro_rads, _ang_vel_body_rads.x, _ang_vel_body_rads.y);
350 3233979 : _motors.set_yaw(rate_target_to_motor_yaw(_rate_gyro_rads.z, _ang_vel_body_rads.z));
351 :
352 3233979 : _pd_scale_used = _pd_scale;
353 3233979 : _i_scale_used = _i_scale;
354 3233979 : _angle_P_scale_used = _angle_P_scale;
355 3233979 : }
356 :
357 : // Update Alt_Hold angle maximum
358 3213461 : void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in)
359 : {
360 3213461 : float althold_lean_angle_max = acosf(constrain_float(throttle_in / AC_ATTITUDE_HELI_ANGLE_LIMIT_THROTTLE_MAX, 0.0f, 1.0f));
361 3213461 : _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 3213461 : }
363 :
364 : //
365 : // private methods
366 : //
367 :
368 : //
369 : // body-frame rate controller
370 : //
371 :
372 : // rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
373 3233979 : void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads)
374 : {
375 3233979 : if (_flags_heli.leaky_i) {
376 2866203 : _pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
377 : }
378 3233979 : float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _dt_s, _motors.limit.roll, _pd_scale.x, _i_scale.x) + _actuator_sysid.x;
379 :
380 3233979 : if (_flags_heli.leaky_i) {
381 2866203 : _pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
382 : }
383 :
384 3233979 : float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _dt_s, _motors.limit.pitch, _pd_scale.y, _i_scale.y) + _actuator_sysid.y;
385 :
386 : // use pid library to calculate ff
387 3233979 : float roll_ff = _pid_rate_roll.get_ff();
388 3233979 : float pitch_ff = _pid_rate_pitch.get_ff();
389 :
390 : // add feed forward and final output
391 3233979 : float roll_out = roll_pid + roll_ff;
392 3233979 : float pitch_out = pitch_pid + pitch_ff;
393 :
394 : // constrain output
395 3233979 : roll_out = constrain_float(roll_out, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
396 3233979 : pitch_out = constrain_float(pitch_out, -AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX);
397 :
398 : // output to motors
399 3233979 : _motors.set_roll(roll_out);
400 3233979 : _motors.set_pitch(pitch_out);
401 :
402 : // Piro-Comp, or Pirouette Compensation is a pre-compensation calculation, which basically rotates the Roll and Pitch Rate I-terms as the
403 : // helicopter rotates in yaw. Much of the built-up I-term is needed to tip the disk into the incoming wind. Fast yawing can create an instability
404 : // as the built-up I-term in one axis must be reduced, while the other increases. This helps solve that by rotating the I-terms before the error occurs.
405 : // It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption.
406 3233979 : if (_piro_comp_enabled) {
407 :
408 : // used to hold current I-terms while doing piro comp:
409 0 : const float piro_roll_i = _pid_rate_roll.get_i();
410 0 : const float piro_pitch_i = _pid_rate_pitch.get_i();
411 :
412 0 : Vector2f yawratevector;
413 0 : yawratevector.x = cosf(-rate_rads.z * _dt_s);
414 0 : yawratevector.y = sinf(-rate_rads.z * _dt_s);
415 0 : yawratevector.normalize();
416 :
417 0 : _pid_rate_roll.set_integrator(piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y);
418 0 : _pid_rate_pitch.set_integrator(piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y);
419 : }
420 :
421 3233979 : }
422 :
423 : // rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second
424 3233979 : float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_rads, float rate_target_rads)
425 : {
426 3233979 : if (_motors.get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
427 1238375 : _pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
428 : }
429 :
430 3233979 : float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _dt_s, _motors.limit.yaw, _pd_scale.z, _i_scale.z) + _actuator_sysid.z;
431 :
432 : // use pid library to calculate ff
433 3233979 : float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar;
434 :
435 : // add feed forward
436 3233979 : float yaw_out = pid + vff;
437 :
438 : // constrain output
439 3233979 : yaw_out = constrain_float(yaw_out, -AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX, AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX);
440 :
441 : // output to motors
442 3233979 : return yaw_out;
443 : }
444 :
445 : //
446 : // throttle functions
447 : //
448 :
449 3213461 : void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
450 : {
451 3213461 : _throttle_in = throttle_in;
452 3213461 : update_althold_lean_angle_max(throttle_in);
453 :
454 3213461 : _motors.set_throttle_filter_cutoff(filter_cutoff);
455 3213461 : if (apply_angle_boost && !((AP_MotorsHeli&)_motors).in_autorotation()) {
456 : // Apply angle boost
457 3181156 : throttle_in = get_throttle_boosted(throttle_in);
458 : } else {
459 : // Clear angle_boost for logging purposes
460 32305 : _angle_boost = 0.0f;
461 : }
462 3213461 : _motors.set_throttle(throttle_in);
463 3213461 : }
464 :
465 : // returns a throttle including compensation for roll/pitch angle
466 : // throttle value should be 0 ~ 1
467 3181156 : float AC_AttitudeControl_Heli::get_throttle_boosted(float throttle_in)
468 : {
469 3181156 : if (!_angle_boost_enabled) {
470 0 : _angle_boost = 0;
471 0 : return throttle_in;
472 : }
473 : // inverted_factor is 1 for tilt angles below 60 degrees
474 : // inverted_factor changes from 1 to -1 for tilt angles between 60 and 120 degrees
475 :
476 3181156 : float cos_tilt = _ahrs.cos_pitch() * _ahrs.cos_roll();
477 3181156 : float inverted_factor = constrain_float(2.0f * cos_tilt, -1.0f, 1.0f);
478 3181156 : float cos_tilt_target = fabsf(cosf(_thrust_angle_rad));
479 3181156 : float boost_factor = 1.0f / constrain_float(cos_tilt_target, 0.1f, 1.0f);
480 :
481 : // angle boost and inverted factor applied about the zero thrust collective
482 3181156 : const float coll_mid = ((AP_MotorsHeli&)_motors).get_coll_mid();
483 3181156 : float throttle_out = ((throttle_in - coll_mid) * inverted_factor * boost_factor) + coll_mid;
484 3181156 : _angle_boost = constrain_float(throttle_out - throttle_in, -1.0f, 1.0f);
485 3181156 : return throttle_out;
486 : }
487 :
488 : // get_roll_trim - angle in centi-degrees to be added to roll angle for learn hover collective. Used by helicopter to counter tail rotor thrust in hover
489 2656346 : float AC_AttitudeControl_Heli::get_roll_trim_cd()
490 : {
491 : // hover roll trim is given the opposite sign in inverted flight since the tail rotor thrust is pointed in the opposite direction.
492 2656346 : float inverted_factor = constrain_float(2.0f * _ahrs.cos_roll(), -1.0f, 1.0f);
493 2656346 : return constrain_float(_hover_roll_trim_scalar * _hover_roll_trim_cd * inverted_factor, -1000.0f,1000.0f);
494 : }
495 :
496 : // Sets desired roll and pitch angles (in radians) and yaw rate (in radians/s).
497 : // Used when roll/pitch stabilization is needed with manual or autonomous yaw rate control.
498 : // Applies acceleration-limited input shaping for smooth transitions and computes body-frame angular velocity targets.
499 1979990 : void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_rate_rads)
500 : {
501 1979990 : if (_inverted_flight) {
502 9840 : euler_roll_angle_rad = wrap_PI(euler_roll_angle_rad + M_PI);
503 : }
504 1979990 : AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_rate_rads);
505 1979990 : }
506 :
507 : // Sets desired roll, pitch, and yaw angles (in radians).
508 : // Used to follow an absolute attitude setpoint. Input shaping and yaw slew limits are applied.
509 : // Outputs are passed to the rate controller via shaped angular velocity targets.
510 451785 : void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw_rad(float euler_roll_angle_rad, float euler_pitch_angle_rad, float euler_yaw_angle_rad, bool slew_yaw)
511 : {
512 451785 : if (_inverted_flight) {
513 0 : euler_roll_angle_rad = wrap_PI(euler_roll_angle_rad + M_PI);
514 : }
515 451785 : AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_angle_rad, slew_yaw);
516 451785 : }
517 :
518 8142 : void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
519 : {
520 : #if AP_FILTER_ENABLED
521 8142 : _pid_rate_roll.set_notch_sample_rate(sample_rate);
522 8142 : _pid_rate_pitch.set_notch_sample_rate(sample_rate);
523 8142 : _pid_rate_yaw.set_notch_sample_rate(sample_rate);
524 : #endif
525 8142 : }
526 :
527 : // Sets desired thrust vector and heading rate (in radians/s).
528 : // Used for tilt-based navigation with independent yaw control.
529 : // The thrust vector defines the desired orientation (e.g., pointing direction for vertical thrust),
530 : // while the heading rate adjusts yaw. The input is shaped by acceleration and slew limits.
531 347469 : void AC_AttitudeControl_Heli::input_thrust_vector_rate_heading_rads(const Vector3f& thrust_vector, float heading_rate_rads, bool slew_yaw)
532 : {
533 :
534 347469 : if (!_inverted_flight) {
535 347469 : AC_AttitudeControl::input_thrust_vector_rate_heading_rads(thrust_vector, heading_rate_rads, slew_yaw);
536 347469 : return;
537 : }
538 : // convert thrust vector to a roll and pitch angles
539 : // this negates the advantage of using thrust vector control, but works just fine
540 0 : Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
541 :
542 0 : angle_target.x = wrap_PI(angle_target.x + M_PI);
543 0 : AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(angle_target.x, angle_target.y, heading_rate_rads);
544 : }
545 :
546 :
547 : // Sets desired thrust vector and heading (in radians) with heading rate (in radians/s).
548 : // Used for advanced attitude control where thrust direction is separated from yaw orientation.
549 : // Heading slew is constrained based on configured limits.
550 417555 : void AC_AttitudeControl_Heli::input_thrust_vector_heading_rad(const Vector3f& thrust_vector, float heading_angle_rad, float heading_rate_rads)
551 : {
552 417555 : if (!_inverted_flight) {
553 417555 : AC_AttitudeControl::input_thrust_vector_heading_rad(thrust_vector, heading_angle_rad, heading_rate_rads);
554 417555 : return;
555 : }
556 : // convert thrust vector to a roll and pitch angles
557 : // this negates the advantage of using thrust vector control, but works just fine
558 0 : Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312();
559 :
560 0 : angle_target.x = wrap_PI(angle_target.x + M_PI);
561 0 : AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(angle_target.x, angle_target.y, heading_angle_rad, true);
562 : }
|
