From e490564815120e3700a2ae1db28b1c3b7bf1709c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 10 Dec 2015 01:44:05 +0100 Subject: [PATCH] More Filter Config Options refactor Bump EEPROM Version --- src/main/common/filter.c | 2 +- src/main/config/config.c | 10 ++++++---- src/main/flight/pid.c | 40 ++++++++++++++++++++++++++++++---------- src/main/io/serial_cli.c | 16 +++++++++------- 4 files changed, 46 insertions(+), 22 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2de5e965d..85eee87b7 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -40,7 +40,7 @@ static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 }, int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level) { - return gyroFIRCoeff_1000[filter_level]; + return gyroFIRCoeff_1000[filter_level-1]; } // 9 Tap FIR filter as described here: diff --git a/src/main/config/config.c b/src/main/config/config.c index 0486db399..0ac9c17d9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 115; +static const uint8_t EEPROM_CONF_VERSION = 116; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -175,7 +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->gyro_soft_lpf = 1; // LOW filtering by default pidProfile->dterm_cut_hz = 40; pidProfile->yaw_pterm_cut_hz = 50; @@ -399,7 +399,7 @@ static void resetConf(void) masterConfig.current_profile_index = 0; // default profile masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 - masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ + masterConfig.gyro_lpf = 1; // 188HZ resetAccelerometerTrims(&masterConfig.accZero); @@ -694,7 +694,9 @@ void activateConfig(void) ¤tProfile->pidProfile ); - useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf)); + if (currentProfile->pidProfile.gyro_soft_lpf) { + useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf)); + } #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4169cca1a..f6477a1bb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -96,7 +96,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa float ITerm,PTerm,DTerm; int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastError[3]; - float delta; + static float delta1[3], delta2[3]; + float delta, deltaSum; int axis; float horizonLevelStrength = 1; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; @@ -198,12 +199,21 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // would be scaled by different dt each time. Division by dT fixes that. delta *= (1.0f / dT); - // Dterm low pass - if (pidProfile->dterm_cut_hz) { - delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); + if (!pidProfile->gyro_soft_lpf) { + // add moving average here to reduce noise + deltaSum = (delta1[axis] + delta2[axis] + delta) / 3; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + } else { + deltaSum = delta; } - DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); + // Dterm low pass + if (pidProfile->dterm_cut_hz) { + deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); + } + + DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); @@ -229,7 +239,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat int32_t errorAngle; int axis; - int32_t delta; + int32_t delta, deltaSum; + static int32_t delta1[3], delta2[3]; int32_t PTerm, ITerm, DTerm; static int32_t lastError[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; @@ -335,12 +346,21 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // would be scaled by different dt each time. Division by dT fixes that. delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; - // Dterm delta low pass - if (pidProfile->dterm_cut_hz) { - delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); + if (!pidProfile->gyro_soft_lpf) { + // add moving average here to reduce noise + deltaSum = delta1[axis] + delta2[axis] + delta; + delta2[axis] = delta1[axis]; + delta1[axis] = delta; + } else { + deltaSum = delta * 2; } - DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Multiplied by 2 to approximately match old scaling + // Dterm delta low pass + if (pidProfile->dterm_cut_hz) { + deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT); + } + + DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8f0fc6f41..c1783bb39 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -348,12 +348,14 @@ static const char * const lookupTableSerialRX[] = { }; static const char * const lookupTableGyroFilter[] = { - "LOW", "MEDIUM", "HIGH" + "OFF", "LOW", "MEDIUM", "HIGH" }; -static const char * const lookupTableGyroSampling[] = { - "8KHZ", - "1KHZ" +static const char * const lookupTableGyroLpf[] = { + "OFF", + "188HZ", + "98HZ", + "42HZ" }; typedef struct lookupTableEntry_s { @@ -377,7 +379,7 @@ typedef enum { TABLE_PID_CONTROLLER, TABLE_SERIAL_RX, TABLE_GYRO_FILTER, - TABLE_GYRO_SAMPLING, + TABLE_GYRO_LPF, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -394,7 +396,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, - { lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) } + { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -545,7 +547,7 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - { "gyro_sampling", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_SAMPLING } }, + { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },