400 lines
12 KiB
C++
400 lines
12 KiB
C++
/**
|
||
* @file uart_transmitter_node.cpp
|
||
* @brief UART 串口收发模块测试节点 (CH340)
|
||
*
|
||
* 默认波特率：115200
|
||
* 协议帧格式：
|
||
* | 0xBB | 0x77 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 |
|
||
* CRC8 | 帧尾 | | 0xAA | 0x55 | 2 bytes | 2 bytes | 2 bytes | 2 bytes | 2
|
||
* bytes|1 byte| 1byte| 0xEE |
|
||
*/
|
||
|
||
#include <cstring>
|
||
#include <errno.h>
|
||
#include <fcntl.h>
|
||
#include <rclcpp/rclcpp.hpp>
|
||
#include <std_msgs/msg/bool.hpp>
|
||
#include <termios.h>
|
||
#include <thread>
|
||
#include <unistd.h>
|
||
#include <vector>
|
||
|
||
// 帧定义
|
||
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
|
||
constexpr uint8_t FRAME_HEADER_2 = 0x77;
|
||
constexpr uint8_t FRAME_TAIL = 0xEE;
|
||
constexpr int FRAME_LENGTH = 15;
|
||
|
||
// 默认串口设备
|
||
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
|
||
constexpr int DEFAULT_BAUDRATE = 115200;
|
||
|
||
class UartTransmitterNode : public rclcpp::Node {
|
||
public:
|
||
UartTransmitterNode() : Node("uart_transmitter_node") {
|
||
// 声明参数
|
||
this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
|
||
this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
|
||
this->declare_parameter("send_frequency", 50.0); // Hz
|
||
|
||
// 控制参数
|
||
this->declare_parameter("x_move", 0); // 平动左右 [-660, 660]
|
||
this->declare_parameter("y_move", 0); // 平动前后 [-660, 660]
|
||
this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660]
|
||
this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660]
|
||
this->declare_parameter("feed", 0); // 拨弹轮 [-660, 660]
|
||
this->declare_parameter("left_switch", 0); // 左拨杆 [1, 3]
|
||
this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3]
|
||
|
||
// 获取参数
|
||
serial_port_ = this->get_parameter("serial_port").as_string();
|
||
baudrate_ = this->get_parameter("baudrate").as_int();
|
||
send_frequency_ = this->get_parameter("send_frequency").as_double();
|
||
|
||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
||
RCLCPP_INFO(this->get_logger(), "UART 收发节点启动");
|
||
RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
|
||
RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
|
||
RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
|
||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
||
|
||
// 初始化串口
|
||
if (!initSerial()) {
|
||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
||
rclcpp::shutdown();
|
||
return;
|
||
}
|
||
|
||
// 创建发布者
|
||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
|
||
"transmitter/connection_status", 10);
|
||
|
||
// 创建定时器 - 发送数据
|
||
auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
|
||
send_timer_ = this->create_wall_timer(
|
||
std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
|
||
std::bind(&UartTransmitterNode::sendControlFrame, this));
|
||
|
||
// 创建定时器 - 发布连接状态
|
||
status_timer_ = this->create_wall_timer(
|
||
std::chrono::seconds(1),
|
||
std::bind(&UartTransmitterNode::publishStatus, this));
|
||
|
||
// 创建接收线程
|
||
receive_thread_ = std::thread(&UartTransmitterNode::receiveLoop, this);
|
||
|
||
RCLCPP_INFO(this->get_logger(), "UART 节点初始化完成");
|
||
}
|
||
|
||
~UartTransmitterNode() {
|
||
running_ = false;
|
||
if (receive_thread_.joinable()) {
|
||
receive_thread_.join();
|
||
}
|
||
if (serial_fd_ >= 0) {
|
||
close(serial_fd_);
|
||
}
|
||
RCLCPP_INFO(this->get_logger(), "串口已关闭");
|
||
}
|
||
|
||
private:
|
||
bool initSerial() {
|
||
RCLCPP_INFO(this->get_logger(), "正在打开串口 %s...", serial_port_.c_str());
|
||
|
||
// 打开串口
|
||
serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
||
if (serial_fd_ < 0) {
|
||
RCLCPP_ERROR(this->get_logger(), "无法打开串口 %s: %s",
|
||
serial_port_.c_str(), strerror(errno));
|
||
return false;
|
||
}
|
||
|
||
// 配置串口
|
||
struct termios tty;
|
||
memset(&tty, 0, sizeof(tty));
|
||
|
||
if (tcgetattr(serial_fd_, &tty) != 0) {
|
||
RCLCPP_ERROR(this->get_logger(), "tcgetattr 错误: %s", strerror(errno));
|
||
close(serial_fd_);
|
||
serial_fd_ = -1;
|
||
return false;
|
||
}
|
||
|
||
// 设置波特率
|
||
speed_t baud = convertBaudrate(baudrate_);
|
||
cfsetospeed(&tty, baud);
|
||
cfsetispeed(&tty, baud);
|
||
|
||
// 8N1
|
||
tty.c_cflag &= ~PARENB; // 无校验
|
||
tty.c_cflag &= ~CSTOPB; // 1位停止位
|
||
tty.c_cflag &= ~CSIZE;
|
||
tty.c_cflag |= CS8; // 8位数据
|
||
tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线
|
||
|
||
// 禁用硬件流控
|
||
tty.c_cflag &= ~CRTSCTS;
|
||
|
||
// 原始模式
|
||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||
tty.c_oflag &= ~OPOST;
|
||
|
||
// 设置超时
|
||
tty.c_cc[VMIN] = 0;
|
||
tty.c_cc[VTIME] = 1; // 100ms 超时
|
||
|
||
if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
|
||
RCLCPP_ERROR(this->get_logger(), "tcsetattr 错误: %s", strerror(errno));
|
||
close(serial_fd_);
|
||
serial_fd_ = -1;
|
||
return false;
|
||
}
|
||
|
||
// 清空缓冲区
|
||
tcflush(serial_fd_, TCIOFLUSH);
|
||
|
||
RCLCPP_INFO(this->get_logger(), "串口打开成功");
|
||
is_connected_ = true;
|
||
return true;
|
||
}
|
||
|
||
speed_t convertBaudrate(int baudrate) {
|
||
switch (baudrate) {
|
||
case 9600:
|
||
return B9600;
|
||
case 19200:
|
||
return B19200;
|
||
case 38400:
|
||
return B38400;
|
||
case 57600:
|
||
return B57600;
|
||
case 115200:
|
||
return B115200;
|
||
case 230400:
|
||
return B230400;
|
||
case 460800:
|
||
return B460800;
|
||
case 921600:
|
||
return B921600;
|
||
default:
|
||
RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200",
|
||
baudrate);
|
||
return B115200;
|
||
}
|
||
}
|
||
|
||
void sendControlFrame() {
|
||
if (serial_fd_ < 0) {
|
||
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
|
||
"串口未打开");
|
||
return;
|
||
}
|
||
|
||
// 获取控制参数
|
||
int16_t x_move =
|
||
static_cast<int16_t>(this->get_parameter("x_move").as_int());
|
||
int16_t y_move =
|
||
static_cast<int16_t>(this->get_parameter("y_move").as_int());
|
||
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
|
||
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
|
||
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
|
||
uint8_t left_switch =
|
||
static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
|
||
0x0F;
|
||
uint8_t right_switch =
|
||
static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) &
|
||
0x0F;
|
||
|
||
// 构建数据帧
|
||
uint8_t frame[FRAME_LENGTH];
|
||
int idx = 0;
|
||
|
||
// 帧头
|
||
frame[idx++] = FRAME_HEADER_1;
|
||
frame[idx++] = FRAME_HEADER_2;
|
||
|
||
// 平动左右 (2 bytes, int16, 小端序)
|
||
frame[idx++] = x_move & 0xFF;
|
||
frame[idx++] = (x_move >> 8) & 0xFF;
|
||
|
||
// 平动前后 (2 bytes, 小端序)
|
||
frame[idx++] = y_move & 0xFF;
|
||
frame[idx++] = (y_move >> 8) & 0xFF;
|
||
|
||
// 云台偏航 (2 bytes, 小端序)
|
||
frame[idx++] = yaw & 0xFF;
|
||
frame[idx++] = (yaw >> 8) & 0xFF;
|
||
|
||
// 云台俯仰 (2 bytes, 小端序)
|
||
frame[idx++] = pitch & 0xFF;
|
||
frame[idx++] = (pitch >> 8) & 0xFF;
|
||
|
||
// 拨弹轮 (2 bytes, 小端序)
|
||
frame[idx++] = feed & 0xFF;
|
||
frame[idx++] = (feed >> 8) & 0xFF;
|
||
|
||
// 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
|
||
frame[idx++] = (left_switch << 4) | right_switch;
|
||
|
||
// CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
|
||
// 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
|
||
// 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(), "[TX](%dB): %s", FRAME_LENGTH,
|
||
frame_hex.c_str());
|
||
|
||
// 发送数据
|
||
ssize_t written = write(serial_fd_, frame, FRAME_LENGTH);
|
||
if (written != FRAME_LENGTH) {
|
||
RCLCPP_WARN(this->get_logger(), "发送数据不完整: %zd/%d", written,
|
||
FRAME_LENGTH);
|
||
}
|
||
}
|
||
|
||
uint8_t calculateCRC8(const uint8_t *data, size_t len) {
|
||
uint8_t crc = 0xFF; // 初始值
|
||
for (size_t i = 0; i < len; i++) {
|
||
crc ^= data[i];
|
||
for (int j = 0; j < 8; j++) {
|
||
if (crc & 0x80) {
|
||
crc = (crc << 1) ^ 0x31; // CRC8-MAXIM 多项式
|
||
} else {
|
||
crc <<= 1;
|
||
}
|
||
}
|
||
}
|
||
return crc;
|
||
}
|
||
|
||
void receiveLoop() {
|
||
uint8_t buffer[256];
|
||
std::vector<uint8_t> frame_buffer;
|
||
frame_buffer.reserve(FRAME_LENGTH);
|
||
|
||
while (running_ && rclcpp::ok()) {
|
||
if (serial_fd_ < 0) {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
continue;
|
||
}
|
||
|
||
// 读取数据
|
||
ssize_t n = read(serial_fd_, buffer, sizeof(buffer));
|
||
if (n > 0) {
|
||
for (ssize_t i = 0; i < n; i++) {
|
||
frame_buffer.push_back(buffer[i]);
|
||
|
||
// 查找帧头
|
||
if (frame_buffer.size() >= 2 && frame_buffer[0] == FRAME_HEADER_1 &&
|
||
frame_buffer[1] == FRAME_HEADER_2) {
|
||
|
||
// 等待完整帧
|
||
if (frame_buffer.size() >= FRAME_LENGTH) {
|
||
// 检查帧尾
|
||
if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
|
||
// 验证 CRC
|
||
uint8_t rx_crc = frame_buffer[FRAME_LENGTH - 2];
|
||
uint8_t calc_crc =
|
||
calculateCRC8(frame_buffer.data() + 2, FRAME_LENGTH - 4);
|
||
|
||
if (rx_crc == calc_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(),
|
||
"[RX]: %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, 帧内容: %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]);
|
||
}
|
||
|
||
// 清空缓冲区，准备下一帧
|
||
frame_buffer.clear();
|
||
}
|
||
} else if (frame_buffer.size() > FRAME_LENGTH) {
|
||
// 缓冲区溢出，清空
|
||
frame_buffer.clear();
|
||
}
|
||
}
|
||
} else if (n < 0 && errno != EAGAIN) {
|
||
RCLCPP_ERROR(this->get_logger(), "读取错误: %s", strerror(errno));
|
||
}
|
||
|
||
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);
|
||
connection_status_pub_->publish(msg);
|
||
}
|
||
|
||
// 成员变量
|
||
std::string serial_port_;
|
||
int baudrate_;
|
||
double send_frequency_;
|
||
|
||
int serial_fd_ = -1;
|
||
bool is_connected_ = false;
|
||
bool running_ = true;
|
||
std::thread receive_thread_;
|
||
|
||
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
||
rclcpp::TimerBase::SharedPtr send_timer_;
|
||
rclcpp::TimerBase::SharedPtr status_timer_;
|
||
};
|
||
|
||
int main(int argc, char *argv[]) {
|
||
rclcpp::init(argc, argv);
|
||
|
||
auto node = std::make_shared<UartTransmitterNode>();
|
||
|
||
if (rclcpp::ok()) {
|
||
rclcpp::spin(node);
|
||
}
|
||
|
||
rclcpp::shutdown();
|
||
return 0;
|
||
} |