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