From d16f35d9cbba50937980569bd7417b9da0cca2ba Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Thu, 28 Mar 2019 22:58:22 +1300
Subject: [PATCH] Fixed defaults for lowpass filters.

---
 src/main/fc/config.c    | 56 ++++++++++++++++++++++++-----------------
 src/main/flight/pid.c   |  6 +++--
 src/main/sensors/gyro.c |  6 +++--
 3 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 92f398b809..c6463cdc32 100644
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -190,34 +190,30 @@ static void validateAndFixConfig(void)
         featureDisable(FEATURE_GPS);
     }
 
-    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
-        systemConfigMutable()->activeRateProfile = 0;
-    }
-    loadControlRateProfile();
-
-    if (systemConfig()->pidProfileIndex >= PID_PROFILE_COUNT) {
-        systemConfigMutable()->pidProfileIndex = 0;
-    }
-    loadPidProfile();
-
-    // Prevent invalid notch cutoff
-    if (currentPidProfile->dterm_notch_cutoff >= currentPidProfile->dterm_notch_hz) {
-        currentPidProfile->dterm_notch_hz = 0;
-    }
+    for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
+        // Prevent invalid notch cutoff
+        if (pidProfilesMutable(i)->dterm_notch_cutoff >= pidProfilesMutable(i)->dterm_notch_hz) {
+            pidProfilesMutable(i)->dterm_notch_hz = 0;
+        }
 
 #ifdef USE_DYN_LPF
-    //PRevent invalid dynamic lowpass
-    if (currentPidProfile->dyn_lpf_dterm_min_hz > currentPidProfile->dyn_lpf_dterm_max_hz) {
-        currentPidProfile->dyn_lpf_dterm_min_hz = 0;
-    }
+        //Prevent invalid dynamic lowpass
+        if (pidProfilesMutable(i)->dyn_lpf_dterm_min_hz > pidProfilesMutable(i)->dyn_lpf_dterm_max_hz) {
+            pidProfilesMutable(i)->dyn_lpf_dterm_min_hz = 0;
+        }
+
+        if (pidProfilesMutable(i)->dyn_lpf_dterm_min_hz > 0) {
+            pidProfilesMutable(i)->dterm_lowpass_hz = 0;
+        }
 #endif
 
-    if (currentPidProfile->motor_output_limit > 100 || currentPidProfile->motor_output_limit == 0) {
-        currentPidProfile->motor_output_limit = 100;
-    }
+        if (pidProfilesMutable(i)->motor_output_limit > 100 || pidProfilesMutable(i)->motor_output_limit == 0) {
+            pidProfilesMutable(i)->motor_output_limit = 100;
+        }
 
-    if (currentPidProfile->auto_profile_cell_count > MAX_AUTO_DETECT_CELL_COUNT || currentPidProfile->auto_profile_cell_count < AUTO_PROFILE_CELL_COUNT_CHANGE) {
-        currentPidProfile->auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY;
+        if (pidProfilesMutable(i)->auto_profile_cell_count > MAX_AUTO_DETECT_CELL_COUNT || pidProfilesMutable(i)->auto_profile_cell_count < AUTO_PROFILE_CELL_COUNT_CHANGE) {
+            pidProfilesMutable(i)->auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY;
+        }
     }
 
     if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
@@ -487,6 +483,10 @@ void validateAndFixGyroConfig(void)
     if (gyroConfig()->dyn_lpf_gyro_min_hz > gyroConfig()->dyn_lpf_gyro_max_hz) {
         gyroConfigMutable()->dyn_lpf_gyro_min_hz = 0;
     }
+
+    if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
+        gyroConfigMutable()->gyro_lowpass_hz = 0;
+    }
 #endif
 
     if (gyroConfig()->gyro_hardware_lpf == GYRO_HARDWARE_LPF_1KHZ_SAMPLE) {
@@ -574,6 +574,16 @@ void validateAndFixGyroConfig(void)
     }
 #endif // USE_SDCARD
 #endif // USE_BLACKBOX
+
+    if (systemConfig()->activeRateProfile >= CONTROL_RATE_PROFILE_COUNT) {
+        systemConfigMutable()->activeRateProfile = 0;
+    }
+    loadControlRateProfile();
+
+    if (systemConfig()->pidProfileIndex >= PID_PROFILE_COUNT) {
+        systemConfigMutable()->pidProfileIndex = 0;
+    }
+    loadPidProfile();
 }
 
 bool readEEPROM(void)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 2b5f1b0cb6..6f11f9692e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -203,7 +203,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .transient_throttle_limit = 15,
     );
 #ifdef USE_DYN_LPF
-    pidProfile->dterm_lowpass_hz = 150;
+    pidProfile->dterm_lowpass_hz = 0;
     pidProfile->dterm_lowpass2_hz = 150;
     pidProfile->dterm_filter_type = FILTER_BIQUAD;
     pidProfile->dterm_filter2_type = FILTER_BIQUAD;
@@ -339,7 +339,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
 
 #ifdef USE_DYN_LPF
-    dterm_lowpass_hz = MAX(pidProfile->dterm_lowpass_hz, pidProfile->dyn_lpf_dterm_min_hz);
+    if (pidProfile->dyn_lpf_dterm_min_hz) {
+        dterm_lowpass_hz = pidProfile->dyn_lpf_dterm_min_hz;
+    }
 #endif
 
     if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 9237d15282..5327a26e8b 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -222,7 +222,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->dyn_notch_q = 120;
     gyroConfig->dyn_notch_min_hz = 150;
 #ifdef USE_DYN_LPF
-    gyroConfig->gyro_lowpass_hz = 150;
+    gyroConfig->gyro_lowpass_hz = 0;
     gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
     gyroConfig->gyro_lowpass2_hz = 0;
 #endif
@@ -744,7 +744,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
     uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
 
 #ifdef USE_DYN_LPF
-    gyro_lowpass_hz = MAX(gyroConfig()->gyro_lowpass_hz, gyroConfig()->dyn_lpf_gyro_min_hz);
+    if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
+        gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
+    }
 #endif
 
     gyroInitLowpassFilterLpf(