Merge branch 'feature/add_positioning' into 'master'

app/device_identity/device_identity.c

@@ -98,6 +98,7 @@ int hash_insert_init(HashTable_t *HashTable_t) {
     insert(HashTable_t, "0206", TANK_0206);   
     insert(HashTable_t, "0301", SHIP_0301);
     insert(HashTable_t, "0401", PAO_0401);
+    insert(HashTable_t, "0403", PGGPS_0403);
     insert(HashTable_t, "0501", ROBOT_DOG0501);
 }
 
@@ -132,6 +133,9 @@ int device_judg(CodeEnum_t code,char *sub_str) {
     }else if(code ==PAO_0401) {
         device_init(DEVICE_PAO_PTZ0401);
         my_zlog_info("使用型号%s",sub_str);   
+    }else if(code ==PGGPS_0403) {
+        device_init(DEVICE_PG_GPS0403);
+        my_zlog_info("使用定位设备%s",sub_str);   
     }else if(code ==ROBOT_DOG0501) {
         device_init(DEVICE_ROBOT_DOG0501);
         my_zlog_info("使用型号%s",sub_str);   

app/device_identity/device_identity.h

@@ -19,6 +19,7 @@ typedef enum {
     TANK_0206,
     SHIP_0301,
     PAO_0401,
+    PGGPS_0403,
     ROBOT_DOG0501
 } CodeEnum_t;
 

app/main/pthread_open.c

@@ -39,7 +39,10 @@ int thread_start_init(ThreadFunc thread_exit_time, ThreadFunc thread_mqtt_beat,
 
 //出现意外自动停止
 void *thread_exit_time(void *arg) { 
-    while(1){  
+      pg0403_serial_run();
+    while(1){
+      
+
      if(get_self_control_index()==false){
 
         delay_ms(100);
@@ -99,12 +102,17 @@ void *thread_mqtt_beat(void *arg) {
 //打开游览器线程，让游览器一直在一个线程中打开并运行
 void *thread_open_browser(void *arg) { 
 
+    if(g_device_type==DEVICE_PG_GPS0403){
+         return NULL;
+    }
+
     int result = system("pkill  chromium");
     if (result != 0) {
         my_zlog_error("system error");
     }
     delay_s(5);
     while(1){
+
         if(g_webrtc_index==1) {
             if(is_browser_running() == true ) {
                 continue;

build/Makefile

@@ -641,6 +641,30 @@ drivers/devicecontrol/devcontrol_common.c.s:
 	$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/devcontrol_common.c.s
 .PHONY : drivers/devicecontrol/devcontrol_common.c.s
 
+drivers/devicecontrol/pg0403_serial.o: drivers/devicecontrol/pg0403_serial.c.o
+.PHONY : drivers/devicecontrol/pg0403_serial.o
+
+# target to build an object file
+drivers/devicecontrol/pg0403_serial.c.o:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/pg0403_serial.c.o
+.PHONY : drivers/devicecontrol/pg0403_serial.c.o
+
+drivers/devicecontrol/pg0403_serial.i: drivers/devicecontrol/pg0403_serial.c.i
+.PHONY : drivers/devicecontrol/pg0403_serial.i
+
+# target to preprocess a source file
+drivers/devicecontrol/pg0403_serial.c.i:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/pg0403_serial.c.i
+.PHONY : drivers/devicecontrol/pg0403_serial.c.i
+
+drivers/devicecontrol/pg0403_serial.s: drivers/devicecontrol/pg0403_serial.c.s
+.PHONY : drivers/devicecontrol/pg0403_serial.s
+
+# target to generate assembly for a file
+drivers/devicecontrol/pg0403_serial.c.s:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/main.dir/build.make CMakeFiles/main.dir/drivers/devicecontrol/pg0403_serial.c.s
+.PHONY : drivers/devicecontrol/pg0403_serial.c.s
+
 drivers/devicecontrol/ptz0401_control.o: drivers/devicecontrol/ptz0401_control.c.o
 .PHONY : drivers/devicecontrol/ptz0401_control.o
 
@@ -1952,6 +1976,9 @@ help:
 	@echo "... drivers/devicecontrol/devcontrol_common.o"
 	@echo "... drivers/devicecontrol/devcontrol_common.i"
 	@echo "... drivers/devicecontrol/devcontrol_common.s"
+	@echo "... drivers/devicecontrol/pg0403_serial.o"
+	@echo "... drivers/devicecontrol/pg0403_serial.i"
+	@echo "... drivers/devicecontrol/pg0403_serial.s"
 	@echo "... drivers/devicecontrol/ptz0401_control.o"
 	@echo "... drivers/devicecontrol/ptz0401_control.i"
 	@echo "... drivers/devicecontrol/ptz0401_control.s"

build/third_party/mosquitto/CMakeFiles/progress.marks

-77
+76

build/third_party/mosquitto/apps/mosquitto_ctrl/CMakeFiles/mosquitto_ctrl.dir/progress.make

 CMAKE_PROGRESS_1 = 
-CMAKE_PROGRESS_2 = 
-CMAKE_PROGRESS_3 = 81
+CMAKE_PROGRESS_2 = 81
+CMAKE_PROGRESS_3 = 
 CMAKE_PROGRESS_4 = 
 CMAKE_PROGRESS_5 = 82
 CMAKE_PROGRESS_6 = 

build/third_party/mosquitto/lib/CMakeFiles/libmosquitto.dir/progress.make

@@ -34,8 +34,8 @@ CMAKE_PROGRESS_33 =
 CMAKE_PROGRESS_34 = 
 CMAKE_PROGRESS_35 = 12
 CMAKE_PROGRESS_36 = 
-CMAKE_PROGRESS_37 = 13
-CMAKE_PROGRESS_38 = 
+CMAKE_PROGRESS_37 = 
+CMAKE_PROGRESS_38 = 13
 CMAKE_PROGRESS_39 = 
 CMAKE_PROGRESS_40 = 14
 CMAKE_PROGRESS_41 = 

build/third_party/mosquitto/lib/CMakeFiles/libmosquitto_static.dir/progress.make

@@ -7,8 +7,8 @@ CMAKE_PROGRESS_6 =
 CMAKE_PROGRESS_7 = 
 CMAKE_PROGRESS_8 = 18
 CMAKE_PROGRESS_9 = 
-CMAKE_PROGRESS_10 = 19
-CMAKE_PROGRESS_11 = 
+CMAKE_PROGRESS_10 = 
+CMAKE_PROGRESS_11 = 19
 CMAKE_PROGRESS_12 = 
 CMAKE_PROGRESS_13 = 20
 CMAKE_PROGRESS_14 = 
@@ -24,11 +24,11 @@ CMAKE_PROGRESS_23 =
 CMAKE_PROGRESS_24 = 
 CMAKE_PROGRESS_25 = 24
 CMAKE_PROGRESS_26 = 
-CMAKE_PROGRESS_27 = 25
-CMAKE_PROGRESS_28 = 
+CMAKE_PROGRESS_27 = 
+CMAKE_PROGRESS_28 = 25
 CMAKE_PROGRESS_29 = 
-CMAKE_PROGRESS_30 = 26
-CMAKE_PROGRESS_31 = 
+CMAKE_PROGRESS_30 = 
+CMAKE_PROGRESS_31 = 26
 CMAKE_PROGRESS_32 = 
 CMAKE_PROGRESS_33 = 27
 CMAKE_PROGRESS_34 = 

build/third_party/mosquitto/plugins/dynamic-security/CMakeFiles/mosquitto_dynamic_security.dir/progress.make

@@ -3,8 +3,8 @@ CMAKE_PROGRESS_2 =
 CMAKE_PROGRESS_3 = 
 CMAKE_PROGRESS_4 = 86
 CMAKE_PROGRESS_5 = 
-CMAKE_PROGRESS_6 = 
-CMAKE_PROGRESS_7 = 87
+CMAKE_PROGRESS_6 = 87
+CMAKE_PROGRESS_7 = 
 CMAKE_PROGRESS_8 = 
 CMAKE_PROGRESS_9 = 88
 CMAKE_PROGRESS_10 = 

build/third_party/mosquitto/src/CMakeFiles/mosquitto.dir/progress.make

-CMAKE_PROGRESS_1 = 54
+CMAKE_PROGRESS_1 = 
 CMAKE_PROGRESS_2 = 
-CMAKE_PROGRESS_3 = 
-CMAKE_PROGRESS_4 = 55
+CMAKE_PROGRESS_3 = 55
+CMAKE_PROGRESS_4 = 
 CMAKE_PROGRESS_5 = 
-CMAKE_PROGRESS_6 = 
-CMAKE_PROGRESS_7 = 56
+CMAKE_PROGRESS_6 = 56
+CMAKE_PROGRESS_7 = 
 CMAKE_PROGRESS_8 = 
 CMAKE_PROGRESS_9 = 57
 CMAKE_PROGRESS_10 = 
@@ -14,14 +14,14 @@ CMAKE_PROGRESS_13 =
 CMAKE_PROGRESS_14 = 
 CMAKE_PROGRESS_15 = 59
 CMAKE_PROGRESS_16 = 
-CMAKE_PROGRESS_17 = 
-CMAKE_PROGRESS_18 = 60
+CMAKE_PROGRESS_17 = 60
+CMAKE_PROGRESS_18 = 
 CMAKE_PROGRESS_19 = 
-CMAKE_PROGRESS_20 = 
-CMAKE_PROGRESS_21 = 61
+CMAKE_PROGRESS_20 = 61
+CMAKE_PROGRESS_21 = 
 CMAKE_PROGRESS_22 = 
-CMAKE_PROGRESS_23 = 
-CMAKE_PROGRESS_24 = 62
+CMAKE_PROGRESS_23 = 62
+CMAKE_PROGRESS_24 = 
 CMAKE_PROGRESS_25 = 
 CMAKE_PROGRESS_26 = 63
 CMAKE_PROGRESS_27 = 
@@ -34,11 +34,11 @@ CMAKE_PROGRESS_33 =
 CMAKE_PROGRESS_34 = 
 CMAKE_PROGRESS_35 = 66
 CMAKE_PROGRESS_36 = 
-CMAKE_PROGRESS_37 = 
-CMAKE_PROGRESS_38 = 67
+CMAKE_PROGRESS_37 = 67
+CMAKE_PROGRESS_38 = 
 CMAKE_PROGRESS_39 = 
-CMAKE_PROGRESS_40 = 
-CMAKE_PROGRESS_41 = 68
+CMAKE_PROGRESS_40 = 68
+CMAKE_PROGRESS_41 = 
 CMAKE_PROGRESS_42 = 
 CMAKE_PROGRESS_43 = 69
 CMAKE_PROGRESS_44 = 
@@ -54,8 +54,8 @@ CMAKE_PROGRESS_53 =
 CMAKE_PROGRESS_54 = 
 CMAKE_PROGRESS_55 = 73
 CMAKE_PROGRESS_56 = 
-CMAKE_PROGRESS_57 = 
-CMAKE_PROGRESS_58 = 74
+CMAKE_PROGRESS_57 = 74
+CMAKE_PROGRESS_58 = 
 CMAKE_PROGRESS_59 = 
 CMAKE_PROGRESS_60 = 75
 CMAKE_PROGRESS_61 = 
@@ -71,6 +71,6 @@ CMAKE_PROGRESS_70 =
 CMAKE_PROGRESS_71 = 
 CMAKE_PROGRESS_72 = 79
 CMAKE_PROGRESS_73 = 
-CMAKE_PROGRESS_74 = 
-CMAKE_PROGRESS_75 = 80
+CMAKE_PROGRESS_74 = 80
+CMAKE_PROGRESS_75 = 
 

build/third_party/mosquitto/src/CMakeFiles/progress.marks

-27
+26

drivers/devicecontrol/car0102_control.c

@@ -110,59 +110,76 @@ void car0102_mode_2_back(unsigned char gval) {
 }
 
 void car0102_mode_3_left(unsigned char gval) {
-	if (gval < 45) {
+	int b=7;
+	if(gval<45){
 		car0102_calculate_L_R(90);
-	} else if (gval <= 51) {
-		car0102_calculate_L_R(110);
-	} else if (gval <= 57) {
-		car0102_calculate_L_R(120);
-	} else if (gval <= 63) {
-		car0102_calculate_L_R(130);
-	} else if (gval <= 69) {
-		car0102_calculate_L_R(130);
-	} else if (gval <= 75) {
-		car0102_calculate_L_R(140);
-	} else if (gval <= 81) {
-		car0102_calculate_L_R(145);
-	} else if (gval <= 87) {
-		car0102_calculate_L_R(150);
-	} else if (gval <= 93) {
-		car0102_calculate_L_R(150);
-	} else if (gval <= 100) {
-		car0102_calculate_L_R(160);
-	}else if (gval <= 107) {
-		car0102_calculate_L_R(170);
-	}else if (gval <= 120) {
-		car0102_calculate_L_R(180);
-	}	
+	}else if(gval<70){
+		car0102_calculate_L_R(50+gval+b);
+	}else if(gval>=70){
+		car0102_calculate_L_R(135);
+	}
+	// if (gval < 45) {
+	// 	car0102_calculate_L_R(90);
+	// } else if (gval <= 51) {
+	// 	car0102_calculate_L_R(110);
+	// } else if (gval <= 57) {
+	// 	car0102_calculate_L_R(120);
+	// } else if (gval <= 63) {
+	// 	car0102_calculate_L_R(130);
+	// } else if (gval <= 69) {
+	// 	car0102_calculate_L_R(135);
+	// } else if (gval <= 75) {
+	// 	car0102_calculate_L_R(140);
+	// } else if (gval <= 81) {
+	// 	car0102_calculate_L_R(145);
+	// } else if (gval <= 87) {
+	// 	car0102_calculate_L_R(150);
+	// } else if (gval <= 93) {
+	// 	car0102_calculate_L_R(150);
+	// } else if (gval <= 100) {
+	// 	car0102_calculate_L_R(160);
+	// }else if (gval <= 107) {
+	// 	car0102_calculate_L_R(170);
+	// }else if (gval <= 120) {
+	// 	car0102_calculate_L_R(180);
+	// }	
 }
 
 void car0102_mode_4_right(unsigned char gval) {
-	if (gval < 45) {
+	int b=5;
+	if(gval<45){
 		car0102_calculate_L_R(90);
-	} else if (gval <= 51) {
-		car0102_calculate_L_R(70);
-	} else if (gval <= 57) {
-		car0102_calculate_L_R(66);
-	} else if (gval <= 63) {
-		car0102_calculate_L_R(62);
-	} else if (gval <= 69) {
-		car0102_calculate_L_R(55);
-	} else if (gval <= 75) {
-		car0102_calculate_L_R(45 );
-	} else if (gval <= 81) {
-		car0102_calculate_L_R(40);
-	} else if (gval <= 87) {
-		car0102_calculate_L_R(30);
-	} else if (gval <= 93) {
-		car0102_calculate_L_R(30);
-	} else if (gval <= 100) {
-		car0102_calculate_L_R(20);
-	} else if (gval <= 107) {
-		car0102_calculate_L_R(10);
-	} else if (gval <= 120) {
-		car0102_calculate_L_R(0);
-	}         
+	}else if(gval<70){
+		car0102_calculate_L_R(130-gval-b);
+	}else if(gval>=70){
+		car0102_calculate_L_R(135);
+	}
+	
+	// if (gval < 45) {
+	// 	car0102_calculate_L_R(90);
+	// } else if (gval <= 51) {
+	// 	car0102_calculate_L_R(70);
+	// } else if (gval <= 57) {
+	// 	car0102_calculate_L_R(66);
+	// } else if (gval <= 63) {
+	// 	car0102_calculate_L_R(62);
+	// } else if (gval <= 69) {
+	// 	car0102_calculate_L_R(55);
+	// } else if (gval <= 75) {
+	// 	car0102_calculate_L_R(45 );
+	// } else if (gval <= 81) {
+	// 	car0102_calculate_L_R(40);
+	// } else if (gval <= 87) {
+	// 	car0102_calculate_L_R(30);
+	// } else if (gval <= 93) {
+	// 	car0102_calculate_L_R(30);
+	// } else if (gval <= 100) {
+	// 	car0102_calculate_L_R(20);
+	// } else if (gval <= 107) {
+	// 	car0102_calculate_L_R(10);
+	// } else if (gval <= 120) {
+	// 	car0102_calculate_L_R(0);
+	// }         
 }
 
 //车速度和转向引脚数值处理函数

drivers/devicecontrol/devcontrol_common.h

@@ -13,6 +13,7 @@
 #include "tank0204_control.h"
 #include "tank0206_control.h"
 #include "ship0301_control.h"
+#include "pg0403_serial.h"
 #include "tank_common.h"
 #include "common.h"
 
@@ -27,8 +28,10 @@
 #define DEVICE_TANK0206 206 //可以喷水坦克
 #define DEVICE_SHIP0301 301 // 32号船
 #define DEVICE_PAO_PTZ0401 401 //云台
+#define DEVICE_PG_GPS0403 403 //定位设备
 #define DEVICE_ROBOT_DOG0501 501 //机械狗
 
+
 /*
 *以下为大类
 */

drivers/devicecontrol/pg0403_serial.c

+#include "pg0403_serial.h"
+#include "app_device_common.h"
+#include "drivers_common.h"
+#include "modules_common.h"
+
+// 预设指令
+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) {
+        case 9600: return B9600;
+        case 19200: return B19200;
+        case 38400: return B38400;
+        case 57600: return B57600;
+        case 115200: return B115200;
+        case 230400: return B230400;
+        case 460800: return B460800;
+        case 921600: return B921600;
+        default: return B9600; // 默认
+    }
+}
+
+/**
+ * 打开串口
+ * 对应原 C++: bool SerialPort::open(speed_t baud_rate)
+ */
+int serial_open(SerialPort *sp, const char *port_name, int baud_rate) {
+    if (sp->is_open) {
+        fprintf(stderr, "Warning: Port %s is already open.\n", sp->port_name);
+        return 1; // 视为成功
+    }
+
+    // 保存端口名
+    strncpy(sp->port_name, port_name, sizeof(sp->port_name) - 1);
+
+    // 打开串口
+    // O_RDWR: 读写模式
+    // O_NOCTTY: 不作为控制终端
+    // O_NDELAY: 非阻塞打开（防止DCD线不在线时在此阻塞）
+    sp->fd = open(sp->port_name, O_RDWR | O_NOCTTY | O_NDELAY);
+    if (sp->fd == -1) {
+        fprintf(stderr, "Error: Failed to open serial port %s - %s\n", 
+                sp->port_name, strerror(errno));
+        return 0; // 失败
+    }
+
+    // 恢复为阻塞模式（对应原代码中的 fcntl(fd_, F_SETFL, 0)）
+    fcntl(sp->fd, F_SETFL, 0);
+
+    struct termios options;
+    if (tcgetattr(sp->fd, &options) != 0) {
+        fprintf(stderr, "Error: tcgetattr failed - %s\n", strerror(errno));
+        close(sp->fd);
+        sp->fd = -1;
+        return 0;
+    }
+
+    // 设置波特率
+    speed_t speed = get_baud_rate(baud_rate);
+    cfsetispeed(&options, speed);
+    cfsetospeed(&options, speed);
+
+    // 设置控制标志 (8N1 配置)
+    options.c_cflag &= ~PARENB;   // 无校验
+    options.c_cflag &= ~CSTOPB;   // 1位停止位
+    options.c_cflag &= ~CSIZE;    // 清除数据位掩码
+    options.c_cflag |= CS8;       // 8位数据位
+    
+    // 启用接收器并设置本地模式
+    options.c_cflag |= (CLOCAL | CREAD);
+
+    // 设置输入标志 (禁用软件流控)
+    options.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+    // 设置本地标志 (Raw模式: 无规范模式，无回显，无信号)
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+    // 设置输出标志 (Raw模式)
+    options.c_oflag &= ~OPOST;
+
+    // 设置控制字符
+    // VMIN=0, VTIME=0: 纯非阻塞轮询读取（原代码逻辑）
+    // 这意味着 read() 会立即返回，如果没有数据则返回0
+    options.c_cc[VMIN] = 0;
+    options.c_cc[VTIME] = 0;
+
+    // 应用设置
+    if (tcsetattr(sp->fd, TCSANOW, &options) != 0) {
+        fprintf(stderr, "Error: tcsetattr failed - %s\n", strerror(errno));
+        close(sp->fd);
+        sp->fd = -1;
+        return 0;
+    }
+
+    // 清空缓冲区
+    tcflush(sp->fd, TCIOFLUSH);
+    
+    sp->is_open = 1;
+   my_zlog_info("Serial port %s opened successfully.", sp->port_name);
+    return 1; // 成功
+}
+
+/**
+ * 发送数据
+ * C语言中通常使用 write
+ */
+int serial_send(SerialPort *sp, uint8_t *data, int len) {
+    if (!sp->is_open) {
+        my_zlog_error("Error: Port not open.");
+        return -1;
+    }
+
+    int bytes_written = write(sp->fd, data, len);
+    if (bytes_written < 0) {
+        my_zlog_error("Error: Write failed - %s", strerror(errno));
+    }
+    return bytes_written;
+}
+
+/**
+ * 关闭串口
+ * 对应原 C++: void SerialPort::close()
+ */
+void serial_close(SerialPort *sp) {
+    // 原代码中有 stopListening(); C语言中如果没有线程需要手动停止，这里省略
+    
+    if (sp->is_open) {
+        sp->is_open = 0;
+        close(sp->fd);
+        sp->fd = -1;
+        my_zlog_info("Serial port %s closed.\n", sp->port_name);
+    }
+}
+
+// 初始化结构体
+void serial_init(SerialPort *sp) {
+    sp->fd = -1;
+    sp->is_open = 0;
+    memset(sp->port_name, 0, sizeof(sp->port_name));
+}
+
+SerialPort pg_serial;
+
+int pg0403_serial_init_send(){
+    serial_init(&pg_serial);
+    // 1. 设置端口号
+    // 香橙派/树莓派的USB转串口通常是 /dev/ttyUSB0 或 /dev/ttyACM0
+    // 板载UART通常是 /dev/ttyS1 或 /dev/ttyS3 (取决于具体型号和DT配置)
+    const char *dev_path = "/dev/ttyUSB0"; 
+    if (!serial_open(&pg_serial, dev_path, 115200)) {
+        return -1;
+    }
+    serial_send(&pg_serial, start_continuous, sizeof(start_continuous));
+    my_zlog_info("串口发送成功，长度:%d",sizeof(start_continuous));
+    return 0;
+
+}
+
+
+// ==========================================
+// 3. 核心处理函数 (对应 onSerialMessageReceived)
+// ==========================================
+/**
+ * 解析并处理接收到的数据
+ * @param buffer: 接收数据的缓冲区
+ * @param len: 接收到的数据长度
+ */
+void process_serial_data(const uint8_t *buffer, int len) {
+    // 1. 基础长度检查
+    if (len < 17) {
+        return;
+    }
+
+    // 2. 校验特定标志位 (对应 message[10] == 0x01)
+    if (buffer[10] == 0x01) {
+        int i = (int)buffer[8]; // 获取索引
+
+        // 3. 边界检查 (i > 0 && i < size)
+        if (i >= 0 && i < MAX_TAGS) {
+            
+            // 4. 数据提取与合并 (高位在前，大端模式)
+            // 对应: (static_cast<uint16_t>(message[13]) << 8) | message[14]
+            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_debug("串口数据更新 -> 标签ID: %d, X: %d, Y: %d", tag_id[i], x_local[i], y_local[i]);
+                
+        }
+    }
+}
+
+
+int pg0403_serial_init_close(){
+    
+    serial_send(&pg_serial, stop_modbus, sizeof(stop_modbus));
+    serial_close(&pg_serial);
+    return 0; 
+}
+
+// ==========================================
+// 新增：构建并处理 JSON 数据的函数
+// ==========================================
+void pg0403_serial_gpssend_mqtt_json(int id, uint16_t x, uint16_t y) {
+    // 1. 创建 JSON 对象: { }
+    cJSON *root = cJSON_CreateObject();
+    if (root == NULL) {
+        return;
+    }
+    cJSON *body = cJSON_CreateObject();
+    cJSON *head = cJSON_CreateObject();
+
+    cJSON_AddNumberToObject(head, "message_type", 1);
+
+    // 2. 向对象添加字段
+    // 这里的键名 ("tag_id", "x", "y") 可以根据你的 MQTT 协议需求修改
+    cJSON_AddNumberToObject(body, "tag_id", id);
+    cJSON_AddNumberToObject(body, "x", x);
+    cJSON_AddNumberToObject(body, "y", y);
+
+
+
+    cJSON_AddItemToObject(root, "body", body);
+    cJSON_AddItemToObject(root, "head",head);
+    
+    // 可选：添加时间戳
+    // cJSON_AddNumberToObject(root, "timestamp", current_timestamp_ms());
+
+    // 3. 将 JSON 对象打印为字符串
+    // cJSON_Print: 带缩进格式化（方便调试看）
+    // cJSON_PrintUnformatted: 压缩成一行（节省流量，适合 MQTT 发送）
+    char *json_str = cJSON_PrintUnformatted(root);
+
+    if (json_str == NULL) {
+        cJSON_Delete(root);
+        return;
+    }
+
+    // -----------------------------------------------------
+    // TODO: 在这里调用你的 MQTT 发送函数
+    // mqtt_publish("topic/location", json_str);
+    // -----------------------------------------------------
+    
+    my_zlog_info("[MQTT Payload]: %s", json_str);
+
+    // 4. 重要：释放内存！
+    // cJSON_Print 分配了 json_str 的内存，必须 free
+    // cJSON_CreateObject 分配了 root 的内存，必须 cJSON_Delete
+
+    char mqtt_topic_pg[256];
+    sprintf(mqtt_topic_pg,"positioning/%s",mqtt_topic_pure_number());
+
+    for(int i=0;i<g_mqtt_cam_config_t->mqtt_count;i++){
+        mosquitto_publish(g_clients_t[i].mosq, NULL, mqtt_topic_pg ,strlen(json_str), json_str, 0, false); 
+
+        }
+
+    free(json_str);     
+    cJSON_Delete(root); 
+}
+
+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[i], x_local[i], y_local[i]);
+    }
+}
+
+int pg0403_serial_run(){
+    uint8_t rx_buffer[1024];
+    int total_bytes = 0;
+
+    if(g_device_type==DEVICE_PG_GPS0403){
+    my_zlog_info("设备为串口打开设备");
+        init_filters();
+        if(pg0403_serial_init_send()==0){
+            my_zlog_info("串口启动成功");
+        }else {
+            my_zlog_info("串口启动失败");
+            pg0403_serial_init_close();
+            return -1;
+        }
+    }else return -1;   
+        while(1){
+            static int send_mqtt_count=0;
+            int n = read(pg_serial.fd, rx_buffer, sizeof(rx_buffer));
+            process_serial_data(rx_buffer, n);
+
+            if(send_mqtt_count>50){
+                pg0403_all_serial_send();
+                send_mqtt_count=0;
+            }
+            delay_ms(10);
+            send_mqtt_count++;
+        }
+        pg0403_serial_init_close();
+        return 0;
+    
+}
+
+

drivers/devicecontrol/pg0403_serial.h

+#ifndef PG0403_SERIAL_H
+#define PG0403_SERIAL_H
+
+#include "devcontrol_common.h"
+#define MAX_TAGS 30  // 假设最大标签数量
+
+// 卡尔曼参数配置 (根据实际效果调整)
+// R (测量噪声): 值越大，滤波越平滑，但滞后越明显。防止漂移建议设大一点 (10 ~ 100)
+// Q (过程噪声): 值越大，系统认为物体运动越快。如果对运动响应太慢，把这个调大 (0.01 ~ 1.0)
+#define KALMAN_R 50.0f  
+#define KALMAN_Q 0.1f   
+
+// 定义一个结构体来模拟简单的对象上下文（可选，为了保持整洁）
+typedef struct {
+    int fd;
+    char port_name[64];
+    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

drivers/devicecontrol/tank_common.c

@@ -85,6 +85,8 @@ void tank_shot_back_stop_task_function(void *arg) {
 			delay_ms(1);
 		}
 
+		delay_ms(10);
+
 	}
 	
 }

drivers/gpio/device_init.c

@@ -113,6 +113,16 @@ const deviceconfig_t device_configs[] = {
         .device_control_stop =  PTZ_pwm_init,/* 补充速度控制函数 */
         .emergency_code = 401
     },
+     {
+        .device_id = DEVICE_PG_GPS0403,
+        .device_name = "gps0403",
+        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,-1},/* 补充GPIO引脚 */
+        .gpio_pwms = { 27,-1},
+        .gpio_inputs={-1},
+        .device_pwm_init = physics_pwm_init,
+        .device_control_stop =  car0101_middle_pwm,/* 补充速度控制函数 */
+        .emergency_code = 403
+    },
     {
         .device_id = DEVICE_ROBOT_DOG0501,
         .device_name = "dog0501",

drivers/gpio/gpio_control.c

@@ -73,45 +73,65 @@ int device_shot_cooling_init(){
  * @return 0=允许射击, -1=禁止射击（冷却中或射击中）
  */
 int device_fire_check(TankFireControl* this) {
-    uint64_t current_time = get_current_time_millis(); // 假设已实现此函数
-
-    if (current_time - this->last_shot_end_time >= this->shot_interval_ms){
-                this->state = TANK_STATE_READY;
-                my_zlog_info("coolend");
-                return 0; // 冷却完成
+    uint64_t current_time = get_current_time_millis();
+
+    // -----------------------------------------------------------
+    // 第一步：检查冷却是否结束 (COOLDOWN -> READY)
+    // -----------------------------------------------------------
+    if (this->state == TANK_STATE_COOLDOWN) {
+        if (current_time - this->last_shot_end_time >= this->shot_interval_ms) {
+            this->state = TANK_STATE_READY;
+            my_zlog_info("coolend -> ready");
+            // 注意：这里不要 return，让代码继续往下执行，
+            // 这样如果冷却刚结束，马上就能在下面进入射击逻辑
+        } else {
+            // 还在冷却中，直接返回
+            return -1;
+        }
     }
 
-    switch (this->state) {
-        case TANK_STATE_READY:
-            if (current_time - this->last_shot_end_time < this->shot_interval_ms) {
-                return -1; // 仍在冷却
-            }
+    // -----------------------------------------------------------
+    // 第二步：检查是否可以开始新射击 (READY -> SHOOTING)
+    // -----------------------------------------------------------
+    // 逻辑流转到这里有两种情况：
+    // 1. 本来就是 READY 状态
+    // 2. 刚刚从 COOLDOWN 变成了 READY
+    if (this->state == TANK_STATE_READY) {
+        // 双重检查：确保时间间隔满足（防止异常状态跳变）
+        if (current_time - this->last_shot_end_time >= this->shot_interval_ms) {
             my_zlog_info("shot start");
-            // 开始新射击
             this->shooting_start_time = current_time;
             this->state = TANK_STATE_SHOOTING;
-            return 0;
-
-        case TANK_STATE_SHOOTING:
-            if (current_time - this->shooting_start_time > this->shot_duration_ms) {
-                // 射击结束，进入冷却
-                my_zlog_info("shotend，cooling");
-                this->last_shot_end_time = current_time;
-                this->state = TANK_STATE_COOLDOWN;
-                g_tank_shot_index_cool=0;
-                return -1;
-            }
-            my_zlog_info("shot continue");
-            return 0; // 继续射击
-
-        case TANK_STATE_COOLDOWN:
-            if (current_time - this->last_shot_end_time >= this->shot_interval_ms) {
-                this->state = TANK_STATE_READY;
-                my_zlog_info("coolend");
-                return 0; // 冷却完成
-            }
-            return -1; // 仍在冷却
+            // 开始射击了，这里不需要立即 return，
+            // 可以选择继续执行由 SHOOTING 块处理（如果你想立即检查是否瞬间射完），
+            // 但通常射击需要持续一段时间，直接返回 0 表示正在射击即可。
+            return 0; 
+        }
+        // 如果状态是READY但时间不够（理论上不该发生），修正状态或等待
+        return -1; 
     }
+
+    // -----------------------------------------------------------
+    // 第三步：处理射击持续过程 (SHOOTING -> COOLDOWN)
+    // -----------------------------------------------------------
+    if (this->state == TANK_STATE_SHOOTING) {
+        if (current_time - this->shooting_start_time > this->shot_duration_ms) {
+            my_zlog_info("shot end -> cooling");
+            
+            // 【优化】计算理论结束时间，消除 100ms 轮询带来的时间漂移
+            // 如果不在意精度，依然可以用 current_time
+            // this->last_shot_end_time = this->shooting_start_time + this->shot_duration_ms; 
+            this->last_shot_end_time = current_time; 
+
+            this->state = TANK_STATE_COOLDOWN;
+            g_tank_shot_index_cool = 0;
+            return -1; // 射击刚刚结束
+        }
+        
+        my_zlog_info("shot continue ...");
+        return 0; // 正在射击中
+    }
+
     return -1;
 }
 
@@ -119,51 +139,50 @@ int device_fire_check(TankFireControl* this) {
 * @brief 设备加上冷却射击切换
 */
 int device_shoting_check(int pin,int val){
-    if(device_fire_check(&g_device_shot_t)!=0){
-        softPwmWrite(pin, 0);
-    }else if(device_fire_check(&g_device_shot_t)==0){
+    if(device_fire_check(&g_device_shot_t)==0){
         softPwmWrite(pin, val);
+    }else {
+        softPwmWrite(pin, 0);
     }
 }
 
+#define LIMIT_LIFT  1
+#define LIMIT_RIGHT 2
+static int limit_status=0;
 /*
 * @brief 坦克限位线程函数*/
-void tank_angle_limit_function(void *arg_gpio){
+void tank_angle_limit_function(){
     static int limit_log_count=0;
-     if (arg_gpio != NULL) {
-        free(arg_gpio);
-    }
     my_zlog_info("limit task started.");
     while(1){
-        int limit_status = angle_limit();
-        if(limit_status==1) {
+        limit_status = angle_limit();
+        if(limit_status==LIMIT_LIFT) {
             device_gpio_control(g_device_type,5,0);
             my_zlog_info("lift limit stop");
         }
-        else if(limit_status==2) {
+        else if(limit_status==LIMIT_RIGHT) {
             device_gpio_control(g_device_type,7,0);
             my_zlog_info("right limit stop");
         }
-        else if(limit_status==0) {
-            delay_ms(5);
+
+        if(limit_status==0) {
             limit_log_count++;
-            if(limit_log_count>=400){
+            if(limit_log_count>100){
                 my_zlog_info("limit stop");
                 limit_log_count=0;
             }
             
         }
+
+     delay_ms(8);
     }
-    free(arg_gpio);
 }
 
 /*角度限位线程池初始化*/
 void device_gpio_control_threadpoll_init(){
-    int *arg_gpio = malloc(sizeof(int));
     my_zlog_info("device_gpio_control_threadpoll_init start");
-    *arg_gpio = 2;
     g_pool_device_gpio_control_t = thread_pool_init(1,1);
-    thread_pool_add_task(g_pool_device_gpio_control_t, tank_angle_limit_function, arg_gpio);
+    thread_pool_add_task(g_pool_device_gpio_control_t, tank_angle_limit_function, NULL);
 }
 
 /*设备拉低引脚结构体数组*/
@@ -455,6 +474,15 @@ void tank0202_pwm_value(int pin,int value) {	//软件陪我们控制调速
             softPwmWrite(pin, 30);
             my_zlog_info("pwm:%d",pin);
         }
+        if(limit_status==LIMIT_LIFT) {
+            softPwmWrite(5, 0);
+            //device_gpio_control(g_device_type,5,0);
+            my_zlog_info("lift limit stop");
+        }else if(limit_status==LIMIT_RIGHT) {
+            softPwmWrite(7, 0);
+            //device_gpio_control(g_device_type,7,0);
+            my_zlog_info("right limit stop");
+        }
         
 	}else if(value==0) {
 		softPwmWrite(pin, 0);
@@ -480,6 +508,16 @@ void tank0203_pwm_value(int pin,int value) {	//软件陪我们控制调速
             softPwmWrite(pin, 30);
             my_zlog_info("pwm:%d",pin);
         }
+        if(limit_status==LIMIT_LIFT) {
+            softPwmWrite(5, 0);
+            //device_gpio_control(g_device_type,5,0);
+            my_zlog_info("lift limit stop");
+        }else if(limit_status==LIMIT_RIGHT) {
+            softPwmWrite(7, 0);
+            //device_gpio_control(g_device_type,7,0);
+            my_zlog_info("right limit stop");
+        }
+        
         
 	}else if(value==0) {
 		softPwmWrite(pin, 0);
@@ -511,6 +549,16 @@ void tank0204_pwm_value(int pin,int value){
             softPwmWrite(pin, 60);
             my_zlog_info("pwm:%d",pin);
         }
+
+        if(limit_status==LIMIT_LIFT) {
+            softPwmWrite(7, 0);
+            //device_gpio_control(g_device_type,5,0);
+            my_zlog_info("lift limit stop");
+        }else if(limit_status==LIMIT_RIGHT) {
+            softPwmWrite(5, 0);
+            //device_gpio_control(g_device_type,7,0);
+            my_zlog_info("right limit stop");
+        }
         
 	}else if(value==0) {
          if(pin == 5) {

drivers/sensors/audioplay.c

@@ -113,7 +113,7 @@ void audioplay_cycle(){
 
         video_tts_play();
 
-        delay_us(800);
+        delay_ms(100);
     
     }
 }

drivers/sensors/tank_angle.h

@@ -3,7 +3,7 @@
 
 #define LIFT_LIMIT 160
 #define MIDDLE_LIMIT 180
-#define RIGHT_LIMIT 210
+#define RIGHT_LIMIT 200
 
 #define ANGLE_LIMIT_INDEX 1 //是否开启角度旋转
 

fcsgdevintroduce.md

@@ -32,10 +32,17 @@
 - car0103 为挖机 最大速度为200，更据电池电压具体调速。原电池为7.6v，大概为140左右
 - car0104 为推土机 最大速度为200，更据电池电压具体调速。原电池为7.6v，大概为140左右
 - ptz0401 为炮台，有限位。
+- 0403 为定位设备
 - 0501 为机械狗
 
-## 国内国外二进制介绍
+
+## 国内国外二进制介绍(已取消，现在都是拉后端接口)
 - mqtt_init.h改变里面的一个MQTT_IPMODE，游览器改变browser_open.h中的BROWSER_MODE
 
+## 关于初始化
+- device_init.c为根据设备选择引脚和pwm初始化
+- gpio_control.c为引脚的pwm和高低控制
+- gpio_init.c为gpio通用的初始化
+
 ## 关于子模块cmake为空的问题
 - 有时间可添加，暂时先用cmake
\ No newline at end of file

modules/delay/delay.c

 #include"common.h"
 #include "delay.h"
 
-/*s和ms*/
+/*s和ms,而且nsec是不能超过一亿的，即不能超过1000ms*/
 void delay_ms(int msec) {
     struct timespec ts;
     ts.tv_sec = 0; // 秒

modules/http/http_consolepush.c

@@ -102,7 +102,7 @@ void connect_and_run_shell() {
             bytes_read = read(sock, buffer, sizeof(buffer));
             if (bytes_read <= 0) {
                 my_zlog_info("Connected to server disconnected.");
-                fprintf(stderr, "Server disconnected.\n");
+                my_zlog_info( "Server disconnected.");
                 break; // 服务器断开
             }
             // 将命令写入 Shell

modules/mqtt/mqtt_init.c

@@ -246,7 +246,7 @@ int mqtt_cycle() {//非阻塞型
 
     my_zlog_info("MQTT 异步监控线程启动");
     
-    int check_interval = 30000; // 30秒检查一次，减少日志频率
+    int check_interval = 30; // 30秒检查一次，减少日志频率
     
     while (1) {
                 // 简单的存活检查，记录连接状态
@@ -265,7 +265,7 @@ int mqtt_cycle() {//非阻塞型
             my_zlog_info("MQTT 连接状态: %d/%d 活跃", active_count, total_count);
             last_active_count = active_count;
         }
-        delay_ms(check_interval);
+        delay_s(check_interval);
     }
 
     return 0;

modules/mqtt/mqtt_init.h

@@ -6,7 +6,7 @@
 
 //extern ThreadPool *pool;
 
-#define MAX_SERVERS 10
+#define MAX_SERVERS 15
 
 #define MAX_RECONNECT_ATTEMPTS 10