From cded901e59439cde5e215233947568505dfb0ab9 Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Mon, 30 Dec 2024 19:27:52 +0100 Subject: [PATCH] add a lpf --- stereo_coder.c | 80 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 68 insertions(+), 12 deletions(-) diff --git a/stereo_coder.c b/stereo_coder.c index bf8248c..e7fe5af 100644 --- a/stereo_coder.c +++ b/stereo_coder.c @@ -8,6 +8,7 @@ // Features // #define PREEMPHASIS +#define LPF #define SAMPLE_RATE 192000 // Don't go lower than 108 KHz, becuase it (53000*2) and (38000+15000) @@ -24,6 +25,10 @@ #define PREEMPHASIS_TAU 0.00005 // 50 microseconds, use 0.000075 if in america #endif +#ifdef LPF +#define LPF_CUTOFF 15000 +#endif + volatile sig_atomic_t to_run = 1; float clip(float sample) { @@ -44,7 +49,10 @@ void uninterleave(const float *input, float *left, float *right, size_t num_samp } } +#define FIR_PHASES 32 +#define FIR_TAPS 32 +#define PI 3.14159265358979323846 #define M_2PI (3.14159265358979323846 * 2.0) // Track phase continuously to maintain frequency accuracy @@ -68,18 +76,14 @@ float get_next_sample(Oscillator *osc) { } #ifdef PREEMPHASIS - typedef struct { - float prev_sample; float alpha; float a0, a1, b0; float x1, y1; } PreEmphasis; -#define FIR_PHASES 32 // IIR pre-emphasis from pifmrds void init_pre_emphasis(PreEmphasis *pe) { - pe->prev_sample = 0.0f; pe->x1 = 0.0f; pe->y1 = 0.0f; @@ -94,24 +98,55 @@ void init_pre_emphasis(PreEmphasis *pe) { pe->a0 = (2.0f*ap+1.0/(SAMPLE_RATE*FIR_PHASES))/(2.0*bp+1.0/(SAMPLE_RATE*FIR_PHASES)); pe->a1 = (-2.0f*ap+1.0/(SAMPLE_RATE*FIR_PHASES))/(2.0*bp+1.0/(SAMPLE_RATE*FIR_PHASES)); pe->b0 = (2.0f*ap-1.0/(SAMPLE_RATE*FIR_PHASES))/(2.0*bp+1.0/(SAMPLE_RATE*FIR_PHASES)); - - // Calculate simple preemphasis coefficient - pe->alpha = exp(-1 / (PREEMPHASIS_TAU*SAMPLE_RATE)); } float apply_pre_emphasis(PreEmphasis *pe, float sample) { - // Simple preemphasis - float output = sample - pe->alpha * pe->prev_sample; - pe->prev_sample = output; - // IIR filtering float y = pe->a0 * sample + pe->a1 * pe->x1 - pe->b0 * pe->y1; pe->x1 = sample; pe->y1 = y; - return y; + return y/4; //its so loud } #endif + +#ifdef LPF +typedef struct { + float low_pass_fir[FIR_PHASES][FIR_TAPS]; + float sample_buffer[FIR_TAPS]; + int buffer_index; +} LowPassFilter; + +void init_low_pass_filter(LowPassFilter *lp) { + for (int i = 0; i < FIR_TAPS; i++) { + for (int j = 0; j < FIR_PHASES; j++) { + int mi = i * FIR_PHASES + j + 1; + float sincpos = mi - (((FIR_TAPS * FIR_PHASES) + 1.0f) / 2.0f); + float firlowpass = (sincpos == 0.0f) ? 1.0f : sinf(M_2PI * LPF_CUTOFF * sincpos / SAMPLE_RATE) / (PI * sincpos); + float window = 0.54f - 0.46f * cosf(M_2PI * mi / (FIR_TAPS * FIR_PHASES)); // Hamming window + lp->low_pass_fir[j][i] = firlowpass * window; + } + } + memset(lp->sample_buffer, 0, sizeof(lp->sample_buffer)); + lp->buffer_index = 0; +} + +float apply_low_pass_filter(LowPassFilter *lp, float sample) { + // Update the sample buffer + lp->sample_buffer[lp->buffer_index] = sample; + lp->buffer_index = (lp->buffer_index + 1) % FIR_TAPS; + + // Apply the filter + float result = 0.0f; + int index = lp->buffer_index; + for (int i = 0; i < FIR_TAPS; i++) { + result += lp->low_pass_fir[0][i] * lp->sample_buffer[index]; + index = (index + 1) % FIR_TAPS; + } + return result; +} +#endif + static void stop(int signum) { (void)signum; printf("\nReceived stop signal. Cleaning up...\n"); @@ -190,6 +225,11 @@ int main() { init_pre_emphasis(&preemp_l); init_pre_emphasis(&preemp_r); #endif +#ifdef LPF + LowPassFilter lpf_l, lpf_r; + init_low_pass_filter(lpf_l); + init_low_pass_filter(lpf_r); +#endif signal(SIGINT, stop); signal(SIGTERM, stop); @@ -211,13 +251,29 @@ int main() { float r_in = right[i]; #ifdef PREEMPHASIS +#ifdef LPF + float lowpassed_left = apply_low_pass_filter(&lpf_l, l_in); + float lowpassed_right = apply_low_pass_filter(&lpf_r, r_in); + float preemphasized_left = apply_pre_emphasis(&preemp_l, lowpassed_left); + float preemphasized_right = apply_pre_emphasis(&preemp_r, lowpassed_right); + float current_left_input = clip(preemphasized_left); + float current_right_input = clip(preemphasized_right); +#else float preemphasized_left = apply_pre_emphasis(&preemp_l, l_in); float preemphasized_right = apply_pre_emphasis(&preemp_r, r_in); float current_left_input = clip(preemphasized_left); float current_right_input = clip(preemphasized_right); +#endif +#else +#ifdef LPF + float lowpassed_left = apply_low_pass_filter(&lpf_l, l_in); + float lowpassed_right = apply_low_pass_filter(&lpf_r, r_in); + float current_left_input = clip(lowpassed_left); + float current_right_input = clip(lowpassed_right); #else float current_left_input = clip(l_in); float current_right_input = clip(r_in); +#endif #endif float mono = (current_left_input + current_right_input) / 2.0f;