From 32f3f3c273584202f14a8c26d666c518b7a883ad Mon Sep 17 00:00:00 2001
From: Thomas Schmid <tom@lfence.de>
Date: Sat, 18 Jun 2022 11:16:07 +0200
Subject: [PATCH] add dc_cancelation filter

Signed-off-by: Thomas Schmid <tom@lfence.de>
---
 firmware/include/dc_cancelation.h             | 11 ++++++++++
 firmware/include/zauberstab.h                 |  2 +-
 .../src/dc_cancelation/dc_canelcation.cpp     | 18 +++++++++++++++
 firmware/src/zauberstab.cpp                   | 22 +++++--------------
 4 files changed, 36 insertions(+), 17 deletions(-)
 create mode 100644 firmware/include/dc_cancelation.h
 create mode 100644 firmware/src/dc_cancelation/dc_canelcation.cpp

diff --git a/firmware/include/dc_cancelation.h b/firmware/include/dc_cancelation.h
new file mode 100644
index 0000000..6ddeefe
--- /dev/null
+++ b/firmware/include/dc_cancelation.h
@@ -0,0 +1,11 @@
+#pragma once
+
+struct dc_cancelation_state {
+    float x_n1;
+    float y_n1;
+    float R; 
+};
+
+void dc_cancelation_init(struct dc_cancelation_state *state, float R);
+void dc_cancelation_set_R(struct dc_cancelation_state *state, float R);
+float dc_cancelation_update(struct dc_cancelation_state *state, float x);
\ No newline at end of file
diff --git a/firmware/include/zauberstab.h b/firmware/include/zauberstab.h
index 57161fc..7c85f25 100644
--- a/firmware/include/zauberstab.h
+++ b/firmware/include/zauberstab.h
@@ -9,4 +9,4 @@
 extern CRGB leds[NUM_LEDS];
 
 int zauberstab_init();
-int32_t get_sample();
\ No newline at end of file
+float get_sample();
\ No newline at end of file
diff --git a/firmware/src/dc_cancelation/dc_canelcation.cpp b/firmware/src/dc_cancelation/dc_canelcation.cpp
new file mode 100644
index 0000000..e0faa79
--- /dev/null
+++ b/firmware/src/dc_cancelation/dc_canelcation.cpp
@@ -0,0 +1,18 @@
+#include "dc_cancelation.h"
+
+void dc_cancelation_init(struct dc_cancelation_state *state, float R){
+    state->R = R; 
+    state->x_n1 = 0.0f;
+    state->y_n1 = 0.0f;
+}
+
+void dc_cancelation_set_R(struct dc_cancelation_state *state, float R) {
+    state->R = R;
+}
+
+float dc_cancelation_update(struct dc_cancelation_state *state, float x) {
+    float y = x-state->x_n1 + state->R * state->y_n1;
+    state->x_n1 = x;
+    state->y_n1 = y;
+    return y;
+}
diff --git a/firmware/src/zauberstab.cpp b/firmware/src/zauberstab.cpp
index f5f3313..dbe8adb 100644
--- a/firmware/src/zauberstab.cpp
+++ b/firmware/src/zauberstab.cpp
@@ -1,5 +1,7 @@
 #include "zauberstab.h"
+#include "dc_cancelation.h"
 
+struct dc_cancelation_state dc_blocker;
 CRGB leds[NUM_LEDS];
 static int16_t mic_offset = 0;
 
@@ -7,25 +9,13 @@ static uint16_t read_mic() {
     return analogRead(MIC_PIN);
 }
 
-static uint16_t get_mic_offset() {
-    float average = 0.f;
-    uint32_t num_samples = 0;
-
-    for(num_samples = 0; num_samples < 10000; num_samples++) {
-        uint16_t sample = read_mic();
-        average += ((float) sample - average) / (num_samples + 1);
-    }
-    
-    return (uint16_t)average;
-}
-
 int zauberstab_init() {
     FastLED.addLeds<WS2812, LED_PIN, RGB>(leds, NUM_LEDS);
-    //FastLED.setMaxPowerInVoltsAndMilliamps(5, 300);
-    mic_offset = get_mic_offset();
+    dc_cancelation_init(&dc_blocker, 0.95);
     return 0;
 }
 
-int32_t get_sample() {
-    return read_mic() - mic_offset;
+float get_sample() {
+    int32_t raw_sample = read_mic();
+    return dc_cancelation_update(&dc_blocker, (float) raw_sample);
 }
\ No newline at end of file