diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index a26aa514eb..9d42a54435 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -925,25 +925,8 @@ void stopInTestMode(void) */ bool inMotorTestMode(void) { static uint32_t resetTime = 0; - uint16_t inactiveMotorCommand; - if (feature(FEATURE_3D)) { - inactiveMotorCommand = flight3DConfig()->neutral3d; -#ifdef USE_DSHOT - } else if (isMotorProtocolDshot()) { - inactiveMotorCommand = DSHOT_DISARM_COMMAND; -#endif - } else { - inactiveMotorCommand = motorConfig()->mincommand; - } - int i; - bool atLeastOneMotorActivated = false; - - // set disarmed motor values - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand); - - if (atLeastOneMotorActivated) { + if (!ARMING_FLAG(ARMED) && areMotorsRunning()) { resetTime = millis() + 5000; // add 5 seconds return true; } else { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index dc5103dba1..0cf6465d42 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -316,16 +316,34 @@ static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; float motorOutputHigh, motorOutputLow; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; -uint8_t getMotorCount() +uint8_t getMotorCount(void) { return motorCount; } -float getMotorMixRange() +float getMotorMixRange(void) { return motorMixRange; } +bool areMotorsRunning(void) +{ + bool motorsRunning = false; + if (ARMING_FLAG(ARMED)) { + motorsRunning = true; + } else { + for (int i = 0; i < motorCount; i++) { + if (motor_disarmed[i] != disarmMotorOutput) { + motorsRunning = true; + + break; + } + } + } + + return motorsRunning; +} + bool mixerIsOutputSaturated(int axis, float errorRate) { if (axis == FD_YAW && (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 12076dd9b2..fd3ef0ceec 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -107,8 +107,9 @@ extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct rxConfig_s; -uint8_t getMotorCount(); -float getMotorMixRange(); +uint8_t getMotorCount(void); +float getMotorMixRange(void); +bool areMotorsRunning(void); bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 6a1486b975..cb2a9430bb 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -363,7 +363,7 @@ void beeperUpdate(timeUs_t currentTimeUs) } #ifdef USE_DSHOT - if (!ARMING_FLAG(ARMED) && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { + if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); } #endif diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index c5829a4c61..b9b42bf201 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -376,7 +376,8 @@ uint32_t rcModeActivationMask; void mspSerialAllocatePorts(void) {} uint32_t getArmingBeepTimeMicros(void) {return 0;} uint16_t getBatteryVoltageLatest(void) {return 0;} -uint8_t getMotorCount() {return 4;} +uint8_t getMotorCount(void) {return 4;} +bool areMotorsRunning(void) { return false; } bool IS_RC_MODE_ACTIVE(boxId_e) {return false;} bool isModeActivationConditionPresent(boxId_e) {return false;} uint32_t millis(void) {return 0;}