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

Timestamp:

Jul 22, 2014, 4:36:25 PM (10 years ago)

Author:

DHERBOMEZ Gérald

Message:

commit after experiments on track
Issue with GPS receiver, no data recorded

File:

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

trunk/Sick/SickLDMRSSensor.cpp

@@ -138 +138 @@
     LOG_TRACE("destructor (" << this->name_ << ")");
 
-    S_socket->sendToServer(QString((u_int32_t)0x0021));
+    S_socket->sendToServer(QString((uint32_t)0x0021));
 
     S_socket->closeSocket();
@@ -157 +157 @@
 
 
-u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
+uint32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
 {
     if (length < 4) {
@@ -164 +164 @@
 
     unsigned long i = 0;
-    while(*((u_int32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
+    while(*((uint32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
         if (i == length) {
             return -1;
@@ -175 +175 @@
 
 
-u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
+uint32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
 {
 
@@ -202 +202 @@
 {
     // address = base + Data header size (24-byte long) + offset
-    msg.hScan.scanNumber = *((u_int16_t*)(msg.body+24));
-    msg.hScan.scannerStatus = *((u_int16_t*)(msg.body+24+2));
-    msg.hScan.phaseOffset = *((u_int16_t*)(msg.body+24+4));
-    msg.hScan.startNtpTime = *((u_int64_t*)(msg.body+24+6));
-    msg.hScan.endNtpTime = *((u_int64_t*)(msg.body+24+14));
-    msg.hScan.ticksPerRot= *((u_int16_t*)(msg.body+24+22)); // needed to compute angle (°)
+    msg.hScan.scanNumber = *((uint16_t*)(msg.body+24));
+    msg.hScan.scannerStatus = *((uint16_t*)(msg.body+24+2));
+    msg.hScan.phaseOffset = *((uint16_t*)(msg.body+24+4));
+    msg.hScan.startNtpTime = *((uint64_t*)(msg.body+24+6));
+    msg.hScan.endNtpTime = *((uint64_t*)(msg.body+24+14));
+    msg.hScan.ticksPerRot= *((uint16_t*)(msg.body+24+22)); // needed to compute angle (°)
     msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
     msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
-    msg.hScan.numPoints = *((u_int16_t*)(msg.body+24+28));
+    msg.hScan.numPoints = *((uint16_t*)(msg.body+24+28));
 
 //    msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
@@ -369 +369 @@
         }
 
-        ScanPoint scanPoints[msg.hScan.numPoints];
+        ScanPoint * scanPoints = new ScanPoint[msg.hScan.numPoints];
 
         // replace memory with structured data
@@ -375 +375 @@
             scanPoints[i].layerEcho = *((uchar*)(msg.body + index));
             scanPoints[i].flags = *((uchar*)(msg.body + index + 1));
-            scanPoints[i].angle = *((u_int16_t*)(msg.body + index + 2));
-            scanPoints[i].distance = *((u_int16_t*)(msg.body + index + 4));
-            scanPoints[i].echoPulseWidth = *((u_int16_t*)(msg.body + index + 6));
+            scanPoints[i].angle = *((uint16_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);
+
+                delete[] scanPoints;
     }
     else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
@@ -434 +436 @@
 void SickLDMRSSensor::configure(){
     // Start measuring
-    // S_socket->sendToServer(QString((u_int32_t)0x0020));
+    // S_socket->sendToServer(QString((uint32_t)0x0020));
 
     // LOG_TRACE(this->name_ +" configured.");