fix dshot beacon (#46)

Co-authored-by: ctzsnooze <[email protected]>

Author: DusKing1

src/main/drivers/dshot_command.c

@@ -235,7 +235,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
         dshotCommandControl_t *commandControl = addCommand();
         if (commandControl) {
             commandControl->repeats = repeats;
-            commandControl->delayAfterCommandUs = delayAfterCommandUs;
+            commandControl->delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
             for (unsigned i = 0; i < motorCount; i++) {
                 if (index == i || index == ALL_MOTORS) {
                     commandControl->command[i] = command;

src/main/fc/core.c

@@ -493,7 +493,7 @@ void tryArm(void)
         const timeUs_t currentTimeUs = micros();
 
 #ifdef USE_DSHOT
-        if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
+        if (cmpTimeUs(currentTimeUs, getLastDshotBeaconCommandTimeUs()) < DSHOT_BEACON_GUARD_DELAY_US) {
             if (tryingToArm == ARMING_DELAYED_DISARMED) {
                 if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
                     tryingToArm = ARMING_DELAYED_CRASHFLIP;

src/main/io/beeper.c

@@ -400,19 +400,21 @@ void beeperUpdate(timeUs_t currentTimeUs)
         return;
     }
 
-    if (!beeperIsOn) {
 #ifdef USE_DSHOT
-        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))))) {
-
-            if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) {
+    if (!areMotorsRunning() && (currentBeeperEntry->mode == BEEPER_RX_SET || currentBeeperEntry->mode == BEEPER_RX_LOST)) {
+        if (cmpTimeUs(currentTimeUs, getLastDisarmTimeUs()) > DSHOT_BEACON_GUARD_DELAY_US && !isTryingToArm()) {
+            const timeDelta_t dShotBeaconInterval = (currentBeeperEntry->mode == BEEPER_RX_SET) ? DSHOT_BEACON_MODE_INTERVAL_US : DSHOT_BEACON_RXLOSS_INTERVAL_US;
+            if (cmpTimeUs(currentTimeUs, lastDshotBeaconCommandTimeUs) > dShotBeaconInterval) {
+                // at least 500ms between DShot beacons to allow time for the sound to fully complete
+                // the DShot Beacon tone duration is determined by the ESC, and should not exceed 250ms
                 lastDshotBeaconCommandTimeUs = currentTimeUs;
                 dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, DSHOT_CMD_TYPE_INLINE);
             }
         }
+    }
 #endif
 
+if (!beeperIsOn) {
         if (currentBeeperEntry->sequence[beeperPos] != 0) {
             if (!(beeperConfig()->beeper_off_flags & BEEPER_GET_FLAG(currentBeeperEntry->mode))) {
                 BEEP_ON;

src/main/io/beeper.h

@@ -27,6 +27,9 @@
 #ifdef USE_DSHOT
 #define DSHOT_BEACON_GUARD_DELAY_US 1200000  // Time to separate dshot beacon and armining/disarming events
                                              // to prevent interference with motor direction commands
+#define DSHOT_BEACON_MODE_INTERVAL_US     450000  // at least 450ms between successive DShot beacon iterations to allow time for ESC to play tone
+#define DSHOT_BEACON_RXLOSS_INTERVAL_US   950000  // at least 950ms between successive DShot beacon iterations to allow time for ESC to play tone
+                                                  // we check beeper every 100ms, so these result in 500ms and 1.0s in practice
 #endif
 
 typedef enum {