2khz Support Added

src/main/common/filter.c

@@ -1,8 +1,18 @@
 /*
- * filter.c
+ * This file is part of Cleanflight.
  *
- *  Created on: 24 jun. 2015
+ * Cleanflight is free software: you can redistribute it and/or modify
- *      Author: borisb
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
 #include <stdbool.h>
@@ -10,8 +20,9 @@
 #include <stdlib.h>
 #include <math.h>
 
-#include "common/filter.h"
 #include "common/axis.h"
+#include "common/filter.h"
+#include "common/maths.h"
 
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@@ -27,23 +38,37 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-// 7 Tap FIR filter as described here:
+static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 };  // looptime=500;
-// Thanks to Qcopter
+static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
-void filterApplyFIR(int16_t data[]) {
+
-    int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
+int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime)
-    static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
+{
+    if (filter_level == 0) {
+        return NULL;
+    }
+
+    // filter for 2kHz looptime
+    if (targetLooptime == 500) {
+        return gyroFIRCoeff_500;
+    } else {    // filter for 1kHz looptime
+        return gyroFIRCoeff_1000;
+    }
+}
+
+// Thanks to Qcopter & BorisB & DigitalEntity
+void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS])
+{
     int32_t FIRsum;
     int axis, i;
 
-    // 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         FIRsum = 0;
-        for (i = 0; i <= 5; i++) {
+        for (i = 0; i <= 7; i++) {
-            gyro_delay[axis][i] = gyro_delay[axis][i + 1];
+            state[axis][i] = state[axis][i + 1];
-            FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
+            FIRsum += state[axis][i] * (int16_t)coeff[i];
         }
-        gyro_delay[axis][6] = data[axis];
+        state[axis][FILTER_TAPS-1] = data[axis];
-        FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
+        FIRsum += state[axis][FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
         data[axis] = FIRsum / 256;
     }
 }

src/main/common/filter.h

@@ -1,16 +1,28 @@
 /*
- * filter.h
+ * This file is part of Cleanflight.
  *
- *  Created on: 24 jun. 2015
+ * Cleanflight is free software: you can redistribute it and/or modify
- *      Author: borisb
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-
+#define FILTER_TAPS 13
 
 typedef struct filterStatePt1_s {
 	float state;
 	float RC;
+	float constdT;
 } filterStatePt1_t;
 
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-void filterApplyFIR(int16_t data[3]);
+int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
+void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]);

src/main/config/config.c

@@ -36,6 +36,7 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/serial.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
@@ -751,7 +752,7 @@ void activateConfig(void)
     );
 
 
-    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf); // Leave this for more coefficients in the future
+    useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, targetLooptime));
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/drivers/gyro_sync.c

@@ -46,10 +46,18 @@ void gyroUpdateSampleRate(uint8_t lpf) {
 
     if (!lpf) {
         gyroSamplePeriod = 125;
-        gyroSyncDenominator = 8; // Sample every 8th gyro measurement
+#ifdef STM32F303xC
+        gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
+#else
+        if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) {
+            gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
+        } else {
+            gyroSyncDenominator = 8; // Sample every 8th gyro measurement 1khz
+        }
+#endif
     } else {
         gyroSamplePeriod = 1000;
-        gyroSyncDenominator = 1; // Full Sampling
+        gyroSyncDenominator = 1; // Full Sampling 1khz
     }
 
     // calculate gyro divider and targetLooptime (expected cycleTime)

src/main/mw.c

@@ -91,7 +91,7 @@ enum {
     ALIGN_MAG = 2
 };
 
-//#define JITTER_DEBUG 0  // Specify debug value for jitter debug
+#define JITTER_DEBUG 0  // Specify debug value for jitter debug
 
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)

src/main/scheduler.c

@@ -27,7 +27,7 @@
 
 #include "drivers/system.h"
 
-//#define SCHEDULER_DEBUG
+#define SCHEDULER_DEBUG
 
 cfTaskId_e currentTaskId = TASK_NONE;
 

src/main/sensors/gyro.c

@@ -38,15 +38,17 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
-static uint8_t gyroFIRFilter;
+static int8_t * gyroFIRTable = 0L;
+static int16_t gyroFIRState[3][FILTER_TAPS];
+
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t filter)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
 {
     gyroConfig = gyroConfigToUse;
-    gyroFIRFilter = filter;
+    gyroFIRTable = filterTableToUse;
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -124,8 +126,8 @@ void gyroUpdate(void)
         return;
     }
 
-    if (gyroFIRFilter) {
+    if (gyroFIRTable) {
-        filterApplyFIR(gyroADC);
+        filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable);
     }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);

src/main/sensors/gyro.h

@@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t filter);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);