lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug-6

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-29 12:46:29 +08:00
parent 9192eb7128
commit 06b873920b
2 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp
View File

@ -6,6 +6,8 @@
#include <cstring> #include <cstring>
#include <string> #include <string>
#include "logger/logger.h"
// Asensing协议4数据帧定义 // Asensing协议4数据帧定义
// 帧头: 0xBD 0xDB // 帧头: 0xBD 0xDB
// 帧类型: 0x0B (组合导航模式，58字节数据) // 帧类型: 0x0B (组合导航模式，58字节数据)
@ -105,6 +107,7 @@ public:
else if (frame_index_ == 1 && data[i] == 0xDB) else if (frame_index_ == 1 && data[i] == 0xDB)
{ {
frame_buf_[frame_index_++] = data[i]; frame_buf_[frame_index_++] = data[i];
LOG_INFO("[DEBUG-PROTO] 找到帧头 BD DB");
} }
else if (frame_index_ == 1 && data[i] != 0xDB) else if (frame_index_ == 1 && data[i] != 0xDB)
{ {
@ -135,9 +138,13 @@ public:
// 所以完整帧长 = 62字节 // 所以完整帧长 = 62字节
if (frame_index_ >= 62) if (frame_index_ >= 62)
{ {
// 打印帧数据用于调试
LOG_INFO("[DEBUG-PROTO] 收到帧数据, 帧类型: 0x%02X", frame_buf_[2]);
// 检查帧类型 // 检查帧类型
if (frame_buf_[2] != 0x0B) if (frame_buf_[2] != 0x0B)
{ {
LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B)", frame_buf_[2]);
// 帧类型不匹配，重新同步 // 帧类型不匹配，重新同步
is_synced_ = false; is_synced_ = false;
frame_index_ = 0; frame_index_ = 0;
@ -153,12 +160,15 @@ public:
if (xor_check != frame_buf_[m_lengthImu]) if (xor_check != frame_buf_[m_lengthImu])
{ {
LOG_INFO("[DEBUG-PROTO] 校验失败: 计算=0x%02X, 接收=0x%02X", xor_check, frame_buf_[m_lengthImu]);
// 校验失败，重新同步 // 校验失败，重新同步
is_synced_ = false; is_synced_ = false;
frame_index_ = 0; frame_index_ = 0;
continue; continue;
} }
LOG_INFO("[DEBUG-PROTO] 校验通过! 开始解析数据");
// 校验通过，解析数据 // 校验通过，解析数据
int sub_index = 3; int sub_index = 3;

23
src/perception/rtk_asensing/src/rtk_asensing_node.cpp
View File

@ -50,6 +50,9 @@ public:
private: private:
void timer_callback() void timer_callback()
{ {
static int read_count = 0;
static int parse_count = 0;
try try
{ {
// 读取串口数据 // 读取串口数据
@ -61,11 +64,31 @@ private:
return; 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); int result = protocol_parser->feedData(reinterpret_cast<uint8_t*>(serial_buf), num);
if (result == 1) if (result == 1)
{ {
parse_count++;
LOG_INFO("[DEBUG] 解析成功! 累计帧数: %d", parse_count);
// 成功解析一帧数据，发布ROS消息 // 成功解析一帧数据，发布ROS消息
publishData(); publishData();
} }