diff --git a/lib/bs412.c b/lib/bs412.c
index 4b40b8a..5511ccd 100644
--- a/lib/bs412.c
+++ b/lib/bs412.c
@@ -1,5 +1,13 @@
 #include "bs412.h"
 
+float dbr_to_deviation(float dbr) {
+	return 19000.0f * powf(10.0f, dbr / 20.0f);
+}
+
+float deviation_to_dbr(float deviation) {
+	return 20*log10f(deviation/19000.0f);
+}
+
 void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
 	mpx->i = 1;
 	mpx->sample = 0;
@@ -7,21 +15,14 @@ void init_modulation_power_measure(MPXPowerMeasurement* mpx, int sample_rate) {
 }
 
 float measure_mpx(MPXPowerMeasurement* mpx, int deviation) {
-	mpx->sample += 20*log10f(deviation/19000.0f);
+	mpx->sample += deviation;
 
-	float modulation_power = mpx->sample/mpx->i;
+	float avg_deviation = mpx->sample/mpx->i;
+	float modulation_power = deviation_to_dbr(avg_deviation);
 
 	if (mpx->i >= mpx->sample_rate*60) {
 		mpx->sample = 0;
 		mpx->i = 1;
 	}	
 	return modulation_power;
-}
-
-float dbr_to_deviation(float dbr) {
-	return 19000.0f * powf(10.0f, dbr / 20.0f);
-}
-
-float deviation_to_dbr(float deviation) {
-	return 20*log10f(deviation/19000.0f);
 }
\ No newline at end of file
diff --git a/lib/bs412.h b/lib/bs412.h
index ddcf2df..047b629 100644
--- a/lib/bs412.h
+++ b/lib/bs412.h
@@ -8,10 +8,8 @@ typedef struct
     float sample;
 } MPXPowerMeasurement;
 
-void init_modulation_power_measure(MPXPowerMeasurement *mpx, int sample_rate);
-
-float measure_mpx(MPXPowerMeasurement *mpx, int deviation);
-
 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, int deviation);
\ No newline at end of file