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 "AP_MotorsHeli_Dual.h"
19 : #include <GCS_MAVLink/GCS.h>
20 :
21 : extern const AP_HAL::HAL& hal;
22 :
23 : const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
24 : AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
25 :
26 : // Indices 1-6 were used by servo position params and should not be used
27 :
28 : // Indices 7-8 were used by phase angle params and should not be used
29 :
30 : // @Param: DUAL_MODE
31 : // @DisplayName: Dual Mode
32 : // @Description: Sets the dual mode of the heli, either as tandem, transverse, or intermeshing/coaxial.
33 : // @Values: 0:Longitudinal, 1:Transverse, 2:Intermeshing/Coaxial
34 : // @User: Standard
35 : AP_GROUPINFO("DUAL_MODE", 9, AP_MotorsHeli_Dual, _dual_mode, AP_MOTORS_HELI_DUAL_MODE_TANDEM),
36 :
37 : // @Param: DCP_SCALER
38 : // @DisplayName: Differential-Collective-Pitch Scaler
39 : // @Description: Scaling factor applied to the differential-collective-pitch
40 : // @Range: 0 1
41 : // @User: Standard
42 : AP_GROUPINFO("DCP_SCALER", 10, AP_MotorsHeli_Dual, _dcp_scaler, AP_MOTORS_HELI_DUAL_DCP_SCALER),
43 :
44 : // @Param: DCP_YAW
45 : // @DisplayName: Differential-Collective-Pitch Yaw Mixing
46 : // @Description: Feed-forward compensation to automatically add yaw input when differential collective pitch is applied. Disabled for intermeshing mode.
47 : // @Range: -10 10
48 : // @Increment: 0.1
49 : // @User: Standard
50 : AP_GROUPINFO("DCP_YAW", 11, AP_MotorsHeli_Dual, _dcp_yaw_effect, 0),
51 :
52 : // @Param: YAW_SCALER
53 : // @DisplayName: Scaler for yaw mixing
54 : // @Description: Scaler for mixing yaw into roll or pitch.
55 : // @Range: -10 10
56 : // @Increment: 0.1
57 : // @User: Standard
58 : AP_GROUPINFO("YAW_SCALER", 12, AP_MotorsHeli_Dual, _yaw_scaler, 1.0f),
59 :
60 : // Indices 13-15 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
61 :
62 : // @Param: COL2_MIN
63 : // @DisplayName: Swash 2 Minimum Collective Pitch
64 : // @Description: Lowest possible servo position in PWM microseconds for swashplate 2
65 : // @Range: 1000 2000
66 : // @Units: PWM
67 : // @Increment: 1
68 : // @User: Standard
69 : AP_GROUPINFO("COL2_MIN", 16, AP_MotorsHeli_Dual, _collective2_min, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN),
70 :
71 : // @Param: COL2_MAX
72 : // @DisplayName: Swash 2 Maximum Collective Pitch
73 : // @Description: Highest possible servo position in PWM microseconds for swashplate 2
74 : // @Range: 1000 2000
75 : // @Units: PWM
76 : // @Increment: 1
77 : // @User: Standard
78 : AP_GROUPINFO("COL2_MAX", 17, AP_MotorsHeli_Dual, _collective2_max, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX),
79 :
80 : // Indice 18 was used by COL2_MID and should not be used
81 :
82 : // Indice 19 was used by COL_CTRL_DIR and should not be used
83 :
84 : // @Param: SW_TYPE
85 : // @DisplayName: Swash 1 Type
86 : // @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
87 : // @Values: 0:H3 Generic,1:H1 non-CPPM,2:H3_140,3:H3_120,4:H4_90,5:H4_45
88 : // @User: Standard
89 :
90 : // @Param: SW_COL_DIR
91 : // @DisplayName: Swash 1 Collective Direction
92 : // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
93 : // @Values: 0:Normal,1:Reversed
94 : // @User: Standard
95 :
96 : // @Param: SW_LIN_SVO
97 : // @DisplayName: Linearize Swash 1 Servos
98 : // @Description: This linearizes the swashplate 1 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.
99 : // @Values: 0:Disabled,1:Enabled
100 : // @User: Standard
101 :
102 : // @Param: SW_PHANG
103 : // @DisplayName: Swash 1 Phase Angle Compensation
104 : // @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.
105 : // @Range: -30 30
106 : // @Units: deg
107 : // @User: Advanced
108 : // @Increment: 1
109 :
110 : // @Param: SW_H3_ENABLE
111 : // @DisplayName: Swash 1 H3 Generic Enable
112 : // @Description: Automatically set when H3 generic swash type is selected for swashplate 1. Do not set manually.
113 : // @Values: 0:Disabled,1:Enabled
114 : // @User: Advanced
115 :
116 : // @Param: SW_H3_SV1_POS
117 : // @DisplayName: Swash 1 H3 Generic Servo 1 Position
118 : // @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
119 : // @Range: -180 180
120 : // @Units: deg
121 : // @User: Advanced
122 :
123 : // @Param: SW_H3_SV2_POS
124 : // @DisplayName: Swash 1 H3 Generic Servo 2 Position
125 : // @Description: Azimuth position on swashplate 1 for servo 2 with the front of the heli being 0 deg
126 : // @Range: -180 180
127 : // @Units: deg
128 : // @User: Advanced
129 :
130 : // @Param: SW_H3_SV3_POS
131 : // @DisplayName: Swash 1 H3 Generic Servo 3 Position
132 : // @Description: Azimuth position on swashplate 1 for servo 3 with the front of the heli being 0 deg
133 : // @Range: -180 180
134 : // @Units: deg
135 : // @User: Advanced
136 : AP_SUBGROUPINFO(_swashplate1, "SW_", 20, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
137 :
138 : // @Param: SW2_TYPE
139 : // @DisplayName: Swash 2 Type
140 : // @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
141 : // @Values: 0:H3 Generic,1:H1 non-CPPM,2:H3_140,3:H3_120,4:H4_90,5:H4_45
142 : // @User: Standard
143 :
144 : // @Param: SW2_COL_DIR
145 : // @DisplayName: Swash 2 Collective Direction
146 : // @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
147 : // @Values: 0:Normal,1:Reversed
148 : // @User: Standard
149 :
150 : // @Param: SW2_LIN_SVO
151 : // @DisplayName: Linearize Swash 2 Servos
152 : // @Description: This linearizes the swashplate 2 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.
153 : // @Values: 0:Disabled,1:Enabled
154 : // @User: Standard
155 :
156 : // @Param: SW2_PHANG
157 : // @DisplayName: Swash 2 Phase Angle Compensation
158 : // @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.
159 : // @Range: -30 30
160 : // @Units: deg
161 : // @User: Advanced
162 : // @Increment: 1
163 :
164 : // @Param: SW2_H3_ENABLE
165 : // @DisplayName: Swash 2 H3 Generic Enable
166 : // @Description: Automatically set when H3 generic swash type is selected for swashplate 2. Do not set manually.
167 : // @Values: 0:Disabled,1:Enabled
168 : // @User: Advanced
169 :
170 : // @Param: SW2_H3_SV1_POS
171 : // @DisplayName: Swash 2 H3 Generic Servo 1 Position
172 : // @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
173 : // @Range: -180 180
174 : // @Units: deg
175 : // @User: Advanced
176 :
177 : // @Param: SW2_H3_SV2_POS
178 : // @DisplayName: Swash 2 H3 Generic Servo 2 Position
179 : // @Description: Azimuth position on swashplate 2 for servo 2 with the front of the heli being 0 deg
180 : // @Range: -180 180
181 : // @Units: deg
182 : // @User: Advanced
183 :
184 : // @Param: SW2_H3_SV3_POS
185 : // @DisplayName: Swash 2 H3 Generic Servo 3 Position
186 : // @Description: Azimuth position on swashplate 2 for servo 3 with the front of the heli being 0 deg
187 : // @Range: -180 180
188 : // @Units: deg
189 : // @User: Advanced
190 : AP_SUBGROUPINFO(_swashplate2, "SW2_", 21, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
191 :
192 : // @Param: DCP_TRIM
193 : // @DisplayName: Differential Collective Pitch Trim
194 : // @Description: Removes I term bias due to center of gravity offsets or discrepancies between rotors in swashplate setup. If DCP axis has I term bias while hovering in calm winds, use value of bias in DCP_TRIM to re-center I term.
195 : // @Range: -0.2 0.2
196 : // @Increment: 0.01
197 : // @User: Standard
198 : AP_GROUPINFO("DCP_TRIM", 22, AP_MotorsHeli_Dual, _dcp_trim, 0.0f),
199 :
200 : // @Param: YAW_REV_EXPO
201 : // @DisplayName: Yaw reverser expo
202 : // @Description: For intermeshing mode only. Yaw revereser smoothing exponent, smoothen transition near zero collective region. Increase this parameter to shink smoothing range. Set to -1 to disable reverser.
203 : // @Range: -1 1000
204 : // @Increment: 1.0
205 : // @User: Standard
206 : AP_GROUPINFO("YAW_REV_EXPO", 23, AP_MotorsHeli_Dual, _yaw_rev_expo, -1),
207 :
208 : AP_GROUPEND
209 : };
210 :
211 : // set update rate to motors - a value in hertz
212 0 : void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
213 : {
214 : // record requested speed
215 0 : _speed_hz = speed_hz;
216 :
217 : // setup fast channels
218 0 : uint32_t mask = _swashplate1.get_output_mask() | _swashplate2.get_output_mask();
219 :
220 0 : rc_set_freq(mask, _speed_hz);
221 0 : }
222 :
223 : // init_outputs
224 0 : void AP_MotorsHeli_Dual::init_outputs()
225 : {
226 0 : if (!initialised_ok()) {
227 : // set rotor servo range
228 0 : _main_rotor.initialize();
229 : }
230 :
231 0 : set_initialised_ok(_frame_class == MOTOR_FRAME_HELI_DUAL);
232 0 : }
233 :
234 : // calculate_scalars
235 0 : void AP_MotorsHeli_Dual::calculate_scalars()
236 : {
237 : // range check collective min, max and mid
238 0 : if( _collective_min >= _collective_max ) {
239 0 : _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN);
240 0 : _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX);
241 : }
242 :
243 :
244 : // range check collective min, max and mid for rear swashplate
245 0 : if( _collective2_min >= _collective2_max ) {
246 0 : _collective2_min.set(AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN);
247 0 : _collective2_max.set(AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX);
248 : }
249 :
250 0 : _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
251 :
252 0 : _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
253 :
254 0 : if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
255 : // calculate collective zero thrust point as a number from 0 to 1
256 0 : _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
257 :
258 : // calculate collective land min point as a number from 0 to 1
259 0 : _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
260 : } else {
261 0 : _collective_zero_thrust_pct = 0.0f;
262 0 : _collective_land_min_pct = 0.0f;
263 : }
264 :
265 0 : _collective2_zero_thrst_pct = _collective_zero_thrust_pct;
266 :
267 : // configure swashplate 1 and update scalars
268 0 : _swashplate1.configure();
269 :
270 : // configure swashplate 2 and update scalars
271 0 : _swashplate2.configure();
272 :
273 : // configure main rotor and update scalars
274 0 : _main_rotor.configure();
275 :
276 : // calculate armed scalars for both main rotors
277 0 : calculate_armed_scalars();
278 :
279 0 : }
280 :
281 : // Mix and output swashplates for tandem
282 0 : void AP_MotorsHeli_Dual::mix_tandem(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input)
283 : {
284 : // Differential cyclic roll is used for yaw and combined for roll
285 0 : const float swash1_roll = roll_input + _yaw_scaler * yaw_input;
286 0 : const float swash2_roll = roll_input - _yaw_scaler * yaw_input;
287 :
288 : // cyclic is not used for pitch control
289 0 : const float swash_pitch = 0.0;
290 :
291 : // Differential collective for pitch and combined for thrust
292 0 : const float swash1_coll = 0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input;
293 0 : const float swash2_coll = -0.45 * _dcp_scaler * (pitch_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
294 :
295 : // Calculate servo positions in swashplate library
296 0 : _swashplate1.calculate(swash1_roll, swash_pitch, swash1_coll);
297 0 : _swashplate2.calculate(swash2_roll, swash_pitch, swash2_coll);
298 0 : }
299 :
300 : // Mix and output swashplates for transverse
301 0 : void AP_MotorsHeli_Dual::mix_transverse(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input)
302 : {
303 : // cyclic is not used for roll control
304 0 : const float swash_roll = 0.0;
305 :
306 : // Differential cyclic pitch is used for yaw
307 0 : const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input;
308 0 : const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input;
309 :
310 : // Differential collective for roll and combined for thrust
311 0 : const float swash1_coll = 0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective1_input;
312 0 : const float swash2_coll = -0.45 * _dcp_scaler * (roll_input + constrain_float(_dcp_trim, -0.2, 0.2)) + collective2_input;
313 :
314 : // Calculate servo positions in swashplate library
315 0 : _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
316 0 : _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
317 0 : }
318 :
319 : // Mix and output swashplates for intermeshing
320 0 : void AP_MotorsHeli_Dual::mix_intermeshing(float pitch_input, float roll_input, float yaw_input, float collective1_input, float collective2_input)
321 : {
322 : // Direct roll control on both swash plates
323 0 : const float swash_roll = roll_input;
324 :
325 : // Differential cyclic pitch is used for yaw and combined for pitch
326 0 : const float swash1_pitch = pitch_input - _yaw_scaler * yaw_input;
327 0 : const float swash2_pitch = pitch_input + _yaw_scaler * yaw_input;
328 :
329 : // Differential collective for yaw and combined for thrust
330 0 : const float swash1_coll = 0.45 * _dcp_scaler * yaw_input + collective1_input;
331 0 : const float swash2_coll = -0.45 * _dcp_scaler * yaw_input + collective2_input;
332 :
333 : // Calculate servo positions in swashplate library
334 0 : _swashplate1.calculate(swash_roll, swash1_pitch, swash1_coll);
335 0 : _swashplate2.calculate(swash_roll, swash2_pitch, swash2_coll);
336 0 : }
337 :
338 : // update_motor_controls - sends commands to motor controllers
339 0 : void AP_MotorsHeli_Dual::update_motor_control(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
340 : {
341 : // Send state update to motors
342 0 : _main_rotor.update(_dt_s);
343 :
344 0 : if (state == AP_MotorsHeli_RSC::DesiredRSCSpoolState::SHUT_DOWN) {
345 : // set engine run enable aux output to not run position to kill engine when disarmed
346 0 : SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN);
347 : } else {
348 : // else if armed, set engine run enable output to run position
349 0 : SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MAX);
350 : }
351 0 : }
352 :
353 : // update_spool_state - updates the spool state based on the desired state
354 0 : AP_Motors::SpoolState AP_MotorsHeli_Dual::update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
355 : {
356 :
357 0 : SpoolState main_rotor_state = _main_rotor.update_spool_state(state, _dt_s);
358 :
359 : // Check if main rotor is run-up complete. Tail rotor run-up is not included in check because currently
360 : // the tail rotor in DDVP uses the same ramp and runup time as the main rotor. This may need changed if
361 : // the RSC is used for DDFP tail rotors.
362 0 : set_rotor_runup_complete(main_rotor_state == AP_MotorsHeli_RSC::RSCSpoolState::THROTTLE_UNLIMITED);
363 :
364 0 : return main_rotor_state;
365 :
366 : }
367 :
368 : //
369 : // move_actuators - moves swash plate to attitude of parameters passed in
370 : // - expected ranges:
371 : // roll : -1 ~ +1
372 : // pitch: -1 ~ +1
373 : // collective: 0 ~ 1
374 : // yaw: -1 ~ +1
375 : //
376 0 : void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
377 : {
378 0 : if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE || _dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING) {
379 0 : if (pitch_out < -_cyclic_max/4500.0f) {
380 0 : pitch_out = -_cyclic_max/4500.0f;
381 0 : limit.pitch = true;
382 : }
383 :
384 0 : if (pitch_out > _cyclic_max/4500.0f) {
385 0 : pitch_out = _cyclic_max/4500.0f;
386 0 : limit.pitch = true;
387 : }
388 : }
389 0 : if (_dual_mode != AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
390 0 : if (roll_out < -_cyclic_max/4500.0f) {
391 0 : roll_out = -_cyclic_max/4500.0f;
392 0 : limit.roll = true;
393 : }
394 :
395 0 : if (roll_out > _cyclic_max/4500.0f) {
396 0 : roll_out = _cyclic_max/4500.0f;
397 0 : limit.roll = true;
398 : }
399 : }
400 :
401 : // constrain collective input
402 0 : float collective_out = collective_in;
403 0 : if (collective_out <= 0.0f) {
404 0 : collective_out = 0.0f;
405 0 : limit.throttle_lower = true;
406 : }
407 0 : if (collective_out >= 1.0f) {
408 0 : collective_out = 1.0f;
409 0 : limit.throttle_upper = true;
410 : }
411 :
412 : // ensure not below landed/landing collective
413 0 : if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
414 0 : collective_out = _collective_land_min_pct;
415 0 : limit.throttle_lower = true;
416 : }
417 :
418 : // updates below land min collective flag
419 0 : _heliflags.below_land_min_coll = !is_positive(collective_out - _collective_land_min_pct);
420 :
421 : // updates takeoff collective flag based on 50% hover collective
422 0 : update_takeoff_collective_flag(collective_out);
423 :
424 : // Set rear collective to midpoint if required
425 0 : float collective2_out = collective_out;
426 0 : if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
427 0 : collective2_out = _collective2_zero_thrst_pct;
428 : }
429 :
430 : // if servo output not in manual mode, process pre-compensation factors
431 0 : if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
432 : // add differential collective pitch yaw compensation
433 0 : float yaw_compensation = 0.0f;
434 :
435 0 : if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_INTERMESHING) {
436 : // for intermeshing, reverse yaw in negative collective region and smoothen transition near zero collective
437 0 : if (_yaw_rev_expo > 0.01f) {
438 : // yaw_compensation range: (-1,1) S-shaped curve (Logistic Model) 1/(1 + e^kt)
439 0 : yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_zero_thrust_pct))));
440 0 : yaw_out = yaw_out * yaw_compensation;
441 : }
442 : } else {
443 0 : if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
444 0 : yaw_compensation = _dcp_yaw_effect * roll_out;
445 : } else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM
446 0 : yaw_compensation = _dcp_yaw_effect * pitch_out;
447 : }
448 0 : yaw_out = yaw_out + yaw_compensation;
449 : }
450 : }
451 :
452 : // scale yaw and update limits
453 0 : if (yaw_out < -_cyclic_max/4500.0f) {
454 0 : yaw_out = -_cyclic_max/4500.0f;
455 0 : limit.yaw = true;
456 : }
457 0 : if (yaw_out > _cyclic_max/4500.0f) {
458 0 : yaw_out = _cyclic_max/4500.0f;
459 0 : limit.yaw = true;
460 : }
461 :
462 : // scale collective pitch for front swashplate (servos 1,2,3)
463 0 : float collective_scaler = ((float)(_collective_max-_collective_min))*0.001f;
464 0 : float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)*0.001f;
465 :
466 : // scale collective pitch for rear swashplate (servos 4,5,6)
467 0 : float collective2_scaler = ((float)(_collective2_max-_collective2_min))*0.001f;
468 0 : float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)*0.001f;
469 :
470 : // feed power estimate into main rotor controller
471 : // ToDo: add main rotor cyclic power?
472 0 : _main_rotor.set_collective(fabsf(collective_out));
473 :
474 : // Mix swash plate
475 0 : switch (_dual_mode) {
476 0 : case AP_MOTORS_HELI_DUAL_MODE_TANDEM:
477 : default:
478 0 : mix_tandem(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled);
479 0 : break;
480 :
481 0 : case AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE:
482 0 : mix_transverse(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled);
483 0 : break;
484 :
485 0 : case AP_MOTORS_HELI_DUAL_MODE_INTERMESHING:
486 0 : mix_intermeshing(pitch_out, roll_out, yaw_out, collective_out_scaled, collective2_out_scaled);
487 0 : break;
488 :
489 : }
490 :
491 0 : }
492 :
493 0 : void AP_MotorsHeli_Dual::output_to_motors()
494 : {
495 0 : if (!initialised_ok()) {
496 0 : return;
497 : }
498 :
499 : // Write swashplate outputs
500 0 : _swashplate1.output();
501 0 : _swashplate2.output();
502 :
503 0 : _main_rotor.output_to_servo();
504 :
505 : }
506 :
507 : // servo_test - move servos through full range of movement
508 0 : void AP_MotorsHeli_Dual::servo_test()
509 : {
510 : // this test cycle is equivalent to that of AP_MotorsHeli_Single, but excluding
511 : // mixing of yaw, as that physical movement is represented by pitch and roll
512 :
513 0 : _servo_test_cycle_time += _dt_s;
514 :
515 0 : if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back
516 0 : (_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){
517 0 : _pitch_test += 2.0 * _dt_s;
518 0 : _oscillate_angle += 8 * M_PI * _dt_s;
519 0 : } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around
520 0 : (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){
521 0 : _oscillate_angle += 0.5 * M_PI * _dt_s;
522 0 : _roll_test = sinf(_oscillate_angle);
523 0 : _pitch_test = cosf(_oscillate_angle);
524 0 : } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level
525 0 : (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){
526 0 : _pitch_test -= 2.0 * _dt_s;
527 0 : _oscillate_angle += 8 * M_PI * _dt_s;
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 : } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom
532 0 : _collective_test -= _dt_s;
533 0 : _oscillate_angle += 2 * M_PI * _dt_s;
534 : } else { // reset cycle
535 0 : _servo_test_cycle_time = 0.0f;
536 0 : _oscillate_angle = 0.0f;
537 0 : _collective_test = 0.0f;
538 0 : _roll_test = 0.0f;
539 0 : _pitch_test = 0.0f;
540 : // decrement servo test cycle counter at the end of the cycle
541 0 : if (_servo_test_cycle_counter > 0){
542 0 : _servo_test_cycle_counter--;
543 : }
544 : }
545 :
546 : // over-ride servo commands to move servos through defined ranges
547 :
548 0 : _throttle_filter.reset(constrain_float(_collective_test, 0.0f, 1.0f));
549 0 : _roll_in = constrain_float(_roll_test, -1.0f, 1.0f);
550 0 : _pitch_in = constrain_float(_pitch_test, -1.0f, 1.0f);
551 0 : }
552 :
553 : // Run arming checks
554 0 : bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
555 : {
556 : // run base class checks
557 0 : if (!AP_MotorsHeli::arming_checks(buflen, buffer)) {
558 0 : return false;
559 : }
560 :
561 : // returns false if Phase Angle is outside of range for H3 swashplate 1
562 0 : if (_swashplate1.get_phase_angle() > 30 || _swashplate1.get_phase_angle() < -30){
563 0 : hal.util->snprintf(buffer, buflen, "H_SW_PHANG out of range");
564 0 : return false;
565 : }
566 :
567 : // returns false if Phase Angle is outside of range for H3 swashplate 2
568 0 : if (_swashplate2.get_phase_angle() > 30 || _swashplate2.get_phase_angle() < -30){
569 0 : hal.util->snprintf(buffer, buflen, "H_SW2_PHANG out of range");
570 0 : return false;
571 : }
572 :
573 0 : return true;
574 : }
575 :
576 : // Helper function for param conversions which are easier to be done in the motors class
577 : // Called from system.cpp
578 0 : void AP_MotorsHeli_Dual::heli_motors_param_conversions(void)
579 : {
580 : // Run common conversions from base class
581 0 : AP_MotorsHeli::heli_motors_param_conversions();
582 :
583 0 : if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H3) {
584 : // PARAMETER_CONVERSION - Added: Mar-2026
585 : // Previous versions had phase angle for only the generic H3 swashplate. Phase angle is now available for all swashplate
586 : // types but needs to be converted for users of the generic H3 type. Convert old H3 phase angle parameter to new parameter
587 : // applicable to all swashplate types.
588 0 : const AP_Param::ConversionInfo sw1_phase_conversion_info[] = {
589 : { 90, 532, AP_PARAM_INT16, "H_SW_PHANG" },
590 : };
591 0 : uint8_t table_size = ARRAY_SIZE(sw1_phase_conversion_info);
592 0 : for (uint8_t i=0; i<table_size; i++) {
593 0 : AP_Param::convert_old_parameter(&sw1_phase_conversion_info[i], 1.0);
594 : }
595 : }
596 0 : if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H3) {
597 : // PARAMETER_CONVERSION - Added: Mar-2026
598 : // Previous versions had phase angle for only the generic H3 swashplate. Phase angle is now available for all swashplate
599 : // types but needs to be converted for users of the generic H3 type. Convert old H3 phase angle parameter to new parameter
600 : // applicable to all swashplate types.
601 0 : const AP_Param::ConversionInfo sw2_phase_conversion_info[] = {
602 : { 90, 533, AP_PARAM_INT16, "H_SW2_PHANG" },
603 : };
604 0 : uint8_t table_size = ARRAY_SIZE(sw2_phase_conversion_info);
605 0 : for (uint8_t i=0; i<table_size; i++) {
606 0 : AP_Param::convert_old_parameter(&sw2_phase_conversion_info[i], 1.0);
607 : }
608 : }
609 :
610 0 : }
611 :
612 : #if HAL_LOGGING_ENABLED
613 : // heli motors logging - called at 10 Hz
614 0 : void AP_MotorsHeli_Dual::Log_Write(void)
615 : {
616 : // write swashplate log
617 0 : _swashplate1.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
618 0 : _swashplate2.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective2_min.get(), _collective2_max.get());
619 :
620 : // write RSC log
621 0 : _main_rotor.write_log();
622 0 : }
623 : #endif
|
