modularization/src/driver/driver_radar_continental_ARS408_SRR308/main.cpp

#include <QCoreApplication>

#include <iostream>
#include <QDateTime>

#include "modulecomm.h"
#include "xmlparam.h"
#include "ivversion.h"
#include "ivbacktrace.h"

#include "canmsg.pb.h"
#include "radarobjectarray.pb.h"
#include "ivfault.h"
#include "ivlog.h"

iv::radar::radarobjectarray gobj;
int gntemp = 0;

void * gpa;
void * gpb;

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

static int gnNotSend = 10;

QTime gTime;

static bool gbinit = false;



iv::radar::radarobjectarray mradararray;

void ShareResult()
{
    char * str = new char[mradararray.ByteSize()];
    int nsize = mradararray.ByteSize();
    if(mradararray.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(gpa,str,nsize);
    }

    givlog->verbose("obj size is %d ",mradararray.obj_size());

//    qDebug("share time is %d ",gTime.elapsed());
    delete str;
}

void ProcRadarMsg(iv::can::canraw xmsg)
{
    if(gbinit == false)gnNotSend = 10;

    if(gnNotSend > 0)
    {
        givlog->verbose("send valid data.");
        iv::can::canraw x;
        x.set_id(0x200);
        x.set_bext(false);
        x.set_bremote(false);
        x.set_len(8);
        unsigned char strdata[8];
        strdata[0] = 0x08;
        strdata[4] = 0x08;
        x.set_data(strdata,8);
        iv::can::canmsg xsend;
        iv::can::canraw * praw = xsend.add_rawmsg();
        praw->CopyFrom(x);
        int nsize = xsend.ByteSize();
        char * str = new char[nsize];
        if(xsend.SerializeToArray(str,nsize))
        {
            iv::modulecomm::ModuleSendMsg(gpb,str,nsize);
        }
        else
        {

        }
        delete str;
        gnNotSend--;
    }
    int i;
    if(xmsg.id() == 0x60A)
    {
        gbinit = true;
        mradararray.set_mstime(QDateTime::currentMSecsSinceEpoch());
        ShareResult();
        mradararray.clear_obj();
    }
    if(xmsg.id() == 0x60B)
    {
        if(xmsg.len()<8)
        {
            std::cout<<"ProcRadarMsg can message length is small "<<xmsg.len()<<std::endl;
            return;
        }
        char data[8];
        memcpy(data,xmsg.data().data(),8);
        unsigned char id;
        id = data[0];
        float y;
        float x;
        float vx,vy;

        unsigned char a,b;
        a = data[1];
        b = data[2];
        b = b&0xf8;
        y = a;
        y = y*256 + b;
        y = y/8;
        y = y*0.2 - 500;
        a = data[2];b = data[3];
        a = a&0x07;
        x = a;
        x = x*256 + b;
        x = x*0.2 -204.6;
        a = data[4];b = data[5];
        b = b&0xc0;b = b/64;
        vy = a; vy = vy*4 + b;
        vy = vy *0.25 - 128;
        a = data[5];b = data[6];
        a = a&0x3f;b = b&0xe0;b = b/32;
        vx = a; vx = vx*8 + b;
        vx = vx *0.25 - 64;

        iv::radar::radarobject xobj;
        xobj.set_bvalid(true);
        xobj.set_x(x*(-1));
        xobj.set_y(y);
        xobj.set_vx(vx*(-1));
        xobj.set_vy(vy);
        xobj.set_vel(vy);

        iv::radar::radarobject * pxobj = mradararray.add_obj();
        pxobj->CopyFrom(xobj);
    }

}

void DecodeRadar(iv::can::canmsg xmsgvetor)
{

    int i;

    for(i=0;i<xmsgvetor.rawmsg_size();i++)
    {
        ProcRadarMsg(xmsgvetor.rawmsg(i));
    }

}

void Listencanmsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    if(nSize<1)return;
    iv::can::canmsg xmsg;
    if(false == xmsg.ParseFromArray(strdata,nSize))
    {
        givlog->error("radar Listencanmsg fail");
        gfault->SetFaultState(1, 0, "radar Listencanmsg error");
        std::cout<<"radar Listencanmsg fail."<<std::endl;
        return;
    }

    DecodeRadar(xmsg);

//    qDebug("can size is %d",xmsg.rawmsg_size());
//    xt = QDateTime::currentMSecsSinceEpoch();
//    qDebug("latence = %ld ",xt-pic.time());

}


int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("driver_radar_conti_ARS408_SRR308");
    QCoreApplication a(argc, argv);

    gfault = new iv::Ivfault("radar_conti_ARS408_SRR308");
    givlog = new iv::Ivlog("radar_conti_ARS408_SRR308");

    QString strpath = QCoreApplication::applicationDirPath();
    if(argc < 2)
        strpath = strpath + "/driver_radar_conti_ARS408_SRR308.xml";
    else
        strpath = argv[1];
    givlog->verbose("%s", strpath.data());
    std::cout<<strpath.toStdString()<<std::endl;

    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    std::string strmemcan = xp.GetParam("canrecv","canrecv0");
    std::string strmemsend = xp.GetParam("cansend","cansend0");
    std::string strmemradar = xp.GetParam("radar","radar0");

    gTime.start();
    gpa = iv::modulecomm::RegisterSend(strmemradar.data(),100000,3);
    gpb = iv::modulecomm::RegisterSend(strmemsend.data(),100000,3);
    void * pa = iv::modulecomm::RegisterRecv(strmemcan.data(),Listencanmsg);



    return a.exec();
}