From 224ad3bc645630d38dac01c67263e41a578fcf7c Mon Sep 17 00:00:00 2001 From: KubaPro010 Date: Sun, 12 Jul 2026 09:58:46 +0200 Subject: [PATCH] proper phase shifts for rds2 - less deviation --- include/constants.h | 3 +++ lib/oscillator.c | 14 ++++++++++++++ lib/oscillator.h | 4 +++- src/fm95.c | 27 ++++++++++++--------------- 4 files changed, 32 insertions(+), 16 deletions(-) diff --git a/include/constants.h b/include/constants.h index 59127bd..f881de7 100644 --- a/include/constants.h +++ b/include/constants.h @@ -4,4 +4,7 @@ #endif #ifndef M_2PI #define M_2PI (M_PI * 2.0) +#endif +#ifndef M_PI_2 +#define M_PI_2 (M_PI / 2.0) #endif \ No newline at end of file diff --git a/lib/oscillator.c b/lib/oscillator.c index d211cd7..5fbfd05 100644 --- a/lib/oscillator.c +++ b/lib/oscillator.c @@ -2,6 +2,7 @@ void init_oscillator(Oscillator *osc, float frequency, float sample_rate) { osc->phase = 0.0f; + osc->unwrapped_phase = 0.0; osc->phase_increment = (M_2PI * frequency) / sample_rate; osc->sample_rate = sample_rate; } @@ -32,8 +33,21 @@ float get_oscillator_cos_multiplier_ni(Oscillator *osc, float multiplier) { inline bool advance_oscillator(Oscillator *osc) { osc->phase += osc->phase_increment; + osc->unwrapped_phase += osc->phase_increment; if (osc->phase >= M_2PI) { osc->phase -= M_2PI; return true; } return false; +} + +bool oscillator_did_cycle(Oscillator *osc, float phase_shift, float *prev_shifted_phase) { + float shifted = osc->phase + phase_shift; + + // wrap into [0, 2π) + while (shifted >= M_2PI) shifted -= M_2PI; + while (shifted < 0.0f) shifted += M_2PI; + + bool crossed = shifted < *prev_shifted_phase; + *prev_shifted_phase = shifted; + return crossed; } \ No newline at end of file diff --git a/lib/oscillator.h b/lib/oscillator.h index b50774b..82c5a6c 100644 --- a/lib/oscillator.h +++ b/lib/oscillator.h @@ -6,6 +6,7 @@ typedef struct { float phase; + double unwrapped_phase; float phase_increment; float sample_rate; } Oscillator; @@ -16,4 +17,5 @@ float get_oscillator_sin_sample(Oscillator *osc); float get_oscillator_cos_sample(Oscillator *osc); float get_oscillator_sin_multiplier_ni(Oscillator *osc, float multiplier); float get_oscillator_cos_multiplier_ni(Oscillator *osc, float multiplier); -bool advance_oscillator(Oscillator *osc); \ No newline at end of file +bool advance_oscillator(Oscillator *osc); +bool oscillator_did_cycle(Oscillator *osc, float phase_shift, float *prev_shifted_phase); \ No newline at end of file diff --git a/src/fm95.c b/src/fm95.c index c376456..fb10088 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -31,7 +31,6 @@ typedef struct { float headroom; float pilot; float rds; - float rds_step; float drive; float makeup; } FM95_Volumes; @@ -111,9 +110,7 @@ static inline bool compare_dvs(const FM95_DeviceNames *a, const FM95_DeviceNames } static float calculate_sharedaudio_volume(const FM95_Volumes volumes, const int rds_streams) { - float rds_volume = 0.0f; - for (int i = 0; i < rds_streams; i++) rds_volume += volumes.rds * powf(volumes.rds_step, i); - return 1.0f - rds_volume - volumes.pilot - volumes.headroom; + return 1.0f - (volumes.rds * rds_streams) - volumes.pilot - volumes.headroom; } static void stop(int signum) { @@ -206,7 +203,8 @@ int run_fm95(FM95_Config* config, FM95_Runtime* runtime, FM95_RunResult* result) } for(uint16_t i = 0; i < BUFFER_SIZE; i++) { - bool cycle = advance_oscillator(&runtime->osc); + advance_oscillator(&runtime->osc); + bool do_result = (i == BUFFER_SIZE - 1); float mpx = 0.0f; @@ -245,25 +243,26 @@ int run_fm95(FM95_Config* config, FM95_Runtime* runtime, FM95_RunResult* result) mpx = stereo_encode(&runtime->stencode, config->stereo, mod_l, mod_r, &audio); { - float rds_level = config->volumes.rds; + static const float stream_shift[4] = {0.0f, (float)M_PI, (float)M_PI_2, (float)(3.0 * M_PI_2)}; + static float prev_shifted_phase[4] = {0.0f, (float)M_PI, (float)M_PI_2, (float)(3.0 * M_PI_2)}; + float clock = get_oscillator_cos_multiplier_ni(&runtime->osc, 1.0f); - for(uint8_t stream = 0; stream < config->rds_streams; stream++) { - if (cycle) { + for (uint8_t stream = 0; stream < config->rds_streams; stream++) { + if (oscillator_did_cycle(&runtime->osc, stream_shift[stream], &prev_shifted_phase[stream])) { uint8_t bit; if (bit_ring_read1(&runtime->rds_bitring[stream], &bit)) runtime->rds_last_bit[stream] = bit; runtime->rds_symbol[stream] = runtime->rds_last_bit[stream] ? 1.0f : -1.0f; } uint8_t osc_stream = 12 + stream; - if(osc_stream >= 13) osc_stream++; - + if (osc_stream >= 13) osc_stream++; + float shaped = 0.0f; iirfilt_rrrf_execute(runtime->rds_filter[stream], runtime->rds_symbol[stream], &shaped); float carrier = get_oscillator_cos_multiplier_ni(&runtime->osc, osc_stream * 4.0f); - if(config->stereo_ssb) carrier = delay_line(&runtime->rds_delays[stream], carrier); - mpx += clock * shaped * carrier * rds_level; - rds_level *= config->volumes.rds_step; // Prepare level for the next stream + if (config->stereo_ssb) carrier = delay_line(&runtime->rds_delays[stream], carrier); + mpx += clock * shaped * carrier * config->volumes.rds; } } @@ -351,7 +350,6 @@ static int config_handler(void* user, const char* section, const char* name, con else if(MATCH("advanced", "makeup")) pconfig->volumes.makeup = strtof(value, NULL); else if(MATCH("volumes", "pilot")) pconfig->volumes.pilot = strtof(value, NULL); else if(MATCH("volumes", "rds")) pconfig->volumes.rds = strtof(value, NULL); - else if(MATCH("volumes", "rds_step")) pconfig->volumes.rds_step = strtof(value, NULL); return 1; } @@ -619,7 +617,6 @@ int main(int argc, char **argv) { .volumes = { .pilot = 0.09f, .rds = 0.045f, - .rds_step = 0.9f, .headroom = 0.05f, .drive = 1.0f, .makeup = 1.0f