diff --git a/lib/filters.c b/lib/filters.c index c5d978b..91cd964 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -15,46 +15,26 @@ float hard_clip(float sample, float threshold) { return fmaxf(-threshold, fminf(threshold, sample)); } -void init_lpf(Biquad* filter, float sample_rate, float cutoff_freq, float Q) { - float omega = 2.0f * M_PI * cutoff_freq / sample_rate; - float alpha = sinf(omega) / (2.0f * Q); - float cos_omega = cosf(omega); +void init_lpf(FIR *filter, float cutoff, int sample_rate) { + float a = tanf(M_PI*cutoff/sample_rate); + float a2 = a*a; + float r; - float norm = 1.0f / (1.0f + alpha); - filter->b0 = (1.0f - cos_omega) * 0.5f * norm; - filter->b1 = (1.0f - cos_omega) * norm; - filter->b2 = filter->b0; - filter->a1 = -2.0f * cos_omega * norm; - filter->a2 = (1.0f - alpha) * norm; - - filter->x1 = filter->x2 = 0.0f; - filter->y1 = filter->y2 = 0.0f; + for(int i = 0; i < FIR_ORDER; i++) { + r = sinf(M_PI*(2.0f*i+1.0f)/(4.0f*FIR_ORDER)); + sample_rate = a2+2.0f*a*r+1.0f; + filter->A[i] = a2 / sample_rate; + filter->d1[i] = 2.0f*(1.0f-a2)/sample_rate; + filter->d2[i] = -(a2 - 2.0f * a * r + 1.0f) / sample_rate; + } } -void init_lpf4(LPF4* filter, float sample_rate, float cutoff_freq) { - float Q1 = 1.0f / (2.0f * cosf(M_PI / 8.0f)); - init_lpf(&filter->section1, sample_rate, cutoff_freq, Q1); - - float Q2 = 1.0f / (2.0f * cosf(3.0f * M_PI / 8.0f)); - init_lpf(&filter->section2, sample_rate, cutoff_freq, Q2); -} -float biquad(Biquad *filter, float input) { - float output = filter->b0 * input - + filter->b1 * filter->x1 - + filter->b2 * filter->x2 - - filter->a1 * filter->y1 - - filter->a2 * filter->y2; - - filter->x2 = filter->x1; - filter->x1 = input; - - filter->y2 = filter->y1; - filter->y1 = output; - - return output; -} -float apply_lpf4(LPF4* filter, float input) { - float output = biquad(&filter->section1, input); - output = biquad(&filter->section2, output); - return output; +float process_lpf(FIR *filter, float x) { + for(int i = 0; i < FIR_ORDER; i++) { + filter->w0[i] = filter->d1[i]*filter->w1[i] + filter->d2[i]*filter->w2[i] + x; + x = filter->A[i] * (filter->w0[i] + 2.0f * filter->w1[i] + filter->w2[i]); + filter->w2[i] = filter->w1[i]; + filter->w1[i] = filter->w0[i]; + } + return x; } \ No newline at end of file diff --git a/lib/filters.h b/lib/filters.h index 7da6657..5cd620d 100644 --- a/lib/filters.h +++ b/lib/filters.h @@ -5,6 +5,8 @@ #include "constants.h" #include "oscillator.h" +#define FIR_ORDER 10 + typedef struct { float alpha; @@ -19,16 +21,12 @@ float hard_clip(float sample, float threshold); typedef struct { - float b0, b1, b2; - float a1, a2; - float x1, x2; - float y1, y2; -} Biquad; -typedef struct { - Biquad section1; - Biquad section2; -} LPF4; -void init_lpf(Biquad* filter, float sample_rate, float cutoff_freq, float Q); -void init_lpf4(LPF4* filter, float sample_rate, float cutoff_freq); -float apply_lpf4(LPF4* filter, float input); -float biquad(Biquad *filter, float input); \ No newline at end of file + float A[FIR_ORDER]; + float d1[FIR_ORDER]; + float d2[FIR_ORDER]; + float w0[FIR_ORDER]; + float w1[FIR_ORDER]; + float w2[FIR_ORDER]; +} FIR; +void init_lpf(FIR *filter, float cutoff, int sample_rate); +float process_lpf(FIR *filter, float x); \ No newline at end of file diff --git a/src/fm95.c b/src/fm95.c index b885021..00c4512 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -414,9 +414,9 @@ int main(int argc, char **argv) { init_preemphasis(&preemp_l, preemphasis_tau, sample_rate); init_preemphasis(&preemp_r, preemphasis_tau, sample_rate); - LPF4 lpf_l, lpf_r; - init_lpf4(&lpf_l, sample_rate, 15000); - init_lpf4(&lpf_r, sample_rate, 15000); + FIR lpf_l, lpf_r; + init_lpf(&lpf_l, 15000, sample_rate); + init_lpf(&lpf_r, 15000, sample_rate); signal(SIGINT, stop); signal(SIGTERM, stop); @@ -473,8 +473,8 @@ int main(int argc, char **argv) { float ready_l = apply_preemphasis(&preemp_l, l_in); float ready_r = apply_preemphasis(&preemp_r, r_in); - ready_l = apply_lpf4(&lpf_l, ready_l); - ready_r = apply_lpf4(&lpf_r, ready_r); + ready_l = process_lpf(&lpf_l, ready_l); + ready_r = process_lpf(&lpf_r, ready_r); ready_l = hard_clip(ready_l*audio_volume, clipper_threshold); ready_r = hard_clip(ready_r*audio_volume, clipper_threshold);