src/main/flight/mixer.c

   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 "build/debug.h"
  26
  27 #include "build/build_config.h"
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31 #include "common/filter.h"
  32
  33 #include "config/parameter_group.h"
  34
  35 #include "drivers/system.h"
  36 #include "drivers/pwm_output.h"
  37 #include "drivers/pwm_mapping.h"
  38 #include "drivers/sensor.h"
  39 #include "drivers/accgyro.h"
  40 #include "drivers/system.h"
  41
  42 #include "rx/rx.h"
  43
  44 #include "io/motors.h"
  45
  46 #include "fc/rc_controls.h"
  47
  48 #include "sensors/sensors.h"
  49 #include "sensors/acceleration.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/parameter_group_ids.h"
  57 #include "config/feature.h"
  58 #include "config/config_reset.h"
  59
  60 #include "fc/runtime_config.h"
  61 #include "fc/config.h"
  62
  63 //#define MIXER_DEBUG
  64
  65 uint8_t motorCount;
  66
  67 int16_t motor[MAX_SUPPORTED_MOTORS];
  68 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
  69
  70 bool motorLimitReached;
  71
  72 motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  73
  74 PG_REGISTER_ARR(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
  75 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
  76 PG_REGISTER_WITH_RESET_TEMPLATE(motor3DConfig_t, motor3DConfig, PG_MOTOR_3D_CONFIG, 0);
  77
  78 PG_RESET_TEMPLATE(motor3DConfig_t, motor3DConfig,
  79     .deadband3d_low = 1406,
  80     .deadband3d_high = 1514,
  81     .neutral3d = 1460,
  82 );
  83
  84
  85 #ifdef USE_SERVOS
  86 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  87     .mixerMode = MIXER_QUADX,
  88     .pid_at_min_throttle = 1,
  89     .yaw_motor_direction = 1,
  90     .yaw_jump_prevention_limit = 200,
  91
  92     .tri_unarmed_servo = 1,
  93     .servo_lowpass_freq = 400.0f,
  94 );
  95 #else
  96 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  97     .mixerMode = MIXER_QUADX,
  98     .pid_at_min_throttle = 1,
  99     .yaw_motor_direction = 1,
 100     .yaw_jump_prevention_limit = 200,
 101 );
 102 #endif
 103
 104 /* QuadX
 105 4CW   2CCW
 106    \ /
 107     X
 108    / \
 109 3CCW  1CW
 110 */
 111 static const motorMixer_t mixerQuadX[] = {
 112 //throttle,  roll, pitch,   yaw
 113     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R  (M1)
 114     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R (M2)
 115     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L  (M3)
 116     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L (M4)
 117 };
 118
 119 #ifndef USE_QUAD_MIXER_ONLY
 120 /* QuadP
 121     4CW
 122      |
 123 3CCW-+-2CCW
 124      |
 125     1CW
 126 */
 127 static const motorMixer_t mixerQuadP[] = {
 128     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
 129     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
 130     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
 131     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
 132 };
 133
 134 /* Vtail4
 135 4CCW-----2CW
 136      ||
 137      ||
 138 3CW\ || /1CCW
 139     \||/
 140 */
 141 static const motorMixer_t mixerVtail4[] = {
 142     { 1.0f, -0.58f,  0.58f,  1.0f },        // REAR_R
 143     { 1.0f, -0.46f, -0.39f, -0.5f },        // FRONT_R
 144     { 1.0f,  0.58f,  0.58f, -1.0f },        // REAR_L
 145     { 1.0f,  0.46f, -0.39f,  0.5f },        // FRONT_L
 146 };
 147
 148 /* Atail4
 149  4CW----2CCW
 150      ||
 151      ||
 152      /\
 153 3CCW/  \1CW
 154 */
 155 static const motorMixer_t mixerAtail4[] = {
 156     { 1.0f, -0.58f,  0.58f, -1.0f },        // REAR_R
 157     { 1.0f, -0.46f, -0.39f,  0.5f },        // FRONT_R
 158     { 1.0f,  0.58f,  0.58f,  1.0f },        // REAR_L
 159     { 1.0f,  0.46f, -0.39f, -0.5f },        // FRONT_L
 160 };
 161
 162 /* Y4
 163  4CW----2CCW
 164      ||
 165      ||
 166     1CW
 167     3CCW
 168 */
 169 static const motorMixer_t mixerY4[] = {
 170     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
 171     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
 172     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
 173     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
 174 };
 175
 176 #if (MAX_SUPPORTED_MOTORS >= 6)
 177 static const motorMixer_t mixerY6[] = {
 178     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 179     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 180     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 181     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 182     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 183     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 184 };
 185
 186 static const motorMixer_t mixerHex6H[] = {
 187     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 188     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 189     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 190     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 191     { 1.0f,  0.0f,  0.0f,  0.0f },          // RIGHT
 192     { 1.0f,  0.0f,  0.0f,  0.0f },          // LEFT
 193 };
 194
 195 static const motorMixer_t mixerHex6P[] = {
 196     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 197     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 198     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 199     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 200     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 201     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 202 };
 203
 204 static const motorMixer_t mixerHex6X[] = {
 205     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 206     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 207     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 208     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 209     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 210     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 211 };
 212 #endif
 213
 214 #if (MAX_SUPPORTED_MOTORS >= 8)
 215 static const motorMixer_t mixerOctoX8[] = {
 216     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 217     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 218     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 219     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 220     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 221     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 222     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 223     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 224 };
 225
 226 static const motorMixer_t mixerOctoFlatP[] = {
 227     { 1.0f,  0.707107f, -0.707107f,  1.0f },// FRONT_L
 228     { 1.0f, -0.707107f, -0.707107f,  1.0f },// FRONT_R
 229     { 1.0f, -0.707107f,  0.707107f,  1.0f },// REAR_R
 230     { 1.0f,  0.707107f,  0.707107f,  1.0f },// REAR_L
 231     { 1.0f,  0.0f,      -1.0f,      -1.0f },// FRONT
 232     { 1.0f, -1.0f,       0.0f,      -1.0f },// RIGHT
 233     { 1.0f,  0.0f,       1.0f,      -1.0f },// REAR
 234     { 1.0f,  1.0f,       0.0f,      -1.0f },// LEFT
 235 };
 236
 237 static const motorMixer_t mixerOctoFlatX[] = {
 238     { 1.0f,  1.0f,      -0.414178f,  1.0f },// MIDFRONT_L
 239     { 1.0f, -0.414178f, -1.0f,       1.0f },// FRONT_R
 240     { 1.0f, -1.0f,       0.414178f,  1.0f },// MIDREAR_R
 241     { 1.0f,  0.414178f,  1.0f,       1.0f },// REAR_L
 242     { 1.0f,  0.414178f, -1.0f,      -1.0f },// FRONT_L
 243     { 1.0f, -1.0f,      -0.414178f, -1.0f },// MIDFRONT_R
 244     { 1.0f, -0.414178f,  1.0f,      -1.0f },// REAR_R
 245     { 1.0f,  1.0f,       0.414178f, -1.0f },// MIDREAR_L
 246 };
 247 #endif
 248
 249 static const motorMixer_t mixerSingleProp[] = {
 250     { 1.0f,  0.0f,  0.0f, 0.0f },
 251 };
 252
 253 static const motorMixer_t mixerDualcopter[] = {
 254     { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 255     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 256 };
 257
 258 static const motorMixer_t mixerBicopter[] = {
 259     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
 260     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
 261 };
 262
 263 static const motorMixer_t mixerTricopter[] = {
 264     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
 265     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
 266     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
 267 };
 268
 269 // Keep synced with mixerMode_e
 270 const mixer_t mixers[] = {
 271     // motors, use servo, motor mixer
 272     { 0, false, NULL },                // entry 0
 273     { 3, true,  mixerTricopter },      // MIXER_TRI
 274     { 4, false, mixerQuadP },          // MIXER_QUADP
 275     { 4, false, mixerQuadX },          // MIXER_QUADX
 276     { 2, true,  mixerBicopter },       // MIXER_BICOPTER
 277     { 0, true,  NULL },                // * MIXER_GIMBAL
 278 #if (MAX_SUPPORTED_MOTORS >= 6)
 279     { 6, false, mixerY6 },             // MIXER_Y6
 280     { 6, false, mixerHex6P },          // MIXER_HEX6
 281 #else
 282     { 6, false, NULL },                // MIXER_Y6
 283     { 6, false, NULL },                // MIXER_HEX6
 284 #endif
 285     { 1, true,  mixerSingleProp },     // * MIXER_FLYING_WING
 286     { 4, false, mixerY4 },             // MIXER_Y4
 287 #if (MAX_SUPPORTED_MOTORS >= 6)
 288     { 6, false, mixerHex6X },          // MIXER_HEX6X
 289 #else
 290     { 6, false, NULL },                // MIXER_HEX6X
 291 #endif
 292 #if (MAX_SUPPORTED_MOTORS >= 8)
 293     { 8, false, mixerOctoX8 },         // MIXER_OCTOX8
 294     { 8, false, mixerOctoFlatP },      // MIXER_OCTOFLATP
 295     { 8, false, mixerOctoFlatX },      // MIXER_OCTOFLATX
 296 #else
 297     { 8, false, NULL },                // MIXER_OCTOX8
 298     { 8, false, NULL },                // MIXER_OCTOFLATP
 299     { 8, false, NULL },                // MIXER_OCTOFLATX
 300 #endif
 301     { 1, true,  mixerSingleProp },     // * MIXER_AIRPLANE
 302     { 0, true,  NULL },                // * MIXER_HELI_120_CCPM
 303     { 0, true,  NULL },                // * MIXER_HELI_90_DEG
 304     { 4, false, mixerVtail4 },         // MIXER_VTAIL4
 305 #if (MAX_SUPPORTED_MOTORS >= 6)
 306     { 6, false, mixerHex6H },          // MIXER_HEX6H
 307 #else
 308     { 6, false, NULL },                // MIXER_HEX6H
 309 #endif
 310     { 0, true,  NULL },                // * MIXER_PPM_TO_SERVO
 311     { 2, true,  mixerDualcopter },     // MIXER_DUALCOPTER
 312     { 1, true,  NULL },                // MIXER_SINGLECOPTER
 313     { 4, false, mixerAtail4 },         // MIXER_ATAIL4
 314     { 0, false, NULL },                // MIXER_CUSTOM
 315     { 2, true,  NULL },                // MIXER_CUSTOM_AIRPLANE
 316     { 3, true,  NULL },                // MIXER_CUSTOM_TRI
 317 };
 318 #endif
 319
 320 motorMixer_t *customMixers;
 321
 322 void mixerInit(motorMixer_t *initialCustomMixers)
 323 {
 324     customMixers = initialCustomMixers;
 325 }
 326
 327 #if !defined(USE_SERVOS) || defined(USE_QUAD_MIXER_ONLY)
 328 void mixerUsePWMIOConfiguration(pwmIOConfiguration_t *pwmIOConfiguration)
 329 {
 330     UNUSED(pwmIOConfiguration);
 331     motorCount = 4;
 332     uint8_t i;
 333     for (i = 0; i < motorCount; i++) {
 334         currentMixer[i] = mixerQuadX[i];
 335     }
 336     mixerResetDisarmedMotors();
 337 }
 338 #endif
 339
 340
 341 #ifndef USE_QUAD_MIXER_ONLY
 342 void mixerLoadMix(int index, motorMixer_t *customMixers)
 343 {
 344     int i;
 345
 346     // we're 1-based
 347     index++;
 348     // clear existing
 349     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 350         customMixers[i].throttle = 0.0f;
 351
 352     // do we have anything here to begin with?
 353     if (mixers[index].motor != NULL) {
 354         for (i = 0; i < mixers[index].motorCount; i++)
 355             customMixers[i] = mixers[index].motor[i];
 356     }
 357 }
 358
 359 #endif
 360
 361 void mixerResetDisarmedMotors(void)
 362 {
 363     int i;
 364     // set disarmed motor values
 365     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 366         motor_disarmed[i] = feature(FEATURE_3D) ? motor3DConfig()->neutral3d : motorConfig()->mincommand;
 367 }
 368
 369 void writeMotors(void)
 370 {
 371     uint8_t i;
 372
 373     for (i = 0; i < motorCount; i++)
 374         pwmWriteMotor(i, motor[i]);
 375
 376
 377     if (feature(FEATURE_ONESHOT125)) {
 378         pwmCompleteOneshotMotorUpdate(motorCount);
 379     }
 380 }
 381
 382 void writeAllMotors(int16_t mc)
 383 {
 384     uint8_t i;
 385
 386     // Sends commands to all motors
 387     for (i = 0; i < motorCount; i++)
 388         motor[i] = mc;
 389     writeMotors();
 390 }
 391
 392 void stopMotors(void)
 393 {
 394     writeAllMotors(feature(FEATURE_3D) ? motor3DConfig()->neutral3d : motorConfig()->mincommand);
 395
 396     delay(50); // give the timers and ESCs a chance to react.
 397 }
 398
 399 void StopPwmAllMotors()
 400 {
 401     pwmShutdownPulsesForAllMotors(motorCount);
 402 }
 403
 404 uint16_t mixConstrainMotorForFailsafeCondition(uint8_t motorIndex)
 405 {
 406     return constrain(motor[motorIndex], motorConfig()->mincommand, motorConfig()->maxthrottle);
 407 }
 408
 409 void mixTable(void)
 410 {
 411     uint32_t i;
 412
 413     bool isFailsafeActive = failsafeIsActive();
 414
 415     if (motorCount >= 4 && mixerConfig()->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
 416         // prevent "yaw jump" during yaw correction
 417         axisPID[FD_YAW] = constrain(axisPID[FD_YAW], -mixerConfig()->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig()->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
 418     }
 419
 420     if (rcModeIsActive(BOXAIRMODE)) {
 421         // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
 422         int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
 423         int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
 424         int16_t rollPitchYawMixMin = 0;
 425
 426         // Find roll/pitch/yaw desired output
 427         for (i = 0; i < motorCount; i++) {
 428             rollPitchYawMix[i] =
 429                 axisPID[FD_PITCH] * currentMixer[i].pitch +
 430                 axisPID[FD_ROLL] * currentMixer[i].roll +
 431                 -mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw;
 432
 433             if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
 434             if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
 435         }
 436
 437         // Scale roll/pitch/yaw uniformly to fit within throttle range
 438         int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
 439         int16_t throttleRange, throttle;
 440         int16_t throttleMin, throttleMax;
 441         static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions in 3D.
 442
 443         // Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check
 444         if (feature(FEATURE_3D)) {
 445             if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 446
 447             if ((rcData[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
 448                 throttleMax = motor3DConfig()->deadband3d_low;
 449                 throttleMin = motorConfig()->minthrottle;
 450                 throttlePrevious = throttle = rcData[THROTTLE];
 451             } else if (rcData[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
 452                 throttleMax = motorConfig()->maxthrottle;
 453                 throttleMin = motor3DConfig()->deadband3d_high;
 454                 throttlePrevious = throttle = rcData[THROTTLE];
 455             } else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)))  { // Deadband handling from negative to positive
 456                 throttle = throttleMax = motor3DConfig()->deadband3d_low;
 457                 throttleMin = motorConfig()->minthrottle;
 458             } else {  // Deadband handling from positive to negative
 459                 throttleMax = motorConfig()->maxthrottle;
 460                 throttle = throttleMin = motor3DConfig()->deadband3d_high;
 461             }
 462         } else {
 463             throttle = rcCommand[THROTTLE];
 464             throttleMin = motorConfig()->minthrottle;
 465             throttleMax = motorConfig()->maxthrottle;
 466         }
 467
 468         throttleRange = throttleMax - throttleMin;
 469
 470         if (rollPitchYawMixRange > throttleRange) {
 471             motorLimitReached = true;
 472             float mixReduction = (float) throttleRange / rollPitchYawMixRange;
 473             for (i = 0; i < motorCount; i++) {
 474                 rollPitchYawMix[i] =  lrintf((float) rollPitchYawMix[i] * mixReduction);
 475             }
 476             // Get the maximum correction by setting throttle offset to center.
 477             throttleMin = throttleMax = throttleMin + (throttleRange / 2);
 478         } else {
 479             motorLimitReached = false;
 480             throttleMin = throttleMin + (rollPitchYawMixRange / 2);
 481             throttleMax = throttleMax - (rollPitchYawMixRange / 2);
 482         }
 483
 484         // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 485         // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 486         for (i = 0; i < motorCount; i++) {
 487             motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
 488
 489             if (isFailsafeActive) {
 490                 motor[i] = mixConstrainMotorForFailsafeCondition(i);
 491             } else if (feature(FEATURE_3D)) {
 492                 if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) {
 493                     motor[i] = constrain(motor[i], motorConfig()->minthrottle, motor3DConfig()->deadband3d_low);
 494                 } else {
 495                     motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
 496                 }
 497             } else {
 498                 motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
 499             }
 500         }
 501     } else {
 502         // motors for non-servo mixes
 503         for (i = 0; i < motorCount; i++) {
 504             motor[i] =
 505                 rcCommand[THROTTLE] * currentMixer[i].throttle +
 506                 axisPID[FD_PITCH] * currentMixer[i].pitch +
 507                 axisPID[FD_ROLL] * currentMixer[i].roll +
 508                 -mixerConfig()->yaw_motor_direction * axisPID[FD_YAW] * currentMixer[i].yaw;
 509         }
 510
 511         // Find the maximum motor output.
 512         int16_t maxMotor = motor[0];
 513         for (i = 1; i < motorCount; i++) {
 514             // If one motor is above the maxthrottle threshold, we reduce the value
 515             // of all motors by the amount of overshoot.  That way, only one motor
 516             // is at max and the relative power of each motor is preserved.
 517             if (motor[i] > maxMotor) {
 518                 maxMotor = motor[i];
 519             }
 520         }
 521
 522         int16_t maxThrottleDifference = 0;
 523         if (maxMotor > motorConfig()->maxthrottle) {
 524             maxThrottleDifference = maxMotor - motorConfig()->maxthrottle;
 525         }
 526
 527         for (i = 0; i < motorCount; i++) {
 528             // this is a way to still have good gyro corrections if at least one motor reaches its max.
 529             motor[i] -= maxThrottleDifference;
 530
 531             if (feature(FEATURE_3D)) {
 532                 if (mixerConfig()->pid_at_min_throttle
 533                         || rcData[THROTTLE] <= rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle
 534                         || rcData[THROTTLE] >= rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle) {
 535                     if (rcData[THROTTLE] > rxConfig()->midrc) {
 536                         motor[i] = constrain(motor[i], motor3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
 537                     } else {
 538                         motor[i] = constrain(motor[i], motorConfig()->mincommand, motor3DConfig()->deadband3d_low);
 539                     }
 540                 } else {
 541                     if (rcData[THROTTLE] > rxConfig()->midrc) {
 542                         motor[i] = motor3DConfig()->deadband3d_high;
 543                     } else {
 544                         motor[i] = motor3DConfig()->deadband3d_low;
 545                     }
 546                 }
 547             } else {
 548                 if (isFailsafeActive) {
 549                     motor[i] = mixConstrainMotorForFailsafeCondition(i);
 550                 } else {
 551                     // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
 552                     // do not spin the motors.
 553                     motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
 554                     if ((rcData[THROTTLE]) < rxConfig()->mincheck) {
 555                         if (feature(FEATURE_MOTOR_STOP)) {
 556                             motor[i] = motorConfig()->mincommand;
 557                         } else if (mixerConfig()->pid_at_min_throttle == 0) {
 558                             motor[i] = motorConfig()->minthrottle;
 559                         }
 560                     }
 561                 }
 562             }
 563         }
 564     }
 565
 566
 567     /* Disarmed for all mixers */
 568     if (!ARMING_FLAG(ARMED)) {
 569         for (i = 0; i < motorCount; i++) {
 570             motor[i] = motor_disarmed[i];
 571         }
 572     }
 573
 574     // motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
 575
 576 #if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS)
 577     servoMixTable();
 578 #endif
 579 }
 580