change driver_lidar_rs32. test point ok.

src/common/ivexit/ivexit.pro

@@ -21,6 +21,8 @@ DEFINES += IVEXIT_LIBRARY
 #VERSION = 1.0.1
 CONFIG += plugin
 
+QMAKE_CXXFLAGS +=  -g
+
 
 SOURCES += ivexit.cpp
 

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -128,20 +128,25 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
         unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
         double fazu = *pazu;
         fazu = fazu * 0.01;
-        fazu = fazu * M_PI/180.0;
+
         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;
@@ -209,8 +214,12 @@ int leishen32::DecodeDevinfo(iv::lidarudppac & xpac)
     }
 
     char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
-    unsigned short  * pmoterspeed = reinterpret_cast<unsigned short *>(pstr+8);//  ( unsigned short *)(pstr+8);
-    mdevinfo.motorspeed = *pmoterspeed;
+ //   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)

src/driver/driver_lidar_leishen32/lidarudp.cpp

@@ -27,6 +27,7 @@ std::vector<iv::lidarudppac> lidarudp::ComsumeRecv(int waitms)
     {
         xvectorlidarudppac = mvectorba;
     }
+    mvectorba.clear();
     mmutex.unlock();
     //If have data, not wait.
     if(xvectorlidarudppac.size()>0)
@@ -49,6 +50,7 @@ std::vector<iv::lidarudppac> lidarudp::ComsumeRecv(int waitms)
     {
         xvectorlidarudppac = mvectorba;
     }
+    mvectorba.clear();
     mmutex.unlock();
 
     return xvectorlidarudppac;

src/tool/tool_xodrobj/main.cpp

@@ -20,7 +20,7 @@ void ExitFunc()
 
 int main(int argc, char *argv[])
 {
-//    iv::ivexit::RegIVExitCall(ExitFunc);
+    iv::ivexit::RegIVExitCall(ExitFunc);
     RegisterIVBackTrace();
     showversion("tool_xodrobj");
     QApplication a(argc, argv);