为定位设备加入了滤波

drivers/devicecontrol/pg0403_serial.c

@@ -7,11 +7,50 @@
 static uint8_t start_continuous[] = {0x01, 0x10, 0x00, 0x3B, 0x00, 0x01, 0x02, 0x00, 0x04, 0xA3, 0x18};
 static uint8_t stop_modbus[] = {0x01, 0x10, 0x00, 0x3B, 0x00, 0x01, 0x02, 0x00, 0x00, 0xA2, 0xDB};
 
-
 uint16_t x_local[MAX_TAGS];
 uint16_t y_local[MAX_TAGS];
 int tag_id[MAX_TAGS]; // 假设 ID 是 int 类型
 
+// 为每个可能的标签ID分配一个滤波器
+TagFilter g_filters[MAX_TAGS]; 
+
+// 初始化滤波器数据的函数 (在 main 开始调用一次)
+void init_filters() {
+    for (int i = 0; i < MAX_TAGS; i++) {
+        g_filters[i].initialized = 0;
+        g_filters[i].kx.estimate = 0;
+        g_filters[i].kx.error_cov = 1.0f;
+        g_filters[i].ky.estimate = 0;
+        g_filters[i].ky.error_cov = 1.0f;
+    }
+}
+/**
+ * 执行单次卡尔曼滤波更新
+ * @param k: 滤波器状态指针
+ * @param measurement: 当前传感器读到的原始坐标
+ * @return: 滤波后的坐标
+ */
+float run_kalman(Kalman1D *k, float measurement) {
+    // 1. 预测阶段 (Prediction)
+    // 假设物体上一刻的位置就是下一刻的位置 (静止模型)，加上过程噪声 Q
+    k->error_cov = k->error_cov + KALMAN_Q;
+
+    // 2. 更新阶段 (Update)
+    // 计算卡尔曼增益 (Kalman Gain)
+    // 增益越大，越相信传感器数据；增益越小，越相信预测值
+    float gain = k->error_cov / (k->error_cov + KALMAN_R);
+
+    // 更新估计值
+    k->estimate = k->estimate + gain * (measurement - k->estimate);
+
+    // 更新误差协方差
+    k->error_cov = (1.0f - gain) * k->error_cov;
+
+    return k->estimate;
+}
+
+
+
 // 辅助函数：将整数波特率转换为 termios 定义的 speed_t
 speed_t get_baud_rate(int baud_rate) {
     switch (baud_rate) {
@@ -187,12 +226,34 @@ void process_serial_data(const uint8_t *buffer, int len) {
             
             // 4. 数据提取与合并 (高位在前，大端模式)
             // 对应: (static_cast<uint16_t>(message[13]) << 8) | message[14]
-            x_local[i] = ((uint16_t)buffer[13] << 8) | buffer[14];
-            y_local[i] = ((uint16_t)buffer[15] << 8) | buffer[16];
+            uint16_t raw_x = ((uint16_t)buffer[13] << 8) | buffer[14];
+            uint16_t raw_y = ((uint16_t)buffer[15] << 8) | buffer[16];
+
+
+            // 3. 执行卡尔曼滤波
+            if (g_filters[i].initialized == 0) {
+                // 如果是第一次收到这个标签的数据，直接初始化为测量值
+                // 否则滤波需要很长时间才能从 0 爬升到 实际坐标
+                g_filters[i].kx.estimate = (float)raw_x;
+                g_filters[i].ky.estimate = (float)raw_y;
+                g_filters[i].kx.error_cov = 1.0f;
+                g_filters[i].ky.error_cov = 1.0f;
+                g_filters[i].initialized = 1;
+
+                x_local[i] = raw_x;
+                y_local[i] = raw_y;
+            } else {
+                // 后续数据进入滤波逻辑
+                float filtered_x = run_kalman(&g_filters[i].kx, (float)raw_x);
+                float filtered_y = run_kalman(&g_filters[i].ky, (float)raw_y);
+
+                // 4. 将滤波结果转回整数存入全局变量
+                x_local[i] = (uint16_t)(filtered_x + 0.5f); // +0.5 为了四舍五入
+                y_local[i] = (uint16_t)(filtered_y + 0.5f);
+            }
 
             tag_id[i]=i;
-            my_zlog_info("串口数据更新 -> 标签ID: %d, X: %d, Y: %d", 
-            tag_id[i], x_local[i], y_local[i]);
+            //my_zlog_debug("串口数据更新 -> 标签ID: %d, X: %d, Y: %d", tag_id[i], x_local[i], y_local[i]);
                 
         }
     }
@@ -269,7 +330,7 @@ void pg0403_serial_gpssend_mqtt_json(int id, uint16_t x, uint16_t y) {
 
 void pg0403_all_serial_send(){
     for(int i=0;i<MAX_TAGS;i++){
-        if(x_local[i]!=0) pg0403_serial_gpssend_mqtt_json(tag_id[MAX_TAGS], x_local[i], y_local[i]);
+        if(x_local[i]!=0) pg0403_serial_gpssend_mqtt_json(tag_id[i], x_local[i], y_local[i]);
     }
 }
 
@@ -279,6 +340,7 @@ int pg0403_serial_run(){
 
     if(g_device_type==DEVICE_PG_GPS0403){
     my_zlog_info("设备为串口打开设备");
+        init_filters();
         if(pg0403_serial_init_send()==0){
             my_zlog_info("串口启动成功");
         }else {
@@ -287,22 +349,22 @@ int pg0403_serial_run(){
             return -1;
         }
     }else return -1;   
-    while(1){
-        static int send_mqtt_count=0;
-        int n = read(pg_serial.fd, rx_buffer, sizeof(rx_buffer));
-        if(n>0)process_serial_data(rx_buffer, n);
-            else delay_ms(100);
-
-        if(send_mqtt_count>40){
-            pg0403_all_serial_send();
-            send_mqtt_count=0;
+        while(1){
+            static int send_mqtt_count=0;
+            int n = read(pg_serial.fd, rx_buffer, sizeof(rx_buffer));
+            if(n>0)process_serial_data(rx_buffer, n);
+                else delay_ms(100);
+
+            if(send_mqtt_count>40){
+                pg0403_all_serial_send();
+                send_mqtt_count=0;
+            }
+            delay_ms(10);
+            send_mqtt_count++;
         }
-        delay_ms(10);
-        send_mqtt_count++;
-    }
-    pg0403_serial_init_close();
-    return 0;
-   
+        pg0403_serial_init_close();
+        return 0;
+    
 }
 
 

drivers/devicecontrol/pg0403_serial.h

@@ -2,8 +2,13 @@
 #define PG0403_SERIAL_H
 
 #include "devcontrol_common.h"
-#define MAX_TAGS 20  // 假设最大标签数量
+#define MAX_TAGS 30  // 假设最大标签数量
 
+// 卡尔曼参数配置 (根据实际效果调整)
+// R (测量噪声): 值越大，滤波越平滑，但滞后越明显。防止漂移建议设大一点 (10 ~ 100)
+// Q (过程噪声): 值越大，系统认为物体运动越快。如果对运动响应太慢，把这个调大 (0.01 ~ 1.0)
+#define KALMAN_R 50.0f  
+#define KALMAN_Q 0.1f   
 
 // 定义一个结构体来模拟简单的对象上下文（可选，为了保持整洁）
 typedef struct {
@@ -12,6 +17,19 @@ typedef struct {
     int is_open;
 } SerialPort;
 
+// 单个维度的卡尔曼状态
+typedef struct {
+    float estimate;  // 当前的最优估计值
+    float error_cov; // 误差协方差 (表示我们多大程度上不确定当前的估计)
+} Kalman1D;
+
+// 每个标签包含 X 和 Y 两个维度的滤波器
+typedef struct {
+    Kalman1D kx;         // X轴滤波
+    Kalman1D ky;         // Y轴滤波
+    uint8_t initialized; // 标记该标签是否是第一次出现
+} TagFilter;
+
 int pg0403_serial_run();
 
 #endif
\ No newline at end of file