Weight/ioctrl/MsgHandler.cxx

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

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

void MsgHandler::HandleDdsMsg(const std::map<std::string, std::string>& msg)
{
    auto cmd = msg.find("cmd");
    
    if (cmd != msg.end())
    {
        if (cmd->second == "config")
        {
            auto port = msg.find("port");
            auto baudrate = msg.find("baudrate");
            auto device = msg.find("device");
            if ((port != msg.end()) && (baudrate != msg.end()) && (device != msg.end()))
            {
                OpenPort(port->second, baudrate->second);
                this->device = device->second;
            }
        }
        else 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);
            }
        }
    }
}

int MsgHandler::ParseDeviceMsg(std::vector<uint8_t>& data, InternalSystem::IoCtrlRsp& rsp)
{
    if (data[0] == this->device_id)
    {
        switch(data[1])
        {
            case 0x50:
                rsp.msg()["open"] = std::to_string(data[3]);
                break;
            case 0x51:
                rsp.msg()["write"] = std::to_string(data[3]);
                break;
            case 0x52:
                rsp.msg()["read"] = "0";
                for (auto &p : this->iomap.imap)
                {
                    // if (p.second.second != data[p.second.first+2])
                    // {
                        p.second.second = data[p.second.first+2];
                        rsp.msg()[p.first] = std::to_string(p.second.second);
                    // }
                }
                break;
        }
        return 1;
    }
    return 0;
}

int MsgHandler::HandleDeviceMsg(InternalSystem::IoCtrlRsp& rsp)
{
    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, rsp));
    }
    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);
}