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 <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/build_config.h"
  26
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29 #include "common/filter.h"
  30
  31 #include "drivers/system.h"
  32 #include "drivers/pwm_output.h"
  33
  34 #include "io/motors.h"
  35
  36 #include "rx/rx.h"
  37
  38 #include "sensors/battery.h"
  39
  40 #include "flight/mixer.h"
  41 #include "flight/failsafe.h"
  42 #include "flight/pid.h"
  43 #include "flight/imu.h"
  44
  45 #include "fc/config.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/runtime_config.h"
  48
  49 #include "config/feature.h"
  50 #include "config/config_master.h"
  51
  52 #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
  53 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
  54 #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
  55 #define EXTERNAL_CONVERSION_MIN_VALUE 1000
  56 #define EXTERNAL_CONVERSION_MAX_VALUE 2000
  57
  58 static uint8_t motorCount;
  59 static float motorMixRange;
  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 motorConfig_t *motorConfig;
  67 static airplaneConfig_t *airplaneConfig;
  68 rxConfig_t *rxConfig;
  69
  70 mixerMode_e currentMixerMode;
  71 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  72
  73
  74 static const motorMixer_t mixerQuadX[] = {
  75     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
  76     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
  77     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
  78     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
  79 };
  80 #ifndef USE_QUAD_MIXER_ONLY
  81 static const motorMixer_t mixerTricopter[] = {
  82     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
  83     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
  84     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
  85 };
  86
  87 static const motorMixer_t mixerQuadP[] = {
  88     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
  89     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
  90     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
  91     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
  92 };
  93
  94 static const motorMixer_t mixerBicopter[] = {
  95     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
  96     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
  97 };
  98
  99 static const motorMixer_t mixerY6[] = {
 100     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 101     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 102     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 103     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 104     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 105     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 106 };
 107
 108 static const motorMixer_t mixerHex6P[] = {
 109     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 110     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 111     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 112     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 113     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 114     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 115 };
 116
 117 static const motorMixer_t mixerY4[] = {
 118     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
 119     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
 120     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
 121     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
 122 };
 123
 124 static const motorMixer_t mixerHex6X[] = {
 125     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 126     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 127     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 128     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 129     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 130     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 131 };
 132
 133 static const motorMixer_t mixerOctoX8[] = {
 134     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 135     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 136     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 137     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 138     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 139     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 140     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 141     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 142 };
 143
 144 static const motorMixer_t mixerOctoFlatP[] = {
 145     { 1.0f,  0.707107f, -0.707107f,  1.0f },    // FRONT_L
 146     { 1.0f, -0.707107f, -0.707107f,  1.0f },    // FRONT_R
 147     { 1.0f, -0.707107f,  0.707107f,  1.0f },    // REAR_R
 148     { 1.0f,  0.707107f,  0.707107f,  1.0f },    // REAR_L
 149     { 1.0f,  0.0f, -1.0f, -1.0f },              // FRONT
 150     { 1.0f, -1.0f,  0.0f, -1.0f },              // RIGHT
 151     { 1.0f,  0.0f,  1.0f, -1.0f },              // REAR
 152     { 1.0f,  1.0f,  0.0f, -1.0f },              // LEFT
 153 };
 154
 155 static const motorMixer_t mixerOctoFlatX[] = {
 156     { 1.0f,  1.0f, -0.414178f,  1.0f },      // MIDFRONT_L
 157     { 1.0f, -0.414178f, -1.0f,  1.0f },      // FRONT_R
 158     { 1.0f, -1.0f,  0.414178f,  1.0f },      // MIDREAR_R
 159     { 1.0f,  0.414178f,  1.0f,  1.0f },      // REAR_L
 160     { 1.0f,  0.414178f, -1.0f, -1.0f },      // FRONT_L
 161     { 1.0f, -1.0f, -0.414178f, -1.0f },      // MIDFRONT_R
 162     { 1.0f, -0.414178f,  1.0f, -1.0f },      // REAR_R
 163     { 1.0f,  1.0f,  0.414178f, -1.0f },      // MIDREAR_L
 164 };
 165
 166 static const motorMixer_t mixerVtail4[] = {
 167     { 1.0f,  -0.58f,  0.58f, 1.0f },        // REAR_R
 168     { 1.0f,  -0.46f, -0.39f, -0.5f },       // FRONT_R
 169     { 1.0f,  0.58f,  0.58f, -1.0f },        // REAR_L
 170     { 1.0f,  0.46f, -0.39f, 0.5f },         // FRONT_L
 171 };
 172
 173 static const motorMixer_t mixerAtail4[] = {
 174     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_R
 175     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R
 176     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_L
 177     { 1.0f,  1.0f, -1.0f, -0.0f },          // FRONT_L
 178 };
 179
 180 static const motorMixer_t mixerHex6H[] = {
 181     { 1.0f, -1.0f,  1.0f, -1.0f },     // REAR_R
 182     { 1.0f, -1.0f, -1.0f,  1.0f },     // FRONT_R
 183     { 1.0f,  1.0f,  1.0f,  1.0f },     // REAR_L
 184     { 1.0f,  1.0f, -1.0f, -1.0f },     // FRONT_L
 185     { 1.0f,  0.0f,  0.0f,  0.0f },     // RIGHT
 186     { 1.0f,  0.0f,  0.0f,  0.0f },     // LEFT
 187 };
 188
 189 static const motorMixer_t mixerDualcopter[] = {
 190     { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 191     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 192 };
 193
 194 static const motorMixer_t mixerSingleProp[] = {
 195     { 1.0f,  0.0f,  0.0f, 0.0f },
 196 };
 197
 198 static const motorMixer_t mixerQuadX1234[] = {
 199     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 200     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 201     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 202     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 203 };
 204
 205 // Keep synced with mixerMode_e
 206 const mixer_t mixers[] = {
 207     // motors, use servo, motor mixer
 208     { 0, false, NULL },                // entry 0
 209     { 3, true,  mixerTricopter },      // MIXER_TRI
 210     { 4, false, mixerQuadP },          // MIXER_QUADP
 211     { 4, false, mixerQuadX },          // MIXER_QUADX
 212     { 2, true,  mixerBicopter },       // MIXER_BICOPTER
 213     { 0, true,  NULL },                // * MIXER_GIMBAL
 214     { 6, false, mixerY6 },             // MIXER_Y6
 215     { 6, false, mixerHex6P },          // MIXER_HEX6
 216     { 1, true,  mixerSingleProp },     // * MIXER_FLYING_WING
 217     { 4, false, mixerY4 },             // MIXER_Y4
 218     { 6, false, mixerHex6X },          // MIXER_HEX6X
 219     { 8, false, mixerOctoX8 },         // MIXER_OCTOX8
 220     { 8, false, mixerOctoFlatP },      // MIXER_OCTOFLATP
 221     { 8, false, mixerOctoFlatX },      // MIXER_OCTOFLATX
 222     { 1, true,  mixerSingleProp },     // * MIXER_AIRPLANE
 223     { 0, true,  NULL },                // * MIXER_HELI_120_CCPM
 224     { 0, true,  NULL },                // * MIXER_HELI_90_DEG
 225     { 4, false, mixerVtail4 },         // MIXER_VTAIL4
 226     { 6, false, mixerHex6H },          // MIXER_HEX6H
 227     { 0, true,  NULL },                // * MIXER_PPM_TO_SERVO
 228     { 2, true,  mixerDualcopter },     // MIXER_DUALCOPTER
 229     { 1, true,  NULL },                // MIXER_SINGLECOPTER
 230     { 4, false, mixerAtail4 },         // MIXER_ATAIL4
 231     { 0, false, NULL },                // MIXER_CUSTOM
 232     { 2, true,  NULL },                // MIXER_CUSTOM_AIRPLANE
 233     { 3, true,  NULL },                // MIXER_CUSTOM_TRI
 234     { 4, false, mixerQuadX1234 },
 235 };
 236 #endif
 237
 238 static motorMixer_t *customMixers;
 239
 240 static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
 241 uint16_t motorOutputHigh, motorOutputLow;
 242 static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
 243
 244 uint8_t getMotorCount()
 245 {
 246     return motorCount;
 247 }
 248
 249 float getMotorMixRange()
 250 {
 251     return motorMixRange;
 252 }
 253
 254 bool isMotorProtocolDshot(void) {
 255 #ifdef USE_DSHOT
 256     switch(motorConfig->motorPwmProtocol) {
 257     case PWM_TYPE_DSHOT1200:
 258     case PWM_TYPE_DSHOT600:
 259     case PWM_TYPE_DSHOT300:
 260     case PWM_TYPE_DSHOT150:
 261         return true;
 262     default:
 263         return false;
 264     }
 265 #else
 266     return false;
 267 #endif
 268 }
 269
 270 // Add here scaled ESC outputs for digital protol
 271 void initEscEndpoints(void) {
 272 #ifdef USE_DSHOT
 273     if (isMotorProtocolDshot()) {
 274         disarmMotorOutput = DSHOT_DISARM_COMMAND;
 275         if (feature(FEATURE_3D))
 276             motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
 277         else
 278             motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
 279         motorOutputHigh = DSHOT_MAX_THROTTLE;
 280         deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
 281         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
 282     } else
 283 #endif
 284     {
 285         disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
 286         motorOutputLow = motorConfig->minthrottle;
 287         motorOutputHigh = motorConfig->maxthrottle;
 288         deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
 289         deadbandMotor3dLow = flight3DConfig->deadband3d_low;
 290     }
 291
 292     rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
 293     rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
 294     rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
 295 }
 296
 297 void mixerUseConfigs(
 298         flight3DConfig_t *flight3DConfigToUse,
 299         motorConfig_t *motorConfigToUse,
 300         mixerConfig_t *mixerConfigToUse,
 301         airplaneConfig_t *airplaneConfigToUse,
 302         rxConfig_t *rxConfigToUse)
 303 {
 304     flight3DConfig = flight3DConfigToUse;
 305     motorConfig = motorConfigToUse;
 306     mixerConfig = mixerConfigToUse;
 307     airplaneConfig = airplaneConfigToUse;
 308     rxConfig = rxConfigToUse;
 309 }
 310
 311 void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
 312 {
 313     currentMixerMode = mixerMode;
 314
 315     customMixers = initialCustomMixers;
 316
 317     initEscEndpoints();
 318 }
 319
 320 #ifndef USE_QUAD_MIXER_ONLY
 321
 322 void mixerConfigureOutput(void)
 323 {
 324     motorCount = 0;
 325
 326     if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
 327         // load custom mixer into currentMixer
 328         for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 329             // check if done
 330             if (customMixers[i].throttle == 0.0f)
 331                 break;
 332             currentMixer[i] = customMixers[i];
 333             motorCount++;
 334         }
 335     } else {
 336         motorCount = mixers[currentMixerMode].motorCount;
 337         if (motorCount > MAX_SUPPORTED_MOTORS) {
 338             motorCount = MAX_SUPPORTED_MOTORS;
 339         }
 340         // copy motor-based mixers
 341         if (mixers[currentMixerMode].motor) {
 342             for (int i = 0; i < motorCount; i++)
 343                 currentMixer[i] = mixers[currentMixerMode].motor[i];
 344         }
 345     }
 346
 347     // in 3D mode, mixer gain has to be halved
 348     if (feature(FEATURE_3D)) {
 349         if (motorCount > 1) {
 350             for (int i = 0; i < motorCount; i++) {
 351                 currentMixer[i].pitch *= 0.5f;
 352                 currentMixer[i].roll *= 0.5f;
 353                 currentMixer[i].yaw *= 0.5f;
 354             }
 355         }
 356     }
 357
 358     mixerResetDisarmedMotors();
 359 }
 360
 361 void mixerLoadMix(int index, motorMixer_t *customMixers)
 362 {
 363     // we're 1-based
 364     index++;
 365     // clear existing
 366     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 367         customMixers[i].throttle = 0.0f;
 368     }
 369
 370     // do we have anything here to begin with?
 371     if (mixers[index].motor != NULL) {
 372         for (int i = 0; i < mixers[index].motorCount; i++) {
 373             customMixers[i] = mixers[index].motor[i];
 374         }
 375     }
 376 }
 377 #else
 378 void mixerConfigureOutput(void)
 379 {
 380     motorCount = QUAD_MOTOR_COUNT;
 381
 382     for (int i = 0; i < motorCount; i++) {
 383         currentMixer[i] = mixerQuadX[i];
 384     }
 385
 386     mixerResetDisarmedMotors();
 387 }
 388 #endif
 389
 390 void mixerResetDisarmedMotors(void)
 391 {
 392     // set disarmed motor values
 393     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 394         motor_disarmed[i] = disarmMotorOutput;
 395     }
 396 }
 397
 398 void writeMotors(void)
 399 {
 400     if (pwmAreMotorsEnabled()) {
 401         for (int i = 0; i < motorCount; i++) {
 402             pwmWriteMotor(i, motor[i]);
 403         }
 404     }
 405
 406     pwmCompleteMotorUpdate(motorCount);
 407 }
 408
 409 static void writeAllMotors(int16_t mc)
 410 {
 411     // Sends commands to all motors
 412     for (int i = 0; i < motorCount; i++) {
 413         motor[i] = mc;
 414     }
 415
 416     writeMotors();
 417 }
 418
 419 void stopMotors(void)
 420 {
 421     writeAllMotors(disarmMotorOutput);
 422
 423     delay(50); // give the timers and ESCs a chance to react.
 424 }
 425
 426 void stopPwmAllMotors(void)
 427 {
 428     pwmShutdownPulsesForAllMotors(motorCount);
 429     delayMicroseconds(1500);
 430 }
 431
 432 void mixTable(pidProfile_t *pidProfile)
 433 {
 434     // Scale roll/pitch/yaw uniformly to fit within throttle range
 435     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
 436     float throttle = 0, currentThrottleInputRange = 0;
 437     uint16_t motorOutputMin, motorOutputMax;
 438     static uint16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 439     bool mixerInversion = false;
 440
 441     // Find min and max throttle based on condition.
 442     if (feature(FEATURE_3D)) {
 443         if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 444
 445         if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
 446             motorOutputMax = deadbandMotor3dLow;
 447             motorOutputMin = motorOutputLow;
 448             throttlePrevious = rcCommand[THROTTLE];
 449             throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
 450             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 451             if(isMotorProtocolDshot()) mixerInversion = true;
 452         } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
 453             motorOutputMax = motorOutputHigh;
 454             motorOutputMin = deadbandMotor3dHigh;
 455             throttlePrevious = rcCommand[THROTTLE];
 456             throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
 457             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 458         } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)))  { // Deadband handling from negative to positive
 459             motorOutputMax = deadbandMotor3dLow;
 460             motorOutputMin = motorOutputLow;
 461             throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
 462             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 463             if(isMotorProtocolDshot()) mixerInversion = true;
 464         } else {  // Deadband handling from positive to negative
 465             motorOutputMax = motorOutputHigh;
 466             motorOutputMin = deadbandMotor3dHigh;
 467             throttle = 0;
 468             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 469         }
 470     } else {
 471         throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
 472         currentThrottleInputRange = rcCommandThrottleRange;
 473         motorOutputMin = motorOutputLow;
 474         motorOutputMax = motorOutputHigh;
 475     }
 476
 477     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
 478     const float motorOutputRange = motorOutputMax - motorOutputMin;
 479
 480     float scaledAxisPIDf[3];
 481     // Limit the PIDsum
 482     for (int axis = 0; axis < 3; axis++) {
 483         scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
 484     }
 485
 486     // Calculate voltage compensation
 487     const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation)  ? calculateVbatPidCompensation() : 1.0f;
 488
 489     // Find roll/pitch/yaw desired output
 490     float motorMix[MAX_SUPPORTED_MOTORS];
 491     float motorMixMax = 0, motorMixMin = 0;
 492     for (int i = 0; i < motorCount; i++) {
 493         motorMix[i] =
 494             scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
 495             scaledAxisPIDf[ROLL]  * currentMixer[i].roll +
 496             scaledAxisPIDf[YAW]   * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
 497
 498         if (vbatCompensationFactor > 1.0f) {
 499             motorMix[i] *= vbatCompensationFactor;  // Add voltage compensation
 500         }
 501
 502         if (motorMix[i] > motorMixMax) {
 503             motorMixMax = motorMix[i];
 504         } else if (motorMix[i] < motorMixMin) {
 505             motorMixMin = motorMix[i];
 506         }
 507     }
 508
 509     motorMixRange = motorMixMax - motorMixMin;
 510
 511     if (motorMixRange > 1.0f) {
 512         for (int i = 0; i < motorCount; i++) {
 513             motorMix[i] /= motorMixRange;
 514         }
 515         // Get the maximum correction by setting offset to center
 516         throttle = 0.5f;
 517     } else {
 518         float throttleLimitOffset = motorMixRange / 2.0f;
 519         throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
 520     }
 521
 522     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 523     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 524     uint32_t i = 0;
 525     for (i = 0; i < motorCount; i++) {
 526         motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
 527
 528         // Dshot works exactly opposite in lower 3D section.
 529         if (mixerInversion) {
 530             motor[i] = motorOutputMin + (motorOutputMax - motor[i]);
 531         }
 532
 533         if (failsafeIsActive()) {
 534             if (isMotorProtocolDshot())
 535                 motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range
 536
 537             motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax);
 538         } else {
 539             motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax);
 540         }
 541
 542         // Motor stop handling
 543         if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
 544             if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
 545                 motor[i] = disarmMotorOutput;
 546             }
 547         }
 548     }
 549
 550     // Disarmed mode
 551     if (!ARMING_FLAG(ARMED)) {
 552         for (i = 0; i < motorCount; i++) {
 553             motor[i] = motor_disarmed[i];
 554         }
 555     }
 556 }
 557
 558 uint16_t convertExternalToMotor(uint16_t externalValue)
 559 {
 560     uint16_t motorValue = externalValue;
 561 #ifdef USE_DSHOT
 562     if (isMotorProtocolDshot()) {
 563         motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
 564     }
 565 #endif
 566
 567     return motorValue;
 568 }
 569
 570 uint16_t convertMotorToExternal(uint16_t motorValue)
 571 {
 572     uint16_t externalValue = motorValue;
 573 #ifdef USE_DSHOT
 574     if (isMotorProtocolDshot()) {
 575         externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain((motorValue / EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE);
 576     }
 577 #endif
 578
 579     return externalValue;
 580 }