From 28ba5433819e7d3b735eebf551ee3bf764db840e Mon Sep 17 00:00:00 2001
From: Thomas Schmid <tom@lfence.de>
Date: Sat, 18 Jun 2022 11:33:56 +0200
Subject: [PATCH] add pt1 filter code

Signed-off-by: Thomas Schmid <tom@lfence.de>
---
 firmware/include/pt1.h   | 10 ++++++++++
 firmware/src/pt1.cpp     | 15 +++++++++++++++
 firmware/src/vumeter.cpp | 14 +++++++++++---
 3 files changed, 36 insertions(+), 3 deletions(-)
 create mode 100644 firmware/include/pt1.h
 create mode 100644 firmware/src/pt1.cpp

diff --git a/firmware/include/pt1.h b/firmware/include/pt1.h
new file mode 100644
index 0000000..8e9a039
--- /dev/null
+++ b/firmware/include/pt1.h
@@ -0,0 +1,10 @@
+#pragma once
+
+struct pt1_state {
+    float y_n1;
+    float K;
+    float T;
+};
+
+void pt1_init(struct pt1_state *state, float K, float T);
+float pt1_update(struct pt1_state *state, float u, float dt);
\ No newline at end of file
diff --git a/firmware/src/pt1.cpp b/firmware/src/pt1.cpp
new file mode 100644
index 0000000..7c9b623
--- /dev/null
+++ b/firmware/src/pt1.cpp
@@ -0,0 +1,15 @@
+#include "pt1.h"
+
+void pt1_init(struct pt1_state *state, float K, float T)
+{
+    state->K = K;
+    state->T = T;
+    state->y_n1 = 0.0f;
+}
+
+float pt1_update(struct pt1_state *state, float u, float dt)
+{
+    float y = state->y_n1 + (state->K * u - state->y_n1) * dt / state->T;
+    state->y_n1 = y;
+    return y;
+}
\ No newline at end of file
diff --git a/firmware/src/vumeter.cpp b/firmware/src/vumeter.cpp
index c8b17ca..2d5ac39 100644
--- a/firmware/src/vumeter.cpp
+++ b/firmware/src/vumeter.cpp
@@ -1,4 +1,5 @@
 #include "zauberstab.h"
+#include "pt1.h"
 
 unsigned long last_sample_time;
 static int sample_counter = 0;
@@ -6,12 +7,19 @@ unsigned int top_led_pos = 0;
 float rms_avg = 0;
 float vu_filt = 0.0f;
 float vu_filt_slow = 0.0f;
+float dt;
+
+struct pt1_state vu_pt1_fast;
+struct pt1_state vu_pt1_slow;
 
 void setup()
 {
     zauberstab_init();
     Serial.begin(115200);
     FastLED.setBrightness(100);
+
+    pt1_init(&vu_pt1_slow, 1, 1.f);
+    pt1_init(&vu_pt1_fast, 1, 0.05);
 }
 
 void loop() {
@@ -26,8 +34,8 @@ void loop() {
     EVERY_N_MILLIS(10){
 
         float vu = 20 * log10f(rms_avg);
-        vu_filt = (vu-vu_filt) * 0.8 + vu_filt;
-        vu_filt_slow = (vu_filt-vu_filt_slow) * 0.01 + vu_filt_slow;
+        vu_filt = pt1_update(&vu_pt1_fast, vu, 0.01f);
+        vu_filt_slow = pt1_update(&vu_pt1_slow, vu_filt, 0.01f);
         //Serial.println(vu);
         int max_led = vu_filt;
         int top_led = vu_filt_slow;
@@ -35,7 +43,7 @@ void loop() {
         max_led = max_led > 0xFF ? 0xFF : max_led;
 
         if (top_led < max_led){
-            vu_filt_slow = vu_filt;
+            vu_pt1_slow.y_n1 = vu_filt;
             top_led = max_led;
         }