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