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 #include "programming/global_functions.h"
  33
  34 #include "config/feature.h"
  35 #include "config/parameter_group.h"
  36 #include "config/parameter_group_ids.h"
  37
  38 #include "drivers/pwm_output.h"
  39 #include "drivers/pwm_mapping.h"
  40 #include "drivers/time.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/rc_controls.h"
  44 #include "fc/rc_modes.h"
  45 #include "fc/runtime_config.h"
  46
  47 #include "flight/failsafe.h"
  48 #include "flight/imu.h"
  49 #include "flight/mixer.h"
  50 #include "flight/pid.h"
  51 #include "flight/servos.h"
  52
  53 #include "navigation/navigation.h"
  54
  55 #include "rx/rx.h"
  56
  57 #include "sensors/battery.h"
  58
  59 FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
  60 FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
  61 static float motorMixRange;
  62 static float mixerScale = 1.0f;
  63 static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
  64 static EXTENDED_FASTRAM uint8_t motorCount = 0;
  65 EXTENDED_FASTRAM int mixerThrottleCommand;
  66 static EXTENDED_FASTRAM int throttleIdleValue = 0;
  67 static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
  68 static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
  69 static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
  70 static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
  71 static EXTENDED_FASTRAM int throttleRangeMin = 0;
  72 static EXTENDED_FASTRAM int throttleRangeMax = 0;
  73 static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
  74
  75 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
  76
  77 PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
  78     .deadband_low = 1406,
  79     .deadband_high = 1514,
  80     .neutral = 1460
  81 );
  82
  83 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3);
  84
  85 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  86     .motorDirectionInverted = 0,
  87     .platformType = PLATFORM_MULTIROTOR,
  88     .hasFlaps = false,
  89     .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only
  90     .fwMinThrottleDownPitchAngle = 0
  91 );
  92
  93 #ifdef BRUSHED_MOTORS
  94 #define DEFAULT_PWM_PROTOCOL    PWM_TYPE_BRUSHED
  95 #define DEFAULT_PWM_RATE        16000
  96 #else
  97 #define DEFAULT_PWM_PROTOCOL    PWM_TYPE_STANDARD
  98 #define DEFAULT_PWM_RATE        400
  99 #endif
 100
 101 #define DEFAULT_MAX_THROTTLE    1850
 102
 103 PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 5);
 104
 105 PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
 106     .motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
 107     .motorPwmRate = DEFAULT_PWM_RATE,
 108     .maxthrottle = DEFAULT_MAX_THROTTLE,
 109     .mincommand = 1000,
 110     .motorAccelTimeMs = 0,
 111     .motorDecelTimeMs = 0,
 112     .throttleIdle = 15.0f,
 113     .throttleScale = 1.0f,
 114     .motorPoleCount = 14            // Most brushless motors that we use are 14 poles
 115 );
 116
 117 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 118
 119 typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
 120 static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
 121
 122 int getThrottleIdleValue(void)
 123 {
 124     if (!throttleIdleValue) {
 125         throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle);
 126     }
 127
 128     return throttleIdleValue;
 129 }
 130
 131 static void computeMotorCount(void)
 132 {
 133     motorCount = 0;
 134     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 135         // check if done
 136         if (primaryMotorMixer(i)->throttle == 0.0f) {
 137             break;
 138         }
 139         motorCount++;
 140     }
 141 }
 142
 143 uint8_t getMotorCount(void) {
 144     return motorCount;
 145 }
 146
 147 float getMotorMixRange(void)
 148 {
 149     return motorMixRange;
 150 }
 151
 152 bool mixerIsOutputSaturated(void)
 153 {
 154     return motorMixRange >= 1.0f;
 155 }
 156
 157 void mixerUpdateStateFlags(void)
 158 {
 159     DISABLE_STATE(FIXED_WING_LEGACY);
 160     DISABLE_STATE(MULTIROTOR);
 161     DISABLE_STATE(ROVER);
 162     DISABLE_STATE(BOAT);
 163     DISABLE_STATE(AIRPLANE);
 164     DISABLE_STATE(MOVE_FORWARD_ONLY);
 165
 166     if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
 167         ENABLE_STATE(FIXED_WING_LEGACY);
 168         ENABLE_STATE(AIRPLANE);
 169         ENABLE_STATE(ALTITUDE_CONTROL);
 170         ENABLE_STATE(MOVE_FORWARD_ONLY);
 171     } if (mixerConfig()->platformType == PLATFORM_ROVER) {
 172         ENABLE_STATE(ROVER);
 173         ENABLE_STATE(FIXED_WING_LEGACY);
 174         ENABLE_STATE(MOVE_FORWARD_ONLY);
 175     } if (mixerConfig()->platformType == PLATFORM_BOAT) {
 176         ENABLE_STATE(BOAT);
 177         ENABLE_STATE(FIXED_WING_LEGACY);
 178         ENABLE_STATE(MOVE_FORWARD_ONLY);
 179     } else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
 180         ENABLE_STATE(MULTIROTOR);
 181         ENABLE_STATE(ALTITUDE_CONTROL);
 182     } else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) {
 183         ENABLE_STATE(MULTIROTOR);
 184         ENABLE_STATE(ALTITUDE_CONTROL);
 185     } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
 186         ENABLE_STATE(MULTIROTOR);
 187         ENABLE_STATE(ALTITUDE_CONTROL);
 188     }
 189
 190     if (mixerConfig()->hasFlaps) {
 191         ENABLE_STATE(FLAPERON_AVAILABLE);
 192     } else {
 193         DISABLE_STATE(FLAPERON_AVAILABLE);
 194     }
 195 }
 196
 197 void nullMotorRateLimiting(const float dT)
 198 {
 199     UNUSED(dT);
 200 }
 201
 202 void applyMotorRateLimiting(const float dT)
 203 {
 204     static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
 205
 206     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 207         // FIXME: Don't apply rate limiting in 3D mode
 208         for (int i = 0; i < motorCount; i++) {
 209             motorPrevious[i] = motor[i];
 210         }
 211     }
 212     else {
 213         // Calculate max motor step
 214         const uint16_t motorRange = motorConfig()->maxthrottle - throttleIdleValue;
 215         const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
 216         const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
 217
 218         for (int i = 0; i < motorCount; i++) {
 219             // Apply motor rate limiting
 220             motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
 221
 222             // Handle throttle below min_throttle (motor start/stop)
 223             if (motorPrevious[i] < throttleIdleValue) {
 224                 if (motor[i] < throttleIdleValue) {
 225                     motorPrevious[i] = motor[i];
 226                 }
 227                 else {
 228                     motorPrevious[i] = throttleIdleValue;
 229                 }
 230             }
 231         }
 232     }
 233
 234     // Update motor values
 235     for (int i = 0; i < motorCount; i++) {
 236         motor[i] = motorPrevious[i];
 237     }
 238 }
 239
 240 void mixerInit(void)
 241 {
 242     computeMotorCount();
 243     loadPrimaryMotorMixer();
 244     // in 3D mode, mixer gain has to be halved
 245     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 246         mixerScale = 0.5f;
 247     }
 248
 249     throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
 250     throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
 251
 252     mixerResetDisarmedMotors();
 253
 254     if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
 255         motorRateLimitingApplyFn = applyMotorRateLimiting;
 256     } else {
 257         motorRateLimitingApplyFn = nullMotorRateLimiting;
 258     }
 259
 260     if (mixerConfig()->motorDirectionInverted) {
 261         motorYawMultiplier = -1;
 262     } else {
 263         motorYawMultiplier = 1;
 264     }
 265 }
 266
 267 void mixerResetDisarmedMotors(void)
 268 {
 269     int motorZeroCommand;
 270
 271     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 272         motorZeroCommand = reversibleMotorsConfig()->neutral;
 273         throttleRangeMin = throttleDeadbandHigh;
 274         throttleRangeMax = motorConfig()->maxthrottle;
 275     } else {
 276         motorZeroCommand = motorConfig()->mincommand;
 277         throttleRangeMin = getThrottleIdleValue();
 278         throttleRangeMax = motorConfig()->maxthrottle;
 279     }
 280
 281     reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 282
 283     if (feature(FEATURE_MOTOR_STOP)) {
 284         motorValueWhenStopped = motorZeroCommand;
 285     } else {
 286         motorValueWhenStopped = getThrottleIdleValue();
 287     }
 288
 289     // set disarmed motor values
 290     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 291         motor_disarmed[i] = motorZeroCommand;
 292     }
 293 }
 294
 295 #ifdef USE_DSHOT
 296 static uint16_t handleOutputScaling(
 297     int16_t input,          // Input value from the mixer
 298     int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
 299     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
 300     int16_t inputScaleMin,  // Input range - min value
 301     int16_t inputScaleMax,  // Input range - max value
 302     int16_t outputScaleMin, // Output range - min value
 303     int16_t outputScaleMax, // Output range - max value
 304     bool moveForward        // If motor should be rotating FORWARD or BACKWARD
 305 )
 306 {
 307     int value;
 308     if (moveForward && input < stopThreshold) {
 309         //Send motor stop command
 310         value = onStopValue;
 311     }
 312     else if (!moveForward && input > stopThreshold) {
 313         //Send motor stop command
 314         value = onStopValue;
 315     }
 316     else {
 317         //Scale input to protocol output values
 318         value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
 319         value = constrain(value, outputScaleMin, outputScaleMax);
 320     }
 321     return value;
 322 }
 323 #endif
 324
 325 void FAST_CODE writeMotors(void)
 326 {
 327     for (int i = 0; i < motorCount; i++) {
 328         uint16_t motorValue;
 329
 330 #ifdef USE_DSHOT
 331         // If we use DSHOT we need to convert motorValue to DSHOT ranges
 332         if (isMotorProtocolDigital()) {
 333
 334             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 335                 if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
 336                     motorValue = handleOutputScaling(
 337                         motor[i],
 338                         throttleRangeMin,
 339                         DSHOT_DISARM_COMMAND,
 340                         throttleRangeMin,
 341                         throttleRangeMax,
 342                         DSHOT_3D_DEADBAND_HIGH,
 343                         DSHOT_MAX_THROTTLE,
 344                         true
 345                     );
 346                 } else {
 347                     motorValue = handleOutputScaling(
 348                         motor[i],
 349                         throttleRangeMax,
 350                         DSHOT_DISARM_COMMAND,
 351                         throttleRangeMin,
 352                         throttleRangeMax,
 353                         DSHOT_MIN_THROTTLE,
 354                         DSHOT_3D_DEADBAND_LOW,
 355                         false
 356                     );
 357                 }
 358             }
 359             else {
 360                 motorValue = handleOutputScaling(
 361                     motor[i],
 362                     throttleIdleValue,
 363                     DSHOT_DISARM_COMMAND,
 364                     motorConfig()->mincommand,
 365                     motorConfig()->maxthrottle,
 366                     DSHOT_MIN_THROTTLE,
 367                     DSHOT_MAX_THROTTLE,
 368                     true
 369                 );
 370             }
 371         }
 372         else {
 373             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 374                 if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
 375                     motorValue = handleOutputScaling(
 376                         motor[i],
 377                         throttleRangeMin,
 378                         motor[i],
 379                         throttleRangeMin,
 380                         throttleRangeMax,
 381                         reversibleMotorsConfig()->deadband_high,
 382                         motorConfig()->maxthrottle,
 383                         true
 384                     );
 385                 } else {
 386                     motorValue = handleOutputScaling(
 387                         motor[i],
 388                         throttleRangeMax,
 389                         motor[i],
 390                         throttleRangeMin,
 391                         throttleRangeMax,
 392                         motorConfig()->mincommand,
 393                         reversibleMotorsConfig()->deadband_low,
 394                         false
 395                     );
 396                 }
 397             } else {
 398                 motorValue = motor[i];
 399             }
 400
 401         }
 402 #else
 403         // We don't define USE_DSHOT
 404         motorValue = motor[i];
 405 #endif
 406
 407         pwmWriteMotor(i, motorValue);
 408     }
 409 }
 410
 411 void writeAllMotors(int16_t mc)
 412 {
 413     // Sends commands to all motors
 414     for (int i = 0; i < motorCount; i++) {
 415         motor[i] = mc;
 416     }
 417     writeMotors();
 418 }
 419
 420 void stopMotors(void)
 421 {
 422     writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
 423
 424     delay(50); // give the timers and ESCs a chance to react.
 425 }
 426
 427 void stopPwmAllMotors(void)
 428 {
 429     pwmShutdownPulsesForAllMotors(motorCount);
 430 }
 431
 432 static int getReversibleMotorsThrottleDeadband(void)
 433 {
 434     int directionValue;
 435
 436     if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
 437         directionValue = reversibleMotorsConfig()->deadband_low;
 438     } else {
 439         directionValue = reversibleMotorsConfig()->deadband_high;
 440     }
 441
 442     return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
 443 }
 444
 445 void FAST_CODE mixTable(const float dT)
 446 {
 447     int16_t input[3];   // RPY, range [-500:+500]
 448     // Allow direct stick input to motors in passthrough mode on airplanes
 449     if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
 450         // Direct passthru from RX
 451         input[ROLL] = rcCommand[ROLL];
 452         input[PITCH] = rcCommand[PITCH];
 453         input[YAW] = rcCommand[YAW];
 454     }
 455     else {
 456         input[ROLL] = axisPID[ROLL];
 457         input[PITCH] = axisPID[PITCH];
 458         input[YAW] = axisPID[YAW];
 459     }
 460
 461     // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
 462     int16_t rpyMix[MAX_SUPPORTED_MOTORS];
 463     int16_t rpyMixMax = 0; // assumption: symetrical about zero.
 464     int16_t rpyMixMin = 0;
 465
 466     // motors for non-servo mixes
 467     for (int i = 0; i < motorCount; i++) {
 468         rpyMix[i] =
 469             (input[PITCH] * currentMixer[i].pitch +
 470             input[ROLL] * currentMixer[i].roll +
 471             -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
 472
 473         if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
 474         if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
 475     }
 476
 477     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
 478     int16_t throttleRange;
 479     int16_t throttleMin, throttleMax;
 480
 481     // Find min and max throttle based on condition.
 482 #ifdef USE_PROGRAMMING_FRAMEWORK
 483     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
 484         throttleRangeMin = throttleIdleValue;
 485         throttleRangeMax = motorConfig()->maxthrottle;
 486         mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
 487     } else
 488 #endif
 489     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 490
 491         if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) {
 492             /*
 493              * Throttle is above deadband, FORWARD direction
 494              */
 495             reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 496             throttleRangeMax = motorConfig()->maxthrottle;
 497             throttleRangeMin = throttleDeadbandHigh;
 498             DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
 499         } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
 500             /*
 501              * Throttle is below deadband, BACKWARD direction
 502              */
 503             reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
 504             throttleRangeMax = throttleDeadbandLow;
 505             throttleRangeMin = motorConfig()->mincommand;
 506         }
 507
 508
 509         motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
 510         mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
 511     } else {
 512         mixerThrottleCommand = rcCommand[THROTTLE];
 513         throttleRangeMin = throttleIdleValue;
 514         throttleRangeMax = motorConfig()->maxthrottle;
 515
 516         // Throttle scaling to limit max throttle when battery is full
 517     #ifdef USE_PROGRAMMING_FRAMEWORK
 518         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin;
 519     #else
 520         mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
 521     #endif
 522         // Throttle compensation based on battery voltage
 523         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
 524             mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
 525         }
 526     }
 527
 528     throttleMin = throttleRangeMin;
 529     throttleMax = throttleRangeMax;
 530
 531     throttleRange = throttleMax - throttleMin;
 532
 533     #define THROTTLE_CLIPPING_FACTOR    0.33f
 534     motorMixRange = (float)rpyMixRange / (float)throttleRange;
 535     if (motorMixRange > 1.0f) {
 536         for (int i = 0; i < motorCount; i++) {
 537             rpyMix[i] /= motorMixRange;
 538         }
 539
 540         // Allow some clipping on edges to soften correction response
 541         throttleMin = throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 542         throttleMax = throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2);
 543     } else {
 544         throttleMin = MIN(throttleMin + (rpyMixRange / 2), throttleMin + (throttleRange / 2) - (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 545         throttleMax = MAX(throttleMax - (rpyMixRange / 2), throttleMin + (throttleRange / 2) + (throttleRange * THROTTLE_CLIPPING_FACTOR / 2));
 546     }
 547
 548     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 549     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 550     if (ARMING_FLAG(ARMED)) {
 551         const motorStatus_e currentMotorStatus = getMotorStatus();
 552         for (int i = 0; i < motorCount; i++) {
 553             motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
 554
 555             if (failsafeIsActive()) {
 556                 motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
 557             } else {
 558                 motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
 559             }
 560
 561             // Motor stop handling
 562             if (currentMotorStatus != MOTOR_RUNNING) {
 563                 motor[i] = motorValueWhenStopped;
 564             }
 565         }
 566     } else {
 567         for (int i = 0; i < motorCount; i++) {
 568             motor[i] = motor_disarmed[i];
 569         }
 570     }
 571
 572     /* Apply motor acceleration/deceleration limit */
 573     motorRateLimitingApplyFn(dT);
 574 }
 575
 576 motorStatus_e getMotorStatus(void)
 577 {
 578     if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) {
 579         return MOTOR_STOPPED_AUTO;
 580     }
 581
 582     if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
 583         if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
 584             return MOTOR_STOPPED_USER;
 585         }
 586     }
 587
 588     return MOTOR_RUNNING;
 589 }
 590
 591 void loadPrimaryMotorMixer(void) {
 592     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 593         currentMixer[i] = *primaryMotorMixer(i);
 594     }
 595 }