src/main/flight/mixer.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <math.h>
  25 #include <float.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/filter.h"
  33 #include "common/maths.h"
  34
  35 #include "config/config.h"
  36 #include "config/feature.h"
  37
  38 #include "drivers/dshot.h"
  39 #include "drivers/io.h"
  40 #include "drivers/motor.h"
  41 #include "drivers/time.h"
  42
  43 #include "fc/controlrate_profile.h"
  44 #include "fc/core.h"
  45 #include "fc/rc.h"
  46 #include "fc/rc_controls.h"
  47 #include "fc/rc_modes.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "flight/failsafe.h"
  51 #include "flight/gps_rescue.h"
  52 #include "flight/imu.h"
  53 #include "flight/mixer_init.h"
  54 #include "flight/mixer_tricopter.h"
  55 #include "flight/pid.h"
  56 #include "flight/rpm_filter.h"
  57
  58 #include "pg/rx.h"
  59
  60 #include "rx/rx.h"
  61
  62 #include "sensors/battery.h"
  63 #include "sensors/gyro.h"
  64
  65 #include "mixer.h"
  66
  67 #define DYN_LPF_THROTTLE_STEPS           100
  68 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
  69
  70 static FAST_DATA_ZERO_INIT float motorMixRange;
  71
  72 float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS];
  73 float motor_disarmed[MAX_SUPPORTED_MOTORS];
  74
  75 static FAST_DATA_ZERO_INIT int throttleAngleCorrection;
  76
  77 float getMotorMixRange(void)
  78 {
  79     return motorMixRange;
  80 }
  81
  82 void writeMotors(void)
  83 {
  84     motorWriteAll(motor);
  85 }
  86
  87 static void writeAllMotors(int16_t mc)
  88 {
  89     // Sends commands to all motors
  90     for (int i = 0; i < mixerRuntime.motorCount; i++) {
  91         motor[i] = mc;
  92     }
  93     writeMotors();
  94 }
  95
  96 void stopMotors(void)
  97 {
  98     writeAllMotors(mixerRuntime.disarmMotorOutput);
  99     delay(50); // give the timers and ESCs a chance to react.
 100 }
 101
 102 static FAST_DATA_ZERO_INIT float throttle = 0;
 103 static FAST_DATA_ZERO_INIT float mixerThrottle = 0;
 104 static FAST_DATA_ZERO_INIT float motorOutputMin;
 105 static FAST_DATA_ZERO_INIT float motorRangeMin;
 106 static FAST_DATA_ZERO_INIT float motorRangeMax;
 107 static FAST_DATA_ZERO_INIT float motorOutputRange;
 108 static FAST_DATA_ZERO_INIT int8_t motorOutputMixSign;
 109
 110 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 111 {
 112     static uint16_t rcThrottlePrevious = 0;   // Store the last throttle direction for deadband transitions
 113     static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode
 114     static float motorRangeMinIncrease = 0;
 115
 116     float currentThrottleInputRange = 0;
 117     if (mixerRuntime.feature3dEnabled) {
 118         uint16_t rcCommand3dDeadBandLow;
 119         uint16_t rcCommand3dDeadBandHigh;
 120
 121         if (!ARMING_FLAG(ARMED)) {
 122             rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
 123         }
 124
 125         if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) {
 126             // The min_check range is halved because the output throttle is scaled to 500us.
 127             // So by using half of min_check we maintain the same low-throttle deadband
 128             // stick travel as normal non-3D mode.
 129             const int mincheckOffset = (rxConfig()->mincheck - PWM_RANGE_MIN) / 2;
 130             rcCommand3dDeadBandLow = rxConfig()->midrc - mincheckOffset;
 131             rcCommand3dDeadBandHigh = rxConfig()->midrc + mincheckOffset;
 132         } else {
 133             rcCommand3dDeadBandLow = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
 134             rcCommand3dDeadBandHigh = rxConfig()->midrc + flight3DConfig()->deadband3d_throttle;
 135         }
 136
 137         const float rcCommandThrottleRange3dLow = rcCommand3dDeadBandLow - PWM_RANGE_MIN;
 138         const float rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rcCommand3dDeadBandHigh;
 139
 140         if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) {
 141             // INVERTED
 142             motorRangeMin = mixerRuntime.motorOutputLow;
 143             motorRangeMax = mixerRuntime.deadbandMotor3dLow;
 144 #ifdef USE_DSHOT
 145             if (isMotorProtocolDshot()) {
 146                 motorOutputMin = mixerRuntime.motorOutputLow;
 147                 motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow;
 148             } else
 149 #endif
 150             {
 151                 motorOutputMin = mixerRuntime.deadbandMotor3dLow;
 152                 motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow;
 153             }
 154
 155             if (motorOutputMixSign != -1) {
 156                 reversalTimeUs = currentTimeUs;
 157             }
 158             motorOutputMixSign = -1;
 159
 160             rcThrottlePrevious = rcCommand[THROTTLE];
 161             throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
 162             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 163         } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) {
 164             // NORMAL
 165             motorRangeMin = mixerRuntime.deadbandMotor3dHigh;
 166             motorRangeMax = mixerRuntime.motorOutputHigh;
 167             motorOutputMin = mixerRuntime.deadbandMotor3dHigh;
 168             motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh;
 169             if (motorOutputMixSign != 1) {
 170                 reversalTimeUs = currentTimeUs;
 171             }
 172             motorOutputMixSign = 1;
 173             rcThrottlePrevious = rcCommand[THROTTLE];
 174             throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
 175             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 176         } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow &&
 177                 !flight3DConfigMutable()->switched_mode3d) ||
 178                 isMotorsReversed()) {
 179             // INVERTED_TO_DEADBAND
 180             motorRangeMin = mixerRuntime.motorOutputLow;
 181             motorRangeMax = mixerRuntime.deadbandMotor3dLow;
 182
 183 #ifdef USE_DSHOT
 184             if (isMotorProtocolDshot()) {
 185                 motorOutputMin = mixerRuntime.motorOutputLow;
 186                 motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow;
 187             } else
 188 #endif
 189             {
 190                 motorOutputMin = mixerRuntime.deadbandMotor3dLow;
 191                 motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow;
 192             }
 193
 194             if (motorOutputMixSign != -1) {
 195                 reversalTimeUs = currentTimeUs;
 196             }
 197             motorOutputMixSign = -1;
 198
 199             throttle = 0;
 200             currentThrottleInputRange = rcCommandThrottleRange3dLow;
 201         } else {
 202             // NORMAL_TO_DEADBAND
 203             motorRangeMin = mixerRuntime.deadbandMotor3dHigh;
 204             motorRangeMax = mixerRuntime.motorOutputHigh;
 205             motorOutputMin = mixerRuntime.deadbandMotor3dHigh;
 206             motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh;
 207             if (motorOutputMixSign != 1) {
 208                 reversalTimeUs = currentTimeUs;
 209             }
 210             motorOutputMixSign = 1;
 211             throttle = 0;
 212             currentThrottleInputRange = rcCommandThrottleRange3dHigh;
 213         }
 214         if (currentTimeUs - reversalTimeUs < 250000) {
 215             // keep iterm zero for 250ms after motor reversal
 216             pidResetIterm();
 217         }
 218     } else {
 219         throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection;
 220         currentThrottleInputRange = PWM_RANGE;
 221
 222 #ifdef USE_DYN_IDLE
 223         if (mixerRuntime.dynIdleMinRps > 0.0f) {
 224             const float maxIncrease = isAirmodeActivated()
 225                 ? mixerRuntime.dynIdleMaxIncrease : mixerRuntime.dynIdleStartIncrease;
 226             float minRps = getMinMotorFrequency();
 227             DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f));
 228             float rpsError = mixerRuntime.dynIdleMinRps - minRps;
 229             // PT1 type lowpass delay and smoothing for D
 230             minRps = mixerRuntime.prevMinRps + mixerRuntime.minRpsDelayK * (minRps - mixerRuntime.prevMinRps);
 231             float dynIdleD = (mixerRuntime.prevMinRps - minRps) * mixerRuntime.dynIdleDGain;
 232             mixerRuntime.prevMinRps = minRps;
 233             float dynIdleP = rpsError * mixerRuntime.dynIdlePGain;
 234             rpsError = MAX(-0.1f, rpsError); //I rises fast, falls slowly
 235             mixerRuntime.dynIdleI += rpsError * mixerRuntime.dynIdleIGain;
 236             mixerRuntime.dynIdleI = constrainf(mixerRuntime.dynIdleI, 0.0f, maxIncrease);
 237             motorRangeMinIncrease = constrainf((dynIdleP + mixerRuntime.dynIdleI + dynIdleD), 0.0f, maxIncrease);
 238             DEBUG_SET(DEBUG_DYN_IDLE, 0, MAX(-1000, lrintf(dynIdleP * 10000)));
 239             DEBUG_SET(DEBUG_DYN_IDLE, 1, lrintf(mixerRuntime.dynIdleI * 10000));
 240             DEBUG_SET(DEBUG_DYN_IDLE, 2, lrintf(dynIdleD * 10000));
 241         } else {
 242             motorRangeMinIncrease = 0;
 243         }
 244 #endif
 245
 246 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 247         float motorRangeAttenuationFactor = 0;
 248         // reduce motorRangeMax when battery is full
 249         if (mixerRuntime.vbatSagCompensationFactor > 0.0f) {
 250             const uint16_t currentCellVoltage = getBatterySagCellVoltage();
 251             // batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow
 252             float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f);
 253             motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor;
 254             DEBUG_SET(DEBUG_BATTERY, 2, lrintf(batteryGoodness * 100));
 255             DEBUG_SET(DEBUG_BATTERY, 3, lrintf(motorRangeAttenuationFactor * 1000));
 256         }
 257         motorRangeMax = isFlipOverAfterCrashActive() ? mixerRuntime.motorOutputHigh : mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
 258 #else
 259         motorRangeMax = mixerRuntime.motorOutputHigh;
 260 #endif
 261
 262         motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
 263         motorOutputMin = motorRangeMin;
 264         motorOutputRange = motorRangeMax - motorRangeMin;
 265         motorOutputMixSign = 1;
 266     }
 267
 268     throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
 269 }
 270
 271 #define CRASH_FLIP_DEADBAND 20
 272 #define CRASH_FLIP_STICK_MINF 0.15f
 273
 274 static void applyFlipOverAfterCrashModeToMotors(void)
 275 {
 276     if (ARMING_FLAG(ARMED)) {
 277         const float flipPowerFactor = 1.0f - mixerConfig()->crashflip_expo / 100.0f;
 278         const float stickDeflectionPitchAbs = getRcDeflectionAbs(FD_PITCH);
 279         const float stickDeflectionRollAbs = getRcDeflectionAbs(FD_ROLL);
 280         const float stickDeflectionYawAbs = getRcDeflectionAbs(FD_YAW);
 281
 282         const float stickDeflectionPitchExpo = flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
 283         const float stickDeflectionRollExpo = flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
 284         const float stickDeflectionYawExpo = flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
 285
 286         float signPitch = getRcDeflection(FD_PITCH) < 0 ? 1 : -1;
 287         float signRoll = getRcDeflection(FD_ROLL) < 0 ? 1 : -1;
 288         float signYaw = (getRcDeflection(FD_YAW) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed ? 1 : -1);
 289
 290         float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
 291         float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
 292
 293         if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
 294             // If yaw is the dominant, disable pitch and roll
 295             stickDeflectionLength = stickDeflectionYawAbs;
 296             stickDeflectionExpoLength = stickDeflectionYawExpo;
 297             signRoll = 0;
 298             signPitch = 0;
 299         } else {
 300             // If pitch/roll dominant, disable yaw
 301             signYaw = 0;
 302         }
 303
 304         const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) / (sqrtf(2.0f) * stickDeflectionLength) : 0;
 305         const float cosThreshold = sqrtf(3.0f)/2.0f; // cos(PI/6.0f)
 306
 307         if (cosPhi < cosThreshold) {
 308             // Enforce either roll or pitch exclusively, if not on diagonal
 309             if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
 310                 signPitch = 0;
 311             } else {
 312                 signRoll = 0;
 313             }
 314         }
 315
 316         // Apply a reasonable amount of stick deadband
 317         const float crashFlipStickMinExpo = flipPowerFactor * CRASH_FLIP_STICK_MINF + power3(CRASH_FLIP_STICK_MINF) * (1 - flipPowerFactor);
 318         const float flipStickRange = 1.0f - crashFlipStickMinExpo;
 319         const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
 320
 321         for (int i = 0; i < mixerRuntime.motorCount; ++i) {
 322             float motorOutputNormalised =
 323                 signPitch * mixerRuntime.currentMixer[i].pitch +
 324                 signRoll * mixerRuntime.currentMixer[i].roll +
 325                 signYaw * mixerRuntime.currentMixer[i].yaw;
 326
 327             if (motorOutputNormalised < 0) {
 328                 if (mixerConfig()->crashflip_motor_percent > 0) {
 329                     motorOutputNormalised = -motorOutputNormalised * (float)mixerConfig()->crashflip_motor_percent / 100.0f;
 330                 } else {
 331                     motorOutputNormalised = 0;
 332                 }
 333             }
 334             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 335             float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange;
 336
 337             // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
 338             motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? mixerRuntime.disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND);
 339
 340             motor[i] = motorOutput;
 341         }
 342     } else {
 343         // Disarmed mode
 344         for (int i = 0; i < mixerRuntime.motorCount; i++) {
 345             motor[i] = motor_disarmed[i];
 346         }
 347     }
 348 }
 349
 350 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
 351 {
 352     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 353     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 354     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 355         float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
 356 #ifdef USE_THRUST_LINEARIZATION
 357         motorOutput = pidApplyThrustLinearization(motorOutput);
 358 #endif
 359         motorOutput = motorOutputMin + motorOutputRange * motorOutput;
 360
 361 #ifdef USE_SERVOS
 362         if (mixerIsTricopter()) {
 363             motorOutput += mixerTricopterMotorCorrection(i);
 364         }
 365 #endif
 366         if (failsafeIsActive()) {
 367 #ifdef USE_DSHOT
 368             if (isMotorProtocolDshot()) {
 369                 motorOutput = (motorOutput < motorRangeMin) ? mixerRuntime.disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
 370             }
 371 #endif
 372             motorOutput = constrainf(motorOutput, mixerRuntime.disarmMotorOutput, motorRangeMax);
 373         } else {
 374             motorOutput = constrainf(motorOutput, motorRangeMin, motorRangeMax);
 375         }
 376         motor[i] = motorOutput;
 377     }
 378
 379     // Disarmed mode
 380     if (!ARMING_FLAG(ARMED)) {
 381         for (int i = 0; i < mixerRuntime.motorCount; i++) {
 382             motor[i] = motor_disarmed[i];
 383         }
 384     }
 385 }
 386
 387 static float applyThrottleLimit(float throttle)
 388 {
 389     if (currentControlRateProfile->throttle_limit_percent < 100) {
 390         const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
 391         switch (currentControlRateProfile->throttle_limit_type) {
 392             case THROTTLE_LIMIT_TYPE_SCALE:
 393                 return throttle * throttleLimitFactor;
 394             case THROTTLE_LIMIT_TYPE_CLIP:
 395                 return MIN(throttle, throttleLimitFactor);
 396         }
 397     }
 398
 399     return throttle;
 400 }
 401
 402 static void applyMotorStop(void)
 403 {
 404     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 405         motor[i] = mixerRuntime.disarmMotorOutput;
 406     }
 407 }
 408
 409 #ifdef USE_DYN_LPF
 410 static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
 411 {
 412     static timeUs_t lastDynLpfUpdateUs = 0;
 413     static int dynLpfPreviousQuantizedThrottle = -1;  // to allow an initial zero throttle to set the filter cutoff
 414
 415     if (cmpTimeUs(currentTimeUs, lastDynLpfUpdateUs) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US) {
 416         const int quantizedThrottle = lrintf(throttle * DYN_LPF_THROTTLE_STEPS); // quantize the throttle reduce the number of filter updates
 417         if (quantizedThrottle != dynLpfPreviousQuantizedThrottle) {
 418             // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable
 419             const float dynLpfThrottle = (float)quantizedThrottle / DYN_LPF_THROTTLE_STEPS;
 420             dynLpfGyroUpdate(dynLpfThrottle);
 421             dynLpfDTermUpdate(dynLpfThrottle);
 422             dynLpfPreviousQuantizedThrottle = quantizedThrottle;
 423             lastDynLpfUpdateUs = currentTimeUs;
 424         }
 425     }
 426 }
 427 #endif
 428
 429 static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled)
 430 {
 431     float airmodeTransitionPercent = 1.0f;
 432     float motorDeltaScale = 0.5f;
 433
 434     if (!airmodeEnabled && throttle < 0.5f) {
 435         // this scales the motor mix authority to be 0.5 at 0 throttle, and 1.0 at 0.5 throttle as airmode off intended for things to work.
 436         // also lays the groundwork for how an airmode percent would work.
 437         airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
 438         motorDeltaScale *= airmodeTransitionPercent; // this should be half of the motor authority allowed
 439     }
 440
 441     const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent;
 442
 443     const float motorMixDelta = motorDeltaScale * motorMixRange;
 444
 445     float minMotor = FLT_MAX;
 446     float maxMotor = FLT_MIN;
 447
 448     for (int i = 0; i < mixerRuntime.motorCount; ++i) {
 449         if (mixerConfig()->mixer_type == MIXER_LINEAR) {
 450             motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
 451         } else {
 452             motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + fabsf(motorMix[i]), motorMix[i] - fabsf(motorMix[i]));
 453         }
 454         motorMix[i] *= motorMixNormalizationFactor;
 455
 456         maxMotor = MAX(motorMix[i], maxMotor);
 457         minMotor = MIN(motorMix[i], minMotor);
 458     }
 459
 460     // constrain throttle so it won't clip any outputs
 461     throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor);
 462 }
 463
 464 static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled)
 465 {
 466 #ifdef USE_AIRMODE_LPF
 467     const float unadjustedThrottle = throttle;
 468     throttle += pidGetAirmodeThrottleOffset();
 469     float airmodeThrottleChange = 0.0f;
 470 #endif
 471     float airmodeTransitionPercent = 1.0f;
 472
 473     if (!airmodeEnabled && throttle < 0.5f) {
 474         // this scales the motor mix authority to be 0.5 at 0 throttle, and 1.0 at 0.5 throttle as airmode off intended for things to work.
 475         // also lays the groundwork for how an airmode percent would work.
 476         airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
 477     }
 478
 479     const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent;
 480
 481     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 482         motorMix[i] *= motorMixNormalizationFactor;
 483     }
 484
 485     const float normalizedMotorMixMin = motorMixMin * motorMixNormalizationFactor;
 486     const float normalizedMotorMixMax = motorMixMax * motorMixNormalizationFactor;
 487     throttle = constrainf(throttle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax);
 488
 489 #ifdef USE_AIRMODE_LPF
 490     airmodeThrottleChange = constrainf(unadjustedThrottle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax) - unadjustedThrottle;
 491     pidUpdateAirmodeLpf(airmodeThrottleChange);
 492 #endif
 493 }
 494
 495 FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
 496 {
 497     // Find min and max throttle based on conditions. Throttle has to be known before mixing
 498     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 499
 500     if (isFlipOverAfterCrashActive()) {
 501         applyFlipOverAfterCrashModeToMotors();
 502
 503         return;
 504     }
 505
 506     const bool launchControlActive = isLaunchControlActive();
 507
 508     motorMixer_t * activeMixer = &mixerRuntime.currentMixer[0];
 509 #ifdef USE_LAUNCH_CONTROL
 510     if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
 511         activeMixer = &mixerRuntime.launchControlMixer[0];
 512     }
 513 #endif
 514
 515     // Calculate and Limit the PID sum
 516     const float scaledAxisPidRoll =
 517         constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
 518     const float scaledAxisPidPitch =
 519         constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
 520
 521     uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
 522
 523 #ifdef USE_YAW_SPIN_RECOVERY
 524     const bool yawSpinDetected = gyroYawSpinDetected();
 525     if (yawSpinDetected) {
 526         yawPidSumLimit = PIDSUM_LIMIT_MAX;   // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
 527     }
 528 #endif // USE_YAW_SPIN_RECOVERY
 529
 530     float scaledAxisPidYaw =
 531         constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
 532
 533     if (!mixerConfig()->yaw_motors_reversed) {
 534         scaledAxisPidYaw = -scaledAxisPidYaw;
 535     }
 536
 537     // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
 538     if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) {
 539         throttle = applyThrottleLimit(throttle);
 540     }
 541
 542     // use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
 543     pidUpdateAntiGravityThrottleFilter(throttle);
 544
 545     // and for TPA
 546     pidUpdateTpaFactor(throttle);
 547
 548 #ifdef USE_DYN_LPF
 549     // keep the changes to dynamic lowpass clean, without unnecessary dynamic changes
 550     updateDynLpfCutoffs(currentTimeUs, throttle);
 551 #endif
 552
 553     // apply throttle boost when throttle moves quickly
 554 #if defined(USE_THROTTLE_BOOST)
 555     if (throttleBoost > 0.0f) {
 556         const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
 557         throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
 558     }
 559 #endif
 560
 561     // send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode
 562     mixerThrottle = throttle;
 563
 564 #ifdef USE_DYN_IDLE
 565     // Set min throttle offset of 1% when stick is at zero and dynamic idle is active
 566     if (mixerRuntime.dynIdleMinRps > 0.0f) {
 567         throttle = MAX(throttle, 0.01f);
 568     }
 569 #endif
 570
 571 #ifdef USE_THRUST_LINEARIZATION
 572     // reduce throttle to offset additional motor output
 573     throttle = pidCompensateThrustLinearization(throttle);
 574 #endif
 575
 576     // Find roll/pitch/yaw desired output
 577     // ??? Where is the optimal location for this code?
 578     float motorMix[MAX_SUPPORTED_MOTORS];
 579     float motorMixMax = 0, motorMixMin = 0;
 580     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 581
 582         float mix =
 583             scaledAxisPidRoll  * activeMixer[i].roll +
 584             scaledAxisPidPitch * activeMixer[i].pitch +
 585             scaledAxisPidYaw   * activeMixer[i].yaw;
 586
 587         if (mix > motorMixMax) {
 588             motorMixMax = mix;
 589         } else if (mix < motorMixMin) {
 590             motorMixMin = mix;
 591         }
 592         motorMix[i] = mix;
 593     }
 594
 595     //  The following fixed throttle values will not be shown in the blackbox log
 596     // ?? Should they be influenced by airmode?  If not, should go after the apply airmode code.
 597     const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
 598 #ifdef USE_YAW_SPIN_RECOVERY
 599     // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
 600     // When airmode is active the throttle setting doesn't impact recovery authority.
 601     if (yawSpinDetected && !airmodeEnabled) {
 602         throttle = 0.5f;
 603     }
 604 #endif // USE_YAW_SPIN_RECOVERY
 605
 606 #ifdef USE_LAUNCH_CONTROL
 607     // While launch control is active keep the throttle at minimum.
 608     // Once the pilot triggers the launch throttle control will be reactivated.
 609     if (launchControlActive) {
 610         throttle = 0.0f;
 611     }
 612 #endif
 613
 614 #ifdef USE_GPS_RESCUE
 615     // If gps rescue is active then override the throttle. This prevents things
 616     // like throttle boost or throttle limit from negatively affecting the throttle.
 617     if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 618         throttle = gpsRescueGetThrottle();
 619     }
 620 #endif
 621
 622     motorMixRange = motorMixMax - motorMixMin;
 623     if (mixerConfig()->mixer_type > MIXER_LEGACY) {
 624         applyMixerAdjustmentLinear(motorMix, airmodeEnabled);
 625     } else {
 626         applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
 627     }
 628
 629     if (featureIsEnabled(FEATURE_MOTOR_STOP)
 630         && ARMING_FLAG(ARMED)
 631         && !mixerRuntime.feature3dEnabled
 632         && !airmodeEnabled
 633         && !FLIGHT_MODE(GPS_RESCUE_MODE)   // disable motor_stop while GPS Rescue is active
 634         && (rcData[THROTTLE] < rxConfig()->mincheck)) {
 635         // motor_stop handling
 636         applyMotorStop();
 637     } else {
 638         // Apply the mix to motor endpoints
 639         applyMixToMotors(motorMix, activeMixer);
 640     }
 641 }
 642
 643 void mixerSetThrottleAngleCorrection(int correctionValue)
 644 {
 645     throttleAngleCorrection = correctionValue;
 646 }
 647
 648 float mixerGetThrottle(void)
 649 {
 650     return mixerThrottle;
 651 }