From e730f13a6d32bbff78d3ccda600299345c888f5b Mon Sep 17 00:00:00 2001
From: KubaPro010 <kuba.pro010@gmail.com>
Date: Fri, 18 Apr 2025 22:02:08 +0200
Subject: [PATCH] make the bs412 actually work (use rms)

---
 lib/bs412.c | 27 +++++++++++++++------------
 lib/bs412.h |  3 ++-
 lib/debug.h |  4 ++++
 3 files changed, 21 insertions(+), 13 deletions(-)
 create mode 100644 lib/debug.h

diff --git a/lib/bs412.c b/lib/bs412.c
index 4a0abde..13ab34d 100644
--- a/lib/bs412.c
+++ b/lib/bs412.c
@@ -1,11 +1,11 @@
 #include "bs412.h"
 
 float dbr_to_deviation(float dbr) {
-	return 19000.0f * powf(10.0f, dbr / 20.0f);
+	return (19000.0f * 0.70710678f) * powf(10.0f, dbr / 20.0f);
 }
 
 float deviation_to_dbr(float deviation) {
-	return 20 * log10f((deviation + 1e-6f) / 19000.0f);
+	return 20 * log10f((deviation + 1e-6f) / (19000.0f * 0.70710678f));
 }
 
 void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
@@ -15,16 +15,19 @@ void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
 }
 
 float measure_mpx(MPXPowerMeasurement* mpx, float deviation) {
-	mpx->sample += deviation;
+    mpx->sample += deviation * deviation;
+    mpx->i++;
 
-	float avg_deviation = (float)mpx->sample / mpx->i;
-	float modulation_power = deviation_to_dbr(avg_deviation);
+    float avg_deviation = sqrtf(mpx->sample / mpx->i);
+    float modulation_power = deviation_to_dbr(avg_deviation);
 
-	mpx->i++;
+    if (mpx->i == 1 || (mpx->i % mpx->sample_rate) == 0) {
+    }
 
-	if (mpx->i >= mpx->sample_rate*60) {
-		mpx->sample = avg_deviation;
-		mpx->i = 2;
-	}	
-	return modulation_power;
-}
\ No newline at end of file
+    if (mpx->i >= mpx->sample_rate * 60) {
+        mpx->sample = 0;
+        mpx->i = 0;
+    }
+
+    return modulation_power;
+}
diff --git a/lib/bs412.h b/lib/bs412.h
index eab4b63..bcd45ab 100644
--- a/lib/bs412.h
+++ b/lib/bs412.h
@@ -1,11 +1,12 @@
 #pragma once
+#include "debug.h"
 #include <math.h>
 
 typedef struct
 {
     int i;
     int sample_rate;
-    float sample;
+    double sample;
 } MPXPowerMeasurement;
 
 float dbr_to_deviation(float dbr);
diff --git a/lib/debug.h b/lib/debug.h
new file mode 100644
index 0000000..7d041ac
--- /dev/null
+++ b/lib/debug.h
@@ -0,0 +1,4 @@
+#pragma once
+#include <stdio.h>
+#define debug_printf(fmt, ...) \
+    printf("[%s:%d in %s] " fmt, __FILE__, __LINE__, __func__, ##__VA_ARGS__)