From 7ff33aa5a71536ef149f2ba1f0830e2de4b8c0fd Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Thu, 27 Mar 2025 18:00:05 +0100
Subject: [PATCH] try to add a lpf

---
 .vscode/.server-controller-port.log |  2 +-
 lib/filters.c                       | 38 +++++++++++++++++++++++++++++
 lib/filters.h                       | 12 ++++++++-
 src/fm95.c                          |  8 ++++--
 4 files changed, 56 insertions(+), 4 deletions(-)

diff --git a/.vscode/.server-controller-port.log b/.vscode/.server-controller-port.log
index 26b40ed..fa21837 100644
--- a/.vscode/.server-controller-port.log
+++ b/.vscode/.server-controller-port.log
@@ -1,5 +1,5 @@
 {
   "port": 13452,
-  "time": 1743019078692,
+  "time": 1743094558477,
   "version": "0.0.3"
 }
\ No newline at end of file
diff --git a/lib/filters.c b/lib/filters.c
index 44b4526..b60ece3 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -13,4 +13,42 @@ float apply_preemphasis(ResistorCapacitor *filter, float sample) {
 
 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;
+}
+
+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;
 }
\ No newline at end of file
diff --git a/lib/filters.h b/lib/filters.h
index d2ea52a..188b703 100644
--- a/lib/filters.h
+++ b/lib/filters.h
@@ -15,4 +15,14 @@ typedef struct
 void init_preemphasis(ResistorCapacitor *filter, float tau, float sample_rate);
 float apply_preemphasis(ResistorCapacitor *filter, float sample);
 
-float hard_clip(float sample, float threshold);
\ No newline at end of file
+float hard_clip(float sample, float threshold);
+
+typedef struct
+{
+	float b0, b1, b2;
+	float a1, a2;
+	float x1, x2;
+	float y1, y2;
+} Biquad;
+void init_butterworth_lpf(Biquad *filter, float cutoff_freq, float sample_rate);
+float biquad(Biquad *filter, float input);
\ No newline at end of file
diff --git a/src/fm95.c b/src/fm95.c
index 484bcf2..7e06252 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -404,7 +404,6 @@ int main(int argc, char **argv) {
 		return 0;
 	}
 
-	// #region Setup Filters/Modulaltors/Oscillators
 	Oscillator osc;
 	init_oscillator(&osc, polar_stereo ? 31250.0 : 4750, sample_rate);
 
@@ -414,7 +413,10 @@ int main(int argc, char **argv) {
 	ResistorCapacitor preemp_l, preemp_r;
 	init_preemphasis(&preemp_l, preemphasis_tau, sample_rate);
 	init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
-	// #endregion
+
+	Biquad lpf_l, lpf_r;
+	init_butterworth_lpf(&lpf_l, 15000, 192000);
+	init_butterworth_lpf(&lpf_r, 15000, 192000);
 
 	signal(SIGINT, stop);
 	signal(SIGTERM, stop);
@@ -471,6 +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(&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);