From ec844f105fcc457525a808c397be0eac8e99136d Mon Sep 17 00:00:00 2001
From: Thomas Schmid <tom@lfence.de>
Date: Sun, 19 Jun 2022 11:03:02 +0200
Subject: [PATCH] add biquad filter code

Signed-off-by: Thomas Schmid <tom@lfence.de>
---
 firmware/include/biquad.h | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)
 create mode 100644 firmware/include/biquad.h

diff --git a/firmware/include/biquad.h b/firmware/include/biquad.h
new file mode 100644
index 0000000..b698234
--- /dev/null
+++ b/firmware/include/biquad.h
@@ -0,0 +1,36 @@
+#pragma once
+
+template<class T>
+struct Biquad {
+    T b0;
+    T b1;
+    T b2;
+    T a1;
+    T a2;
+    T xn1;
+    T xn2;
+    T yn1;
+    T yn2;
+
+    Biquad() = default;
+    Biquad(T a1, T a2, T b0, T b1, T b2) : b0(b0), b1(b1), b2(b2), a1(a1), a2(a2) {};
+    Biquad(T a0, T a1, T a2, T b0, T b1, T b2) {
+        this->a1 = a1/a0;
+        this->a2 = a2/a0;
+        this->b0 = b0/a0;
+        this->b1 = b1/a0;
+        this->b2 = b2/a0;
+    }
+
+    T update(T x) {
+        T y = this->b0 * x + this->b1 * this->xn1 + this->b2 * this->xn2 - this->yn1 * this->a1 - this->yn2 * this->a2;
+
+        this->xn2 = this->xn1;
+        this->xn1 = x;
+
+        this->yn2 = this->yn1;
+        this->yn1 = y;
+
+        return y;
+    }
+};
\ No newline at end of file