From d195deaa4889c56af1c99d68278678670f938032 Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sat, 28 Mar 2026 04:08:07 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BF=81=E7=A7=BB=E5=85=A5tty=E6=8C=81?= =?UTF-8?q?=E7=BB=AD=E8=BF=9E=E6=8E=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/include/serialComm.hpp | 58 ++++++++ armor/include/serialManager.hpp | 126 +++++++++++++++++ armor/include/unifiedManager.hpp | 49 +++++++ armor/src/armor_finder/anti_top/anti_top.cpp | 6 + armor/src/armor_finder/armor_finder.cpp | 14 +- .../armor_finder/send_target/send_target.cpp | 4 + armor/src/serialComm.cpp | 131 ++++++++++++++++++ armor/src/unifiedManager.cpp | 112 +++++++++++++++ main.cpp | 5 +- others/include/config/setconfig.h | 2 +- 10 files changed, 504 insertions(+), 3 deletions(-) create mode 100644 armor/include/serialComm.hpp create mode 100644 armor/include/serialManager.hpp create mode 100644 armor/include/unifiedManager.hpp create mode 100644 armor/src/serialComm.cpp create mode 100644 armor/src/unifiedManager.cpp diff --git a/armor/include/serialComm.hpp b/armor/include/serialComm.hpp new file mode 100644 index 0000000..52c8885 --- /dev/null +++ b/armor/include/serialComm.hpp @@ -0,0 +1,58 @@ +#ifndef SERIAL_COMM_H +#define SERIAL_COMM_H + +#include +#include "CSerialPort/SerialPort.h" +#include "CSerialPort/SerialPortInfo.h" +#include + +using namespace itas109; + +// Serial port configuration +namespace SerialConfig { + constexpr int BAUD_RATE = 115200; + constexpr Parity PARITY_TYPE = Parity::ParityNone; + constexpr DataBits DATA_BITS = DataBits::DataBits8; + constexpr StopBits STOP_BITS = StopBits::StopOne; + constexpr FlowControl FLOW_CTRL = FlowControl::FlowNone; + constexpr int READ_TIMEOUT_MS = 1000; +} + +class SerialComm { +private: + CSerialPort m_serialPort; + char m_portName[256]; + bool m_isOpen; + +public: + SerialComm(); + ~SerialComm(); + + // Find first ttyUSB device + bool findFirstTtyUSB(); + + // Open serial port + bool openPort(); + + // Close serial port + void closePort(); + + // Send data + bool sendData(const char* data, size_t length); + bool sendData(const uint8_t* data, size_t length); + + // Receive data + int receiveData(uint8_t* buffer, size_t maxLength); + int receiveData(char* buffer, size_t maxLength); + + // Check if port is open + bool isOpen() const { return m_isOpen; } + + // Get current port name + const char* getPortName() const { return m_portName; } + + // List all available ports + static void listAllPorts(); +}; + +#endif // SERIAL_COMM_H \ No newline at end of file diff --git a/armor/include/serialManager.hpp b/armor/include/serialManager.hpp new file mode 100644 index 0000000..467dd45 --- /dev/null +++ b/armor/include/serialManager.hpp @@ -0,0 +1,126 @@ +#ifndef SERIAL_MANAGER_HPP +#define SERIAL_MANAGER_HPP + +#include "serialComm.hpp" +#include +#include +#include +#include +#include + +class SerialManager { +private: + SerialComm m_serial; + std::thread m_connectionThread; + std::atomic m_isConnected; + std::atomic m_shouldStop; + std::mutex m_serialMutex; + int m_retryIntervalMs; + + // 后台重连线程函数 + void connectionThreadFunc() { + fmt::print("[I][SerialMgr]: RETRY\n"); + + while (!m_shouldStop.load()) { + if (!m_isConnected.load()) { + + std::lock_guard lock(m_serialMutex); + + if (m_serial.findFirstTtyUSB() && m_serial.openPort()) { + m_isConnected.store(true); + } else { + fmt::print("[W][SerialMgr]: Failed, retry in {}ms\n", m_retryIntervalMs); + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs)); + } + + fmt::print("[I][SerialMgr]: EXIT\n"); + } + +public: + // 构造函数：retryIntervalMs - 重试间隔（毫秒） + SerialManager(int retryIntervalMs = 500) + : m_isConnected(false) + , m_shouldStop(false) + , m_retryIntervalMs(retryIntervalMs) { + } + + ~SerialManager() { + stop(); + } + + // 启动管理器（开始后台重连） + void start() { + if (!m_connectionThread.joinable()) { + m_shouldStop.store(false); + m_connectionThread = std::thread(&SerialManager::connectionThreadFunc, this); + fmt::print("[I][SerialMgr]: START\n"); + } + } + + // 停止管理器 + void stop() { + if (m_connectionThread.joinable()) { + m_shouldStop.store(true); + m_connectionThread.join(); + + std::lock_guard lock(m_serialMutex); + m_serial.closePort(); + m_isConnected.store(false); + + fmt::print("[I][SerialMgr]: STOP\n"); + } + } + + // 检查是否已连接 + bool isConnected() const { + return m_isConnected.load(); + } + + // 发送数据（线程安全） + bool sendData(const char* data, size_t length) { + if (!m_isConnected.load()) { + return false; + } + + std::lock_guard lock(m_serialMutex); + + if (!m_serial.sendData(data, length)) { + // 发送失败，标记为断开 + m_isConnected.store(false); + fmt::print("[W][SerialMgr]: Failed, mark DISCONNECT\n"); + return false; + } + + return true; + } + + // 发送数据（字节数组） + bool sendData(const uint8_t* data, size_t length) { + return sendData(reinterpret_cast(data), length); + } + + // 接收数据（线程安全） + int receiveData(uint8_t* buffer, size_t maxLength) { + if (!m_isConnected.load()) { + return -1; + } + + std::lock_guard lock(m_serialMutex); + return m_serial.receiveData(buffer, maxLength); + } + + // 接收数据（字符数组） + int receiveData(char* buffer, size_t maxLength) { + return receiveData(reinterpret_cast(buffer), maxLength); + } + + // 获取端口名 + const char* getPortName() const { + return m_serial.getPortName(); + } +}; + +#endif // SERIAL_MANAGER_HPP \ No newline at end of file diff --git a/armor/include/unifiedManager.hpp b/armor/include/unifiedManager.hpp new file mode 100644 index 0000000..baae585 --- /dev/null +++ b/armor/include/unifiedManager.hpp @@ -0,0 +1,49 @@ +#ifndef UNIFIED_MANAGER_HPP +#define UNIFIED_MANAGER_HPP + +#include "serialComm.hpp" +#include +#include +#include +#include + +// Unified device manager (Serial ) +class UnifiedDeviceManager { +private: + // Device instances + std::unique_ptr m_serial; + std::unique_ptr m_camera; + + // Thread management + std::thread m_serialReconnectThread; + std::atomic m_serialConnected; + std::atomic m_shouldStop; + + std::mutex m_serialMutex; + + int m_retryIntervalMs; + + // Serial reconnect thread + void serialReconnectThreadFunc(); + +public: + UnifiedDeviceManager(int retryIntervalMs = 500); + ~UnifiedDeviceManager(); + + // Control methods + void start(); + void stop(); + + // Status check + bool isSerialConnected() const { return m_serialConnected.load(); } + + // Serial operations (thread-safe) + bool sendData(const char* data, size_t length); + int receiveData(uint8_t* buffer, size_t maxLength); + + // Camera operations (thread-safe) + bool grabFrame(cv::Mat& frame); + const char* getCameraName() const; +}; + +#endif // UNIFIED_MANAGER_HPP diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 97a1feb..ff2d48e 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -38,6 +38,12 @@ void ArmorFinder::antiTop() { // 通过两次装甲角度为零的时间差计算陀螺旋转周期 // 根据旋转周期计算下一次装甲出现在角度为零的时间点 if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) { + sum_yaw = sum_pitch = 0; + double dx = target_box.rect.x + target_box.rect.width / 2.0 - IMAGE_CENTER_X; + double dy = target_box.rect.y + target_box.rect.height / 2.0 - IMAGE_CENTER_Y; + last_yaw = dx; + last_pitch = dy; + auto front_time = getFrontTime(time_seq, angle_seq); auto once_periodms = getTimeIntervalms(front_time, last_front_time); // if (abs(once_periodms - top_periodms[-1]) > 50) { diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 96fd655..c98d3c8 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -54,7 +54,9 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string ¶s_folder, anti_switch_cnt(0), classifier(paras_folder), contour_area(0), - tracking_cnt(0) { + tracking_cnt(0), + last_yaw(0), last_pitch(0), + sum_yaw(0), sum_pitch(0) { } void ArmorFinder::run(cv::Mat &src) { @@ -75,6 +77,9 @@ void ArmorFinder::run(cv::Mat &src) { tracker->init(src, target_box.rect); state = TRACKING_STATE; tracking_cnt = 0; + last_yaw = target_box.rect.x + target_box.rect.width / 2.0 - IMAGE_CENTER_X; + last_pitch = target_box.rect.y + target_box.rect.height / 2.0 - IMAGE_CENTER_Y; + sum_yaw = sum_pitch = 0; LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track")); } } @@ -82,6 +87,8 @@ void ArmorFinder::run(cv::Mat &src) { case TRACKING_STATE: if (!stateTrackingTarget(src) || ++tracking_cnt > 100) { // 最多追踪100帧图像 state = SEARCHING_STATE; + last_yaw = last_pitch = 0; + sum_yaw = sum_pitch = 0; LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!")); } break; @@ -93,6 +100,11 @@ end: if(is_anti_top) { // 判断当前是否为反陀螺模式 antiTop(); }else if(target_box.rect != cv::Rect2d()) { + if (last_box.rect == cv::Rect2d()) { + sum_yaw = sum_pitch = 0; + last_yaw = target_box.rect.x + target_box.rect.width / 2.0 - IMAGE_CENTER_X; + last_pitch = target_box.rect.y + target_box.rect.height / 2.0 - IMAGE_CENTER_Y; + } anti_top_cnt = 0; time_seq.clear(); angle_seq.clear(); diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 0a77bfd..ee643b9 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -53,8 +53,12 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y; // PID + #define MAX_I 100 // 这里的数值可以根据实际调试情况进一步微调 sum_yaw += dx; sum_pitch += dy; + sum_yaw = (sum_yaw > MAX_I) ? MAX_I : (sum_yaw < -MAX_I ? -MAX_I : sum_yaw); + sum_pitch = (sum_pitch > MAX_I) ? MAX_I : (sum_pitch < -MAX_I ? -MAX_I : sum_pitch); + float yaw_I_component = YAW_AIM_KI * sum_yaw; float pitch_I_component = PITCH_AIM_KI * sum_pitch; diff --git a/armor/src/serialComm.cpp b/armor/src/serialComm.cpp new file mode 100644 index 0000000..167be7c --- /dev/null +++ b/armor/src/serialComm.cpp @@ -0,0 +1,131 @@ +#include "serialComm.hpp" +#include +#include + +SerialComm::SerialComm() : m_isOpen(false) { + std::memset(m_portName, 0, sizeof(m_portName)); + + // Initialize serial port parameters + m_serialPort.init( + "", + SerialConfig::BAUD_RATE, + SerialConfig::PARITY_TYPE, + SerialConfig::DATA_BITS, + SerialConfig::STOP_BITS, + SerialConfig::FLOW_CTRL, + SerialConfig::READ_TIMEOUT_MS + ); +} + +SerialComm::~SerialComm() { + closePort(); +} + +bool SerialComm::findFirstTtyUSB() { + std::vector portInfos = CSerialPortInfo::availablePortInfos(); + + for (const auto& info : portInfos) { + + // 查找第一个 ttyUSB 设备 + const char* portStr = info.portName; + if (std::strstr(portStr, "ttyCH341USB") != nullptr) { + std::strncpy(m_portName, portStr, sizeof(m_portName) - 1); + m_portName[sizeof(m_portName) - 1] = '\0'; // 字符串结束 + fmt::print("[I][SERIAL]: ttyUSB device found: {}\n", m_portName); + return true; + } + } + + return false; +} + +bool SerialComm::openPort() { + if (std::strlen(m_portName) == 0) { + fmt::print("[E][Serial]: Port name is empty, call findFirstTtyUSB() first\n"); + return false; + } + + // Set port name + m_serialPort.init( + m_portName, + SerialConfig::BAUD_RATE, + SerialConfig::PARITY_TYPE, + SerialConfig::DATA_BITS, + SerialConfig::STOP_BITS, + SerialConfig::FLOW_CTRL, + SerialConfig::READ_TIMEOUT_MS + ); + + if (!m_serialPort.open()) { + fmt::print("[E][Serial]: Failed to open port {}\n", m_portName); + fmt::print("[E][Serial]: Error code: {}\n", m_serialPort.getLastError()); + m_isOpen = false; + return false; + } + + m_isOpen = true; + fmt::print("[I][Serial]: Port opened successfully: {}\n", m_portName); + fmt::print("[I][Serial]: Baud rate: {}\n", SerialConfig::BAUD_RATE); + fmt::print("[I][Serial]: Data bits: 8\n"); + fmt::print("[I][Serial]: Stop bits: 1\n"); + fmt::print("[I][Serial]: Parity: None\n"); + + return true; +} + +void SerialComm::closePort() { + if (m_isOpen) { + m_serialPort.close(); + m_isOpen = false; + fmt::print("[I][Serial]: Port closed: {}\n", m_portName); + } +} + +bool SerialComm::sendData(const char* data, size_t length) { + return sendData(reinterpret_cast(data), length); +} + +bool SerialComm::sendData(const uint8_t* data, size_t length) { + if (!m_isOpen) { + fmt::print("[E][Serial]: Port not open\n"); + return false; + } + + int bytesWritten = m_serialPort.writeData(data, length); + + if (bytesWritten > 0) { + fmt::print("[I][Serial]: Sent {} bytes\n", bytesWritten); + return true; + } else { + fmt::print("[E][Serial]: Failed to send data\n"); + return false; + } +} + +int SerialComm::receiveData(char* buffer, size_t maxLength) { + return receiveData(reinterpret_cast(buffer), maxLength); +} + +int SerialComm::receiveData(uint8_t* buffer, size_t maxLength) { + if (!m_isOpen) { + fmt::print("[E][Serial]: Port not open\n"); + return -1; + } + + int bytesRead = m_serialPort.readData(buffer, maxLength); + + if (bytesRead > 0) { + fmt::print("[I][Serial]: Received {} bytes\n", bytesRead); + } + + return bytesRead; +} + +void SerialComm::listAllPorts() { + std::vector portInfos = CSerialPortInfo::availablePortInfos(); + + fmt::print("[I][Serial]: Available ports:\n"); + for (const auto& info : portInfos) { + fmt::print("[I][Serial]: - {}\n", info.portName); + } +} \ No newline at end of file diff --git a/armor/src/unifiedManager.cpp b/armor/src/unifiedManager.cpp new file mode 100644 index 0000000..54f81e4 --- /dev/null +++ b/armor/src/unifiedManager.cpp @@ -0,0 +1,112 @@ +#include "unifiedManager.hpp" + +UnifiedDeviceManager::UnifiedDeviceManager(int retryIntervalMs) + : m_serial(std::make_unique()) + , m_camera(std::make_unique(retryIntervalMs)) + , m_serialConnected(false) + , m_shouldStop(false) + , m_retryIntervalMs(retryIntervalMs) +{ + fmt::print("[I][Managr]: Unified device manager created\n"); +} + +UnifiedDeviceManager::~UnifiedDeviceManager() { + stop(); +} + +void UnifiedDeviceManager::serialReconnectThreadFunc() { + fmt::print("[I][Managr]: Serial reconnect thread started\n"); + + while (!m_shouldStop.load()) { + if (!m_serialConnected.load()) { + fmt::print("[I][Managr]: Attempting to connect serial port...\n"); + + std::lock_guard lock(m_serialMutex); + + if (m_serial->findFirstTtyUSB() && m_serial->openPort()) { + m_serialConnected.store(true); + fmt::print("[I][Managr]: Serial port connected successfully\n"); + } else { + fmt::print("[W][Managr]: Serial connection failed, retry in {}ms\n", + m_retryIntervalMs); + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs)); + } + + fmt::print("[I][Managr]: Serial reconnect thread stopped\n"); +} + +void UnifiedDeviceManager::start() { + fmt::print("[I][Managr]: Starting unified device manager\n"); + + m_shouldStop.store(false); + + // Start serial reconnect thread + if (!m_serialReconnectThread.joinable()) { + m_serialReconnectThread = std::thread(&UnifiedDeviceManager::serialReconnectThreadFunc, this); + } + + // Start camera manager (it has its own reconnect thread) + m_camera->start(); + + fmt::print("[I][Managr]: Unified device manager started\n"); +} + +void UnifiedDeviceManager::stop() { + fmt::print("[I][Managr]: Stopping unified device manager\n"); + + m_shouldStop.store(true); + + // Stop camera manager + m_camera->stop(); + + // Stop serial thread + if (m_serialReconnectThread.joinable()) { + m_serialReconnectThread.join(); + + std::lock_guard lock(m_serialMutex); + m_serial->closePort(); + m_serialConnected.store(false); + } + + fmt::print("[I][Managr]: Unified device manager stopped\n"); +} + +bool UnifiedDeviceManager::isCameraConnected() const { + return m_camera->isConnected(); +} + +bool UnifiedDeviceManager::sendData(const char* data, size_t length) { + if (!m_serialConnected.load()) { + return false; + } + + std::lock_guard lock(m_serialMutex); + + if (!m_serial->sendData(data, length)) { + m_serialConnected.store(false); + fmt::print("[W][Managr]: Serial send failed, marked as disconnected\n"); + return false; + } + + return true; +} + +int UnifiedDeviceManager::receiveData(uint8_t* buffer, size_t maxLength) { + if (!m_serialConnected.load()) { + return -1; + } + + std::lock_guard lock(m_serialMutex); + return m_serial->receiveData(buffer, maxLength); +} + +bool UnifiedDeviceManager::grabFrame(cv::Mat& frame) { + return m_camera->grabFrame(frame); +} + +const char* UnifiedDeviceManager::getCameraName() const { + return m_camera->getCameraName(); +} \ No newline at end of file diff --git a/main.cpp b/main.cpp index 13c9dc3..dc41bbb 100644 --- a/main.cpp +++ b/main.cpp @@ -20,6 +20,7 @@ #include #include #include +#include "serialManager.hpp" #define DO_NOT_CNT_TIME @@ -41,7 +42,8 @@ McuData mcu_data = { // 单片机端回传结构体 WrapperHead *video = nullptr; // 云台摄像头视频源 -Serial serial(115200); // 串口对象 +Serial serial(115200); // 串口对象（旧版，供 armor_finder/energy 使用） +SerialManager serialManager; // TTY 串口自动恢复管理器 uint8_t last_state = ARMOR_STATE; // 上次状态，用于初始化 // 自瞄主程序对象 ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top); @@ -50,6 +52,7 @@ Energy energy(serial, mcu_data.enemy_color); int main(int argc, char *argv[]) { processOptions(argc, argv); // 处理命令行参数 + serialManager.start(); // 启动 TTY 串口自动重连线程 thread receive(uartReceive, &serial); // 开启串口接收线程 int from_camera = 1; // 根据条件选择视频源 diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index 2ff0a4b..a283e44 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -41,7 +41,7 @@ #endif #ifndef YAW_AIM_KD - #define YAW_AIM_KD (0.4) + #define YAW_AIM_KD (0.1) #endif #ifndef YAW_AIM_KP #define YAW_AIM_KP (0.75)