Add guard time between dshot beacon and arming/disarming

Tries to prevent DSHOT beacon commands from interfering with commands to set the motor direction.

Adds a 2 second delay after disarming before DSHOT beacon commands will be sent. This attempts to prevent the beacon commands from interfering with the motor direction reset that happens after using crash flip mode.

During arming if a DSHOT beacon command has been sent within 2 seconds the arming will be delayed until the 2 seconds have passed. This attempts to prevent interference with the motor direction commands sent at arming.

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;
+}
+

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();

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)) {

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

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);

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) {}