add driver_lidar_leishen_c16.

include/common.pri

@@ -21,7 +21,7 @@ LIBS += -L$$PWD/../bin/ -lxmlparam -lmodulecomm  -livlog -livfault -livexit -liv
 
 unix:LIBS += -lboost_system  -lbacktrace -ldl
 
-QMAKE_CXXFLAGS +=  -g
+#QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

src/controller/controller_changan_shenlan_v2/controller_changan_shenlan_v2.pro

@@ -40,7 +40,7 @@ include($$PWD/control/control.pri)
 INCLUDEPATH += $$PWD/../controllercommon
 
 
-#DEFINES += TORQUEBRAKETEST
+DEFINES += TORQUEBRAKETEST
 
 #unix:!macx: LIBS += -L/home/yuchuli/qt/lib -lncomgnss -ladcmemshare
 

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -133,7 +133,7 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
         fazu = fazu * 0.01;
 
         pcl::PointXYZI point;
- //       std::cout<<" fazu: "<<fazu<<std::endl;
+        std::cout<<" fazu: "<<fazu<<std::endl;
 
 
         if(fabs(fazu-gAngleOld)>=300)

src/driver/driver_lidar_leishen32/main.cpp

@@ -181,7 +181,7 @@ void decodeyaml(const char * stryaml)
 
     if(config["devport"])
     {
-        strncpy(gstr_devport,config["port"].as<std::string>().data(),255);
+        strncpy(gstr_devport,config["devport"].as<std::string>().data(),255);
     }
 
     if(config["devmode"])

src/driver/driver_lidar_leishen_c16/.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
+

src/driver/driver_lidar_leishen_c16/driver_lidar_leishen_c16.pro

@@ -0,0 +1,42 @@
+QT -= gui
+
+
+QT       += network
+
+CONFIG += c++14 #console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as 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 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 += main.cpp \
+    leishenc16.cpp \
+    ../common/lidar/lidardecode.cpp \
+    ../common/lidar/lidarudp.cpp
+
+HEADERS += \
+    leishenc16.h \
+    ../common/lidar/lidardecode.h \
+    ../common/lidar/lidarudp.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../common/lidar

src/driver/driver_lidar_leishen_c16/leishenc16.cpp

