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