2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
27 #include "build/debug.h"
29 #include "common/axis.h"
30 #include "common/filter.h"
31 #include "common/maths.h"
33 #include "config/config.h"
34 #include "config/feature.h"
36 #include "drivers/dshot.h"
37 #include "drivers/io.h"
38 #include "drivers/motor.h"
39 #include "drivers/time.h"
41 #include "fc/controlrate_profile.h"
44 #include "fc/rc_controls.h"
45 #include "fc/rc_modes.h"
46 #include "fc/runtime_config.h"
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"
62 #include "sensors/battery.h"
63 #include "sensors/gyro.h"
67 #define DYN_LPF_THROTTLE_STEPS 100
68 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
70 static FAST_DATA_ZERO_INIT
float motorMixRange
;
72 float FAST_DATA_ZERO_INIT motor
[MAX_SUPPORTED_MOTORS
];
73 float motor_disarmed
[MAX_SUPPORTED_MOTORS
];
75 static FAST_DATA_ZERO_INIT
int throttleAngleCorrection
;
77 float getMotorMixRange(void)
82 void writeMotors(void)
87 static void writeAllMotors(int16_t mc
)
89 // Sends commands to all motors
90 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
98 writeAllMotors(mixerRuntime
.disarmMotorOutput
);
99 delay(50); // give the timers and ESCs a chance to react.
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
;
110 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs
)
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;
116 float currentThrottleInputRange
= 0;
117 if (mixerRuntime
.feature3dEnabled
) {
118 uint16_t rcCommand3dDeadBandLow
;
119 uint16_t rcCommand3dDeadBandHigh
;
121 if (!ARMING_FLAG(ARMED
)) {
122 rcThrottlePrevious
= rxConfig()->midrc
; // When disarmed set to mid_rc. It always results in positive direction after arming.
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
;
133 rcCommand3dDeadBandLow
= rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
;
134 rcCommand3dDeadBandHigh
= rxConfig()->midrc
+ flight3DConfig()->deadband3d_throttle
;
137 const float rcCommandThrottleRange3dLow
= rcCommand3dDeadBandLow
- PWM_RANGE_MIN
;
138 const float rcCommandThrottleRange3dHigh
= PWM_RANGE_MAX
- rcCommand3dDeadBandHigh
;
140 if (rcCommand
[THROTTLE
] <= rcCommand3dDeadBandLow
|| isFlipOverAfterCrashActive()) {
142 motorRangeMin
= mixerRuntime
.motorOutputLow
;
143 motorRangeMax
= mixerRuntime
.deadbandMotor3dLow
;
145 if (isMotorProtocolDshot()) {
146 motorOutputMin
= mixerRuntime
.motorOutputLow
;
147 motorOutputRange
= mixerRuntime
.deadbandMotor3dLow
- mixerRuntime
.motorOutputLow
;
151 motorOutputMin
= mixerRuntime
.deadbandMotor3dLow
;
152 motorOutputRange
= mixerRuntime
.motorOutputLow
- mixerRuntime
.deadbandMotor3dLow
;
155 if (motorOutputMixSign
!= -1) {
156 reversalTimeUs
= currentTimeUs
;
158 motorOutputMixSign
= -1;
160 rcThrottlePrevious
= rcCommand
[THROTTLE
];
161 throttle
= rcCommand3dDeadBandLow
- rcCommand
[THROTTLE
];
162 currentThrottleInputRange
= rcCommandThrottleRange3dLow
;
163 } else if (rcCommand
[THROTTLE
] >= rcCommand3dDeadBandHigh
) {
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
;
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
;
184 if (isMotorProtocolDshot()) {
185 motorOutputMin
= mixerRuntime
.motorOutputLow
;
186 motorOutputRange
= mixerRuntime
.deadbandMotor3dLow
- mixerRuntime
.motorOutputLow
;
190 motorOutputMin
= mixerRuntime
.deadbandMotor3dLow
;
191 motorOutputRange
= mixerRuntime
.motorOutputLow
- mixerRuntime
.deadbandMotor3dLow
;
194 if (motorOutputMixSign
!= -1) {
195 reversalTimeUs
= currentTimeUs
;
197 motorOutputMixSign
= -1;
200 currentThrottleInputRange
= rcCommandThrottleRange3dLow
;
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
;
210 motorOutputMixSign
= 1;
212 currentThrottleInputRange
= rcCommandThrottleRange3dHigh
;
214 if (currentTimeUs
- reversalTimeUs
< 250000) {
215 // keep iterm zero for 250ms after motor reversal
219 throttle
= rcCommand
[THROTTLE
] - PWM_RANGE_MIN
+ throttleAngleCorrection
;
220 currentThrottleInputRange
= PWM_RANGE
;
222 if (mixerRuntime
.dynIdleMinRps
> 0.0f
) {
223 const float maxIncrease
= isAirmodeActivated()
224 ? mixerRuntime
.dynIdleMaxIncrease
: mixerRuntime
.dynIdleStartIncrease
;
225 float minRps
= getMinMotorFrequencyHz();
226 DEBUG_SET(DEBUG_DYN_IDLE
, 3, lrintf(minRps
* 10.0f
));
227 float rpsError
= mixerRuntime
.dynIdleMinRps
- minRps
;
228 // PT1 type lowpass delay and smoothing for D
229 minRps
= mixerRuntime
.prevMinRps
+ mixerRuntime
.minRpsDelayK
* (minRps
- mixerRuntime
.prevMinRps
);
230 float dynIdleD
= (mixerRuntime
.prevMinRps
- minRps
) * mixerRuntime
.dynIdleDGain
;
231 mixerRuntime
.prevMinRps
= minRps
;
232 float dynIdleP
= rpsError
* mixerRuntime
.dynIdlePGain
;
233 rpsError
= MAX(-0.1f
, rpsError
); //I rises fast, falls slowly
234 mixerRuntime
.dynIdleI
+= rpsError
* mixerRuntime
.dynIdleIGain
;
235 mixerRuntime
.dynIdleI
= constrainf(mixerRuntime
.dynIdleI
, 0.0f
, maxIncrease
);
236 motorRangeMinIncrease
= constrainf((dynIdleP
+ mixerRuntime
.dynIdleI
+ dynIdleD
), 0.0f
, maxIncrease
);
237 DEBUG_SET(DEBUG_DYN_IDLE
, 0, MAX(-1000, lrintf(dynIdleP
* 10000)));
238 DEBUG_SET(DEBUG_DYN_IDLE
, 1, lrintf(mixerRuntime
.dynIdleI
* 10000));
239 DEBUG_SET(DEBUG_DYN_IDLE
, 2, lrintf(dynIdleD
* 10000));
241 motorRangeMinIncrease
= 0;
245 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
246 float motorRangeAttenuationFactor
= 0;
247 // reduce motorRangeMax when battery is full
248 if (mixerRuntime
.vbatSagCompensationFactor
> 0.0f
) {
249 const uint16_t currentCellVoltage
= getBatterySagCellVoltage();
250 // batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow
251 float batteryGoodness
= 1.0f
- constrainf((mixerRuntime
.vbatFull
- currentCellVoltage
) / mixerRuntime
.vbatRangeToCompensate
, 0.0f
, 1.0f
);
252 motorRangeAttenuationFactor
= (mixerRuntime
.vbatRangeToCompensate
/ mixerRuntime
.vbatFull
) * batteryGoodness
* mixerRuntime
.vbatSagCompensationFactor
;
253 DEBUG_SET(DEBUG_BATTERY
, 2, lrintf(batteryGoodness
* 100));
254 DEBUG_SET(DEBUG_BATTERY
, 3, lrintf(motorRangeAttenuationFactor
* 1000));
256 motorRangeMax
= isFlipOverAfterCrashActive() ? mixerRuntime
.motorOutputHigh
: mixerRuntime
.motorOutputHigh
- motorRangeAttenuationFactor
* (mixerRuntime
.motorOutputHigh
- mixerRuntime
.motorOutputLow
);
258 motorRangeMax
= mixerRuntime
.motorOutputHigh
;
260 motorRangeMin
= mixerRuntime
.motorOutputLow
+ motorRangeMinIncrease
* (mixerRuntime
.motorOutputHigh
- mixerRuntime
.motorOutputLow
);
261 motorOutputMin
= motorRangeMin
;
262 motorOutputRange
= motorRangeMax
- motorRangeMin
;
263 motorOutputMixSign
= 1;
266 throttle
= constrainf(throttle
/ currentThrottleInputRange
, 0.0f
, 1.0f
);
269 #define CRASH_FLIP_DEADBAND 20
270 #define CRASH_FLIP_STICK_MINF 0.15f
272 static void applyFlipOverAfterCrashModeToMotors(void)
274 if (ARMING_FLAG(ARMED
)) {
275 const float flipPowerFactor
= 1.0f
- mixerConfig()->crashflip_expo
/ 100.0f
;
276 const float stickDeflectionPitchAbs
= getRcDeflectionAbs(FD_PITCH
);
277 const float stickDeflectionRollAbs
= getRcDeflectionAbs(FD_ROLL
);
278 const float stickDeflectionYawAbs
= getRcDeflectionAbs(FD_YAW
);
280 const float stickDeflectionPitchExpo
= flipPowerFactor
* stickDeflectionPitchAbs
+ power3(stickDeflectionPitchAbs
) * (1 - flipPowerFactor
);
281 const float stickDeflectionRollExpo
= flipPowerFactor
* stickDeflectionRollAbs
+ power3(stickDeflectionRollAbs
) * (1 - flipPowerFactor
);
282 const float stickDeflectionYawExpo
= flipPowerFactor
* stickDeflectionYawAbs
+ power3(stickDeflectionYawAbs
) * (1 - flipPowerFactor
);
284 float signPitch
= getRcDeflection(FD_PITCH
) < 0 ? 1 : -1;
285 float signRoll
= getRcDeflection(FD_ROLL
) < 0 ? 1 : -1;
286 float signYaw
= (getRcDeflection(FD_YAW
) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed
? 1 : -1);
288 float stickDeflectionLength
= sqrtf(sq(stickDeflectionPitchAbs
) + sq(stickDeflectionRollAbs
));
289 float stickDeflectionExpoLength
= sqrtf(sq(stickDeflectionPitchExpo
) + sq(stickDeflectionRollExpo
));
291 if (stickDeflectionYawAbs
> MAX(stickDeflectionPitchAbs
, stickDeflectionRollAbs
)) {
292 // If yaw is the dominant, disable pitch and roll
293 stickDeflectionLength
= stickDeflectionYawAbs
;
294 stickDeflectionExpoLength
= stickDeflectionYawExpo
;
298 // If pitch/roll dominant, disable yaw
302 const float cosPhi
= (stickDeflectionLength
> 0) ? (stickDeflectionPitchAbs
+ stickDeflectionRollAbs
) / (sqrtf(2.0f
) * stickDeflectionLength
) : 0;
303 const float cosThreshold
= sqrtf(3.0f
)/2.0f
; // cos(PI/6.0f)
305 if (cosPhi
< cosThreshold
) {
306 // Enforce either roll or pitch exclusively, if not on diagonal
307 if (stickDeflectionRollAbs
> stickDeflectionPitchAbs
) {
314 // Apply a reasonable amount of stick deadband
315 const float crashFlipStickMinExpo
= flipPowerFactor
* CRASH_FLIP_STICK_MINF
+ power3(CRASH_FLIP_STICK_MINF
) * (1 - flipPowerFactor
);
316 const float flipStickRange
= 1.0f
- crashFlipStickMinExpo
;
317 const float flipPower
= MAX(0.0f
, stickDeflectionExpoLength
- crashFlipStickMinExpo
) / flipStickRange
;
319 for (int i
= 0; i
< mixerRuntime
.motorCount
; ++i
) {
320 float motorOutputNormalised
=
321 signPitch
* mixerRuntime
.currentMixer
[i
].pitch
+
322 signRoll
* mixerRuntime
.currentMixer
[i
].roll
+
323 signYaw
* mixerRuntime
.currentMixer
[i
].yaw
;
325 if (motorOutputNormalised
< 0) {
326 if (mixerConfig()->crashflip_motor_percent
> 0) {
327 motorOutputNormalised
= -motorOutputNormalised
* (float)mixerConfig()->crashflip_motor_percent
/ 100.0f
;
329 motorOutputNormalised
= 0;
332 motorOutputNormalised
= MIN(1.0f
, flipPower
* motorOutputNormalised
);
333 float motorOutput
= motorOutputMin
+ motorOutputNormalised
* motorOutputRange
;
335 // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
336 motorOutput
= (motorOutput
< motorOutputMin
+ CRASH_FLIP_DEADBAND
) ? mixerRuntime
.disarmMotorOutput
: (motorOutput
- CRASH_FLIP_DEADBAND
);
338 motor
[i
] = motorOutput
;
342 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
343 motor
[i
] = motor_disarmed
[i
];
349 #define STICK_HIGH_DEADBAND 5 // deadband to make sure throttle cap can raise, even with maxcheck set around 2000
350 static void applyRpmLimiter(mixerRuntime_t
*mixer
)
352 static float prevError
= 0.0f
;
353 const float unsmoothedAverageRpm
= getDshotRpmAverage();
354 const float averageRpm
= pt1FilterApply(&mixer
->rpmLimiterAverageRpmFilter
, unsmoothedAverageRpm
);
355 const float error
= averageRpm
- mixer
->rpmLimiterRpmLimit
;
358 const float p
= error
* mixer
->rpmLimiterPGain
;
359 const float d
= (error
- prevError
) * mixer
->rpmLimiterDGain
; // rpmLimiterDGain already adjusted for looprate (see mixer_init.c)
360 mixer
->rpmLimiterI
+= error
* mixer
->rpmLimiterIGain
; // rpmLimiterIGain already adjusted for looprate (see mixer_init.c)
361 mixer
->rpmLimiterI
= MAX(0.0f
, mixer
->rpmLimiterI
);
362 float pidOutput
= p
+ mixer
->rpmLimiterI
+ d
;
364 // Throttle limit learning
365 if (error
> 0.0f
&& rcCommand
[THROTTLE
] < rxConfig()->maxcheck
) {
366 mixer
->rpmLimiterThrottleScale
*= 1.0f
- 4.8f
* pidGetDT();
367 } else if (pidOutput
< -400.0f
* pidGetDT() && lrintf(rcCommand
[THROTTLE
]) >= rxConfig()->maxcheck
- STICK_HIGH_DEADBAND
&& !areMotorsSaturated()) { // Throttle accel corresponds with motor accel
368 mixer
->rpmLimiterThrottleScale
*= 1.0f
+ 3.2f
* pidGetDT();
370 mixer
->rpmLimiterThrottleScale
= constrainf(mixer
->rpmLimiterThrottleScale
, 0.01f
, 1.0f
);
372 float rpmLimiterThrottleScaleOffset
= pt1FilterApply(&mixer
->rpmLimiterThrottleScaleOffsetFilter
, constrainf(mixer
->rpmLimiterRpmLimit
/ motorEstimateMaxRpm(), 0.0f
, 1.0f
) - mixer
->rpmLimiterInitialThrottleScale
);
373 throttle
*= constrainf(mixer
->rpmLimiterThrottleScale
+ rpmLimiterThrottleScaleOffset
, 0.0f
, 1.0f
);
376 pidOutput
= MAX(0.0f
, pidOutput
);
377 throttle
= constrainf(throttle
- pidOutput
, 0.0f
, 1.0f
);
380 DEBUG_SET(DEBUG_RPM_LIMIT
, 0, lrintf(averageRpm
));
381 DEBUG_SET(DEBUG_RPM_LIMIT
, 1, lrintf(rpmLimiterThrottleScaleOffset
* 100.0f
));
382 DEBUG_SET(DEBUG_RPM_LIMIT
, 2, lrintf(mixer
->rpmLimiterThrottleScale
* 100.0f
));
383 DEBUG_SET(DEBUG_RPM_LIMIT
, 3, lrintf(throttle
* 100.0f
));
384 DEBUG_SET(DEBUG_RPM_LIMIT
, 4, lrintf(error
));
385 DEBUG_SET(DEBUG_RPM_LIMIT
, 5, lrintf(p
* 100.0f
));
386 DEBUG_SET(DEBUG_RPM_LIMIT
, 6, lrintf(mixer
->rpmLimiterI
* 100.0f
));
387 DEBUG_SET(DEBUG_RPM_LIMIT
, 7, lrintf(d
* 100.0f
));
389 #endif // USE_RPM_LIMIT
391 static void applyMixToMotors(float motorMix
[MAX_SUPPORTED_MOTORS
], motorMixer_t
*activeMixer
)
393 // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
394 // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
395 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
396 float motorOutput
= motorOutputMixSign
* motorMix
[i
] + throttle
* activeMixer
[i
].throttle
;
397 #ifdef USE_THRUST_LINEARIZATION
398 motorOutput
= pidApplyThrustLinearization(motorOutput
);
400 motorOutput
= motorOutputMin
+ motorOutputRange
* motorOutput
;
403 if (mixerIsTricopter()) {
404 motorOutput
+= mixerTricopterMotorCorrection(i
);
407 if (failsafeIsActive()) {
409 if (isMotorProtocolDshot()) {
410 motorOutput
= (motorOutput
< motorRangeMin
) ? mixerRuntime
.disarmMotorOutput
: motorOutput
; // Prevent getting into special reserved range
413 motorOutput
= constrainf(motorOutput
, mixerRuntime
.disarmMotorOutput
, motorRangeMax
);
415 motorOutput
= constrainf(motorOutput
, motorRangeMin
, motorRangeMax
);
417 motor
[i
] = motorOutput
;
421 if (!ARMING_FLAG(ARMED
)) {
422 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
423 motor
[i
] = motor_disarmed
[i
];
426 DEBUG_SET(DEBUG_EZLANDING
, 1, throttle
* 10000U);
427 // DEBUG_EZLANDING 0 is the ezLanding factor 2 is the throttle limit
430 static float applyThrottleLimit(float throttle
)
432 if (currentControlRateProfile
->throttle_limit_percent
< 100 && !RPM_LIMIT_ACTIVE
) {
433 const float throttleLimitFactor
= currentControlRateProfile
->throttle_limit_percent
/ 100.0f
;
434 switch (currentControlRateProfile
->throttle_limit_type
) {
435 case THROTTLE_LIMIT_TYPE_SCALE
:
436 return throttle
* throttleLimitFactor
;
437 case THROTTLE_LIMIT_TYPE_CLIP
:
438 return MIN(throttle
, throttleLimitFactor
);
445 static void applyMotorStop(void)
447 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
448 motor
[i
] = mixerRuntime
.disarmMotorOutput
;
453 static void updateDynLpfCutoffs(timeUs_t currentTimeUs
, float throttle
)
455 static timeUs_t lastDynLpfUpdateUs
= 0;
456 static int dynLpfPreviousQuantizedThrottle
= -1; // to allow an initial zero throttle to set the filter cutoff
458 if (cmpTimeUs(currentTimeUs
, lastDynLpfUpdateUs
) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US
) {
459 const int quantizedThrottle
= lrintf(throttle
* DYN_LPF_THROTTLE_STEPS
); // quantize the throttle reduce the number of filter updates
460 if (quantizedThrottle
!= dynLpfPreviousQuantizedThrottle
) {
461 // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable
462 const float dynLpfThrottle
= (float)quantizedThrottle
/ DYN_LPF_THROTTLE_STEPS
;
463 dynLpfGyroUpdate(dynLpfThrottle
);
464 dynLpfDTermUpdate(dynLpfThrottle
);
465 dynLpfPreviousQuantizedThrottle
= quantizedThrottle
;
466 lastDynLpfUpdateUs
= currentTimeUs
;
472 static void applyMixerAdjustmentLinear(float *motorMix
, const bool airmodeEnabled
)
474 float airmodeTransitionPercent
= 1.0f
;
475 float motorDeltaScale
= 0.5f
;
477 if (!airmodeEnabled
&& throttle
< 0.5f
) {
478 // 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.
479 // also lays the groundwork for how an airmode percent would work.
480 airmodeTransitionPercent
= scaleRangef(throttle
, 0.0f
, 0.5f
, 0.5f
, 1.0f
); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
481 motorDeltaScale
*= airmodeTransitionPercent
; // this should be half of the motor authority allowed
484 const float motorMixNormalizationFactor
= motorMixRange
> 1.0f
? airmodeTransitionPercent
/ motorMixRange
: airmodeTransitionPercent
;
486 const float motorMixDelta
= motorDeltaScale
* motorMixRange
;
488 float minMotor
= FLT_MAX
;
489 float maxMotor
= FLT_MIN
;
491 for (int i
= 0; i
< mixerRuntime
.motorCount
; ++i
) {
492 if (mixerConfig()->mixer_type
== MIXER_LINEAR
) {
493 motorMix
[i
] = scaleRangef(throttle
, 0.0f
, 1.0f
, motorMix
[i
] + motorMixDelta
, motorMix
[i
] - motorMixDelta
);
495 motorMix
[i
] = scaleRangef(throttle
, 0.0f
, 1.0f
, motorMix
[i
] + fabsf(motorMix
[i
]), motorMix
[i
] - fabsf(motorMix
[i
]));
497 motorMix
[i
] *= motorMixNormalizationFactor
;
499 maxMotor
= MAX(motorMix
[i
], maxMotor
);
500 minMotor
= MIN(motorMix
[i
], minMotor
);
503 // constrain throttle so it won't clip any outputs
504 throttle
= constrainf(throttle
, -minMotor
, 1.0f
- maxMotor
);
507 static float calcEzLandLimit(float maxDeflection
, float speed
)
509 // calculate limit to where the mixer can raise the throttle based on RPY stick deflection
510 // 0.0 = no increas allowed, 1.0 = 100% increase allowed
511 const float deflectionLimit
= mixerRuntime
.ezLandingThreshold
> 0.0f
? fminf(1.0f
, maxDeflection
/ mixerRuntime
.ezLandingThreshold
) : 0.0f
;
512 DEBUG_SET(DEBUG_EZLANDING
, 4, lrintf(deflectionLimit
* 10000.0f
));
514 // calculate limit to where the mixer can raise the throttle based on speed
515 // TODO sanity checks like number of sats, dop, accuracy?
516 const float speedLimit
= mixerRuntime
.ezLandingSpeed
> 0.0f
? fminf(1.0f
, speed
/ mixerRuntime
.ezLandingSpeed
) : 0.0f
;
517 DEBUG_SET(DEBUG_EZLANDING
, 5, lrintf(speedLimit
* 10000.0f
));
519 // get the highest of the limits from deflection, speed, and the base ez_landing_limit
520 const float deflectionAndSpeedLimit
= fmaxf(deflectionLimit
, speedLimit
);
521 return fmaxf(mixerRuntime
.ezLandingLimit
, deflectionAndSpeedLimit
);
524 static void applyMixerAdjustmentEzLand(float *motorMix
, const float motorMixMin
, const float motorMixMax
)
526 // Calculate factor for normalizing motor mix range to <= 1.0
527 const float baseNormalizationFactor
= motorMixRange
> 1.0f
? 1.0f
/ motorMixRange
: 1.0f
;
528 const float normalizedMotorMixMin
= motorMixMin
* baseNormalizationFactor
;
529 const float normalizedMotorMixMax
= motorMixMax
* baseNormalizationFactor
;
532 const float speed
= STATE(GPS_FIX
) ? gpsSol
.speed3d
/ 100.0f
: 0.0f
; // m/s
534 const float speed
= 0.0f
;
537 const float ezLandLimit
= calcEzLandLimit(getMaxRcDeflectionAbs(), speed
);
538 // use the largest of throttle and limit calculated from RPY stick positions
539 float upperLimit
= fmaxf(ezLandLimit
, throttle
);
540 // limit throttle to avoid clipping the highest motor output
541 upperLimit
= fminf(upperLimit
, 1.0f
- normalizedMotorMixMax
);
543 // Lower throttle Limit
544 const float epsilon
= 1.0e-6f
; // add small value to avoid divisions by zero
545 const float absMotorMixMin
= fabsf(normalizedMotorMixMin
) + epsilon
;
546 const float lowerLimit
= fminf(upperLimit
, absMotorMixMin
);
548 // represents how much motor values have to be scaled to avoid clipping
549 const float ezLandFactor
= upperLimit
/ absMotorMixMin
;
551 // scale motor values
552 const float normalizationFactor
= baseNormalizationFactor
* fminf(1.0f
, ezLandFactor
);
553 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
554 motorMix
[i
] *= normalizationFactor
;
556 motorMixRange
*= baseNormalizationFactor
;
557 // Make anti windup recognize reduced authority range
558 motorMixRange
= fmaxf(motorMixRange
, 1.0f
/ ezLandFactor
);
560 // Constrain throttle
561 throttle
= constrainf(throttle
, lowerLimit
, upperLimit
);
563 // Log ezLandFactor, upper throttle limit, and ezLandFactor if throttle was zero
564 DEBUG_SET(DEBUG_EZLANDING
, 0, fminf(1.0f
, ezLandFactor
) * 10000U);
565 // DEBUG_EZLANDING 1 is the adjusted throttle
566 DEBUG_SET(DEBUG_EZLANDING
, 2, upperLimit
* 10000U);
567 DEBUG_SET(DEBUG_EZLANDING
, 3, fminf(1.0f
, ezLandLimit
/ absMotorMixMin
) * 10000U);
568 // DEBUG_EZLANDING 4 and 5 is the upper limits based on stick input and speed respectively
571 static void applyMixerAdjustment(float *motorMix
, const float motorMixMin
, const float motorMixMax
, const bool airmodeEnabled
)
573 #ifdef USE_AIRMODE_LPF
574 const float unadjustedThrottle
= throttle
;
575 throttle
+= pidGetAirmodeThrottleOffset();
576 float airmodeThrottleChange
= 0.0f
;
578 float airmodeTransitionPercent
= 1.0f
;
580 if (!airmodeEnabled
&& throttle
< 0.5f
) {
581 // 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.
582 // also lays the groundwork for how an airmode percent would work.
583 airmodeTransitionPercent
= scaleRangef(throttle
, 0.0f
, 0.5f
, 0.5f
, 1.0f
); // 0.5 throttle is full transition, and 0.0 throttle is 50% airmodeTransitionPercent
586 const float motorMixNormalizationFactor
= motorMixRange
> 1.0f
? airmodeTransitionPercent
/ motorMixRange
: airmodeTransitionPercent
;
588 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
589 motorMix
[i
] *= motorMixNormalizationFactor
;
592 const float normalizedMotorMixMin
= motorMixMin
* motorMixNormalizationFactor
;
593 const float normalizedMotorMixMax
= motorMixMax
* motorMixNormalizationFactor
;
594 throttle
= constrainf(throttle
, -normalizedMotorMixMin
, 1.0f
- normalizedMotorMixMax
);
596 #ifdef USE_AIRMODE_LPF
597 airmodeThrottleChange
= constrainf(unadjustedThrottle
, -normalizedMotorMixMin
, 1.0f
- normalizedMotorMixMax
) - unadjustedThrottle
;
598 pidUpdateAirmodeLpf(airmodeThrottleChange
);
602 FAST_CODE_NOINLINE
void mixTable(timeUs_t currentTimeUs
)
604 // Find min and max throttle based on conditions. Throttle has to be known before mixing
605 calculateThrottleAndCurrentMotorEndpoints(currentTimeUs
);
607 if (isFlipOverAfterCrashActive()) {
608 applyFlipOverAfterCrashModeToMotors();
612 const bool launchControlActive
= isLaunchControlActive();
614 motorMixer_t
* activeMixer
= &mixerRuntime
.currentMixer
[0];
615 #ifdef USE_LAUNCH_CONTROL
616 if (launchControlActive
&& (currentPidProfile
->launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
)) {
617 activeMixer
= &mixerRuntime
.launchControlMixer
[0];
621 // Calculate and Limit the PID sum
622 const float scaledAxisPidRoll
=
623 constrainf(pidData
[FD_ROLL
].Sum
, -currentPidProfile
->pidSumLimit
, currentPidProfile
->pidSumLimit
) / PID_MIXER_SCALING
;
624 const float scaledAxisPidPitch
=
625 constrainf(pidData
[FD_PITCH
].Sum
, -currentPidProfile
->pidSumLimit
, currentPidProfile
->pidSumLimit
) / PID_MIXER_SCALING
;
627 uint16_t yawPidSumLimit
= currentPidProfile
->pidSumLimitYaw
;
629 #ifdef USE_YAW_SPIN_RECOVERY
630 const bool yawSpinDetected
= gyroYawSpinDetected();
631 if (yawSpinDetected
) {
632 yawPidSumLimit
= PIDSUM_LIMIT_MAX
; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
634 #endif // USE_YAW_SPIN_RECOVERY
636 float scaledAxisPidYaw
=
637 constrainf(pidData
[FD_YAW
].Sum
, -yawPidSumLimit
, yawPidSumLimit
) / PID_MIXER_SCALING
;
639 if (!mixerConfig()->yaw_motors_reversed
) {
640 scaledAxisPidYaw
= -scaledAxisPidYaw
;
643 // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
644 if (currentControlRateProfile
->throttle_limit_type
!= THROTTLE_LIMIT_TYPE_OFF
) {
645 throttle
= applyThrottleLimit(throttle
);
648 // use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
649 pidUpdateAntiGravityThrottleFilter(throttle
);
652 pidUpdateTpaFactor(throttle
);
655 // keep the changes to dynamic lowpass clean, without unnecessary dynamic changes
656 updateDynLpfCutoffs(currentTimeUs
, throttle
);
659 // apply throttle boost when throttle moves quickly
660 #if defined(USE_THROTTLE_BOOST)
661 if (throttleBoost
> 0.0f
) {
662 const float throttleHpf
= throttle
- pt1FilterApply(&throttleLpf
, throttle
);
663 throttle
= constrainf(throttle
+ throttleBoost
* throttleHpf
, 0.0f
, 1.0f
);
667 // send throttle value to blackbox, including scaling and throttle boost, but not TL compensation, dyn idle or airmode
668 mixerThrottle
= throttle
;
671 // Set min throttle offset of 1% when stick is at zero and dynamic idle is active
672 if (mixerRuntime
.dynIdleMinRps
> 0.0f
) {
673 throttle
= MAX(throttle
, 0.01f
);
677 #ifdef USE_THRUST_LINEARIZATION
678 // reduce throttle to offset additional motor output
679 throttle
= pidCompensateThrustLinearization(throttle
);
683 if (RPM_LIMIT_ACTIVE
&& useDshotTelemetry
&& ARMING_FLAG(ARMED
)) {
684 applyRpmLimiter(&mixerRuntime
);
688 // Find roll/pitch/yaw desired output
689 // ??? Where is the optimal location for this code?
690 float motorMix
[MAX_SUPPORTED_MOTORS
];
691 float motorMixMax
= 0, motorMixMin
= 0;
692 for (int i
= 0; i
< mixerRuntime
.motorCount
; i
++) {
694 scaledAxisPidRoll
* activeMixer
[i
].roll
+
695 scaledAxisPidPitch
* activeMixer
[i
].pitch
+
696 scaledAxisPidYaw
* activeMixer
[i
].yaw
;
698 if (mix
> motorMixMax
) {
700 } else if (mix
< motorMixMin
) {
706 // The following fixed throttle values will not be shown in the blackbox log
707 // ?? Should they be influenced by airmode? If not, should go after the apply airmode code.
708 const bool airmodeEnabled
= airmodeIsEnabled() || launchControlActive
;
709 #ifdef USE_YAW_SPIN_RECOVERY
710 // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
711 // When airmode is active the throttle setting doesn't impact recovery authority.
712 if (yawSpinDetected
&& !airmodeEnabled
) {
715 #endif // USE_YAW_SPIN_RECOVERY
717 #ifdef USE_LAUNCH_CONTROL
718 // While launch control is active keep the throttle at minimum.
719 // Once the pilot triggers the launch throttle control will be reactivated.
720 if (launchControlActive
) {
725 #ifdef USE_GPS_RESCUE
726 // If gps rescue is active then override the throttle. This prevents things
727 // like throttle boost or throttle limit from negatively affecting the throttle.
728 if (FLIGHT_MODE(GPS_RESCUE_MODE
)) {
729 throttle
= gpsRescueGetThrottle();
733 motorMixRange
= motorMixMax
- motorMixMin
;
735 switch (mixerConfig()->mixer_type
) {
737 applyMixerAdjustment(motorMix
, motorMixMin
, motorMixMax
, airmodeEnabled
);
741 applyMixerAdjustmentLinear(motorMix
, airmodeEnabled
);
743 case MIXER_EZLANDING
:
744 applyMixerAdjustmentEzLand(motorMix
, motorMixMin
, motorMixMax
);
747 applyMixerAdjustment(motorMix
, motorMixMin
, motorMixMax
, airmodeEnabled
);
751 if (featureIsEnabled(FEATURE_MOTOR_STOP
)
752 && ARMING_FLAG(ARMED
)
753 && !mixerRuntime
.feature3dEnabled
755 && !FLIGHT_MODE(GPS_RESCUE_MODE
) // disable motor_stop while GPS Rescue is active
756 && (rcData
[THROTTLE
] < rxConfig()->mincheck
)) {
757 // motor_stop handling
760 // Apply the mix to motor endpoints
761 applyMixToMotors(motorMix
, activeMixer
);
765 void mixerSetThrottleAngleCorrection(int correctionValue
)
767 throttleAngleCorrection
= correctionValue
;
770 float mixerGetThrottle(void)
772 return mixerThrottle
;