|
@@ -1,34 +1,80 @@
|
|
|
#include <QCoreApplication>
|
|
|
|
|
|
-#include <iostream>
|
|
|
#include <QDateTime>
|
|
|
+#include <QTimer>
|
|
|
+#include <QThread>
|
|
|
+#include <QSemaphore>
|
|
|
+
|
|
|
+#include <iostream>
|
|
|
+#include <thread>
|
|
|
|
|
|
#include "modulecomm.h"
|
|
|
-#include "xmlparam.h"
|
|
|
#include "ivversion.h"
|
|
|
#include "ivbacktrace.h"
|
|
|
+#include "ivfault.h"
|
|
|
+#include "ivlog.h"
|
|
|
+#include "ivexit.h"
|
|
|
|
|
|
#include "canmsg.pb.h"
|
|
|
#include "radarobjectarray.pb.h"
|
|
|
-#include "ivfault.h"
|
|
|
-#include "ivlog.h"
|
|
|
+#include "chassis.pb.h"
|
|
|
+#include "gpsimu.pb.h"
|
|
|
|
|
|
-iv::radar::radarobjectarray gobj;
|
|
|
-int gntemp = 0;
|
|
|
+#include "decode_cfg.h"
|
|
|
+
|
|
|
+#ifndef IV_MSGUNIT
|
|
|
+#define IV_MSGUNIT
|
|
|
|
|
|
-void * gpa;
|
|
|
-void * gpb;
|
|
|
+namespace iv {
|
|
|
+struct msgunit
|
|
|
+{
|
|
|
+ char mstrmsgname[256];
|
|
|
+ int mnBufferSize = 10000;
|
|
|
+ int mnBufferCount = 1;
|
|
|
+ void * mpa = nullptr;
|
|
|
+ std::shared_ptr<char> mpstrmsgdata;
|
|
|
+ int mndatasize = 0;
|
|
|
+ bool mbRefresh = false;
|
|
|
+ bool mbImportant = false;
|
|
|
+ int mnkeeptime = 100;
|
|
|
+};
|
|
|
+}
|
|
|
+
|
|
|
+#endif
|
|
|
+
|
|
|
+QCoreApplication * gApp;
|
|
|
|
|
|
iv::Ivfault *gfault = nullptr;
|
|
|
iv::Ivlog *givlog = nullptr;
|
|
|
|
|
|
-static int gnNotSend = 10;
|
|
|
+setupCfg setupConfig;
|
|
|
|
|
|
-QTime gTime;
|
|
|
+iv::msgunit shmCANRecv;
|
|
|
+iv::msgunit shmCANSend;
|
|
|
+iv::msgunit shmRadar;
|
|
|
+iv::msgunit shmGPSIMU;
|
|
|
+iv::msgunit shmChassis;
|
|
|
|
|
|
-static bool gbinit = false;
|
|
|
+const uint16_t CANRecvBufferSize = 4096;
|
|
|
+iv::can::canraw CANRecvBuffer[CANRecvBufferSize];
|
|
|
+QSemaphore CANRecvFreeSpace(CANRecvBufferSize);
|
|
|
+QSemaphore CANRecvUsedSpace(0);
|
|
|
+uint64_t CANRecvProducerPtr = 0;
|
|
|
+uint64_t CANRecvConsumerPtr = 0;
|
|
|
+
|
|
|
+const uint16_t CANSendBufferSize = 4096;
|
|
|
+iv::can::canraw CANSendBuffer[CANSendBufferSize];
|
|
|
+QSemaphore CANSendFreeSpace(CANSendBufferSize);
|
|
|
+QSemaphore CANSendUsedSpace(0);
|
|
|
+uint64_t CANSendProducerPtr = 0;
|
|
|
+uint64_t CANSendConsumerPtr = 0;
|
|
|
|
|
|
+iv::radar::radarobjectarray gobj;
|
|
|
+int gntemp = 0;
|
|
|
|
|
|
+static int gnNotSend = 10;
|
|
|
+
|
|
|
+static bool gbinit = false;
|
|
|
|
|
|
iv::radar::radarobjectarray mradararray;
|
|
|
|
|
@@ -38,7 +84,7 @@ void ShareResult()
|
|
|
int nsize = mradararray.ByteSize();
|
|
|
if(mradararray.SerializeToArray(str,nsize))
|
|
|
{
|
|
|
- iv::modulecomm::ModuleSendMsg(gpa,str,nsize);
|
|
|
+ iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,str,nsize);
|
|
|
}
|
|
|
|
|
|
givlog->verbose("obj size is %d ",mradararray.obj_size());
|
|
@@ -70,7 +116,7 @@ void ProcRadarMsg(iv::can::canraw xmsg)
|
|
|
char * str = new char[nsize];
|
|
|
if(xsend.SerializeToArray(str,nsize))
|
|
|
{
|
|
|
- iv::modulecomm::ModuleSendMsg(gpb,str,nsize);
|
|
|
+ iv::modulecomm::ModuleSendMsg(shmRadar.mpa,str,nsize);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
@@ -150,57 +196,143 @@ void DecodeRadar(iv::can::canmsg xmsgvetor)
|
|
|
|
|
|
}
|
|
|
|
|
|
-void Listencanmsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
{
|
|
|
|
|
|
- if(nSize<1)return;
|
|
|
+ if(nSize<1)
|
|
|
+ {
|
|
|
+ std::cout<<"radar ListenCANMsg data empty."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
iv::can::canmsg xmsg;
|
|
|
- if(false == xmsg.ParseFromArray(strdata,nSize))
|
|
|
+ if(!xmsg.ParseFromArray(strdata,nSize))
|
|
|
{
|
|
|
givlog->error("radar Listencanmsg fail");
|
|
|
gfault->SetFaultState(1, 0, "radar Listencanmsg error");
|
|
|
- std::cout<<"radar Listencanmsg fail."<<std::endl;
|
|
|
+ std::cout<<"radar ListenCANMsg parse fail."<<std::endl;
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
- DecodeRadar(xmsg);
|
|
|
+ CANRecvFreeSpace.acquire(xmsg.rawmsg_size());
|
|
|
+ for(int i=0;i<xmsg.rawmsg_size();i++)
|
|
|
+ {
|
|
|
+ CANRecvBuffer[CANRecvProducerPtr++ % CANRecvBufferSize] = xmsg.rawmsg(i);
|
|
|
+ }
|
|
|
+ CANRecvUsedSpace.release(xmsg.rawmsg_size());
|
|
|
+
|
|
|
+// DecodeRadar(xmsg);
|
|
|
+
|
|
|
+}
|
|
|
|
|
|
-// qDebug("can size is %d",xmsg.rawmsg_size());
|
|
|
-// xt = QDateTime::currentMSecsSinceEpoch();
|
|
|
-// qDebug("latence = %ld ",xt-pic.time());
|
|
|
+void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ if(nSize<1)
|
|
|
+ {
|
|
|
+ std::cout<<"radar ListenGPSIMUMsg data empty."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ iv::gps::gpsimu xdata;
|
|
|
+ if(!xdata.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ std::cout<<" radar ListenGPSIMUMsg parse fail."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
+void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ if(nSize<1)
|
|
|
+ {
|
|
|
+ std::cout<<"radar ListenChassisMsg data empty."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ iv::chassis xdata;
|
|
|
+ if(!xdata.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+ std::cout<<" radar ListenChassisMsg parse fail."<<std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void ExitFunc()
|
|
|
+{
|
|
|
+ gApp->quit();
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
|
}
|
|
|
|
|
|
+void signal_handler(int sig)
|
|
|
+{
|
|
|
+ if(sig == SIGINT)
|
|
|
+ {
|
|
|
+ ExitFunc();
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
|
{
|
|
|
RegisterIVBackTrace();
|
|
|
- showversion("driver_radar_conti_ARS408_SRR308");
|
|
|
+ showversion("driver_radar_continental_ARS408_SRR308");
|
|
|
QCoreApplication a(argc, argv);
|
|
|
|
|
|
- gfault = new iv::Ivfault("radar_conti_ARS408_SRR308");
|
|
|
- givlog = new iv::Ivlog("radar_conti_ARS408_SRR308");
|
|
|
+ gApp = &a;
|
|
|
+
|
|
|
+ gfault = new iv::Ivfault("driver_radar_continental_ARS408_SRR308");
|
|
|
+ givlog = new iv::Ivlog("driver_radar_continental_ARS408_SRR308");
|
|
|
+
|
|
|
+ iv::ivexit::RegIVExitCall(ExitFunc);
|
|
|
+ signal(SIGINT,signal_handler);
|
|
|
|
|
|
QString strpath = QCoreApplication::applicationDirPath();
|
|
|
if(argc < 2)
|
|
|
- strpath = strpath + "/driver_radar_conti_ARS408_SRR308.xml";
|
|
|
+ strpath = strpath + "/driver_radar_continental_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);
|
|
|
+// givlog->verbose("%s", strpath.data());
|
|
|
+// std::cout<<strpath.toStdString()<<std::endl;
|
|
|
|
|
|
+ decode_setup_cfg_from_xml(setupConfig,strpath.toStdString());
|
|
|
|
|
|
+ if(setupConfig.radarCfgApply!=0)
|
|
|
+ {
|
|
|
+ RadarConfiguration_ch0_t radarConfiguration;
|
|
|
+ decode_radar_cfg_from_yaml(radarConfiguration,setupConfig.radarCfgYAML_Path);
|
|
|
+ }
|
|
|
|
|
|
- return a.exec();
|
|
|
+ strncpy(shmCANSend.mstrmsgname,setupConfig.strMemCANSend.data(),255);
|
|
|
+ shmCANSend.mnBufferSize = 100000;
|
|
|
+ shmCANSend.mnBufferCount = 3;
|
|
|
+ shmCANSend.mpa = iv::modulecomm::RegisterSend(shmCANSend.mstrmsgname,shmCANSend.mnBufferSize,shmCANSend.mnBufferCount);
|
|
|
+
|
|
|
+ strncpy(shmRadar.mstrmsgname,setupConfig.strMemRadar.data(),255);
|
|
|
+ shmRadar.mnBufferSize = 100000;
|
|
|
+ shmRadar.mnBufferCount = 1;
|
|
|
+ shmRadar.mpa = iv::modulecomm::RegisterSend(shmRadar.mstrmsgname,shmRadar.mnBufferSize,shmRadar.mnBufferCount);
|
|
|
+
|
|
|
+ strncpy(shmCANRecv.mstrmsgname,setupConfig.strMemCANRecv.data(),255);
|
|
|
+ shmCANRecv.mnBufferSize = 100000;
|
|
|
+ shmCANRecv.mnBufferCount = 3;
|
|
|
+ shmCANRecv.mpa = iv::modulecomm::RegisterRecv(shmCANRecv.mstrmsgname,ListenCANMsg);
|
|
|
+
|
|
|
+ strncpy(shmGPSIMU.mstrmsgname,setupConfig.strMemGPSIMU.data(),255);
|
|
|
+ shmGPSIMU.mnBufferSize = 100000;
|
|
|
+ shmGPSIMU.mnBufferCount = 3;
|
|
|
+ shmGPSIMU.mpa = iv::modulecomm::RegisterRecv(shmGPSIMU.mstrmsgname,ListenGPSIMUMsg);
|
|
|
+
|
|
|
+ strncpy(shmChassis.mstrmsgname,setupConfig.strMemChassis.data(),255);
|
|
|
+ shmChassis.mnBufferSize = 100000;
|
|
|
+ shmChassis.mnBufferCount = 3;
|
|
|
+ shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisMsg);
|
|
|
+
|
|
|
+ int rtn = a.exec();
|
|
|
+
|
|
|
+ if(gfault != nullptr)delete gfault;
|
|
|
+ if(givlog != nullptr)delete givlog;
|
|
|
+ if(shmCANRecv.mpa != nullptr)iv::modulecomm::Unregister(shmCANRecv.mpa);
|
|
|
+ if(shmCANSend.mpa != nullptr)iv::modulecomm::Unregister(shmCANSend.mpa);
|
|
|
+ if(shmRadar.mpa != nullptr)iv::modulecomm::Unregister(shmRadar.mpa);
|
|
|
+ if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
|
|
|
+ if(shmChassis.mpa != nullptr)iv::modulecomm::Unregister(shmChassis.mpa);
|
|
|
+
|
|
|
+ return rtn;
|
|
|
}
|