From 1e6e9e719f94403a0f4cafaa4198b91b49e91614 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 9 Jun 2018 18:44:37 -0400 Subject: [PATCH] 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 | 26 ++++++++++++++++++++++++++ src/main/fc/fc_core.h | 3 +++ src/main/fc/rc_controls.c | 7 +++++++ src/main/io/beeper.c | 18 +++++++++++++++++- src/main/io/beeper.h | 6 ++++++ src/test/unit/rc_controls_unittest.cc | 3 +++ 6 files changed, 62 insertions(+), 1 deletion(-) 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) {}