diff --git a/modulation/stereo_encoder.c b/modulation/stereo_encoder.c index e32101c..3379af1 100644 --- a/modulation/stereo_encoder.c +++ b/modulation/stereo_encoder.c @@ -1,23 +1,63 @@ #include "stereo_encoder.h" +#ifdef STEREO_SSB +static void init_delay_line(struct delay_line_t *delay_line, uint32_t sample_rate) { + delay_line->buffer = malloc(sample_rate * sizeof(float)); + memset(delay_line->buffer, 0, sample_rate * sizeof(float)); +} + +static void set_delay_line(struct delay_line_t *delay_line, uint32_t new_delay) { + delay_line->delay = new_delay; +} + +static inline float delay_line(struct delay_line_t *delay_line, float in) { + delay_line->buffer[delay_line->idx++] = in; + if (delay_line->idx >= delay_line->delay) delay_line->idx = 0; + return delay_line->buffer[delay_line->idx]; +} + +static void exit_delay_line(struct delay_line_t *delay_line) { + free(delay_line->buffer); +} +#endif + // Multiplier is the multiplier to get to 19 khz void init_stereo_encoder(StereoEncoder* st, uint8_t multiplier, Oscillator* osc, float audio_volume, float pilot_volume) { st->multiplier = multiplier; st->osc = osc; st->pilot_volume = pilot_volume; st->audio_volume = audio_volume; +#ifdef STEREO_SSB + init_delay_line(&st->delay, 15); // 7*2+1 +#endif } +#ifdef STEREO_SSB +float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right, firhilbf hilbert) { +#else float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right) { +#endif float mid = (left+right) * 0.5f; if(!enabled) return mid * st->audio_volume; + mid = delay_line(&st->delay, mid); float half_audio = st->audio_volume * 0.5f; float side = (left-right) * 0.5f; + +#ifdef STEREO_SSB + float complex stereo_hilbert; + firhilbf_r2c_execute(hilbert, side, &stereo_hilbert); + float signalx2cos = get_oscillator_cos_multiplier_ni(st->osc, st->multiplier * 2.0f); +#endif float signalx1 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier); float signalx2 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier * 2.0f); +#ifdef STEREO_SSB + float stereo = (crealf(stereo_hilbert) * signalx2cos) - (cimagf(stereo_hilbert) * signalx2); + return (mid*half_audio) + (signalx1*st->pilot_volume) + (stereo * half_audio); +#else return (mid*half_audio) + (signalx1*st->pilot_volume) + ((side*signalx2) * half_audio); +#endif } \ No newline at end of file diff --git a/modulation/stereo_encoder.h b/modulation/stereo_encoder.h index 05e3ed1..73a545a 100644 --- a/modulation/stereo_encoder.h +++ b/modulation/stereo_encoder.h @@ -1,7 +1,19 @@ #pragma once +#define STEREO_SSB + #include #include "../dsp/oscillator.h" +#ifdef STEREO_SSB +#include +#include + +typedef struct delay_line_t { + float *buffer; + uint32_t delay; + uint32_t idx; +} delay_line_t; +#endif typedef struct { @@ -9,8 +21,15 @@ typedef struct Oscillator* osc; float audio_volume; float pilot_volume; +#ifdef STEREO_SSB + struct delay_line_t delay; +#endif } StereoEncoder; void init_stereo_encoder(StereoEncoder *st, uint8_t multiplier, Oscillator *osc, float audio_volume, float pilot_volume); +#ifdef STEREO_SSB +float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right, firhilbf hilbert); +#else float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right); +#endif diff --git a/src/fm95.c b/src/fm95.c index 1e97491..2ed71bc 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -83,6 +83,9 @@ typedef struct float* rds_in; Oscillator osc; iirfilt_rrrf lpf_l, lpf_r; + #ifdef STEREO_SSB + firhilbf stereo_hilbert; + #endif ResistorCapacitor preemp_l, preemp_r; BS412Compressor bs412; StereoEncoder stencode; @@ -140,6 +143,10 @@ void cleanup_runtime(FM95_Runtime* runtime, const FM95_Config config) { iirfilt_rrrf_destroy(runtime->lpf_l); iirfilt_rrrf_destroy(runtime->lpf_r); } + #ifdef STEREO_SSB + firhilbf_destroy(runtime->stereo_hilbert); + free(runtime->stencode.delay.buffer); + #endif } void cleanup_audio_runtime(FM95_Runtime *rt, const FM95_Options options) { @@ -481,6 +488,10 @@ void init_runtime(FM95_Runtime* runtime, const FM95_Config config) { runtime->lpf_r = iirfilt_rrrf_create_prototype(LIQUID_IIRDES_CHEBY2, LIQUID_IIRDES_LOWPASS, LIQUID_IIRDES_SOS, config.lpf_order, (config.lpf_cutoff/config.sample_rate), 0.0f, 1.0f, 60.0f); } + #ifdef STEREO_SSB + runtime->stereo_hilbert = firhilbf_create(7, 60); + #endif + if(config.preemphasis != 0) { init_preemphasis(&runtime->preemp_l, (float)config.preemphasis * 1.0e-6f, config.sample_rate, config.preemp_unity_freq); init_preemphasis(&runtime->preemp_r, (float)config.preemphasis * 1.0e-6f, config.sample_rate, config.preemp_unity_freq);