更改禾赛科技QT64线激光雷达驱动的点云数据计算

src/driver/driver_lidar_hesaipandarqt64/lidar_driver_hesaipandarqt64.cpp

@@ -44,9 +44,24 @@ 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 gV_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 gH_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;
@@ -379,9 +394,9 @@ static void process_hesaipandar64obs(char * strdata,int nLen)
             {
                 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]);
+                Range = ((pstr[bag*npacsize + Group * 258 + 15 + 4 * pointi] << 8) + pstr[bag*npacsize+Group * 258 + 14 + 4 * pointi]);
 //                unsigned char intensity = pstr[bag*npacsize + Group * 194 + 12 + 3 * pointi];
-                unsigned char intensity = pstr[bag*npacsize + Group * 258 + 16 + 3 * pointi];
+                unsigned char intensity = pstr[bag*npacsize + Group * 258 + 16 + 4 * pointi];
                 Range=Range* 0.004;
 
 

src/driver/driver_lidar_hesaipandarqt64/main.cpp

@@ -195,14 +195,14 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     snprintf(gstr_memname,255,"lidar_pc");
-    snprintf(gstr_rollang,255,"270.0");
+    snprintf(gstr_rollang,255,"200");
     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");
+    snprintf(gstr_calibfile,255,"PandarQT_Correction_file.csv");
 
     int nRtn = GetOptLong(argc,argv);
     if(nRtn == 1)  //show help,so exit.
@@ -218,7 +218,7 @@ int main(int argc, char *argv[])
     gIvfault = new  iv::Ivfault(gstr_modulename);
     gIvlog = new iv::Ivlog(gstr_modulename);
     gIvfault->SetFaultState(0,0,"Initial State");
-    gIvlog->info("driver_lidar_hesaipandar64 starting.");
+    gIvlog->info("driver_lidar_hesaipandarqt64 starting.");
 
     StartLidar_hesaipandar64();
     return a.exec();