change driver_lidar_rs16p.

src/driver/driver_lidar_rs16p/driver_lidar_rs16p.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network
 
-CONFIG += c++14 console
+CONFIG += c++14 # console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/driver/driver_lidar_rs16p/main.cpp

@@ -1,8 +1,12 @@
 #include <QCoreApplication>
 
+#include "rs16p.h"
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    rs16p * prs16p = new rs16p("lidar_pc");
+
     return a.exec();
 }

src/driver/driver_lidar_rs16p/rs16p.cpp

@@ -27,10 +27,18 @@ rs16p::rs16p(char * strmemname, double froll ,double finclinationang_xaxis ,
         mthetacos[i] = cos(mprs16p_vertical[i]*M_PI/180.0);
         mthetasin[i] = sin(mprs16p_vertical[i]*M_PI/180.0);
         std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
+
+        mfHor[i] = 0;
     }
 
     mplidarudp = new lidarudp(nDataPort);
+  //   mplidarudp = new lidarudp(7788);
+ //   lidarudp * pdev = new lidarudp(7788);
     mpthread = new std::thread(&rs16p::threaddecode,this);
+
+    int nDevPort = 7788;
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&rs16p::threaddevdecode,this);
 }
 
 rs16p::~rs16p()
@@ -59,12 +67,14 @@ void rs16p::threaddecode( )
 
 int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 {
-    if(xpac.mndatasize != 1206)
+    if(xpac.mndatasize != 1248)
     {
         std::cout<<" DecodeUDPPac: "<<" pac size is not 1206.  pac size: "<<xpac.mndatasize<<std::endl;
         return  -1;
     }
 
+
+
     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;
@@ -90,11 +100,13 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
     int i;
     for(i=0;i<12;i++)
     {
-        char * pstrpac = pstr+i*100;
+        char * pstrpac = pstr+i*100 + 42;
         unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
-        double fazu = *pazu;
+        double fazu =     ((pstrpac[2] << 8) + pstrpac[3]);//*pazu;
         fazu = fazu * 0.01;
 
+  //      std::cout<<" fazu: "<<fazu<<std::endl;
+
         pcl::PointXYZI point;
  //       std::cout<<" fazu: "<<fazu<<std::endl;
 
@@ -111,7 +123,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 
         gAngleOld = fazu;
 
-        fazu = fazu * M_PI/180.0;
+
+  //      std::cout<<" fazu: "<<fazu<<std::endl;
 
         if(gAngleTotal>=360.0)
         {
@@ -133,10 +146,11 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             mpoint_cloud_temp = point_cloud;
         }
 
+         fazu = fazu * M_PI/180.0;
         int j;
         for(j=0;j<16;j++)
         {
-            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
+            double Range = ((pstrpac[4+j*3+0] << 8) + pstrpac[4+j*3+1]);
             unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
             Range=Range* 0.0025;
 
@@ -144,8 +158,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             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.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu- mfHor[j])) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu - mfHor[j]));
             point.z  = static_cast<float>(Range* mthetasin[j]);
             point.intensity = intensity;
 
@@ -171,11 +185,11 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             ++mpoint_cloud_temp->width;
         }
 
-        fazu = fazu + 0.18 * M_PI/180.0;
+        fazu = fazu +0.2 * 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]);
+            double Range = ((pstrpac[4+j*3+0 + 48] << 8) + pstrpac[4+j*3+1 + 48]);
             unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2 + 48]) ;
             Range=Range* 0.0025;
 
@@ -183,8 +197,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             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.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu - mfHor[j] )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu- mfHor[j]));
             point.z  = static_cast<float>(Range* mthetasin[j]);
             point.intensity = intensity;
 
@@ -215,3 +229,50 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 
     return  0;
 }
+
+
+void rs16p::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]);
+            }
+        }
+    }
+}
+
+int rs16p::DecodeDevinfo(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1248)
+    {
+        return  -1;
+    }
+
+
+    unsigned char byte[48];
+    memcpy(byte,xpac.mdata_ptr.get()+564,48);
+
+    double fHor;
+    int i;
+    for(i=0;i<16;i++)
+    {
+        fHor = byte[i*3 + 1] * 256.0 + byte[i*3+2];
+        fHor = fHor * 0.01;
+        if(byte[i*3] == 1)fHor = fHor * (-1.0);
+        std::cout<<" hor: "<<i<<" "<<fHor<<std::endl;
+
+        fHor = fHor * M_PI/180.0;
+        mfHor[i] = fHor;
+    }
+    return 0;
+
+
+}
+

src/driver/driver_lidar_rs16p/rs16p.h

@@ -32,9 +32,14 @@ private:
     double mthetacos[16];
     double mthetasin[16];
 
+    double mfHor[16];
+
     std::thread * mpthread;
     lidarudp * mplidarudp;
 
+    std::thread * mpthreaddev;
+    lidarudp * mplidardevudp;
+
     bool mbrun = true;
 
     double mfinclinationang_xaxis = 0.0;
@@ -48,6 +53,9 @@ private:
 private:
     void threaddecode( );
     int DecodeUDPPac(iv::lidarudppac & xpac);
+
+    void threaddevdecode();
+    int DecodeDevinfo(iv::lidarudppac & xpac);
 };
 
 #endif // RS16P_H