debug-4
This commit is contained in:
parent
b4dfdf1798
commit
fa42266034
@ -4,18 +4,19 @@
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
|
||||
#include "logger/logger.h"
|
||||
#include <string>
|
||||
|
||||
// Asensing协议4数据帧定义
|
||||
// 帧头: 0xBD 0xDB
|
||||
// 帧类型: 0x0B (组合导航模式，58字节数据)
|
||||
// 数据长度: 58字节 (不含帧头3字节)
|
||||
|
||||
#pragma pack(push, 1) // 1字节对齐，避免结构体填充
|
||||
|
||||
// Asensing协议4组合导航数据帧
|
||||
struct AsensingInsData
|
||||
{
|
||||
// 原始帧数据
|
||||
uint8_t frame_header[2]; // 偏移0-1: 帧头 0xBD 0xDB
|
||||
uint8_t frame_type; // 偏移2: 帧类型 0x0B
|
||||
int16_t roll; // 偏移3-4: 横滚角, 系数 360/32768, 单位 deg
|
||||
@ -40,18 +41,44 @@ struct AsensingInsData
|
||||
int16_t data3; // 偏移50-51: 轮循数据3
|
||||
uint32_t gps_time; // 偏移52-55: GPS时间（周内秒）, 系数 2.5e-4, 单位 秒
|
||||
uint8_t pollute_type; // 偏移56: 轮循数据类型
|
||||
uint8_t check1; // 偏移57: 校验位1 (异或校验，包含0-56的数据)
|
||||
uint32_t gps_week; // 偏移58-61: GPS周数
|
||||
uint8_t check2; // 偏移62: 校验位2 (异或校验，包含0-61的数据)
|
||||
uint8_t check1; // 偏移57: 校验位1 (异或校验，包含0-57的数据)
|
||||
|
||||
// 解析后的轮循数据
|
||||
double gps_time_ms; // GPS时间（毫秒）
|
||||
double temperature; // 温度 (℃)
|
||||
uint8_t position_type; // 定位状态
|
||||
uint8_t numsv; // 收星数
|
||||
uint8_t heading_type; // 定向状态
|
||||
uint8_t wheel_speed_status; // 轮速状态
|
||||
|
||||
// 精度数据
|
||||
double latitude_std; // 纬度标准差
|
||||
double longitude_std; // 经度标准差
|
||||
double altitude_std; // 高度标准差
|
||||
double north_velocity_std; // 北向速度标准差
|
||||
double east_velocity_std; // 东向速度标准差
|
||||
double ground_velocity_std; // 地向速度标准差
|
||||
double roll_std; // 横滚角标准差
|
||||
double pitch_std; // 俯仰角标准差
|
||||
double azimuth_std; // 方位角标准差
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
// 从字节流中读取指定类型的数据 (小端序)
|
||||
template<typename T>
|
||||
T toValue(const uint8_t* data, int& index)
|
||||
{
|
||||
T result = *(T*)(data + index);
|
||||
index += sizeof(T);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Asensing协议数据解析类
|
||||
class AsensingProtocol
|
||||
{
|
||||
public:
|
||||
AsensingProtocol() : is_synced_(false), frame_index_(0) {}
|
||||
AsensingProtocol() : is_synced_(false), frame_index_(0), m_lengthImu(58) {}
|
||||
|
||||
// 重置解析状态
|
||||
void reset()
|
||||
@ -65,11 +92,6 @@ class AsensingProtocol
|
||||
// 返回值: 1=成功解析一帧, 0=数据不足, -1=校验失败
|
||||
int feedData(const uint8_t* data, int len)
|
||||
{
|
||||
static int total_sync_attempts = 0;
|
||||
static int total_checksum1_fail = 0;
|
||||
static int total_checksum2_fail = 0;
|
||||
static int total_frame_type_fail = 0;
|
||||
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
if (!is_synced_)
|
||||
@ -82,8 +104,6 @@ class AsensingProtocol
|
||||
else if (frame_index_ == 1 && data[i] == 0xDB)
|
||||
{
|
||||
frame_buf_[frame_index_++] = data[i];
|
||||
total_sync_attempts++;
|
||||
LOG_INFO("[DEBUG-PROTO] 找到帧头 BD DB, 累计尝试: %d", total_sync_attempts);
|
||||
}
|
||||
else if (frame_index_ == 1 && data[i] != 0xDB)
|
||||
{
|
||||
@ -109,81 +129,91 @@ class AsensingProtocol
|
||||
// 已同步帧头，继续接收数据
|
||||
frame_buf_[frame_index_++] = data[i];
|
||||
|
||||
if (frame_index_ == 3)
|
||||
// 完整帧长 = 帧头2字节 + 类型1字节 + 数据58字节 + 校验1字节 = 62字节
|
||||
// 实际数据到偏移57是check1(1字节)
|
||||
// 所以完整帧长 = 62字节
|
||||
if (frame_index_ >= 62)
|
||||
{
|
||||
LOG_INFO("[DEBUG-PROTO] 帧类型: 0x%02X", frame_buf_[2]);
|
||||
}
|
||||
|
||||
// 检查是否接收完整帧 (帧头2字节 + 类型1字节 + 数据58字节 + GPS周4字节 + 校验1字节 = 66字节)
|
||||
// 实际数据到偏移57是check1(1字节)，然后58-61是GPS周，62是check2
|
||||
// 所以完整帧长 = 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)
|
||||
{
|
||||
total_frame_type_fail++;
|
||||
LOG_INFO("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B), 累计失败: %d", frame_buf_[2],
|
||||
total_frame_type_fail);
|
||||
// 帧类型不匹配，重新同步
|
||||
is_synced_ = false;
|
||||
frame_index_ = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 验证校验位1 (异或校验，包含0-56的数据)
|
||||
uint8_t xor_check1 = 0;
|
||||
for (int j = 0; j < 57; j++)
|
||||
// 验证校验位1 (异或校验，包含0-57的数据，共58字节)
|
||||
uint8_t xor_check = 0;
|
||||
for (int j = 0; j < m_lengthImu; j++)
|
||||
{
|
||||
xor_check1 ^= frame_buf_[j];
|
||||
xor_check ^= frame_buf_[j];
|
||||
}
|
||||
|
||||
if (xor_check1 != frame_buf_[57])
|
||||
if (xor_check != frame_buf_[m_lengthImu])
|
||||
{
|
||||
total_checksum1_fail++;
|
||||
LOG_INFO("[DEBUG-PROTO] 校验位1失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", xor_check1,
|
||||
frame_buf_[57], total_checksum1_fail);
|
||||
// 校验失败，重新同步
|
||||
is_synced_ = false;
|
||||
frame_index_ = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 验证校验位2 (异或校验，包含0-61的数据)
|
||||
uint8_t xor_check2 = 0;
|
||||
for (int j = 0; j < 62; j++)
|
||||
{
|
||||
xor_check2 ^= frame_buf_[j];
|
||||
}
|
||||
// 校验通过，解析数据
|
||||
int sub_index = 3;
|
||||
|
||||
if (xor_check2 != frame_buf_[62])
|
||||
{
|
||||
total_checksum2_fail++;
|
||||
LOG_INFO("[DEBUG-PROTO] 校验位2失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", xor_check2,
|
||||
frame_buf_[62], total_checksum2_fail);
|
||||
// 校验失败，重新同步
|
||||
is_synced_ = false;
|
||||
frame_index_ = 0;
|
||||
continue;
|
||||
}
|
||||
// 横滚角、俯仰角、方位角
|
||||
raw_data_.roll = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.pitch = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.yaw = toValue<int16_t>(frame_buf_, sub_index);
|
||||
|
||||
LOG_INFO("[DEBUG-PROTO] 解析成功! 帧头: %02X %02X, 类型: %02X", frame_buf_[0], frame_buf_[1],
|
||||
frame_buf_[2]);
|
||||
// 陀螺
|
||||
raw_data_.gyro_x = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.gyro_y = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.gyro_z = toValue<int16_t>(frame_buf_, sub_index);
|
||||
|
||||
// 校验通过，复制数据到结构体
|
||||
memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData));
|
||||
// 加速度
|
||||
raw_data_.accel_x = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.accel_y = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.accel_z = toValue<int16_t>(frame_buf_, sub_index);
|
||||
|
||||
// 经纬度、高度
|
||||
raw_data_.latitude = toValue<int32_t>(frame_buf_, sub_index);
|
||||
raw_data_.longitude = toValue<int32_t>(frame_buf_, sub_index);
|
||||
raw_data_.altitude = toValue<int32_t>(frame_buf_, sub_index);
|
||||
|
||||
// 速度
|
||||
raw_data_.vel_north = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.vel_east = toValue<int16_t>(frame_buf_, sub_index);
|
||||
raw_data_.vel_down = toValue<int16_t>(frame_buf_, sub_index);
|
||||
|
||||
// 状态
|
||||
raw_data_.status = frame_buf_[sub_index++];
|
||||
|
||||
// 保留字段
|
||||
sub_index += 4;
|
||||
sub_index += 2;
|
||||
|
||||
// 轮循数据
|
||||
int16_t temp[3];
|
||||
temp[0] = toValue<int16_t>(frame_buf_, sub_index);
|
||||
temp[1] = toValue<int16_t>(frame_buf_, sub_index);
|
||||
temp[2] = toValue<int16_t>(frame_buf_, sub_index);
|
||||
|
||||
// GPS时间
|
||||
raw_data_.gps_time = toValue<uint32_t>(frame_buf_, sub_index);
|
||||
raw_data_.gps_time_ms = raw_data_.gps_time / 4000.0;
|
||||
|
||||
// 轮循数据类型
|
||||
raw_data_.pollute_type = frame_buf_[sub_index++];
|
||||
|
||||
// 处理轮循数据
|
||||
processPollData(temp, raw_data_.pollute_type);
|
||||
|
||||
// 校验位1
|
||||
raw_data_.check1 = frame_buf_[sub_index++];
|
||||
|
||||
// GPS周数
|
||||
raw_data_.gps_week = toValue<uint32_t>(frame_buf_, sub_index);
|
||||
|
||||
// 重置解析状态
|
||||
is_synced_ = false;
|
||||
@ -265,7 +295,7 @@ class AsensingProtocol
|
||||
}
|
||||
|
||||
// 获取GPS时间（周内秒）(s)
|
||||
double getGpsTime() const { return static_cast<double>(raw_data_.gps_time) * 2.5e-4; }
|
||||
double getGpsTime() const { return raw_data_.gps_time_ms; }
|
||||
|
||||
// 获取GPS周数
|
||||
uint32_t getGpsWeek() const { return raw_data_.gps_week; }
|
||||
@ -289,37 +319,22 @@ class AsensingProtocol
|
||||
// 获取轮循数据类型
|
||||
uint8_t getPollutionType() const { return raw_data_.pollute_type; }
|
||||
|
||||
// 根据轮循数据类型获取温度 (℃)
|
||||
// 只有当pollution_type == 22时有效
|
||||
double getTemperature() const
|
||||
{
|
||||
if (raw_data_.pollute_type == 22)
|
||||
{
|
||||
return static_cast<double>(raw_data_.data1) * 200.0 / 32768.0;
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
// 获取温度 (℃)
|
||||
double getTemperature() const { return raw_data_.temperature; }
|
||||
|
||||
// 根据轮循数据类型获取GNSS状态信息
|
||||
// pollution_type == 32时: Data1=定位状态, Data2=收星数, Data3=定向状态
|
||||
void getGnssStatus(uint8_t& pos_status, uint8_t& sat_count, uint8_t& heading_status) const
|
||||
{
|
||||
if (raw_data_.pollute_type == 32)
|
||||
{
|
||||
pos_status = static_cast<uint8_t>(raw_data_.data1 & 0xFF);
|
||||
sat_count = static_cast<uint8_t>((raw_data_.data2 >> 8) & 0xFF);
|
||||
heading_status = static_cast<uint8_t>(raw_data_.data3 & 0xFF);
|
||||
}
|
||||
else
|
||||
{
|
||||
pos_status = 0;
|
||||
sat_count = 0;
|
||||
heading_status = 0;
|
||||
}
|
||||
}
|
||||
// 获取定位状态
|
||||
uint8_t getPositionType() const { return raw_data_.position_type; }
|
||||
|
||||
// 获取收星数
|
||||
uint8_t getSatCount() const { return raw_data_.numsv; }
|
||||
|
||||
// 获取定向状态
|
||||
uint8_t getHeadingType() const { return raw_data_.heading_type; }
|
||||
|
||||
// 获取轮速状态
|
||||
uint8_t getWheelSpeedStatus() const { return raw_data_.wheel_speed_status; }
|
||||
|
||||
// 获取IMU状态
|
||||
// pollution_type == 49时: Data1=故障标志位 (1=故障, 0=正常)
|
||||
bool isImuNormal() const
|
||||
{
|
||||
if (raw_data_.pollute_type == 49)
|
||||
@ -329,11 +344,49 @@ class AsensingProtocol
|
||||
return true; // 默认正常
|
||||
}
|
||||
|
||||
private:
|
||||
// 处理轮循数据
|
||||
void processPollData(int16_t temp[3], uint8_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case 0: // 定位精度
|
||||
raw_data_.latitude_std = exp(temp[0] / 100.0);
|
||||
raw_data_.longitude_std = exp(temp[1] / 100.0);
|
||||
raw_data_.altitude_std = exp(temp[2] / 100.0);
|
||||
break;
|
||||
case 1: // 定速精度
|
||||
raw_data_.north_velocity_std = exp(temp[0] / 100.0);
|
||||
raw_data_.east_velocity_std = exp(temp[1] / 100.0);
|
||||
raw_data_.ground_velocity_std = exp(temp[2] / 100.0);
|
||||
break;
|
||||
case 2: // 姿态精度
|
||||
raw_data_.roll_std = exp(temp[0] / 100.0);
|
||||
raw_data_.pitch_std = exp(temp[1] / 100.0);
|
||||
raw_data_.azimuth_std = exp(temp[2] / 100.0);
|
||||
break;
|
||||
case 22: // 温度
|
||||
raw_data_.temperature = temp[0] * 200.0 / 32768.0;
|
||||
break;
|
||||
case 32: // GNSS状态
|
||||
raw_data_.position_type = static_cast<uint8_t>(temp[0] & 0xFF);
|
||||
raw_data_.numsv = static_cast<uint8_t>((temp[1] >> 8) & 0xFF);
|
||||
raw_data_.heading_type = static_cast<uint8_t>(temp[2] & 0xFF);
|
||||
break;
|
||||
case 33: // 轮速状态
|
||||
raw_data_.wheel_speed_status = static_cast<uint8_t>((temp[1] >> 8) & 0xFF);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
AsensingInsData raw_data_; // 解析后的数据
|
||||
uint8_t frame_buf_[63]; // 帧缓冲区
|
||||
uint8_t frame_buf_[128]; // 帧缓冲区（足够大）
|
||||
bool is_synced_; // 帧同步状态
|
||||
int frame_index_; // 当前帧索引
|
||||
int m_lengthImu; // 数据长度（不含帧头）
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -50,10 +50,6 @@ class RtkAsensingNode : public rclcpp::Node
|
||||
private:
|
||||
void timer_callback()
|
||||
{
|
||||
static int read_count = 0;
|
||||
static int parse_count = 0;
|
||||
static int publish_count = 0;
|
||||
|
||||
try
|
||||
{
|
||||
// 读取串口数据
|
||||
@ -65,34 +61,13 @@ class RtkAsensingNode : public rclcpp::Node
|
||||
return;
|
||||
}
|
||||
|
||||
read_count++;
|
||||
|
||||
// 打印读取的原始数据（仅前20字节）
|
||||
if (read_count % 100 == 1) // 每100次打印一次，避免刷屏
|
||||
{
|
||||
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();
|
||||
publish_count++;
|
||||
}
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
@ -118,17 +93,8 @@ class RtkAsensingNode : public rclcpp::Node
|
||||
double speed = protocol_parser->getGroundSpeed();
|
||||
|
||||
// 获取定位质量
|
||||
// 根据轮循数据类型获取状态
|
||||
uint8_t pos_status = 0;
|
||||
uint8_t sat_count = 0;
|
||||
uint8_t heading_status = 0;
|
||||
protocol_parser->getGnssStatus(pos_status, sat_count, heading_status);
|
||||
|
||||
// 映射到p_quality和h_quality
|
||||
// 参考协议文档: 位置状态参考附录一
|
||||
// 这里简化处理: 0=未定位, 1=单点定位, 4=RTK固定解, 5=RTK浮点解
|
||||
int p_quality = pos_status;
|
||||
int h_quality = heading_status;
|
||||
int p_quality = protocol_parser->getPositionType();
|
||||
int h_quality = protocol_parser->getHeadingType();
|
||||
|
||||
message.lat = lat;
|
||||
message.lon = lon;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user