From 10f2d3575968f3023f6240b5faa9face9eb42947 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 10 Jun 2015 10:00:36 +0200 Subject: [PATCH] Soft Filtering (Gyro, Dterm, Pterm) pterm_cut_hz added Let's play with this as well to get more noise filtered Code Cleanup Make filter more flexible for reuse rewrite correction pterm Define static delta in filter Fix array count ident return function for filter Filter Function enhanced Full software filtering (DTerm, PTerm, Gyro, Acc) Normalize Variables Revert Back gyro settings Bugfix gyro/acc filter // (MPU60xx equalize lpf settings) Moved filtering to mw.c This has been done to prevent reusing old cycletime for filter function. acc_cut_hz removed (not needed) Harakiri zero fix --- Makefile | 1 + docs/Cli.md | 3 ++ src/main/config/config.c | 3 ++ src/main/drivers/accgyro_mpu6050.c | 8 ++- src/main/drivers/accgyro_spi_mpu6000.c | 43 ++++++--------- src/main/flight/filter.c | 27 ++++++++++ src/main/flight/filter.h | 11 ++++ src/main/flight/pid.c | 74 +++++++++++++++++++++++++- src/main/flight/pid.h | 5 +- src/main/io/serial_cli.c | 3 ++ src/main/mw.c | 11 ++++ 11 files changed, 156 insertions(+), 33 deletions(-) create mode 100644 src/main/flight/filter.c create mode 100644 src/main/flight/filter.h diff --git a/Makefile b/Makefile index 4dfb2432a5..529c4c0dc2 100644 --- a/Makefile +++ b/Makefile @@ -229,6 +229,7 @@ COMMON_SRC = build_config.c \ flight/imu.c \ flight/mixer.c \ flight/lowpass.c \ + flight/filter.c \ drivers/bus_i2c_soft.c \ drivers/serial.c \ drivers/sound_beeper.c \ diff --git a/docs/Cli.md b/docs/Cli.md index de9d817a1f..040b1cc2bf 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -246,6 +246,9 @@ Re-apply any new defaults as desired. | `p_vel` | | 0 | 200 | 120 | Profile | UINT8 | | `i_vel` | | 0 | 200 | 45 | Profile | UINT8 | | `d_vel` | | 0 | 200 | 1 | Profile | UINT8 | +| `dterm_cut_hz` | Lowpass cutoff filter for Dterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 | +| `pterm_cut_hz` | Lowpass cutoff filter for Pterm for all PID controllers | 0 | 200 | 0 | Profile | UINT8 | +| `gyro_cut_hz` | Lowpass cutoff filter for gyro input | 0 | 200 | 0 | Profile | UINT8 | | 0 | 200 | 0 | Profile | UINT8 | | `yaw_p_limit` | Limiter for yaw P term. This parameter is only affecting PID controller 3-5. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | `blackbox_rate_num` | | 1 | 32 | 1 | Master | UINT8 | | `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 | diff --git a/src/main/config/config.c b/src/main/config/config.c index 3f6da107be..2350b59083 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -171,6 +171,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; + pidProfile->dterm_cut_hz = 0; + pidProfile->pterm_cut_hz = 0; + pidProfile->gyro_cut_hz = 0; pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully pidProfile->I_f[ROLL] = 0.6f; diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 8c02ed7c5a..b67da25d2d 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -372,7 +372,9 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_ // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - if (lpf >= 188) + if (lpf == 256) + mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; + else if (lpf >= 188) mpuLowPassFilter = INV_FILTER_188HZ; else if (lpf >= 98) mpuLowPassFilter = INV_FILTER_98HZ; @@ -382,8 +384,10 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_ mpuLowPassFilter = INV_FILTER_20HZ; else if (lpf >= 10) mpuLowPassFilter = INV_FILTER_10HZ; - else + else if (lpf > 0) mpuLowPassFilter = INV_FILTER_5HZ; + else + mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; return true; } diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index e8f38548b2..56406ffaf9 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -275,33 +275,22 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) int16_t data[3]; // default lpf is 42Hz - switch (lpf) { - case 256: - mpuLowPassFilter = BITS_DLPF_CFG_256HZ; - break; - case 188: - mpuLowPassFilter = BITS_DLPF_CFG_188HZ; - break; - case 98: - mpuLowPassFilter = BITS_DLPF_CFG_98HZ; - break; - default: - case 42: - mpuLowPassFilter = BITS_DLPF_CFG_42HZ; - break; - case 20: - mpuLowPassFilter = BITS_DLPF_CFG_20HZ; - break; - case 10: - mpuLowPassFilter = BITS_DLPF_CFG_10HZ; - break; - case 5: - mpuLowPassFilter = BITS_DLPF_CFG_5HZ; - break; - case 0: - mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF; - break; - } + if (lpf == 256) + mpuLowPassFilter = BITS_DLPF_CFG_256HZ; + else if (lpf >= 188) + mpuLowPassFilter = BITS_DLPF_CFG_188HZ; + else if (lpf >= 98) + mpuLowPassFilter = BITS_DLPF_CFG_98HZ; + else if (lpf >= 42) + mpuLowPassFilter = BITS_DLPF_CFG_42HZ; + else if (lpf >= 20) + mpuLowPassFilter = BITS_DLPF_CFG_20HZ; + else if (lpf >= 10) + mpuLowPassFilter = BITS_DLPF_CFG_10HZ; + else if (lpf > 0) + mpuLowPassFilter = BITS_DLPF_CFG_5HZ; + else + mpuLowPassFilter = BITS_DLPF_CFG_256HZ; spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); diff --git a/src/main/flight/filter.c b/src/main/flight/filter.c new file mode 100644 index 0000000000..3a35f182de --- /dev/null +++ b/src/main/flight/filter.c @@ -0,0 +1,27 @@ +/* + * filter.c + * + * Created on: 24 jun. 2015 + * Author: borisb + */ + +#include +#include +#include +#include + +#include "common/axis.h" + +#include "flight/filter.h" + +extern uint16_t cycleTime; + +// PT1 Low Pass filter +float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut) { + float dT = (float)cycleTime * 0.000001f; + float RC= 1.0f / ( 2.0f * (float)M_PI * f_cut ); + + *state = *state + dT / (RC + dT) * (input - *state); + + return *state; +} diff --git a/src/main/flight/filter.h b/src/main/flight/filter.h new file mode 100644 index 0000000000..5782ba9bf2 --- /dev/null +++ b/src/main/flight/filter.h @@ -0,0 +1,11 @@ +/* + * filter.h + * + * Created on: 24 jun. 2015 + * Author: borisb + */ + + +typedef float filterStatePt1_t; + +float filterApplyPt1(float input, filterStatePt1_t* state, uint8_t f_cut); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 88f51fe3b9..e756500bd3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -42,6 +42,7 @@ #include "flight/imu.h" #include "flight/navigation.h" #include "flight/autotune.h" +#include "flight/filter.h" #include "config/runtime_config.h" @@ -93,6 +94,8 @@ void pidResetErrorGyro(void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; +static filterStatePt1_t PTermState[3], DTermState[3]; + #ifdef AUTOTUNE bool shouldAutotune(void) { @@ -184,6 +187,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } // -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10 errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f); @@ -202,6 +210,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa 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); + } + DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output @@ -282,11 +296,23 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } + delta = (gyroADC[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroADC[axis]; 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); + } + DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; @@ -361,12 +387,22 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm -= ((int32_t)(gyroADC[axis] / 4) * dynP8[axis]) >> 6; // 32 bits is needed for calculation + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } + delta = (gyroADC[axis] - lastGyro[axis]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyro[axis] = gyroADC[axis]; DTerm = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; + // Dterm low pass + if (pidProfile->dterm_cut_hz) { + DTerm = filterApplyPt1(DTerm, &DTermState[axis], pidProfile->dterm_cut_hz); + } + DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm - DTerm; @@ -474,11 +510,22 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con } PTerm -= ((int32_t)gyroADC[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } delta = (gyroADC[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroADC[axis]; 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); + } + DTerm = (deltaSum * dynD8[axis]) / 32; axisPID[axis] = PTerm + ITerm - DTerm; @@ -529,8 +576,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) uint8_t axis; float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; -// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D - MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now + MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz) FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element @@ -585,6 +631,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } PTerm -= gyroADCQuant * dynP8[axis] * 0.003f; + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } + delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS; lastGyro[axis] = gyroADCQuant; @@ -631,6 +683,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } } + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } + axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. @@ -720,6 +778,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Pterm low pass + if (pidProfile->pterm_cut_hz) { + PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz); + } + // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. @@ -743,6 +807,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat deltaSum = delta1[axis] + delta2[axis] + delta; delta2[axis] = delta1[axis]; delta1[axis] = delta; + + // Dterm delta low pass + if (pidProfile->dterm_cut_hz) { + deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz); + } + DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 609728ada7..3ec0004e96 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,7 +19,6 @@ #define GYRO_I_MAX 256 // Gyro I limiter #define RCconstPI 0.159154943092f // 0.5f / M_PI; -#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) #define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw. #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter @@ -63,6 +62,9 @@ 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 pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames + uint8_t gyro_cut_hz; // Used for soft gyro filtering } pidProfile_t; #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) @@ -75,4 +77,3 @@ void pidSetController(pidControllerType_e type); void pidResetErrorAngle(void); void pidResetErrorGyro(void); - diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index acb52c9ec8..68a1e9ff70 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -448,6 +448,9 @@ const clivalue_t valueTable[] = { { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, + { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 }, + { "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 }, + { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, #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 b58f1efe28..7156330d5a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -72,6 +72,7 @@ #include "flight/failsafe.h" #include "flight/autotune.h" #include "flight/navigation.h" +#include "flight/filter.h" #include "config/runtime_config.h" @@ -740,6 +741,16 @@ void loop(void) cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; + // Gyro Low Pass + if (currentProfile->pidProfile.gyro_cut_hz) { + int axis; + static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT]; + + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz); + } + } + annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true;