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

debug-3

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-29 10:32:04 +08:00
parent 2057adf843
commit b4dfdf1798
1 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,6 +4,7 @@
#include <cmath> #include <cmath>
#include <cstdint> #include <cstdint>
#include <cstring> #include <cstring>
#include "logger/logger.h" #include "logger/logger.h"
// Asensing协议4数据帧定义 // Asensing协议4数据帧定义
@ -118,11 +119,24 @@ class AsensingProtocol
// 所以完整帧长 = 63字节 // 所以完整帧长 = 63字节
if (frame_index_ >= 63) if (frame_index_ >= 63)
{ {
// 打印完整帧数据用于调试
LOG_INFO("[DEBUG-PROTO] 完整帧数据(%d字节):", frame_index_);
std::string frame_hex;
for (int j = 0; j < 63 && j < frame_index_; j++)
{
char hex[4];
snprintf(hex, sizeof(hex), "%02X ", frame_buf_[j]);
frame_hex += hex;
if ((j + 1) % 16 == 0) frame_hex += "\n";
}
LOG_INFO("[DEBUG-PROTO] HEX:\n%s", frame_hex.c_str());
// 检查帧类型 // 检查帧类型
if (frame_buf_[2] != 0x0B) if (frame_buf_[2] != 0x0B)
{ {
total_frame_type_fail++; total_frame_type_fail++;
LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B), 累计失败: %d", frame_buf_[2], total_frame_type_fail); LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B), 累计失败: %d", frame_buf_[2],
total_frame_type_fail);
// 帧类型不匹配，重新同步 // 帧类型不匹配，重新同步
is_synced_ = false; is_synced_ = false;
frame_index_ = 0; frame_index_ = 0;
@ -139,8 +153,8 @@ class AsensingProtocol
if (xor_check1 != frame_buf_[57]) if (xor_check1 != frame_buf_[57])
{ {
total_checksum1_fail++; total_checksum1_fail++;
LOG_INFO("[DEBUG-PROTO] 校验位1失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", LOG_INFO("[DEBUG-PROTO] 校验位1失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", xor_check1,
xor_check1, frame_buf_[57], total_checksum1_fail); frame_buf_[57], total_checksum1_fail);
// 校验失败，重新同步 // 校验失败，重新同步
is_synced_ = false; is_synced_ = false;
frame_index_ = 0; frame_index_ = 0;
@ -157,16 +171,16 @@ class AsensingProtocol
if (xor_check2 != frame_buf_[62]) if (xor_check2 != frame_buf_[62])
{ {
total_checksum2_fail++; total_checksum2_fail++;
LOG_INFO("[DEBUG-PROTO] 校验位2失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", LOG_INFO("[DEBUG-PROTO] 校验位2失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", xor_check2,
xor_check2, frame_buf_[62], total_checksum2_fail); frame_buf_[62], total_checksum2_fail);
// 校验失败，重新同步 // 校验失败，重新同步
is_synced_ = false; is_synced_ = false;
frame_index_ = 0; frame_index_ = 0;
continue; continue;
} }
LOG_INFO("[DEBUG-PROTO] 解析成功! 帧头: %02X %02X, 类型: %02X", LOG_INFO("[DEBUG-PROTO] 解析成功! 帧头: %02X %02X, 类型: %02X", frame_buf_[0], frame_buf_[1],
frame_buf_[0], frame_buf_[1], frame_buf_[2]); frame_buf_[2]);
// 校验通过，复制数据到结构体 // 校验通过，复制数据到结构体
memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData)); memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData));