Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Lowpass filter tested

Browse source
This commit is contained in:
Joel Fuster 2015-01-02 16:59:42 -05:00
parent 3eee9eb079
commit a3cc92347b
3 changed files with 117 additions and 114 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/flight/mixer.c Normal file → Executable file
View file

@ -714,28 +714,29 @@ static float lowpass(lowpass_t *filter, float in, int16_t freqIdx )
if (!filter->init) {
filter->freqIdx = freqIdx;
filter->freq = lowpass_table[filter->freqIdx][0];
filter->pCoef = &lowpass_table[filter->freqIdx][1];
filter->b = &lowpass_table[filter->freqIdx][1];
filter->a = &lowpass_table[filter->freqIdx][1 + LOWPASS_NUM_COEF];
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
filter->in[coefIdx] = in;
filter->out[coefIdx] = in;
filter->x[coefIdx] = in;
filter->y[coefIdx] = in;
}
filter->init = true;
}
// Delays
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
filter->in[coefIdx] = filter->in[coefIdx-1];
filter->out[coefIdx] = filter->out[coefIdx-1];
filter->x[coefIdx] = filter->x[coefIdx-1];
filter->y[coefIdx] = filter->y[coefIdx-1];
}
filter->in[0] = in;
filter->x[0] = in;
// Accumulate result
out = filter->in[0] * filter->pCoef[0];
out = filter->x[0] * filter->b[0];
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
out += filter->in[coefIdx] * filter->pCoef[coefIdx];
out -= filter->out[coefIdx] * filter->pCoef[coefIdx + LOWPASS_NUM_COEF - 1];
out += filter->x[coefIdx] * filter->b[coefIdx];
out -= filter->y[coefIdx] * filter->a[coefIdx-1];
}
filter->out[0] = out;
filter->y[0] = out;
return out;
}
@ -747,6 +748,8 @@ void filterServos(void)
if (mixerConfig->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = (int16_t)lowpass(&lowpassFilters[servoIdx], (float)servo[servoIdx], mixerConfig->servo_lowpass_freq_idx);
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
}
}
}