@@ -0,0 +1,324 @@
+#include "leishenc16.h"
+
+#include "ivstdcolorout.h"
+
+
+leishenc16::leishenc16(char * strmemname,double froll,double finclinationang_xaxis ,
+                     double finclinationang_yaxis , int nDevMode ,
+                     unsigned short nDataPort,unsigned short nDevPort )
+{
+    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 = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+
+    RegisterSend(strmemname);
+    mnLidarMode = nDevMode;
+    mfrollang = froll;
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+
+        mpleishenc16_vertical = &leishenc16_vertical[0];
+    int i;
+    for(i=0;i<16;i++)
+    {
+        mthetacos[i] = cos(mpleishenc16_vertical[i]*M_PI/180.0);
+        mthetasin[i] = sin(mpleishenc16_vertical[i]*M_PI/180.0);
+        std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
+        std::cout<<"cos "<<i<<" "<<mthetacos[i]<<std::endl;
+    }
+
+    mplidarudp = new lidarudp(nDataPort);
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&leishenc16::threaddevdecode,this);
+    mpthread = new std::thread(&leishenc16::threaddecode,this);
+}
+
+leishenc16::~leishenc16()
+{
+    mbrun = false;
+    mpthreaddev->join();
+    mpthread->join();
+    delete mplidarudp;
+    delete mplidardevudp;
+}
+
+void leishenc16::threaddevdecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidardevudp->ComsumeRecv(100);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeDevinfo(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+void leishenc16::threaddecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidarudp->ComsumeRecv(10);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeUDPPac(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+int leishenc16::DecodeUDPPac(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1212)
+    {
+        return  -1;
+    }
+    static int nNotDecodeNum = 0;
+    if(mbdevinfo == false)
+    {
+        nNotDecodeNum++;
+        if(nNotDecodeNum > 1000)
+        {
+            ivstdcolorout("No Device Info Packet,Can't Get A1 A2 A3 A4.",iv::STDCOLOR_BOLDYELLOW);
+        }
+        return 0;
+    }
+    nNotDecodeNum = 0;
+
+    double finclinationang_xaxis = mfinclinationang_xaxis * M_PI/180.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = mfinclinationang_yaxis * M_PI/180.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    bool binclix = false;
+    bool bincliy = false;
+    if(fabs(finclinationang_xaxis)>0.00000001)binclix = true;
+    if(fabs(finclinationang_yaxis)>0.00000001)bincliy = true;
+
+    float cos_finclinationang_xaxis = static_cast<float>(cos(finclinationang_xaxis)) ;
+    float sin_finclinationang_xaxis = static_cast<float>(sin(finclinationang_xaxis));
+    float cos_finclinationang_yaxis = static_cast<float>(cos(finclinationang_yaxis));
+    float sin_finclinationang_yaxis = static_cast<float>(sin(finclinationang_yaxis));
+
+    double frollang = mfrollang *M_PI/180.0;
+
+    char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
+
+    static double gAngleTotal = 0;
+    static double gAngleOld = 0;
+    static unsigned int g_seq = 1;
+
+
+
+    int i;
+    for(i=0;i<12;i++)
+    {
+        char * pstrpac = pstr+i*100;
+        unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
+        double fazu = *pazu;
+        fazu = fazu * 0.01;
+
+        pcl::PointXYZI point;
+ //       std::cout<<" fazu: "<<fazu<<std::endl;
+
+
+        if(fabs(fazu-gAngleOld)>=300)
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld)-360);
+ //           std::cout<<" add : "<<fabs(fabs(fazu-gAngleOld)-360)<<std::endl;
+        }
+        else
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld));
+        }
+
+        gAngleOld = fazu;
+
+        fazu = fazu * M_PI/180.0;
+
+
+        if(gAngleTotal>360.0)
+        {
+            gAngleTotal = 0.0;
+            std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
+            ProduceCloud(mpoint_cloud_temp);
+            std::cout<<" produce. "<<std::endl;
+            //share point cloud.
+
+            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 = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+            point_cloud->width = 0;
+            point_cloud->header.seq =g_seq;
+            g_seq++;
+            mpoint_cloud_temp = point_cloud;
+        }
+
+
+        int j;
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
+            Range=Range* 0.004;
+
+
+            pcl::PointXYZI point;
+  //          std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu   )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu ));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float 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)
+            {
+                float 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;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+        fazu = fazu + 0.18 * M_PI/180.0;
+
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1 + 48] << 8) + pstrpac[4+j*3+0 + 48]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2 + 48]) ;
+            Range=Range* 0.004;
+
+
+            pcl::PointXYZI point;
+ //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu  )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu  ));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float 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)
+            {
+                float 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;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+    }
+
+    return  0;
+
+}
+
+int leishenc16::DecodeDevinfo(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1206)
+    {
+        return  -1;
+    }
+
+    char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
+ //   unsigned short  * pmoterspeed = reinterpret_cast<unsigned short *>(pstr+8);//  ( unsigned short *)(pstr+8);
+  //  qDebug("%02x %02x",pstr[8],pstr[9]);
+    mdevinfo.motorspeed = pstr[8] * 256 + pstr[9];
+ //   mdevinfo.motorspeed = *pmoterspeed;
+
+    std::cout<<" motor speed: "<<mdevinfo.motorspeed<<std::endl;
+
+    unsigned short * pmotorun = reinterpret_cast<unsigned short * >(pstr+40);
+    if(*pmotorun == 1)
+    {
+        mdevinfo.bmotorrun = true;
+    }
+    else
+        mdevinfo.bmotorrun = false;
+
+    unsigned short * ppacinterval = reinterpret_cast<unsigned short * >(pstr+42);
+    mdevinfo.pacinterval = *ppacinterval;
+
+    double A[4];
+    int i;
+    for(i=0;i<4;i++)
+    {
+        unsigned short * pA = reinterpret_cast<unsigned short *>(pstr+186+i*2);
+        unsigned short Avalue = *pA;
+        A[i] =((pstr[186+i*2+0] << 8) + pstr[186+i*2+1]);   //Avalue;
+        A[i] = A[i]*0.01;
+    }
+
+//    mdevinfo.A1 = A[0];
+//    mdevinfo.A2 = A[2];
+//    mdevinfo.A3 = A[1];
+//    mdevinfo.A4 = A[3];
+
+//    if(mnLidarMode == 0)
+//    {
+//        mpleishenc16_A[0] = mdevinfo.A2;mpleishenc16_A[1] = mdevinfo.A2;mpleishenc16_A[2] = mdevinfo.A1;mpleishenc16_A[3] = mdevinfo.A1;
+//        mpleishenc16_A[4] = mdevinfo.A2;mpleishenc16_A[5] = mdevinfo.A2;mpleishenc16_A[6] = mdevinfo.A1;mpleishenc16_A[7] = mdevinfo.A1;
+//        mpleishenc16_A[8] = mdevinfo.A2;mpleishenc16_A[9] = mdevinfo.A2;mpleishenc16_A[10] = mdevinfo.A1;mpleishenc16_A[11] = mdevinfo.A1;
+//        mpleishenc16_A[12] = mdevinfo.A2;mpleishenc16_A[13] = mdevinfo.A2;mpleishenc16_A[14] = mdevinfo.A1;mpleishenc16_A[15] = mdevinfo.A1;
+//        mpleishenc16_A[16] = mdevinfo.A2;mpleishenc16_A[17] = mdevinfo.A2;mpleishenc16_A[18] = mdevinfo.A1;mpleishenc16_A[19] = mdevinfo.A1;
+//        mpleishenc16_A[20] = mdevinfo.A2;mpleishenc16_A[21] = mdevinfo.A2;mpleishenc16_A[22] = mdevinfo.A1;mpleishenc16_A[23] = mdevinfo.A1;
+//        mpleishenc16_A[24] = mdevinfo.A2;mpleishenc16_A[25] = mdevinfo.A2;mpleishenc16_A[26] = mdevinfo.A1;mpleishenc16_A[27] = mdevinfo.A1;
+//        mpleishenc16_A[28] = mdevinfo.A2;mpleishenc16_A[29] = mdevinfo.A2;mpleishenc16_A[30] = mdevinfo.A1;mpleishenc16_A[31] = mdevinfo.A1;
+
+//    }
+//    else
+//    {
+//        mpleishenc16_A[0] = mdevinfo.A2;mpleishenc16_A[1] = mdevinfo.A2;mpleishenc16_A[2] = mdevinfo.A1;mpleishenc16_A[3] = mdevinfo.A1;
+//        mpleishenc16_A[4] = mdevinfo.A2;mpleishenc16_A[5] = mdevinfo.A4;mpleishenc16_A[6] = mdevinfo.A1;mpleishenc16_A[7] = mdevinfo.A3;
+//        mpleishenc16_A[8] = mdevinfo.A2;mpleishenc16_A[9] = mdevinfo.A2;mpleishenc16_A[10] = mdevinfo.A1;mpleishenc16_A[11] = mdevinfo.A1;
+//        mpleishenc16_A[12] = mdevinfo.A2;mpleishenc16_A[13] = mdevinfo.A4;mpleishenc16_A[14] = mdevinfo.A1;mpleishenc16_A[15] = mdevinfo.A3;
+//        mpleishenc16_A[16] = mdevinfo.A2;mpleishenc16_A[17] = mdevinfo.A2;mpleishenc16_A[18] = mdevinfo.A1;mpleishenc16_A[19] = mdevinfo.A1;
+//        mpleishenc16_A[20] = mdevinfo.A4;mpleishenc16_A[21] = mdevinfo.A2;mpleishenc16_A[22] = mdevinfo.A3;mpleishenc16_A[23] = mdevinfo.A1;
+//        mpleishenc16_A[24] = mdevinfo.A2;mpleishenc16_A[25] = mdevinfo.A2;mpleishenc16_A[26] = mdevinfo.A1;mpleishenc16_A[27] = mdevinfo.A1;
+//        mpleishenc16_A[28] = mdevinfo.A4;mpleishenc16_A[29] = mdevinfo.A2;mpleishenc16_A[30] = mdevinfo.A3;mpleishenc16_A[31] = mdevinfo.A1;
+//    }
+
+    mbdevinfo = true;
+    return 0;
+}

