Queue/gatectrl/MsgHandler.cxx

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <thread>
#include <unistd.h>
#include <sys/ioctl.h>
#include "MsgHandler.hpp"
#include "DEBUG.hpp"

MsgHandler::MsgHandler() : fd(-1)
{
    this->iomap.omap["FrontBarUp"] = {2, 0};
    this->iomap.omap["FrontBarDown"] = {1, 0};
    this->iomap.omap["BackBarUp"] = {3, 0};
    this->iomap.omap["BackBarDown"] = {4, 0};
    this->iomap.omap["FrontLED"] = {5, 0};
    this->iomap.omap["BackLED"] = {6, 0};

    this->iomap.imap["FrontResistance"] = {5, 1};
    this->iomap.imap["BackResistance"] = {6, 1};

    this->new_bar_state.FrontBarState = DOWN;
    this->new_bar_state.BackBarState = DOWN;
    this->new_bar_state.FrontLEDState = RED;
    this->new_bar_state.BackLEDState = RED;
    this->new_bar_state.FrontResistanceSignal = UNBLOCK;
    this->new_bar_state.BackResistanceSignal = UNBLOCK;

    this->old_bar_state = this->new_bar_state;
    this->old_bar_state.FrontResistanceSignal = BLOCK;
    this->old_bar_state.BackResistanceSignal = BLOCK;
}

void MsgHandler::HandleDdsMsg(const std::map<std::string, std::string>& msg)
{
    auto cmd = msg.find("cmd");
    
    if (cmd != msg.end())
    {
        if (cmd->second == "open")
        {
            if (this->device == "5serial")
            {
                std::vector<uint8_t> data;
                data.insert(data.end(), {this->device_id, 0x50, 1, 1});
                SendDeviceMsg(data);
            }
        }
        else if (cmd->second == "write")
        {
            if (this->device == "5serial")
            {
                std::vector<uint8_t> data;
                data.resize(8, 1);
                data.insert(data.begin(), {this->device_id, 0x51, 1});
                for (const auto &port : this->iomap.omap)
                {
                    data[port.second.first+2] = port.second.second;
                }

                for (auto &m : msg)
                {
                    auto port = this->iomap.omap.find(m.first);
                    if (port != this->iomap.omap.end())
                    {
                        if (m.second == "1")
                        {
                            port->second.second = 1;
                        }
                        else if (m.second == "0")
                        {
                            port->second.second = 0;
                        }
                        data[port->second.first+2] =  port->second.second;
                    }
                }
                SendDeviceMsg(data);
            }
        }
        else if (cmd->second == "pluse")
        {
            if (this->device == "5serial")
            {
                std::vector<uint8_t> data;
                data.resize(8, 1);
                data.insert(data.begin(), {this->device_id, 0x51, 1});
                for (const auto &port : this->iomap.omap)
                {
                    data[port.second.first+2] = port.second.second;
                }

                for (auto &m : msg)
                {
                    auto port = this->iomap.omap.find(m.first);
                    if (port != this->iomap.omap.end())
                    {
                        if (m.second == "1")
                        {
                            port->second.second = 1;
                        }
                        else if (m.second == "0")
                        {
                            port->second.second = 0;
                        }
                        data[port->second.first+2] =  port->second.second;
                    }
                }
                SendDeviceMsg(data);

                std::this_thread::sleep_for(std::chrono::seconds(1));

                for (auto &m : msg)
                {
                    auto port = this->iomap.omap.find(m.first);
                    if (port != this->iomap.omap.end())
                    {
                        if (m.second == "1")
                        {
                            port->second.second = 0;
                        }
                        else if (m.second == "0")
                        {
                            port->second.second = 1;
                        }
                        data[port->second.first+2] =  port->second.second;
                    }
                }
                SendDeviceMsg(data);
            }
        }
        else if (cmd->second == "read")
        {
            if (this->device == "5serial")
            {
                std::vector<uint8_t> data;
                data.insert(data.end(), {this->device_id, 0x52, 1});
                data.insert(data.end(), 1);
                SendDeviceMsg(data);
            }
        }
    }
}

bool MsgHandler::CtrlBar(const std::vector<std::string>& port)
{
    std::vector<uint8_t> data;
    data.resize(8, 0);
    data.insert(data.begin(), {this->device_id, 0x51, 1});
    for (const auto &port : this->iomap.omap)
    {
        data[port.second.first + 2] = port.second.second;
    }

    for (const std::string& p : port)
    {
        auto port = this->iomap.omap.find(p);
        if (port != this->iomap.omap.end())
        {
            port->second.second = 1;
            data[port->second.first + 2] = port->second.second;
        }
    }
    SendDeviceMsg(data);

    std::this_thread::sleep_for(std::chrono::seconds(2));

    for (const std::string& p : port)
    {
        auto port = this->iomap.omap.find(p);
        if (port != this->iomap.omap.end())
        {
            port->second.second = 0;
            data[port->second.first + 2] = port->second.second;
        }
    }
    SendDeviceMsg(data);
    std::this_thread::sleep_for(std::chrono::seconds(1));

    return true;
}

bool MsgHandler::CtrlLight(const std::map<std::string, uint8_t>& port)
{
    std::vector<uint8_t> data;
    data.resize(8, 0);
    data.insert(data.begin(), {this->device_id, 0x51, 1});
    for (const auto &port : this->iomap.omap)
    {
        data[port.second.first + 2] = port.second.second;
    }

    for (auto &m : port)
    {
        auto port = this->iomap.omap.find(m.first);
        if (port != this->iomap.omap.end())
        {
            if (m.second == 1)
            {
                port->second.second = 1;
            }
            else if (m.second == 2)
            {
                port->second.second = 0;
            }
            data[port->second.first + 2] = port->second.second;
        }
    }
    SendDeviceMsg(data);
    std::this_thread::sleep_for(std::chrono::seconds(1));

    return true;
}

int MsgHandler::ParseDeviceMsg(std::vector<uint8_t>& data)
{
    int ret = 0;
    if (data[0] == this->device_id)
    {
        switch(data[1])
        {
            case 0x50:
                DEBUG("can gpio open success");
                ret = 0;
                break;
            case 0x51:
                DEBUG("can gpio write success");
                break;
                ret = 0;
            case 0x52:
                DEBUG("gpio change");
                for (auto &p : this->iomap.imap)
                {
                    p.second.second = data[p.second.first+2];
                    if (p.first == "FrontResistance")
                    {
                        if (p.second.second == 0)
                        {
                            this->new_bar_state.FrontResistanceSignal = 0;
                        }
                        else
                        {
                            this->new_bar_state.FrontResistanceSignal = 1;
                        }
                    }
                    else if (p.first == "BackResistance")
                    {
                        if (p.second.second == 0)
                        {
                            this->new_bar_state.BackResistanceSignal = 0;
                        }
                        else
                        {
                            this->new_bar_state.BackResistanceSignal = 1;
                        }
                    }
                }
                ret = 1;
                break;
        }
    }
    return ret;
}

int MsgHandler::HandleDeviceMsg()
{
    std::vector<uint8_t> m_RecvData;

    if(RecvDeviceMsg(m_RecvData, 100) > 0)
    {
        for (int num : m_RecvData)
        {
            std::cout << std::hex << std::setw(2) << std::setfill('0')
                      << num << " ";
        }
        std::cout << std::endl;
        return(ParseDeviceMsg(m_RecvData));
    }
    return 0;
}

bool MsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
{
    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);
}