fix(driver_ultrasonic_valeo):fix a decode bug, need to test in real vehicle.

src/driver/driver_ultrasonic_valeo_GUMK_HP/decode_cfg.cpp

@@ -7,6 +7,7 @@ int DecodeSonarCfgFromXML(setupConfig_t &setupConfig , const std::string &xmlFil
     setupConfig.strMemCANRecv = xp.GetParam("canrecv_shm","canrecv0");
     setupConfig.strMemCANSend = xp.GetParam("cansend_shm","cansend0");
     setupConfig.strMemSonar = xp.GetParam("sonar_shm","sonar");
+    setupConfig.environmentTemperature = std::strtod(xp.GetParam("temperature","15.0").c_str(),0);
 //    setupConfig.sonarCAN_ID = std::strtoul(xp.GetParam("sonarCAN_ID","0x101").c_str(),0,0);
 
 //    std::cout<<"canrecv_shm : "<<setupConfig.strMemCANRecv<<std::endl;

src/driver/driver_ultrasonic_valeo_GUMK_HP/decode_cfg.h

@@ -11,6 +11,7 @@ struct setupConfig_t
     std::string strMemCANRecv = "canrecv0";
     std::string strMemCANSend = "cansend0";
     std::string strMemSonar = "sonar";
+    double environmentTemperature = 15.0;
 };
 
 int DecodeSonarCfgFromXML(setupConfig_t &setupConfig , const std::string &xmlFilePath);

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP.pro

@@ -18,13 +18,13 @@ DEFINES += QT_DEPRECATED_WARNINGS
 QMAKE_LFLAGS += -no-pie
 
 SOURCES += \
-        ../../include/msgtype/canmsg.pb.cc \
-        ../../include/msgtype/canraw.pb.cc \
-        ../../include/msgtype/ultrasonic.pb.cc \
-        can_producer_consumer.cpp \
-        canrecv_consumer.cpp \
-        decode_cfg.cpp \
-        main.cpp \
+    ../../include/msgtype/canmsg.pb.cc \
+    ../../include/msgtype/canraw.pb.cc \
+    ../../include/msgtype/ultrasonic.pb.cc \
+    can_producer_consumer.cpp \
+    canrecv_consumer.cpp \
+    decode_cfg.cpp \
+    main.cpp \
     byte_qvector_producer_consumer.cpp \
     sdi_datarecv_consumer.cpp
 

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP.xml

@@ -3,5 +3,6 @@
 		<param name="canrecv_shm" value="canrecv0" />
 		<param name="cansend_shm" value="cansend0" />
 		<param name="sonar_shm" value="sonar" />
+		<param name="temperature" value="15.0" /> <!-- double number in celsius degree -->
 	</node>
 </xml>

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP.pro

@@ -1,17 +0,0 @@
-QT -= gui
-
-CONFIG += c++11 console
-CONFIG -= app_bundle
-
-# The following define makes your compiler emit warnings if you use
-# any feature of Qt which as been marked deprecated (the exact warnings
-# depend on your compiler). Please consult the documentation of the
-# deprecated API in order to know how to port your code away from it.
-DEFINES += QT_DEPRECATED_WARNINGS
-
-# You can also make your code fail to compile if you use deprecated APIs.
-# In order to do so, uncomment the following line.
-# You can also select to disable deprecated APIs only up to a certain version of Qt.
-#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-
-SOURCES += main.cpp

src/driver/driver_ultrasonic_valeo_GUMK_HP/driver_ultrasonic_valeo_GUMK_HP/main.cpp

@@ -1,8 +0,0 @@
-#include <QCoreApplication>
-
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
-
-    return a.exec();
-}

src/driver/driver_ultrasonic_valeo_GUMK_HP/main.cpp

@@ -2,6 +2,7 @@
 
 #include <iostream>
 #include <thread>
+#include <math.h>
 
 #include "modulecomm.h"
 #include "ivversion.h"
@@ -17,6 +18,7 @@
 #include "can_producer_consumer.h"
 #include "byte_qvector_producer_consumer.h"
 #include "canrecv_consumer.h"
