modularization/src/driver/driver_can_nvidia_agx_new/canctrl.cpp

#include "canctrl.h"

#include <memory>

canctrl * gc;
static iv::canstate::canstate proCanSt;
void Listencansend0(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::can::canmsg msg;

    if(false == msg.ParseFromArray(strdata,nSize))
    {
        std::cout<<"Listencansend Parse fail."<<std::endl;
        return;
    }

    gc->sendmsg(0,msg);

}

void Listencansend1(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::can::canmsg msg;

    if(false == msg.ParseFromArray(strdata,nSize))
    {
        std::cout<<"Listencansend Parse fail."<<std::endl;
        return;
    }

    gc->sendmsg(1,msg);

}




canctrl::canctrl(const char * strmemsend0,const char * strmemrecv0,const char * strmemsend1,const char * strmemrecv1,const char * strcan0name,const char * strcan1name)
{

    gc = this;

    mparecv0 = iv::modulecomm::RegisterSend(strmemrecv0,100000,3);
    mparecv1 = iv::modulecomm::RegisterSend(strmemrecv1,100000,3);
    mpcanState = iv::modulecomm::RegisterSend("canstate",18,3);

    mpasend0 = iv::modulecomm::RegisterRecv(strmemsend0,Listencansend0);
    mpasend1 = iv::modulecomm::RegisterRecv(strmemsend1,Listencansend1);

    mpcan = new nvcan(strcan0name,strcan1name);
    mspcan.reset(mpcan);
    connect(mpcan,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onCANState(bool,int,const char*)));
    mpcan->startdev();

    mptheadstate = new std::thread(&canctrl::threadstate,this);
}

canctrl::~canctrl()
{
    mpcan->stopdev();

    delete mpcan;

    mbstaterun = false;
    mptheadstate->join();

    iv::modulecomm::Unregister(mpasend0);
    iv::modulecomm::Unregister(mpcanState);
    iv::modulecomm::Unregister(mparecv0);

}

void canctrl::threadstate()
{
    int ncount = 0;
    while(mbstaterun)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        ncount++;
        if(ncount>=10)
        {
            ncount = 0;
            proCanSt.set_b_canstate(true);
            int nsize = proCanSt.ByteSize();
            std::shared_ptr<char > strdata_ptr= std::shared_ptr<char>(new char[nsize]);
            if(proCanSt.SerializeToArray(strdata_ptr.get(),nsize))
            {
                iv::modulecomm::ModuleSendMsg(mpcanState,strdata_ptr.get(),nsize);
            }
        }
    }

    std::cout<<"threadstate exit."<<std::endl;
}

void canctrl::run()
{

    QTime xTime;
    xTime.start();
    int nOldTime = xTime.elapsed();
    int i;

    while(!isInterruptionRequested())
    {
        if(mbCANOpen)
        {
            basecan_msg xmsg[2500];
            int nRec1,nRec2,nSend1,nSend2;
            if((nRec1 =mpcan->GetMessage(0,xmsg,2500))>0)
            {
                sharecanmsg(mparecv0,xmsg,nRec1,0);
            }



            nSend1 = 0;
            nSend2 = 0;

            msleep(5);
        }
        else
        {

            msleep(1);
            mpcan->mivlog->error("%s open can card fail",__func__);

            if(xTime.elapsed()>1000)
            {
                qDebug("Not Open CANCARD exceed 1 second. so exit program.");
                exit(-1);
            }
        }
//        mpcan->mfault->SetFaultState(0, 0, "ok");
    }

    qDebug("thread canctrl complete.");

}

void canctrl::onCANState(bool bCAN, int nR, const char *strres)
{
    mbCANOpen = bCAN;
    mpcan->mivlog->info("can","canstate is %s ",strres);
}

void canctrl::sendmsg(int index, iv::can::canmsg xmsg)
{

    std::vector<basecan_msg> * psendmsgvector;
    QMutex * pMutex;

    if(index == 0)
    {
        pMutex = &mMutexcan1;
        psendmsgvector = &msendmsgvector1;
    }
    else
    {
        if(index == 1)
        {
            pMutex = &mMutexcan2;
            psendmsgvector = &msendmsgvector2;
        }
        else
        {
            std::cout<<" canctrl::sendmsg "<<" index error. index: "<<index<<std::endl;
            return;
        }
    }


    if((int)psendmsgvector->size() > SENDMSGBUFSIZE)
    {
        mpcan->mivlog->warn("sendmsg buf full");
        return;
    }


    pMutex->lock();
    if(psendmsgvector->size() > 1000)psendmsgvector->clear();
    int i;
    for(i=0;i<xmsg.rawmsg_size();i++)
    {
        basecan_msg sendmsg;
        iv::can::canraw x;
        x.CopyFrom(xmsg.rawmsg(i));
        sendmsg.id = x.id();
        sendmsg.isExtern = x.bext();
        sendmsg.isRemote = x.bremote();
        sendmsg.mSetTime = QDateTime::currentMSecsSinceEpoch();
        int nlen = x.len();

        if((nlen < 0) || (nlen > 8))
        {
            nlen = 0;
            mpcan->mivlog->warn("sendmsg nlen err");
            continue;
 //           mpcan->mfault->SetFaultState(1, 0, "sendmsg nlen err");
        }
        sendmsg.nLen = nlen;
        if(sendmsg.nLen > 0)
        {
            memcpy(sendmsg.data,x.data().data(),sendmsg.nLen);
        }

        psendmsgvector->push_back(sendmsg);
    }
    pMutex->unlock();

    if(mbCANOpen)
    {
        pMutex->lock();
        for(i=0;i<psendmsgvector->size();i++)
        {
            mpcan->SetMessage(index,&(psendmsgvector->at(i)));
        }
        psendmsgvector->clear();
        pMutex->unlock();
        mpcan->CmdSend();
    }


}

void canctrl::sharecanmsg(void *xpa, basecan_msg * pxmsg,int ncount,int nch)
{
    iv::can::canmsg xmsg;

    int i;
    for(i=0;i<ncount;i++)
    {
        iv::can::canraw * praw = xmsg.add_rawmsg();
        praw->set_id(pxmsg[i].id);
        praw->set_data(pxmsg[i].data,8);
        praw->set_bext(pxmsg[i].isExtern);
        praw->set_bremote(pxmsg[i].isRemote);
        praw->set_rectime(QDateTime::currentMSecsSinceEpoch());
        praw->set_len(pxmsg[i].nLen);

    }
    xmsg.set_channel(nch);

    xmsg.set_index(mindex[nch]);
    mindex[nch]++;

    int nsize = xmsg.ByteSize();
    char * strdata = new char[xmsg.ByteSize()];
    if(xmsg.SerializePartialToArray(strdata,nsize))
    {
        iv::modulecomm::ModuleSendMsg(xpa,strdata,nsize);
    }
    else
    {
        mpcan->mivlog->warn("canctrl::sharecanmsg serialize error");
//        mpcan->mfault->SetFaultState(1, 0, "sharecanmsg serialize error");
    }
    delete strdata;
}