try to measure mpx power

.vscode/.server-controller-port.log

@@ -1,5 +1,5 @@
 {
   "port": 13452,
-  "time": 1743162622776,
+  "time": 1743269976272,
   "version": "0.0.3"
 }

lib/bs412.c

@@ -0,0 +1,23 @@
+#include "bs412.h"
+
+void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
+	mpx->i = 0;
+	mpx->sample = 0;
+	mpx->sample_rate = sample_rate;
+}
+
+float measure_mpx(MPXPowerMeasurement* mpx, int deviation) {
+	mpx->sample += 20*log10f(deviation/19);
+	mpx->i++;
+
+	if (mpx->i >= mpx->sample_rate) {
+		float modulation_power = mpx->sample/mpx->i;
+
+		mpx->sample = 0.0f;
+		mpx->i = 0;
+
+		return modulation_power;
+	} else {
+		return mpx->sample/mpx->i;
+	}
+}

lib/bs412.h

@@ -0,0 +1,13 @@
+#pragma once
+#include <math.h>
+
+typedef struct
+{
+    int i;
+    int sample_rate;
+    float sample;
+} MPXPowerMeasurement;
+
+void init_modulation_power_measure(MPXPowerMeasurement *mpx, int sample_rate);
+
+float measure_mpx(MPXPowerMeasurement *mpx, int deviation);

src/fm95.c

@@ -19,6 +19,7 @@
 #include "../lib/filters.h"
 #include "../lib/fm_modulator.h"
 #include "../lib/optimization.h"
+#include "../lib/bs412.h"
 
 #define DEFAULT_SAMPLE_RATE 192000
 
@@ -428,6 +429,9 @@ int main(int argc, char **argv) {
 	init_preemphasis(&preemp_l, preemphasis_tau, sample_rate);
 	init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
 
+	MPXPowerMeasurement power;
+	init_modulation_power_measure(&power, sample_rate);
+
 	signal(SIGINT, stop);
 	signal(SIGTERM, stop);
 
@@ -511,6 +515,12 @@ int main(int argc, char **argv) {
 			if(rds_on || stereo) advance_oscillator(&osc);
 			if(mpx_on) output[i] += hard_clip(current_mpx_in, MPX_CLIPPER_THRESHOLD)*MPX_VOLUME;
 			if(sca_on) output[i] += modulate_fm(&sca_mod, hard_clip(current_sca_in, sca_clipper_threshold))*SCA_VOLUME;
+			
+			float mpower = measure_mpx(&power, output[i]*75000);
+			if(mpower > 3) {
+				printf("MPX Power over 3 dbr");
+			}
+
 			output[i] *= master_volume;
 		}