Vehicle-cpp/remote_drive/EgoZJ/EgoSystem/udp_can.cpp

#include "udp_can.h"
#include <thread>
#include "message_queue.h"

#define	GET_BIT(x, bit)	((x & (1 << bit)) >> bit)


int16_t ChangeINT(unsigned char h,unsigned char l)
{
  int16_t hi=(int16_t)(h&0x00FF);
  int16_t low= (int16_t)(l&0x00FF);

   return (hi<<8)|low;
}

CUdpCanSensor::CUdpCanSensor(CMessageQueue* q):_message(q)
{
    _run = false;
}

void CUdpCanSensor::Notify(int8_t * buffer,int32_t size)
{
    if(_run)
    {
        if(!_message->_GSML)
        {
            int16_t m_cmd = ChangeINT(buffer[1],buffer[0]);

            if(m_cmd == RemoNet::CC_CANMSG)
            {
                    _curTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                    _message->SetTickLive(_curTick);

                    RemoNet::CCCanMsg Req;
                    Req.ParseFromArray(buffer + 4, size - 4);

                    cannet_busframe* frames = (cannet_busframe*)alloca(sizeof(cannet_busframe) * Req.frams_size());

                    for (int32_t i = 0; i < Req.frams_size(); i++)
                    {
                            auto& frame = Req.frams(i);
                            frames[i].canid = frame.canid();
                            frames[i].dlc = frame.dlc();
                            memcpy(frames[i].data, frame.data().data(), frame.dlc());
                            //std::cout << (int)m_cmd << " " << frames[i].canid << " " << (int)frames[i].dlc << std::endl;
                    }
                    _message->_CanBusVehicle->Get()->OnMessage(frames,Req.frams_size());
                    
            }
            else if(m_cmd==RemoNet::CC_Ping)
            {
                    RemoNet::CCPing Req;
                    Req.ParseFromArray(buffer + 4,size - 4);
                    CIOBuffer * pBuffer=CIOBuffer::Alloc();
                    Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
                    message->cmd=MessageType::Ping;
                    message->param_l=Req.tick();

                    RemoNet::CCPing Rep;
                    Rep.set_tick(message->param_l);
                    CIOBuffer Buffer;
                    MessageHead Head;
                    Head.Command = RemoNet::CC_Ping;
                    Head.Length = Rep.ByteSizeLong();
                    Head.Serialize(Buffer.Buffer);
                    auto ptr = Buffer.Buffer + MessageHead::Size();
                    Rep.SerializeToArray(ptr, Head.Length);
                    Buffer.Length = Head.Length + MessageHead::Size();

                    _udpcan->Write(&Buffer);
                    pBuffer->Release();
                        
                }
        }
        else
        {
            uint8_t temp = ':',temp1 = ',',*p = NULL,*p1 = NULL;

            p = (uint8_t *)strchr((const char *)buffer,temp);
            unsigned long m_time = strtol((const char *)p + 1,NULL,10);

            p = (uint8_t *)strchr((const char *)buffer,temp1);
            unsigned long m_startrevord = strtol((const char *)p + 1,NULL,10);

            p1 = (uint8_t *)strchr((const char *)p + 1,temp1);
            unsigned long m_deviceid = strtol((const char *)p1 + 1,NULL,10);
            
            _message->SetGsmlInfo(m_time,m_startrevord,m_deviceid);
        }
       
       
    }
}

void CUdpCanSensor::Start()
{
    if(!_run)
    {
        //_udp_thread = std::thread(&CUdpCanSensor::Run, this);
        _run = true;
    } 
}

void CUdpCanSensor::Run()
{
   
}

void CUdpCanSensor::SetSensorSocket(SensorSocket<CUdpCanSensor>* udpcan)
{
    _udpcan = udpcan;
}

void CUdpCanSensor::Stop()
{
    if (_run)
    {
        _run = false;
        //_udp_thread.join();
    }
}