|
|
@@ -0,0 +1,206 @@
|
|
|
+#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();
|
|
|
+}
|
HAPO-9#