From 266ed02a81757a10c865e5c292387f4eb0db3056 Mon Sep 17 00:00:00 2001 From: Stephen Willey Date: Fri, 18 Sep 2015 15:23:09 -0700 Subject: [PATCH] Substitute pt1 filter of gyroADC for 7 Tap FIR filter --- src/main/common/filter.c | 22 ++++++++++++++++++++++ src/main/common/filter.h | 2 ++ src/main/config/config.c | 1 + src/main/flight/pid.c | 11 +++-------- src/main/flight/pid.h | 1 + src/main/io/serial_cli.c | 1 + src/main/mw.c | 4 +++- 7 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 38459f915f..b408eaf7c2 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -11,6 +11,7 @@ #include #include "common/filter.h" +#include "common/axis.h" // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) @@ -25,3 +26,24 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float return filter->state; } + +// 7 Tap FIR filter as described here: +// http://www.rcgroups.com/forums/showpost.php?p=32723398&postcount=2776 +void filterApply7TapFIR(int16_t data[]) { + int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; + static int16_t gyro_delay[3][7] = { {0}, {0}, {0} }; + 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++) { + gyro_delay[axis][i] = gyro_delay[axis][i + 1]; + FIRsum += gyro_delay[axis][i] * FIRcoeff[i]; + } + gyro_delay[axis][6] = data[axis]; + FIRsum += gyro_delay[axis][6] * FIRcoeff[6]; + data[axis] = FIRsum / 256; + } +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 87452436bb..7b949e8046 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -13,3 +13,5 @@ typedef struct filterStatePt1_s { } filterStatePt1_t; float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); + +void filterApply7TapFIR(int16_t data[]); diff --git a/src/main/config/config.c b/src/main/config/config.c index 7a150c2766..25b8a64bc8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -171,6 +171,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; + pidProfile->gyro_fir_filter_enable = 0; pidProfile->gyro_cut_hz = 0; pidProfile->pterm_cut_hz = 60; pidProfile->dterm_cut_hz = 17; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e61b28acdc..7fed92e2f6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -111,8 +111,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float ITerm,PTerm,DTerm; int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastError[3]; - static float delta1[3], delta2[3]; - float delta, deltaSum; + float delta; int axis; float horizonLevelStrength = 1; @@ -210,17 +209,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. delta *= (1.0f / dT); - // add moving average here to reduce noise - deltaSum = delta1[axis] + delta2[axis] + delta; - delta2[axis] = delta1[axis]; - delta1[axis] = delta; // Dterm low pass if (pidProfile->dterm_cut_hz) { - deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT); + delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); } - DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); + DTerm = constrainf((delta / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3ec0004e96..2c2a161181 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -65,6 +65,7 @@ typedef struct pidProfile_s { uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames uint8_t gyro_cut_hz; // Used for soft gyro filtering + uint8_t gyro_fir_filter_enable; // Switches PT1 filter to 7TapFIRFilter for gyro } pidProfile_t; #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 51f8e61d60..80bcd6427a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -522,6 +522,7 @@ const clivalue_t valueTable[] = { { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, { "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 }, { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 }, + { "gyro_fir_filter_enable", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_fir_filter_enable, 0, 1 }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, diff --git a/src/main/mw.c b/src/main/mw.c index 8acf582099..dc839a0267 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -812,7 +812,9 @@ void loop(void) dT = (float)cycleTime * 0.000001f; - if (currentProfile->pidProfile.gyro_cut_hz) { + if (currentProfile->pidProfile.gyro_fir_filter_enable) { + filterApply7TapFIR(gyroADC); + else if (currentProfile->pidProfile.gyro_cut_hz) { filterGyro(); }