数据接收回退，添加串口测试
This commit is contained in:
@@ -34,58 +34,10 @@ struct ControlData {
|
||||
uint8_t right_switch = 3;
|
||||
};
|
||||
|
||||
// 接收的传感器数据
|
||||
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;
|
||||
};
|
||||
|
||||
static std::mutex control_mutex;
|
||||
static ControlData shared_control;
|
||||
static std::mutex sensor_mutex;
|
||||
static SensorData shared_sensor;
|
||||
static std::atomic<bool> sender_running{false};
|
||||
static std::atomic<bool> monitor_running{false};
|
||||
static std::thread sender_thread;
|
||||
static std::thread monitor_thread;
|
||||
|
||||
// 解析接收到的数据，从杆量位置解算出4个float
|
||||
// 协议格式：| 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
|
||||
// 映射关系：x_move -> gyro_x, y_move -> accel_x, yaw -> accel_y, pitch -> accel_z
|
||||
bool parseSensorData(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;
|
||||
}
|
||||
|
||||
// 获取传感器数据接口
|
||||
bool getSensorData(SensorData& sensor) {
|
||||
std::lock_guard<std::mutex> lock(sensor_mutex);
|
||||
if (shared_sensor.valid) {
|
||||
sensor = shared_sensor;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// 更新控制数据
|
||||
void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
|
||||
@@ -98,47 +50,9 @@ void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
|
||||
shared_control.right_switch = right_sw;
|
||||
}
|
||||
|
||||
// 传感器监控线程 - 独立打印传感器数据
|
||||
void sensorMonitorLoop() {
|
||||
LOGM(STR_CTR(WORD_GREEN, "Sensor monitor thread started"));
|
||||
|
||||
while (monitor_running) {
|
||||
SensorData sensor;
|
||||
if (getSensorData(sensor)) {
|
||||
// 每秒打印一次传感器数据
|
||||
LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[Monitor] gyro_x=%.3f, accel_x=%.3f, accel_y=%.3f, accel_z=%.3f"),
|
||||
sensor.gyro_x, sensor.accel_x, sensor.accel_y, sensor.accel_z);
|
||||
}
|
||||
|
||||
// 每秒打印一次
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
}
|
||||
|
||||
LOGM(STR_CTR(WORD_YELLOW, "Sensor monitor thread stopped"));
|
||||
}
|
||||
|
||||
// 启动传感器监控线程
|
||||
void startSensorMonitor() {
|
||||
if (!monitor_running) {
|
||||
monitor_running = true;
|
||||
monitor_thread = std::thread(sensorMonitorLoop);
|
||||
}
|
||||
}
|
||||
|
||||
// 停止传感器监控线程
|
||||
void stopSensorMonitor() {
|
||||
if (monitor_running) {
|
||||
monitor_running = false;
|
||||
if (monitor_thread.joinable()) {
|
||||
monitor_thread.join();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 发送线程函数
|
||||
void senderLoop(Serial &serial) {
|
||||
uint8_t tx_frame[FRAME_LENGTH];
|
||||
uint8_t rx_frame[FRAME_LENGTH];
|
||||
|
||||
while (sender_running) {
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
@@ -185,15 +99,6 @@ void senderLoop(Serial &serial) {
|
||||
// 发送数据
|
||||
serial.WriteData(tx_frame, FRAME_LENGTH);
|
||||
|
||||
// 尝试接收数据（非阻塞）
|
||||
if (serial.ReadData(rx_frame, FRAME_LENGTH)) {
|
||||
SensorData sensor;
|
||||
if (parseSensorData(rx_frame, sensor)) {
|
||||
std::lock_guard<std::mutex> lock(sensor_mutex);
|
||||
shared_sensor = sensor;
|
||||
}
|
||||
}
|
||||
|
||||
// 精确控制发送频率
|
||||
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now() - start).count();
|
||||
@@ -210,9 +115,6 @@ void startSender(Serial &serial) {
|
||||
sender_running = true;
|
||||
sender_thread = std::thread(senderLoop, std::ref(serial));
|
||||
LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
|
||||
|
||||
// 同时启动传感器监控线程
|
||||
startSensorMonitor();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -332,7 +234,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
|
||||
}
|
||||
|
||||
// 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
|
||||
updateControl(300, pitch_scan, 0, 3, 2);
|
||||
updateControl(150, pitch_scan, 0, 3, 2);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
BIN
serial_debug
Executable file
BIN
serial_debug
Executable file
Binary file not shown.
269
serial_debug.cpp
Normal file
269
serial_debug.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user