diff --git a/src/main/config/config.c b/src/main/config/config.c index 25b8a64bc8..ffa89d85ff 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -171,9 +171,6 @@ 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; pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0119051a5c..78053f2f93 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -185,16 +185,6 @@ 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) { - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - else { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); - } - } - // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); @@ -296,16 +286,6 @@ 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) { - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - else { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); - } - } - delta = (gyroADC[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroADC[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; @@ -391,16 +371,6 @@ 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) { - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - else { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); - } - } - 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; @@ -520,16 +490,6 @@ 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) { - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - else { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); - } - } - delta = (gyroADC[axis] - lastGyro[axis]) / 4; lastGyro[axis] = gyroADC[axis]; deltaSum = delta1[axis] + delta2[axis] + delta; @@ -647,11 +607,6 @@ 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, dT); - } - delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS; lastGyro[axis] = gyroADCQuant; @@ -699,11 +654,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) } } - // Pterm low pass - if (pidProfile->pterm_cut_hz) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - axisPID[FD_YAW] = PTerm + ITerm; axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. @@ -793,16 +743,6 @@ 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) { - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], constrain(pidProfile->pterm_cut_hz / YAW_PTERM_CUT_RATIO, 1, pidProfile->pterm_cut_hz), dT); - } - else { - PTerm = filterApplyPt1(PTerm, &PTermState[axis], pidProfile->pterm_cut_hz, dT); - } - } - // -----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. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 80bcd6427a..cf30dbe1f7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -519,10 +519,7 @@ 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 }, - { "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 dc839a0267..ae7528cb45 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -688,21 +688,6 @@ void processRx(void) } -// Gyro Low Pass -void filterGyro(void) { - int axis; - static float dTGyro; - static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT]; - - if (!dTGyro) { - dTGyro = (float)targetLooptime * 0.000001f; - } - - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dTGyro); - } -} - void filterRc(void){ static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 }; @@ -812,11 +797,7 @@ void loop(void) dT = (float)cycleTime * 0.000001f; - if (currentProfile->pidProfile.gyro_fir_filter_enable) { - filterApply7TapFIR(gyroADC); - else if (currentProfile->pidProfile.gyro_cut_hz) { - filterGyro(); - } + filterApply7TapFIR(gyroADC); if (masterConfig.rcSmoothing) { filterRc();