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 79d47ff..b08cf53 100644 --- a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp +++ b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp @@ -4,18 +4,19 @@ #include #include #include - -#include "logger/logger.h" +#include // 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 +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) {} +public: + 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(frame_buf_, sub_index); + raw_data_.pitch = toValue(frame_buf_, sub_index); + raw_data_.yaw = toValue(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(frame_buf_, sub_index); + raw_data_.gyro_y = toValue(frame_buf_, sub_index); + raw_data_.gyro_z = toValue(frame_buf_, sub_index); - // 校验通过，复制数据到结构体 - memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData)); + // 加速度 + raw_data_.accel_x = toValue(frame_buf_, sub_index); + raw_data_.accel_y = toValue(frame_buf_, sub_index); + raw_data_.accel_z = toValue(frame_buf_, sub_index); + + // 经纬度、高度 + raw_data_.latitude = toValue(frame_buf_, sub_index); + raw_data_.longitude = toValue(frame_buf_, sub_index); + raw_data_.altitude = toValue(frame_buf_, sub_index); + + // 速度 + raw_data_.vel_north = toValue(frame_buf_, sub_index); + raw_data_.vel_east = toValue(frame_buf_, sub_index); + raw_data_.vel_down = toValue(frame_buf_, sub_index); + + // 状态 + raw_data_.status = frame_buf_[sub_index++]; + + // 保留字段 + sub_index += 4; + sub_index += 2; + + // 轮循数据 + int16_t temp[3]; + temp[0] = toValue(frame_buf_, sub_index); + temp[1] = toValue(frame_buf_, sub_index); + temp[2] = toValue(frame_buf_, sub_index); + + // GPS时间 + raw_data_.gps_time = toValue(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(frame_buf_, sub_index); // 重置解析状态 is_synced_ = false; @@ -265,7 +295,7 @@ class AsensingProtocol } // 获取GPS时间（周内秒）(s) - double getGpsTime() const { return static_cast(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(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(raw_data_.data1 & 0xFF); - sat_count = static_cast((raw_data_.data2 >> 8) & 0xFF); - heading_status = static_cast(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: +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(temp[0] & 0xFF); + raw_data_.numsv = static_cast((temp[1] >> 8) & 0xFF); + raw_data_.heading_type = static_cast(temp[2] & 0xFF); + break; + case 33: // 轮速状态 + raw_data_.wheel_speed_status = static_cast((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 diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp index 91439c1..18e713c 100644 --- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp +++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp @@ -9,7 +9,7 @@ class RtkAsensingNode : public rclcpp::Node { - public: +public: RtkAsensingNode(std::string name) : Node(name) { LOG_INFO("%s节点已经启动.", name.c_str()); @@ -47,13 +47,9 @@ class RtkAsensingNode : public rclcpp::Node } } - private: +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(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;