From 317118e9c4345db51cc9dd8194e5507f7dbf2771 Mon Sep 17 00:00:00 2001
From: Shengwen Cheng <shengwen1997.tw@gmail.com>
Date: Fri, 16 Feb 2024 17:25:59 +0800
Subject: [PATCH] Import low-pass filter

---
 Makefile      |  3 +++
 filters/lpf.c | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++
 filters/lpf.h | 29 +++++++++++++++++++++++++
 3 files changed, 92 insertions(+)
 create mode 100644 filters/lpf.c
 create mode 100644 filters/lpf.h

diff --git a/Makefile b/Makefile
index a681714..b3dff93 100644
--- a/Makefile
+++ b/Makefile
@@ -20,6 +20,7 @@ MSG_DIR   := ./msg
 MSG_BUILD := ./build/msg
 
 LDFLAGS += -Wl,--no-warn-rwx-segments
+LDFLAGS += -lm
 
 CFLAGS += -O2 -g -mlittle-endian -mthumb \
           -fcommon \
@@ -62,6 +63,7 @@ CFLAGS += -I ./include/fs
 CFLAGS += -I ./include/tenok
 CFLAGS += -I ./include/tenok/sys
 CFLAGS += -I ./include/kernel
+CFLAGS += -I ./filters
 CFLAGS += -I ./user
 CFLAGS += -I ./user/debug-link
 CFLAGS += -I ./build/msg
@@ -122,6 +124,7 @@ SRC += ./arch/v7m_port.c \
        ./kernel/printf.c \
        ./kernel/printk.c \
        ./kernel/softirq.c \
+       ./filters/lpf.c \
        ./main.c
 
 SRC += ./user/debug-link/debug_link.c 
diff --git a/filters/lpf.c b/filters/lpf.c
new file mode 100644
index 0000000..7c86895
--- /dev/null
+++ b/filters/lpf.c
@@ -0,0 +1,60 @@
+#include <math.h>
+
+#include "lpf.h"
+
+void lpf_first_order_init(float *ret_gain,
+                          float sampling_time,
+                          float cutoff_freq)
+{
+    /* Reference: Low-pass Filter (Wikipedia) */
+
+    /* Return the alpha value of the first order low-pass filter */
+    *ret_gain = sampling_time / (sampling_time + 1 / (2 * M_PI * cutoff_freq));
+}
+
+void lpf_first_order(float new, float *filtered, float alpha)
+{
+    *filtered = (new *alpha) + (*filtered * (1.0f - alpha));
+}
+
+void lpf_second_order_init(lpf2_t *lpf, float sampling_freq, float cutoff_freq)
+{
+    /* Reference: How does a low-pass filter programmatically work? */
+
+    float alpha = tan(M_PI * cutoff_freq / sampling_freq);
+    float sqrt2 = sqrt(2.0f);
+    float alpha_squared = alpha * alpha;
+    float sqrt2_alpha = sqrt2 * alpha;
+
+    /* Initialize filter states */
+    lpf->filter_last = 0.0f;
+    lpf->filter_last_last = 0.0f;
+    lpf->input_last = 0.0f;
+    lpf->input_last_last = 0.0f;
+
+    /* Calculate gain coefficients */
+    lpf->k = alpha_squared / (1.0f + sqrt2_alpha + alpha_squared);
+    lpf->a1 =
+        (2.0f * (alpha_squared - 1.0f)) / (1.0f + sqrt2_alpha + alpha_squared);
+    lpf->a2 = (1.0f - sqrt2_alpha + alpha_squared) /
+              (1.0f + sqrt2_alpha + alpha_squared);
+    lpf->b1 = 2.0f;
+    lpf->b2 = 1.0f;
+}
+
+void lpf_second_order(float new_input, float *filtered_data, lpf2_t *lpf)
+{
+    /* Reference: How does a low-pass filter programmatically work? */
+
+    float result = (lpf->k * new_input) + (lpf->k * lpf->b1 * lpf->input_last) +
+                   (lpf->k * lpf->b2 * lpf->input_last_last) -
+                   (lpf->a1 * lpf->filter_last) -
+                   (lpf->a2 * lpf->filter_last_last);
+
+    lpf->filter_last_last = lpf->filter_last;
+    lpf->filter_last = result;
+    lpf->input_last_last = lpf->input_last;
+    lpf->input_last = new_input;
+
+    *filtered_data = result;
+}
diff --git a/filters/lpf.h b/filters/lpf.h
new file mode 100644
index 0000000..77a67e5
--- /dev/null
+++ b/filters/lpf.h
@@ -0,0 +1,29 @@
+#ifndef __LPF_H__
+#define __LPF_H__
+
+/* Second order low-pass filter */
+typedef struct {
+    float k;
+    float a1;
+    float a2;
+    float b1;
+    float b2;
+
+    float filter_last;
+    float filter_last_last;
+
+    float input_last;
+    float input_last_last;
+} lpf2_t;
+
+void lpf_first_order_init(float *ret_gain,
+                          float sampling_time,
+                          float cutoff_freq);
+void lpf_first_order(float new, float *filtered, float alpha);
+
+void lpf_second_order_init(lpf2_t *lpf,
+                           float sampling_freq,
+                           float cuttoff_freq);
+void lpf_second_order(float new_input, float *filtered_data, lpf2_t *lpf);
+
+#endif