diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index d4fc98b..8d08005 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -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 sender_running{false}; -static std::atomic 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 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 lock(sensor_mutex); - shared_sensor = sensor; - } - } - // 精确控制发送频率 auto elapsed = std::chrono::duration_cast( 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; } } \ No newline at end of file diff --git a/serial_debug b/serial_debug new file mode 100755 index 0000000..546a3d7 Binary files /dev/null and b/serial_debug differ diff --git a/serial_debug.cpp b/serial_debug.cpp new file mode 100644 index 0000000..9cdd828 --- /dev/null +++ b/serial_debug.cpp @@ -0,0 +1,269 @@ +/** + * @file serial_debug.cpp + * @brief 串口调试程序：接收数据并打印解算结果和原始数据 + * + * 用法: ./serial_debug [串口设备] + * 示例: ./serial_debug /dev/ttyCH340 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// 协议定义 +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(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(data[0]) << " 0x" << std::setw(2) << static_cast(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(left_switch) << " R=" << static_cast(right_switch) << std::endl; + std::cout << " CRC: 0x" << std::hex << std::uppercase << std::setw(2) << std::setfill('0') + << static_cast(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(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 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(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(FRAME_TAIL) << std::endl; + std::cout << "Got: 0x" << std::setw(2) + << static_cast(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; +} \ No newline at end of file