From 714bb02c543375b5399b429464c0d31ce3cf46d5 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 17 Dec 2016 19:20:30 +0000
Subject: [PATCH] Fixes issue 1844

---
 src/main/fc/fc_msp.c    |  2 +-
 src/main/sensors/gyro.c | 18 +++++++++++-------
 src/main/sensors/gyro.h |  1 +
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 50d9e9e45..510659e2c 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1468,7 +1468,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         }
         // reinitialize the gyro filters with the new values
         validateAndFixGyroConfig();
-        gyroInit(&masterConfig.gyroConfig);
+        gyroInitFilters();
         // reinitialize the PID filters with the new values
         pidInitFilters(&currentProfile->pidProfile);
         break;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index a06597684..deea9ba34 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -228,12 +228,6 @@ case GYRO_MPU9250:
 
 bool gyroInit(const gyroConfig_t *gyroConfigToUse)
 {
-    static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
-    static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
-    static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
-    static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
-    static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
-
     gyroConfig = gyroConfigToUse;
     memset(&gyro, 0, sizeof(gyro));
 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
@@ -247,6 +241,17 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
     gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom);    // Set gyro sample rate before initialisation
     gyro.dev.lpf = gyroConfig->gyro_lpf;
     gyro.dev.init(&gyro.dev);
+    gyroInitFilters();
+    return true;
+}
+
+void gyroInitFilters(void)
+{
+    static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
+    static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
+    static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
+    static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
+    static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
 
     softLpfFilterApplyFn = nullFilterApply;
     notchFilter1ApplyFn = nullFilterApply;
@@ -291,7 +296,6 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
             biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
         }
     }
-    return true;
 }
 
 bool isGyroCalibrationComplete(void)
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index d0525b4d0..1cd410f14 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -57,5 +57,6 @@ typedef struct gyroConfig_s {
 
 void gyroSetCalibrationCycles(void);
 bool gyroInit(const gyroConfig_t *gyroConfigToUse);
+void gyroInitFilters(void);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);