Changeset 74 in pacpussensors for trunk/StdDbtPlayerComponents/DbtPlySickLMSManager.cpp

Timestamp:

Jan 16, 2015, 5:01:19 PM (10 years ago)

Author:

DHERBOMEZ Gérald

Message:

• Modification of Sick LMS sensor to get the RSSI signal (reflectivity of the laser)
• Minor changes in data replay component of Sick LDMRS
• CanGateway: Choose by default of Vector CAN XL card

File:

• trunk/StdDbtPlayerComponents/DbtPlySickLMSManager.cpp (modified) (9 diffs)

trunk/StdDbtPlayerComponents/DbtPlySickLMSManager.cpp

@@ -17 +17 @@
 #include <string>
 
-#include "kernel/Log.h"
-#include "PacpusTools/ShMem.h"
-
+#include "Pacpus/kernel/Log.h"
+#include "Pacpus/PacpusTools/ShMem.h"
+
+#include <Pacpus/kernel/InputOutputInterface.h>
+
+#include <math.h>
 
 #define UTC_MAGIC_WORD  0x55544300
-
+#define PI 3.1415936
 
 namespace pacpus {
@@ -59 +62 @@
     DbtPlyFileManager::configureComponent(config);
 
-    mDataFilename = param.getProperty("binFile");
-
+    //mDataFilename = param.getProperty("binFile");
+        mDataFilename = config.getProperty("binFile");
     return ComponentBase::CONFIGURED_OK;
 }
@@ -96 +99 @@
     mDataFile.close();
 }
+
 
 //////////////////////////////////////////////////////////////////////////
@@ -115 +119 @@
     mSickDbt.scanFrequency = sickLMS_dbt->scanFrequency;
     mSickDbt.angleResolution = sickLMS_dbt->angleResolution;
-    mSickDbt.startAngle = sickLMS_dbt->startAngle;
+    mSickDbt.startAngle = sickLMS_dbt->startAngle; 
+
+        std::cout << "start angle is " << mSickDbt.startAngle  << std::endl;
+        std::cout << "angle step is " << mSickDbt.angleResolution  << std::endl;
     mSickDbt.time = t;
     mSickDbt.timerange = tr;
 
-
-    int sizes[7];
+        /*int sizes[7];*/
+        int sizes[9];
     sizes[0] = mSickDbt.dist_len1 = sickLMS_dbt->dist_len1;
     sizes[1] = mSickDbt.dist_len2 = sickLMS_dbt->dist_len2;
@@ -129 +136 @@
     sizes[6] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2;
 
-    uint32_t pos[7];
+    //sizes[7] = mSickDbt.rssi_len1 = sickLMS_dbt->rssi_len1_8;
+    //sizes[8] = mSickDbt.rssi_len2 = sickLMS_dbt->rssi_len2_8;
+    //uint32_t pos[7];
+        uint32_t pos[9];
     pos[0] = mSickDbt.dataPos_dist1 = sickLMS_dbt->dataPos_dist1;
     pos[1] = mSickDbt.dataPos_dist2 = sickLMS_dbt->dataPos_dist2;
@@ -138 +148 @@
     pos[6] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2;
 
+    //pos[7] = mSickDbt.dataPos_rssi1 = sickLMS_dbt->dataPos_rssi1_8;
+    //pos[8] = mSickDbt.dataPos_rssi2 = sickLMS_dbt->dataPos_rssi2_8;
+
     LOG_TRACE("(ScanPointsInfo) dist1 points\tSize : " << mSickDbt.dist_len1 <<"\tPos. : "<< mSickDbt.dataPos_dist1);
     LOG_TRACE("(ScanPointsInfo) dist2 points\tSize : " << mSickDbt.dist_len2 <<"\tPos. : "<< mSickDbt.dataPos_dist2);
@@ -147 +160 @@
 
 
+
     uint16_t* data[7];
-
+        uint8_t* remission_8[2];
     int sumSizes = 0;
 
     LOG_TRACE("Reading UTC file ... ");
-    for(int i = 0; i<7; ++i){
-
-        if(sizes[i]){
+    for(int i = 0; i<7; ++i)
+        {
+
+        if(sizes[i])
+                {
             mDataFile.seekg(pos[i]); // set the get pointer to the correct place
             // // then copy the data contained in the binary file
             data[i] = (uint16_t*) malloc(sizes[i]*sizeof(uint16_t));
-            for (size_t j = 0; j < sizes[i]; ++j) {
+            for (size_t j = 0; j < sizes[i]; ++j) 
+                        {
                 mDataFile.read(reinterpret_cast<char*>(data[i]+j), sizeof(uint16_t));
-                if(j%500==0){
+                if(j%500==0)
+                                {
                     LOG_TRACE("Data : " << (uint16_t) *(data[i] + j));
                 }
+                                if(i == 0)
+                                {
+
+                                        //std::cout << mSickLMSScan.point[j].distance << std::endl;
+                                }
+                                if(i == 5)
+                                {
+
+                                        std::cout <<  *(data[i]+j) << std::endl;
+                                }
             }
-        }
-        sumSizes += sizes[i];
-    }
-
+
+                }
+                sumSizes += sizes[i];
+    }
+        
+ //   for(int i = 7; i<9; ++i)
+        //{
+
+ //       if(sizes[i])
+        //      {
+ //           mDataFile.seekg(pos[i]); // set the get pointer to the correct place
+ //           // // then copy the data contained in the binary file
+ //           remission_8[i] = (uint8_t*) malloc(sizes[i]*sizeof(uint8_t));
+ //           for (size_t j = 0; j < sizes[i]; ++j) 
+        //              {
+ //               mDataFile.read(reinterpret_cast<char*>(remission_8[i]+j), sizeof(uint8_t));
+ //               if(j%500==0)
+        //                      {
+ //                   LOG_TRACE("Data : " << (uint16_t) *(remission_8[i] + j));
+ //               }
+        //                      if(i == 0)
+        //                      {
+
+        //                              //std::cout << mSickLMSScan.point[j].distance << std::endl;
+        //                      }
+        //                      if(i == 7)
+        //                      {
+
+        //                              std::cout << "the 8 bit remission" << *(remission_8[i]+j) << std::endl;
+        //                      }
+ //           }
+
+        //      }
+        //      sumSizes += sizes[i];
+ //   }
+ //       
 
     // verify that the last value is the UTC magic word
@@ -181 +241 @@
 
         /**********************************/
-        /*      TODO : Send data !        */
+
+
         /**********************************/
     }
@@ -198 +259 @@
 
 
-    for(int i=0; i<7; ++i){
-        free(data[i]);
+    for(int i=0; i<7; ++i)
+        {
+                if(sizes[i])
+                {
+                 free(data[i]);
+                }
     }
 }