Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

Static FIR filter

Browse source
This commit is contained in:
borisbstyle 2015-09-19 00:43:50 +02:00
parent 7a2762dbf6
commit 49bc46bc37
4 changed files with 1 additions and 86 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/config/config.c
View file

@ -171,9 +171,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; 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->dterm_cut_hz = 17;
pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully

60
src/main/flight/pid.c
View file

@ -185,16 +185,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate P component // -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; 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. // -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); 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 -= ((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; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta; 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 -= ((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 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]; lastGyro[axis] = gyroADC[axis];
DTerm = delta1[axis] + delta2[axis] + delta; 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 -= ((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; delta = (gyroADC[axis] - lastGyro[axis]) / 4;
lastGyro[axis] = gyroADC[axis]; lastGyro[axis] = gyroADC[axis];
deltaSum = delta1[axis] + delta2[axis] + delta; deltaSum = delta1[axis] + delta2[axis] + delta;
@ -647,11 +607,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
PTerm -= gyroADCQuant * dynP8[axis] * 0.003f; 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; delta = (gyroADCQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroADCQuant; 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] = PTerm + ITerm;
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result. 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 // -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; 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 // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // 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. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.

3
src/main/io/serial_cli.c
View file

@ -519,10 +519,7 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, { "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 }, { "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 }, { "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 #ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },

21
src/main/mw.c
View file

@ -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){ void filterRc(void){
static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[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; dT = (float)cycleTime * 0.000001f;
if (currentProfile->pidProfile.gyro_fir_filter_enable) { filterApply7TapFIR(gyroADC);
filterApply7TapFIR(gyroADC);
else if (currentProfile->pidProfile.gyro_cut_hz) {
filterGyro();
}
if (masterConfig.rcSmoothing) { if (masterConfig.rcSmoothing) {
filterRc(); filterRc();