debug-4

src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp

@@ -4,18 +4,19 @@
 #include <cmath>
 #include <cstdint>
 #include <cstring>
-
+#include <string>
-#include "logger/logger.h"
 
 // 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的数据)
+    uint8_t check1;           // 偏移57: 校验位1 (异或校验，包含0-57的数据)
-    uint32_t gps_week;        // 偏移58-61: GPS周数
+
-    uint8_t check2;           // 偏移62: 校验位2 (异或校验，包含0-61的数据)
+    // 解析后的轮循数据
+    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的数据)
+                    // 验证校验位1 (异或校验，包含0-57的数据，共58字节)
-                    uint8_t xor_check1 = 0;
+                    uint8_t xor_check = 0;
-                    for (int j = 0; j < 57; j++)
+                    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;
+                    int sub_index = 3;
-                    for (int j = 0; j < 62; j++)
-                    {
-                        xor_check2 ^= frame_buf_[j];
-                    }
 
-                    if (xor_check2 != frame_buf_[62])
+                    // 横滚角、俯仰角、方位角
-                    {
+                    raw_data_.roll = toValue<int16_t>(frame_buf_, sub_index);
-                        total_checksum2_fail++;
+                    raw_data_.pitch = toValue<int16_t>(frame_buf_, sub_index);
-                        LOG_INFO("[DEBUG-PROTO] 校验位2失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", xor_check2,
+                    raw_data_.yaw = toValue<int16_t>(frame_buf_, sub_index);
-                                 frame_buf_[62], total_checksum2_fail);
-                        // 校验失败，重新同步
-                        is_synced_ = false;
-                        frame_index_ = 0;
-                        continue;
-                    }
 
-                    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 { return raw_data_.temperature; }
-    double getTemperature() const
-    {
-        if (raw_data_.pollute_type == 22)
-        {
-            return static_cast<double>(raw_data_.data1) * 200.0 / 32768.0;
-        }
-        return 0.0;
-    }
 
-    // 根据轮循数据类型获取GNSS状态信息
+    // 获取定位状态
-    // pollution_type == 32时: Data1=定位状态, Data2=收星数, Data3=定向状态
+    uint8_t getPositionType() const { return raw_data_.position_type; }
-    void getGnssStatus(uint8_t& pos_status, uint8_t& sat_count, uint8_t& heading_status) const
+
-    {
+    // 获取收星数
-        if (raw_data_.pollute_type == 32)
+    uint8_t getSatCount() const { return raw_data_.numsv; }
-        {
+
-            pos_status = static_cast<uint8_t>(raw_data_.data1 & 0xFF);
+    // 获取定向状态
-            sat_count = static_cast<uint8_t>((raw_data_.data2 >> 8) & 0xFF);
+    uint8_t getHeadingType() const { return raw_data_.heading_type; }
-            heading_status = static_cast<uint8_t>(raw_data_.data3 & 0xFF);
+
-        }
+    // 获取轮速状态
-        else
+    uint8_t getWheelSpeedStatus() const { return raw_data_.wheel_speed_status; }
-        {
-            pos_status = 0;
-            sat_count = 0;
-            heading_status = 0;
-        }
-    }
 
     // 获取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

src/perception/rtk_asensing/src/rtk_asensing_node.cpp

@@ -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();
 
             // 获取定位质量
-            // 根据轮循数据类型获取状态
+            int p_quality = protocol_parser->getPositionType();
-            uint8_t pos_status = 0;
+            int h_quality = protocol_parser->getHeadingType();
-            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;
 
             message.lat = lat;
             message.lon = lon;