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 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29 #include "common/filter.h"
  30 #include "common/maths.h"
  31
  32 #include "config/feature.h"
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35
  36 #include "drivers/pwm_output.h"
  37 #include "drivers/pwm_esc_detect.h"
  38 #include "drivers/time.h"
  39
  40 #include "io/motors.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/rc_controls.h"
  44 #include "fc/rc_modes.h"
  45 #include "fc/runtime_config.h"
  46 #include "fc/fc_core.h"
  47 #include "fc/fc_rc.h"
  48
  49 #include "flight/failsafe.h"
  50 #include "flight/imu.h"
  51 #include "flight/mixer.h"
  52 #include "flight/pid.h"
  53
  54 #include "rx/rx.h"
  55
  56 #include "sensors/battery.h"
  57
  58 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
  59
  60 #ifndef TARGET_DEFAULT_MIXER
  61 #define TARGET_DEFAULT_MIXER    MIXER_QUADX
  62 #endif
  63 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  64     .mixerMode = TARGET_DEFAULT_MIXER,
  65     .yaw_motors_reversed = false,
  66 );
  67
  68 PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
  69
  70 void pgResetFn_motorConfig(motorConfig_t *motorConfig)
  71 {
  72 #ifdef BRUSHED_MOTORS
  73     motorConfig->minthrottle = 1000;
  74     motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
  75     motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
  76     motorConfig->dev.useUnsyncedPwm = true;
  77 #else
  78 #ifdef BRUSHED_ESC_AUTODETECT
  79     if (hardwareMotorType == MOTOR_BRUSHED) {
  80         motorConfig->minthrottle = 1000;
  81         motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
  82         motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
  83         motorConfig->dev.useUnsyncedPwm = true;
  84     } else
  85 #endif
  86     {
  87         motorConfig->minthrottle = 1070;
  88         motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
  89         motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
  90     }
  91 #endif
  92     motorConfig->maxthrottle = 2000;
  93     motorConfig->mincommand = 1000;
  94     motorConfig->digitalIdleOffsetValue = 450;
  95
  96     int motorIndex = 0;
  97     for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
  98         if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
  99             motorConfig->dev.ioTags[motorIndex] = timerHardware[i].tag;
 100             motorIndex++;
 101         }
 102     }
 103 }
 104
 105 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
 106
 107 #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
 108 // (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
 109 #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
 110 #define EXTERNAL_CONVERSION_MIN_VALUE 1000
 111 #define EXTERNAL_CONVERSION_MAX_VALUE 2000
 112 #define EXTERNAL_CONVERSION_3D_MID_VALUE 1500
 113
 114 #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
 115
 116 static uint8_t motorCount;
 117 static float motorMixRange;
 118
 119 float motor[MAX_SUPPORTED_MOTORS];
 120 float motor_disarmed[MAX_SUPPORTED_MOTORS];
 121
 122 mixerMode_e currentMixerMode;
 123 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 124
 125 float pidSumLimit;
 126 float pidSumLimitYaw;
 127
 128
 129 static const motorMixer_t mixerQuadX[] = {
 130     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 131     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 132     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 133     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 134 };
 135 #ifndef USE_QUAD_MIXER_ONLY
 136 static const motorMixer_t mixerTricopter[] = {
 137     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
 138     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
 139     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
 140 };
 141
 142 static const motorMixer_t mixerQuadP[] = {
 143     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
 144     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
 145     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
 146     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
 147 };
 148
 149 #if defined(USE_UNCOMMON_MIXERS)
 150 static const motorMixer_t mixerBicopter[] = {
 151     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
 152     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
 153 };
 154 #else
 155 #define mixerBicopter NULL
 156 #endif
 157
 158 static const motorMixer_t mixerY4[] = {
 159     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
 160     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
 161     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
 162     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
 163 };
 164
 165
 166 #if (MAX_SUPPORTED_MOTORS >= 6)
 167 static const motorMixer_t mixerHex6X[] = {
 168     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 169     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 170     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 171     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 172     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 173     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 174 };
 175
 176 #if defined(USE_UNCOMMON_MIXERS)
 177 static const motorMixer_t mixerHex6H[] = {
 178     { 1.0f, -1.0f,  1.0f, -1.0f },     // REAR_R
 179     { 1.0f, -1.0f, -1.0f,  1.0f },     // FRONT_R
 180     { 1.0f,  1.0f,  1.0f,  1.0f },     // REAR_L
 181     { 1.0f,  1.0f, -1.0f, -1.0f },     // FRONT_L
 182     { 1.0f,  0.0f,  0.0f,  0.0f },     // RIGHT
 183     { 1.0f,  0.0f,  0.0f,  0.0f },     // LEFT
 184 };
 185
 186 static const motorMixer_t mixerHex6P[] = {
 187     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 188     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 189     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 190     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 191     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 192     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 193 };
 194 static const motorMixer_t mixerY6[] = {
 195     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 196     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 197     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 198     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 199     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 200     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 201 };
 202 #else
 203 #define mixerHex6H NULL
 204 #define mixerHex6P NULL
 205 #define mixerY6 NULL
 206 #endif // USE_UNCOMMON_MIXERS
 207 #else
 208 #define mixerHex6X NULL
 209 #endif // MAX_SUPPORTED_MOTORS >= 6
 210
 211 #if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8)
 212 static const motorMixer_t mixerOctoX8[] = {
 213     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 214     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 215     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 216     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 217     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 218     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 219     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 220     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 221 };
 222
 223 static const motorMixer_t mixerOctoFlatP[] = {
 224     { 1.0f,  0.707107f, -0.707107f,  1.0f },    // FRONT_L
 225     { 1.0f, -0.707107f, -0.707107f,  1.0f },    // FRONT_R
 226     { 1.0f, -0.707107f,  0.707107f,  1.0f },    // REAR_R
 227     { 1.0f,  0.707107f,  0.707107f,  1.0f },    // REAR_L
 228     { 1.0f,  0.0f, -1.0f, -1.0f },              // FRONT
 229     { 1.0f, -1.0f,  0.0f, -1.0f },              // RIGHT
 230     { 1.0f,  0.0f,  1.0f, -1.0f },              // REAR
 231     { 1.0f,  1.0f,  0.0f, -1.0f },              // LEFT
 232 };
 233
 234 static const motorMixer_t mixerOctoFlatX[] = {
 235     { 1.0f,  1.0f, -0.414178f,  1.0f },      // MIDFRONT_L
 236     { 1.0f, -0.414178f, -1.0f,  1.0f },      // FRONT_R
 237     { 1.0f, -1.0f,  0.414178f,  1.0f },      // MIDREAR_R
 238     { 1.0f,  0.414178f,  1.0f,  1.0f },      // REAR_L
 239     { 1.0f,  0.414178f, -1.0f, -1.0f },      // FRONT_L
 240     { 1.0f, -1.0f, -0.414178f, -1.0f },      // MIDFRONT_R
 241     { 1.0f, -0.414178f,  1.0f, -1.0f },      // REAR_R
 242     { 1.0f,  1.0f,  0.414178f, -1.0f },      // MIDREAR_L
 243 };
 244 #else
 245 #define mixerOctoX8 NULL
 246 #define mixerOctoFlatP NULL
 247 #define mixerOctoFlatX NULL
 248 #endif
 249
 250 static const motorMixer_t mixerVtail4[] = {
 251     { 1.0f,  -0.58f,  0.58f, 1.0f },        // REAR_R
 252     { 1.0f,  -0.46f, -0.39f, -0.5f },       // FRONT_R
 253     { 1.0f,  0.58f,  0.58f, -1.0f },        // REAR_L
 254     { 1.0f,  0.46f, -0.39f, 0.5f },         // FRONT_L
 255 };
 256
 257 static const motorMixer_t mixerAtail4[] = {
 258     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_R
 259     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R
 260     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_L
 261     { 1.0f,  1.0f, -1.0f, -0.0f },          // FRONT_L
 262 };
 263
 264 #if defined(USE_UNCOMMON_MIXERS)
 265 static const motorMixer_t mixerDualcopter[] = {
 266     { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 267     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 268 };
 269 #else
 270 #define mixerDualcopter NULL
 271 #endif
 272
 273 static const motorMixer_t mixerSingleProp[] = {
 274     { 1.0f,  0.0f,  0.0f, 0.0f },
 275 };
 276
 277 static const motorMixer_t mixerQuadX1234[] = {
 278     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 279     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 280     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 281     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 282 };
 283
 284 // Keep synced with mixerMode_e
 285 const mixer_t mixers[] = {
 286     // motors, use servo, motor mixer
 287     { 0, false, NULL },                // entry 0
 288     { 3, true,  mixerTricopter },      // MIXER_TRI
 289     { 4, false, mixerQuadP },          // MIXER_QUADP
 290     { 4, false, mixerQuadX },          // MIXER_QUADX
 291     { 2, true,  mixerBicopter },       // MIXER_BICOPTER
 292     { 0, true,  NULL },                // * MIXER_GIMBAL
 293     { 6, false, mixerY6 },             // MIXER_Y6
 294     { 6, false, mixerHex6P },          // MIXER_HEX6
 295     { 1, true,  mixerSingleProp },     // * MIXER_FLYING_WING
 296     { 4, false, mixerY4 },             // MIXER_Y4
 297     { 6, false, mixerHex6X },          // MIXER_HEX6X
 298     { 8, false, mixerOctoX8 },         // MIXER_OCTOX8
 299     { 8, false, mixerOctoFlatP },      // MIXER_OCTOFLATP
 300     { 8, false, mixerOctoFlatX },      // MIXER_OCTOFLATX
 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     { 6, false, mixerHex6H },          // MIXER_HEX6H
 306     { 0, true,  NULL },                // * MIXER_PPM_TO_SERVO
 307     { 2, true,  mixerDualcopter },     // MIXER_DUALCOPTER
 308     { 1, true,  NULL },                // MIXER_SINGLECOPTER
 309     { 4, false, mixerAtail4 },         // MIXER_ATAIL4
 310     { 0, false, NULL },                // MIXER_CUSTOM
 311     { 2, true,  NULL },                // MIXER_CUSTOM_AIRPLANE
 312     { 3, true,  NULL },                // MIXER_CUSTOM_TRI
 313     { 4, false, mixerQuadX1234 },
 314 };
 315 #endif // !USE_QUAD_MIXER_ONLY
 316
 317 float motorOutputHigh, motorOutputLow;
 318
 319 static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
 320 static uint16_t rcCommand3dDeadBandLow;
 321 static uint16_t rcCommand3dDeadBandHigh;
 322 static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
 323
 324 uint8_t getMotorCount(void)
 325 {
 326     return motorCount;
 327 }
 328
 329 float getMotorMixRange(void)
 330 {
 331     return motorMixRange;
 332 }
 333
 334 bool areMotorsRunning(void)
 335 {
 336     bool motorsRunning = false;
 337     if (ARMING_FLAG(ARMED)) {
 338         motorsRunning = true;
 339     } else {
 340         for (int i = 0; i < motorCount; i++) {
 341             if (motor_disarmed[i] != disarmMotorOutput) {
 342                 motorsRunning = true;
 343
 344                 break;
 345             }
 346         }
 347     }
 348
 349     return motorsRunning;
 350 }
 351
 352 bool mixerIsOutputSaturated(int axis, float errorRate)
 353 {
 354     if (axis == FD_YAW && (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)) {
 355         return errorRate > TRICOPTER_ERROR_RATE_YAW_SATURATED;
 356     } else {
 357         return motorMixRange >= 1.0f;
 358     }
 359     return false;
 360 }
 361
 362 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
 363 // DSHOT scaling is done to the actual dshot range
 364 void initEscEndpoints(void)
 365 {
 366     // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
 367     switch (motorConfig()->dev.motorPwmProtocol) {
 368 #ifdef USE_DSHOT
 369     case PWM_TYPE_PROSHOT1000:
 370     case PWM_TYPE_DSHOT1200:
 371     case PWM_TYPE_DSHOT600:
 372     case PWM_TYPE_DSHOT300:
 373     case PWM_TYPE_DSHOT150:
 374         disarmMotorOutput = DSHOT_DISARM_COMMAND;
 375         if (feature(FEATURE_3D)) {
 376             motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
 377         } else {
 378             motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
 379         }
 380         motorOutputHigh = DSHOT_MAX_THROTTLE;
 381         deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
 382         deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
 383
 384         break;
 385 #endif
 386     default:
 387         if (feature(FEATURE_3D)) {
 388             disarmMotorOutput = flight3DConfig()->neutral3d;
 389             motorOutputLow = PWM_RANGE_MIN;
 390         } else {
 391             disarmMotorOutput = motorConfig()->mincommand;
 392             motorOutputLow = motorConfig()->minthrottle;
 393         }
 394         motorOutputHigh = motorConfig()->maxthrottle;
 395         deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
 396         deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
 397
 398         break;
 399     }
 400
 401     rcCommandThrottleRange = PWM_RANGE_MAX - rxConfig()->mincheck;
 402
 403     rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
 404     rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
 405
 406     rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
 407     rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
 408 }
 409
 410 void mixerInit(mixerMode_e mixerMode)
 411 {
 412     currentMixerMode = mixerMode;
 413
 414     initEscEndpoints();
 415 }
 416
 417 void pidInitMixer(const struct pidProfile_s *pidProfile)
 418 {
 419     pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit);
 420     pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw);
 421 }
 422
 423 #ifndef USE_QUAD_MIXER_ONLY
 424
 425 void mixerConfigureOutput(void)
 426 {
 427     motorCount = 0;
 428
 429     if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
 430         // load custom mixer into currentMixer
 431         for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 432             // check if done
 433             if (customMotorMixer(i)->throttle == 0.0f)
 434                 break;
 435             currentMixer[i] = *customMotorMixer(i);
 436             motorCount++;
 437         }
 438     } else {
 439         motorCount = mixers[currentMixerMode].motorCount;
 440         if (motorCount > MAX_SUPPORTED_MOTORS) {
 441             motorCount = MAX_SUPPORTED_MOTORS;
 442         }
 443         // copy motor-based mixers
 444         if (mixers[currentMixerMode].motor) {
 445             for (int i = 0; i < motorCount; i++)
 446                 currentMixer[i] = mixers[currentMixerMode].motor[i];
 447         }
 448     }
 449
 450     // in 3D mode, mixer gain has to be halved
 451     if (feature(FEATURE_3D)) {
 452         if (motorCount > 1) {
 453             for (int i = 0; i < motorCount; i++) {
 454                 currentMixer[i].pitch *= 0.5f;
 455                 currentMixer[i].roll *= 0.5f;
 456                 currentMixer[i].yaw *= 0.5f;
 457             }
 458         }
 459     }
 460
 461     mixerResetDisarmedMotors();
 462 }
 463
 464 void mixerLoadMix(int index, motorMixer_t *customMixers)
 465 {
 466     // we're 1-based
 467     index++;
 468     // clear existing
 469     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 470         customMixers[i].throttle = 0.0f;
 471     }
 472
 473     // do we have anything here to begin with?
 474     if (mixers[index].motor != NULL) {
 475         for (int i = 0; i < mixers[index].motorCount; i++) {
 476             customMixers[i] = mixers[index].motor[i];
 477         }
 478     }
 479 }
 480 #else
 481 void mixerConfigureOutput(void)
 482 {
 483     motorCount = QUAD_MOTOR_COUNT;
 484
 485     for (int i = 0; i < motorCount; i++) {
 486         currentMixer[i] = mixerQuadX[i];
 487     }
 488
 489     mixerResetDisarmedMotors();
 490 }
 491 #endif
 492
 493 void mixerResetDisarmedMotors(void)
 494 {
 495     // set disarmed motor values
 496     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 497         motor_disarmed[i] = disarmMotorOutput;
 498     }
 499 }
 500
 501 void writeMotors(void)
 502 {
 503     if (pwmAreMotorsEnabled()) {
 504         for (int i = 0; i < motorCount; i++) {
 505             pwmWriteMotor(i, motor[i]);
 506         }
 507         pwmCompleteMotorUpdate(motorCount);
 508     }
 509 }
 510
 511 static void writeAllMotors(int16_t mc)
 512 {
 513     // Sends commands to all motors
 514     for (int i = 0; i < motorCount; i++) {
 515         motor[i] = mc;
 516     }
 517
 518     writeMotors();
 519 }
 520
 521 void stopMotors(void)
 522 {
 523     writeAllMotors(disarmMotorOutput);
 524
 525     delay(50); // give the timers and ESCs a chance to react.
 526 }
 527
 528 void stopPwmAllMotors(void)
 529 {
 530     pwmShutdownPulsesForAllMotors(motorCount);
 531     delayMicroseconds(1500);
 532 }
 533
 534 static float throttle = 0;
 535 static float motorOutputMin;
 536 static float motorRangeMin;
 537 static float motorRangeMax;
 538 static float motorOutputRange;
 539 static int8_t motorOutputMixSign;
 540
 541 void calculateThrottleAndCurrentMotorEndpoints(void)
 542 {
 543     static uint16_t rcThrottlePrevious = 0;   // Store the last throttle direction for deadband transitions
 544     float currentThrottleInputRange = 0;
 545
 546     if(feature(FEATURE_3D)) {
 547         if (!ARMING_FLAG(ARMED)) {
 548             rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 549         }
 550
 551         if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) {
 552             // INVERTED
 553             motorRangeMin = motorOutputLow;
 554             motorRangeMax = deadbandMotor3dLow;
 555             if(isMotorProtocolDshot()) {
 556                 motorOutputMin = motorOutputLow;
 557                 motorOutputRange = deadbandMotor3dLow - motorOutputLow;
 558             } else {
 559                 motorOutputMin = deadbandMotor3dLow;
 560                 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
 561             }
 562             motorOutputMixSign = -1;
 563             rcThrottlePrevious = rcCommand[THROTTLE];
 564             throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
 565             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 566         } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
 567             // NORMAL
 568             motorRangeMin = deadbandMotor3dHigh;
 569             motorRangeMax = motorOutputHigh;
 570             motorOutputMin = deadbandMotor3dHigh;
 571             motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
 572             motorOutputMixSign = 1;
 573             rcThrottlePrevious = rcCommand[THROTTLE];
 574             throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
 575             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 576         } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow)) {
 577             // INVERTED_TO_DEADBAND
 578             motorRangeMin = motorOutputLow;
 579             motorRangeMax = deadbandMotor3dLow;
 580             if(isMotorProtocolDshot()) {
 581                 motorOutputMin = motorOutputLow;
 582                 motorOutputRange = deadbandMotor3dLow - motorOutputLow;
 583             } else {
 584                 motorOutputMin = deadbandMotor3dLow;
 585                 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
 586             }
 587             motorOutputMixSign = -1;
 588             throttle = 0;
 589             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 590         } else {
 591             // NORMAL_TO_DEADBAND
 592             motorRangeMin = deadbandMotor3dHigh;
 593             motorRangeMax = motorOutputHigh;
 594             motorOutputMin = deadbandMotor3dHigh;
 595             motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
 596             motorOutputMixSign = 1;
 597             throttle = 0;
 598             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 599         }
 600     } else {
 601         throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
 602         currentThrottleInputRange = rcCommandThrottleRange;
 603         motorRangeMin = motorOutputLow;
 604         motorRangeMax = motorOutputHigh;
 605         motorOutputMin = motorOutputLow;
 606         motorOutputRange = motorOutputHigh - motorOutputLow;
 607         motorOutputMixSign = 1;
 608     }
 609
 610     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
 611 }
 612
 613 static void applyFlipOverAfterCrashModeToMotors(void)
 614 {
 615     float motorMix[MAX_SUPPORTED_MOTORS];
 616
 617     for (int i = 0; i < motorCount; i++) {
 618         if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) {
 619             motorMix[i] = getRcDeflection(FD_ROLL)  * currentMixer[i].roll * (-1);
 620         } else {
 621             motorMix[i] = getRcDeflection(FD_PITCH)  * currentMixer[i].pitch * (-1);
 622         }
 623     }
 624     // Apply the mix to motor endpoints
 625     for (uint32_t i = 0; i < motorCount; i++) {
 626         float motorOutput =  motorOutputMin + motorOutputRange * (motorMix[i]);
 627         //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
 628         motorOutput = (motorOutput < motorOutputMin + 20 ) ? disarmMotorOutput : motorOutput;
 629
 630         motor[i] = motorOutput;
 631     }
 632
 633     // Disarmed mode
 634     if (!ARMING_FLAG(ARMED)) {
 635         for (int i = 0; i < motorCount; i++) {
 636             motor[i] = motor_disarmed[i];
 637         }
 638     }
 639 }
 640
 641 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 642 {
 643     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 644     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 645     for (uint32_t i = 0; i < motorCount; i++) {
 646         float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
 647
 648         if (failsafeIsActive()) {
 649             if (isMotorProtocolDshot()) {
 650                 motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
 651             }
 652
 653             motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
 654         } else {
 655             motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
 656         }
 657
 658         // Motor stop handling
 659         if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
 660             if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
 661                 motorOutput = disarmMotorOutput;
 662             }
 663         }
 664         motor[i] = motorOutput;
 665     }
 666
 667     // Disarmed mode
 668     if (!ARMING_FLAG(ARMED)) {
 669         for (int i = 0; i < motorCount; i++) {
 670             motor[i] = motor_disarmed[i];
 671         }
 672     }
 673 }
 674
 675 void mixTable(uint8_t vbatPidCompensation)
 676 {
 677     if (isFlipOverAfterCrashMode()) {
 678         applyFlipOverAfterCrashModeToMotors();
 679         return;
 680     }
 681
 682     // Find min and max throttle based on conditions. Throttle has to be known before mixing
 683     calculateThrottleAndCurrentMotorEndpoints();
 684
 685     // Calculate and Limit the PIDsum
 686     float scaledAxisPidRoll =
 687         constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
 688     float scaledAxisPidPitch =
 689         constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit);
 690     float scaledAxisPidYaw =
 691         -constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw);
 692     if (isMotorsReversed()) {
 693         scaledAxisPidRoll = -scaledAxisPidRoll;
 694         scaledAxisPidPitch = -scaledAxisPidPitch;
 695         scaledAxisPidYaw = -scaledAxisPidYaw;
 696     }
 697     if (mixerConfig()->yaw_motors_reversed) {
 698         scaledAxisPidYaw = -scaledAxisPidYaw;
 699     }
 700
 701     // Calculate voltage compensation
 702     const float vbatCompensationFactor = (vbatPidCompensation)  ? calculateVbatPidCompensation() : 1.0f;
 703
 704     // Find roll/pitch/yaw desired output
 705     float motorMix[MAX_SUPPORTED_MOTORS];
 706     float motorMixMax = 0, motorMixMin = 0;
 707     for (int i = 0; i < motorCount; i++) {
 708         float mix =
 709             scaledAxisPidRoll  * currentMixer[i].roll +
 710             scaledAxisPidPitch * currentMixer[i].pitch +
 711             scaledAxisPidYaw   * currentMixer[i].yaw;
 712
 713         mix *= vbatCompensationFactor;  // Add voltage compensation
 714
 715         if (mix > motorMixMax) {
 716             motorMixMax = mix;
 717         } else if (mix < motorMixMin) {
 718             motorMixMin = mix;
 719         }
 720         motorMix[i] = mix;
 721     }
 722
 723     motorMixRange = motorMixMax - motorMixMin;
 724
 725     if (motorMixRange > 1.0f) {
 726         for (int i = 0; i < motorCount; i++) {
 727             motorMix[i] /= motorMixRange;
 728         }
 729         // Get the maximum correction by setting offset to center when airmode enabled
 730         if (isAirmodeActive()) {
 731             throttle = 0.5f;
 732         }
 733     } else {
 734         if (isAirmodeActive() || throttle > 0.5f) {  // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
 735             const float throttleLimitOffset = motorMixRange / 2.0f;
 736             throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset);
 737         }
 738     }
 739
 740     // Apply the mix to motor endpoints
 741     applyMixToMotors(motorMix);
 742 }
 743
 744 float convertExternalToMotor(uint16_t externalValue)
 745 {
 746     uint16_t motorValue = externalValue;
 747 #ifdef USE_DSHOT
 748     if (isMotorProtocolDshot()) {
 749         // Add 1 to the value, otherwise throttle tops out at 2046
 750         motorValue = externalValue <= EXTERNAL_CONVERSION_MIN_VALUE ? DSHOT_DISARM_COMMAND : constrain((externalValue - EXTERNAL_DSHOT_CONVERSION_OFFSET) * EXTERNAL_DSHOT_CONVERSION_FACTOR + 1, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
 751
 752         if (feature(FEATURE_3D)) {
 753             if (externalValue == EXTERNAL_CONVERSION_3D_MID_VALUE) {
 754                 motorValue = DSHOT_DISARM_COMMAND;
 755             } else if (motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {
 756                 // Add 1 to the value, otherwise throttle tops out at 2046
 757                 motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) + 1;
 758             }
 759         }
 760     }
 761 #endif
 762
 763     return (float)motorValue;
 764 }
 765
 766 uint16_t convertMotorToExternal(float motorValue)
 767 {
 768     uint16_t externalValue = lrintf(motorValue);
 769 #ifdef USE_DSHOT
 770     if (isMotorProtocolDshot()) {
 771         if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) {
 772             // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor()
 773             motorValue = DSHOT_MIN_THROTTLE + (DSHOT_3D_DEADBAND_LOW - motorValue) - 1;
 774         }
 775
 776         // Subtract 1 to compensate for imbalance introduced in convertExternalToMotor()
 777         externalValue = motorValue < DSHOT_MIN_THROTTLE ? EXTERNAL_CONVERSION_MIN_VALUE : constrain(((motorValue - 1)/ EXTERNAL_DSHOT_CONVERSION_FACTOR) + EXTERNAL_DSHOT_CONVERSION_OFFSET, EXTERNAL_CONVERSION_MIN_VALUE + 1, EXTERNAL_CONVERSION_MAX_VALUE);
 778
 779         if (feature(FEATURE_3D) && motorValue == DSHOT_DISARM_COMMAND) {
 780             externalValue = EXTERNAL_CONVERSION_3D_MID_VALUE;
 781         }
 782     }
 783 #endif
 784
 785     return externalValue;
 786 }