From 06b873920ba2f86dece3637d881e2233bf2defbd Mon Sep 17 00:00:00 2001
From: Alvin-lyq <zxwl@44.com>
Date: Wed, 29 Apr 2026 12:46:29 +0800
Subject: [PATCH] debug-6

---
 .../rtk_asensing/asensing_protocol.hpp        | 10 ++++++++
 .../rtk_asensing/src/rtk_asensing_node.cpp    | 23 +++++++++++++++++++
 2 files changed, 33 insertions(+)

diff --git a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp
index 0a10a9d..090d480 100644
--- a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp
+++ b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp
@@ -6,6 +6,8 @@
 #include <cstring>
 #include <string>
 
+#include "logger/logger.h"
+
 // Asensing协议4数据帧定义
 // 帧头: 0xBD 0xDB
 // 帧类型: 0x0B (组合导航模式，58字节数据)
@@ -105,6 +107,7 @@ public:
                 else if (frame_index_ == 1 && data[i] == 0xDB)
                 {
                     frame_buf_[frame_index_++] = data[i];
+                    LOG_INFO("[DEBUG-PROTO] 找到帧头 BD DB");
                 }
                 else if (frame_index_ == 1 && data[i] != 0xDB)
                 {
@@ -135,9 +138,13 @@ public:
                 // 所以完整帧长 = 62字节
                 if (frame_index_ >= 62)
                 {
+                    // 打印帧数据用于调试
+                    LOG_INFO("[DEBUG-PROTO] 收到帧数据, 帧类型: 0x%02X", frame_buf_[2]);
+
                     // 检查帧类型
                     if (frame_buf_[2] != 0x0B)
                     {
+                        LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B)", frame_buf_[2]);
                         // 帧类型不匹配，重新同步
                         is_synced_ = false;
                         frame_index_ = 0;
@@ -153,12 +160,15 @@ public:
 
                     if (xor_check != frame_buf_[m_lengthImu])
                     {
+                        LOG_INFO("[DEBUG-PROTO] 校验失败: 计算=0x%02X, 接收=0x%02X", xor_check, frame_buf_[m_lengthImu]);
                         // 校验失败，重新同步
                         is_synced_ = false;
                         frame_index_ = 0;
                         continue;
                     }
 
+                    LOG_INFO("[DEBUG-PROTO] 校验通过! 开始解析数据");
+
                     // 校验通过，解析数据
                     int sub_index = 3;
 
diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
index 18e713c..a32e9cf 100644
--- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
+++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp
@@ -50,6 +50,9 @@ public:
 private:
     void timer_callback()
     {
+        static int read_count = 0;
+        static int parse_count = 0;
+
         try
         {
             // 读取串口数据
@@ -61,11 +64,31 @@ private:
                 return;
             }
 
+            read_count++;
+
+            // 每100次打印一次原始数据
+            if (read_count % 100 == 1)
+            {
+                LOG_INFO("[DEBUG] 串口读取 %d 字节:", num);
+                int print_len = (num < 20) ? num : 20;
+                std::string hex_str;
+                for (int i = 0; i < print_len; i++)
+                {
+                    char hex[4];
+                    snprintf(hex, sizeof(hex), "%02X ", (uint8_t)serial_buf[i]);
+                    hex_str += hex;
+                }
+                LOG_INFO("[DEBUG] 前%d字节 HEX: %s", print_len, hex_str.c_str());
+            }
+
             // 将数据送入协议解析器
             int result = protocol_parser->feedData(reinterpret_cast<uint8_t*>(serial_buf), num);
 
             if (result == 1)
             {
+                parse_count++;
+                LOG_INFO("[DEBUG] 解析成功! 累计帧数: %d", parse_count);
+
                 // 成功解析一帧数据，发布ROS消息
                 publishData();
             }