?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Rework Airmode saturation // Slow down de-saturation process
[betaflight.git] / src / main / flight / mixer.c
blob9935ce7ad345a7097a9dfddeeed9a04c4d4af077
1 /*
2 * This file is part of Cleanflight.
3 *
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.
8 *
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.
13 *
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/>.
16 */
17
18 #include <stdbool.h>
19 #include <stdint.h>
20 #include <stdlib.h>
21 #include <string.h>
22 #include <math.h>
23
24 #include "platform.h"
25 #include "debug.h"
26
27 #include "build_config.h"
28
29 #include "common/axis.h"
30 #include "common/maths.h"
31 #include "common/filter.h"
32
33 #include "drivers/system.h"
34 #include "drivers/pwm_output.h"
35 #include "drivers/pwm_mapping.h"
36 #include "drivers/sensor.h"
37 #include "drivers/accgyro.h"
38 #include "drivers/system.h"
39 #include "drivers/gyro_sync.h"
40
41 #include "rx/rx.h"
42
43 #include "io/gimbal.h"
44 #include "io/escservo.h"
45 #include "io/rc_controls.h"
46
47 #include "sensors/sensors.h"
48 #include "sensors/acceleration.h"
49 #include "sensors/battery.h"
50
51 #include "flight/mixer.h"
52 #include "flight/failsafe.h"
53 #include "flight/pid.h"
54 #include "flight/imu.h"
55
56 #include "config/runtime_config.h"
57 #include "config/config.h"
58
59 uint8_t motorCount;
60
61 int16_t motor[MAX_SUPPORTED_MOTORS];
62 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
63
64 static mixerConfig_t *mixerConfig;
65 static flight3DConfig_t *flight3DConfig;
66 static escAndServoConfig_t *escAndServoConfig;
67 static airplaneConfig_t *airplaneConfig;
68 static rxConfig_t *rxConfig;
69
70 static mixerMode_e currentMixerMode;
71 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
72
73 #ifdef USE_SERVOS
74 static uint8_t servoRuleCount = 0;
75 static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
76 static gimbalConfig_t *gimbalConfig;
77 int16_t servo[MAX_SUPPORTED_SERVOS];
78 static int useServo;
79 STATIC_UNIT_TESTED uint8_t servoCount;
80 static servoParam_t *servoConf;
81 #endif
82
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
88 };
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
94 };
95
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
101 };
102
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
106 };
107
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
115 };
116
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
124 };
125
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
131 };
132
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
140 };
141
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
151 };
152
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
162 };
163
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
173 };
174
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
180 };
181
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
187 };
188
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
196 };
197
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
201 };
202
203 static const motorMixer_t mixerSingleProp[] = {
204 { 1.0f, 0.0f, 0.0f, 0.0f },
205 };
206
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
212 };
213
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 },
244 };
245 #endif
246
247 #ifdef USE_SERVOS
248
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 },
257 };
258
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 },
265 };
266
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 },
272 };
273
274 static const servoMixer_t servoMixerTri[] = {
275 { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 },
276 };
277
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 },
281 };
282
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 },
292 };
293
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 },
297 };
298
299
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
327 { 0, NULL },
328 };
329
330 static servoMixer_t *customServoMixers;
331 #endif
332
333
334 static motorMixer_t *customMixers;
335
336 void mixerUseConfigs(
337 #ifdef USE_SERVOS
338 servoParam_t *servoConfToUse,
339 gimbalConfig_t *gimbalConfigToUse,
340 #endif
341 flight3DConfig_t *flight3DConfigToUse,
342 escAndServoConfig_t *escAndServoConfigToUse,
343 mixerConfig_t *mixerConfigToUse,
344 airplaneConfig_t *airplaneConfigToUse,
345 rxConfig_t *rxConfigToUse)
346 {
347 #ifdef USE_SERVOS
348 servoConf = servoConfToUse;
349 gimbalConfig = gimbalConfigToUse;
350 #endif
351 flight3DConfig = flight3DConfigToUse;
352 escAndServoConfig = escAndServoConfigToUse;
353 mixerConfig = mixerConfigToUse;
354 airplaneConfig = airplaneConfigToUse;
355 rxConfig = rxConfigToUse;
356 }
357
358 #ifdef USE_SERVOS
359 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
360 {
361 uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
362
363 if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
364 return rcData[channelToForwardFrom];
365 }
366
367 return servoConf[servoIndex].middle;
368 }
369
370
371 int servoDirection(int servoIndex, int inputSource)
372 {
373 // determine the direction (reversed or not) from the direction bitfield of the servo
374 if (servoConf[servoIndex].reversedSources & (1 << inputSource))
375 return -1;
376 else
377 return 1;
378 }
379 #endif
380
381 #ifndef USE_QUAD_MIXER_ONLY
382
383 void loadCustomServoMixer(void)
384 {
385 uint8_t i;
386
387 // reset settings
388 servoRuleCount = 0;
389 memset(currentServoMixer, 0, sizeof(currentServoMixer));
390
391 // load custom mixer into currentServoMixer
392 for (i = 0; i < MAX_SERVO_RULES; i++) {
393 // check if done
394 if (customServoMixers[i].rate == 0)
395 break;
396
397 currentServoMixer[i] = customServoMixers[i];
398 servoRuleCount++;
399 }
400 }
401
402 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers)
403 {
404 currentMixerMode = mixerMode;
405
406 customMixers = initialCustomMotorMixers;
407 customServoMixers = initialCustomServoMixers;
408
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))
413 useServo = 1;
414
415 // give all servos a default command
416 for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
417 servo[i] = DEFAULT_SERVO_MIDDLE;
418 }
419 }
420
421 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
422 {
423 int i;
424
425 motorCount = 0;
426 servoCount = pwmOutputConfiguration->servoCount;
427
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++) {
431 // check if done
432 if (customMixers[i].throttle == 0.0f)
433 break;
434 currentMixer[i] = customMixers[i];
435 motorCount++;
436 }
437 } else {
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];
443 }
444 }
445
446 if (useServo) {
447 servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
448 if (servoMixers[currentMixerMode].rule) {
449 for (i = 0; i < servoRuleCount; i++)
450 currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
451 }
452 }
453
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;
461 }
462 }
463 }
464
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
469 ) {
470 ENABLE_STATE(FIXED_WING);
471
472 if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
473 loadCustomServoMixer();
474 }
475 } else {
476 DISABLE_STATE(FIXED_WING);
477
478 if (currentMixerMode == MIXER_CUSTOM_TRI) {
479 loadCustomServoMixer();
480 }
481 }
482
483 mixerResetDisarmedMotors();
484 }
485
486
487 void servoMixerLoadMix(int index, servoMixer_t *customServoMixers)
488 {
489 int i;
490
491 // we're 1-based
492 index++;
493 // clear existing
494 for (i = 0; i < MAX_SERVO_RULES; i++)
495 customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0;
496
497 for (i = 0; i < servoMixers[index].servoRuleCount; i++)
498 customServoMixers[i] = servoMixers[index].rule[i];
499 }
500
501 void mixerLoadMix(int index, motorMixer_t *customMixers)
502 {
503 int i;
504
505 // we're 1-based
506 index++;
507 // clear existing
508 for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
509 customMixers[i].throttle = 0.0f;
510
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];
515 }
516 }
517
518 #else
519
520 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
521 {
522 currentMixerMode = mixerMode;
523
524 customMixers = initialCustomMixers;
525 }
526
527 void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
528 {
529 UNUSED(pwmOutputConfiguration);
530 motorCount = 4;
531 #ifdef USE_SERVOS
532 servoCount = 0;
533 #endif
534
535 uint8_t i;
536 for (i = 0; i < motorCount; i++) {
537 currentMixer[i] = mixerQuadX[i];
538 }
539
540 mixerResetDisarmedMotors();
541 }
542 #endif
543
544 void mixerResetDisarmedMotors(void)
545 {
546 int i;
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;
550 }
551
552 #ifdef USE_SERVOS
553
554 STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
555 {
556 // start forwarding from this channel
557 uint8_t channelOffset = AUX1;
558
559 uint8_t servoOffset;
560 for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) {
561 pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
562 }
563 }
564
565 static void updateGimbalServos(uint8_t firstServoIndex)
566 {
567 pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
568 pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
569 }
570
571 void writeServos(void)
572 {
573 uint8_t servoIndex = 0;
574
575 switch (currentMixerMode) {
576 case MIXER_BICOPTER:
577 pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
578 pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
579 break;
580
581 case MIXER_TRI:
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]);
586 } else {
587 // otherwise, only move servo when copter is armed
588 if (ARMING_FLAG(ARMED))
589 pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
590 else
591 pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
592 }
593 break;
594
595 case MIXER_FLYING_WING:
596 pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
597 pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
598 break;
599
600 case MIXER_DUALCOPTER:
601 pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
602 pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
603 break;
604
605 case MIXER_CUSTOM_AIRPLANE:
606 case MIXER_AIRPLANE:
607 for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
608 pwmWriteServo(servoIndex++, servo[i]);
609 }
610 break;
611
612 case MIXER_SINGLECOPTER:
613 for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
614 pwmWriteServo(servoIndex++, servo[i]);
615 }
616 break;
617
618 default:
619 break;
620 }
621
622 // Two servos for SERVO_TILT, if enabled
623 if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
624 updateGimbalServos(servoIndex);
625 servoIndex += 2;
626 }
627
628 // forward AUX to remaining servo outputs (not constrained)
629 if (feature(FEATURE_CHANNEL_FORWARDING)) {
630 forwardAuxChannelsToServos(servoIndex);
631 servoIndex += MAX_AUX_CHANNEL_COUNT;
632 }
633 }
634 #endif
635
636 void writeMotors(void)
637 {
638 uint8_t i;
639
640 for (i = 0; i < motorCount; i++)
641 pwmWriteMotor(i, motor[i]);
642
643
644 if (feature(FEATURE_ONESHOT125)) {
645 pwmCompleteOneshotMotorUpdate(motorCount);
646 }
647 }
648
649 void writeAllMotors(int16_t mc)
650 {
651 uint8_t i;
652
653 // Sends commands to all motors
654 for (i = 0; i < motorCount; i++)
655 motor[i] = mc;
656 writeMotors();
657 }
658
659 void stopMotors(void)
660 {
661 writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand);
662
663 delay(50); // give the timers and ESCs a chance to react.
664 }
665
666 void StopPwmAllMotors()
667 {
668 pwmShutdownPulsesForAllMotors(motorCount);
669 }
670
671 #ifndef USE_QUAD_MIXER_ONLY
672 STATIC_UNIT_TESTED void servoMixer(void)
673 {
674 int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
675 static int16_t currentOutput[MAX_SERVO_RULES];
676 uint8_t i;
677
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];
683 } else {
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];
688
689 // Reverse yaw servo when inverted in 3D mode
690 if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
691 input[INPUT_STABILIZED_YAW] *= -1;
692 }
693 }
694
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);
697
698 input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]
699
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
704 // 1500 - 1500 = 0
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;
714
715 for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
716 servo[i] = 0;
717
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;
727
728 if (currentServoMixer[i].speed == 0)
729 currentOutput[i] = input[from];
730 else {
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]);
735 }
736
737 servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
738 } else {
739 currentOutput[i] = 0;
740 }
741 }
742
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);
746 }
747 }
748
749 #endif
750
751 void acroPlusApply(void) {
752 int axis;
753
754 for (axis = 0; axis < 2; axis++) {
755 int16_t factor;
756 q_number_t wowFactor;
757 int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
758 int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5;
759 int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
760 if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2;
761
762 /* acro plus factor handling */
763 if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
764 if (rcCommandDeflection > 0) {
765 rcCommandDeflection -= acroPlusStickOffset;
766 } else {
767 rcCommandDeflection += acroPlusStickOffset;
768 }
769 qConstruct(&wowFactor,ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500, Q12_NUMBER);
770 factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
771 wowFactor.num = wowFactor.den - wowFactor.num;
772 } else {
773 qConstruct(&wowFactor, 1, 1, Q12_NUMBER);
774 factor = 0;
775 }
776 axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
777 }
778 }
779
780 void mixTable(void)
781 {
782 uint32_t i;
783 q_number_t vbatCompensationFactor;
784 static q_number_t mixReduction;
785 uint8_t axis;
786
787 bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
788
789 if (motorLimitReached) {
790 for (axis = 0; axis < 3; axis++) axisPID[axis] *= constrain(qPercent(mixReduction), 10, 100) / 100;
791 if (debugMode == DEBUG_AIRMODE) debug[0] = qPercent(mixReduction);
792 }
793
794 if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
795 acroPlusApply();
796 }
797
798 if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
799 // prevent "yaw jump" during yaw correction
800 axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
801 }
802
803 // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
804 int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
805 int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
806 int16_t rollPitchYawMixMin = 0;
807
808 if (batteryConfig->vbatPidCompensation) vbatCompensationFactor = calculateVbatPidCompensation(); // Calculate voltage compensation
809
810 // Find roll/pitch/yaw desired output
811 for (i = 0; i < motorCount; i++) {
812 rollPitchYawMix[i] =
813 axisPID[PITCH] * currentMixer[i].pitch +
814 axisPID[ROLL] * currentMixer[i].roll +
815 -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
816
817 if (batteryConfig->vbatPidCompensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
818
819 if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
820 if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
821
822 if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i];
823 }
824
825 // Scale roll/pitch/yaw uniformly to fit within throttle range
826 int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
827 int16_t throttleRange, throttle;
828 int16_t throttleMin, throttleMax;
829 static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
830
831 // Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check
832 if (feature(FEATURE_3D)) {
833 if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
834
835 if ((rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
836 throttleMax = flight3DConfig->deadband3d_low;
837 throttleMin = escAndServoConfig->minthrottle;
838 throttlePrevious = throttle = rcData[THROTTLE];
839 } else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
840 throttleMax = escAndServoConfig->maxthrottle;
841 throttleMin = flight3DConfig->deadband3d_high;
842 throttlePrevious = throttle = rcData[THROTTLE];
843 } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
844 throttle = throttleMax = flight3DConfig->deadband3d_low;
845 throttleMin = escAndServoConfig->minthrottle;
846 } else { // Deadband handling from positive to negative
847 throttleMax = escAndServoConfig->maxthrottle;
848 throttle = throttleMin = flight3DConfig->deadband3d_high;
849 }
850 } else {
851 throttle = rcCommand[THROTTLE];
852 throttleMin = escAndServoConfig->minthrottle;
853 throttleMax = escAndServoConfig->maxthrottle;
854 }
855
856 throttleRange = throttleMax - throttleMin;
857
858 if (rollPitchYawMixRange > throttleRange) {
859 motorLimitReached = true;
860 qConstruct(&mixReduction, throttleRange, rollPitchYawMixRange, Q12_NUMBER);
861
862 for (i = 0; i < motorCount; i++) {
863 rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
864 }
865 // Get the maximum correction by setting offset to center
866 throttleMin = throttleMax = throttleMin + (throttleRange / 2);
867
868 if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
869 } else {
870 motorLimitReached = false;
871 throttleMin = throttleMin + (rollPitchYawMixRange / 2);
872 throttleMax = throttleMax - (rollPitchYawMixRange / 2);
873 }
874
875 // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
876 // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
877 for (i = 0; i < motorCount; i++) {
878 motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
879
880 if (isFailsafeActive) {
881 motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
882 } else if (feature(FEATURE_3D)) {
883 if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
884 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
885 } else {
886 motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
887 }
888 } else {
889 motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
890 }
891
892 // Motor stop handling
893 if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
894 if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
895 motor[i] = escAndServoConfig->mincommand;
896 }
897 }
898 }
899
900 // Disarmed mode
901 if (!ARMING_FLAG(ARMED)) {
902 for (i = 0; i < motorCount; i++) {
903 motor[i] = motor_disarmed[i];
904 }
905 }
906
907 // motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
908
909 #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
910
911 // airplane / servo mixes
912 switch (currentMixerMode) {
913 case MIXER_CUSTOM_AIRPLANE:
914 case MIXER_FLYING_WING:
915 case MIXER_AIRPLANE:
916 case MIXER_BICOPTER:
917 case MIXER_CUSTOM_TRI:
918 case MIXER_TRI:
919 case MIXER_DUALCOPTER:
920 case MIXER_SINGLECOPTER:
921 case MIXER_GIMBAL:
922 servoMixer();
923 break;
924
925 /*
926 case MIXER_GIMBAL:
927 servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
928 servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
929 break;
930 */
931
932 default:
933 break;
934 }
935
936 // camera stabilization
937 if (feature(FEATURE_SERVO_TILT)) {
938 // center at fixed position, or vary either pitch or roll by RC channel
939 servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
940 servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
941
942 if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
943 if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
944 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;
945 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;
946 } else {
947 servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
948 servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
949 }
950 }
951 }
952
953 // constrain servos
954 for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
955 servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
956 }
957 #endif
958 }
959
960 #ifdef USE_SERVOS
961 bool isMixerUsingServos(void)
962 {
963 return useServo;
964 }
965 #endif
966
967 void filterServos(void)
968 {
969 #ifdef USE_SERVOS
970 static int16_t servoIdx;
971 static bool servoFilterIsSet;
972 static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS];
973
974 #if defined(MIXER_DEBUG)
975 uint32_t startTime = micros();
976 #endif
977
978 if (mixerConfig->servo_lowpass_enable) {
979 for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
980 if (!servoFilterIsSet) {
981 BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime);
982 servoFilterIsSet = true;
983 }
984
985 servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx]));
986 // Sanity check
987 servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
988 }
989 }
990 #if defined(MIXER_DEBUG)
991 debug[0] = (int16_t)(micros() - startTime);
992 #endif
993
994 #endif
995 }
996