diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 85eee87b70..f37196710d 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -34,18 +34,16 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float * 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz * 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz */ -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 +static int8_t gyroFIRCoeff_1000[7] = { 12, 23, 40, 51, 52, 40, 38 }; // 1khz; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132hz -int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level) +int8_t * filterGetFIRCoefficientsTable(void) { - return gyroFIRCoeff_1000[filter_level-1]; + return gyroFIRCoeff_1000; } // 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]) +void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7]) { int32_t FIRsum; int axis, i; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 6c4402d59e..1406ec55c5 100644 --- a/src/main/common/filter.h +++ b/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 filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]); +int8_t * filterGetFIRCoefficientsTable(void); +void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7]); diff --git a/src/main/config/config.c b/src/main/config/config.c index 0ac9c17d90..c0b6f337a2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -175,7 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; - pidProfile->gyro_soft_lpf = 1; // LOW filtering by default + pidProfile->gyro_soft_lpf = 1; // filtering ON by default pidProfile->dterm_cut_hz = 40; pidProfile->yaw_pterm_cut_hz = 50; @@ -695,7 +695,7 @@ void activateConfig(void) ); if (currentProfile->pidProfile.gyro_soft_lpf) { - useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf)); + useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable()); // Leave this for more coefficients in the future } #ifdef TELEMETRY diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c1783bb39e..af8b65d783 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -347,10 +347,6 @@ static const char * const lookupTableSerialRX[] = { "XB-B-RJ01" }; -static const char * const lookupTableGyroFilter[] = { - "OFF", "LOW", "MEDIUM", "HIGH" -}; - static const char * const lookupTableGyroLpf[] = { "OFF", "188HZ", @@ -378,7 +374,6 @@ typedef enum { TABLE_GIMBAL_MODE, TABLE_PID_CONTROLLER, TABLE_SERIAL_RX, - TABLE_GYRO_FILTER, TABLE_GYRO_LPF, } lookupTableIndex_e; @@ -395,7 +390,6 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, - { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) } }; @@ -651,7 +645,7 @@ 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 } }, - { "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_GYRO_FILTER } }, + { "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } }, { "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 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fcf1bdc0a6..e6ee7f0162 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -39,7 +39,7 @@ 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]; +static int16_t gyroFIRState[3][7]; gyro_t gyro; // gyro access functions sensor_align_e gyroAlign = 0; @@ -126,7 +126,7 @@ void gyroUpdate(void) } if (gyroFIRTable) { - filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable); + filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable); } alignSensors(gyroADC, gyroADC, gyroAlign);