diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index ed782cde88..4cea1e15c1 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -146,6 +146,7 @@ typedef struct { volatile bool isInput; volatile bool hasTelemetry; uint16_t dshotTelemetryValue; + bool dshotTelemetryActive; #ifdef USE_HAL_DRIVER LL_TIM_OC_InitTypeDef ocInitStruct; LL_TIM_IC_InitTypeDef icInitStruct; @@ -253,6 +254,7 @@ bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); uint16_t getDshotTelemetry(uint8_t index); +bool isDshotTelemetryActive(uint8_t index); #endif diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 11f4d5d0fe..67cffeef45 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -223,6 +223,7 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount) } if (value != 0xffff) { dmaMotors[i].dshotTelemetryValue = value; + dmaMotors[i].dshotTelemetryActive = true; if (i < 4) { DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value); } @@ -246,5 +247,10 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount) } } +bool isDshotTelemetryActive(uint8_t index) +{ + return dmaMotors[index].dshotTelemetryActive; +} + #endif #endif diff --git a/src/main/fc/core.c b/src/main/fc/core.c index add639d013..d840e21670 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -56,6 +56,8 @@ #if defined(USE_GYRO_DATA_ANALYSE) #include "sensors/gyroanalyse.h" #endif +#include "sensors/rpm_filter.h" + #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/core.h" @@ -291,6 +293,26 @@ void updateArmingStatus(void) } #endif +#ifdef USE_RPM_FILTER + // USE_RPM_FILTER will only be set if USE_DSHOT and USE_DSHOT_TELEMETRY are defined + // If dshot_bidir is anabled and any motor isn't providing telemetry, then disable arming + if (motorConfig()->dev.useDshotTelemetry + && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics)) { + bool dshotTelemetryActive = true; + for (uint8_t i = 0; i < getMotorCount(); i++) { + if (!isDshotTelemetryActive(i)) { + dshotTelemetryActive = false; + break; + } + } + if (!dshotTelemetryActive) { + setArmingDisabled(ARMING_DISABLED_RPMFILTER); + } else { + unsetArmingDisabled(ARMING_DISABLED_RPMFILTER); + } + } +#endif + if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) { setArmingDisabled(ARMING_DISABLED_PARALYZE); } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 2d079ad153..f0d89b417c 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -53,6 +53,7 @@ const char *armingDisableFlagNames[]= { "PARALYZE", "GPS", "RESCUE SW", + "RPMFILTER", "ARMSWITCH", }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 612b8f246c..d022162e0e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -59,7 +59,8 @@ typedef enum { ARMING_DISABLED_PARALYZE = (1 << 16), ARMING_DISABLED_GPS = (1 << 17), ARMING_DISABLED_RESC = (1 << 18), - ARMING_DISABLED_ARM_SWITCH = (1 << 19), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_RPMFILTER = (1 << 19), + ARMING_DISABLED_ARM_SWITCH = (1 << 20), // Needs to be the last element, since it's always activated if one of the others is active when arming } armingDisableFlags_e; #define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1)