src/main/flight/mixer.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   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.
   9  *
  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.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #include "build/build_config.h"
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/filter.h"
  33 #include "common/maths.h"
  34
  35 #include "config/feature.h"
  36 #include "pg/pg.h"
  37 #include "pg/pg_ids.h"
  38 #include "pg/rx.h"
  39
  40 #include "drivers/pwm_output.h"
  41 #include "drivers/pwm_esc_detect.h"
  42 #include "drivers/time.h"
  43 #include "drivers/io.h"
  44
  45 #include "io/motors.h"
  46
  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"
  54
  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"
  61
  62 #include "rx/rx.h"
  63
  64 #include "sensors/battery.h"
  65 #include "sensors/gyro.h"
  66
  67 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
  68
  69 #define DYN_LPF_THROTTLE_STEPS           100
  70 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
  71
  72 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  73     .mixerMode = DEFAULT_MIXER,
  74     .yaw_motors_reversed = false,
  75     .crashflip_motor_percent = 0,
  76 );
  77
  78 PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
  79
  80 void pgResetFn_motorConfig(motorConfig_t *motorConfig)
  81 {
  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
  96     {
  97         motorConfig->minthrottle = 1070;
  98         motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
  99         motorConfig->dev.motorPwmProtocol = PWM_TYPE_ONESHOT125;
 100     }
 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
 108
 109     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
 110         motorConfig->dev.ioTags[motorIndex] = timerioTagGetByUsage(TIM_USE_MOTOR, motorIndex);
 111     }
 112
 113     motorConfig->motorPoleCount = 14;   // Most brushes motors that we use are 14 poles
 114 }
 115
 116 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
 117
 118 #define PWM_RANGE_MID 1500
 119
 120 static FAST_RAM_ZERO_INIT uint8_t motorCount;
 121 static FAST_RAM_ZERO_INIT float motorMixRange;
 122
 123 float FAST_RAM_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
 124 float motor_disarmed[MAX_SUPPORTED_MOTORS];
 125
 126 mixerMode_e currentMixerMode;
 127 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 128
 129 #ifdef USE_LAUNCH_CONTROL
 130 static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS];
 131 #endif
 132
 133 static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
 134
 135
 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
 141 };
 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
 147 };
 148
 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
 154 };
 155
 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
 160 };
 161 #else
 162 #define mixerBicopter NULL
 163 #endif
 164
 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
 170 };
 171
 172
 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
 181 };
 182
 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
 191 };
 192
 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
 200 };
 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
 208 };
 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
 217
 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
 228 };
 229
 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
 239 };
 240
 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
 250 };
 251 #else
 252 #define mixerOctoX8 NULL
 253 #define mixerOctoFlatP NULL
 254 #define mixerOctoFlatX NULL
 255 #endif
 256
 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
 262 };
 263
 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
 269 };
 270
 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
 275 };
 276 #else
 277 #define mixerDualcopter NULL
 278 #endif
 279
 280 static const motorMixer_t mixerSingleProp[] = {
 281     { 1.0f,  0.0f,  0.0f, 0.0f },
 282 };
 283
 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
 289 };
 290
 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 },
 323 };
 324 #endif // !USE_QUAD_MIXER_ONLY
 325
 326 FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow;
 327
 328 static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow;
 329 static FAST_RAM_ZERO_INIT float rcCommandThrottleRange;
 330
 331 uint8_t getMotorCount(void)
 332 {
 333     return motorCount;
 334 }
 335
 336 float getMotorMixRange(void)
 337 {
 338     return motorMixRange;
 339 }
 340
 341 bool areMotorsRunning(void)
 342 {
 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;
 350
 351                 break;
 352             }
 353         }
 354     }
 355
 356     return motorsRunning;
 357 }
 358
 359 #ifdef USE_SERVOS
 360 bool mixerIsTricopter(void)
 361 {
 362     return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI);
 363 }
 364 #endif
 365
 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)
 369 {
 370     float motorOutputLimit = 1.0f;
 371     if (currentPidProfile->motor_output_limit < 100) {
 372         motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
 373     }
 374
 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:
 383         {
 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;
 394             }
 395         }
 396
 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));
 411         }
 412         break;
 413     }
 414
 415     rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN;
 416 }
 417
 418 void mixerInit(mixerMode_e mixerMode)
 419 {
 420     currentMixerMode = mixerMode;
 421
 422     initEscEndpoints();
 423 #ifdef USE_SERVOS
 424     if (mixerIsTricopter()) {
 425         mixerTricopterInit();
 426     }
 427 #endif
 428 }
 429
 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)
 435 {
 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;
 442         }
 443     }
 444 }
 445 #endif
 446
 447 #ifndef USE_QUAD_MIXER_ONLY
 448
 449 void mixerConfigureOutput(void)
 450 {
 451     motorCount = 0;
 452
 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;
 459             }
 460             currentMixer[i] = *customMotorMixer(i);
 461             motorCount++;
 462         }
 463     } else {
 464         motorCount = mixers[currentMixerMode].motorCount;
 465         if (motorCount > MAX_SUPPORTED_MOTORS) {
 466             motorCount = MAX_SUPPORTED_MOTORS;
 467         }
 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];
 472         }
 473     }
 474 #ifdef USE_LAUNCH_CONTROL
 475     loadLaunchControlMixer();
 476 #endif
 477     mixerResetDisarmedMotors();
 478 }
 479
 480 void mixerLoadMix(int index, motorMixer_t *customMixers)
 481 {
 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;
 487     }
 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];
 492         }
 493     }
 494 }
 495 #else
 496 void mixerConfigureOutput(void)
 497 {
 498     motorCount = QUAD_MOTOR_COUNT;
 499     for (int i = 0; i < motorCount; i++) {
 500         currentMixer[i] = mixerQuadX[i];
 501     }
 502 #ifdef USE_LAUNCH_CONTROL
 503     loadLaunchControlMixer();
 504 #endif
 505     mixerResetDisarmedMotors();
 506 }
 507 #endif // USE_QUAD_MIXER_ONLY
 508
 509 void mixerResetDisarmedMotors(void)
 510 {
 511     // set disarmed motor values
 512     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 513         motor_disarmed[i] = disarmMotorOutput;
 514     }
 515 }
 516
 517 void writeMotors(void)
 518 {
 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]);
 525         }
 526         pwmCompleteMotorUpdate(motorCount);
 527     }
 528 }
 529
 530 static void writeAllMotors(int16_t mc)
 531 {
 532     // Sends commands to all motors
 533     for (int i = 0; i < motorCount; i++) {
 534         motor[i] = mc;
 535     }
 536     writeMotors();
 537 }
 538
 539 void stopMotors(void)
 540 {
 541     writeAllMotors(disarmMotorOutput);
 542     delay(50); // give the timers and ESCs a chance to react.
 543 }
 544
 545 void stopPwmAllMotors(void)
 546 {
 547     pwmShutdownPulsesForAllMotors(motorCount);
 548     delayMicroseconds(1500);
 549 }
 550
 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;
 558
 559 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 560 {
 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;
 564
 565     if (featureIsEnabled(FEATURE_3D)) {
 566         uint16_t rcCommand3dDeadBandLow;
 567         uint16_t rcCommand3dDeadBandHigh;
 568
 569         if (!ARMING_FLAG(ARMED)) {
 570             rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 571         }
 572
 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;
 583         }
 584
 585         const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
 586         const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
 587
 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;
 598             }
 599             if (motorOutputMixSign != -1) {
 600                 reversalTimeUs = currentTimeUs;
 601             }
 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;
 614             }
 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;
 631             }
 632             if (motorOutputMixSign != -1) {
 633                 reversalTimeUs = currentTimeUs;
 634             }
 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;
 646             }
 647             motorOutputMixSign = 1;
 648             throttle = 0;
 649             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 650         }
 651         if (currentTimeUs - reversalTimeUs < 250000) {
 652             // keep iterm zero for 250ms after motor reversal
 653             pidResetIterm();
 654         }
 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;
 663     }
 664
 665     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
 666 }
 667
 668 #define CRASH_FLIP_DEADBAND 20
 669 #define CRASH_FLIP_STICK_MINF 0.15f
 670
 671 static void applyFlipOverAfterCrashModeToMotors(void)
 672 {
 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);
 680
 681         float stickDeflectionLength = sqrtf(stickDeflectionPitchAbs*stickDeflectionPitchAbs + stickDeflectionRollAbs*stickDeflectionRollAbs);
 682
 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;
 691         }
 692
 693         float cosPhi = (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength);
 694         const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
 695
 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;
 702             }
 703         }
 704
 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;
 708
 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;
 714
 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;
 720                 }
 721             }
 722             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 723             float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange;
 724
 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);
 727
 728             motor[i] = motorOutput;
 729         }
 730     } else {
 731         // Disarmed mode
 732         for (int i = 0; i < motorCount; i++) {
 733             motor[i] = motor_disarmed[i];
 734         }
 735     }
 736 }
 737
 738 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
 739 {
 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;
 748
 749 #ifdef USE_SERVOS
 750         if (mixerIsTricopter()) {
 751             motorOutput += mixerTricopterMotorCorrection(i);
 752         }
 753 #endif
 754         if (failsafeIsActive()) {
 755             if (isMotorProtocolDshot()) {
 756                 motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
 757             }
 758             motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax);
 759         } else {
 760             motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
 761         }
 762         motor[i] = motorOutput;
 763     }
 764
 765     // Disarmed mode
 766     if (!ARMING_FLAG(ARMED)) {
 767         for (int i = 0; i < motorCount; i++) {
 768             motor[i] = motor_disarmed[i];
 769         }
 770     }
 771 }
 772
 773 float applyThrottleLimit(float throttle)
 774 {
 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);
 782         }
 783     }
 784
 785     return throttle;
 786 }
 787
 788 void applyMotorStop(void)
 789 {
 790     for (int i = 0; i < motorCount; i++) {
 791         motor[i] = disarmMotorOutput;
 792     }
 793 }
 794
 795 #ifdef USE_DYN_LPF
 796 void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
 797 {
 798     static timeUs_t lastDynLpfUpdateUs = 0;
 799     static int dynLpfPreviousQuantizedThrottle = -1;  // to allow an initial zero throttle to set the filter cutoff
 800
 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;
 810         }
 811     }
 812 }
 813 #endif
 814
 815 FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
 816 {
 817     // Find min and max throttle based on conditions. Throttle has to be known before mixing
 818     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 819
 820     if (isFlipOverAfterCrashActive()) {
 821         applyFlipOverAfterCrashModeToMotors();
 822
 823         return;
 824     }
 825
 826     const bool launchControlActive = isLaunchControlActive();
 827
 828     motorMixer_t * activeMixer = &currentMixer[0];
 829 #ifdef USE_LAUNCH_CONTROL
 830     if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
 831         activeMixer = &launchControlMixer[0];
 832     }
 833 #endif
 834
 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;
 840
 841     uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
 842
 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
 847     }
 848 #endif // USE_YAW_SPIN_RECOVERY
 849
 850     float scaledAxisPidYaw =
 851         constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
 852
 853     if (!mixerConfig()->yaw_motors_reversed) {
 854         scaledAxisPidYaw = -scaledAxisPidYaw;
 855     }
 856
 857     // Calculate voltage compensation
 858     const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
 859
 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);
 863     }
 864
 865     const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
 866
 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;   //
 872     }
 873 #endif // USE_YAW_SPIN_RECOVERY
 874
 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;
 880     }
 881 #endif
 882
 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++) {
 887
 888         float mix =
 889             scaledAxisPidRoll  * activeMixer[i].roll +
 890             scaledAxisPidPitch * activeMixer[i].pitch +
 891             scaledAxisPidYaw   * activeMixer[i].yaw;
 892
 893         mix *= vbatCompensationFactor;  // Add voltage compensation
 894
 895         if (mix > motorMixMax) {
 896             motorMixMax = mix;
 897         } else if (mix < motorMixMin) {
 898             motorMixMin = mix;
 899         }
 900         motorMix[i] = mix;
 901     }
 902
 903     pidUpdateAntiGravityThrottleFilter(throttle);
 904
 905 #ifdef USE_DYN_LPF
 906     updateDynLpfCutoffs(currentTimeUs, throttle);
 907 #endif
 908
 909 #ifdef USE_THRUST_LINEARIZATION
 910     // reestablish old throttle stick feel by counter compensating thrust linearization
 911     throttle = pidCompensateThrustLinearization(throttle);
 912 #endif
 913
 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);
 918     }
 919 #endif
 920
 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();
 926     }
 927 #endif
 928
 929     loggingThrottle = throttle;
 930     motorMixRange = motorMixMax - motorMixMin;
 931     if (motorMixRange > 1.0f) {
 932         for (int i = 0; i < motorCount; i++) {
 933             motorMix[i] /= motorMixRange;
 934         }
 935         // Get the maximum correction by setting offset to center when airmode enabled
 936         if (airmodeEnabled) {
 937             throttle = 0.5f;
 938         }
 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);
 942         }
 943     }
 944
 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);
 956     }
 957 }
 958
 959 float convertExternalToMotor(uint16_t externalValue)
 960 {
 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);
 966
 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);
 974             }
 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);
 977         }
 978
 979         break;
 980     case false:
 981 #endif
 982     default:
 983         motorValue = externalValue;
 984         break;
 985     }
 986
 987     return (float)motorValue;
 988 }
 989
 990 uint16_t convertMotorToExternal(float motorValue)
 991 {
 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);
1003             }
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);
1006         }
1007         break;
1008     case false:
1009 #endif
1010     default:
1011         externalValue = motorValue;
1012         break;
1013     }
1014
1015     return externalValue;
1016 }
1017
1018 void mixerSetThrottleAngleCorrection(int correctionValue)
1019 {
1020     throttleAngleCorrection = correctionValue;
1021 }
1022
1023 float mixerGetLoggingThrottle(void)
1024 {
1025     return loggingThrottle;
1026 }