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 <stdlib.h>
  22 #include <math.h>
  23 #include <float.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 #ifdef USE_DYN_IDLE
 220         if (mixerRuntime.dynIdleMinRps > 0.0f) {
 221             const float maxIncrease = isAirmodeActivated()
 222                 ? mixerRuntime.dynIdleMaxIncrease : mixerRuntime.dynIdleStartIncrease;
 223             float minRps = getMinMotorFrequency();
 224             DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f));
 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, lrintf(dynIdleP * 10000)));
 236             DEBUG_SET(DEBUG_DYN_IDLE, 1, lrintf(mixerRuntime.dynIdleI * 10000));
 237             DEBUG_SET(DEBUG_DYN_IDLE, 2, lrintf(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, lrintf(batteryGoodness * 100));
 252             DEBUG_SET(DEBUG_BATTERY, 3, lrintf(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 #ifdef USE_RPM_LIMIT
 348 static void applyRpmLimiter(mixerRuntime_t *mixer)
 349 {
 350     static float prevError = 0.0f;
 351     static float i = 0.0f;
 352     const float unsmoothedAverageRpm = getDshotAverageRpm();
 353     const float averageRpm = pt1FilterApply(&mixer->averageRpmFilter, unsmoothedAverageRpm);
 354     const float error = averageRpm - mixer->rpmLimiterRpmLimit;
 355
 356     // PID
 357     const float p = error * mixer->rpmLimiterPGain;
 358     const float d = (error - prevError) * mixer->rpmLimiterDGain; // rpmLimiterDGain already adjusted for looprate (see mixer_init.c)
 359     i += error * mixer->rpmLimiterIGain;                          // rpmLimiterIGain already adjusted for looprate (see mixer_init.c)
 360     i = MAX(0.0f, i);
 361     float pidOutput = p + i + d;
 362
 363     // Throttle limit learning
 364     if (error > 0.0f && rcCommand[THROTTLE] < rxConfig()->maxcheck) {
 365         mixer->rpmLimiterThrottleScale *= 1.0f - 4.8f * pidGetDT();
 366     } else if (pidOutput < -400 * pidGetDT() && rcCommand[THROTTLE] >= rxConfig()->maxcheck && !areMotorsSaturated()) { // Throttle accel corresponds with motor accel
 367         mixer->rpmLimiterThrottleScale *= 1.0f + 3.2f * pidGetDT();
 368     }
 369     mixer->rpmLimiterThrottleScale = constrainf(mixer->rpmLimiterThrottleScale, 0.01f, 1.0f);
 370     throttle *= mixer->rpmLimiterThrottleScale;
 371
 372     // Output
 373     pidOutput = MAX(0.0f, pidOutput);
 374     throttle = constrainf(throttle - pidOutput, 0.0f, 1.0f);
 375     prevError = error;
 376
 377     DEBUG_SET(DEBUG_RPM_LIMIT, 0, lrintf(averageRpm));
 378     DEBUG_SET(DEBUG_RPM_LIMIT, 1, lrintf(unsmoothedAverageRpm));
 379     DEBUG_SET(DEBUG_RPM_LIMIT, 2, lrintf(mixer->rpmLimiterThrottleScale * 100.0f));
 380     DEBUG_SET(DEBUG_RPM_LIMIT, 3, lrintf(throttle * 100.0f));
 381     DEBUG_SET(DEBUG_RPM_LIMIT, 4, lrintf(error));
 382     DEBUG_SET(DEBUG_RPM_LIMIT, 5, lrintf(p * 100.0f));
 383     DEBUG_SET(DEBUG_RPM_LIMIT, 6, lrintf(i * 100.0f));
 384     DEBUG_SET(DEBUG_RPM_LIMIT, 7, lrintf(d * 100.0f));
 385 }
 386 #endif // USE_RPM_LIMIT
 387
 388 static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
 389 {
 390     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
 391     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
 392     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 393         float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
 394 #ifdef USE_THRUST_LINEARIZATION
 395         motorOutput = pidApplyThrustLinearization(motorOutput);
 396 #endif
 397         motorOutput = motorOutputMin + motorOutputRange * motorOutput;
 398
 399 #ifdef USE_SERVOS
 400         if (mixerIsTricopter()) {
 401             motorOutput += mixerTricopterMotorCorrection(i);
 402         }
 403 #endif
 404         if (failsafeIsActive()) {
 405 #ifdef USE_DSHOT
 406             if (isMotorProtocolDshot()) {
 407                 motorOutput = (motorOutput < motorRangeMin) ? mixerRuntime.disarmMotorOutput : motorOutput; // Prevent getting into special reserved range
 408             }
 409 #endif
 410             motorOutput = constrainf(motorOutput, mixerRuntime.disarmMotorOutput, motorRangeMax);
 411         } else {
 412             motorOutput = constrainf(motorOutput, motorRangeMin, motorRangeMax);
 413         }
 414         motor[i] = motorOutput;
 415     }
 416
 417     // Disarmed mode
 418     if (!ARMING_FLAG(ARMED)) {
 419         for (int i = 0; i < mixerRuntime.motorCount; i++) {
 420             motor[i] = motor_disarmed[i];
 421         }
 422     }
 423 }
 424
 425 static float applyThrottleLimit(float throttle)
 426 {
 427     if (currentControlRateProfile->throttle_limit_percent < 100 && !RPM_LIMIT_ACTIVE) {
 428         const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f;
 429         switch (currentControlRateProfile->throttle_limit_type) {
 430             case THROTTLE_LIMIT_TYPE_SCALE:
 431                 return throttle * throttleLimitFactor;
 432             case THROTTLE_LIMIT_TYPE_CLIP:
 433                 return MIN(throttle, throttleLimitFactor);
 434         }
 435     }
 436
 437     return throttle;
 438 }
 439
 440 static void applyMotorStop(void)
 441 {
 442     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 443         motor[i] = mixerRuntime.disarmMotorOutput;
 444     }
 445 }
 446
 447 #ifdef USE_DYN_LPF
 448 static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
 449 {
 450     static timeUs_t lastDynLpfUpdateUs = 0;
 451     static int dynLpfPreviousQuantizedThrottle = -1;  // to allow an initial zero throttle to set the filter cutoff
 452
 453     if (cmpTimeUs(currentTimeUs, lastDynLpfUpdateUs) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US) {
 454         const int quantizedThrottle = lrintf(throttle * DYN_LPF_THROTTLE_STEPS); // quantize the throttle reduce the number of filter updates
 455         if (quantizedThrottle != dynLpfPreviousQuantizedThrottle) {
 456             // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable
 457             const float dynLpfThrottle = (float)quantizedThrottle / DYN_LPF_THROTTLE_STEPS;
 458             dynLpfGyroUpdate(dynLpfThrottle);
 459             dynLpfDTermUpdate(dynLpfThrottle);
 460             dynLpfPreviousQuantizedThrottle = quantizedThrottle;
 461             lastDynLpfUpdateUs = currentTimeUs;
 462         }
 463     }
 464 }
 465 #endif
 466
 467 static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnabled)
 468 {
 469     float airmodeTransitionPercent = 1.0f;
 470     float motorDeltaScale = 0.5f;
 471
 472     if (!airmodeEnabled && throttle < 0.5f) {
 473         // 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.
 474         // also lays the groundwork for how an airmode percent would work.
 475         airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
 476         motorDeltaScale *= airmodeTransitionPercent; // this should be half of the motor authority allowed
 477     }
 478
 479     const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent;
 480
 481     const float motorMixDelta = motorDeltaScale * motorMixRange;
 482
 483     float minMotor = FLT_MAX;
 484     float maxMotor = FLT_MIN;
 485
 486     for (int i = 0; i < mixerRuntime.motorCount; ++i) {
 487         if (mixerConfig()->mixer_type == MIXER_LINEAR) {
 488             motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
 489         } else {
 490             motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + fabsf(motorMix[i]), motorMix[i] - fabsf(motorMix[i]));
 491         }
 492         motorMix[i] *= motorMixNormalizationFactor;
 493
 494         maxMotor = MAX(motorMix[i], maxMotor);
 495         minMotor = MIN(motorMix[i], minMotor);
 496     }
 497
 498     // constrain throttle so it won't clip any outputs
 499     throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor);
 500 }
 501
 502 static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled)
 503 {
 504 #ifdef USE_AIRMODE_LPF
 505     const float unadjustedThrottle = throttle;
 506     throttle += pidGetAirmodeThrottleOffset();
 507     float airmodeThrottleChange = 0.0f;
 508 #endif
 509     float airmodeTransitionPercent = 1.0f;
 510
 511     if (!airmodeEnabled && throttle < 0.5f) {
 512         // 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.
 513         // also lays the groundwork for how an airmode percent would work.
 514         airmodeTransitionPercent = scaleRangef(throttle, 0.0f, 0.5f, 0.5f, 1.0f); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
 515     }
 516
 517     const float motorMixNormalizationFactor = motorMixRange > 1.0f ? airmodeTransitionPercent / motorMixRange : airmodeTransitionPercent;
 518
 519     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 520         motorMix[i] *= motorMixNormalizationFactor;
 521     }
 522
 523     const float normalizedMotorMixMin = motorMixMin * motorMixNormalizationFactor;
 524     const float normalizedMotorMixMax = motorMixMax * motorMixNormalizationFactor;
 525     throttle = constrainf(throttle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax);
 526
 527 #ifdef USE_AIRMODE_LPF
 528     airmodeThrottleChange = constrainf(unadjustedThrottle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax) - unadjustedThrottle;
 529     pidUpdateAirmodeLpf(airmodeThrottleChange);
 530 #endif
 531 }
 532
 533 FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
 534 {
 535     // Find min and max throttle based on conditions. Throttle has to be known before mixing
 536     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 537
 538     if (isFlipOverAfterCrashActive()) {
 539         applyFlipOverAfterCrashModeToMotors();
 540         return;
 541     }
 542
 543     const bool launchControlActive = isLaunchControlActive();
 544
 545     motorMixer_t * activeMixer = &mixerRuntime.currentMixer[0];
 546 #ifdef USE_LAUNCH_CONTROL
 547     if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
 548         activeMixer = &mixerRuntime.launchControlMixer[0];
 549     }
 550 #endif
 551
 552     // Calculate and Limit the PID sum
 553     const float scaledAxisPidRoll =
 554         constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
 555     const float scaledAxisPidPitch =
 556         constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
 557
 558     uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
 559
 560 #ifdef USE_YAW_SPIN_RECOVERY
 561     const bool yawSpinDetected = gyroYawSpinDetected();
 562     if (yawSpinDetected) {
 563         yawPidSumLimit = PIDSUM_LIMIT_MAX;   // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
 564     }
 565 #endif // USE_YAW_SPIN_RECOVERY
 566
 567     float scaledAxisPidYaw =
 568         constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
 569
 570     if (!mixerConfig()->yaw_motors_reversed) {
 571         scaledAxisPidYaw = -scaledAxisPidYaw;
 572     }
 573
 574     // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
 575     if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) {
 576         throttle = applyThrottleLimit(throttle);
 577     }
 578
 579     // use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
 580     pidUpdateAntiGravityThrottleFilter(throttle);
 581
 582     // and for TPA
 583     pidUpdateTpaFactor(throttle);
 584
 585 #ifdef USE_DYN_LPF
 586     // keep the changes to dynamic lowpass clean, without unnecessary dynamic changes
 587     updateDynLpfCutoffs(currentTimeUs, throttle);
 588 #endif
 589
 590     // apply throttle boost when throttle moves quickly
 591 #if defined(USE_THROTTLE_BOOST)
 592     if (throttleBoost > 0.0f) {
 593         const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
 594         throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
 595     }
 596 #endif
 597
 598     // send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode
 599     mixerThrottle = throttle;
 600
 601 #ifdef USE_DYN_IDLE
 602     // Set min throttle offset of 1% when stick is at zero and dynamic idle is active
 603     if (mixerRuntime.dynIdleMinRps > 0.0f) {
 604         throttle = MAX(throttle, 0.01f);
 605     }
 606 #endif
 607
 608 #ifdef USE_THRUST_LINEARIZATION
 609     // reduce throttle to offset additional motor output
 610     throttle = pidCompensateThrustLinearization(throttle);
 611 #endif
 612
 613 #ifdef USE_RPM_LIMIT
 614     if (RPM_LIMIT_ACTIVE && motorConfig()->dev.useDshotTelemetry && ARMING_FLAG(ARMED)) {
 615         applyRpmLimiter(&mixerRuntime);
 616     }
 617 #endif
 618
 619     // Find roll/pitch/yaw desired output
 620     // ??? Where is the optimal location for this code?
 621     float motorMix[MAX_SUPPORTED_MOTORS];
 622     float motorMixMax = 0, motorMixMin = 0;
 623     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 624         float mix =
 625             scaledAxisPidRoll  * activeMixer[i].roll +
 626             scaledAxisPidPitch * activeMixer[i].pitch +
 627             scaledAxisPidYaw   * activeMixer[i].yaw;
 628
 629         if (mix > motorMixMax) {
 630             motorMixMax = mix;
 631         } else if (mix < motorMixMin) {
 632             motorMixMin = mix;
 633         }
 634         motorMix[i] = mix;
 635     }
 636
 637     //  The following fixed throttle values will not be shown in the blackbox log
 638     // ?? Should they be influenced by airmode?  If not, should go after the apply airmode code.
 639     const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
 640 #ifdef USE_YAW_SPIN_RECOVERY
 641     // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
 642     // When airmode is active the throttle setting doesn't impact recovery authority.
 643     if (yawSpinDetected && !airmodeEnabled) {
 644         throttle = 0.5f;
 645     }
 646 #endif // USE_YAW_SPIN_RECOVERY
 647
 648 #ifdef USE_LAUNCH_CONTROL
 649     // While launch control is active keep the throttle at minimum.
 650     // Once the pilot triggers the launch throttle control will be reactivated.
 651     if (launchControlActive) {
 652         throttle = 0.0f;
 653     }
 654 #endif
 655
 656 #ifdef USE_GPS_RESCUE
 657     // If gps rescue is active then override the throttle. This prevents things
 658     // like throttle boost or throttle limit from negatively affecting the throttle.
 659     if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 660         throttle = gpsRescueGetThrottle();
 661     }
 662 #endif
 663
 664     motorMixRange = motorMixMax - motorMixMin;
 665     if (mixerConfig()->mixer_type > MIXER_LEGACY) {
 666         applyMixerAdjustmentLinear(motorMix, airmodeEnabled);
 667     } else {
 668         applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
 669     }
 670
 671     if (featureIsEnabled(FEATURE_MOTOR_STOP)
 672         && ARMING_FLAG(ARMED)
 673         && !mixerRuntime.feature3dEnabled
 674         && !airmodeEnabled
 675         && !FLIGHT_MODE(GPS_RESCUE_MODE)   // disable motor_stop while GPS Rescue is active
 676         && (rcData[THROTTLE] < rxConfig()->mincheck)) {
 677         // motor_stop handling
 678         applyMotorStop();
 679     } else {
 680         // Apply the mix to motor endpoints
 681         applyMixToMotors(motorMix, activeMixer);
 682     }
 683 }
 684
 685 void mixerSetThrottleAngleCorrection(int correctionValue)
 686 {
 687     throttleAngleCorrection = correctionValue;
 688 }
 689
 690 float mixerGetThrottle(void)
 691 {
 692     return mixerThrottle;
 693 }