start modularization

filter/bs412.c

@@ -0,0 +1,46 @@
+#include "bs412.h"
+
+#define LOG2_19000 log2f(19000.0f)
+
+inline float dbr_to_deviation(float dbr) {
+	return 19000.0f * powf(2.0f, dbr * 0.332193f);
+}
+
+inline float deviation_to_dbr(float deviation) {
+	if(deviation == 0.0f) return -100.0f;
+	return 10.0f * (log2f(deviation) - LOG2_19000) * 0.30103f;
+}
+
+void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
+	mpx->sample_counter = 0;
+	mpx->sample = 0;
+	mpx->sample_rate = sample_rate;
+	#ifdef BS412_DEBUG
+	debug_printf("Initialized MPX power measurement with sample rate: %d\n", sample_rate);
+	#endif
+}
+
+float measure_mpx(MPXPowerMeasurement* mpx, float deviation) {
+	mpx->sample += deviation * deviation; // rmS
+	mpx->sample_counter++;
+
+	float inv_counter = 1.0f / mpx->sample_counter;
+	float avg_deviation = sqrtf(mpx->sample * inv_counter); // RMs
+	float modulation_power = deviation_to_dbr(avg_deviation);
+
+	#ifdef BS412_DEBUG
+	if(mpx->sample_counter % mpx->sample_rate == 0) {
+		debug_printf("MPX power: %f dBr\n", modulation_power);
+	}
+	#endif
+
+	if (mpx->sample_counter >= mpx->sample_rate * 60) {
+		#ifdef BS412_DEBUG
+		debug_printf("Resetting MPX power measurement\n");
+		#endif
+		mpx->sample = avg_deviation * avg_deviation;
+		mpx->sample_counter = 1;
+	}
+
+	return modulation_power;
+}

filter/bs412.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#ifdef DEBUG
+#define BS412_DEBUG
+#endif
+
+#include <math.h>
+#ifdef BS412_DEBUG
+#include "../lib/debug.h"
+#endif
+
+typedef struct
+{
+	int sample_counter;
+	int sample_rate;
+	double sample;
+} MPXPowerMeasurement;
+
+float dbr_to_deviation(float dbr);
+float deviation_to_dbr(float deviation);
+
+void init_modulation_power_measure(MPXPowerMeasurement *mpx, int sample_rate);
+float measure_mpx(MPXPowerMeasurement *mpx, float deviation);

filter/gain_control.c

@@ -0,0 +1,37 @@
+#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;
+	agc->rmsAlpha = expf(-1.0f / (sampleRate * 0.02f));
+}
+
+float process_agc(AGC* agc, float sidechain) {
+	float x2 = sidechain * sidechain;
+
+	agc->rms_buffer = agc->rmsAlpha * agc->rms_buffer + (1.0f - agc->rmsAlpha) * x2;
+	const float instantLevel = sqrtf(agc->rms_buffer);
+	
+	const float levelAlpha = (instantLevel > agc->currentLevel) ? agc->attackCoef : agc->releaseCoef;
+	agc->currentLevel = levelAlpha * agc->currentLevel + (1.0f - levelAlpha) * instantLevel;
+
+	float desiredGain = agc->targetLevel / (agc->currentLevel + 1e-10f);
+	desiredGain = fminf(fmaxf(desiredGain, agc->minGain), agc->maxGain);
+
+	const float gainAlpha = (desiredGain > agc->currentGain) ? agc->attackCoef : agc->releaseCoef;
+	agc->currentGain = gainAlpha * agc->currentGain + (1.0f - gainAlpha) * desiredGain;
+
+	return agc->currentGain;
+}

filter/gain_control.h

@@ -0,0 +1,23 @@
+#pragma once
+#include <math.h>
+
+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;
+	float rmsAlpha;
+} AGC;
+
+void initAGC(AGC* agc, int sampleRate, float targetLevel, float minGain, float maxGain, float attackTime, float releaseTime);
+float process_agc(AGC* agc, float sidechain);

filter/iir.c

@@ -0,0 +1,16 @@
+#include "iir.h"
+
+void init_preemphasis(ResistorCapacitor *filter, float tau, float sample_rate, float ref_freq) {
+    float dt = 1.0f / sample_rate;
+    filter->alpha = tau / (tau + dt);
+
+    float omega = M_2PI * ref_freq / sample_rate;
+    filter->gain = 1.0f / sqrtf(1.0f + filter->alpha * filter->alpha - 2.0f * filter->alpha * cosf(omega));
+
+    filter->prev_sample = 0.0f;
+}
+inline float apply_preemphasis(ResistorCapacitor *filter, float sample) {
+	float out = (sample - filter->alpha * filter->prev_sample) * filter->gain;
+	filter->prev_sample = sample;
+	return out;
+}

filter/iir.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <math.h>
+#include "../lib/constants.h"
+
+typedef struct
+{
+	float alpha;
+	float prev_sample;
+	float gain;
+} ResistorCapacitor;
+
+void init_preemphasis(ResistorCapacitor *filter, float tau, float sample_rate, float ref_freq);
+float apply_preemphasis(ResistorCapacitor *filter, float sample);