modularization/src/driver/driver_can_nvidia_agx/canctrl.cpp

#include "canctrl.h"

canctrl * gc;
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 * strmemsend1,const char * strmemrecv0,const char * strmemrecv1)
{

    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();
    mspcan.reset(mpcan);
    connect(mpcan,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onCANState(bool,int,const char*)));
    mpcan->startdev();
}

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);
            }

            if((nRec2 =mpcan->GetMessage(1,xmsg,2500))>0)
            {
                sharecanmsg(mparecv1,xmsg,nRec2,1);
            }

            nSend1 = 0;
            nSend2 = 0;

            mMutexcan1.lock();
            for(i=0;i<msendmsgvector1.size();i++)
            {
                mpcan->SetMessage(0,&(msendmsgvector1.at(i)));
            }
            msendmsgvector1.clear();
            mMutexcan1.unlock();

            mMutexcan2.lock();
            for(i=0;i<msendmsgvector2.size();i++)
            {
                mpcan->SetMessage(1,&(msendmsgvector2.at(i)));
            }
            msendmsgvector2.clear();
            mMutexcan2.unlock();

            proCanSt.set_b_canstate(true);
            int nsize = proCanSt.ByteSize();
            char * strdata = new char[proCanSt.ByteSize()];
            if(proCanSt.SerializePartialToArray(strdata,nsize))
            {
                iv::modulecomm::ModuleSendMsg(mpcanState,strdata,nsize);
            }
            msleep(5);
        }
        else
        {
            proCanSt.set_b_canstate(false);
            int nsize = proCanSt.ByteSize();
            char * strdata = new char[proCanSt.ByteSize()];
            if(proCanSt.SerializePartialToArray(strdata,nsize))
            {
                iv::modulecomm::ModuleSendMsg(mpcanState,strdata,nsize);
            }
            msleep(1);
            mpcan->mivlog->error("%s open can card fail",__func__);
        }
        mpcan->mfault->SetFaultState(0, 0, "ok");
    }

}

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)
{
    if((index <0)||(index > 1))
    {
        mpcan->mfault->SetFaultState(1, 0, "sendmsg index error");
        mpcan->mivlog->error("sendmsg index err");
        std::cout<<"canctrl::sendmsg index error"<<std::endl;
        return;
    }

    std::vector<basecan_msg> * psendmsgvector;
    QMutex * pMutex;
    if(index == 0)
    {
        pMutex = &mMutexcan1;
        psendmsgvector = &msendmsgvector1;
    }
    if(index == 1)
    {
        pMutex = &mMutexcan2;
        psendmsgvector = &msendmsgvector2;
    }

    if(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();
        int nlen = x.len();

        if((nlen < 0) || (nlen > 8))
        {
            nlen = 0;
            mpcan->mivlog->error("sendmsg nlen err");
            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();


}

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->error("canctrl::sharecanmsg serialize error");
        mpcan->mfault->SetFaultState(1, 0, "sharecanmsg serialize error");
    }
}