diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d0aaf44fcd..4d03c0ebb5 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -135,6 +135,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m bool isRXDataNew; static int lastArmingDisabledReason = 0; +static timeUs_t lastDisarmTimeUs; +static bool tryingToArm; #ifdef USE_RUNAWAY_TAKEOFF static timeUs_t runawayTakeoffDeactivateUs = 0; @@ -318,6 +320,7 @@ void disarm(void) { if (ARMING_FLAG(ARMED)) { DISABLE_ARMING_FLAG(ARMED); + lastDisarmTimeUs = micros(); #ifdef USE_BLACKBOX if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON @@ -353,6 +356,10 @@ void tryArm(void) return; } #ifdef USE_DSHOT + if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { + tryingToArm = true; + return; + } if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { flipOverAfterCrashMode = false; @@ -374,6 +381,8 @@ void tryArm(void) ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); + resetTryingToArm(); + #ifdef USE_ACRO_TRAINER pidAcroTrainerInit(); #endif // USE_ACRO_TRAINER @@ -404,6 +413,7 @@ void tryArm(void) runawayTakeoffTriggerUs = 0; #endif } else { + resetTryingToArm(); if (!isFirstArmingGyroCalibrationRunning()) { int armingDisabledReason = ffs(getArmingDisableFlags()); if (lastArmingDisabledReason != armingDisabledReason) { @@ -1031,3 +1041,19 @@ bool isFlipOverAfterCrashMode(void) { return flipOverAfterCrashMode; } + +timeUs_t getLastDisarmTimeUs(void) +{ + return lastDisarmTimeUs; +} + +bool isTryingToArm() +{ + return tryingToArm; +} + +void resetTryingToArm() +{ + tryingToArm = false; +} + diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index e2ed7fcc9a..2da50dc574 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -53,3 +53,6 @@ bool isFlipOverAfterCrashMode(void); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); bool isAirmodeActivated(); +timeUs_t getLastDisarmTimeUs(void); +bool isTryingToArm(); +void resetTryingToArm(); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f16b4dc3aa..902a8ad944 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -179,6 +179,7 @@ void processRcStickPositions() // Arming via ARM BOX tryArm(); } else { + resetTryingToArm(); // Disarming via ARM BOX resetArmingDisabled(); if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { @@ -192,6 +193,7 @@ void processRcStickPositions() if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) { doNotRepeat = true; // Disarm on throttle down + yaw + resetTryingToArm(); if (ARMING_FLAG(ARMED)) disarm(); else { @@ -214,11 +216,16 @@ void processRcStickPositions() if (!ARMING_FLAG(ARMED)) { // Arm via YAW tryArm(); + if (isTryingToArm()) { + doNotRepeat = false; + } } else { resetArmingDisabled(); } } return; + } else { + resetTryingToArm(); } if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS || (getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) { diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 1ea4992f8d..8819ebec98 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -36,6 +36,7 @@ #include "flight/mixer.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/runtime_config.h" #include "io/statusindicator.h" @@ -78,6 +79,10 @@ #define BEEPER_COMMAND_REPEAT 0xFE #define BEEPER_COMMAND_STOP 0xFF +#ifdef USE_DSHOT +static timeUs_t lastDshotBeaconCommandTimeUs; +#endif + #ifdef USE_BEEPER /* Beeper Sound Sequences: (Square wave generation) * Sequence must end with 0xFF or 0xFE. 0xFE repeats the sequence from @@ -394,7 +399,11 @@ void beeperUpdate(timeUs_t currentTimeUs) if (!areMotorsRunning() && ((currentBeeperEntry->mode == BEEPER_RX_SET && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET))) || (currentBeeperEntry->mode == BEEPER_RX_LOST && !(beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST))))) { - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false); + + if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) { + lastDshotBeaconCommandTimeUs = currentTimeUs; + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false); + } } #endif @@ -514,3 +523,10 @@ int beeperTableEntryCount(void) {return 0;} bool isBeeperOn(void) {return false;} #endif + +#ifdef USE_DSHOT +timeUs_t getLastDshotBeaconCommandTimeUs(void) +{ + return lastDshotBeaconCommandTimeUs; +} +#endif diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 7893cb18c7..effc003e51 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -24,6 +24,11 @@ #define BEEPER_GET_FLAG(mode) (1 << (mode - 1)) +#ifdef USE_DSHOT +#define DSHOT_BEACON_GUARD_DELAY_US 2000000 // Time to separate dshot beacon and armining/disarming events + // to prevent interference with motor direction commands +#endif + typedef enum { // IMPORTANT: these are in priority order, 0 = Highest BEEPER_SILENCE = 0, // Silence, see beeperSilence() @@ -94,3 +99,4 @@ uint32_t beeperModeMaskForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); int beeperTableEntryCount(void); bool isBeeperOn(void); +timeUs_t getLastDshotBeaconCommandTimeUs(void); diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 13992d02ea..bf20ccaf28 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -52,6 +52,7 @@ extern "C" { #include "fc/rc_controls.h" #include "fc/runtime_config.h" + #include "fc/fc_core.h" #include "scheduler/scheduler.h" } @@ -737,3 +738,5 @@ timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; } armingDisableFlags_e getArmingDisableFlags(void) { return (armingDisableFlags_e) 0; } +bool isTryingToArm(void) { return false; } +void resetTryingToArm(void) {}