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;