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
  22 #include "platform.h"
  23
  24 #include "build/debug.h"
  25
  26 #include "common/axis.h"
  27 #include "common/filter.h"
  28 #include "common/maths.h"
  29 #include "common/utils.h"
  30
  31 #include "config/config_reset.h"
  32 #include "config/feature.h"
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35 #include "config/config_reset.h"
  36
  37 #include "drivers/pwm_output.h"
  38 #include "drivers/pwm_mapping.h"
  39 #include "drivers/time.h"
  40
  41 #include "fc/config.h"
  42 #include "fc/rc_controls.h"
  43 #include "fc/rc_modes.h"
  44 #include "fc/runtime_config.h"
  45 #include "fc/controlrate_profile.h"
  46 #include "fc/settings.h"
  47
  48 #include "flight/failsafe.h"
  49 #include "flight/imu.h"
  50 #include "flight/mixer.h"
  51 #include "flight/pid.h"
  52 #include "flight/servos.h"
  53
  54 #include "navigation/navigation.h"
  55
  56 #include "rx/rx.h"
  57
  58 #include "sensors/battery.h"
  59
  60 FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
  61 FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
  62 static float motorMixRange;
  63 static float mixerScale = 1.0f;
  64 static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  65 static EXTENDED_FASTRAM uint8_t motorCount = 0;
  66 EXTENDED_FASTRAM int mixerThrottleCommand;
  67 static EXTENDED_FASTRAM int throttleIdleValue = 0;
  68 static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
  69 static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
  70 static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
  71 static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
  72 static EXTENDED_FASTRAM int throttleRangeMin = 0;
  73 static EXTENDED_FASTRAM int throttleRangeMax = 0;
  74 static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
  75
  76 int motorZeroCommand = 0;
  77
  78 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
  79
  80 PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
  81     .deadband_low = SETTING_3D_DEADBAND_LOW_DEFAULT,
  82     .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT,
  83     .neutral = SETTING_3D_NEUTRAL_DEFAULT
  84 );
  85
  86 PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10);
  87
  88 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
  89     .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
  90     .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT,
  91     .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT,
  92     .mincommand = SETTING_MIN_COMMAND_DEFAULT,
  93     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
  94 );
  95 PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
  96
  97 #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
  98
  99 void pgResetFn_timerOverrides(timerOverride_t *instance)
 100 {
 101     for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
 102         RESET_CONFIG(timerOverride_t, &instance[i], .outputMode = OUTPUT_MODE_AUTO);
 103     }
 104 }
 105
 106 int getThrottleIdleValue(void)
 107 {
 108     if (!throttleIdleValue) {
 109         throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * currentBatteryProfile->motor.throttleIdle);
 110     }
 111
 112     return throttleIdleValue;
 113 }
 114
 115 static void computeMotorCount(void)
 116 {
 117     static bool firstRun = true;
 118     if (!firstRun) {
 119         return;
 120     }
 121     motorCount = 0;
 122     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 123         bool isMotorUsed = false;
 124         for(int j = 0; j< MAX_MIXER_PROFILE_COUNT; j++){
 125             if (mixerMotorMixersByIndex(j)[i].throttle != 0.0f) {
 126                 isMotorUsed = true;
 127             }
 128         }
 129         // check if done
 130         if (!isMotorUsed) {
 131             break;
 132         }
 133         motorCount++;
 134     }
 135     firstRun = false;
 136 }
 137
 138 bool ifMotorstopFeatureEnabled(void){
 139     return currentMixerConfig.motorstopOnLow;
 140 }
 141
 142 uint8_t getMotorCount(void) {
 143     return motorCount;
 144 }
 145
 146 float getMotorMixRange(void)
 147 {
 148     return motorMixRange;
 149 }
 150
 151 bool mixerIsOutputSaturated(void)
 152 {
 153     return motorMixRange >= 1.0f;
 154 }
 155
 156 void mixerUpdateStateFlags(void)
 157 {
 158     DISABLE_STATE(FIXED_WING_LEGACY);
 159     DISABLE_STATE(MULTIROTOR);
 160     DISABLE_STATE(ROVER);
 161     DISABLE_STATE(BOAT);
 162     DISABLE_STATE(AIRPLANE);
 163     DISABLE_STATE(MOVE_FORWARD_ONLY);
 164
 165     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
 166         ENABLE_STATE(FIXED_WING_LEGACY);
 167         ENABLE_STATE(AIRPLANE);
 168         ENABLE_STATE(ALTITUDE_CONTROL);
 169         ENABLE_STATE(MOVE_FORWARD_ONLY);
 170     } if (currentMixerConfig.platformType == PLATFORM_ROVER) {
 171         ENABLE_STATE(ROVER);
 172         ENABLE_STATE(FIXED_WING_LEGACY);
 173         ENABLE_STATE(MOVE_FORWARD_ONLY);
 174     } if (currentMixerConfig.platformType == PLATFORM_BOAT) {
 175         ENABLE_STATE(BOAT);
 176         ENABLE_STATE(FIXED_WING_LEGACY);
 177         ENABLE_STATE(MOVE_FORWARD_ONLY);
 178     } else if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR) {
 179         ENABLE_STATE(MULTIROTOR);
 180         ENABLE_STATE(ALTITUDE_CONTROL);
 181     } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) {
 182         ENABLE_STATE(MULTIROTOR);
 183         ENABLE_STATE(ALTITUDE_CONTROL);
 184     } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) {
 185         ENABLE_STATE(MULTIROTOR);
 186         ENABLE_STATE(ALTITUDE_CONTROL);
 187     }
 188
 189     if (currentMixerConfig.hasFlaps) {
 190         ENABLE_STATE(FLAPERON_AVAILABLE);
 191     } else {
 192         DISABLE_STATE(FLAPERON_AVAILABLE);
 193     }
 194     if (
 195         currentMixerConfig.platformType == PLATFORM_BOAT ||
 196         currentMixerConfig.platformType == PLATFORM_ROVER ||
 197         navConfig()->fw.useFwNavYawControl
 198     ) {
 199         ENABLE_STATE(FW_HEADING_USE_YAW);
 200     } else {
 201         DISABLE_STATE(FW_HEADING_USE_YAW);
 202     }
 203 }
 204
 205 void nullMotorRateLimiting(const float dT)
 206 {
 207     UNUSED(dT);
 208 }
 209
 210 void mixerInit(void)
 211 {
 212     computeMotorCount();
 213     loadPrimaryMotorMixer();
 214     // in 3D mode, mixer gain has to be halved
 215     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 216         mixerScale = 0.5f;
 217     }
 218
 219     throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
 220     throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
 221
 222     mixerResetDisarmedMotors();
 223
 224     if (currentMixerConfig.motorDirectionInverted) {
 225         motorYawMultiplier = -1;
 226     } else {
 227         motorYawMultiplier = 1;
 228     }
 229 }
 230
 231 void mixerResetDisarmedMotors(void)
 232 {
 233     getThrottleIdleValue();
 234
 235     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 236         motorZeroCommand = reversibleMotorsConfig()->neutral;
 237         throttleRangeMin = throttleDeadbandHigh;
 238         throttleRangeMax = motorConfig()->maxthrottle;
 239     } else {
 240         motorZeroCommand = motorConfig()->mincommand;
 241         throttleRangeMin = throttleIdleValue;
 242         throttleRangeMax = motorConfig()->maxthrottle;
 243     }
 244
 245     reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 246
 247     if (ifMotorstopFeatureEnabled()) {
 248         motorValueWhenStopped = motorZeroCommand;
 249     } else {
 250         motorValueWhenStopped = throttleIdleValue;
 251     }
 252
 253     // set disarmed motor values
 254     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 255         motor_disarmed[i] = motorZeroCommand;
 256     }
 257 }
 258
 259 #ifdef USE_DSHOT
 260 static uint16_t handleOutputScaling(
 261     int16_t input,          // Input value from the mixer
 262     int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
 263     int16_t onStopValue,    // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
 264     int16_t inputScaleMin,  // Input range - min value
 265     int16_t inputScaleMax,  // Input range - max value
 266     int16_t outputScaleMin, // Output range - min value
 267     int16_t outputScaleMax, // Output range - max value
 268     bool moveForward        // If motor should be rotating FORWARD or BACKWARD
 269 )
 270 {
 271     int value;
 272     if (moveForward && input < stopThreshold) {
 273         //Send motor stop command
 274         value = onStopValue;
 275     }
 276     else if (!moveForward && input > stopThreshold) {
 277         //Send motor stop command
 278         value = onStopValue;
 279     }
 280     else {
 281         //Scale input to protocol output values
 282         value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
 283         value = constrain(value, outputScaleMin, outputScaleMax);
 284     }
 285     return value;
 286 }
 287 static void applyTurtleModeToMotors(void) {
 288
 289     if (ARMING_FLAG(ARMED)) {
 290         const float flipPowerFactor = ((float)currentBatteryProfile->motor.turtleModePowerFactor)/100.0f;
 291         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
 292         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
 293         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
 294         //deflection stick position
 295
 296         const float stickDeflectionPitchExpo =
 297                 flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
 298         const float stickDeflectionRollExpo =
 299                 flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
 300         const float stickDeflectionYawExpo =
 301                 flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
 302
 303         float signPitch = rcCommand[PITCH] < 0 ? 1 : -1;
 304         float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
 305         float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (currentMixerConfig.motorDirectionInverted ? 1 : -1));
 306
 307         float stickDeflectionLength = calc_length_pythagorean_2D(stickDeflectionPitchAbs, stickDeflectionRollAbs);
 308         float stickDeflectionExpoLength = calc_length_pythagorean_2D(stickDeflectionPitchExpo, stickDeflectionRollExpo);
 309
 310         if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
 311             // If yaw is the dominant, disable pitch and roll
 312             stickDeflectionLength = stickDeflectionYawAbs;
 313             stickDeflectionExpoLength = stickDeflectionYawExpo;
 314             signRoll = 0;
 315             signPitch = 0;
 316         } else {
 317             // If pitch/roll dominant, disable yaw
 318             signYaw = 0;
 319         }
 320
 321         const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
 322                                                            (fast_fsqrtf(2.0f) * stickDeflectionLength) : 0;
 323         const float cosThreshold = fast_fsqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
 324
 325         if (cosPhi < cosThreshold) {
 326             // Enforce either roll or pitch exclusively, if not on diagonal
 327             if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
 328                 signPitch = 0;
 329             } else {
 330                 signRoll = 0;
 331             }
 332         }
 333
 334         // Apply a reasonable amount of stick deadband
 335         const float crashFlipStickMinExpo =
 336                 flipPowerFactor * CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN + power3(CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN) * (1 - flipPowerFactor);
 337         const float flipStickRange = 1.0f - crashFlipStickMinExpo;
 338         const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
 339
 340         for (int i = 0; i < motorCount; ++i) {
 341
 342             float motorOutputNormalised =
 343                     signPitch * currentMixer[i].pitch +
 344                     signRoll * currentMixer[i].roll +
 345                     signYaw * currentMixer[i].yaw;
 346
 347             if (motorOutputNormalised < 0) {
 348                 motorOutputNormalised = 0;
 349             }
 350
 351             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 352
 353             motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle);
 354         }
 355     } else {
 356         // Disarmed mode
 357         stopMotors();
 358     }
 359 }
 360 #endif
 361
 362 void FAST_CODE writeMotors(void)
 363 {
 364 #if !defined(SITL_BUILD)
 365     for (int i = 0; i < motorCount; i++) {
 366         uint16_t motorValue;
 367
 368 #ifdef USE_DSHOT
 369         // If we use DSHOT we need to convert motorValue to DSHOT ranges
 370         if (isMotorProtocolDigital()) {
 371
 372             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 373                 if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
 374                     motorValue = handleOutputScaling(
 375                         motor[i],
 376                         throttleRangeMin,
 377                         DSHOT_DISARM_COMMAND,
 378                         throttleRangeMin,
 379                         throttleRangeMax,
 380                         DSHOT_3D_DEADBAND_HIGH,
 381                         DSHOT_MAX_THROTTLE,
 382                         true
 383                     );
 384                 } else {
 385                     motorValue = handleOutputScaling(
 386                         motor[i],
 387                         throttleRangeMax,
 388                         DSHOT_DISARM_COMMAND,
 389                         throttleRangeMin,
 390                         throttleRangeMax,
 391                         DSHOT_MIN_THROTTLE,
 392                         DSHOT_3D_DEADBAND_LOW,
 393                         false
 394                     );
 395                 }
 396             }
 397             else {
 398                 motorValue = handleOutputScaling(
 399                     motor[i],
 400                     throttleIdleValue,
 401                     DSHOT_DISARM_COMMAND,
 402                     motorConfig()->mincommand,
 403                     motorConfig()->maxthrottle,
 404                     DSHOT_MIN_THROTTLE,
 405                     DSHOT_MAX_THROTTLE,
 406                     true
 407                 );
 408             }
 409         }
 410         else {
 411             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 412                 if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
 413                     motorValue = handleOutputScaling(
 414                         motor[i],
 415                         throttleRangeMin,
 416                         motor[i],
 417                         throttleRangeMin,
 418                         throttleRangeMax,
 419                         reversibleMotorsConfig()->deadband_high,
 420                         motorConfig()->maxthrottle,
 421                         true
 422                     );
 423                 } else {
 424                     motorValue = handleOutputScaling(
 425                         motor[i],
 426                         throttleRangeMax,
 427                         motor[i],
 428                         throttleRangeMin,
 429                         throttleRangeMax,
 430                         motorConfig()->mincommand,
 431                         reversibleMotorsConfig()->deadband_low,
 432                         false
 433                     );
 434                 }
 435             } else {
 436                 motorValue = motor[i];
 437             }
 438
 439         }
 440 #else
 441         // We don't define USE_DSHOT
 442         motorValue = motor[i];
 443 #endif
 444
 445         pwmWriteMotor(i, motorValue);
 446     }
 447 #endif
 448 }
 449
 450 void writeAllMotors(int16_t mc)
 451 {
 452     // Sends commands to all motors
 453     for (int i = 0; i < motorCount; i++) {
 454         motor[i] = mc;
 455     }
 456     writeMotors();
 457 }
 458
 459 void stopMotors(void)
 460 {
 461     writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
 462
 463     delay(50); // give the timers and ESCs a chance to react.
 464 }
 465
 466 void stopPwmAllMotors(void)
 467 {
 468 #if !defined(SITL_BUILD)
 469     pwmShutdownPulsesForAllMotors(motorCount);
 470 #endif
 471
 472 }
 473
 474 static int getReversibleMotorsThrottleDeadband(void)
 475 {
 476     int directionValue;
 477
 478     if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
 479         directionValue = reversibleMotorsConfig()->deadband_low;
 480     } else {
 481         directionValue = reversibleMotorsConfig()->deadband_high;
 482     }
 483
 484     return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue;
 485 }
 486
 487 void FAST_CODE mixTable(void)
 488 {
 489 #ifdef USE_DSHOT
 490     if (FLIGHT_MODE(TURTLE_MODE)) {
 491         applyTurtleModeToMotors();
 492         return;
 493     }
 494 #endif
 495 #ifdef USE_DEV_TOOLS
 496     bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode;
 497 #else
 498     bool isDisarmed = !ARMING_FLAG(ARMED);
 499 #endif
 500     bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed;
 501     if (isDisarmed || motorStopIsActive) {
 502         for (int i = 0; i < motorCount; i++) {
 503             motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped;
 504         }
 505         mixerThrottleCommand = motor[0];
 506         return;
 507     }
 508
 509     int16_t input[3];   // RPY, range [-500:+500]
 510     // Allow direct stick input to motors in passthrough mode on airplanes
 511     if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
 512         // Direct passthru from RX
 513         input[ROLL] = rcCommand[ROLL];
 514         input[PITCH] = rcCommand[PITCH];
 515         input[YAW] = rcCommand[YAW];
 516     }
 517     else {
 518         input[ROLL] = axisPID[ROLL];
 519         input[PITCH] = axisPID[PITCH];
 520         input[YAW] = axisPID[YAW];
 521     }
 522
 523     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
 524     int16_t rpyMix[MAX_SUPPORTED_MOTORS];
 525     int16_t rpyMixMax = 0; // assumption: symetrical about zero.
 526     int16_t rpyMixMin = 0;
 527
 528     // motors for non-servo mixes
 529     for (int i = 0; i < motorCount; i++) {
 530         rpyMix[i] =
 531             (input[PITCH] * currentMixer[i].pitch +
 532             input[ROLL] * currentMixer[i].roll +
 533             -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
 534
 535         if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
 536         if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
 537     }
 538
 539     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
 540     int16_t throttleRange;
 541     int16_t throttleMin, throttleMax;
 542
 543     // Find min and max throttle based on condition.
 544 #ifdef USE_PROGRAMMING_FRAMEWORK
 545     if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) {
 546         throttleRangeMin = throttleIdleValue;
 547         throttleRangeMax = motorConfig()->maxthrottle;
 548         mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
 549     } else
 550 #endif
 551     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 552
 553         if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) {
 554             /*
 555              * Throttle is above deadband, FORWARD direction
 556              */
 557             reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 558             throttleRangeMax = motorConfig()->maxthrottle;
 559             throttleRangeMin = throttleDeadbandHigh;
 560             DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
 561         } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
 562             /*
 563              * Throttle is below deadband, BACKWARD direction
 564              */
 565             reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
 566             throttleRangeMax = throttleDeadbandLow;
 567             throttleRangeMin = motorConfig()->mincommand;
 568         }
 569
 570
 571         motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
 572         mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
 573
 574 #ifdef USE_DSHOT
 575         if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
 576             /*
 577              * We need to start the throttle output from stick input to start in the middle of the stick at the low and.
 578              * Without this, it's starting at the high side.
 579              */
 580             int throttleDistanceToMax = throttleRangeMax - rcCommand[THROTTLE];
 581             mixerThrottleCommand = throttleRangeMin + throttleDistanceToMax;
 582         }
 583 #endif
 584     } else {
 585         mixerThrottleCommand = rcCommand[THROTTLE];
 586         throttleRangeMin = throttleIdleValue;
 587         throttleRangeMax = motorConfig()->maxthrottle;
 588
 589         // Throttle scaling to limit max throttle when battery is full
 590 #ifdef USE_PROGRAMMING_FRAMEWORK
 591         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
 592 #else
 593         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
 594 #endif
 595         // Throttle compensation based on battery voltage
 596         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
 597             mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
 598         }
 599     }
 600
 601     throttleMin = throttleRangeMin;
 602     throttleMax = throttleRangeMax;
 603     throttleRange = throttleMax - throttleMin;
 604
 605     #define THROTTLE_CLIPPING_FACTOR    0.33f
 606     motorMixRange = (float)rpyMixRange / (float)throttleRange;
 607     if (motorMixRange > 1.0f) {
 608         for (int i = 0; i < motorCount; i++) {
 609             rpyMix[i] /= motorMixRange;
 610         }
 611
 612         // Allow some clipping on edges to soften correction response
 613         throttleMin = throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 614         throttleMax = throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 615     } else {
 616         throttleMin = MIN(throttleMin + (rpyMixRange / 2), throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 617         throttleMax = MAX(throttleMax - (rpyMixRange / 2), throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 618     }
 619
 620     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 621     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 622     for (int i = 0; i < motorCount; i++) {
 623         motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
 624
 625         if (failsafeIsActive()) {
 626             motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
 627         } else {
 628             motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
 629         }
 630
 631         //stop motors
 632         if (currentMixer[i].throttle <= 0.0f) {
 633             motor[i] = motorZeroCommand;
 634         }
 635         //spin stopped motors only in mixer transition mode
 636         if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) {
 637             motor[i] = -currentMixer[i].throttle * 1000;
 638             motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
 639         }
 640     }
 641 }
 642
 643 int16_t getThrottlePercent(bool useScaled)
 644 {
 645     int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX);
 646
 647     if (useScaled) {
 648        thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue);
 649     } else {
 650         thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 651     }
 652     return thr;
 653 }
 654
 655 uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop)
 656 {
 657     const uint16_t throttleIdleValue = getThrottleIdleValue();
 658
 659     if (allowMotorStop && throttle < throttleIdleValue) {
 660         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 661         return throttle;
 662     }
 663     return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle);
 664 }
 665
 666 motorStatus_e getMotorStatus(void)
 667 {
 668     if (STATE(NAV_MOTOR_STOP_OR_IDLE)) {
 669         return MOTOR_STOPPED_AUTO;
 670     }
 671
 672     const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
 673
 674     if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
 675         if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
 676             // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
 677             // and either on a plane or on a quad with inactive airmode - stop motor
 678             return MOTOR_STOPPED_USER;
 679
 680         } else if (!failsafeIsActive()) {
 681             // If user is holding stick low, we are not in failsafe and either on a plane or on a quad with inactive
 682             // airmode - we need to check if we are allowing navigation to override MOTOR_STOP
 683
 684             switch (navConfig()->general.flags.nav_overrides_motor_stop) {
 685                 case NOMS_ALL_NAV:
 686                     return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
 687
 688                 case NOMS_AUTO_ONLY:
 689                     return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
 690
 691                 case NOMS_OFF:
 692                 default:
 693                     return MOTOR_STOPPED_USER;
 694             }
 695         }
 696     }
 697
 698     return MOTOR_RUNNING;
 699 }
 700
 701 void loadPrimaryMotorMixer(void) {
 702     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 703         currentMixer[i] = *primaryMotorMixer(i);
 704     }
 705 }
 706
 707 bool areMotorsRunning(void)
 708 {
 709     if (ARMING_FLAG(ARMED)) {
 710         return true;
 711     } else {
 712         for (int i = 0; i < motorCount; i++) {
 713             if (motor_disarmed[i] != motorZeroCommand) {
 714                 return true;
 715             }
 716         }
 717     }
 718
 719     return false;
 720 }