Gyro Soft LPF Rework // Filter rework

remove 3 point average

lux optimize

src/main/common/filter.c

@@ -10,8 +10,10 @@
 #include <stdlib.h>
 #include <math.h>
 
+#include "common/axis.h"
 #include "common/filter.h"
 #include "common/axis.h"
+#include "common/maths.h"
 
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@@ -27,23 +29,35 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-// 7 Tap FIR filter as described here:
+/**
-// Thanks to Qcopter
+ * Typical quadcopter motor noise frequency (at 50% throttle):
-void filterApply7TapFIR(int16_t data[]) {
+ *  450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
-    int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
+ *  250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
-    static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
+ */
+static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 },    // 1khz; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
+                                          { 18, 30, 42, 46, 40, 34, 22, 8, 8},     // 1khz; group delay 3ms;   -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
+                                          { 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // 1khz; group delay 4ms;   -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
+
+int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level)
+{
+    return gyroFIRCoeff_1000[filter_level];
+}
+
+// 9 Tap FIR filter as described here:
+// Thanks to Qcopter & BorisB & DigitalEntity
+void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
+{
     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][8] = data[axis];
-        FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
+        FIRsum += state[axis][8] * coeff[8];
         data[axis] = FIRsum / 256;
     }
 }

src/main/common/filter.h

@@ -13,5 +13,5 @@ typedef struct filterStatePt1_s {
 } filterStatePt1_t;
 
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-
+int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level);
-void filterApply7TapFIR(int16_t data[]);
+void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);

src/main/config/config.c

@@ -26,6 +26,7 @@
 #include "common/color.h"
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
@@ -174,6 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->I8[PIDVEL] = 45;
     pidProfile->D8[PIDVEL] = 1;
 
+    pidProfile->gyro_soft_lpf = 0;   // LOW filtering by default
     pidProfile->dterm_cut_hz = 40;
     pidProfile->yaw_pterm_cut_hz = 50;
 
@@ -691,7 +693,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig);
+    useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/flight/pid.c

@@ -142,11 +142,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
          } else {
             // calculate error and limit the angle to the max inclination
 #ifdef GPS
-            errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+            errorAngle = (constrainf(((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f)) + GPS_angle[axis], -((int) max_angle_inclination),
-                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
+                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
 #else
-            errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+            errorAngle = (constrainf((float)rcCommand[axis] * ((float)max_angle_inclination / 500.0f), -((int) max_angle_inclination),
-                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
+                    +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f;
 #endif
 
             if (FLIGHT_MODE(ANGLE_MODE)) {
@@ -341,7 +341,7 @@ void pidSetController(pidControllerType_e type)
 {
     switch (type) {
         default:
-        case PID_CONTROLLER_REWRITE:
+        case PID_CONTROLLER_MWREWRITE:
             pid_controller = pidRewrite;
             break;
         case PID_CONTROLLER_LUX_FLOAT:

src/main/flight/pid.h

@@ -18,6 +18,8 @@
 #pragma once
 
 #define GYRO_I_MAX 256                      // Gyro I limiter
+#define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
+#define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
 
 typedef enum {
     PIDROLL,
@@ -34,7 +36,7 @@ typedef enum {
 } pidIndex_e;
 
 typedef enum {
-    PID_CONTROLLER_REWRITE = 1,
+    PID_CONTROLLER_MWREWRITE,
     PID_CONTROLLER_LUX_FLOAT,
     PID_COUNT
 } pidControllerType_e;
@@ -42,7 +44,7 @@ typedef enum {
 #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
 
 typedef struct pidProfile_s {
-    uint8_t pidController;                  // 1 = rewrite, 2 = luxfloat
+    uint8_t pidController;                  // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid
 
     uint8_t P8[PID_ITEM_COUNT];
     uint8_t I8[PID_ITEM_COUNT];
@@ -55,8 +57,10 @@ typedef struct pidProfile_s {
     float H_level;
     uint8_t H_sensitivity;
 
+    uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
-    uint8_t yaw_pterm_cut_hz;              // Yaw P low pass filter for pterm. very usefull on noisy setups
+    uint8_t yaw_pterm_cut_hz;               // Used for filering Pterm noise on noisy frames
+    uint8_t gyro_soft_lpf;                  // Gyro FIR filter
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune

src/main/io/serial_cli.c

@@ -350,6 +350,10 @@ static const char * const lookupTableSerialRX[] = {
     "XB-B-RJ01"
 };
 
+static const char * const lookupTableGyroFilter[] = {
+    "OFF", "LOW", "MEDIUM", "HIGH"
+};
+
 
 typedef struct lookupTableEntry_s {
     const char * const *values;
@@ -367,7 +371,8 @@ typedef enum {
     TABLE_CURRENT_SENSOR,
     TABLE_GIMBAL_MODE,
     TABLE_PID_CONTROLLER,
-    TABLE_SERIAL_RX
+    TABLE_SERIAL_RX,
+    TABLE_GYRO_FILTER,
 } lookupTableIndex_e;
 
 static const lookupTableEntry_t lookupTables[] = {
@@ -381,7 +386,8 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) },
     { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
     { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
-    { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }
+    { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
+	{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }
 };
 
 #define VALUE_TYPE_OFFSET 0
@@ -626,8 +632,9 @@ const clivalue_t valueTable[] = {
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0,  200 } },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0,  200 } },
 
-    { "yaw_pterm_cut_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = { 0,  200 } },
+    { "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_GYRO_FILTER } },
-	{ "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = { 0,  200 } },
+    { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
+    { "yaw_pterm_cut_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
 
 #ifdef GTUNE
     { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10,  200 } },

src/main/sensors/gyro.c

@@ -38,13 +38,16 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
+static int8_t * gyroFIRTable = 0L;
+static int16_t gyroFIRState[3][9];
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
 {
     gyroConfig = gyroConfigToUse;
+    gyroFIRTable = filterTableToUse;
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -121,9 +124,12 @@ void gyroUpdate(void)
     if (!gyro.read(gyroADC)) {
         return;
     }
-    alignSensors(gyroADC, gyroADC, gyroAlign);
 
-    filterApply7TapFIR(gyroADC); // Apply filter to gyro
+    if (gyroFIRTable) {
+        filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
+    }
+
+    alignSensors(gyroADC, gyroADC, gyroAlign);
 
     if (!isGyroCalibrationComplete()) {
         performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);

src/main/sensors/gyro.h

@@ -41,7 +41,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);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);