From 3fdb0e2b2ee787b3e524e41324eda0c8108540e5 Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Fri, 29 Mar 2019 08:53:34 -0400
Subject: [PATCH] Disallow dual gyro in "BOTH" mode if RPM filter enabled RPM
 Filter currently doesn't support dual-gyro running in "BOTH" mode. Switch to
 the "FIRST" gyro if configured for "BOTH" and RPM filter is enabled.

Once RPM filter is updated to support dual-gyro using both sensors then this check should be removed.
---
 src/main/fc/config.c          |  8 ++++++++
 src/main/fc/core.c            | 11 ++++-------
 src/main/sensors/rpm_filter.c |  5 +++++
 src/main/sensors/rpm_filter.h |  1 +
 4 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 92f398b809..9a1d185dbe 100644
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -461,6 +461,14 @@ static void validateAndFixConfig(void)
     }
 #endif
 
+    // Temporary workaround until RPM Filter supports dual-gyro using both sensors
+    // Once support is added remove this block
+#if defined(USE_MULTI_GYRO) && defined(USE_RPM_FILTER)
+    if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH && isRpmFilterEnabled()) {
+        gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
+    }
+#endif
+
 #if defined(TARGET_VALIDATECONFIG)
     targetValidateConfiguration();
 #endif
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index ef46fff714..c7b7bafd1c 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -296,13 +296,10 @@ void updateArmingStatus(void)
 #ifdef USE_RPM_FILTER
         // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined
         // If the RPM filter 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)) {
-            if (!isDshotTelemetryActive()) {
-                setArmingDisabled(ARMING_DISABLED_RPMFILTER);
-            } else {
-                unsetArmingDisabled(ARMING_DISABLED_RPMFILTER);
-            }
+        if (isRpmFilterEnabled() && !isDshotTelemetryActive()) {
+            setArmingDisabled(ARMING_DISABLED_RPMFILTER);
+        } else {
+            unsetArmingDisabled(ARMING_DISABLED_RPMFILTER);
         }
 #endif
 
diff --git a/src/main/sensors/rpm_filter.c b/src/main/sensors/rpm_filter.c
index dd20be73be..bc322c04a3 100644
--- a/src/main/sensors/rpm_filter.c
+++ b/src/main/sensors/rpm_filter.c
@@ -209,4 +209,9 @@ FAST_CODE_NOINLINE void rpmFilterUpdate()
     }
 }
 
+bool isRpmFilterEnabled(void)
+{
+    return (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics));
+}
+
 #endif
diff --git a/src/main/sensors/rpm_filter.h b/src/main/sensors/rpm_filter.h
index 19b0455a36..fab963ba9d 100644
--- a/src/main/sensors/rpm_filter.h
+++ b/src/main/sensors/rpm_filter.h
@@ -42,3 +42,4 @@ void  rpmFilterInit(const rpmFilterConfig_t *config);
 float rpmFilterGyro(int axis, float values);
 float rpmFilterDterm(int axis, float values);
 void  rpmFilterUpdate();
+bool isRpmFilterEnabled(void);