Queue/ioctrl/MsgHandler.cxx

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <thread>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>      // for O_WRONLY
#include <chrono>
#include "MsgHandler.hpp"

MsgHandler::MsgHandler() : fd(-1) {}


static int gpio_export(int n)
{
    int fd = open("/sys/class/gpio/export", O_WRONLY);
    if (fd < 0) return -1;
    char buf[16]; int len = snprintf(buf, sizeof(buf), "%d", n);
    write(fd, buf, len);
    close(fd);
    usleep(100000);
    return 0;
}

static int gpio_set_dir(int n, const char* dir)
{
    char path[64]; snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/direction", n);
    int fd = open(path, O_WRONLY);
    if (fd < 0) return -1;
    write(fd, dir, strlen(dir));
    close(fd);
    return 0;
}

static int gpio_write(int n, int val)
{
    char path[64]; snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", n);
    int fd = open(path, O_WRONLY);
    if (fd < 0) return -1;
    char b = val ? '1' : '0';
    write(fd, &b, 1);
    close(fd);
    return 0;
}


bool MsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
{
    std::cout << "[debug] OpenPort device=" << this->device
              << " omap.size=" << this->iomap.omap.size() << std::endl;

    if (this->device == "gpio")
    {
        // ★ 前拦车器两个 GPIO：先显式 export + direction + 写 0（不依赖 map 顺序）
        gpio_export(48);
        gpio_set_dir(48, "out");
        gpio_write(48, 0);

        gpio_export(49);
        gpio_set_dir(49, "out");
        gpio_write(49, 0);

        return true;
    }
    return true;
}

// 发送数据
int MsgHandler::SendMsg(const void *buf, size_t len)
{
    if (fd == -1) {
        perror("设备未打开");
        return -1;
    }
    
    int bytesWritten = write(fd, buf, len);
    if (bytesWritten < 0) {
        perror("发送数据失败");
    }

    return bytesWritten;
}

int MsgHandler::RecvMsg(void *buf, size_t len, int timeoutMs)
{
    if (fd == -1) {
        perror("设备未打开");
        return -1;
    }
    
    // 使用select实现超时
    fd_set readfds;
    struct timeval tv;
    
    FD_ZERO(&readfds);
    FD_SET(fd, &readfds);
    
    tv.tv_sec = timeoutMs / 1000;
    tv.tv_usec = (timeoutMs % 1000) * 1000;
    
    int ret = select(fd + 1, &readfds, NULL, NULL, &tv);
    if (ret == -1) {
        perror("select错误");
        return -1;
    } else if (ret == 0) {
        return 0;
    }
    
    // 有数据可读
    int bytes;
    if (len == 0)
    {
        if (ioctl(fd, FIONREAD, &bytes) < 0)
        {
            perror("ioctl FIONREAD失败");
            return -1;
        }
    }
    else
    {
        bytes = len;
    }

    int bytesRead = read(fd, buf, bytes);
    if (bytesRead < 0) {
        perror("读取数据失败");
        return -1;
    }
    
    return bytesRead;
}

void MsgHandler::ClosePort()
{
    if (fd != -1)
    {
        close(fd);
        fd = -1;
    }
}

int MsgHandler::SendDeviceMsg(std::vector<uint8_t>& data)
{
    for (int num : data) {
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << num << " ";
    }
    std::cout << std::endl;

    return SendMsg(data.data(), data.size());
}

int MsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
{
    return RecvMsg(data.data(), data.size(), timeoutMs);
}

void MsgHandler::HandleBarCommand(const WeighingSystem::BarCommandUpdate& cmd)
{
    if (this->device != "gpio") return;

    // 前拦车器升杆
    if (cmd.FrontBarEnable() && cmd.FrontBarSignalUp())
    {
        auto up = this->iomap.omap.find("frontbarrup");
        if (up != this->iomap.omap.end())
        {
            // ① 抬杆前：先 init 48 + 49 全部拉低
            std::cout << "[GPIO] >>> init 48+49 before raise" << std::endl;
            this->InitFrontBarGpioPair();

            // ② 抬杆：48 置 1
            int n = up->second.first;
            std::cout << "[GPIO] " << n << " <- 1 (raise start)" << std::endl;
            int rc = gpio_write(n, 1);
            std::cout << "[GPIO] " << n << " <- 1 rc=" << rc << std::endl;
            up->second.second = 1;

            // ③ 保持 10 秒
            std::cout << "[GPIO] hold raise 10s..." << std::endl;
            std::this_thread::sleep_for(std::chrono::seconds(10));

            // ④ 抬杆后：再 init 一次，强制两脚回 0
            std::cout << "[GPIO] >>> init 48+49 after 10s (reset to 0)" << std::endl;
            this->InitFrontBarGpioPair();
            up->second.second = 0;
            std::cout << "[GPIO] reset done" << std::endl;
        }
    }
}


void MsgHandler::InitFrontBarGpioPair()
{
    // 强制重新 init frontbarrup (48) 和 frontbardown (49)
    auto up = iomap.omap.find("frontbarrup");
    auto dn = iomap.omap.find("frontbardown");

    if (up != iomap.omap.end())
    {
        int n = up->second.first;     // 48
        gpio_export(n);
        gpio_set_dir(n, "out");
        gpio_write(n, 0);
        std::cout << "[GPIO] " << n << " init <- 0" << std::endl;
    }

    if (dn != iomap.omap.end())
    {
        int n = dn->second.first;     // 49
        gpio_export(n);
        gpio_set_dir(n, "out");
        gpio_write(n, 0);
        std::cout << "[GPIO] " << n << " init <- 0" << std::endl;
    }
}