of course

lib/delay.c

@@ -0,0 +1,25 @@
+#include <stdlib.h>
+#include <string.h>
+#include "delay.h"
+
+void init_delay_line(delay_line_t *dl, uint32_t delay_samples) {
+    dl->delay = delay_samples;
+    dl->idx = 0;
+    dl->buffer = calloc(delay_samples, sizeof(float));
+}
+
+float delay_line(delay_line_t *dl, float in) {
+    float out = dl->buffer[dl->idx];   // read delayed sample
+
+    dl->buffer[dl->idx] = in;          // write new input
+
+    dl->idx++;
+    if (dl->idx >= dl->delay) dl->idx = 0;
+
+    return out;
+}
+
+void exit_delay_line(delay_line_t *dl) {
+    if(dl->buffer != NULL) free(dl->buffer);
+    dl->buffer = NULL;
+}

lib/delay.h

@@ -0,0 +1,13 @@
+#include <string.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+typedef struct delay_line_t {
+	float *buffer;
+	uint32_t delay;
+	uint32_t idx;
+} delay_line_t;
+
+void init_delay_line(delay_line_t *dl, uint32_t delay_samples);
+float delay_line(delay_line_t *dl, float in);
+void exit_delay_line(delay_line_t *dl);

lib/stereo_encoder.c

@@ -0,0 +1,54 @@
+#include "stereo_encoder.h"
+
+// Multiplier is the multiplier to get to 19 khz
+void init_stereo_encoder(StereoEncoder* st, uint8_t stereo_ssb, 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 * 0.5f;
+    if(stereo_ssb) {
+        init_delay_line(&st->delay_pilot, stereo_ssb*2);
+        init_delay_line(&st->delay, stereo_ssb*2);
+        st->stereo_hilbert = firhilbf_create(stereo_ssb, 80);
+    } else st->stereo_hilbert = NULL;
+}
+
+float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right, float *audio) {
+    float mid = (left+right) * 0.5f;
+    if(!enabled) {
+        *audio = mid * 2.0f * st->audio_volume;
+        return 0.0f;
+    }
+
+    if(st->stereo_hilbert) mid = delay_line(&st->delay, mid);
+
+    float side = (left-right) * 0.5f;
+
+    float complex stereo_hilbert = 0+0*I;
+    if(st->stereo_hilbert) firhilbf_r2c_execute(st->stereo_hilbert, side, &stereo_hilbert);
+    
+    float signalx1 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier);
+
+    float signalx2cos = 0.0f;
+    if(st->stereo_hilbert) {
+        signalx1 = delay_line(&st->delay_pilot, signalx1);
+        signalx2cos = get_oscillator_cos_multiplier_ni(st->osc, st->multiplier * 2.0f);
+    }
+    float signalx2 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier * 2.0f);
+
+    *audio = mid * st->audio_volume;
+    if(st->stereo_hilbert) {
+        float stereo = (crealf(stereo_hilbert) * signalx2) - (cimagf(stereo_hilbert) * signalx2cos);
+        *audio += stereo * st->audio_volume;
+    } else *audio += (side*signalx2) * st->audio_volume;
+    return signalx1 * st->pilot_volume;
+}
+
+void exit_stereo_encoder(StereoEncoder* st) {
+    if(st->stereo_hilbert) {
+        exit_delay_line(&st->delay);
+        exit_delay_line(&st->delay_pilot);
+	    firhilbf_destroy(st->stereo_hilbert);
+        st->stereo_hilbert = NULL;
+    }
+}

lib/stereo_encoder.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <stdint.h>
+#include "oscillator.h"
+#include <liquid/liquid.h>
+#include <complex.h>
+#include "delay.h"
+
+typedef struct
+{
+    uint8_t multiplier;
+    Oscillator* osc;
+    float audio_volume;
+    float pilot_volume;
+    delay_line_t delay;
+    delay_line_t delay_pilot;
+	firhilbf stereo_hilbert;
+} StereoEncoder;
+
+void init_stereo_encoder(StereoEncoder *st, uint8_t stereo_ssb, uint8_t multiplier, Oscillator *osc, float audio_volume, float pilot_volume);
+float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right, float *audio);
+void exit_stereo_encoder(StereoEncoder* st);