diff --git a/lib/bs412.h b/lib/bs412.h index c8effc6..6c395b1 100644 --- a/lib/bs412.h +++ b/lib/bs412.h @@ -3,9 +3,9 @@ typedef struct { - int i; - int sample_rate; - double sample; + int i; + int sample_rate; + double sample; } MPXPowerMeasurement; float dbr_to_deviation(float dbr); diff --git a/lib/debug.h b/lib/debug.h index 7d041ac..868ec76 100644 --- a/lib/debug.h +++ b/lib/debug.h @@ -1,4 +1,4 @@ #pragma once #include #define debug_printf(fmt, ...) \ - printf("[%s:%d in %s] " fmt, __FILE__, __LINE__, __func__, ##__VA_ARGS__) + printf("[%s:%d in %s] " fmt, __FILE__, __LINE__, __func__, ##__VA_ARGS__) diff --git a/lib/filters.h b/lib/filters.h index d4b3266..76615c6 100644 --- a/lib/filters.h +++ b/lib/filters.h @@ -7,12 +7,6 @@ #include "optimization.h" #include "oscillator.h" -#if USE_NEON -#define LPF_ORDER 20 // neon has to have divisable by 4 -#else -#define LPF_ORDER 10 -#endif - typedef struct { float alpha; diff --git a/lib/gain_control.c b/lib/gain_control.c new file mode 100644 index 0000000..e5d0cec --- /dev/null +++ b/lib/gain_control.c @@ -0,0 +1,40 @@ +#include "gain_control.h" + +void initAGC(AGC* agc, int sampleRate, float targetLevel, float minGain, float maxGain, float attackTime, float releaseTime) { + agc->sampleRate = sampleRate; + agc->targetLevel = targetLevel; + agc->minGain = minGain; + agc->maxGain = maxGain; + agc->attackTime = attackTime; + agc->releaseTime = releaseTime; + + agc->attackCoef = expf(-1.0f / (sampleRate * attackTime)); + agc->releaseCoef = expf(-1.0f / (sampleRate * releaseTime)); + + agc->currentGain = 1.0f; + agc->currentLevel = 0.0f; + + agc->rms_buffer = 0.0f; +} + +float process_agc_stereo(AGC* agc, float left, float right, float *right_out) { + float sample = (left+right)/2; + + float x2 = sample * sample; + + float rmsAlpha = expf(-1.0f / (agc->sampleRate * 0.04)); + agc->rms_buffer = rmsAlpha * agc->rms_buffer + (1.0f - rmsAlpha) * x2; + float instantLevel = sqrtf(agc->rms_buffer); + + float alpha = (instantLevel > agc->currentLevel) ? agc->attackCoef : agc->releaseCoef; + agc->currentLevel = alpha * agc->currentLevel + (1.0f - alpha) * instantLevel; + + float desiredGain = agc->targetLevel / fmaxf(agc->currentLevel, 1e-10f); + desiredGain = fminf(fmaxf(desiredGain, agc->minGain), agc->maxGain); + + float gainAlpha = (desiredGain > agc->currentGain) ? agc->attackCoef : agc->releaseCoef; + agc->currentGain = gainAlpha * agc->currentGain + (1.0f - gainAlpha) * desiredGain; + + *right_out = right * agc->currentGain; + return left * agc->currentGain; +} \ No newline at end of file diff --git a/lib/gain_control.h b/lib/gain_control.h new file mode 100644 index 0000000..b5ebd29 --- /dev/null +++ b/lib/gain_control.h @@ -0,0 +1,22 @@ +#pragma once +#include + +typedef struct { + float targetLevel; + float maxGain; + float minGain; + float attackTime; + float releaseTime; + + float currentGain; + float currentLevel; + + int sampleRate; + float attackCoef; + float releaseCoef; + + float rms_buffer; +} AGC; + +void initAGC(AGC* agc, int sampleRate, float targetLevel, float minGain, float maxGain, float attackTime, float releaseTime); +float process_agc_stereo(AGC* agc, float left, float right, float *right_out); \ No newline at end of file diff --git a/lib/oscillator.c b/lib/oscillator.c index b206bea..2008d56 100644 --- a/lib/oscillator.c +++ b/lib/oscillator.c @@ -23,15 +23,15 @@ float get_oscillator_cos_sample(Oscillator *osc) { } float get_oscillator_sin_multiplier_ni(Oscillator *osc, float multiplier) { - float new_phase = osc->phase * multiplier; - new_phase -= (new_phase >= M_2PI) ? M_2PI : 0.0f; - return sinf(new_phase); + float new_phase = osc->phase * multiplier; + new_phase -= (new_phase >= M_2PI) ? M_2PI : 0.0f; + return sinf(new_phase); } float get_oscillator_cos_multiplier_ni(Oscillator *osc, float multiplier) { float new_phase = osc->phase * multiplier; - new_phase -= (new_phase >= M_2PI) ? M_2PI : 0.0f; - return cosf(new_phase); + new_phase -= (new_phase >= M_2PI) ? M_2PI : 0.0f; + return cosf(new_phase); } void advance_oscillator(Oscillator *osc) { diff --git a/src/dcf95.c b/src/dcf95.c index 1a5f00a..82d7c7f 100644 --- a/src/dcf95.c +++ b/src/dcf95.c @@ -133,41 +133,41 @@ void calculate_dcf77_bits(time_t now, int *bits) { bits[20] = 1; int minutes = t->tm_min; - for (int i = 0; i < 4; i++) { - bits[21 + i] = (minutes % 10 >> i) & 1; - } - for (int i = 0; i < 3; i++) { - bits[25 + i] = (minutes / 10 >> i) & 1; - } + for (int i = 0; i < 4; i++) { + bits[21 + i] = (minutes % 10 >> i) & 1; + } + for (int i = 0; i < 3; i++) { + bits[25 + i] = (minutes / 10 >> i) & 1; + } - int minute_parity = 0; - for (int i = 21; i <= 27; i++) { - minute_parity ^= bits[i]; - } - bits[28] = minute_parity; + int minute_parity = 0; + for (int i = 21; i <= 27; i++) { + minute_parity ^= bits[i]; + } + bits[28] = minute_parity; int hours = t->tm_hour; if(cest) hours += 1; for (int i = 0; i < 4; i++) { - bits[29 + i] = (hours % 10 >> i) & 1; - } - for (int i = 0; i < 2; i++) { - bits[33 + i] = (hours / 10 >> i) & 1; - } + bits[29 + i] = (hours % 10 >> i) & 1; + } + for (int i = 0; i < 2; i++) { + bits[33 + i] = (hours / 10 >> i) & 1; + } int hour_parity = 0; - for (int i = 29; i <= 34; i++) { - hour_parity ^= bits[i]; - } - bits[35] = hour_parity; + for (int i = 29; i <= 34; i++) { + hour_parity ^= bits[i]; + } + bits[35] = hour_parity; int day = t->tm_mday; for (int i = 0; i < 4; i++) { - bits[36 + i] = (day % 10 >> i) & 1; - } - for (int i = 0; i < 2; i++) { - bits[40 + i] = (day / 10 >> i) & 1; - } + bits[36 + i] = (day % 10 >> i) & 1; + } + for (int i = 0; i < 2; i++) { + bits[40 + i] = (day / 10 >> i) & 1; + } int dow = t->tm_wday == 0 ? 7 : t->tm_wday; bits[42] = dow & 0x01; @@ -176,17 +176,17 @@ void calculate_dcf77_bits(time_t now, int *bits) { int month = t->tm_mon + 1; for (int i = 0; i < 4; i++) { - bits[45 + i] = (month % 10 >> i) & 1; - } + bits[45 + i] = (month % 10 >> i) & 1; + } bits[49] = (month / 10) & 0x01; int year = t->tm_year % 100; for (int i = 0; i < 4; i++) { - bits[50 + i] = (year % 10 >> i) & 1; + bits[50 + i] = (year % 10 >> i) & 1; } for (int i = 0; i < 4; i++) { - bits[54 + i] = (year / 10 >> i) & 1; - } + bits[54 + i] = (year / 10 >> i) & 1; + } int year_parity = 0; for (int i = 36; i <= 57; i++) { diff --git a/src/fm95.c b/src/fm95.c index be00a5d..acc3608 100644 --- a/src/fm95.c +++ b/src/fm95.c @@ -21,6 +21,7 @@ #include "../lib/fm_modulator.h" #include "../lib/optimization.h" #include "../lib/bs412.h" +#include "../lib/gain_control.h" #define DEFAULT_SAMPLE_RATE 192000 @@ -453,6 +454,10 @@ int main(int argc, char **argv) { MPXPowerMeasurement mpx_only_power; init_modulation_power_measure(&mpx_only_power, sample_rate); + AGC agc; + void initAGC(AGC* agc, int sampleRate, float targetLevel, float minGain, float maxGain, float attackTime, float releaseTime); + initAGC(&agc, sample_rate, 0.707f, 0.25f, 2.5f, 0.01f, 0.4f); + signal(SIGINT, stop); signal(SIGTERM, stop); @@ -512,6 +517,7 @@ int main(int argc, char **argv) { float ready_l = apply_preemphasis(&preemp_l, l_in); float ready_r = apply_preemphasis(&preemp_r, r_in); + ready_l = process_agc_stereo(&agc, ready_l, ready_r, &ready_r); ready_l = hard_clip(ready_l*audio_volume, clipper_threshold); ready_r = hard_clip(ready_r*audio_volume, clipper_threshold);