change src1/detection_radar_delphi_esr. complete. but not test.

src1/decision/decision_brain/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src1/decision/decision_brain/decision_brain.pro

@@ -0,0 +1,31 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../interface/ivdecision.pri ) {
+    error( "Couldn't find the ivdecision.pri file!" )
+}

src1/decision/decision_brain/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}

src1/decision/interface/ivdecision.cpp

@@ -0,0 +1,11 @@
+#include "ivdecision.h"
+
+namespace iv {
+
+
+ivdecision::ivdecision()
+{
+
+}
+
+}

src1/decision/interface/ivdecision.h

@@ -0,0 +1,14 @@
+#ifndef IVDECISION_H
+#define IVDECISION_H
+
+namespace iv {
+
+class ivdecision
+{
+public:
+    ivdecision();
+};
+
+}
+
+#endif // IVDECISION_H

src1/detection/detection_radar_delphi_esr/detection_radar_delphi_esr.pro

@@ -15,6 +15,7 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        ivdetection_radar_delphi_esr.cpp \
         main.cpp
 
 # Default rules for deployment.
@@ -29,3 +30,6 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !include(../interface/ivdetection_radar.pri ) {
     error( "Couldn't find the ivdetection_radar.pri file!" )
 }
+
+HEADERS += \
+    ivdetection_radar_delphi_esr.h

src1/detection/detection_radar_delphi_esr/ivdetection_radar_delphi_esr.cpp

@@ -0,0 +1,94 @@
+#include "ivdetection_radar_delphi_esr.h"
+
+#include <math.h>
+
+namespace iv {
+
+
+ivdetection_radar_delphi_esr::ivdetection_radar_delphi_esr(std::string strxmlpath)
+{
+    iv::xmlparam::Xmlparam xp(strxmlpath);
+
+    std::string strcanmsgname = xp.GetParam("canrecv","canrecv1");
+    std::string strradarmsgname = xp.GetParam("radar","radar");
+    setcanmsgname(strcanmsgname);
+    setradarmsgname(strradarmsgname);
+}
+
+int ivdetection_radar_delphi_esr::DecodeCANMsg(radar::radarobjectarray &xradar, can::canraw *prawmsg)
+{
+    int nrtn = 0;
+    int32_t range, rate, angle;
+    static int ncount = 0;
+    static qint64 nlastsend = 0;
+
+    ncount++;
+
+    iv::can::canraw canmsg;
+    canmsg.CopyFrom(*prawmsg);
+
+    if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53f))
+    {
+        int  data[8];
+        unsigned char cdata[8];
+        memcpy(cdata,canmsg.data().data(),8);
+
+        int radar_ID_index = canmsg.id() - 0x500;
+
+        int j;
+        for(j=0;j<8;j++)data[j] = cdata[j];
+
+        angle = ((data[1] & 0x1F) << 5) + ((data[2] & 0xF8) / 8);
+        range = ((data[2] & 0x07) << 8) + data[3];
+        rate = ((data[6] & 0x3F) << 8) | data[7];
+        if (angle & 0x200) {
+            angle = angle | 0xFFFFFC00;
+        }
+        if (rate & 0x2000) {
+            rate = rate | 0xFFFFC000;
+        }
+        //           qDebug("range = %d angle = %d ",range,angle);
+        //If angle and range both are 0, it is an invalid data.
+        if (angle != 0 || range != 0) {
+            iv::radar::radarobject * pobj = xradar.add_obj();
+            pobj->set_bvalid(true);
+
+            pobj->set_x(range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI));
+            pobj->set_y(range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI));
+            pobj->set_vel(rate * 1.0 / 100.0);
+            pobj->set_vx(pobj->vel() * sin(angle / 1800.0 * M_PI));
+            pobj->set_vy(pobj->vel() * cos(angle / 1800.0 * M_PI));
+        }
+        else {
+            iv::radar::radarobject * pobj = xradar.add_obj();
+            pobj->set_bvalid(false);
+        }
+
+
+        if(canmsg.id() == 0x53f)
+        {
+            nrtn = 1;
+        }
+
+        if(ncount > 100)
+        {
+            nrtn = 1;
+        }
+
+        if((QDateTime::currentMSecsSinceEpoch() - nlastsend) > 100)
+        {
+            nrtn = 1;
+        }
+    }
+
+    if(nrtn == 1)
+    {
+        ncount = 0;
+        nlastsend = QDateTime::currentMSecsSinceEpoch();
+    }
+    //       qDebug("id is %08x",canmsg.id());
+
+    return nrtn;
+}
+
+}

