Merge pull request #6079 from etracer65/dshot_beacon_motor_direction

Add guard time between dshot beacon and arming/disarming

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