Changeset 71 in pacpussensors for trunk/Sick/SickLDMRSSensor.cpp

Timestamp:

11/19/14 10:24:51 (10 years ago)

Author:

DHERBOMEZ Gérald

Message:

Correction of Sick LIDAR LDMRS code: problem when decoding the points cloud (with Chunlei Yu).
Correction of a small bug in Win32SerialPort when receiving RX_FLAG.

File:

• trunk/Sick/SickLDMRSSensor.cpp (modified) (4 diffs)

trunk/Sick/SickLDMRSSensor.cpp

@@ -126 +126 @@
         LOG_TRACE("(Recording) Not recording...");
 
-
 #ifdef SICKLDMRS_SH_MEM
     shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
@@ -323 +322 @@
         // we have a complete message available that we can add to the list
         MessageLDMRS msg;
-
-        // we copy the bytes in the body message
-//        char* messageData = (char*)malloc(msgSize);
-//        if(messageData == NULL){
-//            LOG_FATAL("(Packet reconstitution) Malloc FAILED. Packet lost.");
-//            return;
-//        }
         memcpy(msg.body, pendingBytes.data.c_str() + index, msgSize);
 
@@ -375 +367 @@
             scanPoints[i].layerEcho = *((uchar*)(msg.body + index));
             scanPoints[i].flags = *((uchar*)(msg.body + index + 1));
-            scanPoints[i].angle = *((uint16_t*)(msg.body + index + 2));
+            scanPoints[i].angle = *((int16_t*)(msg.body + index + 2));
             scanPoints[i].distance = *((uint16_t*)(msg.body + index + 4));
             scanPoints[i].echoPulseWidth = *((uint16_t*)(msg.body + index + 6));
-        }
-
-        memcpy(msg.body, scanPoints, sizeof(ScanPoint) * msg.hScan.numPoints);
+                        index+=10;
+                } 
+                memcpy(msg.body, scanPoints, sizeof(ScanPoint) * msg.hScan.numPoints);
 
                 delete[] scanPoints;
@@ -392 +384 @@
         // TODO
     }
-
-
     return msg.hData.dataType;
 }