#include "ivdetection_radar.h" #include <iostream> namespace iv { ivdetection_radar::ivdetection_radar() { } void ivdetection_radar::setcanmsgname(const std::string str) { mstrcanmsgname = str; } void ivdetection_radar::setradarmsgname(const std::string str) { mstrradarmsgname = str; } void ivdetection_radar::modulerun() { iv::can::canmsg xmsg; ModuleFun funcan = std::bind(&iv::ivdetection_radar::UpdateCANMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5); mpmsgcan = iv::modulecomm::RegisterRecvPlus(mstrcanmsgname.data(),funcan); mpmsgradar = iv::modulecomm::RegisterSend(mstrradarmsgname.data(),100000,3); int nnothavedata = 0; iv::radar::radarobjectarray * pradar = new iv::radar::radarobjectarray; while(mbrun) { mWaitMutex.lock(); mwc.wait(&mWaitMutex,100); mWaitMutex.unlock(); if(mbUpdateCANMsg) { mMutex.lock(); xmsg.CopyFrom(mCANMsg); mbUpdateCANMsg = false; mMutex.unlock(); nnothavedata = 0; int nRtn; int i; for(i=0;i<xmsg.rawmsg_size();i++) { nRtn = DecodeCANMsg(*pradar,xmsg.mutable_rawmsg(i)); if(nRtn == 1) { ShareRadarMsg(mpmsgradar,pradar); delete pradar; pradar = new iv::radar::radarobjectarray; } } //Decode } else { nnothavedata++; } if(nnothavedata == 100) { } if(nnothavedata == 1000) { } } iv::modulecomm::Unregister(mpmsgradar); iv::modulecomm::Unregister(mpmsgcan); } void ivdetection_radar::UpdateCANMsg(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname) { (void )&index; (void )&dt; (void )strmemname; if(nSize<1)return; iv::can::canmsg xmsg; if(false == xmsg.ParseFromArray(strdata,nSize)) { std::cout<<"esr Listencan0 fail."<<std::endl; return; } mMutex.lock(); mCANMsg.CopyFrom(xmsg); mbUpdateCANMsg = true; mMutex.unlock(); mwc.wakeAll(); } void ivdetection_radar::ShareRadarMsg(void *pa, radar::radarobjectarray *pradar) { static qint64 oldrecvtime = 0; char * str = new char[pradar->ByteSize()]; int nsize = pradar->ByteSize(); if(pradar->SerializeToArray(str,nsize)) { iv::modulecomm::ModuleSendMsg(pa,str,nsize); } else { std::cout<<"ivdetection_radar::ShareRadarMsg SerializeToArray error."<<std::endl; } if(((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)&&(oldrecvtime != 0)) { // givlog->warn("radar interval is more than 100ms. value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime); } oldrecvtime = QDateTime::currentMSecsSinceEpoch(); // givlog->verbose("share time is %d ",gTime.elapsed()); // qDebug("share time is %d ",gTime.elapsed()); delete str; } }