From 9b4cec73c9229201d05e24cd9535755ef26ca758 Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Fri, 31 Jan 2025 17:40:47 +0100 Subject: [PATCH] change frequency filters implemententaton --- .vscode/.server-controller-port.log | 2 +- lib/filters.c | 121 ++++++++++++---------------- lib/filters.h | 12 +-- src/fm95.c | 20 ++--- 4 files changed, 66 insertions(+), 89 deletions(-) diff --git a/.vscode/.server-controller-port.log b/.vscode/.server-controller-port.log index ffdf8bd..9b0e698 100644 --- a/.vscode/.server-controller-port.log +++ b/.vscode/.server-controller-port.log @@ -1,5 +1,5 @@ { "port": 13452, - "time": 1738336716329, + "time": 1738341533118, "version": "0.0.3" } \ No newline at end of file diff --git a/lib/filters.c b/lib/filters.c index 1ffb989..1712a77 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -48,80 +48,63 @@ float apply_preemphasis(BiquadFilter *filter, float input) { return output; } -void init_lpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate) { - float nyquist = sampleRate / 2.0f; - float fc = cutoffFreq / nyquist; - - // Blackman window for sharp transition - for (int n = 0; n < FILTER_TAPS; n++) { - float m = n - (FILTER_TAPS - 1.0f) / 2.0f; - - // Sinc function - float sinc = (m == 0) ? 1.0f : sinf(M_PI * m * fc) / (M_PI * m); - - // Blackman window - float window = 0.42f - 0.5f * cosf(2.0f * M_PI * n / (FILTER_TAPS - 1)) - + 0.08f * cosf(4.0f * M_PI * n / (FILTER_TAPS - 1)); - - filter->coeffs[n] = sinc * window; - } - - // Normalize - float sum = 0; - for (int i = 0; i < FILTER_TAPS; i++) sum += filter->coeffs[i]; - for (int i = 0; i < FILTER_TAPS; i++) filter->coeffs[i] /= sum; - - // Clear delay line - memset(filter->delay, 0, sizeof(filter->delay)); - filter->index = 0; +void init_lpf(BiquadFilter* filter, float cutoffFreq, float qFactor, float sampleRate) { + float x = (cutoffFreq * M_2PI) / sampleRate; + float sinX = sin(x); + float y = sinX / (qFactor*2.0f); + float cosX = cos(x); + float z = (1.0f-cosX)/2.0f; + + float _a0 = y + 1.0f; + float _a1 = cosX * -2.0f; + float _a2 = 1.0f - y; + float _b0 = z; + float _b1 = 1.0f - cosX; + float _b2 = z; + + filter->y2 = 0; + filter->y1 = 0; + filter->x2 = 0; + filter->x1 = 0; + filter->b0 = _b0/_a0; + filter->b1 = _b1/_a0; + filter->b2 = _b2/_a0; + filter->a1 = -_a1/_a0; + filter->a2 = -_a2/_a0; } -void init_hpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate) { - float nyquist = sampleRate / 2.0f; - float fc = cutoffFreq / nyquist; - - // Blackman window for sharp transition - for (int n = 0; n < FILTER_TAPS; n++) { - float m = n - (FILTER_TAPS - 1.0f) / 2.0f; - - // Sinc function - float sinc = (m == 0) ? 1.0f : -sinf(M_PI * m * fc) / (M_PI * m); - - // Blackman window - float window = 0.42f - 0.5f * cosf(2.0f * M_PI * n / (FILTER_TAPS - 1)) - + 0.08f * cosf(4.0f * M_PI * n / (FILTER_TAPS - 1)); - - filter->coeffs[n] = sinc * window; - } +void init_hpf(BiquadFilter* filter, float cutoffFreq, float qFactor, float sampleRate) { + float x = (cutoffFreq * M_2PI) / sampleRate; + float sinX = sin(x); + float y = sinX / (qFactor*2.0f); + float cosX = cos(x); + float z = (1.0f-cosX)/2.0f; - filter->coeffs[FILTER_TAPS/2] += 1.0f; - - // Normalize - float sum = 0; - for (int i = 0; i < FILTER_TAPS; i++) sum += filter->coeffs[i]; - for (int i = 0; i < FILTER_TAPS; i++) filter->coeffs[i] /= sum; - - // Clear delay line - memset(filter->delay, 0, sizeof(filter->delay)); - filter->index = 0; + float _a0 = y + 1.0f; + float _a1 = cosX * -2.0f; + float _a2 = 1.0f - y; + float _b0 = 1.0f - z; + float _b1 = cosX * -2.0f; + float _b2 = 1.0f - z; + + filter->y2 = 0; + filter->y1 = 0; + filter->x2 = 0; + filter->x1 = 0; + filter->b0 = _b0/_a0; + filter->b1 = _b1/_a0; + filter->b2 = _b2/_a0; + filter->a1 = -_a1/_a0; + filter->a2 = -_a2/_a0; } -float apply_frequency_filter(FrequencyFilter* filter, float input) { - // Shift delay line - filter->delay[filter->index] = input; - - // Compute output - float output = 0; - int j = filter->index; - for (int i = 0; i < FILTER_TAPS; i++) { - output += filter->coeffs[i] * filter->delay[j]; - j = (j + 1) % FILTER_TAPS; - } - - // Update index - filter->index = (filter->index + FILTER_TAPS - 1) % FILTER_TAPS; - - return output; +float apply_frequency_filter(BiquadFilter* filter, float input) { + float out = input*filter->b0+filter->x1*filter->b1+filter->x2*filter->b2+filter->y1*filter->a1+filter->y2*filter->a2; + filter->y2 = filter->y1; + filter->y1 = out; + filter->x2 = filter->x1; + filter->x1 = input; + return out; } float hard_clip(float sample, float threshold) { diff --git a/lib/filters.h b/lib/filters.h index dbb2035..89d01ba 100644 --- a/lib/filters.h +++ b/lib/filters.h @@ -15,15 +15,9 @@ typedef struct { void init_preemphasis(BiquadFilter *filter, float tau, float sample_rate); float apply_preemphasis(BiquadFilter *filter, float input); -typedef struct { - float coeffs[FILTER_TAPS]; - float delay[FILTER_TAPS]; - int index; -} FrequencyFilter; - -void init_lpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate); -void init_hpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate); -float apply_frequency_filter(FrequencyFilter* filter, float input); +void init_lpf(BiquadFilter* filter, float cutoffFreq, float qFactor, float sampleRate); +void init_hpf(BiquadFilter* filter, float cutoffFreq, float qFactor, float sampleRate); +float apply_frequency_filter(BiquadFilter* filter, float input); float hard_clip(float sample, float threshold); float soft_clip(float sample, float threshold); diff --git a/src/fm95.c b/src/fm95.c index d7d6742..2a77094 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -11,7 +11,7 @@ #define DEFAULT_STEREO_SSB 0 #define DEFAULT_CLIPPER_THRESHOLD 1.0f #define DEFAULT_SOFT_CLIPPER_THRESHOLD 0.95f -#define DEFAULT_ALSA_OUTPUT 1 +#define DEFAULT_ALSA_OUTPUT 0 #define DEFAULT_SCA_FREQUENCY 67000.0f #define DEFAULT_SCA_DEVIATION 7000.0f #define DEFAULT_SCA_CLIPPER_THRESHOLD 1.0f // Full deviation, if you set this to 0.5 then you may as well set the deviation to 3.5k @@ -28,7 +28,7 @@ #define SAMPLE_RATE 192000 #define INPUT_DEVICE "FM_Audio.monitor" -#define OUTPUT_DEVICE "plughw:1,0" +#define OUTPUT_DEVICE "alsa_output.pci-0000_00_14.2.analog-stereo" #define MPX_DEVICE "FM_MPX.monitor" // #define SCA_DEVICE "" @@ -470,16 +470,16 @@ int main(int argc, char **argv) { init_preemphasis(&preemp_l, preemphasis_tau, SAMPLE_RATE); init_preemphasis(&preemp_r, preemphasis_tau, SAMPLE_RATE); - FrequencyFilter lpf_l, lpf_r; - init_lpf(&lpf_l, LPF_CUTOFF, SAMPLE_RATE); - init_lpf(&lpf_r, LPF_CUTOFF, SAMPLE_RATE); + BiquadFilter lpf_l, lpf_r; + init_lpf(&lpf_l, LPF_CUTOFF, 1.0f, SAMPLE_RATE); + init_lpf(&lpf_r, LPF_CUTOFF, 1.0f, SAMPLE_RATE); - FrequencyFilter hpf_l, hpf_r; - init_hpf(&hpf_l, HPF_CUTOFF, SAMPLE_RATE); - init_hpf(&hpf_r, HPF_CUTOFF, SAMPLE_RATE); + BiquadFilter hpf_l, hpf_r; + init_hpf(&hpf_l, HPF_CUTOFF, 1.0f, SAMPLE_RATE); + init_hpf(&hpf_r, HPF_CUTOFF, 1.0f, SAMPLE_RATE); - FrequencyFilter bass_hpf; - init_hpf(&bass_hpf, CENTER_BASS, SAMPLE_RATE); + BiquadFilter bass_hpf; + init_hpf(&bass_hpf, CENTER_BASS, 1.0f, SAMPLE_RATE); // #endregion signal(SIGINT, stop);