From e670fe084851487d2c61fe142979cf96d4ef00f0 Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Mon, 23 Mar 2026 08:47:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9A=82=E6=97=B6=E6=94=BE=E5=BC=83=20CRC=20?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/Transmit/ctrl_frame.md | 2 -- launch/uart_transmitter.launch.py | 8 ++--- src/uart_transmitter_node.cpp | 55 +++++++++++++++++++++++++++---- 3 files changed, 53 insertions(+), 12 deletions(-) diff --git a/docs/Transmit/ctrl_frame.md b/docs/Transmit/ctrl_frame.md index 3628804..b066656 100644 --- a/docs/Transmit/ctrl_frame.md +++ b/docs/Transmit/ctrl_frame.md @@ -20,8 +20,6 @@ SOF(2) + x_move(2) + y_move(2) + yaw(2) + pitch(2) + feed(2) + key(1) + crc8(1) + EOF(1) = 15 bytes ``` -**FRAME_LENGTH = 15** - ## 字节序 - 所有 int16_t 类型字段（x_move, y_move, yaw, pitch, feed）使用 **小端序** diff --git a/launch/uart_transmitter.launch.py b/launch/uart_transmitter.launch.py index 316ca7d..1e52044 100644 --- a/launch/uart_transmitter.launch.py +++ b/launch/uart_transmitter.launch.py @@ -57,14 +57,14 @@ def generate_launch_description(): left_switch_arg = DeclareLaunchArgument( 'left_switch', - default_value='0', - description='左拨杆 [0, 15]' + default_value='3', + description='左拨杆 [1, 3]' ) right_switch_arg = DeclareLaunchArgument( 'right_switch', - default_value='0', - description='右拨杆 [0, 15]' + default_value='3', + description='右拨杆 [1, 3]' ) # 创建节点 diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp index 1fc5931..9063546 100644 --- a/src/uart_transmitter_node.cpp +++ b/src/uart_transmitter_node.cpp @@ -239,11 +239,22 @@ private: // CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身) // 当前idx=13, 数据长度=11 (从frame[2]到frame[12]) - frame[idx++] = calculateCRC8(frame + 2, 11); + // uint8_t crc_value = calculateCRC8(frame + 2, 11); + frame[idx++] = 0xCC; // crc_value // 帧尾 frame[idx++] = FRAME_TAIL; + // 调试输出：打印完整帧内容 + std::string frame_hex; + char buf[4]; + for (int i = 0; i < FRAME_LENGTH; i++) { + snprintf(buf, sizeof(buf), "%02X ", frame[i]); + frame_hex += buf; + } + RCLCPP_INFO(this->get_logger(), "[发包](%d字节): %s", FRAME_LENGTH, + frame_hex.c_str()); + // 发送数据 ssize_t written = write(serial_fd_, frame, FRAME_LENGTH); if (written != FRAME_LENGTH) { @@ -298,12 +309,34 @@ private: calculateCRC8(frame_buffer.data() + 2, FRAME_LENGTH - 4); if (rx_crc == calc_crc) { - RCLCPP_INFO(this->get_logger(), "收到有效帧，CRC 验证通过"); + // 解析数据（小端序） + int16_t x_move = frame_buffer[2] | (frame_buffer[3] << 8); + int16_t y_move = frame_buffer[4] | (frame_buffer[5] << 8); + int16_t yaw = frame_buffer[6] | (frame_buffer[7] << 8); + int16_t pitch = frame_buffer[8] | (frame_buffer[9] << 8); + int16_t feed = frame_buffer[10] | (frame_buffer[11] << 8); + uint8_t key = frame_buffer[12]; + uint8_t left_switch = (key >> 4) & 0x0F; + uint8_t right_switch = key & 0x0F; + + // 输出详细日志 + RCLCPP_INFO(this->get_logger(), + "[收包]: %02X %02X | x:%d y:%d yaw:%d " + "pitch:%d feed:%d | L:%d R:%d | CRC:%02X | %02X", + frame_buffer[0], frame_buffer[1], x_move, y_move, + yaw, pitch, feed, left_switch, right_switch, + rx_crc, frame_buffer[FRAME_LENGTH - 1]); } else { - RCLCPP_WARN(this->get_logger(), - "CRC 错误: 接收=%02X, 计算=%02X", rx_crc, - calc_crc); + RCLCPP_WARN( + this->get_logger(), + "[CRC错误] 接收=%02X, 计算=%02X, 帧内容: %s", rx_crc, + calc_crc, + bytesToHex(frame_buffer.data(), FRAME_LENGTH).c_str()); } + } else { + RCLCPP_WARN(this->get_logger(), + "[帧尾错误] 期望=%02X, 实际=%02X", FRAME_TAIL, + frame_buffer[FRAME_LENGTH - 1]); } // 清空缓冲区，准备下一帧 @@ -318,10 +351,20 @@ private: RCLCPP_ERROR(this->get_logger(), "读取错误: %s", strerror(errno)); } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } + std::string bytesToHex(const uint8_t *data, size_t len) { + std::string result; + char buf[4]; + for (size_t i = 0; i < len; i++) { + snprintf(buf, sizeof(buf), "%02X ", data[i]); + result += buf; + } + return result; + } + void publishStatus() { auto msg = std_msgs::msg::Bool(); msg.data = is_connected_ && (serial_fd_ >= 0);