269 lines
10 KiB
C++
269 lines
10 KiB
C++
/**
|
||
* @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;
|
||
} |