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 : #include <stdlib.h>
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <SRV_Channel/SRV_Channel.h>
19 : #include "AP_MotorsHeli_Single.h"
20 : #include <GCS_MAVLink/GCS.h>
21 :
22 : extern const AP_HAL::HAL& hal;
23 :
24 : const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
25 : AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
26 :
27 : // Indices 1-3 were used by servo position params and should not be used
28 :
29 : // @Param: TAIL_TYPE
30 : // @DisplayName: Tail Type
31 : // @Description: Tail type selection. Servo Only uses tail rotor pitch to provide yaw control (including stabilization) via an output assigned to Motor4. Servo with External Gyro uses an external gyro to control tail rotor pitch via a servo. Yaw control without stabilization is passed to the external gyro via the output assigned to Motor4. Direct Drive Variable Pitch(DDVP) is used for tails that have a motor whose ESC is connected to an output with function HeliTailRSC. Tail pitch is still accomplished with a servo on an output assigned to Motor4 function. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above with a motor whose ESC is controlled by an output whose function is Motor4. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above with a motor whose ESC is controlled by an output whose function is Motor4. In both DDFP cases, no servo is used for the tail and the tail motor esc on Motor4 output is used to control the yaw axis using motor speed.
32 : // @Values: 0:Servo only,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW
33 : // @User: Standard
34 : AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, float(TAIL_TYPE::SERVO)),
35 :
36 : // Indice 5 was used by SWASH_TYPE and should not be used
37 :
38 : // Indice 6 was used by GYR_GAIN and should not be used
39 :
40 : // Index 7 was used for phase angle and should not be used
41 :
42 : // Indice 8 was used by COLYAW and should not be used
43 :
44 : // Indice 9 was used by FLYBAR_MODE and should not be used
45 :
46 : // @Param: TAIL_SPEED
47 : // @DisplayName: DDVP Tail ESC speed
48 : // @Description: Direct drive, variable pitch tail ESC speed in percent output to the tail motor esc (HeliTailRSC Servo) when motor interlock enabled (throttle hold off) and speed fully ramped up after spoolup.
49 : // @Range: 0 100
50 : // @Units: %
51 : // @Increment: 1
52 : // @User: Standard
53 : AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT),
54 :
55 : // Indice 11 was used by GYR_GAIN_ACRO and should not be used
56 :
57 : // Indices 16-19 were used by RSC_PWM_MIN, RSC_PWM_MAX, RSC_PWM_REV, and COL_CTRL_DIR and should not be used
58 :
59 : // @Param: SW_TYPE
60 : // @DisplayName: Swashplate Type
61 : // @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
62 : // @Values: 0:H3 Generic,1:H1 non-CPPM,2:H3_140,3:H3_120,4:H4_90,5:H4_45
63 : // @User: Standard
64 :
65 : // @Param: SW_COL_DIR
66 : // @DisplayName: Collective Direction
67 : // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
68 : // @Values: 0:Normal,1:Reversed
69 : // @User: Standard
70 :
71 : // @Param: SW_LIN_SVO
72 : // @DisplayName: Linearize Swash Servos
73 : // @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
74 : // @Values: 0:Disabled,1:Enabled
75 : // @User: Standard
76 :
77 : // @Param: SW_PHANG
78 : // @DisplayName: Swashplate Phase Angle Compensation
79 : // @Description: Phase angle compensation can be used to correct control coupling issues. If pitching the swash forward induces a roll, this can be correct the problem.
80 : // @Range: -30 30
81 : // @Units: deg
82 : // @User: Advanced
83 : // @Increment: 1
84 :
85 : // @Param: SW_H3_ENABLE
86 : // @DisplayName: H3 Generic Enable
87 : // @Description: Automatically set when H3 generic swash type is selected for swashplate. Do not set manually.
88 : // @Values: 0:Disabled,1:Enabled
89 : // @User: Advanced
90 :
91 : // @Param: SW_H3_SV1_POS
92 : // @DisplayName: H3 Generic Servo 1 Position
93 : // @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
94 : // @Range: -180 180
95 : // @Units: deg
96 : // @User: Advanced
97 :
98 : // @Param: SW_H3_SV2_POS
99 : // @DisplayName: H3 Generic Servo 2 Position
100 : // @Description: Azimuth position on swashplate for servo 2 with the front of the heli being 0 deg
101 : // @Range: -180 180
102 : // @Units: deg
103 : // @User: Advanced
104 :
105 : // @Param: SW_H3_SV3_POS
106 : // @DisplayName: H3 Generic Servo 3 Position
107 : // @Description: Azimuth position on swashplate for servo 3 with the front of the heli being 0 deg
108 : // @Range: -180 180
109 : // @Units: deg
110 : // @User: Advanced
111 : AP_SUBGROUPINFO(_swashplate, "SW_", 20, AP_MotorsHeli_Single, AP_MotorsHeli_Swash),
112 :
113 : // @Param: COL2YAW
114 : // @DisplayName: Collective-Yaw Mixing
115 : // @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
116 : // @Range: -2 2
117 : // @Increment: 0.1
118 : // @User: Standard
119 : AP_GROUPINFO("COL2YAW", 21, AP_MotorsHeli_Single, _collective_yaw_scale, 0),
120 :
121 : // @Param: DDFP_THST_EXPO
122 : // @DisplayName: DDFP Tail Rotor Thrust Curve Expo
123 : // @Description: Tail rotor DDFP motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
124 : // @Range: -1 1
125 : // @User: Standard
126 :
127 : // @Param: DDFP_SPIN_MIN
128 : // @DisplayName: DDFP Tail Rotor Motor Spin minimum
129 : // @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range.
130 : // @Values: 0.0:Low, 0.15:Default, 0.3:High
131 : // @Range: 0.0 0.3
132 : // @User: Standard
133 :
134 : // @Param: DDFP_SPIN_MAX
135 : // @DisplayName: DDFP Tail Rotor Motor Spin maximum
136 : // @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
137 : // @Values: 0.9:Low, 0.95:Default, 1.0:High
138 : // @Range: 0.9 1.0
139 : // @User: Standard
140 :
141 : // @Param: DDFP_BAT_IDX
142 : // @DisplayName: DDFP Tail Rotor Battery compensation index
143 : // @Description: Which battery monitor should be used for doing compensation
144 : // @Values: 0:First battery, 1:Second battery
145 : // @Range: 0 15
146 : // @User: Standard
147 :
148 : // @Param: DDFP_BAT_V_MAX
149 : // @DisplayName: Battery voltage compensation maximum voltage
150 : // @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.2 * cell count, 0 = Disabled
151 : // @Range: 6 53
152 : // @Units: V
153 : // @User: Standard
154 :
155 : // @Param: DDFP_BAT_V_MIN
156 : // @DisplayName: Battery voltage compensation minimum voltage
157 : // @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.3 * cell count, 0 = Disabled
158 : // @Range: 6 42
159 : // @Units: V
160 : // @User: Standard
161 : AP_SUBGROUPINFO(thr_lin, "DDFP_", 22, AP_MotorsHeli_Single, Thrust_Linearization),
162 :
163 : // @Param: YAW_TRIM
164 : // @DisplayName: Tail Rotor Trim
165 : // @Description: Fixed offset applied to yaw output to minimize yaw I-term contribution needed to counter rotor drag. Currently only works for DDFP tails (H_TAIL_TYPE = 3 or H_TAIL_TYPE = 4). If using the H_COL2YAW compensation this trim is used to compensate for the main rotor profile drag. If H_COL2YAW is not used, this value can be set to reduce the yaw I-term to zero when in a steady hover. In this case, move trim in the direction of the I-term found in PIDY.I log message (e.g. negative I means you want negative trim).
166 : // @Range: -1 1
167 : // @User: Standard
168 : AP_GROUPINFO("YAW_TRIM", 23, AP_MotorsHeli_Single, _yaw_trim, 0.0f),
169 :
170 : // @Param: TAIL_RAMP_TIME
171 : // @DisplayName: Tail RSC Throttle Ramp Time
172 : // @Description: Time in seconds for throttle output (TailHeliRSC servo) to ramp from ground idle (RSC_IDLE) to flight idle throttle setting when motor interlock is enabled (throttle hold off).
173 : // @Range: 0 60
174 : // @Units: s
175 : // @User: Standard
176 : AP_GROUPINFO("TAIL_RAMP_TIME", 24, AP_MotorsHeli_Single, _direct_drive_ramp_time, AP_MOTORS_HELI_SINGLE_TAIL_RAMP_TIME_DEFAULT),
177 :
178 : AP_GROUPEND
179 : };
180 :
181 : #define YAW_SERVO_MAX_ANGLE 4500
182 :
183 : // set update rate to motors - a value in hertz
184 160 : void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
185 : {
186 : // record requested speed
187 160 : _speed_hz = speed_hz;
188 :
189 : // setup fast channels
190 160 : uint32_t mask = (1U << AP_MOTORS_MOT_4) | _swashplate.get_output_mask();
191 :
192 160 : if (get_tail_type() == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW || get_tail_type() == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW) {
193 4 : mask = mask | _tail_rotor.get_output_mask();
194 : }
195 160 : rc_set_freq(mask, _speed_hz);
196 160 : }
197 :
198 : // init_outputs - initialise Servo/PWM ranges and endpoints
199 859845 : void AP_MotorsHeli_Single::init_outputs()
200 : {
201 859845 : if (!initialised_ok()) {
202 : // yaw servo
203 80 : add_motor_num(CH_4);
204 :
205 : // initialize main rotor servo
206 80 : _main_rotor.initialize();
207 :
208 80 : switch (get_tail_type()) {
209 2 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW:
210 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
211 : // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation.
212 2 : _tail_rotor.initialize();
213 2 : break;
214 :
215 1 : case TAIL_TYPE::DIRECTDRIVE_VARPITCH:
216 : case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV:
217 1 : _tail_rotor.initialize();
218 : // yaw servo is an angle from -4500 to 4500
219 1 : SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
220 1 : break;
221 :
222 77 : case TAIL_TYPE::SERVO:
223 : default:
224 : // yaw servo is an angle from -4500 to 4500
225 77 : SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
226 77 : break;
227 : }
228 : }
229 :
230 859845 : set_initialised_ok(_frame_class == MOTOR_FRAME_HELI);
231 859845 : }
232 :
233 : // calculate_scalars - recalculates various scalers used.
234 3052343 : void AP_MotorsHeli_Single::calculate_armed_scalars()
235 : {
236 3052343 : if (use_tail_RSC()) {
237 : // configure armed scalars for ddvp tail rotor
238 198906 : _tail_rotor.configure_armed();
239 : }
240 :
241 : // calculate armed scalars for main rotor
242 3052343 : AP_MotorsHeli::calculate_armed_scalars();
243 3052343 : }
244 :
245 : // calculate_scalars - recalculates various scalers used.
246 859845 : void AP_MotorsHeli_Single::calculate_scalars()
247 : {
248 : // range check collective min, max and zero
249 859845 : if( _collective_min >= _collective_max ) {
250 0 : _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN);
251 0 : _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX);
252 : }
253 :
254 859845 : _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
255 :
256 859845 : _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
257 :
258 859845 : if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
259 : // calculate collective zero thrust point as a number from 0 to 1
260 859845 : _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
261 :
262 : // calculate collective land min point as a number from 0 to 1
263 859845 : _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
264 : } else {
265 0 : _collective_zero_thrust_pct = 0.0f;
266 0 : _collective_land_min_pct = 0.0f;
267 : }
268 :
269 : // configure swashplate and update scalars
270 859845 : _swashplate.configure();
271 :
272 : // configure main rotor and update scalars
273 859845 : _main_rotor.configure();
274 :
275 : // configure DDVP and DDFP tail rotor controllers
276 859845 : switch (get_tail_type()) {
277 33974 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
278 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW:
279 33974 : _tail_rotor.configure(ROTOR_CONTROL_MODE_DDFP, _direct_drive_ramp_time.get(), _main_rotor._runup_time.get(), _main_rotor._critical_speed.get(), _main_rotor._idle_output.get());
280 33974 : break;
281 17503 : case TAIL_TYPE::DIRECTDRIVE_VARPITCH:
282 : case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV:
283 17503 : _tail_rotor.configure(ROTOR_CONTROL_MODE_SETPOINT, _direct_drive_ramp_time.get(), _main_rotor._runup_time.get(), _main_rotor._critical_speed.get(), _main_rotor._idle_output.get());
284 17503 : _tail_rotor.set_setpoint_desired_rotor_speed(_direct_drive_tailspeed.get());
285 17503 : break;
286 808368 : case TAIL_TYPE::SERVO:
287 : default:
288 : // disable for all other tail types as they do not use the RSC controller for the tail rotor
289 808368 : _tail_rotor.configure(ROTOR_CONTROL_MODE_DISABLED, 0, 0, 0, 0);
290 808368 : break;
291 : }
292 :
293 : // calculate armed scalars for both main and tail rotor
294 859845 : calculate_armed_scalars();
295 859845 : }
296 :
297 : // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
298 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
299 79 : uint32_t AP_MotorsHeli_Single::get_motor_mask()
300 : {
301 79 : return _main_rotor.get_output_mask() | _tail_rotor.get_output_mask();
302 : }
303 :
304 : // update_motor_controls - sends commands to motor controllers
305 3052397 : void AP_MotorsHeli_Single::update_motor_control(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
306 : {
307 : // Send state update to motors
308 3052397 : _tail_rotor.update(_dt_s);
309 3052397 : _main_rotor.update(_dt_s);
310 :
311 3052397 : if (state == AP_MotorsHeli_RSC::DesiredRSCSpoolState::SHUT_DOWN){
312 : // set engine run enable aux output to not run position to kill engine when disarmed
313 859899 : SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN);
314 : } else {
315 : // else if armed, set engine run enable output to run position
316 2192498 : SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MAX);
317 : }
318 :
319 3052397 : }
320 :
321 : // update_spool_state - updates the spool state based on the desired state
322 3052397 : AP_Motors::SpoolState AP_MotorsHeli_Single::update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
323 : {
324 :
325 3052397 : _tail_rotor.update_spool_state(state, _dt_s);
326 3052397 : SpoolState main_rotor_state = _main_rotor.update_spool_state(state, _dt_s);
327 :
328 : // Check if main rotor is run-up complete. Tail rotor run-up is not included in check because currently
329 : // the tail rotor in DDVP uses the same ramp and runup time as the main rotor. This may need changed if
330 : // the RSC is used for DDFP tail rotors.
331 3052397 : set_rotor_runup_complete(main_rotor_state == AP_MotorsHeli_RSC::RSCSpoolState::THROTTLE_UNLIMITED);
332 :
333 3052397 : return main_rotor_state;
334 :
335 : }
336 :
337 : //
338 : // move_actuators - moves swash plate and tail rotor
339 : // - expected ranges:
340 : // roll : -1 ~ +1
341 : // pitch: -1 ~ +1
342 : // collective: 0 ~ 1
343 : // yaw: -1 ~ +1
344 : //
345 3052397 : void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)
346 : {
347 : // rescale roll_out and pitch_out into the min and max ranges to provide linear motion
348 : // across the input range instead of stopping when the input hits the constrain value
349 : // these calculations are based on an assumption of the user specified cyclic_max
350 : // coming into this equation at 4500 or less
351 3052397 : float total_out = norm(pitch_out, roll_out);
352 :
353 3052397 : if (total_out > (_cyclic_max/4500.0f)) {
354 50157 : float ratio = (float)(_cyclic_max/4500.0f) / total_out;
355 50157 : roll_out *= ratio;
356 50157 : pitch_out *= ratio;
357 50157 : limit.roll = true;
358 50157 : limit.pitch = true;
359 : }
360 :
361 : // constrain collective input
362 3052397 : float collective_out = coll_in;
363 3052397 : if (collective_out <= 0.0f) {
364 1337056 : collective_out = 0.0f;
365 1337056 : limit.throttle_lower = true;
366 : }
367 3052397 : if (collective_out >= 1.0f) {
368 20087 : collective_out = 1.0f;
369 20087 : limit.throttle_upper = true;
370 : }
371 :
372 : // ensure not below landed/landing collective
373 3052397 : if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
374 30607 : collective_out = _collective_land_min_pct;
375 30607 : limit.throttle_lower = true;
376 : }
377 :
378 : // updates below land min collective flag
379 3052397 : _heliflags.below_land_min_coll = !is_positive(collective_out - _collective_land_min_pct);
380 :
381 : // updates takeoff collective flag based on 50% hover collective
382 3052397 : update_takeoff_collective_flag(collective_out);
383 :
384 : // feed power estimate into main rotor controller
385 : // ToDo: include tail rotor power?
386 : // ToDo: add main rotor cyclic power?
387 3052397 : _main_rotor.set_collective(fabsf(collective_out));
388 :
389 : // scale collective pitch for swashplate servos
390 3052397 : float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
391 3052397 : float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
392 :
393 : // Caculate servo positions from swashplate library
394 3052397 : _swashplate.calculate(roll_out, pitch_out, collective_out_scaled);
395 :
396 3052397 : if (have_DDFP_tail()) {
397 : // calc filtered battery voltage and lift_max
398 151002 : thr_lin.update_lift_max_from_batt_voltage();
399 : // apply compensation gain to yaw output
400 151002 : yaw_out *= thr_lin.get_compensation_gain();
401 : }
402 :
403 : // Get yaw offset required to cancel out steady state main rotor torque
404 3052397 : const float yaw_offset = get_yaw_offset(collective_out);
405 :
406 : // update the yaw rate using the tail rotor/servo
407 3052397 : move_yaw(yaw_out + yaw_offset);
408 3052397 : }
409 :
410 : // move_yaw
411 3052397 : void AP_MotorsHeli_Single::move_yaw(float yaw_out)
412 : {
413 : // sanity check yaw_out
414 3052397 : if (yaw_out < -1.0f) {
415 918 : yaw_out = -1.0f;
416 918 : limit.yaw = true;
417 : }
418 3052397 : if (yaw_out > 1.0f) {
419 57747 : yaw_out = 1.0f;
420 57747 : limit.yaw = true;
421 : }
422 :
423 3052397 : _servo4_out = yaw_out;
424 3052397 : }
425 :
426 : // Get yaw offset required to cancel out steady state main rotor torque
427 3052397 : float AP_MotorsHeli_Single::get_yaw_offset(float collective)
428 : {
429 3052397 : if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
430 : // Not in direct control of tail with external gyro or manual servo mode
431 0 : return 0.0;
432 : }
433 :
434 3052397 : if (_main_rotor.autorotation.active() || (_main_rotor.get_control_output() <= _main_rotor.get_idle_output())) {
435 : // Motor is stopped or at idle, and thus not creating torque
436 1086993 : return 0.0;
437 : }
438 :
439 : // sanity check collective_yaw_scale
440 1965404 : _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE));
441 :
442 : // This feedforward compensation follows the hover performance theory that hover power required
443 : // is a function of gross weight to the 3/2 power
444 1965404 : float yaw_offset = _collective_yaw_scale * powf(fabsf(collective - _collective_zero_thrust_pct),1.5f);
445 :
446 : // Add yaw trim for DDFP tails
447 1965404 : if (have_DDFP_tail()) {
448 114948 : yaw_offset += _yaw_trim.get();
449 : }
450 :
451 1965404 : return yaw_offset;
452 : }
453 :
454 3052397 : void AP_MotorsHeli_Single::output_to_motors()
455 : {
456 3052397 : if (!initialised_ok()) {
457 0 : return;
458 : }
459 :
460 : // Write swashplate outputs
461 3052397 : _swashplate.output();
462 :
463 : // Output tail rotor
464 3052397 : switch (get_tail_type()) {
465 0 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
466 : // Invert output for CCW tail
467 0 : _servo4_out *= -1.0;
468 : FALLTHROUGH;
469 :
470 151002 : case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW:
471 151002 : output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out));
472 151002 : break;
473 :
474 2901395 : case TAIL_TYPE::SERVO:
475 : case TAIL_TYPE::DIRECTDRIVE_VARPITCH:
476 : case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV:
477 : default:
478 2901395 : rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
479 2901395 : break;
480 : }
481 :
482 : // Output main rotor and tail rotor. must be last so DDFP desired speed gets set before outputting to the motor.
483 3052397 : _main_rotor.output_to_servo();
484 3052397 : _tail_rotor.output_to_servo();
485 :
486 : }
487 :
488 : // handle output limit flags and send throttle to servos lib
489 151002 : void AP_MotorsHeli_Single::output_to_ddfp_tail(float throttle)
490 : {
491 : // Note: yaw trim thrust has already been applied. the output should only be from 0 to 1.
492 : // Upper limit
493 151002 : if (throttle >= 1.0){
494 0 : throttle = 1.0;
495 0 : limit.yaw = true;
496 : }
497 :
498 : // Lower limit
499 151002 : if (throttle <= 0.0){
500 151002 : throttle = 0.0;
501 151002 : limit.yaw = true;
502 : }
503 :
504 151002 : _tail_rotor.set_ddfp_desired_rotor_speed(throttle);
505 151002 : }
506 :
507 : // servo_test - move servos through full range of movement
508 0 : void AP_MotorsHeli_Single::servo_test()
509 : {
510 0 : _servo_test_cycle_time += _dt_s;
511 :
512 0 : if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back
513 0 : (_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){
514 0 : _pitch_test += 2.0 * _dt_s;
515 0 : _oscillate_angle += 8 * M_PI * _dt_s;
516 0 : _yaw_test = 0.5f * sinf(_oscillate_angle);
517 0 : } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around
518 0 : (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){
519 0 : _oscillate_angle += 0.5 * M_PI * _dt_s;
520 0 : _roll_test = sinf(_oscillate_angle);
521 0 : _pitch_test = cosf(_oscillate_angle);
522 0 : _yaw_test = sinf(_oscillate_angle);
523 0 : } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level
524 0 : (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){
525 0 : _pitch_test -= 2.0 * _dt_s;
526 0 : _oscillate_angle += 8 * M_PI * _dt_s;
527 0 : _yaw_test = 0.5f * sinf(_oscillate_angle);
528 0 : } else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top
529 0 : _collective_test += _dt_s;
530 0 : _oscillate_angle += 2 * M_PI * _dt_s;
531 0 : _yaw_test = sinf(_oscillate_angle);
532 0 : } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom
533 0 : _collective_test -= _dt_s;
534 0 : _oscillate_angle += 2 * M_PI * _dt_s;
535 0 : _yaw_test = sinf(_oscillate_angle);
536 : } else { // reset cycle
537 0 : _servo_test_cycle_time = 0.0f;
538 0 : _oscillate_angle = 0.0f;
539 0 : _collective_test = 0.0f;
540 0 : _roll_test = 0.0f;
541 0 : _pitch_test = 0.0f;
542 0 : _yaw_test = 0.0f;
543 : // decrement servo test cycle counter at the end of the cycle
544 0 : if (_servo_test_cycle_counter > 0){
545 0 : _servo_test_cycle_counter--;
546 : }
547 : }
548 :
549 : // over-ride servo commands to move servos through defined ranges
550 0 : _throttle_filter.reset(constrain_float(_collective_test, 0.0f, 1.0f));
551 0 : _roll_in = constrain_float(_roll_test, -1.0f, 1.0f);
552 0 : _pitch_in = constrain_float(_pitch_test, -1.0f, 1.0f);
553 0 : _yaw_in = constrain_float(_yaw_test, -1.0f, 1.0f);
554 0 : }
555 :
556 : // Run arming checks
557 2206 : bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const
558 : {
559 : // run base class checks
560 2206 : if (!AP_MotorsHeli::arming_checks(buflen, buffer)) {
561 0 : return false;
562 : }
563 :
564 : // returns false if direct drive tailspeed is outside of range
565 2206 : if ((_direct_drive_tailspeed < 0) || (_direct_drive_tailspeed > 100)) {
566 0 : hal.util->snprintf(buffer, buflen, "H_TAIL_SPEED out of range");
567 0 : return false;
568 : }
569 :
570 : // returns false if Phase Angle is outside of range for H3 swashplate
571 2206 : if (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30){
572 0 : hal.util->snprintf(buffer, buflen, "H_SW_PHANG out of range");
573 0 : return false;
574 : }
575 :
576 : // returns false if external gyro is selected for tail control but not supported by tail type
577 2206 : if (get_tail_type()==TAIL_TYPE::SERVO_EXTGYRO_OLD){
578 0 : hal.util->snprintf(buffer, buflen, "External gyro no longer supported. See Wiki");
579 0 : return false;
580 : }
581 :
582 : // returns false if tailRSC is not set as a Servo output for DDVP or DDFP tail. This is required for DDVP adn DDFP tails to work properly.
583 : uint8_t tail_rsc_channel;
584 2206 : if (use_tail_RSC() && !SRV_Channels::find_channel(SRV_Channel::k_heli_tail_rsc, tail_rsc_channel)) {
585 0 : hal.util->snprintf(buffer, buflen, "DDVP and DDFP tails require a servo output to be assigned to tailRSC");
586 0 : return false;
587 : }
588 :
589 : // returns false if RSC Mode is not set to DDFP for a DDFP tail
590 2206 : if (have_DDFP_tail() && _tail_rotor._rsc_mode.get() != (int8_t)ROTOR_CONTROL_MODE_DDFP) {
591 0 : hal.util->snprintf(buffer, buflen, "DDFP tail requires RSC mode to be set to DDFP");
592 0 : return false;
593 : }
594 :
595 : // returns false if the DDFP RSC control mode is selected for the main rotor. The code does not currently support
596 : // DDFP control for the main rotor.
597 2206 : if (_main_rotor.get_rsc_control_mode() == ROTOR_CONTROL_MODE_DDFP){
598 0 : hal.util->snprintf(buffer, buflen, "Main rotor RSC does not support Direct Drive Fixed Pitch main rotors");
599 0 : return false;
600 : }
601 :
602 : // returns false if main rotor RSC Runup Time is less than tail rotor ramp time as this could cause undesired behaviour
603 2206 : if (_main_rotor._runup_time.get() <= _tail_rotor._ramp_time.get()){
604 0 : hal.util->snprintf(buffer, buflen, "H_RUNUP_TIME is less than H_TAIL_RAMP_TIME");
605 0 : return false;
606 : }
607 :
608 2206 : return true;
609 : }
610 :
611 : // Helper function for param conversions which are easier to be done in the motors class
612 : // Called from system.cpp
613 80 : void AP_MotorsHeli_Single::heli_motors_param_conversions(void)
614 : {
615 : // Run common conversions from base class
616 80 : AP_MotorsHeli::heli_motors_param_conversions();
617 :
618 : // PARAMETER_CONVERSION - Added: Nov-2023
619 : // Convert trim for DDFP tails
620 : // Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim
621 : // to H_YAW_TRIM. Default thrust linearisation gives linear thrust to throttle relationship to preserve previous setup behaviours so
622 : // we can assume linear relationship in the conversion.
623 80 : if (have_DDFP_tail() && !_yaw_trim.configured()) {
624 :
625 2 : SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4);
626 2 : if (c != nullptr) {
627 0 : uint16_t pwm_min = c->get_output_min();
628 0 : uint16_t pwm_max = c->get_output_max();
629 0 : uint16_t pwm_trim = c->get_trim();
630 :
631 0 : float trim = (float)(pwm_trim - pwm_min) / constrain_uint16(pwm_max - pwm_min, 1, 2000);
632 0 : _yaw_trim.set(trim);
633 : }
634 : // Motor 4 may not have been assigned to an output yet in which case this is unlikely to be a conversion from old setup.
635 : // Prevent future attempts to convert the param so we don't put a non-sense value in.
636 2 : _yaw_trim.save();
637 : }
638 :
639 80 : if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H3) {
640 : // PARAMETER_CONVERSION - Added: Mar-2026
641 : // Previous versions had phase angle for only the generic H3 swashplate. Phase angle is now available for all swashplate
642 : // types but needs to be converted for users of the generic H3 type. Convert old H3 phase angle parameter to new parameter
643 : // applicable to all swashplate types.
644 80 : const AP_Param::ConversionInfo sw_phase_conversion_info[] = {
645 : { 90, 532, AP_PARAM_INT16, "H_SW_PHANG" },
646 : };
647 80 : uint8_t table_size = ARRAY_SIZE(sw_phase_conversion_info);
648 160 : for (uint8_t i=0; i<table_size; i++) {
649 80 : AP_Param::convert_old_parameter(&sw_phase_conversion_info[i], 1.0);
650 : }
651 : }
652 :
653 80 : if (use_tail_RSC()) {
654 : // PARAMETER_CONVERSION - Added: Jun-2026 for ArduPilot-4.8
655 : // New parameter added to allow users to specify Tail RSC ramp time. Thus
656 : // if the tail rsc is being used then the H_RSC_RAMP_TIME is assigned to the
657 : // H_TAIL_RAMP_TIME parameter.
658 3 : const AP_Param::ConversionInfo ramp_time_conversion_info = { 90, 13888, AP_PARAM_INT8, "H_TAIL_RAMP_TIME" };
659 3 : AP_Param::convert_old_parameter(&ramp_time_conversion_info, 1.0);
660 : }
661 80 : }
662 :
663 : // Helper to return true for direct drive fixed pitch tail, either CW or CCW
664 5020087 : bool AP_MotorsHeli_Single::have_DDFP_tail() const
665 : {
666 5020087 : const TAIL_TYPE type = get_tail_type();
667 5020087 : return (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) ||
668 5020087 : (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW);
669 : }
670 :
671 : // Helper to return true if the tail RSC should be used
672 3054629 : bool AP_MotorsHeli_Single::use_tail_RSC() const
673 : {
674 3054629 : const TAIL_TYPE type = get_tail_type();
675 3006678 : return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) ||
676 3006678 : (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV) ||
677 6061307 : (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) ||
678 3054629 : (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW);
679 : }
680 :
681 : #if HAL_LOGGING_ENABLED
682 76247 : void AP_MotorsHeli_Single::Log_Write(void)
683 : {
684 : // Write swash plate logging
685 : // For single heli we have to apply an additional cyclic scaler of sqrt(2.0) because the
686 : // definition of when we achieve _cyclic_max is different to dual heli. In single, _cyclic_max
687 : // is limited at sqrt(2.0), in dual it is limited at 1.0
688 76247 : float cyclic_angle_scaler = get_cyclic_angle_scaler() * sqrtf(2.0);
689 76247 : _swashplate.write_log(cyclic_angle_scaler, _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
690 :
691 : // Write RSC logging
692 76247 : _main_rotor.write_log();
693 76247 : _tail_rotor.write_log();
694 76247 : }
695 : #endif
|
