From 568f8194ee730fa493aa020ca432c94a12e89c05 Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Sat, 8 Mar 2025 21:19:15 +0100
Subject: [PATCH] add a upsampler

---
 lib/filters.c | 22 ++++++++++++++++++++++
 lib/filters.h |  9 +++++++++
 src/fm95.c    | 21 ++++++++++++++++-----
 3 files changed, 47 insertions(+), 5 deletions(-)

diff --git a/lib/filters.c b/lib/filters.c
index 1efdc70..9d40585 100644
--- a/lib/filters.c
+++ b/lib/filters.c
@@ -31,6 +31,28 @@ float apply_biquad(BiquadFilter* filter, float input) {
     return out;
 }
 
+void init_upsampler(Upsampler* up, int ratio, float sample_rate) {
+    up->i = 0;
+    up->ratio = ratio;
+    init_lpf(&up->lpf, sample_rate*ratio, 0.70710678f, sample_rate);
+}
+
+float upsample(Upsampler* up, float sample) {
+    float output = 0.0f;
+
+    if (up->i == 0) {
+        output = sample * up->ratio;
+    } else {
+        output = 0.0f;
+    }
+    
+    output = apply_biquad(&up->lpf, output);
+    
+    up->i = (up->i + 1) % up->ratio;
+    
+    return output;
+}
+
 float hard_clip(float sample, float threshold) {
     if (sample > threshold) {
         return threshold;  // Clip to the upper threshold
diff --git a/lib/filters.h b/lib/filters.h
index d56788d..3a548c7 100644
--- a/lib/filters.h
+++ b/lib/filters.h
@@ -23,6 +23,15 @@ typedef struct {
 void init_lpf(BiquadFilter* filter, float cutoffFreq, float qFactor, float sampleRate);
 float apply_biquad(BiquadFilter* filter, float input);
 
+typedef struct
+{
+    int i;
+    int ratio;
+    BiquadFilter lpf;
+} Upsampler;
+void init_upsampler(Upsampler* up, int ratio, float sample_rate);
+float upsample(Upsampler *up, float sample);
+
 float hard_clip(float sample, float threshold);
 float voltage_db_to_voltage(float db);
 float power_db_to_voltage(float db);
diff --git a/src/fm95.c b/src/fm95.c
index 2f8008e..1d0ec7e 100644
--- a/src/fm95.c
+++ b/src/fm95.c
@@ -21,6 +21,7 @@
 #include "../lib/filters.h"
 #include "../lib/fm_modulator.h"
 
+#define INPUT_SAMPLE_RATE 32000 // make sure that the output is a multiple of this
 #define SAMPLE_RATE 192000
 
 #define INPUT_DEVICE "FM_Audio.monitor"
@@ -238,6 +239,12 @@ int main(int argc, char **argv) {
         .channels = 1,
         .rate = SAMPLE_RATE
     };
+    pa_sample_spec main_input_format = {
+        .format = PA_SAMPLE_FLOAT32NE,
+        .channels = 2,
+        .rate = INPUT_SAMPLE_RATE
+    };
+
 
     pa_buffer_attr input_buffer_atr = {
         .maxlength = buffer_maxlength,
@@ -259,7 +266,7 @@ int main(int argc, char **argv) {
         PA_STREAM_RECORD,
         audio_input_device,
         "Main Audio Input",
-        &stereo_format,
+        &main_input_format,
         NULL,
         &input_buffer_atr,
         &opentime_pulse_error
@@ -373,8 +380,12 @@ int main(int argc, char **argv) {
 
     // Use https://www.earlevel.com/main/2021/09/02/biquad-calculator-v3/
     BiquadFilter lpf_l, lpf_r;
-    init_lpf(&lpf_l, LPF_CUTOFF, 0.3535f, SAMPLE_RATE);
-    init_lpf(&lpf_r, LPF_CUTOFF, 0.3535f, SAMPLE_RATE);
+    init_lpf(&lpf_l, LPF_CUTOFF, 0.70710678f, SAMPLE_RATE);
+    init_lpf(&lpf_r, LPF_CUTOFF, 0.70710678f, SAMPLE_RATE);
+
+    Upsampler upsampler_l, upsampler_r;
+    init_upsampler(&upsampler_l, SAMPLE_RATE/INPUT_SAMPLE_RATE, SAMPLE_RATE);
+    init_upsampler(&upsampler_r, SAMPLE_RATE/INPUT_SAMPLE_RATE, SAMPLE_RATE);
     // #endregion
 
     signal(SIGINT, stop);
@@ -410,8 +421,8 @@ int main(int argc, char **argv) {
         }
 
         for (int i = 0; i < BUFFER_SIZE; i++) {
-            float l_in = left[i];
-            float r_in = right[i];
+            float l_in = upsample(&upsampler_l, left[i]);
+            float r_in = upsample(&upsampler_r, right[i]);
             float current_mpx_in = mpx_in[i];
             float current_sca_in = sca_in[i];