2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
27 #include "build_config.h"
29 #include "common/axis.h"
30 #include "common/maths.h"
32 #include "drivers/system.h"
33 #include "drivers/pwm_output.h"
34 #include "drivers/pwm_mapping.h"
35 #include "drivers/sensor.h"
36 #include "drivers/accgyro.h"
37 #include "drivers/system.h"
41 #include "io/gimbal.h"
42 #include "io/escservo.h"
43 #include "io/rc_controls.h"
45 #include "sensors/sensors.h"
46 #include "sensors/acceleration.h"
47 #include "sensors/battery.h"
49 #include "flight/mixer.h"
50 #include "flight/failsafe.h"
51 #include "flight/pid.h"
52 #include "flight/imu.h"
53 #include "flight/lowpass.h"
55 #include "config/runtime_config.h"
56 #include "config/config.h"
60 int16_t motor
[MAX_SUPPORTED_MOTORS
];
61 int16_t motor_disarmed
[MAX_SUPPORTED_MOTORS
];
63 static mixerConfig_t
*mixerConfig
;
64 static flight3DConfig_t
*flight3DConfig
;
65 static escAndServoConfig_t
*escAndServoConfig
;
66 static airplaneConfig_t
*airplaneConfig
;
67 static rxConfig_t
*rxConfig
;
69 static mixerMode_e currentMixerMode
;
70 static motorMixer_t currentMixer
[MAX_SUPPORTED_MOTORS
];
73 static uint8_t servoRuleCount
= 0;
74 static servoMixer_t currentServoMixer
[MAX_SERVO_RULES
];
75 static gimbalConfig_t
*gimbalConfig
;
76 int16_t servo
[MAX_SUPPORTED_SERVOS
];
78 STATIC_UNIT_TESTED
uint8_t servoCount
;
79 static servoParam_t
*servoConf
;
80 static lowpass_t lowpassFilters
[MAX_SUPPORTED_SERVOS
];
83 static const motorMixer_t mixerQuadX
[] = {
84 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
85 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
86 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
87 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
89 #ifndef USE_QUAD_MIXER_ONLY
90 static const motorMixer_t mixerTricopter
[] = {
91 { 1.0f
, 0.0f
, 1.333333f
, 0.0f
}, // REAR
92 { 1.0f
, -1.0f
, -0.666667f
, 0.0f
}, // RIGHT
93 { 1.0f
, 1.0f
, -0.666667f
, 0.0f
}, // LEFT
96 static const motorMixer_t mixerQuadP
[] = {
97 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
98 { 1.0f
, -1.0f
, 0.0f
, 1.0f
}, // RIGHT
99 { 1.0f
, 1.0f
, 0.0f
, 1.0f
}, // LEFT
100 { 1.0f
, 0.0f
, -1.0f
, -1.0f
}, // FRONT
103 static const motorMixer_t mixerBicopter
[] = {
104 { 1.0f
, 1.0f
, 0.0f
, 0.0f
}, // LEFT
105 { 1.0f
, -1.0f
, 0.0f
, 0.0f
}, // RIGHT
108 static const motorMixer_t mixerY6
[] = {
109 { 1.0f
, 0.0f
, 1.333333f
, 1.0f
}, // REAR
110 { 1.0f
, -1.0f
, -0.666667f
, -1.0f
}, // RIGHT
111 { 1.0f
, 1.0f
, -0.666667f
, -1.0f
}, // LEFT
112 { 1.0f
, 0.0f
, 1.333333f
, -1.0f
}, // UNDER_REAR
113 { 1.0f
, -1.0f
, -0.666667f
, 1.0f
}, // UNDER_RIGHT
114 { 1.0f
, 1.0f
, -0.666667f
, 1.0f
}, // UNDER_LEFT
117 static const motorMixer_t mixerHex6P
[] = {
118 { 1.0f
, -0.866025f
, 0.5f
, 1.0f
}, // REAR_R
119 { 1.0f
, -0.866025f
, -0.5f
, -1.0f
}, // FRONT_R
120 { 1.0f
, 0.866025f
, 0.5f
, 1.0f
}, // REAR_L
121 { 1.0f
, 0.866025f
, -0.5f
, -1.0f
}, // FRONT_L
122 { 1.0f
, 0.0f
, -1.0f
, 1.0f
}, // FRONT
123 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
126 static const motorMixer_t mixerY4
[] = {
127 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR_TOP CW
128 { 1.0f
, -1.0f
, -1.0f
, 0.0f
}, // FRONT_R CCW
129 { 1.0f
, 0.0f
, 1.0f
, 1.0f
}, // REAR_BOTTOM CCW
130 { 1.0f
, 1.0f
, -1.0f
, 0.0f
}, // FRONT_L CW
133 static const motorMixer_t mixerHex6X
[] = {
134 { 1.0f
, -0.5f
, 0.866025f
, 1.0f
}, // REAR_R
135 { 1.0f
, -0.5f
, -0.866025f
, 1.0f
}, // FRONT_R
136 { 1.0f
, 0.5f
, 0.866025f
, -1.0f
}, // REAR_L
137 { 1.0f
, 0.5f
, -0.866025f
, -1.0f
}, // FRONT_L
138 { 1.0f
, -1.0f
, 0.0f
, -1.0f
}, // RIGHT
139 { 1.0f
, 1.0f
, 0.0f
, 1.0f
}, // LEFT
142 static const motorMixer_t mixerOctoX8
[] = {
143 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
144 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
145 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
146 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
147 { 1.0f
, -1.0f
, 1.0f
, 1.0f
}, // UNDER_REAR_R
148 { 1.0f
, -1.0f
, -1.0f
, -1.0f
}, // UNDER_FRONT_R
149 { 1.0f
, 1.0f
, 1.0f
, -1.0f
}, // UNDER_REAR_L
150 { 1.0f
, 1.0f
, -1.0f
, 1.0f
}, // UNDER_FRONT_L
153 static const motorMixer_t mixerOctoFlatP
[] = {
154 { 1.0f
, 0.707107f
, -0.707107f
, 1.0f
}, // FRONT_L
155 { 1.0f
, -0.707107f
, -0.707107f
, 1.0f
}, // FRONT_R
156 { 1.0f
, -0.707107f
, 0.707107f
, 1.0f
}, // REAR_R
157 { 1.0f
, 0.707107f
, 0.707107f
, 1.0f
}, // REAR_L
158 { 1.0f
, 0.0f
, -1.0f
, -1.0f
}, // FRONT
159 { 1.0f
, -1.0f
, 0.0f
, -1.0f
}, // RIGHT
160 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
161 { 1.0f
, 1.0f
, 0.0f
, -1.0f
}, // LEFT
164 static const motorMixer_t mixerOctoFlatX
[] = {
165 { 1.0f
, 1.0f
, -0.414178f
, 1.0f
}, // MIDFRONT_L
166 { 1.0f
, -0.414178f
, -1.0f
, 1.0f
}, // FRONT_R
167 { 1.0f
, -1.0f
, 0.414178f
, 1.0f
}, // MIDREAR_R
168 { 1.0f
, 0.414178f
, 1.0f
, 1.0f
}, // REAR_L
169 { 1.0f
, 0.414178f
, -1.0f
, -1.0f
}, // FRONT_L
170 { 1.0f
, -1.0f
, -0.414178f
, -1.0f
}, // MIDFRONT_R
171 { 1.0f
, -0.414178f
, 1.0f
, -1.0f
}, // REAR_R
172 { 1.0f
, 1.0f
, 0.414178f
, -1.0f
}, // MIDREAR_L
175 static const motorMixer_t mixerVtail4
[] = {
176 { 1.0f
, -0.58f
, 0.58f
, 1.0f
}, // REAR_R
177 { 1.0f
, -0.46f
, -0.39f
, -0.5f
}, // FRONT_R
178 { 1.0f
, 0.58f
, 0.58f
, -1.0f
}, // REAR_L
179 { 1.0f
, 0.46f
, -0.39f
, 0.5f
}, // FRONT_L
182 static const motorMixer_t mixerAtail4
[] = {
183 { 1.0f
, 0.0f
, 1.0f
, 1.0f
}, // REAR_R
184 { 1.0f
, -1.0f
, -1.0f
, 0.0f
}, // FRONT_R
185 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR_L
186 { 1.0f
, 1.0f
, -1.0f
, -0.0f
}, // FRONT_L
189 static const motorMixer_t mixerHex6H
[] = {
190 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
191 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
192 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
193 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
194 { 1.0f
, 0.0f
, 0.0f
, 0.0f
}, // RIGHT
195 { 1.0f
, 0.0f
, 0.0f
, 0.0f
}, // LEFT
198 static const motorMixer_t mixerDualcopter
[] = {
199 { 1.0f
, 0.0f
, 0.0f
, -1.0f
}, // LEFT
200 { 1.0f
, 0.0f
, 0.0f
, 1.0f
}, // RIGHT
203 static const motorMixer_t mixerSingleProp
[] = {
204 { 1.0f
, 0.0f
, 0.0f
, 0.0f
},
207 static const motorMixer_t mixerQuadX1234
[] = {
208 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
209 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
210 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
211 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
214 // Keep synced with mixerMode_e
215 const mixer_t mixers
[] = {
216 // motors, use servo, motor mixer
217 { 0, false, NULL
}, // entry 0
218 { 3, true, mixerTricopter
}, // MIXER_TRI
219 { 4, false, mixerQuadP
}, // MIXER_QUADP
220 { 4, false, mixerQuadX
}, // MIXER_QUADX
221 { 2, true, mixerBicopter
}, // MIXER_BICOPTER
222 { 0, true, NULL
}, // * MIXER_GIMBAL
223 { 6, false, mixerY6
}, // MIXER_Y6
224 { 6, false, mixerHex6P
}, // MIXER_HEX6
225 { 1, true, mixerSingleProp
}, // * MIXER_FLYING_WING
226 { 4, false, mixerY4
}, // MIXER_Y4
227 { 6, false, mixerHex6X
}, // MIXER_HEX6X
228 { 8, false, mixerOctoX8
}, // MIXER_OCTOX8
229 { 8, false, mixerOctoFlatP
}, // MIXER_OCTOFLATP
230 { 8, false, mixerOctoFlatX
}, // MIXER_OCTOFLATX
231 { 1, true, mixerSingleProp
}, // * MIXER_AIRPLANE
232 { 0, true, NULL
}, // * MIXER_HELI_120_CCPM
233 { 0, true, NULL
}, // * MIXER_HELI_90_DEG
234 { 4, false, mixerVtail4
}, // MIXER_VTAIL4
235 { 6, false, mixerHex6H
}, // MIXER_HEX6H
236 { 0, true, NULL
}, // * MIXER_PPM_TO_SERVO
237 { 2, true, mixerDualcopter
}, // MIXER_DUALCOPTER
238 { 1, true, NULL
}, // MIXER_SINGLECOPTER
239 { 4, false, mixerAtail4
}, // MIXER_ATAIL4
240 { 0, false, NULL
}, // MIXER_CUSTOM
241 { 2, true, NULL
}, // MIXER_CUSTOM_AIRPLANE
242 { 3, true, NULL
}, // MIXER_CUSTOM_TRI
243 { 4, false, mixerQuadX1234
},
249 #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
250 // mixer rule format servo, input, rate, speed, min, max, box
251 static const servoMixer_t servoMixerAirplane
[] = {
252 { SERVO_FLAPPERON_1
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
253 { SERVO_FLAPPERON_2
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
254 { SERVO_RUDDER
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
255 { SERVO_ELEVATOR
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
256 { SERVO_THROTTLE
, INPUT_STABILIZED_THROTTLE
, 100, 0, 0, 100, 0 },
259 static const servoMixer_t servoMixerFlyingWing
[] = {
260 { SERVO_FLAPPERON_1
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
261 { SERVO_FLAPPERON_1
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
262 { SERVO_FLAPPERON_2
, INPUT_STABILIZED_ROLL
, -100, 0, 0, 100, 0 },
263 { SERVO_FLAPPERON_2
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
264 { SERVO_THROTTLE
, INPUT_STABILIZED_THROTTLE
, 100, 0, 0, 100, 0 },
267 static const servoMixer_t servoMixerBI
[] = {
268 { SERVO_BICOPTER_LEFT
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
269 { SERVO_BICOPTER_LEFT
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
270 { SERVO_BICOPTER_RIGHT
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
271 { SERVO_BICOPTER_RIGHT
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
274 static const servoMixer_t servoMixerTri
[] = {
275 { SERVO_RUDDER
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
278 static const servoMixer_t servoMixerDual
[] = {
279 { SERVO_DUALCOPTER_LEFT
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
280 { SERVO_DUALCOPTER_RIGHT
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
283 static const servoMixer_t servoMixerSingle
[] = {
284 { SERVO_SINGLECOPTER_1
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
285 { SERVO_SINGLECOPTER_1
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
286 { SERVO_SINGLECOPTER_2
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
287 { SERVO_SINGLECOPTER_2
, INPUT_STABILIZED_PITCH
, 100, 0, 0, 100, 0 },
288 { SERVO_SINGLECOPTER_3
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
289 { SERVO_SINGLECOPTER_3
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
290 { SERVO_SINGLECOPTER_4
, INPUT_STABILIZED_YAW
, 100, 0, 0, 100, 0 },
291 { SERVO_SINGLECOPTER_4
, INPUT_STABILIZED_ROLL
, 100, 0, 0, 100, 0 },
294 static const servoMixer_t servoMixerGimbal
[] = {
295 { SERVO_GIMBAL_PITCH
, INPUT_GIMBAL_PITCH
, 125, 0, 0, 100, 0 },
296 { SERVO_GIMBAL_ROLL
, INPUT_GIMBAL_ROLL
, 125, 0, 0, 100, 0 },
300 const mixerRules_t servoMixers
[] = {
301 { 0, NULL
}, // entry 0
302 { COUNT_SERVO_RULES(servoMixerTri
), servoMixerTri
}, // MULTITYPE_TRI
303 { 0, NULL
}, // MULTITYPE_QUADP
304 { 0, NULL
}, // MULTITYPE_QUADX
305 { COUNT_SERVO_RULES(servoMixerBI
), servoMixerBI
}, // MULTITYPE_BI
306 { COUNT_SERVO_RULES(servoMixerGimbal
), servoMixerGimbal
}, // * MULTITYPE_GIMBAL
307 { 0, NULL
}, // MULTITYPE_Y6
308 { 0, NULL
}, // MULTITYPE_HEX6
309 { COUNT_SERVO_RULES(servoMixerFlyingWing
), servoMixerFlyingWing
},// * MULTITYPE_FLYING_WING
310 { 0, NULL
}, // MULTITYPE_Y4
311 { 0, NULL
}, // MULTITYPE_HEX6X
312 { 0, NULL
}, // MULTITYPE_OCTOX8
313 { 0, NULL
}, // MULTITYPE_OCTOFLATP
314 { 0, NULL
}, // MULTITYPE_OCTOFLATX
315 { COUNT_SERVO_RULES(servoMixerAirplane
), servoMixerAirplane
}, // * MULTITYPE_AIRPLANE
316 { 0, NULL
}, // * MULTITYPE_HELI_120_CCPM
317 { 0, NULL
}, // * MULTITYPE_HELI_90_DEG
318 { 0, NULL
}, // MULTITYPE_VTAIL4
319 { 0, NULL
}, // MULTITYPE_HEX6H
320 { 0, NULL
}, // * MULTITYPE_PPM_TO_SERVO
321 { COUNT_SERVO_RULES(servoMixerDual
), servoMixerDual
}, // MULTITYPE_DUALCOPTER
322 { COUNT_SERVO_RULES(servoMixerSingle
), servoMixerSingle
}, // MULTITYPE_SINGLECOPTER
323 { 0, NULL
}, // MULTITYPE_ATAIL4
324 { 0, NULL
}, // MULTITYPE_CUSTOM
325 { 0, NULL
}, // MULTITYPE_CUSTOM_PLANE
326 { 0, NULL
}, // MULTITYPE_CUSTOM_TRI
330 static servoMixer_t
*customServoMixers
;
334 static motorMixer_t
*customMixers
;
336 void mixerUseConfigs(
338 servoParam_t
*servoConfToUse
,
339 gimbalConfig_t
*gimbalConfigToUse
,
341 flight3DConfig_t
*flight3DConfigToUse
,
342 escAndServoConfig_t
*escAndServoConfigToUse
,
343 mixerConfig_t
*mixerConfigToUse
,
344 airplaneConfig_t
*airplaneConfigToUse
,
345 rxConfig_t
*rxConfigToUse
)
348 servoConf
= servoConfToUse
;
349 gimbalConfig
= gimbalConfigToUse
;
351 flight3DConfig
= flight3DConfigToUse
;
352 escAndServoConfig
= escAndServoConfigToUse
;
353 mixerConfig
= mixerConfigToUse
;
354 airplaneConfig
= airplaneConfigToUse
;
355 rxConfig
= rxConfigToUse
;
359 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex
)
361 uint8_t channelToForwardFrom
= servoConf
[servoIndex
].forwardFromChannel
;
363 if (channelToForwardFrom
!= CHANNEL_FORWARDING_DISABLED
&& channelToForwardFrom
< rxRuntimeConfig
.channelCount
) {
364 return rcData
[channelToForwardFrom
];
367 return servoConf
[servoIndex
].middle
;
371 int servoDirection(int servoIndex
, int inputSource
)
373 // determine the direction (reversed or not) from the direction bitfield of the servo
374 if (servoConf
[servoIndex
].reversedSources
& (1 << inputSource
))
381 #ifndef USE_QUAD_MIXER_ONLY
383 void loadCustomServoMixer(void)
389 memset(currentServoMixer
, 0, sizeof(currentServoMixer
));
391 // load custom mixer into currentServoMixer
392 for (i
= 0; i
< MAX_SERVO_RULES
; i
++) {
394 if (customServoMixers
[i
].rate
== 0)
397 currentServoMixer
[i
] = customServoMixers
[i
];
402 void mixerInit(mixerMode_e mixerMode
, motorMixer_t
*initialCustomMotorMixers
, servoMixer_t
*initialCustomServoMixers
)
404 currentMixerMode
= mixerMode
;
406 customMixers
= initialCustomMotorMixers
;
407 customServoMixers
= initialCustomServoMixers
;
409 // enable servos for mixes that require them. note, this shifts motor counts.
410 useServo
= mixers
[currentMixerMode
].useServo
;
411 // if we want camstab/trig, that also enables servos, even if mixer doesn't
412 if (feature(FEATURE_SERVO_TILT
))
415 // give all servos a default command
416 for (uint8_t i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
417 servo
[i
] = DEFAULT_SERVO_MIDDLE
;
421 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t
*pwmOutputConfiguration
)
426 servoCount
= pwmOutputConfiguration
->servoCount
;
428 if (currentMixerMode
== MIXER_CUSTOM
|| currentMixerMode
== MIXER_CUSTOM_TRI
|| currentMixerMode
== MIXER_CUSTOM_AIRPLANE
) {
429 // load custom mixer into currentMixer
430 for (i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++) {
432 if (customMixers
[i
].throttle
== 0.0f
)
434 currentMixer
[i
] = customMixers
[i
];
438 motorCount
= mixers
[currentMixerMode
].motorCount
;
439 // copy motor-based mixers
440 if (mixers
[currentMixerMode
].motor
) {
441 for (i
= 0; i
< motorCount
; i
++)
442 currentMixer
[i
] = mixers
[currentMixerMode
].motor
[i
];
447 servoRuleCount
= servoMixers
[currentMixerMode
].servoRuleCount
;
448 if (servoMixers
[currentMixerMode
].rule
) {
449 for (i
= 0; i
< servoRuleCount
; i
++)
450 currentServoMixer
[i
] = servoMixers
[currentMixerMode
].rule
[i
];
454 // in 3D mode, mixer gain has to be halved
455 if (feature(FEATURE_3D
)) {
456 if (motorCount
> 1) {
457 for (i
= 0; i
< motorCount
; i
++) {
458 currentMixer
[i
].pitch
*= 0.5f
;
459 currentMixer
[i
].roll
*= 0.5f
;
460 currentMixer
[i
].yaw
*= 0.5f
;
465 // set flag that we're on something with wings
466 if (currentMixerMode
== MIXER_FLYING_WING
||
467 currentMixerMode
== MIXER_AIRPLANE
||
468 currentMixerMode
== MIXER_CUSTOM_AIRPLANE
470 ENABLE_STATE(FIXED_WING
);
472 if (currentMixerMode
== MIXER_CUSTOM_AIRPLANE
) {
473 loadCustomServoMixer();
476 DISABLE_STATE(FIXED_WING
);
478 if (currentMixerMode
== MIXER_CUSTOM_TRI
) {
479 loadCustomServoMixer();
483 mixerResetDisarmedMotors();
487 void servoMixerLoadMix(int index
, servoMixer_t
*customServoMixers
)
494 for (i
= 0; i
< MAX_SERVO_RULES
; i
++)
495 customServoMixers
[i
].targetChannel
= customServoMixers
[i
].inputSource
= customServoMixers
[i
].rate
= customServoMixers
[i
].box
= 0;
497 for (i
= 0; i
< servoMixers
[index
].servoRuleCount
; i
++)
498 customServoMixers
[i
] = servoMixers
[index
].rule
[i
];
501 void mixerLoadMix(int index
, motorMixer_t
*customMixers
)
508 for (i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++)
509 customMixers
[i
].throttle
= 0.0f
;
511 // do we have anything here to begin with?
512 if (mixers
[index
].motor
!= NULL
) {
513 for (i
= 0; i
< mixers
[index
].motorCount
; i
++)
514 customMixers
[i
] = mixers
[index
].motor
[i
];
520 void mixerInit(mixerMode_e mixerMode
, motorMixer_t
*initialCustomMixers
)
522 currentMixerMode
= mixerMode
;
524 customMixers
= initialCustomMixers
;
527 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t
*pwmOutputConfiguration
)
529 UNUSED(pwmOutputConfiguration
);
536 for (i
= 0; i
< motorCount
; i
++) {
537 currentMixer
[i
] = mixerQuadX
[i
];
540 mixerResetDisarmedMotors();
544 void mixerResetDisarmedMotors(void)
547 // set disarmed motor values
548 for (i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++)
549 motor_disarmed
[i
] = feature(FEATURE_3D
) ? flight3DConfig
->neutral3d
: escAndServoConfig
->mincommand
;
554 STATIC_UNIT_TESTED
void forwardAuxChannelsToServos(uint8_t firstServoIndex
)
556 // start forwarding from this channel
557 uint8_t channelOffset
= AUX1
;
560 for (servoOffset
= 0; servoOffset
< MAX_AUX_CHANNEL_COUNT
&& channelOffset
< MAX_SUPPORTED_RC_CHANNEL_COUNT
; servoOffset
++) {
561 pwmWriteServo(firstServoIndex
+ servoOffset
, rcData
[channelOffset
++]);
565 static void updateGimbalServos(uint8_t firstServoIndex
)
567 pwmWriteServo(firstServoIndex
+ 0, servo
[SERVO_GIMBAL_PITCH
]);
568 pwmWriteServo(firstServoIndex
+ 1, servo
[SERVO_GIMBAL_ROLL
]);
571 void writeServos(void)
573 uint8_t servoIndex
= 0;
575 switch (currentMixerMode
) {
577 pwmWriteServo(servoIndex
++, servo
[SERVO_BICOPTER_LEFT
]);
578 pwmWriteServo(servoIndex
++, servo
[SERVO_BICOPTER_RIGHT
]);
582 case MIXER_CUSTOM_TRI
:
583 if (mixerConfig
->tri_unarmed_servo
) {
584 // if unarmed flag set, we always move servo
585 pwmWriteServo(servoIndex
++, servo
[SERVO_RUDDER
]);
587 // otherwise, only move servo when copter is armed
588 if (ARMING_FLAG(ARMED
))
589 pwmWriteServo(servoIndex
++, servo
[SERVO_RUDDER
]);
591 pwmWriteServo(servoIndex
++, 0); // kill servo signal completely.
595 case MIXER_FLYING_WING
:
596 pwmWriteServo(servoIndex
++, servo
[SERVO_FLAPPERON_1
]);
597 pwmWriteServo(servoIndex
++, servo
[SERVO_FLAPPERON_2
]);
600 case MIXER_DUALCOPTER
:
601 pwmWriteServo(servoIndex
++, servo
[SERVO_DUALCOPTER_LEFT
]);
602 pwmWriteServo(servoIndex
++, servo
[SERVO_DUALCOPTER_RIGHT
]);
605 case MIXER_CUSTOM_AIRPLANE
:
607 for (int i
= SERVO_PLANE_INDEX_MIN
; i
<= SERVO_PLANE_INDEX_MAX
; i
++) {
608 pwmWriteServo(servoIndex
++, servo
[i
]);
612 case MIXER_SINGLECOPTER
:
613 for (int i
= SERVO_SINGLECOPTER_INDEX_MIN
; i
<= SERVO_SINGLECOPTER_INDEX_MAX
; i
++) {
614 pwmWriteServo(servoIndex
++, servo
[i
]);
622 // Two servos for SERVO_TILT, if enabled
623 if (feature(FEATURE_SERVO_TILT
) || currentMixerMode
== MIXER_GIMBAL
) {
624 updateGimbalServos(servoIndex
);
628 // forward AUX to remaining servo outputs (not constrained)
629 if (feature(FEATURE_CHANNEL_FORWARDING
)) {
630 forwardAuxChannelsToServos(servoIndex
);
631 servoIndex
+= MAX_AUX_CHANNEL_COUNT
;
636 void writeMotors(void)
640 for (i
= 0; i
< motorCount
; i
++)
641 pwmWriteMotor(i
, motor
[i
]);
644 if (feature(FEATURE_ONESHOT125
)) {
645 pwmCompleteOneshotMotorUpdate(motorCount
);
649 void writeAllMotors(int16_t mc
)
653 // Sends commands to all motors
654 for (i
= 0; i
< motorCount
; i
++)
659 void stopMotors(void)
661 writeAllMotors(feature(FEATURE_3D
) ? flight3DConfig
->neutral3d
: escAndServoConfig
->mincommand
);
663 delay(50); // give the timers and ESCs a chance to react.
666 void StopPwmAllMotors()
668 pwmShutdownPulsesForAllMotors(motorCount
);
671 #ifndef USE_QUAD_MIXER_ONLY
672 STATIC_UNIT_TESTED
void servoMixer(void)
674 int16_t input
[INPUT_SOURCE_COUNT
]; // Range [-500:+500]
675 static int16_t currentOutput
[MAX_SERVO_RULES
];
678 if (FLIGHT_MODE(PASSTHRU_MODE
)) {
679 // Direct passthru from RX
680 input
[INPUT_STABILIZED_ROLL
] = rcCommand
[ROLL
];
681 input
[INPUT_STABILIZED_PITCH
] = rcCommand
[PITCH
];
682 input
[INPUT_STABILIZED_YAW
] = rcCommand
[YAW
];
684 // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
685 input
[INPUT_STABILIZED_ROLL
] = axisPID
[ROLL
];
686 input
[INPUT_STABILIZED_PITCH
] = axisPID
[PITCH
];
687 input
[INPUT_STABILIZED_YAW
] = axisPID
[YAW
];
689 // Reverse yaw servo when inverted in 3D mode
690 if (feature(FEATURE_3D
) && (rcData
[THROTTLE
] < rxConfig
->midrc
)) {
691 input
[INPUT_STABILIZED_YAW
] *= -1;
695 input
[INPUT_GIMBAL_PITCH
] = scaleRange(attitude
.values
.pitch
, -1800, 1800, -500, +500);
696 input
[INPUT_GIMBAL_ROLL
] = scaleRange(attitude
.values
.roll
, -1800, 1800, -500, +500);
698 input
[INPUT_STABILIZED_THROTTLE
] = motor
[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
700 // center the RC input value around the RC middle value
701 // by subtracting the RC middle value from the RC input value, we get:
702 // data - middle = input
703 // 2000 - 1500 = +500
705 // 1000 - 1500 = -500
706 input
[INPUT_RC_ROLL
] = rcData
[ROLL
] - rxConfig
->midrc
;
707 input
[INPUT_RC_PITCH
] = rcData
[PITCH
] - rxConfig
->midrc
;
708 input
[INPUT_RC_YAW
] = rcData
[YAW
] - rxConfig
->midrc
;
709 input
[INPUT_RC_THROTTLE
] = rcData
[THROTTLE
] - rxConfig
->midrc
;
710 input
[INPUT_RC_AUX1
] = rcData
[AUX1
] - rxConfig
->midrc
;
711 input
[INPUT_RC_AUX2
] = rcData
[AUX2
] - rxConfig
->midrc
;
712 input
[INPUT_RC_AUX3
] = rcData
[AUX3
] - rxConfig
->midrc
;
713 input
[INPUT_RC_AUX4
] = rcData
[AUX4
] - rxConfig
->midrc
;
715 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++)
718 // mix servos according to rules
719 for (i
= 0; i
< servoRuleCount
; i
++) {
720 // consider rule if no box assigned or box is active
721 if (currentServoMixer
[i
].box
== 0 || IS_RC_MODE_ACTIVE(BOXSERVO1
+ currentServoMixer
[i
].box
- 1)) {
722 uint8_t target
= currentServoMixer
[i
].targetChannel
;
723 uint8_t from
= currentServoMixer
[i
].inputSource
;
724 uint16_t servo_width
= servoConf
[target
].max
- servoConf
[target
].min
;
725 int16_t min
= currentServoMixer
[i
].min
* servo_width
/ 100 - servo_width
/ 2;
726 int16_t max
= currentServoMixer
[i
].max
* servo_width
/ 100 - servo_width
/ 2;
728 if (currentServoMixer
[i
].speed
== 0)
729 currentOutput
[i
] = input
[from
];
731 if (currentOutput
[i
] < input
[from
])
732 currentOutput
[i
] = constrain(currentOutput
[i
] + currentServoMixer
[i
].speed
, currentOutput
[i
], input
[from
]);
733 else if (currentOutput
[i
] > input
[from
])
734 currentOutput
[i
] = constrain(currentOutput
[i
] - currentServoMixer
[i
].speed
, input
[from
], currentOutput
[i
]);
737 servo
[target
] += servoDirection(target
, from
) * constrain(((int32_t)currentOutput
[i
] * currentServoMixer
[i
].rate
) / 100, min
, max
);
739 currentOutput
[i
] = 0;
743 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
744 servo
[i
] = ((int32_t)servoConf
[i
].rate
* servo
[i
]) / 100L;
745 servo
[i
] += determineServoMiddleOrForwardFromChannel(i
);
755 bool isFailsafeActive
= failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
757 if (motorCount
>= 4 && mixerConfig
->yaw_jump_prevention_limit
< YAW_JUMP_PREVENTION_LIMIT_HIGH
) {
758 // prevent "yaw jump" during yaw correction
759 axisPID
[YAW
] = constrain(axisPID
[YAW
], -mixerConfig
->yaw_jump_prevention_limit
- ABS(rcCommand
[YAW
]), mixerConfig
->yaw_jump_prevention_limit
+ ABS(rcCommand
[YAW
]));
762 // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
763 int16_t rollPitchYawMix
[MAX_SUPPORTED_MOTORS
];
764 int16_t rollPitchYawMixMax
= 0; // assumption: symetrical about zero.
765 int16_t rollPitchYawMixMin
= 0;
767 // Find roll/pitch/yaw desired output
768 for (i
= 0; i
< motorCount
; i
++) {
770 axisPID
[PITCH
] * currentMixer
[i
].pitch
+
771 axisPID
[ROLL
] * currentMixer
[i
].roll
+
772 -mixerConfig
->yaw_motor_direction
* axisPID
[YAW
] * currentMixer
[i
].yaw
;
774 if (batteryConfig
->vbatPidCompensation
) rollPitchYawMix
[i
] *= calculateVbatPidCompensation(); // Add voltage PID compensation
776 if (rollPitchYawMix
[i
] > rollPitchYawMixMax
) rollPitchYawMixMax
= rollPitchYawMix
[i
];
777 if (rollPitchYawMix
[i
] < rollPitchYawMixMin
) rollPitchYawMixMin
= rollPitchYawMix
[i
];
780 // Scale roll/pitch/yaw uniformly to fit within throttle range
781 int16_t rollPitchYawMixRange
= rollPitchYawMixMax
- rollPitchYawMixMin
;
782 int16_t throttleRange
, throttle
;
783 int16_t throttleMin
, throttleMax
;
785 // Find min and max throttle based on condition
786 if (feature(FEATURE_3D
)) {
787 static int16_t throttlePrevious
= 0; // Store the last throttle direction for deadband transitions
789 if (rcData
[THROTTLE
] <= (rxConfig
->midrc
- flight3DConfig
->deadband3d_throttle
)) { // Out of deadband handling
790 throttleMax
= flight3DConfig
->deadband3d_low
;
791 throttleMin
= escAndServoConfig
->minthrottle
;
792 throttlePrevious
= throttle
= rcCommand
[THROTTLE
];
793 } else if (rcData
[THROTTLE
] >= (rxConfig
->midrc
+ flight3DConfig
->deadband3d_throttle
)) {
794 throttleMax
= escAndServoConfig
->maxthrottle
;
795 throttleMin
= flight3DConfig
->deadband3d_high
;
796 throttlePrevious
= throttle
= rcCommand
[THROTTLE
];
797 } else { // Deadband handling
798 // Always start positive. When coming from positive keep positive throttle within deadband
799 if ((throttlePrevious
>= (rxConfig
->midrc
+ flight3DConfig
->deadband3d_throttle
)) || !throttlePrevious
) {
800 throttleMax
= escAndServoConfig
->maxthrottle
;
801 throttle
= throttleMin
= flight3DConfig
->deadband3d_high
;
802 // When coming from negative. Keep negative throttle within deadband
803 } else if (throttlePrevious
<= (rxConfig
->midrc
- flight3DConfig
->deadband3d_throttle
)) {
804 throttle
= throttleMax
= flight3DConfig
->deadband3d_low
;
805 throttleMin
= escAndServoConfig
->minthrottle
;
809 throttle
= rcCommand
[THROTTLE
];
810 throttleMin
= escAndServoConfig
->minthrottle
;
811 throttleMax
= escAndServoConfig
->maxthrottle
;
814 throttleRange
= throttleMax
- throttleMin
;
816 if (rollPitchYawMixRange
> throttleRange
) {
817 motorLimitReached
= true;
818 float mixReduction
= (float) throttleRange
/ rollPitchYawMixRange
;
819 for (i
= 0; i
< motorCount
; i
++) {
820 rollPitchYawMix
[i
] = lrintf((float) rollPitchYawMix
[i
] * mixReduction
);
822 // Get the maximum correction by setting offset to center. Only active below 50% of saturation levels to reduce spazzing out in crashes
823 if ((mixReduction
> (mixerConfig
->airmode_saturation_limit
/ 100.0f
)) && IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
824 throttleMin
= throttleMax
= throttleMin
+ (throttleRange
/ 2);
827 motorLimitReached
= false;
828 throttleMin
= throttleMin
+ (rollPitchYawMixRange
/ 2);
829 throttleMax
= throttleMax
- (rollPitchYawMixRange
/ 2);
832 // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
833 // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
834 for (i
= 0; i
< motorCount
; i
++) {
835 motor
[i
] = rollPitchYawMix
[i
] + constrain(throttle
* currentMixer
[i
].throttle
, throttleMin
, throttleMax
);
837 if (isFailsafeActive
) {
838 motor
[i
] = constrain(motor
[i
], escAndServoConfig
->mincommand
, escAndServoConfig
->maxthrottle
);
839 } else if (feature(FEATURE_3D
)) {
840 if (throttle
>= flight3DConfig
->deadband3d_high
) {
841 motor
[i
] = constrain(motor
[i
], flight3DConfig
->deadband3d_high
, escAndServoConfig
->maxthrottle
);
843 motor
[i
] = constrain(motor
[i
], escAndServoConfig
->minthrottle
, flight3DConfig
->deadband3d_low
);
846 motor
[i
] = constrain(motor
[i
], escAndServoConfig
->minthrottle
, escAndServoConfig
->maxthrottle
);
849 // Motor stop handling
850 if (feature(FEATURE_MOTOR_STOP
) && ARMING_FLAG(ARMED
) && !feature(FEATURE_3D
) && !IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
851 if (((rcData
[THROTTLE
]) < rxConfig
->mincheck
)) {
852 motor
[i
] = escAndServoConfig
->mincommand
;
858 if (!ARMING_FLAG(ARMED
)) {
859 for (i
= 0; i
< motorCount
; i
++) {
860 motor
[i
] = motor_disarmed
[i
];
864 // motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
866 #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
868 // airplane / servo mixes
869 switch (currentMixerMode
) {
870 case MIXER_CUSTOM_AIRPLANE
:
871 case MIXER_FLYING_WING
:
874 case MIXER_CUSTOM_TRI
:
876 case MIXER_DUALCOPTER
:
877 case MIXER_SINGLECOPTER
:
884 servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
885 servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
893 // camera stabilization
894 if (feature(FEATURE_SERVO_TILT
)) {
895 // center at fixed position, or vary either pitch or roll by RC channel
896 servo
[SERVO_GIMBAL_PITCH
] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH
);
897 servo
[SERVO_GIMBAL_ROLL
] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL
);
899 if (IS_RC_MODE_ACTIVE(BOXCAMSTAB
)) {
900 if (gimbalConfig
->mode
== GIMBAL_MODE_MIXTILT
) {
901 servo
[SERVO_GIMBAL_PITCH
] -= (-(int32_t)servoConf
[SERVO_GIMBAL_PITCH
].rate
) * attitude
.values
.pitch
/ 50 - (int32_t)servoConf
[SERVO_GIMBAL_ROLL
].rate
* attitude
.values
.roll
/ 50;
902 servo
[SERVO_GIMBAL_ROLL
] += (-(int32_t)servoConf
[SERVO_GIMBAL_PITCH
].rate
) * attitude
.values
.pitch
/ 50 + (int32_t)servoConf
[SERVO_GIMBAL_ROLL
].rate
* attitude
.values
.roll
/ 50;
904 servo
[SERVO_GIMBAL_PITCH
] += (int32_t)servoConf
[SERVO_GIMBAL_PITCH
].rate
* attitude
.values
.pitch
/ 50;
905 servo
[SERVO_GIMBAL_ROLL
] += (int32_t)servoConf
[SERVO_GIMBAL_ROLL
].rate
* attitude
.values
.roll
/ 50;
911 for (i
= 0; i
< MAX_SUPPORTED_SERVOS
; i
++) {
912 servo
[i
] = constrain(servo
[i
], servoConf
[i
].min
, servoConf
[i
].max
); // limit the values
918 bool isMixerUsingServos(void)
924 void filterServos(void)
929 #if defined(MIXER_DEBUG)
930 uint32_t startTime
= micros();
933 if (mixerConfig
->servo_lowpass_enable
) {
934 for (servoIdx
= 0; servoIdx
< MAX_SUPPORTED_SERVOS
; servoIdx
++) {
935 servo
[servoIdx
] = (int16_t)lowpassFixed(&lowpassFilters
[servoIdx
], servo
[servoIdx
], mixerConfig
->servo_lowpass_freq
);
938 servo
[servoIdx
] = constrain(servo
[servoIdx
], servoConf
[servoIdx
].min
, servoConf
[servoIdx
].max
);
941 #if defined(MIXER_DEBUG)
942 debug
[0] = (int16_t)(micros() - startTime
);