+#include "sdi_datarecv_consumer.h"
 
 QCoreApplication * gApp;
 
@@ -24,6 +26,7 @@ iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
 
 setupConfig_t setupConfig;
+double gsoundVelocity = 340.0; // m/s
 
 iv::msgunit shmCANRecv;
 //iv::msgunit shmCANSend;
@@ -38,6 +41,7 @@ Byte_Qvector_Producer_Consumer FreeSpaceDataRecv(128);
 //CANSend_Producer* can_send_producer;
 //CANSend_Consumer* can_send_consumer;
 CANRecv_Consumer* can_recv_consumer;
+SDI_DataRecv_Consumer* sdi_data_recv_consumer;
 
 void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -90,6 +94,8 @@ int main(int argc, char *argv[])
 
     DecodeSonarCfgFromXML(setupConfig,strpath.toStdString());
 
+    gsoundVelocity = 331.45 * sqrt(setupConfig.environmentTemperature/273.0 + 1);
+
 //    strncpy(shmCANSend.mstrmsgname,setupConfig.strMemCANSend.data(),255);
 //    shmCANSend.mnBufferSize = 100000;
 //    shmCANSend.mnBufferCount = 3;
@@ -110,11 +116,15 @@ int main(int argc, char *argv[])
     can_recv_consumer = new CANRecv_Consumer(&CANRecv);
     can_recv_consumer->start();
 
+    sdi_data_recv_consumer = new SDI_DataRecv_Consumer(&SDIDataRecv);
+    sdi_data_recv_consumer->start();
+
     int rtn = a.exec();
 
 //    can_send_producer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
 //    can_send_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
     can_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
+    sdi_data_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
 
     if(gfault != nullptr)delete gfault;
     if(givlog != nullptr)delete givlog;

src/driver/driver_ultrasonic_valeo_GUMK_HP/sdi_datarecv_consumer.cpp

