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 <AP_HAL/AP_HAL.h>
17 : #include "AP_MotorsMatrix.h"
18 : #include <AP_Vehicle/AP_Vehicle_Type.h>
19 :
20 : extern const AP_HAL::HAL& hal;
21 :
22 : // init
23 1194 : void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
24 : {
25 : // record requested frame class and type
26 1194 : _active_frame_class = frame_class;
27 1194 : _active_frame_type = frame_type;
28 :
29 1194 : if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
30 : // if Scripting frame class, do nothing scripting must call its own dedicated init function
31 9 : return;
32 : }
33 :
34 : // setup the motors
35 1185 : setup_motors(frame_class, frame_type);
36 :
37 : // enable fast channels or instant pwm
38 1185 : set_update_rate(_speed_hz);
39 : }
40 :
41 : #if AP_SCRIPTING_ENABLED
42 : // dedicated init for lua scripting
43 3 : bool AP_MotorsMatrix::init(uint8_t expected_num_motors)
44 : {
45 3 : if (_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) {
46 : // not the correct class
47 0 : return false;
48 : }
49 :
50 : // Make sure the correct number of motors have been added
51 3 : uint8_t num_motors = 0;
52 99 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
53 96 : if (motor_enabled[i]) {
54 64 : num_motors++;
55 : }
56 : }
57 :
58 3 : set_initialised_ok(expected_num_motors == num_motors);
59 :
60 3 : if (!initialised_ok()) {
61 0 : _mav_type = MAV_TYPE_GENERIC;
62 0 : return false;
63 : }
64 :
65 3 : switch (num_motors) {
66 0 : case 3:
67 0 : _mav_type = MAV_TYPE_TRICOPTER;
68 0 : break;
69 0 : case 4:
70 0 : _mav_type = MAV_TYPE_QUADROTOR;
71 0 : break;
72 0 : case 6:
73 0 : _mav_type = MAV_TYPE_HEXAROTOR;
74 0 : break;
75 0 : case 8:
76 0 : _mav_type = MAV_TYPE_OCTOROTOR;
77 0 : break;
78 0 : case 10:
79 0 : _mav_type = MAV_TYPE_DECAROTOR;
80 0 : break;
81 0 : case 12:
82 0 : _mav_type = MAV_TYPE_DODECAROTOR;
83 0 : break;
84 3 : default:
85 3 : _mav_type = MAV_TYPE_GENERIC;
86 : }
87 :
88 3 : normalise_rpy_factors();
89 :
90 3 : set_update_rate(_speed_hz);
91 :
92 3 : return true;
93 : }
94 :
95 : // Set throttle factor from scripting
96 0 : bool AP_MotorsMatrix::set_throttle_factor(int8_t motor_num, float throttle_factor)
97 : {
98 0 : if ((_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) ) {
99 : // not the correct class
100 0 : return false;
101 : }
102 :
103 0 : if (initialised_ok() || !motor_enabled[motor_num]) {
104 : // Already setup or given motor is not enabled
105 0 : return false;
106 : }
107 :
108 0 : _throttle_factor[motor_num] = throttle_factor;
109 0 : return true;
110 : }
111 :
112 : #endif // AP_SCRIPTING_ENABLED
113 :
114 : // set update rate to motors - a value in hertz
115 2377 : void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
116 : {
117 : // record requested speed
118 2377 : _speed_hz = speed_hz;
119 :
120 2377 : uint32_t mask = 0;
121 78441 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
122 76064 : if (motor_enabled[i]) {
123 9684 : mask |= 1U << i;
124 : }
125 : }
126 2377 : rc_set_freq(mask, _speed_hz);
127 2377 : }
128 :
129 : // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
130 23528 : void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
131 : {
132 : // exit immediately if armed or no change
133 23528 : if (armed() || (frame_class == _active_frame_class && _active_frame_type == frame_type)) {
134 23523 : return;
135 : }
136 5 : _active_frame_class = frame_class;
137 5 : _active_frame_type = frame_type;
138 :
139 5 : init(frame_class, frame_type);
140 :
141 : }
142 :
143 33291922 : void AP_MotorsMatrix::output_to_motors()
144 : {
145 : int8_t i;
146 :
147 33291922 : switch (_spool_state) {
148 13702802 : case SpoolState::SHUT_DOWN: {
149 : // no output
150 452192466 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
151 438489664 : if (motor_enabled_mask(i)) {
152 57035538 : _actuator[i] = 0.0f;
153 : }
154 : }
155 13702802 : break;
156 : }
157 251034 : case SpoolState::GROUND_IDLE:
158 : // sends output to motors when armed but not flying
159 8284122 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
160 8033088 : if (motor_enabled[i]) {
161 1033138 : set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
162 : }
163 : }
164 251034 : break;
165 19338086 : case SpoolState::SPOOLING_UP:
166 : case SpoolState::THROTTLE_UNLIMITED:
167 : case SpoolState::SPOOLING_DOWN:
168 : // set motor output based on thrust requests
169 638156838 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
170 618818752 : if (motor_enabled[i]) {
171 81723534 : set_actuator_with_slew(_actuator[i], thr_lin.thrust_to_actuator(_thrust_rpyt_out[i]));
172 : }
173 : }
174 19338086 : break;
175 : }
176 :
177 : // convert output to PWM and send to each motor
178 1098633426 : for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
179 1065341504 : if (motor_enabled[i]) {
180 140179240 : rc_write(i, output_to_pwm(_actuator[i]));
181 : }
182 : }
183 33291922 : }
184 :
185 : // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
186 : // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
187 32788 : uint32_t AP_MotorsMatrix::get_motor_mask()
188 : {
189 32788 : uint32_t motor_mask = 0;
190 1082004 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
191 1049216 : if (motor_enabled[i]) {
192 142274 : motor_mask |= 1U << i;
193 : }
194 : }
195 32788 : uint32_t mask = motor_mask_to_srv_channel_mask(motor_mask);
196 :
197 : // add parent's mask
198 32788 : mask |= AP_MotorsMulticopter::get_motor_mask();
199 :
200 32788 : return mask;
201 : }
202 :
203 : // helper to return value scaled between boost and normal based on the value of _thrust_boost_ratio
204 : // _thrust_boost_ratio of 1 -> return = boost_value
205 : // _thrust_boost_ratio of 0 -> return = normal_value
206 66596701 : float AP_MotorsMatrix::boost_ratio(float boost_value, float normal_value) const
207 : {
208 66596701 : return _thrust_boost_ratio * boost_value + (1.0 - _thrust_boost_ratio) * normal_value;
209 : }
210 :
211 : // output_armed - sends commands to the motors
212 : // includes new scaling stability patch
213 33291922 : void AP_MotorsMatrix::output_armed_stabilizing()
214 : {
215 : // apply voltage and air pressure compensation
216 33291922 : const float compensation_gain = thr_lin.get_compensation_gain(); // compensation for battery voltage and altitude
217 :
218 : // pitch thrust input value, +/- 1.0
219 33291922 : const float roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
220 :
221 : // pitch thrust input value, +/- 1.0
222 33291922 : const float pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
223 :
224 : // yaw thrust input value, +/- 1.0
225 33291922 : float yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
226 :
227 : // throttle thrust input value, 0.0 - 1.0
228 33291922 : float throttle_thrust = get_throttle() * compensation_gain;
229 :
230 : // throttle thrust average maximum value, 0.0 - 1.0
231 33291922 : float throttle_avg_max = _throttle_avg_max * compensation_gain;
232 :
233 : // throttle thrust maximum value, 0.0 - 1.0, If thrust boost is active then do not limit maximum thrust
234 33291922 : const float throttle_thrust_max = boost_ratio(1.0, _throttle_thrust_max * compensation_gain);
235 :
236 : // sanity check throttle is above zero and below current limited throttle
237 33291922 : if (throttle_thrust <= 0.0f) {
238 14512910 : throttle_thrust = 0.0f;
239 14512910 : limit.throttle_lower = true;
240 : }
241 33291922 : if (throttle_thrust >= throttle_thrust_max) {
242 14007035 : throttle_thrust = throttle_thrust_max;
243 14007035 : limit.throttle_upper = true;
244 : }
245 :
246 : // ensure that throttle_avg_max is between the input throttle and the maximum throttle
247 33291922 : throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);
248 :
249 : // throttle providing maximum roll, pitch and yaw range
250 : // calculate the highest allowed average thrust that will provide maximum control range
251 33291922 : float throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
252 :
253 : // calculate throttle that gives most possible room for yaw which is the lower of:
254 : // 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
255 : // 2. the higher of:
256 : // a) the pilot's throttle input
257 : // b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
258 : // Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
259 : // Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
260 : // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
261 : // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
262 :
263 : // Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
264 : // To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
265 : // Y6 : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
266 : // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
267 : // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75, ATC_RAT_PIT_IMAX = 0.75, ATC_RAT_YAW_IMAX = 0.375
268 : // Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
269 : // Hex : MOT_YAW_HEADROOM = 0, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
270 : // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25, ATC_RAT_PIT_IMAX = 0.25, ATC_RAT_YAW_IMAX = 0.25
271 : // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5, ATC_RAT_PIT_IMAX = 0.5, ATC_RAT_YAW_IMAX = 0.25
272 : // Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.
273 :
274 : // calculate amount of yaw we can fit into the throttle range
275 : // this is always equal to or less than the requested yaw from the pilot or rate controller
276 33291922 : float yaw_allowed = 1.0f; // amount of yaw we can fit in
277 1098633426 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
278 1065341504 : if (motor_enabled[i]) {
279 : // calculate the thrust outputs for roll and pitch
280 140179240 : _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
281 :
282 : // Check the maximum yaw control that can be used on this channel
283 : // Exclude any lost motors if thrust boost is enabled
284 140179240 : if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)) {
285 139252793 : const float thrust_rp_best_throttle = throttle_thrust_best_rpy + _thrust_rpyt_out[i];
286 : float motor_room;
287 139252793 : if (is_positive(yaw_thrust * _yaw_factor[i])) {
288 : // room to upper limit
289 62372677 : motor_room = 1.0 - thrust_rp_best_throttle;
290 : } else {
291 : // room to lower limit
292 76880116 : motor_room = thrust_rp_best_throttle;
293 : }
294 139252793 : const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(_yaw_factor[i]);
295 139252793 : yaw_allowed = MIN(yaw_allowed, motor_yaw_allowed);
296 : }
297 : }
298 : }
299 :
300 : // calculate the maximum yaw control that can be used
301 : // todo: make _yaw_headroom 0 to 1
302 33291922 : float yaw_allowed_min = (float)_yaw_headroom * 0.001f;
303 :
304 : // increase yaw headroom to 50% if thrust boost enabled
305 33291922 : yaw_allowed_min = boost_ratio(0.5, yaw_allowed_min);
306 :
307 : // Let yaw access minimum amount of head room
308 33291922 : yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);
309 :
310 : // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
311 33291922 : if (_thrust_boost && motor_enabled[_motor_lost_index]) {
312 : // Check the maximum yaw control that can be used on this channel
313 : // Exclude any lost motors if thrust boost is enabled
314 10549 : if (!is_zero(_yaw_factor[_motor_lost_index])){
315 7843 : const float thrust_rp_best_throttle = throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index];
316 : float motor_room;
317 7843 : if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index])) {
318 5929 : motor_room = 1.0 - thrust_rp_best_throttle;
319 : } else {
320 1914 : motor_room = thrust_rp_best_throttle;
321 : }
322 7843 : const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(_yaw_factor[_motor_lost_index]);
323 7843 : yaw_allowed = boost_ratio(yaw_allowed, MIN(yaw_allowed, motor_yaw_allowed));
324 : }
325 : }
326 :
327 33291922 : if (fabsf(yaw_thrust) > yaw_allowed) {
328 : // not all commanded yaw can be used
329 38523 : yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
330 38523 : limit.yaw = true;
331 : }
332 :
333 : // add yaw control to thrust outputs
334 33291922 : float rpy_low = 1.0f; // lowest thrust value
335 33291922 : float rpy_high = -1.0f; // highest thrust value
336 1098633426 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
337 1065341504 : if (motor_enabled[i]) {
338 140179240 : _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
339 :
340 : // record lowest roll + pitch + yaw command
341 140179240 : if (_thrust_rpyt_out[i] < rpy_low) {
342 65748390 : rpy_low = _thrust_rpyt_out[i];
343 : }
344 : // record highest roll + pitch + yaw command
345 : // Exclude any lost motors if thrust boost is enabled
346 140179240 : if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
347 71606578 : rpy_high = _thrust_rpyt_out[i];
348 : }
349 : }
350 : }
351 : // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
352 33291922 : if (_thrust_boost) {
353 : // record highest roll + pitch + yaw command
354 10549 : if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
355 5014 : rpy_high = boost_ratio(rpy_high, _thrust_rpyt_out[_motor_lost_index]);
356 : }
357 : }
358 :
359 : // calculate any scaling needed to make the combined thrust outputs fit within the output range
360 33291922 : float rpy_scale = 1.0f;
361 33291922 : if (rpy_high - rpy_low > 1.0f) {
362 342051 : rpy_scale = 1.0f / (rpy_high - rpy_low);
363 : }
364 33291922 : if (throttle_avg_max + rpy_low < 0) {
365 10431473 : rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
366 : }
367 :
368 : // calculate how close the motors can come to the desired throttle
369 33291922 : rpy_high *= rpy_scale;
370 33291922 : rpy_low *= rpy_scale;
371 33291922 : throttle_thrust_best_rpy = -rpy_low;
372 33291922 : float thr_adj = throttle_thrust - throttle_thrust_best_rpy;
373 33291922 : if (rpy_scale < 1.0f) {
374 : // Full range is being used by roll, pitch, and yaw.
375 10436918 : limit.set_rpy(true);
376 10436918 : if (thr_adj > 0.0f) {
377 11905 : limit.throttle_upper = true;
378 : }
379 10436918 : thr_adj = 0.0f;
380 22855004 : } else if (thr_adj < 0.0f) {
381 : // Throttle can't be reduced to desired value
382 : // todo: add lower limit flag and ensure it is handled correctly in altitude controller
383 601854 : thr_adj = 0.0f;
384 22253150 : } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
385 : // Throttle can't be increased to desired value
386 93747 : thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
387 93747 : limit.throttle_upper = true;
388 : }
389 :
390 : // add scaled roll, pitch, constrained yaw and throttle for each motor
391 33291922 : const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
392 1098633426 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
393 1065341504 : if (motor_enabled[i]) {
394 140179240 : _thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
395 : }
396 : }
397 :
398 : // determine throttle thrust for harmonic notch
399 : // compensation_gain can never be zero
400 33291922 : _throttle_out = throttle_thrust_best_plus_adj / compensation_gain;
401 :
402 : // check for failed motor
403 33291922 : check_for_failed_motor(throttle_thrust_best_plus_adj);
404 33291922 : }
405 :
406 : // check for failed motor
407 : // should be run immediately after output_armed_stabilizing
408 : // first argument is the sum of:
409 : // a) throttle_thrust_best_rpy : throttle level (from 0 to 1) providing maximum roll, pitch and yaw range without climbing
410 : // b) thr_adj: the difference between the pilot's desired throttle and throttle_thrust_best_rpy
411 : // records filtered motor output values in _thrust_rpyt_out_filt array
412 : // sets thrust_balanced to true if motors are balanced, false if a motor failure is detected
413 : // sets _motor_lost_index to index of failed motor
414 33291922 : void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj)
415 : {
416 : // record filtered and scaled thrust output for motor loss monitoring purposes
417 33291922 : float alpha = _dt_s / (_dt_s + 0.5f);
418 1098633426 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
419 1065341504 : if (motor_enabled[i]) {
420 140179240 : _thrust_rpyt_out_filt[i] += alpha * (_thrust_rpyt_out[i] - _thrust_rpyt_out_filt[i]);
421 : }
422 : }
423 :
424 33291922 : float rpyt_high = 0.0f;
425 33291922 : float rpyt_sum = 0.0f;
426 33291922 : uint8_t number_motors = 0.0f;
427 1098633426 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
428 1065341504 : if (motor_enabled[i]) {
429 140179240 : number_motors += 1;
430 140179240 : rpyt_sum += _thrust_rpyt_out_filt[i];
431 : // record highest filtered thrust command
432 140179240 : if (_thrust_rpyt_out_filt[i] > rpyt_high) {
433 48406060 : rpyt_high = _thrust_rpyt_out_filt[i];
434 : // hold motor lost index constant while thrust boost is active
435 48406060 : if (!_thrust_boost) {
436 48388029 : _motor_lost_index = i;
437 : }
438 : }
439 : }
440 : }
441 :
442 33291922 : float thrust_balance = 1.0f;
443 33291922 : if (rpyt_sum > 0.1f) {
444 18997431 : thrust_balance = rpyt_high * number_motors / rpyt_sum;
445 : }
446 : // ensure thrust balance does not activate for multirotors with less than 6 motors
447 : // also disable for co-rotating frames which have intentionally different throttle factors
448 66535114 : const bool is_corotating = _active_frame_type == MOTOR_FRAME_TYPE_X_COR ||
449 33243192 : _active_frame_type == MOTOR_FRAME_TYPE_CW_X_COR;
450 33291922 : if (number_motors >= 6 && thrust_balance >= 1.5f && _thrust_balanced && !is_corotating) {
451 52 : _thrust_balanced = false;
452 : }
453 33291922 : if (thrust_balance <= 1.25f && !_thrust_balanced) {
454 52 : _thrust_balanced = true;
455 : }
456 :
457 : // check to see if thrust boost is using more throttle than _throttle_thrust_max
458 33291922 : if ((_throttle_thrust_max * thr_lin.get_compensation_gain() > throttle_thrust_best_plus_adj) && (rpyt_high < 0.9f) && _thrust_balanced) {
459 19178344 : _thrust_boost = false;
460 : }
461 33291922 : }
462 :
463 : // output_test_seq - spin a motor at the pwm value specified
464 : // motor_seq is the motor's sequence number from 1 to the number of motors on the frame
465 : // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
466 11185 : void AP_MotorsMatrix::_output_test_seq(uint8_t motor_seq, int16_t pwm)
467 : {
468 : // loop through all the possible orders spinning any motors that match that description
469 369105 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
470 357920 : if (motor_enabled[i] && _test_order[i] == motor_seq) {
471 : // turn on this motor
472 11185 : rc_write(i, pwm);
473 : }
474 : }
475 11185 : }
476 :
477 : // output_test_num - spin a motor connected to the specified output channel
478 : // (should only be performed during testing)
479 : // If a motor output channel is remapped, the mapped channel is used.
480 : // Returns true if motor output is set, false otherwise
481 : // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
482 0 : bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
483 : {
484 0 : if (!armed()) {
485 0 : return false;
486 : }
487 :
488 : // Is channel in supported range?
489 0 : if (output_channel > AP_MOTORS_MAX_NUM_MOTORS - 1) {
490 0 : return false;
491 : }
492 :
493 : // Is motor enabled?
494 0 : if (!motor_enabled[output_channel]) {
495 0 : return false;
496 : }
497 :
498 0 : rc_write(output_channel, pwm); // output
499 0 : return true;
500 : }
501 :
502 : // add_motor
503 5126 : void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
504 : {
505 5126 : if (initialised_ok()) {
506 : // do not allow motors to be set if the current frame type has init correctly
507 0 : return;
508 : }
509 :
510 : // ensure valid motor number is provided
511 5126 : if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
512 :
513 : // enable motor
514 5126 : motor_enabled[motor_num] = true;
515 :
516 : // set roll, pitch, yaw and throttle factors
517 5126 : _roll_factor[motor_num] = roll_fac;
518 5126 : _pitch_factor[motor_num] = pitch_fac;
519 5126 : _yaw_factor[motor_num] = yaw_fac;
520 5126 : _throttle_factor[motor_num] = throttle_factor;
521 :
522 : // set order that motor appears in test
523 5126 : _test_order[motor_num] = testing_order;
524 :
525 : // call parent class method
526 5126 : add_motor_num(motor_num);
527 : }
528 : }
529 :
530 : // add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
531 4544 : void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
532 : {
533 4544 : add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
534 4544 : }
535 :
536 : // add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
537 4544 : void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
538 : {
539 4544 : add_motor_raw(
540 : motor_num,
541 : cosf(radians(roll_factor_in_degrees + 90)),
542 : cosf(radians(pitch_factor_in_degrees)),
543 : yaw_factor,
544 : testing_order);
545 4544 : }
546 :
547 : // remove_motor - disabled motor and clears all roll, pitch, throttle factors for this motor
548 37856 : void AP_MotorsMatrix::remove_motor(int8_t motor_num)
549 : {
550 : // ensure valid motor number is provided
551 37856 : if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
552 : // disable the motor, set all factors to zero
553 37856 : motor_enabled[motor_num] = false;
554 37856 : _roll_factor[motor_num] = 0.0f;
555 37856 : _pitch_factor[motor_num] = 0.0f;
556 37856 : _yaw_factor[motor_num] = 0.0f;
557 37856 : _throttle_factor[motor_num] = 0.0f;
558 : }
559 37856 : }
560 :
561 1093 : void AP_MotorsMatrix::add_motors(const struct MotorDef *motors, uint8_t num_motors)
562 : {
563 5637 : for (uint8_t i=0; i<num_motors; i++) {
564 4544 : const auto &motor = motors[i];
565 4544 : add_motor(i, motor.angle_degrees, motor.yaw_factor, motor.testing_order);
566 : }
567 1093 : }
568 3 : void AP_MotorsMatrix::add_motors_raw(const struct MotorDefRaw *motors, uint8_t num_motors)
569 : {
570 21 : for (uint8_t i=0; i<num_motors; i++) {
571 18 : const auto &m = motors[i];
572 18 : add_motor_raw(i, m.roll_fac, m.pitch_fac, m.yaw_fac, m.testing_order);
573 : }
574 3 : }
575 : #if AP_MOTORS_FRAME_QUAD_ENABLED
576 1050 : bool AP_MotorsMatrix::setup_quad_matrix(motor_frame_type frame_type)
577 : {
578 1050 : _frame_class_string = "QUAD";
579 1050 : _mav_type = MAV_TYPE_QUADROTOR;
580 1050 : switch (frame_type) {
581 852 : case MOTOR_FRAME_TYPE_PLUS: {
582 852 : _frame_type_string = "PLUS";
583 : static const AP_MotorsMatrix::MotorDef motors[] {
584 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
585 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
586 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
587 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
588 : };
589 852 : add_motors(motors, ARRAY_SIZE(motors));
590 852 : break;
591 : }
592 192 : case MOTOR_FRAME_TYPE_X: {
593 192 : _frame_type_string = "X";
594 : static const AP_MotorsMatrix::MotorDef motors[] {
595 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
596 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
597 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
598 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
599 : };
600 192 : add_motors(motors, ARRAY_SIZE(motors));
601 192 : break;
602 : }
603 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
604 0 : case MOTOR_FRAME_TYPE_NYT_PLUS: {
605 0 : _frame_type_string = "NYT_PLUS";
606 : static const AP_MotorsMatrix::MotorDef motors[] {
607 : { 90, 0, 2 },
608 : { -90, 0, 4 },
609 : { 0, 0, 1 },
610 : { 180, 0, 3 },
611 : };
612 0 : add_motors(motors, ARRAY_SIZE(motors));
613 0 : break;
614 : }
615 0 : case MOTOR_FRAME_TYPE_NYT_X: {
616 0 : _frame_type_string = "NYT_X";
617 : static const AP_MotorsMatrix::MotorDef motors[] {
618 : { 45, 0, 1 },
619 : { -135, 0, 3 },
620 : { -45, 0, 4 },
621 : { 135, 0, 2 },
622 : };
623 0 : add_motors(motors, ARRAY_SIZE(motors));
624 0 : break;
625 : }
626 : #endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
627 1 : case MOTOR_FRAME_TYPE_BF_X: {
628 : // betaflight quad X order
629 : // see: https://fpvfrenzy.com/betaflight-motor-order/
630 1 : _frame_type_string = "BF_X";
631 : static const AP_MotorsMatrix::MotorDef motors[] {
632 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
633 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
634 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
635 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
636 : };
637 1 : add_motors(motors, ARRAY_SIZE(motors));
638 1 : break;
639 : }
640 0 : case MOTOR_FRAME_TYPE_BF_X_REV: {
641 : // betaflight quad X order, reversed motors
642 0 : _frame_type_string = "X_REV";
643 : static const AP_MotorsMatrix::MotorDef motors[] {
644 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
645 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
646 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
647 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
648 : };
649 0 : add_motors(motors, ARRAY_SIZE(motors));
650 0 : break;
651 : }
652 1 : case MOTOR_FRAME_TYPE_DJI_X: {
653 : // DJI quad X order
654 : // see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
655 1 : _frame_type_string = "DJI_X";
656 : static const AP_MotorsMatrix::MotorDef motors[] {
657 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
658 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
659 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
660 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
661 : };
662 1 : add_motors(motors, ARRAY_SIZE(motors));
663 1 : break;
664 : }
665 1 : case MOTOR_FRAME_TYPE_CW_X: {
666 : // "clockwise X" motor order. Motors are ordered clockwise from front right
667 : // matching test order
668 1 : _frame_type_string = "CW_X";
669 : static const AP_MotorsMatrix::MotorDef motors[] {
670 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
671 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
672 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
673 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
674 : };
675 1 : add_motors(motors, ARRAY_SIZE(motors));
676 1 : break;
677 : }
678 0 : case MOTOR_FRAME_TYPE_V: {
679 0 : _frame_type_string = "V";
680 : static const AP_MotorsMatrix::MotorDef motors[] {
681 : { 45, 0.7981f, 1 },
682 : { -135, 1.0000f, 3 },
683 : { -45, -0.7981f, 4 },
684 : { 135, -1.0000f, 2 },
685 : };
686 0 : add_motors(motors, ARRAY_SIZE(motors));
687 0 : break;
688 : }
689 3 : case MOTOR_FRAME_TYPE_H: {
690 : // H frame set-up - same as X but motors spin in opposite directions
691 3 : _frame_type_string = "H";
692 : static const AP_MotorsMatrix::MotorDef motors[] {
693 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
694 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
695 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
696 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
697 : };
698 3 : add_motors(motors, ARRAY_SIZE(motors));
699 3 : break;
700 : }
701 0 : case MOTOR_FRAME_TYPE_VTAIL: {
702 : /*
703 : Tested with: Lynxmotion Hunter Vtail 400
704 : - inverted rear outward blowing motors (at a 40 degree angle)
705 : - should also work with non-inverted rear outward blowing motors
706 : - no roll in rear motors
707 : - no yaw in front motors
708 : - should fly like some mix between a tricopter and X Quadcopter
709 :
710 : Roll control comes only from the front motors, Yaw control only from the rear motors.
711 : Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.
712 :
713 : Note: if we want the front motors to help with yaw,
714 : motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
715 : motors 3's yaw factor should be changed to -sin(radians(40))
716 : */
717 0 : _frame_type_string = "VTAIL";
718 0 : add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
719 0 : add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
720 0 : add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
721 0 : add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
722 0 : break;
723 : }
724 0 : case MOTOR_FRAME_TYPE_ATAIL:
725 : /*
726 : The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
727 : - The Yaw factors are reversed, because the rear motors are facing different directions
728 :
729 : With V-Shaped VTails, the props make a V-Shape when spinning, but with
730 : A-Shaped VTails, the props make an A-Shape when spinning.
731 : - Rear thrust on a V-Shaped V-Tail Quad is outward
732 : - Rear thrust on an A-Shaped V-Tail Quad is inward
733 :
734 : Still functions the same as the V-Shaped VTail mixing below:
735 : - Yaw control is entirely in the rear motors
736 : - Roll is is entirely in the front motors
737 : */
738 0 : _frame_type_string = "ATAIL";
739 0 : add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
740 0 : add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
741 0 : add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
742 0 : add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
743 0 : break;
744 0 : case MOTOR_FRAME_TYPE_PLUSREV: {
745 : // plus with reversed motor directions
746 0 : _frame_type_string = "PLUSREV";
747 : static const AP_MotorsMatrix::MotorDef motors[] {
748 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
749 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
750 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
751 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
752 : };
753 0 : add_motors(motors, ARRAY_SIZE(motors));
754 0 : break;
755 : }
756 0 : case MOTOR_FRAME_TYPE_Y4:
757 0 : _frame_type_string = "Y4";
758 : // Y4 motor definition with right front CCW, left front CW
759 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
760 : { -1.0f, 1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
761 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
762 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
763 : { 1.0f, 1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
764 : };
765 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
766 0 : break;
767 0 : default:
768 : // quad frame class does not support this frame type
769 0 : return false;
770 : }
771 1050 : return true;
772 : }
773 : #endif //AP_MOTORS_FRAME_QUAD_ENABLED
774 : #if AP_MOTORS_FRAME_HEXA_ENABLED
775 4 : bool AP_MotorsMatrix::setup_hexa_matrix(motor_frame_type frame_type)
776 : {
777 4 : _frame_class_string = "HEXA";
778 4 : _mav_type = MAV_TYPE_HEXAROTOR;
779 4 : switch (frame_type) {
780 1 : case MOTOR_FRAME_TYPE_PLUS: {
781 1 : _frame_type_string = "PLUS";
782 : static const AP_MotorsMatrix::MotorDef motors[] {
783 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
784 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
785 : { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
786 : { 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
787 : { -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
788 : { 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
789 : };
790 1 : add_motors(motors, ARRAY_SIZE(motors));
791 1 : break;
792 : }
793 1 : case MOTOR_FRAME_TYPE_X: {
794 1 : _frame_type_string = "X";
795 : static const AP_MotorsMatrix::MotorDef motors[] {
796 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
797 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
798 : { -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
799 : { 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
800 : { 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
801 : { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
802 : };
803 1 : add_motors(motors, ARRAY_SIZE(motors));
804 1 : break;
805 : }
806 0 : case MOTOR_FRAME_TYPE_H: {
807 : // H is same as X except middle motors are closer to center
808 0 : _frame_type_string = "H";
809 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
810 : { -1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
811 : { 1.0f, 0.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
812 : { 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
813 : { -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
814 : { -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
815 : { 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
816 : };
817 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
818 0 : break;
819 : }
820 1 : case MOTOR_FRAME_TYPE_DJI_X: {
821 1 : _frame_type_string = "DJI_X";
822 : static const AP_MotorsMatrix::MotorDef motors[] {
823 : { 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
824 : { -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
825 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
826 : { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
827 : { 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
828 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
829 : };
830 1 : add_motors(motors, ARRAY_SIZE(motors));
831 1 : break;
832 : }
833 1 : case MOTOR_FRAME_TYPE_CW_X: {
834 1 : _frame_type_string = "CW_X";
835 : static const AP_MotorsMatrix::MotorDef motors[] {
836 : { 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
837 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
838 : { 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
839 : { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
840 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
841 : { -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
842 : };
843 1 : add_motors(motors, ARRAY_SIZE(motors));
844 1 : break;
845 : }
846 0 : default:
847 : // hexa frame class does not support this frame type
848 0 : return false;
849 : } //hexa
850 4 : return true;
851 : }
852 : #endif ////AP_MOTORS_FRAME_HEXA_ENABLED
853 : #if AP_MOTORS_FRAME_OCTA_ENABLED
854 7 : bool AP_MotorsMatrix::setup_octa_matrix(motor_frame_type frame_type)
855 : {
856 7 : _frame_class_string = "OCTA";
857 7 : _mav_type = MAV_TYPE_OCTOROTOR;
858 7 : switch (frame_type) {
859 5 : case MOTOR_FRAME_TYPE_PLUS: {
860 5 : _frame_type_string = "PLUS";
861 : static const AP_MotorsMatrix::MotorDef motors[] {
862 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
863 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
864 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
865 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
866 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
867 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
868 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
869 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
870 : };
871 :
872 5 : add_motors(motors, ARRAY_SIZE(motors));
873 5 : break;
874 : }
875 0 : case MOTOR_FRAME_TYPE_X: {
876 0 : _frame_type_string = "X";
877 : static const AP_MotorsMatrix::MotorDef motors[] {
878 : { 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
879 : { -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
880 : { 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
881 : { 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
882 : { -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
883 : { -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
884 : { -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
885 : { 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
886 : };
887 0 : add_motors(motors, ARRAY_SIZE(motors));
888 0 : break;
889 : }
890 0 : case MOTOR_FRAME_TYPE_V: {
891 0 : _frame_type_string = "V";
892 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
893 : { 0.83f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
894 : { -0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
895 : { 0.67f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
896 : { -0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
897 : { 1.00f, 1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
898 : { -0.83f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
899 : { -1.00f, 1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
900 : { 0.50f, -1.00f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
901 : };
902 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
903 0 : break;
904 : }
905 0 : case MOTOR_FRAME_TYPE_H: {
906 0 : _frame_type_string = "H";
907 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
908 : { -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
909 : { 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
910 : { -1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
911 : { -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
912 : { 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
913 : { 1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
914 : { 1.0f, 0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
915 : { -1.0f, -0.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
916 : };
917 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
918 0 : break;
919 : }
920 0 : case MOTOR_FRAME_TYPE_I: {
921 0 : _frame_type_string = "I";
922 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
923 : { 0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
924 : { -0.333f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
925 : { 1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
926 : { 0.333f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
927 : { -0.333f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
928 : { -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
929 : { -1.0f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
930 : { 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
931 : };
932 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
933 0 : break;
934 : }
935 1 : case MOTOR_FRAME_TYPE_DJI_X: {
936 1 : _frame_type_string = "DJI_X";
937 : static const AP_MotorsMatrix::MotorDef motors[] {
938 : { 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
939 : { -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
940 : { -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
941 : { -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
942 : { -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
943 : { 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
944 : { 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
945 : { 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
946 : };
947 1 : add_motors(motors, ARRAY_SIZE(motors));
948 1 : break;
949 : }
950 1 : case MOTOR_FRAME_TYPE_CW_X: {
951 1 : _frame_type_string = "CW_X";
952 : static const AP_MotorsMatrix::MotorDef motors[] {
953 : { 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
954 : { 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
955 : { 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
956 : { 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
957 : { -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
958 : { -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
959 : { -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
960 : { -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
961 : };
962 1 : add_motors(motors, ARRAY_SIZE(motors));
963 1 : break;
964 : }
965 0 : default:
966 : // octa frame class does not support this frame type
967 0 : return false;
968 : } // octa frame type
969 7 : return true;
970 : }
971 : #endif //AP_MOTORS_FRAME_OCTA_ENABLED
972 : #if AP_MOTORS_FRAME_OCTAQUAD_ENABLED
973 29 : bool AP_MotorsMatrix::setup_octaquad_matrix(motor_frame_type frame_type)
974 : {
975 29 : _mav_type = MAV_TYPE_OCTOROTOR;
976 29 : _frame_class_string = "OCTAQUAD";
977 29 : switch (frame_type) {
978 1 : case MOTOR_FRAME_TYPE_PLUS: {
979 1 : _frame_type_string = "PLUS";
980 : static const AP_MotorsMatrix::MotorDef motors[] {
981 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
982 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
983 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
984 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
985 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
986 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
987 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
988 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
989 : };
990 1 : add_motors(motors, ARRAY_SIZE(motors));
991 1 : break;
992 : }
993 0 : case MOTOR_FRAME_TYPE_X: {
994 0 : _frame_type_string = "X";
995 : static const AP_MotorsMatrix::MotorDef motors[] {
996 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
997 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
998 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
999 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1000 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
1001 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1002 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
1003 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1004 : };
1005 0 : add_motors(motors, ARRAY_SIZE(motors));
1006 0 : break;
1007 : }
1008 0 : case MOTOR_FRAME_TYPE_V: {
1009 0 : _frame_type_string = "V";
1010 : static const AP_MotorsMatrix::MotorDef motors[] {
1011 : { 45, 0.7981f, 1 },
1012 : { -45, -0.7981f, 7 },
1013 : { -135, 1.0000f, 5 },
1014 : { 135, -1.0000f, 3 },
1015 : { -45, 0.7981f, 8 },
1016 : { 45, -0.7981f, 2 },
1017 : { 135, 1.0000f, 4 },
1018 : { -135, -1.0000f, 6 },
1019 : };
1020 0 : add_motors(motors, ARRAY_SIZE(motors));
1021 0 : break;
1022 : }
1023 0 : case MOTOR_FRAME_TYPE_H: {
1024 : // H frame set-up - same as X but motors spin in opposite directions
1025 0 : _frame_type_string = "H";
1026 : static const AP_MotorsMatrix::MotorDef motors[] {
1027 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
1028 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
1029 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
1030 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1031 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1032 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1033 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1034 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1035 : };
1036 0 : add_motors(motors, ARRAY_SIZE(motors));
1037 0 : break;
1038 : }
1039 26 : case MOTOR_FRAME_TYPE_CW_X: {
1040 26 : _frame_type_string = "CW_X";
1041 : static const AP_MotorsMatrix::MotorDef motors[] {
1042 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1043 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1044 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1045 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
1046 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1047 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1048 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
1049 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
1050 : };
1051 26 : add_motors(motors, ARRAY_SIZE(motors));
1052 26 : break;
1053 : }
1054 : // BF/X cinelifters using two 4-in-1 ESCs are quite common
1055 : // see: https://fpvfrenzy.com/betaflight-motor-order/
1056 0 : case MOTOR_FRAME_TYPE_BF_X: {
1057 0 : _frame_type_string = "BF_X";
1058 : static const AP_MotorsMatrix::MotorDef motors[] {
1059 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1060 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1061 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1062 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
1063 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
1064 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1065 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1066 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
1067 : };
1068 0 : add_motors(motors, ARRAY_SIZE(motors));
1069 0 : break;
1070 : }
1071 0 : case MOTOR_FRAME_TYPE_BF_X_REV: {
1072 : // betaflight octa quad X order, reversed motors
1073 0 : _frame_type_string = "X_REV";
1074 : static const AP_MotorsMatrix::MotorDef motors[] {
1075 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1076 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
1077 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
1078 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
1079 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1080 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1081 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1082 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1083 : };
1084 0 : add_motors(motors, ARRAY_SIZE(motors));
1085 0 : break;
1086 : }
1087 1 : case MOTOR_FRAME_TYPE_X_COR: {
1088 : static_assert(AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR < 1.0, "AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR must be less than 1.0");
1089 1 : _frame_type_string = "X_COR";
1090 : static const AP_MotorsMatrix::MotorDef motors[] {
1091 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1092 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
1093 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1094 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1095 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1096 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1097 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1098 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1099 : };
1100 1 : add_motors(motors, ARRAY_SIZE(motors));
1101 : // scale top layer to prevent a beat frequency between layers in co-rotating setups
1102 5 : for (uint8_t i=0; i<4; i++) {
1103 4 : _roll_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1104 4 : _pitch_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1105 4 : _yaw_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1106 4 : _throttle_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1107 : }
1108 1 : break;
1109 : }
1110 1 : case MOTOR_FRAME_TYPE_CW_X_COR: {
1111 1 : _frame_type_string = "CW_X_COR";
1112 : static const AP_MotorsMatrix::MotorDef motors[] {
1113 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1114 : { 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1115 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1116 : { 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1117 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1118 : { -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1119 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 },
1120 : { -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1121 : };
1122 1 : add_motors(motors, ARRAY_SIZE(motors));
1123 : // scale top layer to prevent a beat frequency between layers in co-rotating setups
1124 5 : for (uint8_t i=0; i<8; i += 2) {
1125 4 : _roll_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1126 4 : _pitch_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1127 4 : _yaw_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1128 4 : _throttle_factor[i] *= AP_MOTORS_FRAME_OCTAQUAD_COROTATING_SCALE_FACTOR;
1129 : }
1130 1 : break;
1131 : }
1132 0 : default:
1133 : // octaquad frame class does not support this frame type
1134 0 : return false;
1135 : } //octaquad
1136 29 : return true;
1137 : }
1138 : #endif // AP_MOTORS_FRAME_OCTAQUAD_ENABLED
1139 : #if AP_MOTORS_FRAME_DODECAHEXA_ENABLED
1140 1 : bool AP_MotorsMatrix::setup_dodecahexa_matrix(motor_frame_type frame_type)
1141 : {
1142 1 : _mav_type = MAV_TYPE_DODECAROTOR;
1143 1 : _frame_class_string = "DODECAHEXA";
1144 1 : switch (frame_type) {
1145 1 : case MOTOR_FRAME_TYPE_PLUS: {
1146 1 : _frame_type_string = "PLUS";
1147 : static const AP_MotorsMatrix::MotorDef motors[] {
1148 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 }, // forward-top
1149 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 }, // forward-bottom
1150 : { 60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 }, // forward-right-top
1151 : { 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 }, // forward-right-bottom
1152 : { 120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 }, // back-right-top
1153 : { 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 }, // back-right-bottom
1154 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 }, // back-top
1155 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 }, // back-bottom
1156 : { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9 }, // back-left-top
1157 : { -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10 }, // back-left-bottom
1158 : { -60, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11 }, // forward-left-top
1159 : { -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12 }, // forward-left-bottom
1160 : };
1161 1 : add_motors(motors, ARRAY_SIZE(motors));
1162 1 : break;
1163 : }
1164 0 : case MOTOR_FRAME_TYPE_X: {
1165 0 : _frame_type_string = "X";
1166 : static const AP_MotorsMatrix::MotorDef motors[] {
1167 : { 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 }, // forward-right-top
1168 : { 30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 }, // forward-right-bottom
1169 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 }, // right-top
1170 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 }, // right-bottom
1171 : { 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 }, // back-right-top
1172 : { 150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 }, // back-right-bottom
1173 : { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7 }, // back-left-top
1174 : { -150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 }, // back-left-bottom
1175 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9 }, // left-top
1176 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10 }, // left-bottom
1177 : { -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 11 }, // forward-left-top
1178 : { -30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 12 }, // forward-left-bottom
1179 : };
1180 0 : add_motors(motors, ARRAY_SIZE(motors));
1181 0 : break;
1182 : }
1183 0 : default:
1184 : // dodeca-hexa frame class does not support this frame type
1185 0 : return false;
1186 : } //dodecahexa
1187 1 : return true;
1188 : }
1189 : #endif //AP_MOTORS_FRAME_DODECAHEXA_ENABLED
1190 : #if AP_MOTORS_FRAME_Y6_ENABLED
1191 3 : bool AP_MotorsMatrix::setup_y6_matrix(motor_frame_type frame_type)
1192 : {
1193 3 : _mav_type = MAV_TYPE_HEXAROTOR;
1194 3 : _frame_class_string = "Y6";
1195 3 : switch (frame_type) {
1196 0 : case MOTOR_FRAME_TYPE_Y6B: {
1197 : // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
1198 0 : _frame_type_string = "Y6B";
1199 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
1200 : { -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
1201 : { -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1202 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3 },
1203 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
1204 : { 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
1205 : { 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1206 : };
1207 0 : add_motors_raw(motors, ARRAY_SIZE(motors));
1208 0 : break;
1209 : }
1210 2 : case MOTOR_FRAME_TYPE_Y6F: {
1211 : // Y6 motor layout for FireFlyY6
1212 2 : _frame_type_string = "Y6F";
1213 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
1214 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1215 : { -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1216 : { 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1217 : { 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1218 : { -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1219 : { 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1220 : };
1221 2 : add_motors_raw(motors, ARRAY_SIZE(motors));
1222 2 : break;
1223 : }
1224 1 : default: {
1225 1 : _frame_type_string = "default";
1226 : static const AP_MotorsMatrix::MotorDefRaw motors[] {
1227 : { -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
1228 : { 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5 },
1229 : { 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
1230 : { 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1231 : { -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1 },
1232 : { 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1233 : };
1234 1 : add_motors_raw(motors, ARRAY_SIZE(motors));
1235 1 : break;
1236 : }
1237 : } //y6
1238 3 : return true;
1239 : }
1240 : #endif // AP_MOTORS_FRAME_Y6_ENABLED
1241 : #if AP_MOTORS_FRAME_DECA_ENABLED
1242 2 : bool AP_MotorsMatrix::setup_deca_matrix(motor_frame_type frame_type)
1243 : {
1244 2 : _mav_type = MAV_TYPE_DECAROTOR;
1245 2 : _frame_class_string = "DECA";
1246 2 : switch (frame_type) {
1247 1 : case MOTOR_FRAME_TYPE_PLUS: {
1248 1 : _frame_type_string = "PLUS";
1249 : static const AP_MotorsMatrix::MotorDef motors[] {
1250 : { 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1251 : { 36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1252 : { 72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1253 : { 108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1254 : { 144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1255 : { 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1256 : { -144, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
1257 : { -108, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1258 : { -72, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9 },
1259 : { -36, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10 },
1260 : };
1261 1 : add_motors(motors, ARRAY_SIZE(motors));
1262 1 : break;
1263 : }
1264 1 : case MOTOR_FRAME_TYPE_X:
1265 : case MOTOR_FRAME_TYPE_CW_X: {
1266 1 : _frame_type_string = "X/CW_X";
1267 : static const AP_MotorsMatrix::MotorDef motors[] {
1268 : { 18, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
1269 : { 54, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
1270 : { 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
1271 : { 126, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
1272 : { 162, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
1273 : { -162, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
1274 : { -126, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
1275 : { -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8 },
1276 : { -54, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 9 },
1277 : { -18, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 10 },
1278 : };
1279 1 : add_motors(motors, ARRAY_SIZE(motors));
1280 1 : break;
1281 : }
1282 0 : default:
1283 : // deca frame class does not support this frame type
1284 0 : return false;
1285 : } //deca
1286 2 : return true;
1287 : }
1288 : #endif // AP_MOTORS_FRAME_DECA_ENABLED
1289 :
1290 1100 : void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
1291 : {
1292 : // remove existing motors
1293 36300 : for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
1294 35200 : remove_motor(i);
1295 : }
1296 1100 : set_initialised_ok(false);
1297 1100 : bool success = true;
1298 :
1299 1100 : switch (frame_class) {
1300 : #if AP_MOTORS_FRAME_QUAD_ENABLED
1301 1050 : case MOTOR_FRAME_QUAD:
1302 1050 : success = setup_quad_matrix(frame_type);
1303 1050 : break; // quad
1304 : #endif //AP_MOTORS_FRAME_QUAD_ENABLED
1305 : #if AP_MOTORS_FRAME_HEXA_ENABLED
1306 4 : case MOTOR_FRAME_HEXA:
1307 4 : success = setup_hexa_matrix(frame_type);
1308 4 : break;
1309 : #endif //AP_MOTORS_FRAME_HEXA_ENABLED
1310 : #if AP_MOTORS_FRAME_OCTA_ENABLED
1311 7 : case MOTOR_FRAME_OCTA:
1312 7 : success = setup_octa_matrix(frame_type);
1313 7 : break;
1314 : #endif //AP_MOTORS_FRAME_OCTA_ENABLED
1315 : #if AP_MOTORS_FRAME_OCTAQUAD_ENABLED
1316 29 : case MOTOR_FRAME_OCTAQUAD:
1317 29 : success = setup_octaquad_matrix(frame_type);
1318 29 : break;
1319 : #endif //AP_MOTORS_FRAME_OCTAQUAD_ENABLED
1320 : #if AP_MOTORS_FRAME_DODECAHEXA_ENABLED
1321 1 : case MOTOR_FRAME_DODECAHEXA:
1322 1 : success = setup_dodecahexa_matrix(frame_type);
1323 1 : break;
1324 : #endif //AP_MOTORS_FRAME_DODECAHEXA_ENABLED
1325 : #if AP_MOTORS_FRAME_Y6_ENABLED
1326 3 : case MOTOR_FRAME_Y6:
1327 3 : success = setup_y6_matrix(frame_type);
1328 3 : break;
1329 : #endif //AP_MOTORS_FRAME_Y6_ENABLED
1330 : #if AP_MOTORS_FRAME_DECA_ENABLED
1331 2 : case MOTOR_FRAME_DECA:
1332 2 : success = setup_deca_matrix(frame_type);
1333 2 : break;
1334 : #endif //AP_MOTORS_FRAME_DECA_ENABLED
1335 4 : default:
1336 : // matrix doesn't support the configured class
1337 4 : success = false;
1338 4 : _mav_type = MAV_TYPE_GENERIC;
1339 4 : break;
1340 : } // switch frame_class
1341 :
1342 : // normalise factors to magnitude 0.5
1343 1100 : normalise_rpy_factors();
1344 :
1345 1100 : if (!success) {
1346 4 : _frame_class_string = "UNSUPPORTED";
1347 : }
1348 1100 : set_initialised_ok(success);
1349 1100 : }
1350 :
1351 : // normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
1352 : // normalizes throttle factors so max value is 1 and no value is less than 0
1353 1103 : void AP_MotorsMatrix::normalise_rpy_factors()
1354 : {
1355 1103 : float roll_fac = 0.0f;
1356 1103 : float pitch_fac = 0.0f;
1357 1103 : float yaw_fac = 0.0f;
1358 1103 : float throttle_fac = 0.0f;
1359 :
1360 : // find maximum roll, pitch and yaw factors
1361 36399 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
1362 35296 : if (motor_enabled[i]) {
1363 4626 : roll_fac = MAX(roll_fac,fabsf(_roll_factor[i]));
1364 4626 : pitch_fac = MAX(pitch_fac,fabsf(_pitch_factor[i]));
1365 4626 : yaw_fac = MAX(yaw_fac,fabsf(_yaw_factor[i]));
1366 4626 : throttle_fac = MAX(throttle_fac,MAX(0.0f,_throttle_factor[i]));
1367 : }
1368 : }
1369 :
1370 : // scale factors back to -0.5 to +0.5 for each axis
1371 36399 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
1372 35296 : if (motor_enabled[i]) {
1373 4626 : if (!is_zero(roll_fac)) {
1374 4626 : _roll_factor[i] = 0.5f * _roll_factor[i] / roll_fac;
1375 : }
1376 4626 : if (!is_zero(pitch_fac)) {
1377 4626 : _pitch_factor[i] = 0.5f * _pitch_factor[i] / pitch_fac;
1378 : }
1379 4626 : if (!is_zero(yaw_fac)) {
1380 4626 : _yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
1381 : }
1382 4626 : if (!is_zero(throttle_fac)) {
1383 4626 : _throttle_factor[i] = MAX(0.0f,_throttle_factor[i] / throttle_fac);
1384 : }
1385 : }
1386 : }
1387 1103 : }
1388 :
1389 :
1390 : /*
1391 : call vehicle supplied thrust compensation if set. This allows
1392 : vehicle code to compensate for vehicle specific motor arrangements
1393 : such as tiltrotors or tiltwings
1394 : */
1395 35522565 : void AP_MotorsMatrix::thrust_compensation(void)
1396 : {
1397 35522565 : if (_thrust_compensation_callback) {
1398 320854 : _thrust_compensation_callback(_thrust_rpyt_out, AP_MOTORS_MAX_NUM_MOTORS);
1399 : }
1400 35522565 : }
1401 :
1402 : /*
1403 : disable the use of motor torque to control yaw. Used when an
1404 : external mechanism such as vectoring is used for yaw control
1405 : */
1406 5 : void AP_MotorsMatrix::disable_yaw_torque(void)
1407 : {
1408 165 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
1409 160 : _yaw_factor[i] = 0;
1410 : }
1411 5 : }
1412 :
1413 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
1414 : // examples can pull values direct
1415 : float AP_MotorsMatrix::get_thrust_rpyt_out(uint8_t i) const
1416 : {
1417 : if (i < AP_MOTORS_MAX_NUM_MOTORS) {
1418 : return _thrust_rpyt_out[i];
1419 : }
1420 : return 0.0;
1421 : }
1422 :
1423 : bool AP_MotorsMatrix::get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const
1424 : {
1425 : if ((i < AP_MOTORS_MAX_NUM_MOTORS) && motor_enabled[i]) {
1426 : roll = _roll_factor[i];
1427 : pitch = _pitch_factor[i];
1428 : yaw = _yaw_factor[i];
1429 : throttle = _throttle_factor[i];
1430 : testing_order = _test_order[i];
1431 : return true;
1432 : }
1433 : return false;
1434 : }
1435 : #endif
1436 :
1437 : // singleton instance
1438 : AP_MotorsMatrix *AP_MotorsMatrix::_singleton;
|
