From 117777c09298630c98391b01a64dc1c427b824ee Mon Sep 17 00:00:00 2001
From: 95384 <664090429@qq.com>
Date: Sat, 18 Jan 2025 08:52:38 +0800
Subject: [PATCH] =?UTF-8?q?=E5=9C=A8=E4=B8=80=E9=98=B6=E4=BD=8E=E9=80=9A?=
 =?UTF-8?q?=E6=BB=A4=E6=B3=A2=E5=99=A8=E5=90=8E=EF=BC=8C=E5=8E=BB=E9=99=A4?=
 =?UTF-8?q?=E6=B3=A2=E5=8A=A8=E8=BE=83=E5=A4=A7=E7=9A=84=E6=B3=A2=E5=8A=A8?=
 =?UTF-8?q?=E5=80=BC=EF=BC=8C=E8=A7=A3=E5=86=B3=E4=BA=86=E6=9C=89=E6=B0=B4?=
 =?UTF-8?q?=E6=97=B6=E8=A7=A6=E6=91=B8=E5=AF=BC=E8=87=B4=E7=9A=84=E9=A3=8E?=
 =?UTF-8?q?=E9=80=9F=E7=AA=81=E5=8F=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Drivers/Filter/LowPassFilter.c | 38 ++++++++++++++++++++++++++++++----
 Drivers/Filter/LowPassFilter.h |  2 ++
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/Drivers/Filter/LowPassFilter.c b/Drivers/Filter/LowPassFilter.c
index 49c4da5..f4879c8 100644
--- a/Drivers/Filter/LowPassFilter.c
+++ b/Drivers/Filter/LowPassFilter.c
@@ -10,14 +10,44 @@ void initLowPassFilter(SL_LowPassFilter* filter, float alpha)
     filter->alpha = alpha;
     // 初始化上一次的输出值为0
     filter->previous = 0.0f;
+    
+    filter->x = 0;
+    filter->times = 0;
 }
 
 // 更新滤波器输出值
 float updateFilter(SL_LowPassFilter* filter, float new_input) 
 {
-    // 更新滤波后的值
-    float output = (1.0f - filter->alpha) * filter->previous + filter->alpha * new_input;
+    // 滤波新值
+    float filtedData = (1.0f - filter->alpha) * filter->previous + filter->alpha * new_input;
+    
+    //差值大于一定值认为有问题，暂定4
+    if((filter->x) - filtedData > 4 || (filtedData) - filter->x > 4 )
+    {
+//        将有问题的值存起来
+        (filter->x) = filtedData;
+//        使用上一次的正确值当结果
+        filtedData = filter->previous;
+//        清空计数
+        filter->times = 0;
+    }else
+    {
+//        差值在允许范围内
+//        差值合理一定时间后认为数据没问题
+        if(filter->times < 3)
+        {
+            filter->times++;
+//            没满足次数,使用旧值
+            filtedData = filter->previous;
+        }
+        else
+        {
+//            将值存起来，直接使用滤波后的值
+            (filter->x) = filtedData;
+        }
+    }
+    
     // 更新上一次的输出值
-    filter->previous = output;
-    return output;
+    filter->previous = filtedData;
+    return filtedData;
 }
\ No newline at end of file
diff --git a/Drivers/Filter/LowPassFilter.h b/Drivers/Filter/LowPassFilter.h
index 53fa63b..00d2d91 100644
--- a/Drivers/Filter/LowPassFilter.h
+++ b/Drivers/Filter/LowPassFilter.h
@@ -9,6 +9,8 @@
 typedef struct {
     float alpha;      // 滤波器系数
     float previous;   // 上一次的输出值
+    float x;          // 上一次的输入值
+    uint8_t times;          // 次数
 } SL_LowPassFilter;
 
 extern SL_LowPassFilter low_pass_filter_x;