VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

数据接收回退，添加串口测试

Browse Source
This commit is contained in:
Luo Yufei
2026-03-28 09:45:59 +08:00
parent 7fe0cb5d10
commit c4965756e6
3 changed files with 270 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
serial_debug.cpp Normal file
View File

@@ -0,0 +1,269 @@
/**
* @file serial_debug.cpp
* @brief 串口调试程序：接收数据并打印解算结果和原始数据
*
* 用法: ./serial_debug [串口设备]
* 示例: ./serial_debug /dev/ttyCH340
*/
#include <iostream>
#include <iomanip>
#include <cstring>
#include <vector>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <chrono>
#include <thread>
#include <csignal>
// 协议定义
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 BAUDRATE = 115200;
// 传感器数据结构
struct SensorData {
float gyro_x = 0.0f; // 陀螺仪x
float accel_x = 0.0f; // 加速度计x
float accel_y = 0.0f; // 加速度计y
float accel_z = 0.0f; // 加速度计z
bool valid = false;
};
// 全局标志用于退出
volatile bool running = true;
void signalHandler(int) {
running = false;
}
// 解析接收到的数据
bool parseFrame(const uint8_t* data, SensorData& sensor) {
// 检查帧头帧尾
if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) {
return false;
}
// 解析4个杆量值（小端序 int16）
int16_t x_move = data[2] | (data[3] << 8);
int16_t y_move = data[4] | (data[5] << 8);
int16_t yaw_val = data[6] | (data[7] << 8);
int16_t pitch_val = data[8] | (data[9] << 8);
// 转换为float（假设原始数据是放大了100倍的）
sensor.gyro_x = x_move / 100.0f;
sensor.accel_x = y_move / 100.0f;
sensor.accel_y = yaw_val / 100.0f;
sensor.accel_z = pitch_val / 100.0f;
sensor.valid = true;
return true;
}
// 打印原始数据帧（十六进制）
void printRawData(const uint8_t* data, int len) {
std::cout << "Raw Data: ";
for (int i = 0; i < len; i++) {
std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(data[i]) << " ";
}
std::cout << std::dec << std::nouppercase << std::endl;
}
// 打印解算的传感器数据
void printSensorData(const SensorData& sensor) {
std::cout << "========================================" << std::endl;
std::cout << " gyro_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.gyro_x << std::endl;
std::cout << " accel_x: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_x << std::endl;
std::cout << " accel_y: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_y << std::endl;
std::cout << " accel_z: " << std::setw(8) << std::fixed << std::setprecision(3) << sensor.accel_z << std::endl;
std::cout << "========================================" << std::endl;
}
// 解析并打印帧中其他字段
void printFrameDetails(const uint8_t* data) {
int16_t x_move = data[2] | (data[3] << 8);
int16_t y_move = data[4] | (data[5] << 8);
int16_t yaw = data[6] | (data[7] << 8);
int16_t pitch = data[8] | (data[9] << 8);
int16_t feed = data[10] | (data[11] << 8);
uint8_t switches = data[12];
uint8_t left_switch = (switches >> 4) & 0x0F;
uint8_t right_switch = switches & 0x0F;
uint8_t crc = data[13];
std::cout << "Frame Details:" << std::endl;
std::cout << " Header: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(data[0]) << " 0x" << std::setw(2) << static_cast<int>(data[1]) << std::endl;
std::cout << std::dec << std::nouppercase;
std::cout << " x_move: " << std::setw(5) << x_move << std::endl;
std::cout << " y_move: " << std::setw(5) << y_move << std::endl;
std::cout << " yaw: " << std::setw(5) << yaw << std::endl;
std::cout << " pitch: " << std::setw(5) << pitch << std::endl;
std::cout << " feed: " << std::setw(5) << feed << std::endl;
std::cout << " switch: L=" << static_cast<int>(left_switch) << " R=" << static_cast<int>(right_switch) << std::endl;
std::cout << " CRC: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(crc) << std::endl;
std::cout << std::dec << std::nouppercase;
std::cout << " Tail: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
<< static_cast<int>(data[14]) << std::endl;
std::cout << std::dec << std::nouppercase << std::endl;
}
// 初始化串口
int initSerial(const char* port, int baudrate) {
int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
if (fd < 0) {
std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl;
return -1;
}
struct termios tty;
memset(&tty, 0, sizeof(tty));
if (tcgetattr(fd, &tty) != 0) {
std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl;
close(fd);
return -1;
}
// 设置波特率
cfsetospeed(&tty, B115200);
cfsetispeed(&tty, B115200);
// 8N1
tty.c_cflag &= ~PARENB;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8;
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;
if (tcsetattr(fd, TCSANOW, &tty) != 0) {
std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl;
close(fd);
return -1;
}
tcflush(fd, TCIOFLUSH);
std::cout << "串口 " << port << " 打开成功" << std::endl;
return fd;
}
int main(int argc, char* argv[]) {
// 设置信号处理
signal(SIGINT, signalHandler);
signal(SIGTERM, signalHandler);
// 获取串口设备
const char* port = (argc > 1) ? argv[1] : DEFAULT_SERIAL_PORT;
std::cout << "========================================" << std::endl;
std::cout << " 串口调试程序 - Serial Debugger" << std::endl;
std::cout << " 设备: " << port << std::endl;
std::cout << " 波特率: " << BAUDRATE << std::endl;
std::cout << " 按 Ctrl+C 停止" << std::endl;
std::cout << "========================================" << std::endl << std::endl;
// 初始化串口
int fd = initSerial(port, BAUDRATE);
if (fd < 0) {
return -1;
}
uint8_t buffer[256];
std::vector<uint8_t> frame_buffer;
frame_buffer.reserve(FRAME_LENGTH);
int frame_count = 0;
int valid_count = 0;
auto start_time = std::chrono::steady_clock::now();
while (running) {
// 读取数据
ssize_t n = read(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) {
frame_count++;
// 打印帧号和时间
auto now = std::chrono::steady_clock::now();
auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count();
std::cout << "\n========== Frame #" << frame_count
<< " (Time: " << elapsed << "s) ==========" << std::endl;
// 打印原始数据
printRawData(frame_buffer.data(), FRAME_LENGTH);
// 检查帧尾
if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
// 解析并打印数据
SensorData sensor;
if (parseFrame(frame_buffer.data(), sensor)) {
valid_count++;
std::cout << "Status: VALID" << std::endl;
printSensorData(sensor);
}
// 打印帧详情
printFrameDetails(frame_buffer.data());
} else {
std::cout << "Status: INVALID (Frame tail error)" << std::endl;
std::cout << "Expected: 0x" << std::hex << std::uppercase
<< std::setw(2) << std::setfill('0')
<< static_cast<int>(FRAME_TAIL) << std::endl;
std::cout << "Got: 0x" << std::setw(2)
<< static_cast<int>(frame_buffer[FRAME_LENGTH - 1]) << std::endl;
std::cout << std::dec << std::nouppercase << std::endl;
}
// 清空缓冲区
frame_buffer.clear();
}
} else if (frame_buffer.size() > FRAME_LENGTH) {
// 缓冲区溢出，丢弃数据
frame_buffer.erase(frame_buffer.begin());
}
}
} else if (n < 0 && errno != EAGAIN) {
std::cerr << "读取错误: " << strerror(errno) << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
// 打印统计信息
std::cout << "\n========================================" << std::endl;
std::cout << " 统计信息:" << std::endl;
std::cout << " 总帧数: " << frame_count << std::endl;
std::cout << " 有效帧: " << valid_count << std::endl;
std::cout << " 有效率: " << (frame_count > 0 ? (100.0 * valid_count / frame_count) : 0) << "%" << std::endl;
std::cout << "========================================" << std::endl;
close(fd);
std::cout << "串口已关闭" << std::endl;
return 0;
}