From 9d06f8d80e67f697f86268f278f6523b15d7d187 Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Mon, 30 Dec 2024 12:26:27 +0100 Subject: [PATCH] add a version with rds --- compile_stereo_rds | 1 + stereo_coder.c | 19 ++--- stereo_coder_rds.c | 200 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 210 insertions(+), 10 deletions(-) create mode 100755 compile_stereo_rds create mode 100644 stereo_coder_rds.c diff --git a/compile_stereo_rds b/compile_stereo_rds new file mode 100755 index 0000000..408dc81 --- /dev/null +++ b/compile_stereo_rds @@ -0,0 +1 @@ +gcc stereo_coder_rds.c -lpulse -lpulse-simple -lm -o stereo_coder diff --git a/stereo_coder.c b/stereo_coder.c index 4b305d8..920632d 100644 --- a/stereo_coder.c +++ b/stereo_coder.c @@ -9,13 +9,12 @@ #define OUTPUT_DEVICE "alsa_output.platform-soc_sound.stereo-fallback" #define BUFFER_SIZE 512 -#define MONO_VOLUME 0.5f -#define PILOT_VOLUME 0.025f -#define STEREO_VOLUME 0.275f - +#define MONO_VOLUME 0.5f // L+R Signal +#define PILOT_VOLUME 0.025f // 19 KHz Pilot +#define STEREO_VOLUME 0.275f // L-R signal #define TWO_BUFFER_SIZE (BUFFER_SIZE*2) // Don't touch this -volatile int to_run = 1; +volatile sig_atomic_t to_run = 1; const float format_scale = 1.0f / 32768.0f; void stereo_s16le_to_float(const int16_t *input, float *left, float *right, size_t num_samples) { @@ -66,12 +65,12 @@ int main() { const float STEREO_FREQ = 38000.0f; // Define formats and buffer atributes - pa_sample_spec input_format = { + pa_sample_spec stereo_format = { .format = PA_SAMPLE_S16LE, .channels = 2, .rate = SAMPLE_RATE }; - pa_sample_spec output_format = { + pa_sample_spec mono_format = { .format = PA_SAMPLE_S16LE, .channels = 1, .rate = SAMPLE_RATE @@ -95,7 +94,7 @@ int main() { PA_STREAM_RECORD, INPUT_DEVICE, "Audio Input", - &input_format, + &stereo_format, NULL, &input_buffer_atr, NULL @@ -113,7 +112,7 @@ int main() { PA_STREAM_PLAYBACK, OUTPUT_DEVICE, "MPX", - &output_format, + &mono_format, NULL, &output_buffer_atr, NULL @@ -148,7 +147,7 @@ int main() { float mono = (left[i] + right[i]) / 2.0f; float stereo = (left[i] - right[i]) / 2.0f; - + mpx[i] = mono*MONO_VOLUME + (stereo * stereo_carrier)*STEREO_VOLUME + (pilot * PILOT_VOLUME); diff --git a/stereo_coder_rds.c b/stereo_coder_rds.c new file mode 100644 index 0000000..9908284 --- /dev/null +++ b/stereo_coder_rds.c @@ -0,0 +1,200 @@ +#include +#include +#include +#include +#include +#include + +#define INPUT_DEVICE "real_real_tx_audio_input.monitor" +#define OUTPUT_DEVICE "alsa_output.platform-soc_sound.stereo-fallback" +#define RDS_INPUT "RDS.monitor" +#define BUFFER_SIZE 512 + +#define MONO_VOLUME 0.5f // L+R Signal +#define PILOT_VOLUME 0.025f // 19 KHz Pilot +#define STEREO_VOLUME 0.275f // L-R signal +#define RDS_VOLUME 0.04f // RDS Signal + +volatile sig_atomic_t to_run = 1; + +const float format_scale = 1.0f / 32768.0f; +void stereo_s16le_to_float(const int16_t *input, float *left, float *right, size_t num_samples) { + for (size_t i = 0; i < num_samples/2; i++) { + left[i] = input[i * 2] * format_scale; + right[i] = input[i * 2 + 1] * format_scale; + } +} +void mono_s16le_to_float(const int16_t *input, float *output, size_t num_samples) { + for (size_t i = 0; i < num_samples; i++) { + output[i] = input[i * 2] * format_scale; + } +} + +void float_array_to_s16le(const float *input, int16_t *output, size_t num_samples) { + for (size_t i = 0; i < num_samples; i++) { + output[i] = (int16_t)((fminf(fmaxf(input[i], -1.0f), 1.0f)) * 32767.0f); + } +} + +#define M_2PI (3.14159265358979323846 * 2.0) + +// Track phase continuously to maintain frequency accuracy +typedef struct { + float phase; + float phase_increment; +} Oscillator; + +void init_oscillator(Oscillator *osc, float frequency, float sample_rate) { + osc->phase = 0.0f; + osc->phase_increment = (M_2PI * frequency) / sample_rate; +} + +float get_next_sample(Oscillator *osc) { + float sample = sinf(osc->phase); + osc->phase += osc->phase_increment; + if (osc->phase >= M_2PI) { + osc->phase -= M_2PI; + } + return sample; +} + +static void stop(int signum) { + (void)signum; + printf("\nReceived stop signal. Cleaning up...\n"); + to_run = 0; +} + +int main() { + printf("STCode : Stereo encoder made by radio95 (with help of ChatGPT and Claude, thanks!)\n"); + const float SAMPLE_RATE = 192000.0f; // Don't go lower than 176 KHz + const float PILOT_FREQ = 19000.0f; + const float STEREO_FREQ = 38000.0f; + const float RDS_FREQ = 57000.0f; + + // Define formats and buffer atributes + pa_sample_spec stereo_format = { + .format = PA_SAMPLE_S16LE, + .channels = 2, + .rate = SAMPLE_RATE + }; + pa_sample_spec mono_format = { + .format = PA_SAMPLE_S16LE, + .channels = 1, + .rate = SAMPLE_RATE + }; + + pa_buffer_attr input_buffer_atr = { + .maxlength = 4096, + .fragsize = 2048 + }; + pa_buffer_attr output_buffer_atr = { + .maxlength = 4096, + .tlength = 2048, + .prebuf = 0 + }; + + printf("Connecting to input devices... (%s, %s)\n", INPUT_DEVICE, RDS_INPUT); + + pa_simple *input_device = pa_simple_new( + NULL, + "StereoEncoder", + PA_STREAM_RECORD, + INPUT_DEVICE, + "Audio Input", + &stereo_format, + NULL, + &input_buffer_atr, + NULL + ); + if (!input_device) { + fprintf(stderr, "Error: cannot open input device.\n"); + return 1; + } + pa_simple *input_device_rds = pa_simple_new( + NULL, + "StereoEncoder", + PA_STREAM_RECORD, + RDS_INPUT, + "Audio Input", + &mono_format, + NULL, + &input_buffer_atr, + NULL + ); + if (!input_device_rds) { + fprintf(stderr, "Error: cannot open input device.\n"); + pa_simple_free(input_device); + return 1; + } + + printf("Connecting to output device... (%s)\n", OUTPUT_DEVICE); + + pa_simple *output_device = pa_simple_new( + NULL, + "StereoEncoder", + PA_STREAM_PLAYBACK, + OUTPUT_DEVICE, + "MPX", + &mono_format, + NULL, + &output_buffer_atr, + NULL + ); + if (!output_device) { + fprintf(stderr, "Error: cannot open output device.\n"); + pa_simple_free(input_device); + pa_simple_free(input_device_rds); + return 1; + } + + Oscillator pilot_osc, stereo_osc, rds_osc; + init_oscillator(&pilot_osc, PILOT_FREQ, SAMPLE_RATE); + init_oscillator(&stereo_osc, STEREO_FREQ, SAMPLE_RATE); + init_oscillator(&rds_osc, RDS_FREQ, SAMPLE_RATE); + + signal(SIGINT, stop); + signal(SIGTERM, stop); + + int16_t input[BUFFER_SIZE*2], input_rds[BUFFER_SIZE]; + float rds[BUFFER_SIZE]; + float left[BUFFER_SIZE], right[BUFFER_SIZE]; + float mpx[BUFFER_SIZE]; + int16_t output[BUFFER_SIZE]; + while (to_run) { + if (pa_simple_read(input_device, input, sizeof(input), NULL) < 0) { + fprintf(stderr, "Error reading from input device.\n"); + break; + } + if (pa_simple_read(input_device_rds, input_rds, sizeof(input_rds), NULL) < 0) { + fprintf(stderr, "Error reading from input device.\n"); + break; + } + stereo_s16le_to_float(input, left, right, sizeof(input)); + mono_s16le_to_float(input_rds, rds, sizeof(input_rds)); + + for (int i = 0; i < BUFFER_SIZE; i++) { + float pilot = get_next_sample(&pilot_osc); + float stereo_carrier = get_next_sample(&stereo_osc); + float rds_carrier = get_next_sample(&rds_osc); + + float mono = (left[i] + right[i]) / 2.0f; + float stereo = (left[i] - right[i]) / 2.0f; + float rds_sample = rds[i]; + + mpx[i] = mono*MONO_VOLUME + + (stereo * stereo_carrier)*STEREO_VOLUME + + (pilot * PILOT_VOLUME) + + (rds_sample * rds_carrier)*RDS_VOLUME; + } + + float_array_to_s16le(mpx, output, BUFFER_SIZE); + if (pa_simple_write(output_device, output, sizeof(output), NULL) < 0) { + fprintf(stderr, "Error writing to output device.\n"); + break; + } + } + printf("Cleaning up...\n"); + pa_simple_free(input_device); + pa_simple_free(output_device); + return 0; +}