Halved motor_output_limit for 3D modes.
[betaflight.git] / src / main / flight / mixer.c
blobaace0fe4ca06836aded6b6540deda92589d3aeb9
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
24 #include <math.h>
26 #include "platform.h"
28 #include "build/build_config.h"
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/filter.h"
33 #include "common/maths.h"
35 #include "config/feature.h"
36 #include "pg/pg.h"
37 #include "pg/pg_ids.h"
38 #include "pg/rx.h"
40 #include "drivers/pwm_output.h"
41 #include "drivers/pwm_esc_detect.h"
42 #include "drivers/time.h"
43 #include "drivers/io.h"
45 #include "io/motors.h"
47 #include "fc/config.h"
48 #include "fc/controlrate_profile.h"
49 #include "fc/rc_controls.h"
50 #include "fc/rc_modes.h"
51 #include "fc/runtime_config.h"
52 #include "fc/core.h"
53 #include "fc/rc.h"
55 #include "flight/failsafe.h"
56 #include "flight/imu.h"
57 #include "flight/gps_rescue.h"
58 #include "flight/mixer.h"
59 #include "flight/mixer_tricopter.h"
60 #include "flight/pid.h"
62 #include "rx/rx.h"
64 #include "sensors/battery.h"
65 #include "sensors/gyro.h"
67 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
69 #define DYN_LPF_THROTTLE_STEPS 100
70 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
72 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
73 .mixerMode = DEFAULT_MIXER,
74 .yaw_motors_reversed = false,
75 .crashflip_motor_percent = 0,
78 PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
80 void pgResetFn_motorConfig(motorConfig_t *motorConfig)
82 #ifdef BRUSHED_MOTORS
83 motorConfig->minthrottle = 1000;
84 motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
85 motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
86 motorConfig->dev.useUnsyncedPwm = true;
87 #else
88 #ifdef USE_BRUSHED_ESC_AUTODETECT
89 if (hardwareMotorType == MOTOR_BRUSHED) {
90 motorConfig->minthrottle = 1000;
91 motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
92 motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
93 motorConfig->dev.useUnsyncedPwm = true;
94 } else
95 #endif
97 motorConfig->minthrottle = 1070;
98 motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
99 motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
101 #endif
102 motorConfig->maxthrottle = 2000;
103 motorConfig->mincommand = 1000;
104 motorConfig->digitalIdleOffsetValue = 550;
105 #ifdef USE_DSHOT_DMAR
106 motorConfig->dev.useBurstDshot = ENABLE_DSHOT_DMAR;
107 #endif
109 for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
110 motorConfig->dev.ioTags[motorIndex] = timerioTagGetByUsage(TIM_USE_MOTOR, motorIndex);
113 motorConfig->motorPoleCount = 14; // Most brushes motors that we use are 14 poles
116 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
118 #define PWM_RANGE_MID 1500
120 static FAST_RAM_ZERO_INIT uint8_t motorCount;
121 static FAST_RAM_ZERO_INIT float motorMixRange;
123 float FAST_RAM_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
124 float motor_disarmed[MAX_SUPPORTED_MOTORS];
126 mixerMode_e currentMixerMode;
127 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
129 #ifdef USE_LAUNCH_CONTROL
130 static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS];
131 #endif
133 static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
136 static const motorMixer_t mixerQuadX[] = {
137 { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
138 { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
139 { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
140 { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
142 #ifndef USE_QUAD_MIXER_ONLY
143 static const motorMixer_t mixerTricopter[] = {
144 { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
145 { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
146 { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT
149 static const motorMixer_t mixerQuadP[] = {
150 { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
151 { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT
152 { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
153 { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
156 #if defined(USE_UNCOMMON_MIXERS)
157 static const motorMixer_t mixerBicopter[] = {
158 { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
159 { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
161 #else
162 #define mixerBicopter NULL
163 #endif
165 static const motorMixer_t mixerY4[] = {
166 { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW
167 { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW
168 { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW
169 { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW
173 #if (MAX_SUPPORTED_MOTORS >= 6)
174 static const motorMixer_t mixerHex6X[] = {
175 { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R
176 { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R
177 { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L
178 { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L
179 { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
180 { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
183 #if defined(USE_UNCOMMON_MIXERS)
184 static const motorMixer_t mixerHex6H[] = {
185 { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
186 { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
187 { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
188 { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
189 { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT
190 { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT
193 static const motorMixer_t mixerHex6P[] = {
194 { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R
195 { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R
196 { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L
197 { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L
198 { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT
199 { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
201 static const motorMixer_t mixerY6[] = {
202 { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR
203 { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT
204 { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT
205 { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR
206 { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT
207 { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT
209 #else
210 #define mixerHex6H NULL
211 #define mixerHex6P NULL
212 #define mixerY6 NULL
213 #endif // USE_UNCOMMON_MIXERS
214 #else
215 #define mixerHex6X NULL
216 #endif // MAX_SUPPORTED_MOTORS >= 6
218 #if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8)
219 static const motorMixer_t mixerOctoX8[] = {
220 { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
221 { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
222 { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
223 { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
224 { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R
225 { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R
226 { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L
227 { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L
230 static const motorMixer_t mixerOctoFlatP[] = {
231 { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L
232 { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R
233 { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R
234 { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L
235 { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
236 { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
237 { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
238 { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT
241 static const motorMixer_t mixerOctoFlatX[] = {
242 { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L
243 { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R
244 { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R
245 { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L
246 { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L
247 { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R
248 { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R
249 { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L
251 #else
252 #define mixerOctoX8 NULL
253 #define mixerOctoFlatP NULL
254 #define mixerOctoFlatX NULL
255 #endif
257 static const motorMixer_t mixerVtail4[] = {
258 { 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R
259 { 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R
260 { 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L
261 { 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L
264 static const motorMixer_t mixerAtail4[] = {
265 { 1.0f, -0.58f, 0.58f, -1.0f }, // REAR_R
266 { 1.0f, -0.46f, -0.39f, 0.5f }, // FRONT_R
267 { 1.0f, 0.58f, 0.58f, 1.0f }, // REAR_L
268 { 1.0f, 0.46f, -0.39f, -0.5f }, // FRONT_L
271 #if defined(USE_UNCOMMON_MIXERS)
272 static const motorMixer_t mixerDualcopter[] = {
273 { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT
274 { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT
276 #else
277 #define mixerDualcopter NULL
278 #endif
280 static const motorMixer_t mixerSingleProp[] = {
281 { 1.0f, 0.0f, 0.0f, 0.0f },
284 static const motorMixer_t mixerQuadX1234[] = {
285 { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
286 { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R
287 { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
288 { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
291 // Keep synced with mixerMode_e
292 // Some of these entries are bogus when servos (USE_SERVOS) are not configured,
293 // but left untouched to keep ordinals synced with mixerMode_e (and configurator).
294 const mixer_t mixers[] = {
295 // motors, use servo, motor mixer
296 { 0, false, NULL }, // entry 0
297 { 3, true, mixerTricopter }, // MIXER_TRI
298 { 4, false, mixerQuadP }, // MIXER_QUADP
299 { 4, false, mixerQuadX }, // MIXER_QUADX
300 { 2, true, mixerBicopter }, // MIXER_BICOPTER
301 { 0, true, NULL }, // * MIXER_GIMBAL
302 { 6, false, mixerY6 }, // MIXER_Y6
303 { 6, false, mixerHex6P }, // MIXER_HEX6
304 { 1, true, mixerSingleProp }, // * MIXER_FLYING_WING
305 { 4, false, mixerY4 }, // MIXER_Y4
306 { 6, false, mixerHex6X }, // MIXER_HEX6X
307 { 8, false, mixerOctoX8 }, // MIXER_OCTOX8
308 { 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP
309 { 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX
310 { 1, true, mixerSingleProp }, // * MIXER_AIRPLANE
311 { 1, true, mixerSingleProp }, // * MIXER_HELI_120_CCPM
312 { 0, true, NULL }, // * MIXER_HELI_90_DEG
313 { 4, false, mixerVtail4 }, // MIXER_VTAIL4
314 { 6, false, mixerHex6H }, // MIXER_HEX6H
315 { 0, true, NULL }, // * MIXER_PPM_TO_SERVO
316 { 2, true, mixerDualcopter }, // MIXER_DUALCOPTER
317 { 1, true, NULL }, // MIXER_SINGLECOPTER
318 { 4, false, mixerAtail4 }, // MIXER_ATAIL4
319 { 0, false, NULL }, // MIXER_CUSTOM
320 { 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE
321 { 3, true, NULL }, // MIXER_CUSTOM_TRI
322 { 4, false, mixerQuadX1234 },
324 #endif // !USE_QUAD_MIXER_ONLY
326 FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow;
328 static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
329 static FAST_RAM_ZERO_INIT float rcCommandThrottleRange;
331 uint8_t getMotorCount(void)
333 return motorCount;
336 float getMotorMixRange(void)
338 return motorMixRange;
341 bool areMotorsRunning(void)
343 bool motorsRunning = false;
344 if (ARMING_FLAG(ARMED)) {
345 motorsRunning = true;
346 } else {
347 for (int i = 0; i < motorCount; i++) {
348 if (motor_disarmed[i] != disarmMotorOutput) {
349 motorsRunning = true;
351 break;
356 return motorsRunning;
359 #ifdef USE_SERVOS
360 bool mixerIsTricopter(void)
362 return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI);
364 #endif
366 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
367 // DSHOT scaling is done to the actual dshot range
368 void initEscEndpoints(void)
370 float motorOutputLimit = 1.0f;
371 if (currentPidProfile->motor_output_limit < 100) {
372 motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
375 // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
376 switch (motorConfig()->dev.motorPwmProtocol) {
377 #ifdef USE_DSHOT
378 case PWM_TYPE_PROSHOT1000:
379 case PWM_TYPE_DSHOT1200:
380 case PWM_TYPE_DSHOT600:
381 case PWM_TYPE_DSHOT300:
382 case PWM_TYPE_DSHOT150:
384 float outputLimitOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) * (1 - motorOutputLimit);
385 disarmMotorOutput = DSHOT_CMD_MOTOR_STOP;
386 if (featureIsEnabled(FEATURE_3D)) {
387 motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
388 motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset / 2;
389 deadbandMotor3dHigh = DSHOT_3D_FORWARD_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_3D_FORWARD_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
390 deadbandMotor3dLow = DSHOT_3D_FORWARD_MIN_THROTTLE - 1 - outputLimitOffset / 2;
391 } else {
392 motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
393 motorOutputHigh = DSHOT_MAX_THROTTLE - outputLimitOffset;
397 break;
398 #endif
399 default:
400 if (featureIsEnabled(FEATURE_3D)) {
401 float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - motorOutputLimit) / 2;
402 disarmMotorOutput = flight3DConfig()->neutral3d;
403 motorOutputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
404 motorOutputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
405 deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
406 deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
407 } else {
408 disarmMotorOutput = motorConfig()->mincommand;
409 motorOutputLow = motorConfig()->minthrottle;
410 motorOutputHigh = motorConfig()->maxthrottle - ((motorConfig()->maxthrottle - motorConfig()->minthrottle) * (1 - motorOutputLimit));
412 break;
415 rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN;
418 void mixerInit(mixerMode_e mixerMode)
420 currentMixerMode = mixerMode;
422 initEscEndpoints();
423 #ifdef USE_SERVOS
424 if (mixerIsTricopter()) {
425 mixerTricopterInit();
427 #endif
430 #ifdef USE_LAUNCH_CONTROL
431 // Create a custom mixer for launch control based on the current settings
432 // but disable the front motors. We don't care about roll or yaw because they
433 // are limited in the PID controller.
434 void loadLaunchControlMixer(void)
436 for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
437 launchControlMixer[i] = currentMixer[i];
438 // limit the front motors to minimum output
439 if (launchControlMixer[i].pitch < 0.0f) {
440 launchControlMixer[i].pitch = 0.0f;
441 launchControlMixer[i].throttle = 0.0f;
445 #endif
447 #ifndef USE_QUAD_MIXER_ONLY
449 void mixerConfigureOutput(void)
451 motorCount = 0;
453 if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
454 // load custom mixer into currentMixer
455 for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
456 // check if done
457 if (customMotorMixer(i)->throttle == 0.0f) {
458 break;
460 currentMixer[i] = *customMotorMixer(i);
461 motorCount++;
463 } else {
464 motorCount = mixers[currentMixerMode].motorCount;
465 if (motorCount > MAX_SUPPORTED_MOTORS) {
466 motorCount = MAX_SUPPORTED_MOTORS;
468 // copy motor-based mixers
469 if (mixers[currentMixerMode].motor) {
470 for (int i = 0; i < motorCount; i++)
471 currentMixer[i] = mixers[currentMixerMode].motor[i];
474 #ifdef USE_LAUNCH_CONTROL
475 loadLaunchControlMixer();
476 #endif
477 mixerResetDisarmedMotors();
480 void mixerLoadMix(int index, motorMixer_t *customMixers)
482 // we're 1-based
483 index++;
484 // clear existing
485 for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
486 customMixers[i].throttle = 0.0f;
488 // do we have anything here to begin with?
489 if (mixers[index].motor != NULL) {
490 for (int i = 0; i < mixers[index].motorCount; i++) {
491 customMixers[i] = mixers[index].motor[i];
495 #else
496 void mixerConfigureOutput(void)
498 motorCount = QUAD_MOTOR_COUNT;
499 for (int i = 0; i < motorCount; i++) {
500 currentMixer[i] = mixerQuadX[i];
502 #ifdef USE_LAUNCH_CONTROL
503 loadLaunchControlMixer();
504 #endif
505 mixerResetDisarmedMotors();
507 #endif // USE_QUAD_MIXER_ONLY
509 void mixerResetDisarmedMotors(void)
511 // set disarmed motor values
512 for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
513 motor_disarmed[i] = disarmMotorOutput;
517 void writeMotors(void)
519 if (pwmAreMotorsEnabled()) {
520 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
521 pwmStartMotorUpdate(motorCount);
522 #endif
523 for (int i = 0; i < motorCount; i++) {
524 pwmWriteMotor(i, motor[i]);
526 pwmCompleteMotorUpdate(motorCount);
530 static void writeAllMotors(int16_t mc)
532 // Sends commands to all motors
533 for (int i = 0; i < motorCount; i++) {
534 motor[i] = mc;
536 writeMotors();
539 void stopMotors(void)
541 writeAllMotors(disarmMotorOutput);
542 delay(50); // give the timers and ESCs a chance to react.
545 void stopPwmAllMotors(void)
547 pwmShutdownPulsesForAllMotors(motorCount);
548 delayMicroseconds(1500);
551 static FAST_RAM_ZERO_INIT float throttle = 0;
552 static FAST_RAM_ZERO_INIT float loggingThrottle = 0;
553 static FAST_RAM_ZERO_INIT float motorOutputMin;
554 static FAST_RAM_ZERO_INIT float motorRangeMin;
555 static FAST_RAM_ZERO_INIT float motorRangeMax;
556 static FAST_RAM_ZERO_INIT float motorOutputRange;
557 static FAST_RAM_ZERO_INIT int8_t motorOutputMixSign;
559 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
561 static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions
562 static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode
563 float currentThrottleInputRange = 0;
565 if (featureIsEnabled(FEATURE_3D)) {
566 uint16_t rcCommand3dDeadBandLow;
567 uint16_t rcCommand3dDeadBandHigh;
569 if (!ARMING_FLAG(ARMED)) {
570 rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
573 if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
574 // The min_check range is halved because the output throttle is scaled to 500us.
575 // So by using half of min_check we maintain the same low-throttle deadband
576 // stick travel as normal non-3D mode.
577 const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2;
578 rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset;
579 rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset;
580 } else {
581 rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
582 rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
585 const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
586 const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
588 if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) {
589 // INVERTED
590 motorRangeMin = motorOutputLow;
591 motorRangeMax = deadbandMotor3dLow;
592 if (isMotorProtocolDshot()) {
593 motorOutputMin = motorOutputLow;
594 motorOutputRange = deadbandMotor3dLow - motorOutputLow;
595 } else {
596 motorOutputMin = deadbandMotor3dLow;
597 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
599 if (motorOutputMixSign != -1) {
600 reversalTimeUs = currentTimeUs;
602 motorOutputMixSign = -1;
603 rcThrottlePrevious = rcCommand[THROTTLE];
604 throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
605 currentThrottleInputRange = rcCommandThrottleRange3dLow;
606 } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
607 // NORMAL
608 motorRangeMin = deadbandMotor3dHigh;
609 motorRangeMax = motorOutputHigh;
610 motorOutputMin = deadbandMotor3dHigh;
611 motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
612 if (motorOutputMixSign != 1) {
613 reversalTimeUs = currentTimeUs;
615 motorOutputMixSign = 1;
616 rcThrottlePrevious = rcCommand[THROTTLE];
617 throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
618 currentThrottleInputRange = rcCommandThrottleRange3dHigh;
619 } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow &&
620 !flight3DConfigMutable()->switched_mode3d) ||
621 isMotorsReversed()) {
622 // INVERTED_TO_DEADBAND
623 motorRangeMin = motorOutputLow;
624 motorRangeMax = deadbandMotor3dLow;
625 if (isMotorProtocolDshot()) {
626 motorOutputMin = motorOutputLow;
627 motorOutputRange = deadbandMotor3dLow - motorOutputLow;
628 } else {
629 motorOutputMin = deadbandMotor3dLow;
630 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
632 if (motorOutputMixSign != -1) {
633 reversalTimeUs = currentTimeUs;
635 motorOutputMixSign = -1;
636 throttle = 0;
637 currentThrottleInputRange = rcCommandThrottleRange3dLow;
638 } else {
639 // NORMAL_TO_DEADBAND
640 motorRangeMin = deadbandMotor3dHigh;
641 motorRangeMax = motorOutputHigh;
642 motorOutputMin = deadbandMotor3dHigh;
643 motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
644 if (motorOutputMixSign != 1) {
645 reversalTimeUs = currentTimeUs;
647 motorOutputMixSign = 1;
648 throttle = 0;
649 currentThrottleInputRange = rcCommandThrottleRange3dHigh;
651 if (currentTimeUs - reversalTimeUs < 250000) {
652 // keep iterm zero for 250ms after motor reversal
653 pidResetIterm();
655 } else {
656 throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
657 currentThrottleInputRange = rcCommandThrottleRange;
658 motorRangeMin = motorOutputLow;
659 motorRangeMax = motorOutputHigh;
660 motorOutputMin = motorOutputLow;
661 motorOutputRange = motorOutputHigh - motorOutputLow;
662 motorOutputMixSign = 1;
665 throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
668 #define CRASH_FLIP_DEADBAND 20
669 #define CRASH_FLIP_STICK_MINF 0.15f
671 static void applyFlipOverAfterCrashModeToMotors(void)
673 if (ARMING_FLAG(ARMED)) {
674 float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH);
675 float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL);
676 float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW);
677 float signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1;
678 float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1;
679 float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1);
681 float stickDeflectionLength = sqrtf(stickDeflectionPitchAbs*stickDeflectionPitchAbs + stickDeflectionRollAbs*stickDeflectionRollAbs);
683 if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
684 // If yaw is the dominant, disable pitch and roll
685 stickDeflectionLength = stickDeflectionYawAbs;
686 signRoll = 0;
687 signPitch = 0;
688 } else {
689 // If pitch/roll dominant, disable yaw
690 signYaw = 0;
693 float cosPhi = (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength);
694 const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
696 if (cosPhi < cosThreshold) {
697 // Enforce either roll or pitch exclusively, if not on diagonal
698 if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
699 signPitch = 0;
700 } else {
701 signRoll = 0;
705 // Apply a reasonable amount of stick deadband
706 const float flipStickRange = 1.0f - CRASH_FLIP_STICK_MINF;
707 float flipPower = MAX(0.0f, stickDeflectionLength - CRASH_FLIP_STICK_MINF) / flipStickRange;
709 for (int i = 0; i < motorCount; ++i) {
710 float motorOutputNormalised =
711 signPitch*currentMixer[i].pitch +
712 signRoll*currentMixer[i].roll +
713 signYaw*currentMixer[i].yaw;
715 if (motorOutputNormalised < 0) {
716 if (mixerConfig()->crashflip_motor_percent > 0) {
717 motorOutputNormalised = -motorOutputNormalised * (float)mixerConfig()->crashflip_motor_percent / 100.0f;
718 } else {
719 motorOutputNormalised = 0;
722 motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
723 float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange;
725 // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
726 motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND);
728 motor[i] = motorOutput;
730 } else {
731 // Disarmed mode
732 for (int i = 0; i < motorCount; i++) {
733 motor[i] = motor_disarmed[i];
738 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
740 // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
741 // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
742 for (int i = 0; i < motorCount; i++) {
743 float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
744 #ifdef USE_THRUST_LINEARIZATION
745 motorOutput = pidApplyThrustLinearization(motorOutput);
746 #endif
747 motorOutput = motorOutputMin + motorOutputRange * motorOutput;
749 #ifdef USE_SERVOS
750 if (mixerIsTricopter()) {
751 motorOutput += mixerTricopterMotorCorrection(i);
753 #endif
754 if (failsafeIsActive()) {
755 if (isMotorProtocolDshot()) {
756 motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
758 motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
759 } else {
760 motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
762 motor[i] = motorOutput;
765 // Disarmed mode
766 if (!ARMING_FLAG(ARMED)) {
767 for (int i = 0; i < motorCount; i++) {
768 motor[i] = motor_disarmed[i];
773 float applyThrottleLimit(float throttle)
775 if (currentControlRateProfile->throttle_limit_percent < 100) {
776 const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
777 switch (currentControlRateProfile->throttle_limit_type) {
778 case THROTTLE_LIMIT_TYPE_SCALE:
779 return throttle * throttleLimitFactor;
780 case THROTTLE_LIMIT_TYPE_CLIP:
781 return MIN(throttle, throttleLimitFactor);
785 return throttle;
788 void applyMotorStop(void)
790 for (int i = 0; i < motorCount; i++) {
791 motor[i] = disarmMotorOutput;
795 #ifdef USE_DYN_LPF
796 void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
798 static timeUs_t lastDynLpfUpdateUs = 0;
799 static int dynLpfPreviousQuantizedThrottle = -1; // to allow an initial zero throttle to set the filter cutoff
801 if (cmpTimeUs(currentTimeUs, lastDynLpfUpdateUs) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US) {
802 const int quantizedThrottle = lrintf(throttle * DYN_LPF_THROTTLE_STEPS); // quantize the throttle reduce the number of filter updates
803 if (quantizedThrottle != dynLpfPreviousQuantizedThrottle) {
804 // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable
805 const float dynLpfThrottle = (float)quantizedThrottle / DYN_LPF_THROTTLE_STEPS;
806 dynLpfGyroUpdate(dynLpfThrottle);
807 dynLpfDTermUpdate(dynLpfThrottle);
808 dynLpfPreviousQuantizedThrottle = quantizedThrottle;
809 lastDynLpfUpdateUs = currentTimeUs;
813 #endif
815 FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
817 // Find min and max throttle based on conditions. Throttle has to be known before mixing
818 calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
820 if (isFlipOverAfterCrashActive()) {
821 applyFlipOverAfterCrashModeToMotors();
823 return;
826 const bool launchControlActive = isLaunchControlActive();
828 motorMixer_t * activeMixer = &currentMixer[0];
829 #ifdef USE_LAUNCH_CONTROL
830 if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
831 activeMixer = &launchControlMixer[0];
833 #endif
835 // Calculate and Limit the PID sum
836 const float scaledAxisPidRoll =
837 constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
838 const float scaledAxisPidPitch =
839 constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
841 uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
843 #ifdef USE_YAW_SPIN_RECOVERY
844 const bool yawSpinDetected = gyroYawSpinDetected();
845 if (yawSpinDetected) {
846 yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
848 #endif // USE_YAW_SPIN_RECOVERY
850 float scaledAxisPidYaw =
851 constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
853 if (!mixerConfig()->yaw_motors_reversed) {
854 scaledAxisPidYaw = -scaledAxisPidYaw;
857 // Calculate voltage compensation
858 const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
860 // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
861 if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) {
862 throttle = applyThrottleLimit(throttle);
865 const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
867 #ifdef USE_YAW_SPIN_RECOVERY
868 // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
869 // When airmode is active the throttle setting doesn't impact recovery authority.
870 if (yawSpinDetected && !airmodeEnabled) {
871 throttle = 0.5f; //
873 #endif // USE_YAW_SPIN_RECOVERY
875 #ifdef USE_LAUNCH_CONTROL
876 // While launch control is active keep the throttle at minimum.
877 // Once the pilot triggers the launch throttle control will be reactivated.
878 if (launchControlActive) {
879 throttle = 0.0f;
881 #endif
883 // Find roll/pitch/yaw desired output
884 float motorMix[MAX_SUPPORTED_MOTORS];
885 float motorMixMax = 0, motorMixMin = 0;
886 for (int i = 0; i < motorCount; i++) {
888 float mix =
889 scaledAxisPidRoll * activeMixer[i].roll +
890 scaledAxisPidPitch * activeMixer[i].pitch +
891 scaledAxisPidYaw * activeMixer[i].yaw;
893 mix *= vbatCompensationFactor; // Add voltage compensation
895 if (mix > motorMixMax) {
896 motorMixMax = mix;
897 } else if (mix < motorMixMin) {
898 motorMixMin = mix;
900 motorMix[i] = mix;
903 pidUpdateAntiGravityThrottleFilter(throttle);
905 #ifdef USE_DYN_LPF
906 updateDynLpfCutoffs(currentTimeUs, throttle);
907 #endif
909 #ifdef USE_THRUST_LINEARIZATION
910 // reestablish old throttle stick feel by counter compensating thrust linearization
911 throttle = pidCompensateThrustLinearization(throttle);
912 #endif
914 #if defined(USE_THROTTLE_BOOST)
915 if (throttleBoost > 0.0f) {
916 const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
917 throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
919 #endif
921 #ifdef USE_GPS_RESCUE
922 // If gps rescue is active then override the throttle. This prevents things
923 // like throttle boost or throttle limit from negatively affecting the throttle.
924 if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
925 throttle = gpsRescueGetThrottle();
927 #endif
929 loggingThrottle = throttle;
930 motorMixRange = motorMixMax - motorMixMin;
931 if (motorMixRange > 1.0f) {
932 for (int i = 0; i < motorCount; i++) {
933 motorMix[i] /= motorMixRange;
935 // Get the maximum correction by setting offset to center when airmode enabled
936 if (airmodeEnabled) {
937 throttle = 0.5f;
939 } else {
940 if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
941 throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
945 if (featureIsEnabled(FEATURE_MOTOR_STOP)
946 && ARMING_FLAG(ARMED)
947 && !featureIsEnabled(FEATURE_3D)
948 && !airmodeEnabled
949 && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable motor_stop while GPS Rescue is active
950 && (rcData[THROTTLE] < rxConfig()->mincheck)) {
951 // motor_stop handling
952 applyMotorStop();
953 } else {
954 // Apply the mix to motor endpoints
955 applyMixToMotors(motorMix, activeMixer);
959 float convertExternalToMotor(uint16_t externalValue)
961 uint16_t motorValue;
962 switch ((int)isMotorProtocolDshot()) {
963 #ifdef USE_DSHOT
964 case true:
965 externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX);
967 if (featureIsEnabled(FEATURE_3D)) {
968 if (externalValue == PWM_RANGE_MID) {
969 motorValue = DSHOT_CMD_MOTOR_STOP;
970 } else if (externalValue < PWM_RANGE_MID) {
971 motorValue = scaleRange(externalValue, PWM_RANGE_MIN, PWM_RANGE_MID - 1, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, DSHOT_MIN_THROTTLE);
972 } else {
973 motorValue = scaleRange(externalValue, PWM_RANGE_MID + 1, PWM_RANGE_MAX, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
975 } else {
976 motorValue = (externalValue == PWM_RANGE_MIN) ? DSHOT_CMD_MOTOR_STOP : scaleRange(externalValue, PWM_RANGE_MIN + 1, PWM_RANGE_MAX, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
979 break;
980 case false:
981 #endif
982 default:
983 motorValue = externalValue;
984 break;
987 return (float)motorValue;
990 uint16_t convertMotorToExternal(float motorValue)
992 uint16_t externalValue;
993 switch ((int)isMotorProtocolDshot()) {
994 #ifdef USE_DSHOT
995 case true:
996 if (featureIsEnabled(FEATURE_3D)) {
997 if (motorValue == DSHOT_CMD_MOTOR_STOP || motorValue < DSHOT_MIN_THROTTLE) {
998 externalValue = PWM_RANGE_MID;
999 } else if (motorValue <= DSHOT_3D_FORWARD_MIN_THROTTLE - 1) {
1000 externalValue = scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_FORWARD_MIN_THROTTLE - 1, PWM_RANGE_MID - 1, PWM_RANGE_MIN);
1001 } else {
1002 externalValue = scaleRange(motorValue, DSHOT_3D_FORWARD_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MID + 1, PWM_RANGE_MAX);
1004 } else {
1005 externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX);
1007 break;
1008 case false:
1009 #endif
1010 default:
1011 externalValue = motorValue;
1012 break;
1015 return externalValue;
1018 void mixerSetThrottleAngleCorrection(int correctionValue)
1020 throttleAngleCorrection = correctionValue;
1023 float mixerGetLoggingThrottle(void)
1025 return loggingThrottle;