迁移入tty持续连接

armor/include/serialComm.hpp

@@ -0,0 +1,58 @@
+#ifndef SERIAL_COMM_H
+#define SERIAL_COMM_H
+
+#include <vector>
+#include "CSerialPort/SerialPort.h"
+#include "CSerialPort/SerialPortInfo.h"
+#include <fmt/core.h>
+
+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

armor/include/serialManager.hpp

@@ -0,0 +1,126 @@
+#ifndef SERIAL_MANAGER_HPP
+#define SERIAL_MANAGER_HPP
+
+#include "serialComm.hpp"
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <fmt/core.h>
+#include <chrono>
+
+class SerialManager {
+private:
+    SerialComm m_serial;
+    std::thread m_connectionThread;
+    std::atomic<bool> m_isConnected;
+    std::atomic<bool> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<const char*>(data), length);
+    }
+
+    // 接收数据（线程安全）
+    int receiveData(uint8_t* buffer, size_t maxLength) {
+        if (!m_isConnected.load()) {
+            return -1;
+        }
+
+        std::lock_guard<std::mutex> lock(m_serialMutex);
+        return m_serial.receiveData(buffer, maxLength);
+    }
+
+    // 接收数据（字符数组）
+    int receiveData(char* buffer, size_t maxLength) {
+        return receiveData(reinterpret_cast<uint8_t*>(buffer), maxLength);
+    }
+
+    // 获取端口名
+    const char* getPortName() const {
+        return m_serial.getPortName();
+    }
+};
+
+#endif // SERIAL_MANAGER_HPP

armor/include/unifiedManager.hpp

@@ -0,0 +1,49 @@
+#ifndef UNIFIED_MANAGER_HPP
+#define UNIFIED_MANAGER_HPP
+
+#include "serialComm.hpp"
+#include <memory>
+#include <thread>
+#include <atomic>
+#include <mutex>
+
+// Unified device manager (Serial )
+class UnifiedDeviceManager {
+private:
+    // Device instances
+    std::unique_ptr<SerialComm> m_serial;
+    std::unique_ptr<CameraManager> m_camera;
+    
+    // Thread management
+    std::thread m_serialReconnectThread;
+    std::atomic<bool> m_serialConnected;
+    std::atomic<bool> 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

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) {

armor/src/armor_finder/armor_finder.cpp

@@ -54,7 +54,9 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_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();

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;
 

armor/src/serialComm.cpp

@@ -0,0 +1,131 @@
+#include "serialComm.hpp"
+#include <fmt/format.h>
+#include <cstring>
+
+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<SerialPortInfo> 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<const uint8_t*>(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<uint8_t*>(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<SerialPortInfo> portInfos = CSerialPortInfo::availablePortInfos();
+    
+    fmt::print("[I][Serial]: Available ports:\n");
+    for (const auto& info : portInfos) {
+        fmt::print("[I][Serial]:   - {}\n", info.portName);
+    }
+}

armor/src/unifiedManager.cpp

@@ -0,0 +1,112 @@
+#include "unifiedManager.hpp"
+
+UnifiedDeviceManager::UnifiedDeviceManager(int retryIntervalMs)
+    : m_serial(std::make_unique<SerialComm>())
+    , m_camera(std::make_unique<CameraManager>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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();
+}

main.cpp

@@ -20,6 +20,7 @@
 #include <options.h>
 #include <additions.h>
 #include <config/setconfig.h>
+#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;                    // 根据条件选择视频源

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)