src/driver/driver_lidar_leishen_c16/leishenc16.h

@@ -0,0 +1,81 @@
+#ifndef leishenc16_H
+#define leishenc16_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+
+#include "lidarudp.h"
+
+#include "lidardecode.h"
+
+
+
+namespace iv {
+struct leishenc16_devinfo
+{
+    int motorspeed;
+    bool bmotorrun;
+    int pacinterval;
+    double A1;
+    double A2;
+    double A3;
+    double A4;
+
+};
+}
+
+class leishenc16 : public lidardecode
+{
+public:
+    leishenc16(char * strmemname, double froll = 0.0,double finclinationang_xaxis = 0.0,
+              double finclinationang_yaxis = 0.0, int nDevMode = 0,
+              unsigned short nDataPort = 2368,unsigned short nDevPort = 2369);
+    ~leishenc16();
+
+private:
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+    iv::leishenc16_devinfo mdevinfo;
+    bool mbdevinfo = false;
+
+    const double * mpleishenc16_vertical;
+    double  mpleishenc16_A[16];
+
+    double mthetacos[16];
+    double mthetasin[16];
+
+    std::thread * mpthread;
+    std::thread * mpthreaddev;
+    lidarudp * mplidarudp;
+    lidarudp * mplidardevudp;
+
+    bool mbrun = true;
+
+    int mnLidarMode = 0; //0 1degree   1 0.33degree
+
+    double mfinclinationang_xaxis = 0.0;
+    double mfinclinationang_yaxis = 0.0;
+    double mfrollang = 0.0;
+
+private:
+    const double leishenc16_vertical[16]={-16,0,-14,2,-12,4,-10,6,
+                                    -8,8,-6,10,-4,12,-2,14};
+
+
+
+private:
+    void threaddecode( );
+    void threaddevdecode();
+    int DecodeUDPPac(iv::lidarudppac & xpac);
+    int DecodeDevinfo(iv::lidarudppac & xpac);
+
+
+};
+
+#endif // leishenc16_H

