add hesaipandarqt lidar driver pro

src/driver/driver_lidar_hesaipandarqt64/driver_lidar_hesaipandarqt64.pro

@@ -0,0 +1,54 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2018-12-14T18:35:51
+#
+#-------------------------------------------------
+
+QT       += network
+
+QT       -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+QMAKE_LFLAGS += -no-pie
+
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as 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
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+# You can also make your code fail to compile if you use 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 += \
+        lidar_driver_hesaipandarqt64.cpp \
+    main.cpp
+
+HEADERS += \
+        lidar_driver_hesaipandarqt64.h \
+    lidar_hesaipandarqt64_rawdata.h
+

src/driver/driver_lidar_hesaipandarqt64/lidar_driver_hesaipandarqt64.cpp

@@ -0,0 +1,600 @@
+
+#include <thread>
+#include <iostream>
+#include <QUdpSocket>
+//#include <QNetworkDatagram>
+#include <iostream>
+#include <QMutex>
+#include <QDateTime>
+
+#include <QFile>
+
+
+//#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "modulecomm.h"
+
+#include "lidar_driver_hesaipandarqt64.h"
+#include "lidar_hesaipandarqt64_rawdata.h"
+
+#include "ivexit.h"
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+
+#ifdef VV7_1
+
+int vv7;
+#endif
+
+
+
+#define Lidar_Pi 3.1415926535897932384626433832795
+#define Lidar32 (unsigned long)3405883584//192.168.1.203
+#define Lidar_roll_ang 90*Lidar_Pi/180.0
+
+
+static std::thread * g_phesaipandar64Thread;
+static std::thread * g_phesaipandar64ProcThread;
+
+
+
+//float gV_theta[16] =  {-15,1,-13,3,-11,5,-9,7,-7,9,-5,11,-3,13,-1,15};   //16 Angles
+//static float gV_theta[64] =  {14.7,10.85,7.877,4.875,2.858,1.846,1.678,1.506,1.34,1.169,1.002,0.831,0.664,0.493,0.326,0.155,-0.013,-0.182,-0.351,-0.519,-0.69,-0.857,-1.027,-1.195,-1.366,-1.533,-1.704,-1.87,-2.042,-2.21,-2.38,-2.547,-2.718,-2.882,-3.055,-3.222,-3.392,-3.557,-3.73,-3.894,-4.066,-4.232,-4.403,-4.567,-4.74,-4.902,-5.074,-5.239,-5.411,-5.573,-5.747,-5.908,-6.08,-6.243,-7.245,-8.241,-9.242,-10.227,-11.214,-12.188,-13.156,-14.112,-19.071,-25.079};   //64 Angles
+static float gV_theta[64] = {8.736,8.314,7.964,7.669,7.417,7.198,7.007,6.838,6.688,6.554,6.434,6.326,6.228,6.14,6.059,5.987,-5.27,-5.216,-5.167,-5.123,-5.083,-5.047,-5.016,-4.988,-4.963,-4.942,-4.924,-4.91,-4.898,-4.889,-4.884,-4.881,5.493,5.496,5.502,5.512,5.525,5.541,5.561,5.584,5.611,5.642,5.676,5.716,5.759,5.808,5.862,5.921,-5.33,-5.396,-5.469,-5.55,-5.64,-5.74,-5.85,-5.974,-6.113,-6.269,-6.447,-6.651,-6.887,-7.163,-7.493,-7.892};
+//static float gH_theta[64] = {-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,1.042,3.125,5.208,-5.208,-3.125,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042,-1.042};
+static float gH_theta[64] = {-52.121,-49.785,-47.577,-45.477,-43.465,-41.528,-39.653,-37.831,-36.055,-34.32,-32.619,-30.95,-29.308,-27.69,-26.094,-24.517,-22.964,-21.42,-19.889,-18.372,-16.865,-15.368,-13.88,-12.399,-10.925,-9.457,-7.994,-6.535,-5.079,-3.626,-2.175,-0.725,0.725,2.175,3.626,5.079,6.534,7.993,9.456,10.923,12.397,13.877,15.365,16.861,18.368,19.885,21.415,22.959,24.524,26.101,27.697,29.315,30.957,32.627,34.328,36.064,37.84,39.662,41.537,43.475,45.487,47.587,49.795,52.133};
+static float gV_theta_cos[64],gV_theta_sin[64];
+
+//static void * g_hesaipandar64_raw;
+
+static void * g_lidar_pc;
+
+
+static bool g_bhesaipandar64_run = false;
+static bool g_bhesaipandar64_running = false;
+static bool g_bhesaipandar64_Proc_running = false;
+
+static int g_seq = 0;
+
+static unsigned short glidar_port=2368;
+
+extern char gstr_memname[256];
+extern char gstr_rollang[256];
+extern char gstr_inclinationang_yaxis[256];  //from y axis
+extern char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+extern char gstr_port[256];
+extern char gstr_yaml[256];
+extern char gstr_calibfile[256];
+
+
+extern iv::Ivfault * gIvfault;
+extern iv::Ivlog * gIvlog;
+
+/**
+ * @brief The hesaipandar64_Buf class
+ *  Use for Lidar UDP DATA Save
+ */
+class hesaipandar64_Buf
+{
+private:
+    char * mstrdata;
+    int mnSize;  //Data SizeUse
+    QMutex mMutex;
+    int mIndex;
+public:
+    hesaipandar64_Buf()
+    {
+        mstrdata = new char[BK32_DATA_BUFSIZE];
+        mIndex = 0;
+        mnSize = 0;
+    }
+    ~hesaipandar64_Buf()
+    {
+        delete mstrdata;
+    }
+    void WriteData(const char * strdata,const int nSize)
+    {
+        mMutex.lock();
+        memcpy(mstrdata,strdata,nSize);
+        mnSize = nSize;
+        mIndex++;
+        mMutex.unlock();
+    }
+    int ReadData(char * strdata,const int nRead,int * pnIndex)
+    {
+        int nRtn = 0;
+        if(mnSize <= 0)return 0;
+        mMutex.lock();
+        nRtn = mnSize;
+        if(nRtn >nRead)
+        {
+            nRtn = nRead;
+            std::cout<<"lidar_hesaipandarQT64 hesaipandarQT64_Buf ReadData data nRead = "<<nRead<<" is small"<<std::endl;
+        }
+        memcpy(strdata,mstrdata,nRtn);
+        mnSize = 0;
+        if(pnIndex != 0)*pnIndex = mIndex;
+        mMutex.unlock();
+        return nRtn;
+    }
+};
+
+
+
+static hesaipandar64_Buf  * g_hesaipandar64_Buf;
+static char * g_RawData_Buf;  //Buffer UDP Data
+static int gnRawPos = 0;
+static float gAngle_Old = 0;
+static float gAngle_Total = 0;
+
+static unsigned short gold = 0;
+
+static int gnPac = 0;
+
+#include <QTime>
+
+static QTime gTime;
+
+/**
+ * @brief processhesaipandar64_Data
+ * @param ba UDP Buffer
+ * 1.UDP ByteArray Length is npacsize.
+ * 2.if Angle is More than 360. Tell Another thread process.
+ */
+static void processhesaipandar64_Data(QByteArray ba)
+{
+
+    gnPac++;
+    unsigned short * pAng;
+    float fAng;
+
+    char * pdata;
+    pdata = ba.data();
+//    const int npacsize = 1194;
+    const int npacsize = 1072;
+    if(ba.length() >= npacsize)
+    {
+//        pAng = (unsigned short *)(pdata+8);
+        pAng = (unsigned short *)(pdata+6+6);
+        fAng = *pAng;fAng = fAng*0.01;
+        if(fabs(fAng-gAngle_Old)>300)
+        {
+            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old)-360);
+        }
+        else
+        {
+            gAngle_Total = gAngle_Total + fabs(fabs(fAng-gAngle_Old));
+        }
+            gAngle_Old = fAng;
+            if(gAngle_Total > 360)
+            {
+                g_hesaipandar64_Buf->WriteData(g_RawData_Buf,gnRawPos);
+                lidar_hesaipandar64_raw * p = new lidar_hesaipandar64_raw();
+                p->mnLen = gnRawPos;
+                memcpy(p->mstrdata,g_RawData_Buf,gnRawPos);
+//                iv::modulecomm::ModuleSendMsg(g_hesaipandar64_raw,(char *)p,sizeof(lidar_hesaipandar64_raw));
+                delete p;
+                memcpy(g_RawData_Buf,pdata,npacsize);
+                gnRawPos = npacsize;
+  //                  std::cout<<"index = "<<gnPac<<" time ="<<gTime.elapsed()<<" a cycle"<<std::endl;
+                    gAngle_Total = 0;
+            }
+            else
+            {
+                if((gnRawPos+npacsize)<= BK32_DATA_BUFSIZE)
+                {
+                    memcpy(g_RawData_Buf+gnRawPos,pdata,npacsize);
+                    gnRawPos= gnRawPos+npacsize;
+                }
+                else
+                {
+                    std::cout<<"lidar_hesaipandar64 processhesaipandar64_Data data is very big gnRawPos = "<<gnRawPos<<std::endl;
+                }
+            }
+
+ //       std::cout<<*pAng<<std::endl;
+
+   //     gold = *pAng;
+        if(gnRawPos == 0)
+        {
+            gAngle_Total = 0;
+            gAngle_Old = *pAng;
+            gAngle_Old = gAngle_Old*0.01;
+            memcpy(g_RawData_Buf,pdata,npacsize);
+            gnRawPos = gnRawPos+npacsize;
+        }
+    }
+    else
+    {
+        std::cout<<"lidar_hesaipandar64 processhesaipandar64_Data receive data packet len is not npacsize "<<std::endl;
+    }
+}
+
+
+
+
+/**
+ * @brief hesaipandar64_Func thread for receive udp socket
+ * @param n
+ */
+static void hesaipandar64_Func(int n)
+{
+
+    gTime.start();
+    std::cout<<"Enter hesaipandar64_Func."<<std::endl;
+
+    QUdpSocket * udpSocket = new QUdpSocket( );
+    udpSocket->bind(QHostAddress::Any, glidar_port);
+
+    unsigned int ndatamisstime = 0;
+
+    int nstate = 0;
+    int nlaststate = 0;
+    while(g_bhesaipandar64_run)
+    {
+        if(udpSocket->hasPendingDatagrams())
+        {
+            ndatamisstime = 0;
+  //          std::cout<<"have data."<<std::endl;
+            QByteArray datagram;
+            datagram.resize(udpSocket->pendingDatagramSize());
+            QHostAddress sender;
+            quint16 senderPort;
+
+            udpSocket->readDatagram(datagram.data(), datagram.size(),
+                                            &sender, &senderPort);
+
+//                    processTheDatagram(datagram);
+  //          std::cout<<"have data."<<std::endl;
+//            QNetworkDatagram datagram = udpSocket->receiveDatagram();
+            processhesaipandar64_Data(datagram);
+            datagram.clear();
+
+        }
+        else
+        {
+//            std::cout<<"running."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            if(ndatamisstime <  1000000) ndatamisstime++;
+        }
+
+        if(ndatamisstime > 1000)
+        {
+            nstate = 1;
+        }
+        if(ndatamisstime > 60000)
+        {
+            nstate = 2;
+        }
+        if(ndatamisstime < 100)
+        {
+            nstate = 0;
+        }
+        if(nlaststate != nstate)
+        {
+            nlaststate = nstate;
+            switch (nstate) {
+            case 0:
+                gIvfault->SetFaultState(0,0,"OK");
+                gIvlog->info("received udp data.device is ok.");
+                break;
+            case 1:
+                gIvfault->SetFaultState(1,1,"No data");
+                gIvlog->warn("more than 1 second no data. warning.");
+                break;
+            case 2:
+                gIvfault->SetFaultState(2,2,"No data,Please Check Device or setting.");
+                gIvlog->error("more than 60 seconds no data. error. Please check device or setting.");
+                break;
+            default:
+                break;
+            }
+        }
+
+    }
+    udpSocket->close();
+    delete udpSocket;
+    g_bhesaipandar64_running = false;
+    std::cout<<"hesaipandar64_Func Exit."<<std::endl;
+}
+
+
+/**
+ * @brief process_hesaipandar64obs  Make PointCloud And Share
+ * @param strdata  pointer to data
+ * @param nLen  data length
+ */
+static void process_hesaipandar64obs(char * strdata,int nLen)
+{
+
+    double frollang = atof(gstr_rollang) *M_PI/180.0;    //Roll Angle
+    double finclinationang_xaxis = atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    bool binclix = false;
+    bool bincliy = false;
+    if(finclinationang_xaxis != 0.0)binclix = true;
+    if(finclinationang_yaxis != 0.0)bincliy = true;
+
+    double cos_finclinationang_xaxis = cos(finclinationang_xaxis);
+    double sin_finclinationang_xaxis = sin(finclinationang_xaxis);
+    double cos_finclinationang_yaxis = cos(finclinationang_yaxis);
+    double sin_finclinationang_yaxis = sin(finclinationang_yaxis);
+
+
+    QDateTime dt = QDateTime::currentDateTime();
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
+    point_cloud->width = 0;
+    point_cloud->header.seq =g_seq;
+    g_seq++;
+
+    unsigned char * pstr = (unsigned char *)strdata;
+
+//    std::cout<<"enter obs."<<std::endl;
+//    float w = 0.0036;
+    float Ang = 0;
+    float Range = 0;
+    int bag = 0;
+    int Group = 0;
+    int pointi = 0;
+    float wt = 0;
+    int wt1 = 0;
+
+
+//    const int npacsize = 1194;
+    const int npacsize = 1072;
+    int buf1len = nLen/npacsize;
+
+//    unsigned short * pAng;
+//    double ang1,ang2;
+//    pAng = (unsigned short *)(strdata+2+0*100);
+//    ang1 = *pAng;ang1 = ang1/100.0;
+//    pAng = (unsigned short *)(strdata+2+1*100);
+//    ang2 = *pAng;ang2 = ang2/100.0;
+
+//    double angdiff = ang2 - ang1;
+//    if(angdiff<0)angdiff = angdiff + 360.0;
+//    angdiff = angdiff/2.0;
+
+    for (bag = 0; bag < buf1len; bag++)
+    {
+//        for (Group = 0; Group < 6; Group++)
+        for (Group = 0; Group < 4; Group++)
+        {
+//            wt1 = ((pstr[bag*npacsize +9 + Group * 194] *256) + pstr[bag*npacsize + 8 + Group * 194]) ;
+            wt1 = ((pstr[bag*npacsize +13 + Group * 258] *256) + pstr[bag*npacsize + 12 + Group * 258]) ;
+            wt = wt1/ 100.0;
+
+            for (pointi = 0; pointi <64; pointi++)
+            {
+                Ang = (0 - wt-gH_theta[pointi]) / 180.0 * Lidar_Pi;
+//                Range = ((pstr[bag*npacsize + Group * 194 + 11 + 3 * pointi] << 8) + pstr[bag*npacsize+Group * 194 + 10 + 3 * pointi]);
+                Range = ((pstr[bag*npacsize + Group * 258 + 15 + 3 * pointi] << 8) + pstr[bag*npacsize+Group * 258 + 14 + 3 * pointi]);
+//                unsigned char intensity = pstr[bag*npacsize + Group * 194 + 12 + 3 * pointi];
+                unsigned char intensity = pstr[bag*npacsize + Group * 258 + 16 + 3 * pointi];
+                Range=Range* 0.004;
+
+
+                pcl::PointXYZI point;
+                point.x  = Range* gV_theta_cos[pointi]*cos(Ang + frollang);
+                point.y  = Range* gV_theta_cos[pointi] *sin(Ang + frollang);
+                point.z  = Range* gV_theta_sin[pointi];
+                point.intensity = intensity;
+
+                if(binclix)
+                {
+                    double y,z;
+                    y = point.y;z = point.z;
+                    point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                    point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+                }
+                if(bincliy)
+                {
+                    double z,x;
+                    z = point.z;x = point.x;
+                    point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                    point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+                }
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+
+            }
+
+        }
+    }
+
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+
+//            std::cout<<"point cloud width = "<<point_cloud->width<<"  size = "<<point_cloud->size()<<std::endl;
+}
+
+
+/**
+ * @brief hesaipandar64_Proc_Func
+ *    thread for process data to PointCloud
+ * @param n
+ */
+static void hesaipandar64_Proc_Func(int n)
+{
+    std::cout<<"Enter hesaipandar64_Proc_Func"<<std::endl;
+    char * strdata = new char[BK32_DATA_BUFSIZE];
+    int nIndex;
+    int nRead;
+    while(g_bhesaipandar64_run)
+    {
+        if((nRead = g_hesaipandar64_Buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
+        {
+            //process data.
+            process_hesaipandar64obs(strdata,nRead);
+        }
+        else
+        {
+ //           std::cout<<"running."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+
+    g_bhesaipandar64_Proc_running = false;
+    std::cout<<"Exit hesaipandar64_Proc_Func"<<std::endl;
+}
+
+
+
+void exitfunc()
+{
+    StopLidar_hesaipandar64();
+
+}
+
+
+static void decodecsv(char * strpath)
+{
+    QFile xFile;
+    xFile.setFileName(strpath);
+    if(!xFile.open(QIODevice::ReadOnly))
+    {
+        std::cout<<"open csv file "<<strpath<<" fail."<<std::endl;
+        return;
+    }
+    QByteArray ba = xFile.readAll();
+    QString strdata = ba;
+    QStringList strline = strdata.split("\n");
+    const int nline= 64;
+    if(strline.size()<(nline+1))
+    {
+        std::cout<<"File not complete. line size is "<<strline.size()<<std::endl;
+        xFile.close();
+        return;
+    }
+    int i;
+    int nlinesize = strline.size();
+    float * pxV,*pxH;
+    pxV = new float[nline];
+    pxH = new float[nline];
+    bool bAllOK = true;
+    for(i=1;i<(nline +1);i++)
+    {
+        QString strx = strline.at(i);
+        QStringList strlistvalue = strx.split(",");
+        if(strlistvalue.size()>=3)
+        {
+
+            pxV[i-1] = QString(strlistvalue.at(1)).toDouble();
+            pxH[i-1] = QString(strlistvalue.at(2)).toDouble();
+        }
+        else
+        {
+            bAllOK = false;
+        }
+    }
+
+    std::cout<<"Elevation:"<<std::endl;
+    for(i=0;i<nline;i++)
+    {
+        std::cout<<pxV[i]<<",";
+    }
+    std::cout<<std::endl;
+    std::cout<<"Azimuth:"<<std::endl;
+    for(i=0;i<nline;i++)
+    {
+        std::cout<<pxH[i]<<",";
+    }
+    std::cout<<std::endl;
+
+    if(bAllOK)
+    {
+        memcpy(gV_theta,pxV,64*sizeof(float));
+        memcpy(gH_theta,pxH,64*sizeof(float));
+    }
+    else
+    {
+        std::cout<<"Not All  Data  OK. Please check."<<std::endl;
+    }
+    delete pxV;
+    delete pxH;
+    xFile.close();
+}
+
+/**
+ * @brief StartLidar_hesaipandar64x Start
+ * @return
+ */
+int LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StartLidar_hesaipandar64()
+{
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+
+    std::cout<<"Now Start hesaipandar64 Listen."<<std::endl;
+    g_RawData_Buf = new char[BK32_DATA_BUFSIZE];
+
+    decodecsv(gstr_calibfile);
+
+    int i;
+    for(i=0;i<64;i++)
+    {
+        gV_theta[i] = gV_theta[i]*M_PI/180.0;
+        gV_theta_cos[i] = cos(gV_theta[i]);
+        gV_theta_sin[i] = sin(gV_theta[i]);
+    }
+
+    g_hesaipandar64_Buf = new hesaipandar64_Buf();
+    g_bhesaipandar64_run = true;
+    g_bhesaipandar64_running = true;
+    g_bhesaipandar64_Proc_running = true;
+
+    glidar_port = atoi(gstr_port);
+
+//    g_hesaipandar64_raw = iv::modulecomm::RegisterSend(strmemnameraw,10*sizeof(lidar_hesaipandar64_raw),10);
+    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);
+    g_phesaipandar64Thread = new std::thread(hesaipandar64_Func,0);
+    g_phesaipandar64ProcThread = new std::thread(hesaipandar64_Proc_Func,0);
+
+    return 0;
+}
+
+/**
+ * @brief StopLidar_hesaipandar64 Stop
+ */
+void LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StopLidar_hesaipandar64()
+{
+    std::cout<<"Now Close hesaipandar64. "<<std::endl;
+
+
+    g_bhesaipandar64_run = false;
+    g_phesaipandar64Thread->join();
+    g_phesaipandar64ProcThread->join();
+
+    iv::modulecomm::Unregister(g_lidar_pc);
+
+    delete g_phesaipandar64ProcThread;
+    delete g_phesaipandar64Thread;
+
+    delete g_hesaipandar64_Buf;
+    delete g_RawData_Buf;
+
+}

src/driver/driver_lidar_hesaipandarqt64/lidar_driver_hesaipandarqt64.h

@@ -0,0 +1,17 @@
+#ifndef LIDAR_DRIVER_HESAIPANDARQT64_H
+#define LIDAR_DRIVER_HESAIPANDARQT64_H
+
+
+#include <QtCore/qglobal.h>
+
+#if defined(LIDAR_DRIVER_HESAIPANDARQT64_LIBRARY)
+#  define LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT Q_DECL_EXPORT
+#else
+#  define LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT
+#endif
+
+
+int LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StartLidar_hesaipandar64();
+void LIDAR_DRIVER_HESAIPANDARQT64SHARED_EXPORT StopLidar_hesaipandar64();
+
+#endif //

src/driver/driver_lidar_hesaipandarqt64/lidar_hesaipandarqt64_rawdata.h

@@ -0,0 +1,13 @@
+#ifndef LIDAR_HESAIPANDARQT64_RAWDATA_H
+#define LIDAR_HESAIPANDARQT64_RAWDATA_H
+
+#define BK32_DATA_BUFSIZE 10000000
+
+class lidar_hesaipandar64_raw
+{
+public:
+    unsigned int mnLen;
+    char mstrdata[BK32_DATA_BUFSIZE];
+};
+
+#endif //

src/driver/driver_lidar_hesaipandarqt64/main.cpp

@@ -0,0 +1,225 @@
+#include <QCoreApplication>
+
+#include "ivversion.h"
+
+#include "lidar_driver_hesaipandarqt64.h"
+
+
+#include <getopt.h>
+#include <iostream>
+
+#include <iostream>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+iv::Ivfault * gIvfault;
+iv::Ivlog * gIvlog;
+
+char gstr_memname[256];
+char gstr_rollang[256];
+char gstr_inclinationang_yaxis[256];  //from y axis
+char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+char gstr_port[256];
+char gstr_yaml[256];
+char gstr_modulename[256];
+char gstr_calibfile[256];
+
+/**
+ * @brief print_useage
+ */
+void print_useage()
+{
+    std::cout<<" -n --modulename $modulename : set module name. eq.  -n lidarleft"<<std::endl;
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+//    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -c --calib $calib     eq.  -c /home/nvidia/1.csv"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:s:c:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"modulename", required_argument, NULL, 'n'},
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"port", required_argument, NULL, 'p'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"calib", required_argument, NULL, 'c'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'n':
+            strncpy(gstr_modulename,optarg,255);
+            break;
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+//        case 'o':
+//            strncpy(gstr_hostip,optarg,255);
+//            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'c':
+            strncpy(gstr_calibfile,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+/**
+ * @brief decodeyaml
+ * @param stryaml yaml path
+ */
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+//    if(config["hostip"])
+//    {
+//        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+//    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+//    std::cout<<gstr_memname<<std::endl;
+//    std::cout<<gstr_rollang<<std::endl;
+//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+//    std::cout<<gstr_hostip<<std::endl;
+//    std::cout<<gstr_port<<std::endl;
+}
+
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_lidar_hesaipandarqt64");
+    QCoreApplication a(argc, argv);
+
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.0");
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+//    snprintf(gstr_hostip,255,"192.168.1.102");
+    snprintf(gstr_port,255,"2368");
+    snprintf(gstr_yaml,255,"");
+    snprintf(gstr_modulename,255,"driver_lidar_hesaipandarqt64");
+    snprintf(gstr_calibfile,255,"5.PA6439CE509739CE56-correction.csv");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>0)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    gIvfault = new  iv::Ivfault(gstr_modulename);
+    gIvlog = new iv::Ivlog(gstr_modulename);
+    gIvfault->SetFaultState(0,0,"Initial State");
+    gIvlog->info("driver_lidar_hesaipandar64 starting.");
+
+    StartLidar_hesaipandar64();
+    return a.exec();
+}