diff --git a/lib/filters.c b/lib/filters.c index b60ece3..1a1df56 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -15,26 +15,28 @@ float hard_clip(float sample, float threshold) { return fmaxf(-threshold, fminf(threshold, sample)); } -void init_butterworth_lpf(Biquad *filter, float cutoff_freq, float sample_rate) { - float omega = M_2PI * cutoff_freq / sample_rate; - float Q = 1.0f / sqrtf(2.0f); - float alpha = sinf(omega) / (2.0f * Q); - - float b0 = (1.0f - cosf(omega)) / 2.0f; - float b1 = 1.0f - cosf(omega); - float b2 = (1.0f - cosf(omega)) / 2.0f; - float a0 = 1.0f + alpha; - float a1 = -2.0f * cosf(omega); - float a2 = 1.0f - alpha; - - filter->b0 = b0 / a0; - filter->b1 = b1 / a0; - filter->b2 = b2 / a0; - filter->a1 = a1 / a0; - filter->a2 = a2 / a0; - - filter->x1 = filter->x2 = 0.0f; - filter->y1 = filter->y2 = 0.0f; +void init_chebyshev_lpf(Biquad* filter, float cutoff_freq, float sample_rate, float ripple_db) { + float Wn = M_2PI * cutoff_freq / sample_rate; + float eps = sqrtf(powf(10.0f, ripple_db / 10.0f) - 1.0f); + int n = 2; + float gamma = (2.0f * eps) / (n + 1.0f); + float phi = acoshf(1.0f / gamma); + float sin_phi = sinhf(phi / n); + float cos_phi = coshf(phi / n); + float alpha = sin_phi * cosf(Wn); + float beta = sin_phi * sinf(Wn); + + float d = 1.0f + 2.0f * alpha + (alpha * alpha + beta * beta); + + filter->b0 = (1.0f - (alpha * alpha + beta * beta)) / d; + filter->b1 = 2.0f * ((alpha * alpha + beta * beta) - 1.0f) / d; + filter->b2 = (1.0f - 2.0f * alpha + (alpha * alpha + beta * beta)) / d; + + filter->a1 = -2.0f * (1.0f + 2.0f * alpha - (alpha * alpha + beta * beta)) / d; + filter->a2 = (1.0f - 2.0f * alpha + (alpha * alpha + beta * beta)) / d; + + filter->x1 = filter->x2 = 0.0f; + filter->y1 = filter->y2 = 0.0f; } float biquad(Biquad *filter, float input) { diff --git a/lib/filters.h b/lib/filters.h index 188b703..7a9845d 100644 --- a/lib/filters.h +++ b/lib/filters.h @@ -24,5 +24,5 @@ typedef struct float x1, x2; float y1, y2; } Biquad; -void init_butterworth_lpf(Biquad *filter, float cutoff_freq, float sample_rate); +void init_chebyshev_lpf(Biquad* filter, float cutoff_freq, float sample_rate, float ripple_db); float biquad(Biquad *filter, float input); \ No newline at end of file diff --git a/src/fm95.c b/src/fm95.c index 8ea0a60..d533afe 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -414,11 +414,9 @@ int main(int argc, char **argv) { init_preemphasis(&preemp_l, preemphasis_tau, sample_rate); init_preemphasis(&preemp_r, preemphasis_tau, sample_rate); - Biquad lpf1_l, lpf1_r, lpf2_l, lpf2_r; - init_butterworth_lpf(&lpf1_l, 15000, 192000); - init_butterworth_lpf(&lpf1_r, 15000, 192000); - init_butterworth_lpf(&lpf2_l, 15000, 192000); - init_butterworth_lpf(&lpf2_r, 15000, 192000); + Biquad lpf_l, lpf_r; + init_chebyshev_lpf(&lpf_l, 15000, sample_rate, 1.0f); + init_chebyshev_lpf(&lpf_r, 15000, sample_rate, 1.0f); signal(SIGINT, stop); signal(SIGTERM, stop); @@ -475,10 +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 = biquad(&lpf1_l, ready_l); - ready_l = biquad(&lpf1_r, ready_r); - ready_l = biquad(&lpf2_l, ready_l); - ready_l = biquad(&lpf2_r, ready_r); + ready_l = biquad(&lpf_l, ready_l); + ready_l = biquad(&lpf_r, ready_r); ready_l = hard_clip(ready_l*audio_volume, clipper_threshold); ready_r = hard_clip(ready_r*audio_volume, clipper_threshold);