src/driver/driver_lidar_leishen_c16/main.cpp

@@ -0,0 +1,234 @@
+#include <QCoreApplication>
+
+
+#include "leishenc16.h"
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+
+static char gstr_memname[256];
+static char gstr_rollang[256];
+static char gstr_inclinationang_yaxis[256];  //from y axis
+static char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+static char gstr_port[256];
+static char gstr_devport[256];
+static char gstr_yaml[256];
+static char gstr_modulename[256];
+static char gstr_devmode[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<<" -d --devport $port : port . eq.  -d 2369"<<std::endl;
+    std::cout<<" -f --devport $port : port . eq.  -f 0"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<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; // 设置短参数类型及是否需要参数
+    (void)digit_optind;
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:d:f:s: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'},
+        {"devport", required_argument, NULL, 'd'},
+        {"devmode", required_argument, NULL, 'f'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"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 'd':
+            strncpy(gstr_devport,optarg,255);
+            break;
+        case 'f':
+            strncpy(gstr_devmode,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,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);
+    }
+
+    if(config["devport"])
+    {
+        strncpy(gstr_devport,config["devport"].as<std::string>().data(),255);
+    }
+
+    if(config["devmode"])
+    {
+        strncpy(gstr_devmode,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[])
+{
+    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_devport,255,"2369");//默认端口号
+    snprintf(gstr_devmode,255,"0");//默认端口号
+    snprintf(gstr_yaml,255," ");
+    snprintf(gstr_modulename,255,"driver_lidar_leishenc16");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    leishenc16 * pleishenc16 = new leishenc16(gstr_memname,atof(gstr_rollang),atof(gstr_inclinationang_xaxis),
+                                           atof(gstr_inclinationang_yaxis),atoi(gstr_devmode),static_cast<unsigned short>(atoi(gstr_port)),
+                                           static_cast<unsigned short>(atoi(gstr_devport)));
+    (void)pleishenc16;
+
+    return a.exec();
+}

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -435,7 +435,7 @@ void MainWindow::onTimer()
         {
             if(mvectortrace.size()>0)
             {
-                if(fabs(mvectortrace[mvectortrace.size()-1].y) > 50.0)
+                if(fabs(mvectortrace[mvectortrace.size()-1].y) > 30.0)
                 {
                     CalcCalibRoll();
                     mbCalcCalib_Roll = true;