fix(driver_ultrasonic_valeo):complete decode but has a litte bug, need to be fixed.

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.h

@@ -27,13 +27,13 @@ class CANRecv_Consumer : public QThread
 public:
     CANRecv_Consumer(CAN_Producer_Consumer *pBuf);
     ~CANRecv_Consumer();
-    void Clear_CAN_PrivateTempVariable(void);
 
 public slots:
     void Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID);
 
 protected:
     void run();
+    void Clear_CAN_PrivateTempVariable(void);
     uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw); //only for standard data frame, return can id
 
 signals:

src/driver/driver_ultrasonic_valeo_GUMK_HP/canrecv_consumer.cpp

@@ -2,9 +2,6 @@
 
 #include "math.h"
 
-extern setupConfig_t setupConfig;
-extern iv::msgunit shmSonar;
-
 extern Byte_Qvector_Producer_Consumer SDIDataRecv;
 extern Byte_Qvector_Producer_Consumer FreeSpaceDataRecv;
 
@@ -77,6 +74,10 @@ void CANRecv_Consumer::run()
                 }
                 if(SDI_data_ptr != 0 && tempMsgCnt >= SDI_msg_CNT && SDI_data_ptr >= SDI_payload_size)
                 {
+                    uint8_t tempPayload_L = SDI_payload_size & 0x00FF;
+                    uint8_t tempPayload_H = (SDI_payload_size >> 8) & 0x00FF;
+                    SDI_data[SDI_data_ptr] = tempPayload_L;
+                    SDI_data[SDI_data_ptr + 1] = tempPayload_H;
                     SDI_data_fine_lock.lock();
                     SDI_data_fine.swap(SDI_data);
                     SDI_data_fine_lock.unlock();
@@ -105,7 +106,7 @@ void CANRecv_Consumer::run()
         else if(freeSpace_recvStatus == RecevingData && tempCANRaw.id() > 0x700 && tempCANRaw.id() < 0x800)
         {
             Trans_From_CANRaw(tempCANRaw);
-            uint16_t tempMsgCnt = CAN_ID - 0x600;
+            uint16_t tempMsgCnt = CAN_ID - 0x700;
             if(tempMsgCnt <= freeSpace_msg_CNT)
             {
                 for(int i=0;i<CAN_DLC;i++)
@@ -115,6 +116,10 @@ void CANRecv_Consumer::run()
                 }
                 if(freeSpace_data_ptr != 0 && tempMsgCnt >= freeSpace_msg_CNT && freeSpace_data_ptr >= freeSpace_payload_size)
                 {
+                    uint8_t tempPayload_L = freeSpace_payload_size & 0x00FF;
+                    uint8_t tempPayload_H = (freeSpace_payload_size >> 8) & 0x00FF;
+                    freeSpace_data[freeSpace_data_ptr] = tempPayload_L;
+                    freeSpace_data[freeSpace_data_ptr + 1] = tempPayload_H;
                     freeSpace_data_fine_lock.lock();
                     freeSpace_data_fine.swap(freeSpace_data);
                     freeSpace_data_fine_lock.unlock();
@@ -144,7 +149,7 @@ void CANRecv_Consumer::SDI_Data_Ready_Slot(void)
 void CANRecv_Consumer::FreeSpace_Data_Ready_Slot(void)
 {
     freeSpace_data_fine_lock.lock();
-    FreeSpaceDataRecv.Produce_Element(SDI_data_fine);
+//    FreeSpaceDataRecv.Produce_Element(SDI_data_fine);
     freeSpace_data_fine.clear();
     freeSpace_data_fine_lock.unlock();
 }

src/driver/driver_ultrasonic_valeo_GUMK_HP/canrecv_consumer.h

@@ -11,8 +11,6 @@
 
 #include "can_producer_consumer.h"
 #include "byte_qvector_producer_consumer.h"
-#include "decode_cfg.h"
-#include "iv_msgunit.h"
 
 enum dataRecvStatus
 {
@@ -26,10 +24,12 @@ class CANRecv_Consumer : public QThread
 public:
     CANRecv_Consumer(CAN_Producer_Consumer *pBuf);
     ~CANRecv_Consumer();
-    void Clear_CAN_PrivateTempVariable(void);
+    uint16_t Get_SDI_Payload_Size(void);
+    uint16_t Get_FreeSpace_Payload_Size(void);
 
 protected:
     void run();
+    void Clear_CAN_PrivateTempVariable(void);
     uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw); //only for standard data frame, return can id
 
 signals:

src/driver/driver_ultrasonic_valeo_GUMK_HP/decode_cfg.h

@@ -11,7 +11,6 @@ struct setupConfig_t
     std::string strMemCANRecv = "canrecv0";
     std::string strMemCANSend = "cansend0";
     std::string strMemSonar = "sonar";
-//    uint32_t sonarCAN_ID = 0x301;
 };
 
 int DecodeSonarCfgFromXML(setupConfig_t &setupConfig , const std::string &xmlFilePath);

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP.pro

@@ -25,7 +25,8 @@ SOURCES += \
         canrecv_consumer.cpp \
         decode_cfg.cpp \
         main.cpp \
-    byte_qvector_producer_consumer.cpp
+    byte_qvector_producer_consumer.cpp \
+    sdi_datarecv_consumer.cpp
 
 HEADERS += \
     ../../include/msgtype/canmsg.pb.h \
@@ -35,7 +36,8 @@ HEADERS += \
     canrecv_consumer.h \
     decode_cfg.h \
     iv_msgunit.h \
-    byte_qvector_producer_consumer.h
+    byte_qvector_producer_consumer.h \
+    sdi_datarecv_consumer.h
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin

src/driver/driver_ultrasonic_valeo_GUMK_HP/sdi_datarecv_consumer.cpp

@@ -0,0 +1,36 @@
+#include "sdi_datarecv_consumer.h"
+
+extern setupConfig_t setupConfig;
+extern iv::msgunit shmSonar;
+
+SDI_DataRecv_Consumer::SDI_DataRecv_Consumer(Byte_Qvector_Producer_Consumer *pBuf)
+{
+    pBuffer = pBuf;
+
+
+}
+
+SDI_DataRecv_Consumer::~SDI_DataRecv_Consumer()
+{
+    requestInterruption();
+    while(this->isFinished() == false);
+}
+
+void SDI_DataRecv_Consumer::run()
+{
+    QVector<uint8_t> tempData;
+
+    while (!QThread::isInterruptionRequested())
+    {
+        tempData = pBuffer->Consume_Element();
+        uint16_t tempPtr = tempData.size();
+
+        uint8_t payloadSize_H = tempData.at(tempPtr - 1);
+        uint8_t payloadSize_L = tempData.at(tempPtr - 2);
+        SDI_payload_size = payloadSize_H;
+        SDI_payload_size = ((SDI_payload_size << 8) & 0xFF00) | payloadSize_L;
+
+
+
+    }
+}

src/driver/driver_ultrasonic_valeo_GUMK_HP/sdi_datarecv_consumer.h

@@ -0,0 +1,33 @@
+#ifndef SDI_DATARECV_CONSUMER_H
+#define SDI_DATARECV_CONSUMER_H
+
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QThread>
+
+#include "modulecomm.h"
+#include "ultrasonic.pb.h"
+
+#include "byte_qvector_producer_consumer.h"
+
+#include "decode_cfg.h"
+#include "iv_msgunit.h"
+
+class SDI_DataRecv_Consumer : public QThread
+{
+    Q_OBJECT
+public:
+    SDI_DataRecv_Consumer(Byte_Qvector_Producer_Consumer *pBuf);
+    ~SDI_DataRecv_Consumer();
+
+protected:
+    void run();
+
+private:
+    Byte_Qvector_Producer_Consumer *pBuffer;
+
+    uint16_t SDI_payload_size = 0;
+};
+
+#endif // SDI_DATARECV_CONSUMER_H

src/tool/pointcloudviewer/pointcloudviewer.pro

@@ -20,7 +20,7 @@ QMAKE_LFLAGS += -no-pie
 
 SOURCES += main.cpp
 
-INCLUDEPATH += /opt/ros/kinetic/include
+INCLUDEPATH += /opt/ros/melodic/include
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/eigen3