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/>.
28 #include "build/build_config.h"
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/filter.h"
33 #include "common/maths.h"
35 #include "config/feature.h"
37 #include "pg/pg_ids.h"
40 #include "drivers/pwm_output.h"
41 #include "drivers/pwm_esc_detect.h"
42 #include "drivers/time.h"
43 #include "drivers/io.h"
45 #include "io/motors.h"
47 #include "fc/config.h"
48 #include "fc/controlrate_profile.h"
49 #include "fc/rc_controls.h"
50 #include "fc/rc_modes.h"
51 #include "fc/runtime_config.h"
55 #include "flight/failsafe.h"
56 #include "flight/imu.h"
57 #include "flight/gps_rescue.h"
58 #include "flight/mixer.h"
59 #include "flight/mixer_tricopter.h"
60 #include "flight/pid.h"
64 #include "sensors/battery.h"
65 #include "sensors/gyro.h"
67 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t
, mixerConfig
, PG_MIXER_CONFIG
, 0);
69 #define DYN_LPF_THROTTLE_STEPS 100
70 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates
72 PG_RESET_TEMPLATE(mixerConfig_t
, mixerConfig
,
73 .mixerMode
= DEFAULT_MIXER
,
74 .yaw_motors_reversed
= false,
75 .crashflip_motor_percent
= 0,
78 PG_REGISTER_WITH_RESET_FN(motorConfig_t
, motorConfig
, PG_MOTOR_CONFIG
, 1);
80 void pgResetFn_motorConfig(motorConfig_t
*motorConfig
)
83 motorConfig
->minthrottle
= 1000;
84 motorConfig
->dev
.motorPwmRate
= BRUSHED_MOTORS_PWM_RATE
;
85 motorConfig
->dev
.motorPwmProtocol
= PWM_TYPE_BRUSHED
;
86 motorConfig
->dev
.useUnsyncedPwm
= true;
88 #ifdef USE_BRUSHED_ESC_AUTODETECT
89 if (hardwareMotorType
== MOTOR_BRUSHED
) {
90 motorConfig
->minthrottle
= 1000;
91 motorConfig
->dev
.motorPwmRate
= BRUSHED_MOTORS_PWM_RATE
;
92 motorConfig
->dev
.motorPwmProtocol
= PWM_TYPE_BRUSHED
;
93 motorConfig
->dev
.useUnsyncedPwm
= true;
97 motorConfig
->minthrottle
= 1070;
98 motorConfig
->dev
.motorPwmRate
= BRUSHLESS_MOTORS_PWM_RATE
;
99 motorConfig
->dev
.motorPwmProtocol
= PWM_TYPE_ONESHOT125
;
102 motorConfig
->maxthrottle
= 2000;
103 motorConfig
->mincommand
= 1000;
104 motorConfig
->digitalIdleOffsetValue
= 550;
105 #ifdef USE_DSHOT_DMAR
106 motorConfig
->dev
.useBurstDshot
= ENABLE_DSHOT_DMAR
;
109 for (int motorIndex
= 0; motorIndex
< MAX_SUPPORTED_MOTORS
; motorIndex
++) {
110 motorConfig
->dev
.ioTags
[motorIndex
] = timerioTagGetByUsage(TIM_USE_MOTOR
, motorIndex
);
113 motorConfig
->motorPoleCount
= 14; // Most brushes motors that we use are 14 poles
116 PG_REGISTER_ARRAY(motorMixer_t
, MAX_SUPPORTED_MOTORS
, customMotorMixer
, PG_MOTOR_MIXER
, 0);
118 #define PWM_RANGE_MID 1500
120 static FAST_RAM_ZERO_INIT
uint8_t motorCount
;
121 static FAST_RAM_ZERO_INIT
float motorMixRange
;
123 float FAST_RAM_ZERO_INIT motor
[MAX_SUPPORTED_MOTORS
];
124 float motor_disarmed
[MAX_SUPPORTED_MOTORS
];
126 mixerMode_e currentMixerMode
;
127 static motorMixer_t currentMixer
[MAX_SUPPORTED_MOTORS
];
129 #ifdef USE_LAUNCH_CONTROL
130 static motorMixer_t launchControlMixer
[MAX_SUPPORTED_MOTORS
];
133 static FAST_RAM_ZERO_INIT
int throttleAngleCorrection
;
136 static const motorMixer_t mixerQuadX
[] = {
137 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
138 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
139 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
140 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
142 #ifndef USE_QUAD_MIXER_ONLY
143 static const motorMixer_t mixerTricopter
[] = {
144 { 1.0f
, 0.0f
, 1.333333f
, 0.0f
}, // REAR
145 { 1.0f
, -1.0f
, -0.666667f
, 0.0f
}, // RIGHT
146 { 1.0f
, 1.0f
, -0.666667f
, 0.0f
}, // LEFT
149 static const motorMixer_t mixerQuadP
[] = {
150 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
151 { 1.0f
, -1.0f
, 0.0f
, 1.0f
}, // RIGHT
152 { 1.0f
, 1.0f
, 0.0f
, 1.0f
}, // LEFT
153 { 1.0f
, 0.0f
, -1.0f
, -1.0f
}, // FRONT
156 #if defined(USE_UNCOMMON_MIXERS)
157 static const motorMixer_t mixerBicopter
[] = {
158 { 1.0f
, 1.0f
, 0.0f
, 0.0f
}, // LEFT
159 { 1.0f
, -1.0f
, 0.0f
, 0.0f
}, // RIGHT
162 #define mixerBicopter NULL
165 static const motorMixer_t mixerY4
[] = {
166 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR_TOP CW
167 { 1.0f
, -1.0f
, -1.0f
, 0.0f
}, // FRONT_R CCW
168 { 1.0f
, 0.0f
, 1.0f
, 1.0f
}, // REAR_BOTTOM CCW
169 { 1.0f
, 1.0f
, -1.0f
, 0.0f
}, // FRONT_L CW
173 #if (MAX_SUPPORTED_MOTORS >= 6)
174 static const motorMixer_t mixerHex6X
[] = {
175 { 1.0f
, -0.5f
, 0.866025f
, 1.0f
}, // REAR_R
176 { 1.0f
, -0.5f
, -0.866025f
, 1.0f
}, // FRONT_R
177 { 1.0f
, 0.5f
, 0.866025f
, -1.0f
}, // REAR_L
178 { 1.0f
, 0.5f
, -0.866025f
, -1.0f
}, // FRONT_L
179 { 1.0f
, -1.0f
, 0.0f
, -1.0f
}, // RIGHT
180 { 1.0f
, 1.0f
, 0.0f
, 1.0f
}, // LEFT
183 #if defined(USE_UNCOMMON_MIXERS)
184 static const motorMixer_t mixerHex6H
[] = {
185 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
186 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
187 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
188 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
189 { 1.0f
, 0.0f
, 0.0f
, 0.0f
}, // RIGHT
190 { 1.0f
, 0.0f
, 0.0f
, 0.0f
}, // LEFT
193 static const motorMixer_t mixerHex6P
[] = {
194 { 1.0f
, -0.866025f
, 0.5f
, 1.0f
}, // REAR_R
195 { 1.0f
, -0.866025f
, -0.5f
, -1.0f
}, // FRONT_R
196 { 1.0f
, 0.866025f
, 0.5f
, 1.0f
}, // REAR_L
197 { 1.0f
, 0.866025f
, -0.5f
, -1.0f
}, // FRONT_L
198 { 1.0f
, 0.0f
, -1.0f
, 1.0f
}, // FRONT
199 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
201 static const motorMixer_t mixerY6
[] = {
202 { 1.0f
, 0.0f
, 1.333333f
, 1.0f
}, // REAR
203 { 1.0f
, -1.0f
, -0.666667f
, -1.0f
}, // RIGHT
204 { 1.0f
, 1.0f
, -0.666667f
, -1.0f
}, // LEFT
205 { 1.0f
, 0.0f
, 1.333333f
, -1.0f
}, // UNDER_REAR
206 { 1.0f
, -1.0f
, -0.666667f
, 1.0f
}, // UNDER_RIGHT
207 { 1.0f
, 1.0f
, -0.666667f
, 1.0f
}, // UNDER_LEFT
210 #define mixerHex6H NULL
211 #define mixerHex6P NULL
213 #endif // USE_UNCOMMON_MIXERS
215 #define mixerHex6X NULL
216 #endif // MAX_SUPPORTED_MOTORS >= 6
218 #if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8)
219 static const motorMixer_t mixerOctoX8
[] = {
220 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
221 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
222 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
223 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
224 { 1.0f
, -1.0f
, 1.0f
, 1.0f
}, // UNDER_REAR_R
225 { 1.0f
, -1.0f
, -1.0f
, -1.0f
}, // UNDER_FRONT_R
226 { 1.0f
, 1.0f
, 1.0f
, -1.0f
}, // UNDER_REAR_L
227 { 1.0f
, 1.0f
, -1.0f
, 1.0f
}, // UNDER_FRONT_L
230 static const motorMixer_t mixerOctoFlatP
[] = {
231 { 1.0f
, 0.707107f
, -0.707107f
, 1.0f
}, // FRONT_L
232 { 1.0f
, -0.707107f
, -0.707107f
, 1.0f
}, // FRONT_R
233 { 1.0f
, -0.707107f
, 0.707107f
, 1.0f
}, // REAR_R
234 { 1.0f
, 0.707107f
, 0.707107f
, 1.0f
}, // REAR_L
235 { 1.0f
, 0.0f
, -1.0f
, -1.0f
}, // FRONT
236 { 1.0f
, -1.0f
, 0.0f
, -1.0f
}, // RIGHT
237 { 1.0f
, 0.0f
, 1.0f
, -1.0f
}, // REAR
238 { 1.0f
, 1.0f
, 0.0f
, -1.0f
}, // LEFT
241 static const motorMixer_t mixerOctoFlatX
[] = {
242 { 1.0f
, 1.0f
, -0.414178f
, 1.0f
}, // MIDFRONT_L
243 { 1.0f
, -0.414178f
, -1.0f
, 1.0f
}, // FRONT_R
244 { 1.0f
, -1.0f
, 0.414178f
, 1.0f
}, // MIDREAR_R
245 { 1.0f
, 0.414178f
, 1.0f
, 1.0f
}, // REAR_L
246 { 1.0f
, 0.414178f
, -1.0f
, -1.0f
}, // FRONT_L
247 { 1.0f
, -1.0f
, -0.414178f
, -1.0f
}, // MIDFRONT_R
248 { 1.0f
, -0.414178f
, 1.0f
, -1.0f
}, // REAR_R
249 { 1.0f
, 1.0f
, 0.414178f
, -1.0f
}, // MIDREAR_L
252 #define mixerOctoX8 NULL
253 #define mixerOctoFlatP NULL
254 #define mixerOctoFlatX NULL
257 static const motorMixer_t mixerVtail4
[] = {
258 { 1.0f
, -0.58f
, 0.58f
, 1.0f
}, // REAR_R
259 { 1.0f
, -0.46f
, -0.39f
, -0.5f
}, // FRONT_R
260 { 1.0f
, 0.58f
, 0.58f
, -1.0f
}, // REAR_L
261 { 1.0f
, 0.46f
, -0.39f
, 0.5f
}, // FRONT_L
264 static const motorMixer_t mixerAtail4
[] = {
265 { 1.0f
, -0.58f
, 0.58f
, -1.0f
}, // REAR_R
266 { 1.0f
, -0.46f
, -0.39f
, 0.5f
}, // FRONT_R
267 { 1.0f
, 0.58f
, 0.58f
, 1.0f
}, // REAR_L
268 { 1.0f
, 0.46f
, -0.39f
, -0.5f
}, // FRONT_L
271 #if defined(USE_UNCOMMON_MIXERS)
272 static const motorMixer_t mixerDualcopter
[] = {
273 { 1.0f
, 0.0f
, 0.0f
, -1.0f
}, // LEFT
274 { 1.0f
, 0.0f
, 0.0f
, 1.0f
}, // RIGHT
277 #define mixerDualcopter NULL
280 static const motorMixer_t mixerSingleProp
[] = {
281 { 1.0f
, 0.0f
, 0.0f
, 0.0f
},
284 static const motorMixer_t mixerQuadX1234
[] = {
285 { 1.0f
, 1.0f
, -1.0f
, -1.0f
}, // FRONT_L
286 { 1.0f
, -1.0f
, -1.0f
, 1.0f
}, // FRONT_R
287 { 1.0f
, -1.0f
, 1.0f
, -1.0f
}, // REAR_R
288 { 1.0f
, 1.0f
, 1.0f
, 1.0f
}, // REAR_L
291 // Keep synced with mixerMode_e
292 // Some of these entries are bogus when servos (USE_SERVOS) are not configured,
293 // but left untouched to keep ordinals synced with mixerMode_e (and configurator).
294 const mixer_t mixers
[] = {
295 // motors, use servo, motor mixer
296 { 0, false, NULL
}, // entry 0
297 { 3, true, mixerTricopter
}, // MIXER_TRI
298 { 4, false, mixerQuadP
}, // MIXER_QUADP
299 { 4, false, mixerQuadX
}, // MIXER_QUADX
300 { 2, true, mixerBicopter
}, // MIXER_BICOPTER
301 { 0, true, NULL
}, // * MIXER_GIMBAL
302 { 6, false, mixerY6
}, // MIXER_Y6
303 { 6, false, mixerHex6P
}, // MIXER_HEX6
304 { 1, true, mixerSingleProp
}, // * MIXER_FLYING_WING
305 { 4, false, mixerY4
}, // MIXER_Y4
306 { 6, false, mixerHex6X
}, // MIXER_HEX6X
307 { 8, false, mixerOctoX8
}, // MIXER_OCTOX8
308 { 8, false, mixerOctoFlatP
}, // MIXER_OCTOFLATP
309 { 8, false, mixerOctoFlatX
}, // MIXER_OCTOFLATX
310 { 1, true, mixerSingleProp
}, // * MIXER_AIRPLANE
311 { 1, true, mixerSingleProp
}, // * MIXER_HELI_120_CCPM
312 { 0, true, NULL
}, // * MIXER_HELI_90_DEG
313 { 4, false, mixerVtail4
}, // MIXER_VTAIL4
314 { 6, false, mixerHex6H
}, // MIXER_HEX6H
315 { 0, true, NULL
}, // * MIXER_PPM_TO_SERVO
316 { 2, true, mixerDualcopter
}, // MIXER_DUALCOPTER
317 { 1, true, NULL
}, // MIXER_SINGLECOPTER
318 { 4, false, mixerAtail4
}, // MIXER_ATAIL4
319 { 0, false, NULL
}, // MIXER_CUSTOM
320 { 2, true, NULL
}, // MIXER_CUSTOM_AIRPLANE
321 { 3, true, NULL
}, // MIXER_CUSTOM_TRI
322 { 4, false, mixerQuadX1234
},
324 #endif // !USE_QUAD_MIXER_ONLY
326 FAST_RAM_ZERO_INIT
float motorOutputHigh
, motorOutputLow
;
328 static FAST_RAM_ZERO_INIT
float disarmMotorOutput
, deadbandMotor3dHigh
, deadbandMotor3dLow
;
329 static FAST_RAM_ZERO_INIT
float rcCommandThrottleRange
;
331 uint8_t getMotorCount(void)
336 float getMotorMixRange(void)
338 return motorMixRange
;
341 bool areMotorsRunning(void)
343 bool motorsRunning
= false;
344 if (ARMING_FLAG(ARMED
)) {
345 motorsRunning
= true;
347 for (int i
= 0; i
< motorCount
; i
++) {
348 if (motor_disarmed
[i
] != disarmMotorOutput
) {
349 motorsRunning
= true;
356 return motorsRunning
;
360 bool mixerIsTricopter(void)
362 return (currentMixerMode
== MIXER_TRI
|| currentMixerMode
== MIXER_CUSTOM_TRI
);
366 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
367 // DSHOT scaling is done to the actual dshot range
368 void initEscEndpoints(void)
370 float motorOutputLimit
= 1.0f
;
371 if (currentPidProfile
->motor_output_limit
< 100) {
372 motorOutputLimit
= currentPidProfile
->motor_output_limit
/ 100.0f
;
375 // Can't use 'isMotorProtocolDshot()' here since motors haven't been initialised yet
376 switch (motorConfig()->dev
.motorPwmProtocol
) {
378 case PWM_TYPE_PROSHOT1000
:
379 case PWM_TYPE_DSHOT1200
:
380 case PWM_TYPE_DSHOT600
:
381 case PWM_TYPE_DSHOT300
:
382 case PWM_TYPE_DSHOT150
:
384 float outputLimitOffset
= (DSHOT_MAX_THROTTLE
- DSHOT_MIN_THROTTLE
) * (1 - motorOutputLimit
);
385 disarmMotorOutput
= DSHOT_CMD_MOTOR_STOP
;
386 if (featureIsEnabled(FEATURE_3D
)) {
387 motorOutputLow
= DSHOT_MIN_THROTTLE
+ ((DSHOT_3D_FORWARD_MIN_THROTTLE
- 1 - DSHOT_MIN_THROTTLE
) / 100.0f
) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue
);
388 motorOutputHigh
= DSHOT_MAX_THROTTLE
- outputLimitOffset
/ 2;
389 deadbandMotor3dHigh
= DSHOT_3D_FORWARD_MIN_THROTTLE
+ ((DSHOT_MAX_THROTTLE
- DSHOT_3D_FORWARD_MIN_THROTTLE
) / 100.0f
) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue
);
390 deadbandMotor3dLow
= DSHOT_3D_FORWARD_MIN_THROTTLE
- 1 - outputLimitOffset
/ 2;
392 motorOutputLow
= DSHOT_MIN_THROTTLE
+ ((DSHOT_MAX_THROTTLE
- DSHOT_MIN_THROTTLE
) / 100.0f
) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue
);
393 motorOutputHigh
= DSHOT_MAX_THROTTLE
- outputLimitOffset
;
400 if (featureIsEnabled(FEATURE_3D
)) {
401 float outputLimitOffset
= (flight3DConfig()->limit3d_high
- flight3DConfig()->limit3d_low
) * (1 - motorOutputLimit
) / 2;
402 disarmMotorOutput
= flight3DConfig()->neutral3d
;
403 motorOutputLow
= flight3DConfig()->limit3d_low
+ outputLimitOffset
;
404 motorOutputHigh
= flight3DConfig()->limit3d_high
- outputLimitOffset
;
405 deadbandMotor3dHigh
= flight3DConfig()->deadband3d_high
;
406 deadbandMotor3dLow
= flight3DConfig()->deadband3d_low
;
408 disarmMotorOutput
= motorConfig()->mincommand
;
409 motorOutputLow
= motorConfig()->minthrottle
;
410 motorOutputHigh
= motorConfig()->maxthrottle
- ((motorConfig()->maxthrottle
- motorConfig()->minthrottle
) * (1 - motorOutputLimit
));
415 rcCommandThrottleRange
= PWM_RANGE_MAX
- PWM_RANGE_MIN
;
418 void mixerInit(mixerMode_e mixerMode
)
420 currentMixerMode
= mixerMode
;
424 if (mixerIsTricopter()) {
425 mixerTricopterInit();
430 #ifdef USE_LAUNCH_CONTROL
431 // Create a custom mixer for launch control based on the current settings
432 // but disable the front motors. We don't care about roll or yaw because they
433 // are limited in the PID controller.
434 void loadLaunchControlMixer(void)
436 for (int i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++) {
437 launchControlMixer
[i
] = currentMixer
[i
];
438 // limit the front motors to minimum output
439 if (launchControlMixer
[i
].pitch
< 0.0f
) {
440 launchControlMixer
[i
].pitch
= 0.0f
;
441 launchControlMixer
[i
].throttle
= 0.0f
;
447 #ifndef USE_QUAD_MIXER_ONLY
449 void mixerConfigureOutput(void)
453 if (currentMixerMode
== MIXER_CUSTOM
|| currentMixerMode
== MIXER_CUSTOM_TRI
|| currentMixerMode
== MIXER_CUSTOM_AIRPLANE
) {
454 // load custom mixer into currentMixer
455 for (int i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++) {
457 if (customMotorMixer(i
)->throttle
== 0.0f
) {
460 currentMixer
[i
] = *customMotorMixer(i
);
464 motorCount
= mixers
[currentMixerMode
].motorCount
;
465 if (motorCount
> MAX_SUPPORTED_MOTORS
) {
466 motorCount
= MAX_SUPPORTED_MOTORS
;
468 // copy motor-based mixers
469 if (mixers
[currentMixerMode
].motor
) {
470 for (int i
= 0; i
< motorCount
; i
++)
471 currentMixer
[i
] = mixers
[currentMixerMode
].motor
[i
];
474 #ifdef USE_LAUNCH_CONTROL
475 loadLaunchControlMixer();
477 mixerResetDisarmedMotors();
480 void mixerLoadMix(int index
, motorMixer_t
*customMixers
)
485 for (int i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++) {
486 customMixers
[i
].throttle
= 0.0f
;
488 // do we have anything here to begin with?
489 if (mixers
[index
].motor
!= NULL
) {
490 for (int i
= 0; i
< mixers
[index
].motorCount
; i
++) {
491 customMixers
[i
] = mixers
[index
].motor
[i
];
496 void mixerConfigureOutput(void)
498 motorCount
= QUAD_MOTOR_COUNT
;
499 for (int i
= 0; i
< motorCount
; i
++) {
500 currentMixer
[i
] = mixerQuadX
[i
];
502 #ifdef USE_LAUNCH_CONTROL
503 loadLaunchControlMixer();
505 mixerResetDisarmedMotors();
507 #endif // USE_QUAD_MIXER_ONLY
509 void mixerResetDisarmedMotors(void)
511 // set disarmed motor values
512 for (int i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++) {
513 motor_disarmed
[i
] = disarmMotorOutput
;
517 void writeMotors(void)
519 if (pwmAreMotorsEnabled()) {
520 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
521 pwmStartMotorUpdate(motorCount
);
523 for (int i
= 0; i
< motorCount
; i
++) {
524 pwmWriteMotor(i
, motor
[i
]);
526 pwmCompleteMotorUpdate(motorCount
);
530 static void writeAllMotors(int16_t mc
)
532 // Sends commands to all motors
533 for (int i
= 0; i
< motorCount
; i
++) {
539 void stopMotors(void)
541 writeAllMotors(disarmMotorOutput
);
542 delay(50); // give the timers and ESCs a chance to react.
545 void stopPwmAllMotors(void)
547 pwmShutdownPulsesForAllMotors(motorCount
);
548 delayMicroseconds(1500);
551 static FAST_RAM_ZERO_INIT
float throttle
= 0;
552 static FAST_RAM_ZERO_INIT
float loggingThrottle
= 0;
553 static FAST_RAM_ZERO_INIT
float motorOutputMin
;
554 static FAST_RAM_ZERO_INIT
float motorRangeMin
;
555 static FAST_RAM_ZERO_INIT
float motorRangeMax
;
556 static FAST_RAM_ZERO_INIT
float motorOutputRange
;
557 static FAST_RAM_ZERO_INIT
int8_t motorOutputMixSign
;
559 static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs
)
561 static uint16_t rcThrottlePrevious
= 0; // Store the last throttle direction for deadband transitions
562 static timeUs_t reversalTimeUs
= 0; // time when motors last reversed in 3D mode
563 float currentThrottleInputRange
= 0;
565 if (featureIsEnabled(FEATURE_3D
)) {
566 uint16_t rcCommand3dDeadBandLow
;
567 uint16_t rcCommand3dDeadBandHigh
;
569 if (!ARMING_FLAG(ARMED
)) {
570 rcThrottlePrevious
= rxConfig()->midrc
; // When disarmed set to mid_rc. It always results in positive direction after arming.
573 if (IS_RC_MODE_ACTIVE(BOX3D
) || flight3DConfig()->switched_mode3d
) {
574 // The min_check range is halved because the output throttle is scaled to 500us.
575 // So by using half of min_check we maintain the same low-throttle deadband
576 // stick travel as normal non-3D mode.
577 const int mincheckOffset
= (rxConfig()->mincheck
- PWM_RANGE_MIN
) / 2;
578 rcCommand3dDeadBandLow
= rxConfig()->midrc
- mincheckOffset
;
579 rcCommand3dDeadBandHigh
= rxConfig()->midrc
+ mincheckOffset
;
581 rcCommand3dDeadBandLow
= rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
;
582 rcCommand3dDeadBandHigh
= rxConfig()->midrc
+ flight3DConfig()->deadband3d_throttle
;
585 const float rcCommandThrottleRange3dLow
= rcCommand3dDeadBandLow
- PWM_RANGE_MIN
;
586 const float rcCommandThrottleRange3dHigh
= PWM_RANGE_MAX
- rcCommand3dDeadBandHigh
;
588 if (rcCommand
[THROTTLE
] <= rcCommand3dDeadBandLow
|| isFlipOverAfterCrashActive()) {
590 motorRangeMin
= motorOutputLow
;
591 motorRangeMax
= deadbandMotor3dLow
;
592 if (isMotorProtocolDshot()) {
593 motorOutputMin
= motorOutputLow
;
594 motorOutputRange
= deadbandMotor3dLow
- motorOutputLow
;
596 motorOutputMin
= deadbandMotor3dLow
;
597 motorOutputRange
= motorOutputLow
- deadbandMotor3dLow
;
599 if (motorOutputMixSign
!= -1) {
600 reversalTimeUs
= currentTimeUs
;
602 motorOutputMixSign
= -1;
603 rcThrottlePrevious
= rcCommand
[THROTTLE
];
604 throttle
= rcCommand3dDeadBandLow
- rcCommand
[THROTTLE
];
605 currentThrottleInputRange
= rcCommandThrottleRange3dLow
;
606 } else if (rcCommand
[THROTTLE
] >= rcCommand3dDeadBandHigh
) {
608 motorRangeMin
= deadbandMotor3dHigh
;
609 motorRangeMax
= motorOutputHigh
;
610 motorOutputMin
= deadbandMotor3dHigh
;
611 motorOutputRange
= motorOutputHigh
- deadbandMotor3dHigh
;
612 if (motorOutputMixSign
!= 1) {
613 reversalTimeUs
= currentTimeUs
;
615 motorOutputMixSign
= 1;
616 rcThrottlePrevious
= rcCommand
[THROTTLE
];
617 throttle
= rcCommand
[THROTTLE
] - rcCommand3dDeadBandHigh
;
618 currentThrottleInputRange
= rcCommandThrottleRange3dHigh
;
619 } else if ((rcThrottlePrevious
<= rcCommand3dDeadBandLow
&&
620 !flight3DConfigMutable()->switched_mode3d
) ||
621 isMotorsReversed()) {
622 // INVERTED_TO_DEADBAND
623 motorRangeMin
= motorOutputLow
;
624 motorRangeMax
= deadbandMotor3dLow
;
625 if (isMotorProtocolDshot()) {
626 motorOutputMin
= motorOutputLow
;
627 motorOutputRange
= deadbandMotor3dLow
- motorOutputLow
;
629 motorOutputMin
= deadbandMotor3dLow
;
630 motorOutputRange
= motorOutputLow
- deadbandMotor3dLow
;
632 if (motorOutputMixSign
!= -1) {
633 reversalTimeUs
= currentTimeUs
;
635 motorOutputMixSign
= -1;
637 currentThrottleInputRange
= rcCommandThrottleRange3dLow
;
639 // NORMAL_TO_DEADBAND
640 motorRangeMin
= deadbandMotor3dHigh
;
641 motorRangeMax
= motorOutputHigh
;
642 motorOutputMin
= deadbandMotor3dHigh
;
643 motorOutputRange
= motorOutputHigh
- deadbandMotor3dHigh
;
644 if (motorOutputMixSign
!= 1) {
645 reversalTimeUs
= currentTimeUs
;
647 motorOutputMixSign
= 1;
649 currentThrottleInputRange
= rcCommandThrottleRange3dHigh
;
651 if (currentTimeUs
- reversalTimeUs
< 250000) {
652 // keep iterm zero for 250ms after motor reversal
656 throttle
= rcCommand
[THROTTLE
] - PWM_RANGE_MIN
+ throttleAngleCorrection
;
657 currentThrottleInputRange
= rcCommandThrottleRange
;
658 motorRangeMin
= motorOutputLow
;
659 motorRangeMax
= motorOutputHigh
;
660 motorOutputMin
= motorOutputLow
;
661 motorOutputRange
= motorOutputHigh
- motorOutputLow
;
662 motorOutputMixSign
= 1;
665 throttle
= constrainf(throttle
/ currentThrottleInputRange
, 0.0f
, 1.0f
);
668 #define CRASH_FLIP_DEADBAND 20
669 #define CRASH_FLIP_STICK_MINF 0.15f
671 static void applyFlipOverAfterCrashModeToMotors(void)
673 if (ARMING_FLAG(ARMED
)) {
674 float stickDeflectionPitchAbs
= getRcDeflectionAbs(FD_PITCH
);
675 float stickDeflectionRollAbs
= getRcDeflectionAbs(FD_ROLL
);
676 float stickDeflectionYawAbs
= getRcDeflectionAbs(FD_YAW
);
677 float signPitch
= getRcDeflection(FD_PITCH
) < 0 ? 1 : -1;
678 float signRoll
= getRcDeflection(FD_ROLL
) < 0 ? 1 : -1;
679 float signYaw
= (getRcDeflection(FD_YAW
) < 0 ? 1 : -1) * (mixerConfig()->yaw_motors_reversed
? 1 : -1);
681 float stickDeflectionLength
= sqrtf(stickDeflectionPitchAbs
*stickDeflectionPitchAbs
+ stickDeflectionRollAbs
*stickDeflectionRollAbs
);
683 if (stickDeflectionYawAbs
> MAX(stickDeflectionPitchAbs
, stickDeflectionRollAbs
)) {
684 // If yaw is the dominant, disable pitch and roll
685 stickDeflectionLength
= stickDeflectionYawAbs
;
689 // If pitch/roll dominant, disable yaw
693 float cosPhi
= (stickDeflectionPitchAbs
+ stickDeflectionRollAbs
) / (sqrtf(2.0f
) * stickDeflectionLength
);
694 const float cosThreshold
= sqrtf(3.0f
)/2.0f
; // cos(PI/6.0f)
696 if (cosPhi
< cosThreshold
) {
697 // Enforce either roll or pitch exclusively, if not on diagonal
698 if (stickDeflectionRollAbs
> stickDeflectionPitchAbs
) {
705 // Apply a reasonable amount of stick deadband
706 const float flipStickRange
= 1.0f
- CRASH_FLIP_STICK_MINF
;
707 float flipPower
= MAX(0.0f
, stickDeflectionLength
- CRASH_FLIP_STICK_MINF
) / flipStickRange
;
709 for (int i
= 0; i
< motorCount
; ++i
) {
710 float motorOutputNormalised
=
711 signPitch
*currentMixer
[i
].pitch
+
712 signRoll
*currentMixer
[i
].roll
+
713 signYaw
*currentMixer
[i
].yaw
;
715 if (motorOutputNormalised
< 0) {
716 if (mixerConfig()->crashflip_motor_percent
> 0) {
717 motorOutputNormalised
= -motorOutputNormalised
* (float)mixerConfig()->crashflip_motor_percent
/ 100.0f
;
719 motorOutputNormalised
= 0;
722 motorOutputNormalised
= MIN(1.0f
, flipPower
* motorOutputNormalised
);
723 float motorOutput
= motorOutputMin
+ motorOutputNormalised
* motorOutputRange
;
725 // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
726 motorOutput
= (motorOutput
< motorOutputMin
+ CRASH_FLIP_DEADBAND
) ? disarmMotorOutput
: (motorOutput
- CRASH_FLIP_DEADBAND
);
728 motor
[i
] = motorOutput
;
732 for (int i
= 0; i
< motorCount
; i
++) {
733 motor
[i
] = motor_disarmed
[i
];
738 static void applyMixToMotors(float motorMix
[MAX_SUPPORTED_MOTORS
], motorMixer_t
*activeMixer
)
740 // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
741 // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
742 for (int i
= 0; i
< motorCount
; i
++) {
743 float motorOutput
= motorOutputMixSign
* motorMix
[i
] + throttle
* activeMixer
[i
].throttle
;
744 #ifdef USE_THRUST_LINEARIZATION
745 motorOutput
= pidApplyThrustLinearization(motorOutput
);
747 motorOutput
= motorOutputMin
+ motorOutputRange
* motorOutput
;
750 if (mixerIsTricopter()) {
751 motorOutput
+= mixerTricopterMotorCorrection(i
);
754 if (failsafeIsActive()) {
755 if (isMotorProtocolDshot()) {
756 motorOutput
= (motorOutput
< motorRangeMin
) ? disarmMotorOutput
: motorOutput
; // Prevent getting into special reserved range
758 motorOutput
= constrain(motorOutput
, disarmMotorOutput
, motorRangeMax
);
760 motorOutput
= constrain(motorOutput
, motorRangeMin
, motorRangeMax
);
762 motor
[i
] = motorOutput
;
766 if (!ARMING_FLAG(ARMED
)) {
767 for (int i
= 0; i
< motorCount
; i
++) {
768 motor
[i
] = motor_disarmed
[i
];
773 float applyThrottleLimit(float throttle
)
775 if (currentControlRateProfile
->throttle_limit_percent
< 100) {
776 const float throttleLimitFactor
= currentControlRateProfile
->throttle_limit_percent
/ 100.0f
;
777 switch (currentControlRateProfile
->throttle_limit_type
) {
778 case THROTTLE_LIMIT_TYPE_SCALE
:
779 return throttle
* throttleLimitFactor
;
780 case THROTTLE_LIMIT_TYPE_CLIP
:
781 return MIN(throttle
, throttleLimitFactor
);
788 void applyMotorStop(void)
790 for (int i
= 0; i
< motorCount
; i
++) {
791 motor
[i
] = disarmMotorOutput
;
796 void updateDynLpfCutoffs(timeUs_t currentTimeUs
, float throttle
)
798 static timeUs_t lastDynLpfUpdateUs
= 0;
799 static int dynLpfPreviousQuantizedThrottle
= -1; // to allow an initial zero throttle to set the filter cutoff
801 if (cmpTimeUs(currentTimeUs
, lastDynLpfUpdateUs
) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US
) {
802 const int quantizedThrottle
= lrintf(throttle
* DYN_LPF_THROTTLE_STEPS
); // quantize the throttle reduce the number of filter updates
803 if (quantizedThrottle
!= dynLpfPreviousQuantizedThrottle
) {
804 // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable
805 const float dynLpfThrottle
= (float)quantizedThrottle
/ DYN_LPF_THROTTLE_STEPS
;
806 dynLpfGyroUpdate(dynLpfThrottle
);
807 dynLpfDTermUpdate(dynLpfThrottle
);
808 dynLpfPreviousQuantizedThrottle
= quantizedThrottle
;
809 lastDynLpfUpdateUs
= currentTimeUs
;
815 FAST_CODE_NOINLINE
void mixTable(timeUs_t currentTimeUs
, uint8_t vbatPidCompensation
)
817 // Find min and max throttle based on conditions. Throttle has to be known before mixing
818 calculateThrottleAndCurrentMotorEndpoints(currentTimeUs
);
820 if (isFlipOverAfterCrashActive()) {
821 applyFlipOverAfterCrashModeToMotors();
826 const bool launchControlActive
= isLaunchControlActive();
828 motorMixer_t
* activeMixer
= ¤tMixer
[0];
829 #ifdef USE_LAUNCH_CONTROL
830 if (launchControlActive
&& (currentPidProfile
->launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
)) {
831 activeMixer
= &launchControlMixer
[0];
835 // Calculate and Limit the PID sum
836 const float scaledAxisPidRoll
=
837 constrainf(pidData
[FD_ROLL
].Sum
, -currentPidProfile
->pidSumLimit
, currentPidProfile
->pidSumLimit
) / PID_MIXER_SCALING
;
838 const float scaledAxisPidPitch
=
839 constrainf(pidData
[FD_PITCH
].Sum
, -currentPidProfile
->pidSumLimit
, currentPidProfile
->pidSumLimit
) / PID_MIXER_SCALING
;
841 uint16_t yawPidSumLimit
= currentPidProfile
->pidSumLimitYaw
;
843 #ifdef USE_YAW_SPIN_RECOVERY
844 const bool yawSpinDetected
= gyroYawSpinDetected();
845 if (yawSpinDetected
) {
846 yawPidSumLimit
= PIDSUM_LIMIT_MAX
; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
848 #endif // USE_YAW_SPIN_RECOVERY
850 float scaledAxisPidYaw
=
851 constrainf(pidData
[FD_YAW
].Sum
, -yawPidSumLimit
, yawPidSumLimit
) / PID_MIXER_SCALING
;
853 if (!mixerConfig()->yaw_motors_reversed
) {
854 scaledAxisPidYaw
= -scaledAxisPidYaw
;
857 // Calculate voltage compensation
858 const float vbatCompensationFactor
= vbatPidCompensation
? calculateVbatPidCompensation() : 1.0f
;
860 // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type
861 if (currentControlRateProfile
->throttle_limit_type
!= THROTTLE_LIMIT_TYPE_OFF
) {
862 throttle
= applyThrottleLimit(throttle
);
865 const bool airmodeEnabled
= airmodeIsEnabled() || launchControlActive
;
867 #ifdef USE_YAW_SPIN_RECOVERY
868 // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
869 // When airmode is active the throttle setting doesn't impact recovery authority.
870 if (yawSpinDetected
&& !airmodeEnabled
) {
873 #endif // USE_YAW_SPIN_RECOVERY
875 #ifdef USE_LAUNCH_CONTROL
876 // While launch control is active keep the throttle at minimum.
877 // Once the pilot triggers the launch throttle control will be reactivated.
878 if (launchControlActive
) {
883 // Find roll/pitch/yaw desired output
884 float motorMix
[MAX_SUPPORTED_MOTORS
];
885 float motorMixMax
= 0, motorMixMin
= 0;
886 for (int i
= 0; i
< motorCount
; i
++) {
889 scaledAxisPidRoll
* activeMixer
[i
].roll
+
890 scaledAxisPidPitch
* activeMixer
[i
].pitch
+
891 scaledAxisPidYaw
* activeMixer
[i
].yaw
;
893 mix
*= vbatCompensationFactor
; // Add voltage compensation
895 if (mix
> motorMixMax
) {
897 } else if (mix
< motorMixMin
) {
903 pidUpdateAntiGravityThrottleFilter(throttle
);
906 updateDynLpfCutoffs(currentTimeUs
, throttle
);
909 #ifdef USE_THRUST_LINEARIZATION
910 // reestablish old throttle stick feel by counter compensating thrust linearization
911 throttle
= pidCompensateThrustLinearization(throttle
);
914 #if defined(USE_THROTTLE_BOOST)
915 if (throttleBoost
> 0.0f
) {
916 const float throttleHpf
= throttle
- pt1FilterApply(&throttleLpf
, throttle
);
917 throttle
= constrainf(throttle
+ throttleBoost
* throttleHpf
, 0.0f
, 1.0f
);
921 #ifdef USE_GPS_RESCUE
922 // If gps rescue is active then override the throttle. This prevents things
923 // like throttle boost or throttle limit from negatively affecting the throttle.
924 if (FLIGHT_MODE(GPS_RESCUE_MODE
)) {
925 throttle
= gpsRescueGetThrottle();
929 loggingThrottle
= throttle
;
930 motorMixRange
= motorMixMax
- motorMixMin
;
931 if (motorMixRange
> 1.0f
) {
932 for (int i
= 0; i
< motorCount
; i
++) {
933 motorMix
[i
] /= motorMixRange
;
935 // Get the maximum correction by setting offset to center when airmode enabled
936 if (airmodeEnabled
) {
940 if (airmodeEnabled
|| throttle
> 0.5f
) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
941 throttle
= constrainf(throttle
, -motorMixMin
, 1.0f
- motorMixMax
);
945 if (featureIsEnabled(FEATURE_MOTOR_STOP
)
946 && ARMING_FLAG(ARMED
)
947 && !featureIsEnabled(FEATURE_3D
)
949 && !FLIGHT_MODE(GPS_RESCUE_MODE
) // disable motor_stop while GPS Rescue is active
950 && (rcData
[THROTTLE
] < rxConfig()->mincheck
)) {
951 // motor_stop handling
954 // Apply the mix to motor endpoints
955 applyMixToMotors(motorMix
, activeMixer
);
959 float convertExternalToMotor(uint16_t externalValue
)
962 switch ((int)isMotorProtocolDshot()) {
965 externalValue
= constrain(externalValue
, PWM_RANGE_MIN
, PWM_RANGE_MAX
);
967 if (featureIsEnabled(FEATURE_3D
)) {
968 if (externalValue
== PWM_RANGE_MID
) {
969 motorValue
= DSHOT_CMD_MOTOR_STOP
;
970 } else if (externalValue
< PWM_RANGE_MID
) {
971 motorValue
= scaleRange(externalValue
, PWM_RANGE_MIN
, PWM_RANGE_MID
- 1, DSHOT_3D_FORWARD_MIN_THROTTLE
- 1, DSHOT_MIN_THROTTLE
);
973 motorValue
= scaleRange(externalValue
, PWM_RANGE_MID
+ 1, PWM_RANGE_MAX
, DSHOT_3D_FORWARD_MIN_THROTTLE
, DSHOT_MAX_THROTTLE
);
976 motorValue
= (externalValue
== PWM_RANGE_MIN
) ? DSHOT_CMD_MOTOR_STOP
: scaleRange(externalValue
, PWM_RANGE_MIN
+ 1, PWM_RANGE_MAX
, DSHOT_MIN_THROTTLE
, DSHOT_MAX_THROTTLE
);
983 motorValue
= externalValue
;
987 return (float)motorValue
;
990 uint16_t convertMotorToExternal(float motorValue
)
992 uint16_t externalValue
;
993 switch ((int)isMotorProtocolDshot()) {
996 if (featureIsEnabled(FEATURE_3D
)) {
997 if (motorValue
== DSHOT_CMD_MOTOR_STOP
|| motorValue
< DSHOT_MIN_THROTTLE
) {
998 externalValue
= PWM_RANGE_MID
;
999 } else if (motorValue
<= DSHOT_3D_FORWARD_MIN_THROTTLE
- 1) {
1000 externalValue
= scaleRange(motorValue
, DSHOT_MIN_THROTTLE
, DSHOT_3D_FORWARD_MIN_THROTTLE
- 1, PWM_RANGE_MID
- 1, PWM_RANGE_MIN
);
1002 externalValue
= scaleRange(motorValue
, DSHOT_3D_FORWARD_MIN_THROTTLE
, DSHOT_MAX_THROTTLE
, PWM_RANGE_MID
+ 1, PWM_RANGE_MAX
);
1005 externalValue
= (motorValue
< DSHOT_MIN_THROTTLE
) ? PWM_RANGE_MIN
: scaleRange(motorValue
, DSHOT_MIN_THROTTLE
, DSHOT_MAX_THROTTLE
, PWM_RANGE_MIN
+ 1, PWM_RANGE_MAX
);
1011 externalValue
= motorValue
;
1015 return externalValue
;
1018 void mixerSetThrottleAngleCorrection(int correctionValue
)
1020 throttleAngleCorrection
= correctionValue
;
1023 float mixerGetLoggingThrottle(void)
1025 return loggingThrottle
;