From 3a1904511fc63fb80af29ff4043072af51981b7f Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Sat, 29 Mar 2025 19:01:19 +0100
Subject: [PATCH] try to measure mpx power

---
 .vscode/.server-controller-port.log |  2 +-
 lib/bs412.c                         | 23 +++++++++++++++++++++++
 lib/bs412.h                         | 13 +++++++++++++
 src/fm95.c                          | 10 ++++++++++
 4 files changed, 47 insertions(+), 1 deletion(-)
 create mode 100644 lib/bs412.c
 create mode 100644 lib/bs412.h

diff --git a/.vscode/.server-controller-port.log b/.vscode/.server-controller-port.log
index a20bb8f..5f0cf8f 100644
--- a/.vscode/.server-controller-port.log
+++ b/.vscode/.server-controller-port.log
@@ -1,5 +1,5 @@
 {
   "port": 13452,
-  "time": 1743162622776,
+  "time": 1743269976272,
   "version": "0.0.3"
 }
\ No newline at end of file
diff --git a/lib/bs412.c b/lib/bs412.c
new file mode 100644
index 0000000..7dc9507
--- /dev/null
+++ b/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;
+	}
+}
\ No newline at end of file
diff --git a/lib/bs412.h b/lib/bs412.h
new file mode 100644
index 0000000..6338bbe
--- /dev/null
+++ b/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);
diff --git a/src/fm95.c b/src/fm95.c
index f8cd040..010ba0e 100644
--- a/src/fm95.c
+++ b/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;
 		}