src1/detection/detection_radar_delphi_esr/ivdetection_radar_delphi_esr.h

@@ -0,0 +1,21 @@
+#ifndef IVDETECTION_RADAR_DELPHI_ESR_H
+#define IVDETECTION_RADAR_DELPHI_ESR_H
+
+#include "ivdetection_radar.h"
+
+#include "xmlparam.h"
+
+namespace iv {
+
+class ivdetection_radar_delphi_esr : public ivdetection_radar
+{
+public:
+    ivdetection_radar_delphi_esr(std::string strxmlpath);
+
+public:
+    virtual int DecodeCANMsg(radar::radarobjectarray &xradar, can::canraw *prawmsg);
+};
+
+}
+
+#endif // IVDETECTION_RADAR_DELPHI_ESR_H

src1/detection/detection_radar_delphi_esr/main.cpp

@@ -1,8 +1,23 @@
 #include <QCoreApplication>
 
+#include <iostream>
+
+#include "ivdetection_radar_delphi_esr.h"
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    QString strpath = QCoreApplication::applicationDirPath();
+    if(argc < 2)
+        strpath = strpath + "/detection_radar_esr.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+
+    iv::ivmodule * pivmodule = new iv::ivdetection_radar_delphi_esr(strpath.toStdString());
+
+    pivmodule->start();
+
     return a.exec();
 }

src1/detection/interface/ivdetection_radar.cpp

@@ -1,9 +1,130 @@
 #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;
+}
+
 }

src1/detection/interface/ivdetection_radar.h

@@ -3,11 +3,44 @@
 
 #include "ivdetection.h"
 
+#include "modulecomm.h"
+
+#include <QMutex>
+#include <QWaitCondition>
+
+#include "canmsg.pb.h"
+#include "canraw.pb.h"
+#include "radarobjectarray.pb.h"
+
 namespace iv {
 class ivdetection_radar : public ivdetection
 {
 public:
     ivdetection_radar();
+
+public:
+    void setcanmsgname(const std::string str);
+    void setradarmsgname(const std::string str);
+
+private:
+    void UpdateCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+    void ShareRadarMsg(void * pa,iv::radar::radarobjectarray * pradar);
+public:
+    virtual int DecodeCANMsg(iv::radar::radarobjectarray & xradar,iv::can::canraw *prawmsg) = 0;
+public:
+    virtual void modulerun();
+private:
+    void * mpmsgcan;
+    void * mpmsgradar;
+    std::string mstrcanmsgname;
+    std::string mstrradarmsgname;
+    bool mbUpdateCANMsg = false;
+    iv::can::canmsg mCANMsg;
+    QMutex mMutex;
+    QMutex mWaitMutex;
+    QWaitCondition mwc;
+
 };
 
 }

src1/detection/interface/ivdetection_radar.pri

@@ -1,9 +1,17 @@
 HEADERS += \
+    $$PWD/../../../src/include/msgtype/canmsg.pb.h \
+    $$PWD/../../../src/include/msgtype/canraw.pb.h \
+    $$PWD/../../../src/include/msgtype/radarobject.pb.h \
+    $$PWD/../../../src/include/msgtype/radarobjectarray.pb.h \
     $$PWD/../../interface/ivmodule.h \
     $$PWD/ivdetection.h \
     $$PWD/ivdetection_radar.h
 
 SOURCES += \
+    $$PWD/../../../src/include/msgtype/canmsg.pb.cc \
+    $$PWD/../../../src/include/msgtype/canraw.pb.cc \
+    $$PWD/../../../src/include/msgtype/radarobject.pb.cc \
+    $$PWD/../../../src/include/msgtype/radarobjectarray.pb.cc \
     $$PWD/../../interface/ivmodule.cpp \
     $$PWD/ivdetection.cpp \
     $$PWD/ivdetection_radar.cpp