diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 00c0a98b50..87e34e9d4b 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -130,7 +130,7 @@ int16_t magHold; static bool flipOverAfterCrashMode = false; -static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero +static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; static int lastArmingDisabledReason = 0; @@ -338,8 +338,11 @@ void tryArm(void) if (ARMING_FLAG(ARMED)) { return; } + + const timeUs_t currentTimeUs = micros(); + #ifdef USE_DSHOT - if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { + if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { tryingToArm = ARMING_DELAYED_CRASHFLIP; @@ -381,7 +384,7 @@ void tryArm(void) } imuQuaternionHeadfreeOffsetSet(); - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero + disarmAt = currentTimeUs + armingConfig()->auto_disarm_delay * 1e6; // start disarm timeout, will be extended when throttle is nonzero lastArmingDisabledReason = 0; @@ -666,6 +669,7 @@ bool processRx(timeUs_t currentTimeUs) // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough + const timeUs_t autoDisarmDelayUs = armingConfig()->auto_disarm_delay * 1e6; if (ARMING_FLAG(ARMED) && featureIsEnabled(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) @@ -674,9 +678,7 @@ bool processRx(timeUs_t currentTimeUs) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { - if (armingConfig()->auto_disarm_delay != 0 - && (int32_t)(disarmAt - millis()) < 0 - ) { + if ((autoDisarmDelayUs > 0) && (currentTimeUs > disarmAt)) { // auto-disarm configured and delay is over disarm(); armedBeeperOn = false; @@ -686,11 +688,8 @@ bool processRx(timeUs_t currentTimeUs) armedBeeperOn = true; } } else { - // throttle is not low - if (armingConfig()->auto_disarm_delay != 0) { - // extend disarm time - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; - } + // throttle is not low - extend disarm time + disarmAt = currentTimeUs + autoDisarmDelayUs; if (armedBeeperOn) { beeperSilence(); @@ -707,6 +706,8 @@ bool processRx(timeUs_t currentTimeUs) armedBeeperOn = false; } } + } else { + disarmAt = currentTimeUs + autoDisarmDelayUs; // extend auto-disarm timer } processRcStickPositions();