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 "blackbox/blackbox.h"
29 #include "blackbox/blackbox_fielddefs.h"
31 #include "build/debug.h"
37 #include "common/axis.h"
38 #include "common/filter.h"
39 #include "common/maths.h"
40 #include "common/utils.h"
42 #include "config/config.h"
43 #include "config/feature.h"
45 #include "drivers/dshot.h"
46 #include "drivers/dshot_command.h"
47 #include "drivers/light_led.h"
48 #include "drivers/motor.h"
49 #include "drivers/sound_beeper.h"
50 #include "drivers/system.h"
51 #include "drivers/time.h"
52 #include "drivers/transponder_ir.h"
54 #include "fc/controlrate_profile.h"
56 #include "fc/rc_adjustments.h"
57 #include "fc/rc_controls.h"
58 #include "fc/runtime_config.h"
61 #include "flight/failsafe.h"
62 #include "flight/gps_rescue.h"
64 #if defined(USE_GYRO_DATA_ANALYSE)
65 #include "flight/gyroanalyse.h"
68 #include "flight/imu.h"
69 #include "flight/mixer.h"
70 #include "flight/pid.h"
71 #include "flight/position.h"
72 #include "flight/rpm_filter.h"
73 #include "flight/servos.h"
75 #include "io/beeper.h"
77 #include "io/motors.h"
78 #include "io/pidaudio.h"
79 #include "io/serial.h"
80 #include "io/servos.h"
81 #include "io/statusindicator.h"
82 #include "io/transponder_ir.h"
83 #include "io/vtx_control.h"
84 #include "io/vtx_rtc6705.h"
86 #include "msp/msp_serial.h"
92 #include "pg/pg_ids.h"
97 #include "scheduler/scheduler.h"
99 #include "sensors/acceleration.h"
100 #include "sensors/barometer.h"
101 #include "sensors/battery.h"
102 #include "sensors/boardalignment.h"
103 #include "sensors/compass.h"
104 #include "sensors/gyro.h"
106 #include "telemetry/telemetry.h"
118 ARMING_DELAYED_DISARMED
= 0,
119 ARMING_DELAYED_NORMAL
= 1,
120 ARMING_DELAYED_CRASHFLIP
= 2,
121 ARMING_DELAYED_LAUNCH_CONTROL
= 3,
124 #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
126 #ifdef USE_RUNAWAY_TAKEOFF
127 #define RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD 600 // The pidSum threshold required to trigger - corresponds to a pidSum value of 60% (raw 600) in the blackbox viewer
128 #define RUNAWAY_TAKEOFF_ACTIVATE_DELAY 75000 // (75ms) Time in microseconds where pidSum is above threshold to trigger
129 #define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15 // 15% - minimum stick deflection during deactivation phase
130 #define RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT 100 // 10.0% - pidSum limit during deactivation phase
131 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_RP 15 // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props
132 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW 50 // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props
133 #define RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT 75 // High throttle limit to accelerate deactivation (halves the deactivation delay)
135 #define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE 0
136 #define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY 1
137 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY 2
138 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME 3
140 #define DEBUG_RUNAWAY_TAKEOFF_TRUE 1
141 #define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
144 #if defined(USE_GPS) || defined(USE_MAG)
148 static FAST_RAM_ZERO_INIT
uint8_t pidUpdateCounter
;
150 static bool flipOverAfterCrashActive
= false;
152 static timeUs_t disarmAt
; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
154 static int lastArmingDisabledReason
= 0;
155 static timeUs_t lastDisarmTimeUs
;
156 static int tryingToArm
= ARMING_DELAYED_DISARMED
;
158 #ifdef USE_RUNAWAY_TAKEOFF
159 static timeUs_t runawayTakeoffDeactivateUs
= 0;
160 static timeUs_t runawayTakeoffAccumulatedUs
= 0;
161 static bool runawayTakeoffCheckDisabled
= false;
162 static timeUs_t runawayTakeoffTriggerUs
= 0;
163 static bool runawayTakeoffTemporarilyDisabled
= false;
166 #ifdef USE_LAUNCH_CONTROL
167 static launchControlState_e launchControlState
= LAUNCH_CONTROL_DISABLED
;
169 const char * const osdLaunchControlModeNames
[] = {
176 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t
, throttleCorrectionConfig
, PG_THROTTLE_CORRECTION_CONFIG
, 0);
178 PG_RESET_TEMPLATE(throttleCorrectionConfig_t
, throttleCorrectionConfig
,
179 .throttle_correction_value
= 0, // could 10 with althold or 40 for fpv
180 .throttle_correction_angle
= 800 // could be 80.0 deg with atlhold or 45.0 for fpv
183 static bool isCalibrating(void)
185 return (sensors(SENSOR_GYRO
) && !gyroIsCalibrationComplete())
187 || (sensors(SENSOR_ACC
) && !accIsCalibrationComplete())
190 || (sensors(SENSOR_BARO
) && !baroIsCalibrationComplete())
193 || (sensors(SENSOR_MAG
) && !compassIsCalibrationComplete())
198 #ifdef USE_LAUNCH_CONTROL
199 bool canUseLaunchControl(void)
202 && !isUsingSticksForArming() // require switch arming for safety
203 && IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL
)
204 && (!featureIsEnabled(FEATURE_MOTOR_STOP
) || airmodeIsEnabled()) // can't use when motors are stopped
205 && !featureIsEnabled(FEATURE_3D
) // pitch control is not 3D aware
206 && (flightModeFlags
== 0)) { // don't want to use unless in acro mode
213 void resetArmingDisabled(void)
215 lastArmingDisabledReason
= 0;
219 static bool accNeedsCalibration(void)
221 if (sensors(SENSOR_ACC
)) {
223 // Check to see if the ACC has already been calibrated
224 if (accHasBeenCalibrated()) {
228 // We've determined that there's a detected ACC that has not
229 // yet been calibrated. Check to see if anything is using the
230 // ACC that would be affected by the lack of calibration.
232 // Check for any configured modes that use the ACC
233 if (isModeActivationConditionPresent(BOXANGLE
) ||
234 isModeActivationConditionPresent(BOXHORIZON
) ||
235 isModeActivationConditionPresent(BOXGPSRESCUE
) ||
236 isModeActivationConditionPresent(BOXCAMSTAB
) ||
237 isModeActivationConditionPresent(BOXCALIB
) ||
238 isModeActivationConditionPresent(BOXACROTRAINER
)) {
243 // Launch Control only requires the ACC if a angle limit is set
244 if (isModeActivationConditionPresent(BOXLAUNCHCONTROL
) && currentPidProfile
->launchControlAngleLimit
) {
249 // Check for any enabled OSD elements that need the ACC
250 if (featureIsEnabled(FEATURE_OSD
)) {
251 if (osdNeedsAccelerometer()) {
257 #ifdef USE_GPS_RESCUE
258 // Check if failsafe will use GPS Rescue
259 if (failsafeConfig()->failsafe_procedure
== FAILSAFE_PROCEDURE_GPS_RESCUE
) {
269 void updateArmingStatus(void)
271 if (ARMING_FLAG(ARMED
)) {
274 // Check if the power on arming grace time has elapsed
275 if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME
) && (millis() >= systemConfig()->powerOnArmingGraceTime
* 1000)
277 // We also need to prevent arming until it's possible to send DSHOT commands.
278 // Otherwise if the initial arming is in crash-flip the motor direction commands
279 // might not be sent.
280 && (!isMotorProtocolDshot() || dshotCommandsAreEnabled(DSHOT_CMD_TYPE_INLINE
))
283 // If so, unset the grace time arming disable flag
284 unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME
);
287 // Clear the crash flip active status
288 flipOverAfterCrashActive
= false;
290 // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
291 if (!isUsingSticksForArming()) {
292 static bool hadRx
= false;
293 const bool haveRx
= rxIsReceivingSignal();
295 const bool justGotRxBack
= !hadRx
&& haveRx
;
297 if (justGotRxBack
&& IS_RC_MODE_ACTIVE(BOXARM
)) {
298 // If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction
299 setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY
);
300 } else if (haveRx
&& !IS_RC_MODE_ACTIVE(BOXARM
)) {
301 // If RX signal is OK and the arm switch is off, remove arming restriction
302 unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY
);
308 if (IS_RC_MODE_ACTIVE(BOXFAILSAFE
)) {
309 setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE
);
311 unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE
);
314 if (calculateThrottleStatus() != THROTTLE_LOW
) {
315 setArmingDisabled(ARMING_DISABLED_THROTTLE
);
317 unsetArmingDisabled(ARMING_DISABLED_THROTTLE
);
320 if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH
)) {
321 setArmingDisabled(ARMING_DISABLED_ANGLE
);
323 unsetArmingDisabled(ARMING_DISABLED_ANGLE
);
326 if (getAverageSystemLoadPercent() > LOAD_PERCENTAGE_ONE
) {
327 setArmingDisabled(ARMING_DISABLED_LOAD
);
329 unsetArmingDisabled(ARMING_DISABLED_LOAD
);
332 if (isCalibrating()) {
333 setArmingDisabled(ARMING_DISABLED_CALIBRATING
);
335 unsetArmingDisabled(ARMING_DISABLED_CALIBRATING
);
338 if (isModeActivationConditionPresent(BOXPREARM
)) {
339 if (IS_RC_MODE_ACTIVE(BOXPREARM
) && !ARMING_FLAG(WAS_ARMED_WITH_PREARM
)) {
340 unsetArmingDisabled(ARMING_DISABLED_NOPREARM
);
342 setArmingDisabled(ARMING_DISABLED_NOPREARM
);
346 #ifdef USE_GPS_RESCUE
347 if (gpsRescueIsConfigured()) {
348 if (gpsRescueConfig()->allowArmingWithoutFix
|| STATE(GPS_FIX
) || ARMING_FLAG(WAS_EVER_ARMED
) || IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH
)) {
349 unsetArmingDisabled(ARMING_DISABLED_GPS
);
351 setArmingDisabled(ARMING_DISABLED_GPS
);
353 if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE
)) {
354 setArmingDisabled(ARMING_DISABLED_RESC
);
356 unsetArmingDisabled(ARMING_DISABLED_RESC
);
361 #ifdef USE_RPM_FILTER
362 // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined
363 // If the RPM filter is anabled and any motor isn't providing telemetry, then disable arming
364 if (isRpmFilterEnabled() && !isDshotTelemetryActive()) {
365 setArmingDisabled(ARMING_DISABLED_RPMFILTER
);
367 unsetArmingDisabled(ARMING_DISABLED_RPMFILTER
);
371 #ifdef USE_DSHOT_BITBANG
372 if (isDshotBitbangActive(&motorConfig()->dev
) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK
) {
373 setArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG
);
375 unsetArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG
);
379 if (IS_RC_MODE_ACTIVE(BOXPARALYZE
)) {
380 setArmingDisabled(ARMING_DISABLED_PARALYZE
);
384 if (accNeedsCalibration()) {
385 setArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION
);
387 unsetArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION
);
391 if (!isMotorProtocolEnabled()) {
392 setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL
);
395 if (!isUsingSticksForArming()) {
396 if (!IS_RC_MODE_ACTIVE(BOXARM
)) {
397 #ifdef USE_RUNAWAY_TAKEOFF
398 unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF
);
400 unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED
);
403 /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */
404 bool ignoreGyro
= armingConfig()->gyro_cal_on_first_arm
405 && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH
| ARMING_DISABLED_CALIBRATING
));
407 /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */
408 bool ignoreThrottle
= featureIsEnabled(FEATURE_3D
)
409 && !IS_RC_MODE_ACTIVE(BOX3D
)
410 && !flight3DConfig()->switched_mode3d
411 && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH
| ARMING_DISABLED_THROTTLE
));
413 // If arming is disabled and the ARM switch is on
414 if (isArmingDisabled()
417 && IS_RC_MODE_ACTIVE(BOXARM
)) {
418 setArmingDisabled(ARMING_DISABLED_ARM_SWITCH
);
419 } else if (!IS_RC_MODE_ACTIVE(BOXARM
)) {
420 unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH
);
424 if (isArmingDisabled()) {
434 void disarm(flightLogDisarmReason_e reason
)
436 if (ARMING_FLAG(ARMED
)) {
437 if (!flipOverAfterCrashActive
) {
438 ENABLE_ARMING_FLAG(WAS_EVER_ARMED
);
440 DISABLE_ARMING_FLAG(ARMED
);
441 lastDisarmTimeUs
= micros();
444 if (flipOverAfterCrashActive
|| isLaunchControlActive()) {
445 osdSuppressStats(true);
450 flightLogEvent_disarm_t eventData
;
451 eventData
.reason
= reason
;
452 blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM
, (flightLogEventData_t
*)&eventData
);
454 if (blackboxConfig()->device
&& blackboxConfig()->mode
!= BLACKBOX_MODE_ALWAYS_ON
) { // Close the log upon disarm except when logging mode is ALWAYS ON
462 if (isMotorProtocolDshot() && flipOverAfterCrashActive
&& !featureIsEnabled(FEATURE_3D
)) {
463 dshotCommandWrite(ALL_MOTORS
, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL
, DSHOT_CMD_TYPE_INLINE
);
466 flipOverAfterCrashActive
= false;
468 #ifdef USE_PERSISTENT_STATS
472 // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead
473 if (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF
| ARMING_DISABLED_CRASH_DETECTED
))) {
474 beeper(BEEPER_DISARMING
); // emit disarm tone
481 if (armingConfig()->gyro_cal_on_first_arm
) {
482 gyroStartCalibration(true);
485 updateArmingStatus();
487 if (!isArmingDisabled()) {
488 if (ARMING_FLAG(ARMED
)) {
492 const timeUs_t currentTimeUs
= micros();
495 if (currentTimeUs
- getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US
) {
496 if (tryingToArm
== ARMING_DELAYED_DISARMED
) {
497 if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH
)) {
498 tryingToArm
= ARMING_DELAYED_CRASHFLIP
;
499 #ifdef USE_LAUNCH_CONTROL
500 } else if (canUseLaunchControl()) {
501 tryingToArm
= ARMING_DELAYED_LAUNCH_CONTROL
;
504 tryingToArm
= ARMING_DELAYED_NORMAL
;
510 if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH
)) {
511 if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH
) || (tryingToArm
== ARMING_DELAYED_CRASHFLIP
))) {
512 flipOverAfterCrashActive
= false;
513 if (!featureIsEnabled(FEATURE_3D
)) {
514 dshotCommandWrite(ALL_MOTORS
, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL
, DSHOT_CMD_TYPE_INLINE
);
517 flipOverAfterCrashActive
= true;
518 #ifdef USE_RUNAWAY_TAKEOFF
519 runawayTakeoffCheckDisabled
= false;
521 if (!featureIsEnabled(FEATURE_3D
)) {
522 dshotCommandWrite(ALL_MOTORS
, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED
, DSHOT_CMD_TYPE_INLINE
);
528 #ifdef USE_LAUNCH_CONTROL
529 if (!flipOverAfterCrashActive
&& (canUseLaunchControl() || (tryingToArm
== ARMING_DELAYED_LAUNCH_CONTROL
))) {
530 if (launchControlState
== LAUNCH_CONTROL_DISABLED
) { // only activate if it hasn't already been triggered
531 launchControlState
= LAUNCH_CONTROL_ACTIVE
;
537 osdSuppressStats(false);
539 ENABLE_ARMING_FLAG(ARMED
);
543 #ifdef USE_ACRO_TRAINER
544 pidAcroTrainerInit();
545 #endif // USE_ACRO_TRAINER
547 if (isModeActivationConditionPresent(BOXPREARM
)) {
548 ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM
);
550 imuQuaternionHeadfreeOffsetSet();
552 #if defined(USE_GYRO_DATA_ANALYSE)
556 disarmAt
= currentTimeUs
+ armingConfig()->auto_disarm_delay
* 1e6
; // start disarm timeout, will be extended when throttle is nonzero
558 lastArmingDisabledReason
= 0;
561 GPS_reset_home_position();
563 //beep to indicate arming
564 if (featureIsEnabled(FEATURE_GPS
)) {
565 if (STATE(GPS_FIX
) && gpsSol
.numSat
>= 5) {
566 beeper(BEEPER_ARMING_GPS_FIX
);
568 beeper(BEEPER_ARMING_GPS_NO_FIX
);
571 beeper(BEEPER_ARMING
);
574 beeper(BEEPER_ARMING
);
577 #ifdef USE_PERSISTENT_STATS
581 #ifdef USE_RUNAWAY_TAKEOFF
582 runawayTakeoffDeactivateUs
= 0;
583 runawayTakeoffAccumulatedUs
= 0;
584 runawayTakeoffTriggerUs
= 0;
588 if (!isFirstArmingGyroCalibrationRunning()) {
589 int armingDisabledReason
= ffs(getArmingDisableFlags());
590 if (lastArmingDisabledReason
!= armingDisabledReason
) {
591 lastArmingDisabledReason
= armingDisabledReason
;
593 beeperWarningBeeps(armingDisabledReason
);
599 // Automatic ACC Offset Calibration
600 bool AccInflightCalibrationArmed
= false;
601 bool AccInflightCalibrationMeasurementDone
= false;
602 bool AccInflightCalibrationSavetoEEProm
= false;
603 bool AccInflightCalibrationActive
= false;
604 uint16_t InflightcalibratingA
= 0;
606 void handleInflightCalibrationStickPosition(void)
608 if (AccInflightCalibrationMeasurementDone
) {
609 // trigger saving into eeprom after landing
610 AccInflightCalibrationMeasurementDone
= false;
611 AccInflightCalibrationSavetoEEProm
= true;
613 AccInflightCalibrationArmed
= !AccInflightCalibrationArmed
;
614 if (AccInflightCalibrationArmed
) {
615 beeper(BEEPER_ACC_CALIBRATION
);
617 beeper(BEEPER_ACC_CALIBRATION_FAIL
);
622 static void updateInflightCalibrationState(void)
624 if (AccInflightCalibrationArmed
&& ARMING_FLAG(ARMED
) && rcData
[THROTTLE
] > rxConfig()->mincheck
&& !IS_RC_MODE_ACTIVE(BOXARM
)) { // Copter is airborne and you are turning it off via boxarm : start measurement
625 InflightcalibratingA
= 50;
626 AccInflightCalibrationArmed
= false;
628 if (IS_RC_MODE_ACTIVE(BOXCALIB
)) { // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
629 if (!AccInflightCalibrationActive
&& !AccInflightCalibrationMeasurementDone
)
630 InflightcalibratingA
= 50;
631 AccInflightCalibrationActive
= true;
632 } else if (AccInflightCalibrationMeasurementDone
&& !ARMING_FLAG(ARMED
)) {
633 AccInflightCalibrationMeasurementDone
= false;
634 AccInflightCalibrationSavetoEEProm
= true;
638 #if defined(USE_GPS) || defined(USE_MAG)
639 static void updateMagHold(void)
641 if (fabsf(rcCommand
[YAW
]) < 15 && FLIGHT_MODE(MAG_MODE
)) {
642 int16_t dif
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
) - magHold
;
647 dif
*= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed
);
649 rcCommand
[YAW
] -= dif
* currentPidProfile
->pid
[PID_MAG
].P
/ 30; // 18 deg
652 magHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
);
656 #ifdef USE_VTX_CONTROL
657 static bool canUpdateVTX(void)
659 #ifdef USE_VTX_RTC6705
660 return vtxRTC6705CanUpdate();
666 #if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE)
667 // determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
668 bool areSticksActive(uint8_t stickPercentLimit
)
670 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
671 const uint8_t deadband
= axis
== FD_YAW
? rcControlsConfig()->yaw_deadband
: rcControlsConfig()->deadband
;
672 uint8_t stickPercent
= 0;
673 if ((rcData
[axis
] >= PWM_RANGE_MAX
) || (rcData
[axis
] <= PWM_RANGE_MIN
)) {
676 if (rcData
[axis
] > (rxConfig()->midrc
+ deadband
)) {
677 stickPercent
= ((rcData
[axis
] - rxConfig()->midrc
- deadband
) * 100) / (PWM_RANGE_MAX
- rxConfig()->midrc
- deadband
);
678 } else if (rcData
[axis
] < (rxConfig()->midrc
- deadband
)) {
679 stickPercent
= ((rxConfig()->midrc
- deadband
- rcData
[axis
]) * 100) / (rxConfig()->midrc
- deadband
- PWM_RANGE_MIN
);
682 if (stickPercent
>= stickPercentLimit
) {
690 #ifdef USE_RUNAWAY_TAKEOFF
691 // allow temporarily disabling runaway takeoff prevention if we are connected
692 // to the configurator and the ARMING_DISABLED_MSP flag is cleared.
693 void runawayTakeoffTemporaryDisable(uint8_t disableFlag
)
695 runawayTakeoffTemporarilyDisabled
= disableFlag
;
700 // calculate the throttle stick percent - integer math is good enough here.
701 // returns negative values for reversed thrust in 3D mode
702 int8_t calculateThrottlePercent(void)
705 int channelData
= constrain(rcData
[THROTTLE
], PWM_RANGE_MIN
, PWM_RANGE_MAX
);
707 if (featureIsEnabled(FEATURE_3D
)
708 && !IS_RC_MODE_ACTIVE(BOX3D
)
709 && !flight3DConfig()->switched_mode3d
) {
711 if (channelData
> (rxConfig()->midrc
+ flight3DConfig()->deadband3d_throttle
)) {
712 ret
= ((channelData
- rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
) * 100) / (PWM_RANGE_MAX
- rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
);
713 } else if (channelData
< (rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
)) {
714 ret
= -((rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
- channelData
) * 100) / (rxConfig()->midrc
- flight3DConfig()->deadband3d_throttle
- PWM_RANGE_MIN
);
717 ret
= constrain(((channelData
- rxConfig()->mincheck
) * 100) / (PWM_RANGE_MAX
- rxConfig()->mincheck
), 0, 100);
718 if (featureIsEnabled(FEATURE_3D
)
719 && IS_RC_MODE_ACTIVE(BOX3D
)
720 && flight3DConfig()->switched_mode3d
) {
722 ret
= -ret
; // 3D on a switch is active
728 uint8_t calculateThrottlePercentAbs(void)
730 return ABS(calculateThrottlePercent());
733 static bool airmodeIsActivated
;
735 bool isAirmodeActivated()
737 return airmodeIsActivated
;
742 * processRx called from taskUpdateRxMain
744 bool processRx(timeUs_t currentTimeUs
)
746 static bool armedBeeperOn
= false;
748 static bool sharedPortTelemetryEnabled
= false;
751 timeDelta_t frameAgeUs
;
752 timeDelta_t frameDeltaUs
= rxGetFrameDelta(&frameAgeUs
);
754 DEBUG_SET(DEBUG_RX_TIMING
, 0, MIN(frameDeltaUs
/ 10, INT16_MAX
));
755 DEBUG_SET(DEBUG_RX_TIMING
, 1, MIN(frameAgeUs
/ 10, INT16_MAX
));
757 if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs
)) {
761 updateRcRefreshRate(currentTimeUs
);
763 // in 3D mode, we need to be able to disarm by switch at any time
764 if (featureIsEnabled(FEATURE_3D
)) {
765 if (!IS_RC_MODE_ACTIVE(BOXARM
))
766 disarm(DISARM_REASON_SWITCH
);
769 updateRSSI(currentTimeUs
);
771 if (currentTimeUs
> FAILSAFE_POWER_ON_DELAY_US
&& !failsafeIsMonitoring()) {
772 failsafeStartMonitoring();
774 failsafeUpdateState();
776 const throttleStatus_e throttleStatus
= calculateThrottleStatus();
777 const uint8_t throttlePercent
= calculateThrottlePercentAbs();
779 const bool launchControlActive
= isLaunchControlActive();
781 if (airmodeIsEnabled() && ARMING_FLAG(ARMED
) && !launchControlActive
) {
782 if (throttlePercent
>= rxConfig()->airModeActivateThreshold
) {
783 airmodeIsActivated
= true; // Prevent iterm from being reset
786 airmodeIsActivated
= false;
789 /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
790 This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
791 if (throttleStatus
== THROTTLE_LOW
&& !airmodeIsActivated
&& !launchControlActive
) {
792 pidSetItermReset(true);
793 if (currentPidProfile
->pidAtMinThrottle
)
794 pidStabilisationState(PID_STABILISATION_ON
);
796 pidStabilisationState(PID_STABILISATION_OFF
);
798 pidSetItermReset(false);
799 pidStabilisationState(PID_STABILISATION_ON
);
802 #ifdef USE_RUNAWAY_TAKEOFF
803 // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
804 // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
805 // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
806 // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
807 // prevention for the remainder of the battery.
809 if (ARMING_FLAG(ARMED
)
810 && pidConfig()->runaway_takeoff_prevention
811 && !runawayTakeoffCheckDisabled
812 && !flipOverAfterCrashActive
813 && !runawayTakeoffTemporarilyDisabled
816 // Determine if we're in "flight"
818 // - throttle over runaway_takeoff_deactivate_throttle_percent
819 // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
820 // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
821 bool inStableFlight
= false;
822 if (!featureIsEnabled(FEATURE_MOTOR_STOP
) || airmodeIsEnabled() || (throttleStatus
!= THROTTLE_LOW
)) { // are motors running?
823 const uint8_t lowThrottleLimit
= pidConfig()->runaway_takeoff_deactivate_throttle
;
824 const uint8_t midThrottleLimit
= constrain(lowThrottleLimit
* 2, lowThrottleLimit
* 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT
);
825 if ((((throttlePercent
>= lowThrottleLimit
) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT
)) || (throttlePercent
>= midThrottleLimit
))
826 && (fabsf(pidData
[FD_PITCH
].Sum
) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT
)
827 && (fabsf(pidData
[FD_ROLL
].Sum
) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT
)
828 && (fabsf(pidData
[FD_YAW
].Sum
) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT
)) {
830 inStableFlight
= true;
831 if (runawayTakeoffDeactivateUs
== 0) {
832 runawayTakeoffDeactivateUs
= currentTimeUs
;
837 // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
838 if (inStableFlight
) {
839 if (runawayTakeoffDeactivateUs
== 0) {
840 runawayTakeoffDeactivateUs
= currentTimeUs
;
842 uint16_t deactivateDelay
= pidConfig()->runaway_takeoff_deactivate_delay
;
843 // at high throttle levels reduce deactivation delay by 50%
844 if (throttlePercent
>= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT
) {
845 deactivateDelay
= deactivateDelay
/ 2;
847 if ((cmpTimeUs(currentTimeUs
, runawayTakeoffDeactivateUs
) + runawayTakeoffAccumulatedUs
) > deactivateDelay
* 1000) {
848 runawayTakeoffCheckDisabled
= true;
852 if (runawayTakeoffDeactivateUs
!= 0) {
853 runawayTakeoffAccumulatedUs
+= cmpTimeUs(currentTimeUs
, runawayTakeoffDeactivateUs
);
855 runawayTakeoffDeactivateUs
= 0;
857 if (runawayTakeoffDeactivateUs
== 0) {
858 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY
, DEBUG_RUNAWAY_TAKEOFF_FALSE
);
859 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME
, runawayTakeoffAccumulatedUs
/ 1000);
861 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY
, DEBUG_RUNAWAY_TAKEOFF_TRUE
);
862 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME
, (cmpTimeUs(currentTimeUs
, runawayTakeoffDeactivateUs
) + runawayTakeoffAccumulatedUs
) / 1000);
865 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY
, DEBUG_RUNAWAY_TAKEOFF_FALSE
);
866 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME
, DEBUG_RUNAWAY_TAKEOFF_FALSE
);
870 #ifdef USE_LAUNCH_CONTROL
871 if (ARMING_FLAG(ARMED
)) {
872 if (launchControlActive
&& (throttlePercent
> currentPidProfile
->launchControlThrottlePercent
)) {
873 // throttle limit trigger reached, launch triggered
874 // reset the iterms as they may be at high values from holding the launch position
875 launchControlState
= LAUNCH_CONTROL_TRIGGERED
;
879 if (launchControlState
== LAUNCH_CONTROL_TRIGGERED
) {
880 // If trigger mode is MULTIPLE then reset the state when disarmed
881 // and the mode switch is turned off.
882 // For trigger mode SINGLE we never reset the state and only a single
883 // launch is allowed until a reboot.
884 if (currentPidProfile
->launchControlAllowTriggerReset
&& !IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL
)) {
885 launchControlState
= LAUNCH_CONTROL_DISABLED
;
888 launchControlState
= LAUNCH_CONTROL_DISABLED
;
893 // When armed and motors aren't spinning, do beeps and then disarm
894 // board after delay so users without buzzer won't lose fingers.
895 // mixTable constrains motor commands, so checking throttleStatus is enough
896 const timeUs_t autoDisarmDelayUs
= armingConfig()->auto_disarm_delay
* 1e6
;
897 if (ARMING_FLAG(ARMED
)
898 && featureIsEnabled(FEATURE_MOTOR_STOP
)
900 && !featureIsEnabled(FEATURE_3D
)
901 && !airmodeIsEnabled()
902 && !FLIGHT_MODE(GPS_RESCUE_MODE
) // disable auto-disarm when GPS Rescue is active
904 if (isUsingSticksForArming()) {
905 if (throttleStatus
== THROTTLE_LOW
) {
906 if ((autoDisarmDelayUs
> 0) && (currentTimeUs
> disarmAt
)) {
907 // auto-disarm configured and delay is over
908 disarm(DISARM_REASON_THROTTLE_TIMEOUT
);
909 armedBeeperOn
= false;
911 // still armed; do warning beeps while armed
912 beeper(BEEPER_ARMED
);
913 armedBeeperOn
= true;
916 // throttle is not low - extend disarm time
917 disarmAt
= currentTimeUs
+ autoDisarmDelayUs
;
921 armedBeeperOn
= false;
925 // arming is via AUX switch; beep while throttle low
926 if (throttleStatus
== THROTTLE_LOW
) {
927 beeper(BEEPER_ARMED
);
928 armedBeeperOn
= true;
929 } else if (armedBeeperOn
) {
931 armedBeeperOn
= false;
935 disarmAt
= currentTimeUs
+ autoDisarmDelayUs
; // extend auto-disarm timer
938 if (!IS_RC_MODE_ACTIVE(BOXPARALYZE
)
943 processRcStickPositions();
946 if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL
)) {
947 updateInflightCalibrationState();
950 updateActivatedModes();
953 /* Enable beep warning when the crash flip mode is active */
954 if (flipOverAfterCrashActive
) {
955 beeper(BEEPER_CRASH_FLIP_MODE
);
959 if (!cliMode
&& !IS_RC_MODE_ACTIVE(BOXPARALYZE
)) {
960 processRcAdjustments(currentControlRateProfile
);
963 bool canUseHorizonMode
= true;
965 if ((IS_RC_MODE_ACTIVE(BOXANGLE
) || failsafeIsActive()) && (sensors(SENSOR_ACC
))) {
966 // bumpless transfer to Level mode
967 canUseHorizonMode
= false;
969 if (!FLIGHT_MODE(ANGLE_MODE
)) {
970 ENABLE_FLIGHT_MODE(ANGLE_MODE
);
973 DISABLE_FLIGHT_MODE(ANGLE_MODE
); // failsafe support
976 if (IS_RC_MODE_ACTIVE(BOXHORIZON
) && canUseHorizonMode
) {
978 DISABLE_FLIGHT_MODE(ANGLE_MODE
);
980 if (!FLIGHT_MODE(HORIZON_MODE
)) {
981 ENABLE_FLIGHT_MODE(HORIZON_MODE
);
984 DISABLE_FLIGHT_MODE(HORIZON_MODE
);
987 #ifdef USE_GPS_RESCUE
988 if (ARMING_FLAG(ARMED
) && (IS_RC_MODE_ACTIVE(BOXGPSRESCUE
) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure
== FAILSAFE_PROCEDURE_GPS_RESCUE
))) {
989 if (!FLIGHT_MODE(GPS_RESCUE_MODE
)) {
990 ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE
);
993 DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE
);
997 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
999 // increase frequency of attitude task to reduce drift when in angle or horizon mode
1000 rescheduleTask(TASK_ATTITUDE
, TASK_PERIOD_HZ(500));
1003 rescheduleTask(TASK_ATTITUDE
, TASK_PERIOD_HZ(100));
1006 if (!IS_RC_MODE_ACTIVE(BOXPREARM
) && ARMING_FLAG(WAS_ARMED_WITH_PREARM
)) {
1007 DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM
);
1010 #if defined(USE_ACC) || defined(USE_MAG)
1011 if (sensors(SENSOR_ACC
) || sensors(SENSOR_MAG
)) {
1012 #if defined(USE_GPS) || defined(USE_MAG)
1013 if (IS_RC_MODE_ACTIVE(BOXMAG
)) {
1014 if (!FLIGHT_MODE(MAG_MODE
)) {
1015 ENABLE_FLIGHT_MODE(MAG_MODE
);
1016 magHold
= DECIDEGREES_TO_DEGREES(attitude
.values
.yaw
);
1019 DISABLE_FLIGHT_MODE(MAG_MODE
);
1022 if (IS_RC_MODE_ACTIVE(BOXHEADFREE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
1023 if (!FLIGHT_MODE(HEADFREE_MODE
)) {
1024 ENABLE_FLIGHT_MODE(HEADFREE_MODE
);
1027 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
1029 if (IS_RC_MODE_ACTIVE(BOXHEADADJ
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
1030 if (imuQuaternionHeadfreeOffsetSet()) {
1031 beeper(BEEPER_RX_SET
);
1037 if (IS_RC_MODE_ACTIVE(BOXPASSTHRU
)) {
1038 ENABLE_FLIGHT_MODE(PASSTHRU_MODE
);
1040 DISABLE_FLIGHT_MODE(PASSTHRU_MODE
);
1043 if (mixerConfig()->mixerMode
== MIXER_FLYING_WING
|| mixerConfig()->mixerMode
== MIXER_AIRPLANE
) {
1044 DISABLE_FLIGHT_MODE(HEADFREE_MODE
);
1047 #ifdef USE_TELEMETRY
1048 if (featureIsEnabled(FEATURE_TELEMETRY
)) {
1049 bool enableSharedPortTelemetry
= (!isModeActivationConditionPresent(BOXTELEMETRY
) && ARMING_FLAG(ARMED
)) || (isModeActivationConditionPresent(BOXTELEMETRY
) && IS_RC_MODE_ACTIVE(BOXTELEMETRY
));
1050 if (enableSharedPortTelemetry
&& !sharedPortTelemetryEnabled
) {
1051 mspSerialReleaseSharedTelemetryPorts();
1052 telemetryCheckState();
1054 sharedPortTelemetryEnabled
= true;
1055 } else if (!enableSharedPortTelemetry
&& sharedPortTelemetryEnabled
) {
1056 // the telemetry state must be checked immediately so that shared serial ports are released.
1057 telemetryCheckState();
1058 mspSerialAllocatePorts();
1060 sharedPortTelemetryEnabled
= false;
1065 #ifdef USE_VTX_CONTROL
1066 vtxUpdateActivatedChannel();
1068 if (canUpdateVTX()) {
1069 handleVTXControlButton();
1073 #ifdef USE_ACRO_TRAINER
1074 pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER
) && sensors(SENSOR_ACC
));
1075 #endif // USE_ACRO_TRAINER
1077 #ifdef USE_RC_SMOOTHING_FILTER
1078 if (ARMING_FLAG(ARMED
) && !rcSmoothingInitializationComplete()) {
1079 beeper(BEEPER_RC_SMOOTHING_INIT_FAIL
);
1083 pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY
) || featureIsEnabled(FEATURE_ANTI_GRAVITY
));
1088 static FAST_CODE
void subTaskPidController(timeUs_t currentTimeUs
)
1090 uint32_t startTime
= 0;
1091 if (debugMode
== DEBUG_PIDLOOP
) {startTime
= micros();}
1092 // PID - note this is function pointer set by setPIDController()
1093 pidController(currentPidProfile
, currentTimeUs
);
1094 DEBUG_SET(DEBUG_PIDLOOP
, 1, micros() - startTime
);
1096 #ifdef USE_RUNAWAY_TAKEOFF
1097 // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
1098 // and gyro rate for any axis is above the limit for at least the activate delay period.
1099 // If so, disarm for safety
1100 if (ARMING_FLAG(ARMED
)
1102 && pidConfig()->runaway_takeoff_prevention
1103 && !runawayTakeoffCheckDisabled
1104 && !flipOverAfterCrashActive
1105 && !runawayTakeoffTemporarilyDisabled
1106 && !FLIGHT_MODE(GPS_RESCUE_MODE
) // disable Runaway Takeoff triggering if GPS Rescue is active
1107 && (!featureIsEnabled(FEATURE_MOTOR_STOP
) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW
))) {
1109 if (((fabsf(pidData
[FD_PITCH
].Sum
) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD
)
1110 || (fabsf(pidData
[FD_ROLL
].Sum
) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD
)
1111 || (fabsf(pidData
[FD_YAW
].Sum
) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD
))
1112 && ((gyroAbsRateDps(FD_PITCH
) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP
)
1113 || (gyroAbsRateDps(FD_ROLL
) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP
)
1114 || (gyroAbsRateDps(FD_YAW
) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW
))) {
1116 if (runawayTakeoffTriggerUs
== 0) {
1117 runawayTakeoffTriggerUs
= currentTimeUs
+ RUNAWAY_TAKEOFF_ACTIVATE_DELAY
;
1118 } else if (currentTimeUs
> runawayTakeoffTriggerUs
) {
1119 setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF
);
1120 disarm(DISARM_REASON_RUNAWAY_TAKEOFF
);
1123 runawayTakeoffTriggerUs
= 0;
1125 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE
, DEBUG_RUNAWAY_TAKEOFF_TRUE
);
1126 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY
, runawayTakeoffTriggerUs
== 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE
: DEBUG_RUNAWAY_TAKEOFF_TRUE
);
1128 runawayTakeoffTriggerUs
= 0;
1129 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE
, DEBUG_RUNAWAY_TAKEOFF_FALSE
);
1130 DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF
, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY
, DEBUG_RUNAWAY_TAKEOFF_FALSE
);
1135 #ifdef USE_PID_AUDIO
1136 if (isModeActivationConditionPresent(BOXPIDAUDIO
)) {
1142 static FAST_CODE_NOINLINE
void subTaskPidSubprocesses(timeUs_t currentTimeUs
)
1144 uint32_t startTime
= 0;
1145 if (debugMode
== DEBUG_PIDLOOP
) {
1146 startTime
= micros();
1149 #if defined(USE_GPS) || defined(USE_MAG)
1150 if (sensors(SENSOR_GPS
) || sensors(SENSOR_MAG
)) {
1156 if (!cliMode
&& blackboxConfig()->device
) {
1157 blackboxUpdate(currentTimeUs
);
1160 UNUSED(currentTimeUs
);
1163 DEBUG_SET(DEBUG_PIDLOOP
, 3, micros() - startTime
);
1166 #ifdef USE_TELEMETRY
1167 #define GYRO_TEMP_READ_DELAY_US 3e6 // Only read the gyro temp every 3 seconds
1168 void subTaskTelemetryPollSensors(timeUs_t currentTimeUs
)
1170 static timeUs_t lastGyroTempTimeUs
= 0;
1172 if (cmpTimeUs(currentTimeUs
, lastGyroTempTimeUs
) >= GYRO_TEMP_READ_DELAY_US
) {
1173 // Read out gyro temperature if used for telemmetry
1174 gyroReadTemperature();
1175 lastGyroTempTimeUs
= currentTimeUs
;
1180 static FAST_CODE
void subTaskMotorUpdate(timeUs_t currentTimeUs
)
1182 uint32_t startTime
= 0;
1183 if (debugMode
== DEBUG_CYCLETIME
) {
1184 startTime
= micros();
1185 static uint32_t previousMotorUpdateTime
;
1186 const uint32_t currentDeltaTime
= startTime
- previousMotorUpdateTime
;
1187 debug
[2] = currentDeltaTime
;
1188 debug
[3] = currentDeltaTime
- targetPidLooptime
;
1189 previousMotorUpdateTime
= startTime
;
1190 } else if (debugMode
== DEBUG_PIDLOOP
) {
1191 startTime
= micros();
1194 mixTable(currentTimeUs
, currentPidProfile
->vbatPidCompensation
);
1197 // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
1198 if (isMixerUsingServos()) {
1205 #ifdef USE_DSHOT_TELEMETRY_STATS
1206 if (debugMode
== DEBUG_DSHOT_RPM_ERRORS
&& useDshotTelemetry
) {
1207 const uint8_t motorCount
= MIN(getMotorCount(), 4);
1208 for (uint8_t i
= 0; i
< motorCount
; i
++) {
1209 debug
[i
] = getDshotTelemetryMotorInvalidPercent(i
);
1214 DEBUG_SET(DEBUG_PIDLOOP
, 2, micros() - startTime
);
1217 static FAST_CODE_NOINLINE
void subTaskRcCommand(timeUs_t currentTimeUs
)
1219 UNUSED(currentTimeUs
);
1221 // If we're armed, at minimum throttle, and we do arming via the
1222 // sticks, do not process yaw input from the rx. We do this so the
1223 // motors do not spin up while we are trying to arm or disarm.
1224 // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
1225 if (isUsingSticksForArming() && rcData
[THROTTLE
] <= rxConfig()->mincheck
1226 #ifndef USE_QUAD_MIXER_ONLY
1228 && !((mixerConfig()->mixerMode
== MIXER_TRI
|| mixerConfig()->mixerMode
== MIXER_CUSTOM_TRI
) && servoConfig()->tri_unarmed_servo
)
1230 && mixerConfig()->mixerMode
!= MIXER_AIRPLANE
1231 && mixerConfig()->mixerMode
!= MIXER_FLYING_WING
1240 FAST_CODE
void taskGyroSample(timeUs_t currentTimeUs
)
1242 UNUSED(currentTimeUs
);
1244 if (pidUpdateCounter
% activePidLoopDenom
== 0) {
1245 pidUpdateCounter
= 0;
1250 FAST_CODE
bool gyroFilterReady(void)
1252 if (pidUpdateCounter
% activePidLoopDenom
== 0) {
1259 FAST_CODE
bool pidLoopReady(void)
1261 if ((pidUpdateCounter
% activePidLoopDenom
) == (activePidLoopDenom
/ 2)) {
1267 FAST_CODE
void taskFiltering(timeUs_t currentTimeUs
)
1269 gyroFiltering(currentTimeUs
);
1273 // Function for loop trigger
1274 FAST_CODE
void taskMainPidLoop(timeUs_t currentTimeUs
)
1277 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
1278 if (lockMainPID() != 0) return;
1281 // DEBUG_PIDLOOP, timings for:
1283 // 1 - subTaskPidController()
1284 // 2 - subTaskMotorUpdate()
1285 // 3 - subTaskPidSubprocesses()
1286 DEBUG_SET(DEBUG_PIDLOOP
, 0, micros() - currentTimeUs
);
1288 subTaskRcCommand(currentTimeUs
);
1289 subTaskPidController(currentTimeUs
);
1290 subTaskMotorUpdate(currentTimeUs
);
1291 subTaskPidSubprocesses(currentTimeUs
);
1293 if (debugMode
== DEBUG_CYCLETIME
) {
1294 DEBUG_SET(DEBUG_CYCLETIME
, 0, getTaskDeltaTimeUs(TASK_SELF
));
1295 DEBUG_SET(DEBUG_CYCLETIME
, 1, getAverageSystemLoadPercent());
1299 bool isFlipOverAfterCrashActive(void)
1301 return flipOverAfterCrashActive
;
1304 timeUs_t
getLastDisarmTimeUs(void)
1306 return lastDisarmTimeUs
;
1309 bool isTryingToArm()
1311 return (tryingToArm
!= ARMING_DELAYED_DISARMED
);
1314 void resetTryingToArm()
1316 tryingToArm
= ARMING_DELAYED_DISARMED
;
1319 bool isLaunchControlActive(void)
1321 #ifdef USE_LAUNCH_CONTROL
1322 return launchControlState
== LAUNCH_CONTROL_ACTIVE
;