@@ -1,7 +1,8 @@
-#include "sdi_datarecv_consumer.h"
+#include "sdi_datarecv_consumer.h"
 
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmSonar;
+extern double gsoundVelocity;
 
 SDI_DataRecv_Consumer::SDI_DataRecv_Consumer(Byte_Qvector_Producer_Consumer *pBuf)
 {
@@ -20,17 +21,702 @@ void SDI_DataRecv_Consumer::run()
 {
     QVector<uint8_t> tempData;
 
+    BYTES2UINT bytes2uint;
+    bytes2uint.wordData = 0xFF00;
+
     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;
+        bytes2uint.byteData[1] = tempData.at(tempPtr - 1);
+        bytes2uint.byteData[0] = tempData.at(tempPtr - 2);
+        SDI_payload_size = bytes2uint.wordData;
+
+        SDI_sensor_CNT = tempData.at(8);
+        bytes2uint.byteData[0] = tempData.at(4);
+        bytes2uint.byteData[1] = tempData.at(5);
+        SDI_TX_Mask = bytes2uint.wordData;
+
+        if(SDI_sensor_CNT == 4 && SDI_payload_size >= 0x0061)
+        {
+            if(SDI_TX_Mask == 0x0041)
+            {
+                if(tempData.at(9)==0x00&&tempData.at(39)==0x01&&tempData.at(61)==0x06&&tempData.at(91)==0x07)
+                {
+                    Clear_Sensor_Data();
+
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 0
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_0.append(bytes2uint.wordData);
+                        sensorMagnitude_0.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(40); // sensor 1
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+41);
+                        bytes2uint.byteData[1] = tempData.at(4*i+42);
+                        sensorTOF_1.append(bytes2uint.wordData);
+                        sensorMagnitude_1.append(tempData.at(4*i+43));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 6
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_6.append(bytes2uint.wordData);
+                        sensorMagnitude_6.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(92); // sensor 7
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+93);
+                        bytes2uint.byteData[1] = tempData.at(4*i+94);
+                        sensorTOF_7.append(bytes2uint.wordData);
+                        sensorMagnitude_7.append(tempData.at(4*i+95));
+                    }
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else if(SDI_TX_Mask == 0x0820)
+            {
+                if(tempData.at(9)==0x04&&tempData.at(31)==0x05&&tempData.at(61)==0x0A&&tempData.at(83)==0x0B)
+                {
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 4
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_4.append(bytes2uint.wordData);
+                        sensorMagnitude_4.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(32); // sensor 5
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+33);
+                        bytes2uint.byteData[1] = tempData.at(4*i+34);
+                        sensorTOF_5.append(bytes2uint.wordData);
+                        sensorMagnitude_5.append(tempData.at(4*i+35));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 10
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_10.append(bytes2uint.wordData);
+                        sensorMagnitude_10.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(84); // sensor 11
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+85);
+                        bytes2uint.byteData[1] = tempData.at(4*i+86);
+                        sensorTOF_11.append(bytes2uint.wordData);
+                        sensorMagnitude_11.append(tempData.at(4*i+87));
+                    }
+
+                    this->ResultFilterAndPublish();
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else
+            {
+                std::cout<<"TX Mask error. Sensor may broken?"<<std::endl;
+            }
+        }
+        else if(SDI_sensor_CNT == 6 && SDI_payload_size >= 0x008D)
+        {
+            if(SDI_TX_Mask == 0x0082)
+            {
+                if(tempData.at(9)==0x00&&tempData.at(31)==0x01&&tempData.at(61)==0x02&&tempData.at(83)==0x06&&tempData.at(105)==0x07&&tempData.at(135)==0x08)
+                {
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 0
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_0.append(bytes2uint.wordData);
+                        sensorMagnitude_0.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(32); // sensor 1
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+33);
+                        bytes2uint.byteData[1] = tempData.at(4*i+34);
+                        sensorTOF_1.append(bytes2uint.wordData);
+                        sensorMagnitude_1.append(tempData.at(4*i+35));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 2
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_2.append(bytes2uint.wordData);
+                        sensorMagnitude_2.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(84); // sensor 6
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+85);
+                        bytes2uint.byteData[1] = tempData.at(4*i+86);
+                        sensorTOF_6.append(bytes2uint.wordData);
+                        sensorMagnitude_6.append(tempData.at(4*i+87));
+                    }
+
+                    tempEchosCNT = tempData.at(106); // sensor 7
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+107);
+                        bytes2uint.byteData[1] = tempData.at(4*i+108);
+                        sensorTOF_7.append(bytes2uint.wordData);
+                        sensorMagnitude_7.append(tempData.at(4*i+109));
+                    }
+
+                    tempEchosCNT = tempData.at(136); // sensor 8
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+137);
+                        bytes2uint.byteData[1] = tempData.at(4*i+138);
+                        sensorTOF_8.append(bytes2uint.wordData);
+                        sensorMagnitude_8.append(tempData.at(4*i+139));
+                    }
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else if(SDI_TX_Mask == 0x0104)
+            {
+                if(tempData.at(9)==0x01&&tempData.at(31)==0x02&&tempData.at(61)==0x03&&tempData.at(83)==0x07&&tempData.at(105)==0x08&&tempData.at(135)==0x09)
+                {
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 1
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_1.append(bytes2uint.wordData);
+                        sensorMagnitude_1.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(32); // sensor 2
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+33);
+                        bytes2uint.byteData[1] = tempData.at(4*i+34);
+                        sensorTOF_2.append(bytes2uint.wordData);
+                        sensorMagnitude_2.append(tempData.at(4*i+35));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 3
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_3.append(bytes2uint.wordData);
+                        sensorMagnitude_3.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(84); // sensor 7
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+85);
+                        bytes2uint.byteData[1] = tempData.at(4*i+86);
+                        sensorTOF_7.append(bytes2uint.wordData);
+                        sensorMagnitude_7.append(tempData.at(4*i+87));
+                    }
+
+                    tempEchosCNT = tempData.at(106); // sensor 8
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+107);
+                        bytes2uint.byteData[1] = tempData.at(4*i+108);
+                        sensorTOF_8.append(bytes2uint.wordData);
+                        sensorMagnitude_8.append(tempData.at(4*i+109));
+                    }
+
+                    tempEchosCNT = tempData.at(136); // sensor 9
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+137);
+                        bytes2uint.byteData[1] = tempData.at(4*i+138);
+                        sensorTOF_9.append(bytes2uint.wordData);
+                        sensorMagnitude_9.append(tempData.at(4*i+139));
+                    }
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else if(SDI_TX_Mask == 0x0208)
+            {
+                if(tempData.at(9)==0x02&&tempData.at(31)==0x03&&tempData.at(61)==0x04&&tempData.at(83)==0x08&&tempData.at(105)==0x09&&tempData.at(135)==0x0A)
+                {
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 2
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_2.append(bytes2uint.wordData);
+                        sensorMagnitude_2.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(32); // sensor 3
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+33);
+                        bytes2uint.byteData[1] = tempData.at(4*i+34);
+                        sensorTOF_3.append(bytes2uint.wordData);
+                        sensorMagnitude_3.append(tempData.at(4*i+35));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 4
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_4.append(bytes2uint.wordData);
+                        sensorMagnitude_4.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(84); // sensor 8
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+85);
+                        bytes2uint.byteData[1] = tempData.at(4*i+86);
+                        sensorTOF_8.append(bytes2uint.wordData);
+                        sensorMagnitude_8.append(tempData.at(4*i+87));
+                    }
+
+                    tempEchosCNT = tempData.at(106); // sensor 9
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+107);
+                        bytes2uint.byteData[1] = tempData.at(4*i+108);
+                        sensorTOF_9.append(bytes2uint.wordData);
+                        sensorMagnitude_9.append(tempData.at(4*i+109));
+                    }
+
+                    tempEchosCNT = tempData.at(136); // sensor 10
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+137);
+                        bytes2uint.byteData[1] = tempData.at(4*i+138);
+                        sensorTOF_10.append(bytes2uint.wordData);
+                        sensorMagnitude_10.append(tempData.at(4*i+139));
+                    }
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else if(SDI_TX_Mask == 0x0410)
+            {
+                if(tempData.at(9)==0x03&&tempData.at(31)==0x04&&tempData.at(61)==0x05&&tempData.at(83)==0x09&&tempData.at(105)==0x0A&&tempData.at(135)==0x0B)
+                {
+                    uint8_t tempEchosCNT = tempData.at(10); // sensor 3
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+11);
+                        bytes2uint.byteData[1] = tempData.at(4*i+12);
+                        sensorTOF_3.append(bytes2uint.wordData);
+                        sensorMagnitude_3.append(tempData.at(4*i+13));
+                    }
+
+                    tempEchosCNT = tempData.at(32); // sensor 4
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+33);
+                        bytes2uint.byteData[1] = tempData.at(4*i+34);
+                        sensorTOF_4.append(bytes2uint.wordData);
+                        sensorMagnitude_4.append(tempData.at(4*i+35));
+                    }
+
+                    tempEchosCNT = tempData.at(62); // sensor 5
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+63);
+                        bytes2uint.byteData[1] = tempData.at(4*i+64);
+                        sensorTOF_5.append(bytes2uint.wordData);
+                        sensorMagnitude_5.append(tempData.at(4*i+65));
+                    }
+
+                    tempEchosCNT = tempData.at(84); // sensor 9
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+85);
+                        bytes2uint.byteData[1] = tempData.at(4*i+86);
+                        sensorTOF_9.append(bytes2uint.wordData);
+                        sensorMagnitude_9.append(tempData.at(4*i+87));
+                    }
+
+                    tempEchosCNT = tempData.at(106); // sensor 10
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+107);
+                        bytes2uint.byteData[1] = tempData.at(4*i+108);
+                        sensorTOF_10.append(bytes2uint.wordData);
+                        sensorMagnitude_10.append(tempData.at(4*i+109));
+                    }
+
+                    tempEchosCNT = tempData.at(136); // sensor 11
+                    for(unsigned int i=0;i<tempEchosCNT;i++)
+                    {
+                        bytes2uint.byteData[0] = tempData.at(4*i+137);
+                        bytes2uint.byteData[1] = tempData.at(4*i+138);
+                        sensorTOF_11.append(bytes2uint.wordData);
+                        sensorMagnitude_11.append(tempData.at(4*i+139));
+                    }
+                }
+                else
+                {
+                    std::cout<<"sensor ID is not correct, payload data may broken. "<<std::endl;
+                }
+            }
+            else
+            {
+                std::cout<<"TX Mask error. Sensor may broken?"<<std::endl;
+            }
+        }
+        else
+        {
+            std::cout<<"number of working sensor is "<<(int)SDI_sensor_CNT<<"payload size is "<<(int)SDI_payload_size<<" may not correct. default is 4 and 6."<<std::endl;
+        }
+
+    }
+}
+
+void SDI_DataRecv_Consumer::Clear_Sensor_Data(void)
+{
+    sensorTOF_0.clear();
+    sensorTOF_1.clear();
+    sensorTOF_2.clear();
+    sensorTOF_3.clear();
+    sensorTOF_4.clear();
+    sensorTOF_5.clear();
+    sensorTOF_6.clear();
+    sensorTOF_7.clear();
+    sensorTOF_8.clear();
+    sensorTOF_9.clear();
+    sensorTOF_10.clear();
+    sensorTOF_11.clear();
+
+    sensorMagnitude_0.clear();
+    sensorMagnitude_1.clear();
+    sensorMagnitude_2.clear();
+    sensorMagnitude_3.clear();
+    sensorMagnitude_4.clear();
+    sensorMagnitude_5.clear();
+    sensorMagnitude_6.clear();
+    sensorMagnitude_7.clear();
+    sensorMagnitude_8.clear();
+    sensorMagnitude_9.clear();
+    sensorMagnitude_10.clear();
+    sensorMagnitude_11.clear();
+}
+
+void SDI_DataRecv_Consumer::ResultFilterAndPublish(void)
+{
+    if(sensorTOF_0.size()!=0 && sensorMagnitude_0.size()!=0)
+        this->BubbleSort(sensorTOF_0,sensorMagnitude_0);
+    else
+        return;
+
+    if(sensorTOF_1.size()!=0 && sensorMagnitude_1.size()!=0)
+        this->BubbleSort(sensorTOF_1,sensorMagnitude_1);
+    else
+        return;
+
+    if(sensorTOF_2.size()!=0 && sensorMagnitude_2.size()!=0)
+        this->BubbleSort(sensorTOF_2,sensorMagnitude_2);
+    else
+        return;
+
+    if(sensorTOF_3.size()!=0 && sensorMagnitude_3.size()!=0)
+        this->BubbleSort(sensorTOF_3,sensorMagnitude_3);
+    else
+        return;
+
+    if(sensorTOF_4.size()!=0 && sensorMagnitude_4.size()!=0)
+        this->BubbleSort(sensorTOF_4,sensorMagnitude_4);
+    else
+        return;
+
+    if(sensorTOF_5.size()!=0 && sensorMagnitude_5.size()!=0)
+        this->BubbleSort(sensorTOF_5,sensorMagnitude_5);
+    else
+        return;
+
+    if(sensorTOF_6.size()!=0 && sensorMagnitude_6.size()!=0)
+        this->BubbleSort(sensorTOF_6,sensorMagnitude_6);
+    else
+        return;
+
+    if(sensorTOF_7.size()!=0 && sensorMagnitude_7.size()!=0)
+        this->BubbleSort(sensorTOF_7,sensorMagnitude_7);
+    else
+        return;
 
+    if(sensorTOF_8.size()!=0 && sensorMagnitude_8.size()!=0)
+        this->BubbleSort(sensorTOF_8,sensorMagnitude_8);
+    else
+        return;
 
+    if(sensorTOF_9.size()!=0 && sensorMagnitude_9.size()!=0)
+        this->BubbleSort(sensorTOF_9,sensorMagnitude_9);
+    else
+        return;
 
+    if(sensorTOF_10.size()!=0 && sensorMagnitude_10.size()!=0)
+        this->BubbleSort(sensorTOF_10,sensorMagnitude_10);
+    else
+        return;
+
+    if(sensorTOF_11.size()!=0 && sensorMagnitude_11.size()!=0)
+        this->BubbleSort(sensorTOF_11,sensorMagnitude_11);
+    else
+        return;
+
+    iv::ultrasonic::ultrasonic xmsg;
+
+    for(int i=0;i<sensorTOF_0.size();i++)
+    {
+        if(sensorTOF_0.at(i)>0 && sensorMagnitude_0.at(i)>5)
+        {
+            xmsg.set_sigobjdist_flside((uint32_t)(gsoundVelocity * sensorTOF_0.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_ls(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_flside(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_ls(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_1.size();i++)
+    {
+        if(sensorTOF_1.at(i)>0 && sensorMagnitude_1.at(i)>5)
+        {
+            xmsg.set_sigobjdist_flcorner((uint32_t)(gsoundVelocity * sensorTOF_1.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_l(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_flcorner(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_l(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_2.size();i++)
+    {
+        if(sensorTOF_2.at(i)>0 && sensorMagnitude_2.at(i)>5)
+        {
+            xmsg.set_sigobjdist_flmiddle((uint32_t)(gsoundVelocity * sensorTOF_2.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_lm(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_flmiddle(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_lm(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_3.size();i++)
+    {
+        if(sensorTOF_3.at(i)>0 && sensorMagnitude_3.at(i)>5)
+        {
+            xmsg.set_sigobjdist_frmiddle((uint32_t)(gsoundVelocity * sensorTOF_3.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_rm(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_frmiddle(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_rm(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_4.size();i++)
+    {
+        if(sensorTOF_4.at(i)>0 && sensorMagnitude_4.at(i)>5)
+        {
+            xmsg.set_sigobjdist_frcorner((uint32_t)(gsoundVelocity * sensorTOF_4.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_r(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_frcorner(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_r(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_5.size();i++)
+    {
+        if(sensorTOF_5.at(i)>0 && sensorMagnitude_5.at(i)>5)
+        {
+            xmsg.set_sigobjdist_frside((uint32_t)(gsoundVelocity * sensorTOF_5.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_front_rs(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_frside(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_front_rs(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_6.size();i++)
+    {
+        if(sensorTOF_6.at(i)>0 && sensorMagnitude_6.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rrside((uint32_t)(gsoundVelocity * sensorTOF_6.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_rs(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rrside(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_rs(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_7.size();i++)
+    {
+        if(sensorTOF_7.at(i)>0 && sensorMagnitude_7.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rrcorner((uint32_t)(gsoundVelocity * sensorTOF_7.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_r(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rrcorner(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_r(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_8.size();i++)
+    {
+        if(sensorTOF_8.at(i)>0 && sensorMagnitude_8.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rrmiddle((uint32_t)(gsoundVelocity * sensorTOF_8.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_rm(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rrmiddle(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_rm(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_9.size();i++)
+    {
+        if(sensorTOF_9.at(i)>0 && sensorMagnitude_9.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rlmiddle((uint32_t)(gsoundVelocity * sensorTOF_9.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_lm(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rlmiddle(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_lm(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_10.size();i++)
+    {
+        if(sensorTOF_10.at(i)>0 && sensorMagnitude_10.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rlcorner((uint32_t)(gsoundVelocity * sensorTOF_10.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_l(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rlcorner(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_l(false);
+        }
+    }
+
+    for(int i=0;i<sensorTOF_11.size();i++)
+    {
+        if(sensorTOF_11.at(i)>0 && sensorMagnitude_11.at(i)>5)
+        {
+            xmsg.set_sigobjdist_rlside((uint32_t)(gsoundVelocity * sensorTOF_11.at(i) * 51.2 / 2000.0)); // mm
+            xmsg.set_sigsensor_rear_ls(true);
+            break;
+        }
+        else
+        {
+            xmsg.set_sigobjdist_rlside(DIST_ERROR); // 50000 mm
+            xmsg.set_sigsensor_rear_ls(false);
+        }
+    }
+
+    xmsg.set_timestamp(QDateTime::currentMSecsSinceEpoch());
+    int ndatasize = xmsg.ByteSize();
+    char * strser = new char[ndatasize];
+    std::shared_ptr<char> pstrser;
+    pstrser.reset(strser);
+    if(xmsg.SerializePartialToArray(strser,ndatasize))
+    {
+        iv::modulecomm::ModuleSendMsg(shmSonar.mpa,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<"ultrasonic data serialize error."<<std::endl;
+    }
+}
+
+void SDI_DataRecv_Consumer::BubbleSort(QVector<uint16_t> &sensorTOF, QVector<uint16_t> &sensorMagnitude)
+{
+    if(sensorTOF.size()<2||sensorMagnitude.size()<2)return;
+    //notice vector size must >= 2
+    int i = 0,j = 0;
+    uint16_t tempTOF = 0;
+    uint8_t tempMagnitude = 0;
+    if(sensorTOF.size() == sensorMagnitude.size())
+    {
+        for(i=0;i<sensorTOF.size()-1;i++)
+        {
+            for(j=0;j<sensorTOF.size()-1-i;j++)
+            {
+                if(sensorTOF.at(j)>sensorTOF.at(j+1))
+                {
+                    tempTOF = sensorTOF.at(j);
+                    sensorTOF[j] = sensorTOF.at(j+1);
+                    sensorTOF[j+1] = tempTOF;
+                    tempMagnitude = sensorMagnitude.at(j);
+                    sensorMagnitude[j] = sensorMagnitude.at(j+1);
+                    sensorMagnitude[j+1] = tempMagnitude;
+                }
+            }
+        }
     }
 }

src/driver/driver_ultrasonic_valeo_GUMK_HP/sdi_datarecv_consumer.h

@@ -14,6 +14,16 @@
 #include "decode_cfg.h"
 #include "iv_msgunit.h"
 
+#ifndef DIST_ERROR
+#define DIST_ERROR 50000 //50000mm
+#endif
+
+union BYTES2UINT
+{
+    uint16_t wordData = 0;
+    uint8_t byteData[2];
+};
+
 class SDI_DataRecv_Consumer : public QThread
 {
     Q_OBJECT
@@ -25,9 +35,42 @@ protected:
     void run();
 
 private:
+    void Clear_Sensor_Data(void);
+    void ResultFilterAndPublish(void);
+    void BubbleSort(QVector<uint16_t> &sensorTOF, QVector<uint16_t> &sensorMagnitude);
+
     Byte_Qvector_Producer_Consumer *pBuffer;
 
     uint16_t SDI_payload_size = 0;
+    uint8_t SDI_sensor_CNT = 0;
+    uint16_t SDI_TX_Mask = 0;
+
+    QVector<uint16_t> sensorTOF_0;
+    QVector<uint16_t> sensorTOF_1;
+    QVector<uint16_t> sensorTOF_2;
+    QVector<uint16_t> sensorTOF_3;
+    QVector<uint16_t> sensorTOF_4;
+    QVector<uint16_t> sensorTOF_5;
+    QVector<uint16_t> sensorTOF_6;
+    QVector<uint16_t> sensorTOF_7;
+    QVector<uint16_t> sensorTOF_8;
+    QVector<uint16_t> sensorTOF_9;
+    QVector<uint16_t> sensorTOF_10;
+    QVector<uint16_t> sensorTOF_11;
+
+    QVector<uint16_t> sensorMagnitude_0;
+    QVector<uint16_t> sensorMagnitude_1;
+    QVector<uint16_t> sensorMagnitude_2;
+    QVector<uint16_t> sensorMagnitude_3;
+    QVector<uint16_t> sensorMagnitude_4;
+    QVector<uint16_t> sensorMagnitude_5;
+    QVector<uint16_t> sensorMagnitude_6;
+    QVector<uint16_t> sensorMagnitude_7;
+    QVector<uint16_t> sensorMagnitude_8;
+    QVector<uint16_t> sensorMagnitude_9;
+    QVector<uint16_t> sensorMagnitude_10;
+    QVector<uint16_t> sensorMagnitude_11;
+
 };
 
 #endif // SDI_DATARECV_CONSUMER_H