sweeper_video/src/serial_port.cpp

#include "serial_port.h"

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

#include <chrono>
#include <cstring>
#include <thread>

// --------- 波特率映射 ---------
static speed_t baud_to_speed(int baud)
{
    switch (baud)
    {
        case 9600:
            return B9600;
        case 19200:
            return B19200;
        case 38400:
            return B38400;
        case 57600:
            return B57600;
        case 115200:
            return B115200;
#ifdef B230400
        case 230400:
            return B230400;
#endif
        default:
            return B115200;
    }
}

SerialPort::SerialPort(const std::string& id, const std::string& device, int baudrate, int retry_interval)
    : id_(id), device_(device), baudrate_(baudrate), retry_interval_(retry_interval)
{
}

SerialPort::~SerialPort() { stop(); }

void SerialPort::start()
{
    stop_flag_ = false;
    reconnect_thread_ = std::thread(&SerialPort::reconnect_loop, this);
}

void SerialPort::stop()
{
    stop_flag_ = true;
    running_ = false;

    // 先关闭 fd，打断 reader 的阻塞 read()
    close_port();

    if (reader_thread_.joinable()) reader_thread_.join();
    if (reconnect_thread_.joinable()) reconnect_thread_.join();
}

bool SerialPort::open_port()
{
    fd_ = open(device_.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
    if (fd_ < 0)
    {
        LOG_ERROR("[" + id_ + "] Failed to open " + device_ + ": " + strerror(errno));
        return false;
    }

    if (!configure_port(fd_))
    {
        LOG_ERROR("[" + id_ + "] Failed to configure " + device_);
        close(fd_);
        fd_ = -1;
        return false;
    }

    running_ = true;
    reader_thread_ = std::thread(&SerialPort::reader_loop, this);

    LOG_INFO("[" + id_ + "] Opened serial port " + device_ + " at " + std::to_string(baudrate_) + " baud");
    return true;
}

void SerialPort::close_port()
{
    running_ = false;

    if (fd_ >= 0)
    {
        close(fd_);
        fd_ = -1;
    }
}

bool SerialPort::is_open() const { return fd_ >= 0; }

bool SerialPort::send_data(const std::vector<uint8_t>& data)
{
    if (fd_ < 0) return false;

    std::lock_guard<std::mutex> lock(send_mutex_);
    ssize_t n = write(fd_, data.data(), data.size());
    return n == static_cast<ssize_t>(data.size());
}

bool SerialPort::send_data(const std::string& data)
{
    return send_data(std::vector<uint8_t>(data.begin(), data.end()));
}

void SerialPort::set_receive_callback(ReceiveCallback cb) { receive_callback_ = std::move(cb); }

void SerialPort::set_receive_callback(ReceiveStringCallback cb)
{
    receive_callback_ = [cb](const std::vector<uint8_t>& data) { cb(std::string(data.begin(), data.end())); };
}

void SerialPort::reader_loop()
{
    std::vector<uint8_t> buffer(1024);

    while (running_ && !stop_flag_)
    {
        int n = read(fd_, buffer.data(), buffer.size());

        if (n > 0)
        {
            if (receive_callback_) receive_callback_({buffer.begin(), buffer.begin() + n});
        }
        else if (n < 0)
        {
            LOG_ERROR("[" + id_ + "] Read error: " + std::string(strerror(errno)));
            break;
        }
        // n == 0：超时，正常情况，继续读
    }

    running_ = false;
}

static void interruptible_sleep(std::atomic<bool>& stop_flag, int seconds)
{
    using namespace std::chrono;
    for (int i = 0; i < seconds * 10 && !stop_flag.load(std::memory_order_relaxed); ++i)
        std::this_thread::sleep_for(100ms);
}

void SerialPort::reconnect_loop()
{
    int current_interval = retry_interval_;
    const int max_interval = 300;

    while (!stop_flag_)
    {
        if (open_port())
        {
            // 等待读线程结束
            if (reader_thread_.joinable()) reader_thread_.join();

            close_port();
            LOG_WARN("[" + id_ + "] Port closed, will retry");

            current_interval = retry_interval_;
        }
        else
        {
            LOG_INFO("[" + id_ + "] Connect failed, retry in " + std::to_string(current_interval) + "s");

            interruptible_sleep(stop_flag_, current_interval);
            current_interval = std::min(current_interval * 2, max_interval);
        }
    }
}

bool SerialPort::configure_port(int fd)
{
    struct termios tty{};
    if (tcgetattr(fd, &tty) != 0) return false;

    speed_t spd = baud_to_speed(baudrate_);
    cfsetospeed(&tty, spd);
    cfsetispeed(&tty, spd);

    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
    tty.c_iflag &= ~IGNBRK;
    tty.c_lflag = 0;
    tty.c_oflag = 0;
    tty.c_cc[VMIN] = 1;
    tty.c_cc[VTIME] = 1;

    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_cflag |= (CLOCAL | CREAD);
    tty.c_cflag &= ~(PARENB | PARODD);
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CRTSCTS;

    return tcsetattr(fd, TCSANOW, &tty) == 0;
}