diff --git a/.vscode/.server-controller-port.log b/.vscode/.server-controller-port.log index 44cfd71..e06a8cf 100644 --- a/.vscode/.server-controller-port.log +++ b/.vscode/.server-controller-port.log @@ -1,5 +1,5 @@ { "port": 13452, - "time": 1737891766403, + "time": 1737910081584, "version": "0.0.3" } \ No newline at end of file diff --git a/README.md b/README.md index c8a86be..84476d0 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # fm95 FM95 is a audio processor for FM, it does: - Pre-Emphasis +- Low Pass Filter - Stereo - SSB Stereo - Polar Stereo diff --git a/lib/filters.c b/lib/filters.c index 43d12c6..3373ec0 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -16,6 +16,82 @@ float apply_pre_emphasis(ResistorCapacitor *rc, float sample) { return audio; } +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(PI * m * fc) / (PI * m); + + // Blackman window + float window = 0.42f - 0.5f * cosf(2.0f * PI * n / (FILTER_TAPS - 1)) + + 0.08f * cosf(4.0f * 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_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(PI * m * fc) / (PI * m); + + // Blackman window + float window = 0.42f - 0.5f * cosf(2.0f * PI * n / (FILTER_TAPS - 1)) + + 0.08f * cosf(4.0f * PI * n / (FILTER_TAPS - 1)); + + filter->coeffs[n] = sinc * window; + } + + 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 apply_freqeuncy_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; +} + void init_delay_line(DelayLine *delay_line, int max_delay) { delay_line->buffer = (float *)calloc(max_delay, sizeof(float)); delay_line->size = max_delay; diff --git a/lib/filters.h b/lib/filters.h index 621632a..61bcba4 100644 --- a/lib/filters.h +++ b/lib/filters.h @@ -1,5 +1,5 @@ #pragma once - +#define FILTER_TAPS 256 #include #include #include @@ -14,6 +14,15 @@ void init_rc(ResistorCapacitor *pe, float alpha); void init_rc_tau(ResistorCapacitor *pe, float tau, float sample_rate); float apply_pre_emphasis(ResistorCapacitor *pe, float sample); +typedef struct { + float coeffs[FILTER_TAPS]; + float delay[FILTER_TAPS]; + int index; +} FrequencyFilter; + +void init_lpf(FrequencyFilter* filter, float cutoffFreq, float sampleRate); +float apply_freqeuncy_filter(FrequencyFilter* filter, float input); + typedef struct { float *buffer; int write_idx; // Write position diff --git a/lib/hilbert.c b/lib/hilbert.c index b31424e..cec50fb 100644 --- a/lib/hilbert.c +++ b/lib/hilbert.c @@ -1,24 +1,43 @@ #include "hilbert.h" -void init_hilbert(HilbertTransformer *hilbert) { - hilbert->delay = calloc(D_SIZE, sizeof(float)); - hilbert->dptr = 0; +void compute_hilbert_coeffs(float* coeffs, int taps) { + int mid = taps / 2; + for (int i = 0; i < taps; i++) { + if ((i - mid) % 2 == 0) { + coeffs[i] = 0.0f; + } else { + coeffs[i] = 2.0f / (PI * (i - mid)); + } + } } -void apply_hilbert(HilbertTransformer *hilbert, float sample, float *output_0deg, float *output_90deg) { - float hilb; +// Initialize the Hilbert transformer +void init_hilbert(HilbertTransformer* filter) { + compute_hilbert_coeffs(filter->coeffs, HILBERT_TAPS); + memset(filter->delay, 0, sizeof(filter->delay)); + filter->index = 0; +} - hilbert->delay[hilbert->dptr] = sample; - hilb = 0.0f; - for(int i = 0; i < NZEROS/2; i++) { - hilb += (xcoeffs[i] * hilbert->delay[(hilbert->dptr - i*2) & (D_SIZE - 1)]); +// Apply the Hilbert transformer +void apply_hilbert(HilbertTransformer* filter, float input, float* inphase, float* quadrature) { + // Insert the new sample into the circular buffer + filter->delay[filter->index] = input; + + // Compute the in-phase and quadrature components + float i_sum = 0.0f; // In-phase (0-degree output) + float q_sum = 0.0f; // Quadrature (90-degree output) + + int coeff_index = 0; + for (int i = filter->index; coeff_index < HILBERT_TAPS; coeff_index++) { + i_sum += filter->delay[i] * (coeff_index == HILBERT_TAPS / 2 ? 1.0f : 0.0f); + q_sum += filter->delay[i] * filter->coeffs[coeff_index]; + + i = (i > 0) ? i - 1 : HILBERT_TAPS - 1; } - *output_0deg = hilbert->delay[(hilbert->dptr - 99) & (D_SIZE - 1)]; - *output_90deg = hilb; - hilbert->dptr = (hilbert->dptr + 1) & (D_SIZE - 1); -} + // Update the index for the next sample + filter->index = (filter->index + 1) % HILBERT_TAPS; -void exit_hilbert(HilbertTransformer *hilbert) { - free(hilbert->delay); + *inphase = i_sum; + *quadrature = q_sum; } \ No newline at end of file diff --git a/lib/hilbert.h b/lib/hilbert.h index 5e796ab..8ca9a6e 100644 --- a/lib/hilbert.h +++ b/lib/hilbert.h @@ -1,44 +1,17 @@ #pragma once #include +#include +#include "constants.h" +#include -#define D_SIZE 256 -#define NZEROS 200 - -/* The non-zero taps of the Hilbert transformer */ -static float xcoeffs[] = { - +0.0008103736f, +0.0008457886f, +0.0009017196f, +0.0009793364f, - +0.0010798341f, +0.0012044365f, +0.0013544008f, +0.0015310235f, - +0.0017356466f, +0.0019696659f, +0.0022345404f, +0.0025318040f, - +0.0028630784f, +0.0032300896f, +0.0036346867f, +0.0040788644f, - +0.0045647903f, +0.0050948365f, +0.0056716186f, +0.0062980419f, - +0.0069773575f, +0.0077132300f, +0.0085098208f, +0.0093718901f, - +0.0103049226f, +0.0113152847f, +0.0124104218f, +0.0135991079f, - +0.0148917649f, +0.0163008758f, +0.0178415242f, +0.0195321089f, - +0.0213953037f, +0.0234593652f, +0.0257599469f, +0.0283426636f, - +0.0312667947f, +0.0346107648f, +0.0384804823f, +0.0430224431f, - +0.0484451086f, +0.0550553725f, +0.0633242001f, +0.0740128560f, - +0.0884368322f, +0.1090816773f, +0.1412745301f, +0.1988673273f, - +0.3326528346f, +0.9997730178f, -0.9997730178f, -0.3326528346f, - -0.1988673273f, -0.1412745301f, -0.1090816773f, -0.0884368322f, - -0.0740128560f, -0.0633242001f, -0.0550553725f, -0.0484451086f, - -0.0430224431f, -0.0384804823f, -0.0346107648f, -0.0312667947f, - -0.0283426636f, -0.0257599469f, -0.0234593652f, -0.0213953037f, - -0.0195321089f, -0.0178415242f, -0.0163008758f, -0.0148917649f, - -0.0135991079f, -0.0124104218f, -0.0113152847f, -0.0103049226f, - -0.0093718901f, -0.0085098208f, -0.0077132300f, -0.0069773575f, - -0.0062980419f, -0.0056716186f, -0.0050948365f, -0.0045647903f, - -0.0040788644f, -0.0036346867f, -0.0032300896f, -0.0028630784f, - -0.0025318040f, -0.0022345404f, -0.0019696659f, -0.0017356466f, - -0.0015310235f, -0.0013544008f, -0.0012044365f, -0.0010798341f, - -0.0009793364f, -0.0009017196f, -0.0008457886f, -0.0008103736f, -}; +#define HILBERT_TAPS 95 typedef struct { - float* delay; - int dptr; + float coeffs[HILBERT_TAPS]; + float delay[HILBERT_TAPS]; + int index; } HilbertTransformer; -void init_hilbert(HilbertTransformer *hilbert); -void apply_hilbert(HilbertTransformer *hilbert, float sample, float *output_0deg, float *output_90deg); -void exit_hilbert(HilbertTransformer *hilbert); \ No newline at end of file +void init_hilbert(HilbertTransformer* filter); +void apply_hilbert(HilbertTransformer* filter, float input, float* inphase, float* quadrature); \ No newline at end of file diff --git a/src/fm95.c b/src/fm95.c index cfe7d3d..c0f37ee 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -3,7 +3,7 @@ #include #define buffer_maxlength 12288 -#define buffer_tlength_fragsize 8192 +#define buffer_tlength_fragsize 12288 #define buffer_prebuf 16 #define DEFAULT_STEREO 1 @@ -43,6 +43,7 @@ #define SCA_VOLUME 0.1f #define MPX_VOLUME 1.0f +#define LPF_CUTOFF 15000 volatile sig_atomic_t to_run = 1; @@ -410,12 +411,16 @@ int main(int argc, char **argv) { HilbertTransformer hilbert; // An Hilbert shifts a signal in quadrature, generating the I/Q data init_hilbert(&hilbert); DelayLine monoDelay; // Hilbert introduces a delay of 99 samples, this should be here to sync the mono with stereo to a sample - init_delay_line(&monoDelay, 99); + init_delay_line(&monoDelay, (HILBERT_TAPS-1)/2); ResistorCapacitor preemp_l, preemp_r; init_rc_tau(&preemp_l, preemphasis_tau, SAMPLE_RATE); init_rc_tau(&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); + signal(SIGINT, stop); signal(SIGTERM, stop); @@ -454,8 +459,8 @@ int main(int argc, char **argv) { float current_mpx_in = mpx_in[i]; float current_sca_in = sca_in[i]; - float ready_l = l_in; - float ready_r = r_in; + float ready_l = apply_freqeuncy_filter(&lpf_l, r_in); + float ready_r = apply_freqeuncy_filter(&lpf_r, l_in); ready_l = apply_pre_emphasis(&preemp_l, ready_l)*2; ready_r = apply_pre_emphasis(&preemp_r, ready_r)*2; ready_l = hard_clip(ready_l, clipper_threshold); @@ -473,9 +478,9 @@ int main(int argc, char **argv) { stereo += 0.2; apply_hilbert(&hilbert, stereo, &stereo_i, &stereo_q); // Compute I/Q #ifdef USB - float signal = (stereo_i*stereo_carrier_cos+stereo_q*(stereo_carrier*0.775f)); + float signal = (stereo_i*stereo_carrier_cos+stereo_q*(stereo_carrier)); #else - float signal = (stereo_i*stereo_carrier_cos-stereo_q*(stereo_carrier*0.775f)); + float signal = (stereo_i*stereo_carrier_cos-stereo_q*(stereo_carrier)); #endif output[i] = delay_line(&monoDelay, mono)*MONO_VOLUME + signal*STEREO_VOLUME; @@ -496,14 +501,10 @@ int main(int argc, char **argv) { float stereo_i, stereo_q; apply_hilbert(&hilbert, stereo, &stereo_i, &stereo_q); // Compute I/Q - #ifdef USB - float signal = (stereo_i*stereo_carrier_cos+stereo_q*(stereo_carrier*0.775f)); - #else - float signal = (stereo_i*stereo_carrier_cos-stereo_q*(stereo_carrier*0.775f)); - #endif + output[i] = delay_line(&monoDelay, mono)*MONO_VOLUME + pilot*PILOT_VOLUME + - signal*STEREO_VOLUME; + (stereo_i*stereo_carrier_cos+stereo_q*stereo_carrier)*STEREO_VOLUME; if(strlen(audio_mpx_device) != 0) output[i] += current_mpx_in*MPX_VOLUME; if(strlen(audio_sca_device) != 0) output[i] += modulate_fm(&sca_mod, hard_clip(current_sca_in, sca_clipper_threshold))*SCA_VOLUME; } else { @@ -544,7 +545,6 @@ int main(int argc, char **argv) { snd_pcm_close(output_handle); snd_pcm_hw_params_free(output_params); } - exit_hilbert(&hilbert); exit_delay_line(&monoDelay); return 0; }