From cbaad53c462fcfe20e83e792d8e8b7c264fe79ee Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Fri, 8 Sep 2017 00:14:30 +0100
Subject: [PATCH] Do runtime check for gyro slew filter

---
 src/main/sensors/gyro.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 4b717ef25e..94e226260f 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -76,6 +76,7 @@
 
 gyro_t gyro;
 static uint8_t gyroDebugMode;
+static bool gyroUseSlewFilter;
 
 
 typedef struct gyroCalibration_s {
@@ -363,6 +364,18 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
         break;
     }
 
+    switch (gyroHardware) {
+    case GYRO_ICM20601:
+    case GYRO_ICM20602:
+    case GYRO_ICM20608G:
+    case GYRO_ICM20689:
+        gyroUseSlewFilter = true;
+        break;
+    default:
+        gyroUseSlewFilter = false;
+        break;
+    }
+
     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
     gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
     gyroSensor->gyroDev.lpf = gyroConfig()->gyro_lpf;
@@ -604,6 +617,9 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
 int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 {
     int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
+    if (gyroUseSlewFilter == false) {
+        return newRawGyro;
+    }
 
     if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
         newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];