change radar_4d. not complete. receive data must set vlan.

src/driver/driver_radar_4d_ars548/ar548pac.cpp

@@ -61,6 +61,11 @@ ars548pac::ars548pac(const char * pdata,int ndatalen)
 
 }
 
+ars548pac::~ars548pac()
+{
+
+}
+
 
 iv::ars548pac_header ars548pac::GetHeader()
 {
@@ -239,6 +244,9 @@ void ars548pac::DecodeDetect()
         xdet.set_unaligned_detection_azimuth_angle(fAngle);
         xdet.set_azimuth_angle_std(fAngleStd);
         switch (InvalidFlags) {
+        case 0:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::Valid);
+            break;
         case 1:
             xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InValidDistance);
             break;
@@ -407,23 +415,23 @@ void ars548pac::DecodeObj()
         unsigned int u_ID;SetMotoData(ppayload,nPos2,(char *)&u_ID,4);nPos2 = nPos2 + 4;
         unsigned short u_Age;SetMotoData(ppayload,nPos2,(char *)&u_Age,2);nPos2 = nPos2 + 2;
         unsigned char u_StatusMeasurement;SetMotoData(ppayload,nPos2,(char *)&u_StatusMeasurement,1);nPos2 = nPos2 + 1;
-        unsigned char u_StatusMovement;SetMotoData(ppayload,nPos2,(char *)&u_StatusMovement,1);nPos2 = nPos2 + 1;
+        unsigned char u_StatusMovement;SetMotoData(ppayload,nPos2,(char *)&u_StatusMovement,1);nPos2 = nPos2 + 1;//
         unsigned short u_Position_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Position_InvalidFlags,2);nPos2 = nPos2 + 2;
         unsigned char u_Position_Reference;SetMotoData(ppayload,nPos2,(char *)&u_Position_Reference,1);nPos2 = nPos2 + 1;
         float u_Position_X;SetMotoData(ppayload,nPos2,(char *)&u_Position_X,4);nPos2 = nPos2 + 4;
-        float u_Position_X_STD;SetMotoData(ppayload,nPos2,(char *)&u_Position_X_STD,4);nPos2 = nPos2 + 4;
+        float u_Position_X_STD;SetMotoData(ppayload,nPos2,(char *)&u_Position_X_STD,4);nPos2 = nPos2 + 4;//
         float u_Position_Y;SetMotoData(ppayload,nPos2,(char *)&u_Position_Y,4);nPos2 = nPos2 + 4;
         float u_Position_Y_STD;SetMotoData(ppayload,nPos2,(char *)&u_Position_Y_STD,4);nPos2 = nPos2 + 4;
-        float u_Position_Z;SetMotoData(ppayload,nPos2,(char *)&u_Position_Z,4);nPos2 = nPos2 + 4;
+        float u_Position_Z;SetMotoData(ppayload,nPos2,(char *)&u_Position_Z,4);nPos2 = nPos2 + 4;//
         float u_Position_Z_STD;SetMotoData(ppayload,nPos2,(char *)&u_Position_Z_STD,4);nPos2 = nPos2 + 4;
-        float u_Position_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&u_Position_CovarianceXY,4);nPos2 = nPos2 + 4;
+        float u_Position_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&u_Position_CovarianceXY,4);nPos2 = nPos2 + 4;//
         float u_Position_Orientation;SetMotoData(ppayload,nPos2,(char *)&u_Position_Orientation,4);nPos2 = nPos2 + 4;
         float u_Position_Orientation_STD;SetMotoData(ppayload,nPos2,(char *)&u_Position_Orientation_STD,4);nPos2 = nPos2 + 4;
-        unsigned char u_Existence_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Existence_InvalidFlags,1);nPos2 = nPos2 + 1;
+        unsigned char u_Existence_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Existence_InvalidFlags,1);nPos2 = nPos2 + 1;//
         float u_Existence_Probability;SetMotoData(ppayload,nPos2,(char *)&u_Existence_Probability,4);nPos2 = nPos2 + 4;
         float u_Existence_PPV;SetMotoData(ppayload,nPos2,(char *)&u_Existence_PPV,4);nPos2 = nPos2 + 4;
         unsigned char u_Classification_Car;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Car,1);nPos2 = nPos2 + 1;
-        unsigned char u_Classification_Truck;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Truck,1);nPos2 = nPos2 + 1;
+        unsigned char u_Classification_Truck;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Truck,1);nPos2 = nPos2 + 1;//
         unsigned char u_Classification_Motorcycle;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Motorcycle,1);nPos2 = nPos2 + 1;
         unsigned char u_Classification_Bicycle;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Bicycle,1);nPos2 = nPos2 + 1;
         unsigned char u_Classification_Pedestrian;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Pedestrian,1);nPos2 = nPos2 + 1;
@@ -433,40 +441,40 @@ void ars548pac::DecodeObj()
         unsigned char u_Classification_Overdrivable;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Overdrivable,1);nPos2 = nPos2 + 1;
         unsigned char u_Classification_Underdrivable;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Underdrivable,1);nPos2 = nPos2 + 1;
         unsigned char u_Dynamics_AbsVel_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_AbsVel_InvalidFlags,1);nPos2 = nPos2 + 1;
-        float f_Dynamics_AbsVel_X;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_X,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_AbsVel_X;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_X,4);nPos2 = nPos2 + 4;//
         float f_Dynamics_AbsVel_X_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_X_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_AbsVel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_Y,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_AbsVel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_Y,4);nPos2 = nPos2 + 4;//
         float f_Dynamics_AbsVel_Y_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_Y_STD,4);nPos2 = nPos2 + 4;
         float f_Dynamics_AbsVel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsVel_CovarianceXY,4);nPos2 = nPos2 + 4;
-        unsigned char u_Dynamics_RelVel_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_RelVel_InvalidFlags,1);nPos2 = nPos2 + 1;
+        unsigned char u_Dynamics_RelVel_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_RelVel_InvalidFlags,1);nPos2 = nPos2 + 1;//
         float f_Dynamics_RelVel_X;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_X,4);nPos2 = nPos2 + 4;
         float f_Dynamics_RelVel_X_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_X_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_RelVel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_Y,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_RelVel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_Y,4);nPos2 = nPos2 + 4;//
         float f_Dynamics_RelVel_Y_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_Y_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_RelVel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_CovarianceXY,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_RelVel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelVel_CovarianceXY,4);nPos2 = nPos2 + 4;//
         unsigned char u_Dynamics_AbsAccel_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_AbsAccel_InvalidFlags,1);nPos2 = nPos2 + 1;
         float f_Dynamics_AbsAccel_X;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_X,4);nPos2 = nPos2 + 4;
         float f_Dynamics_AbsAccel_X_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_X_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_AbsAccel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_Y,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_AbsAccel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_Y,4);nPos2 = nPos2 + 4;//
         float f_Dynamics_AbsAccel_Y_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_Y_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_AbsAccel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_CovarianceXY,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_AbsAccel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_AbsAccel_CovarianceXY,4);nPos2 = nPos2 + 4;//
         unsigned char u_Dynamics_RelAccel_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_RelAccel_InvalidFlags,1);nPos2 = nPos2 + 1;
         float f_Dynamics_RelAccel_X;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_X,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_RelAccel_X_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_X_STD,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_RelAccel_X_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_X_STD,4);nPos2 = nPos2 + 4;//
         float f_Dynamics_RelAccel_Y;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_Y,4);nPos2 = nPos2 + 4;
         float f_Dynamics_RelAccel_Y_STD;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_Y_STD,4);nPos2 = nPos2 + 4;
-        float f_Dynamics_RelAccel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_CovarianceXY,4);nPos2 = nPos2 + 4;
+        float f_Dynamics_RelAccel_CovarianceXY;SetMotoData(ppayload,nPos2,(char *)&f_Dynamics_RelAccel_CovarianceXY,4);nPos2 = nPos2 + 4;//
         unsigned char u_Dynamics_Orientation_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_Orientation_InvalidFlags,1);nPos2 = nPos2 + 1;
         float u_Dynamics_Orientation_Rate_Mean;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_Orientation_Rate_Mean,4);nPos2 = nPos2 + 4;
-        float u_Dynamics_Orientation_Rate_STD;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_Orientation_Rate_STD,4);nPos2 = nPos2 + 4;
+        float u_Dynamics_Orientation_Rate_STD;SetMotoData(ppayload,nPos2,(char *)&u_Dynamics_Orientation_Rate_STD,4);nPos2 = nPos2 + 4;//
         unsigned int u_Shape_Length_Status;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Status,4);nPos2 = nPos2 + 4;
-        unsigned char u_Shape_Length_Edge_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Edge_InvalidFlags,4);nPos2 = nPos2 + 4;
-        float u_Shape_Length_Edge_Mean;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Edge_Mean,4);nPos2 = nPos2 + 4;
+        unsigned char u_Shape_Length_Edge_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Edge_InvalidFlags,1);nPos2 = nPos2 + 1;
+        float u_Shape_Length_Edge_Mean;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Edge_Mean,4);nPos2 = nPos2 + 4;//
         float u_Shape_Length_Edge_STD;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Length_Edge_STD,4);nPos2 = nPos2 + 4;
-        unsigned int  u_Shape_Width_Status;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Status,4);nPos2 = nPos2 + 4;
+        unsigned int  u_Shape_Width_Status;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Status,4);nPos2 = nPos2 + 4;//
         unsigned char u_Shape_Width_Edge_InvalidFlags;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Edge_InvalidFlags,1);nPos2 = nPos2 + 1;
         float u_Shape_Width_Edge_Mean;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Edge_Mean,4);nPos2 = nPos2 + 4;
-        float u_Shape_Width_Edge_STD;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Edge_STD,4);nPos2 = nPos2 + 4;
+        float u_Shape_Width_Edge_STD;SetMotoData(ppayload,nPos2,(char *)&u_Shape_Width_Edge_STD,4);nPos2 = nPos2 + 4;//
 
         iv::radar::radar4dobject xobj;
         xobj.set_u_statussensor(u_StatusSensor);

src/driver/driver_radar_4d_ars548/ar548pac.h

@@ -63,10 +63,6 @@ public:
     iv::radar::radar4ddetectarray * GetDetArray();
     iv::radar::radar4dobjectarray * GetObjArray();
 
-
-
-
-
 private:
     void mototointel_unsignedshort(unsigned short &value);
     void mototointel_unsignedint(unsigned int &value);

src/driver/driver_radar_4d_ars548/ars548recv.cpp

@@ -1,17 +1,31 @@
 #include "ars548recv.h"
 
+#include <string>
+#include <memory>
+
 ars548recv::ars548recv()
 {
     InitSock();
 
     std::thread * pthread = new std::thread(&ars548recv::threadrecv,this);
+    mpthreadsock = pthread;
+    mpthreaddecode = new std::thread(&ars548recv::threaddecode,this);
+}
+
+ars548recv::~ars548recv()
+{
+    mbRun = false;
+    mpthreaddecode->join();
+    mpthreadsock->join();
+
 }
 
 
 void ars548recv::InitSock()
 {
-    std::string mcast_ip("224.0.2.2");
+     std::string mcast_ip("224.0.2.2");
      uint16_t mcast_port = 42102;
+     const char * streth0ip = "192.168.1.11";
 
 
      notify_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
@@ -53,24 +67,25 @@ void ars548recv::InitSock()
        return ;
      }
 
-     int loop = 1;
-     if (setsockopt(listen_fd_, IPPROTO_IP, IP_MULTICAST_ALL, &loop,
-                    sizeof(loop)) < 0) {
-       std::cout << "fail to setsockopt IP_MULTICAST_LOOP, " << strerror(errno);
-       return ;
-     }
 
      struct ip_mreq mreq;
+     // 转换为eth0的IP地址
+     struct in_addr eth0_addr;
+     if (inet_pton(AF_INET, streth0ip, &eth0_addr) <= 0) {
+         perror("inet_pton failed");
+         exit(EXIT_FAILURE);
+     }
 
-//     mreq.imr_interface.s_addr = htonl(INADDR_ANY);
+     // 设置组播选项
+     mreq.imr_multiaddr.s_addr = inet_addr(mcast_ip.data());
+     mreq.imr_interface.s_addr = eth0_addr.s_addr; // 使用eth0的IP地址
 
-     inet_aton( "192.168.1.102", &(mreq.imr_interface) );
-     if (setsockopt(listen_fd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq,
-                    sizeof(mreq)) < 0) {
-       std::cout << "fail to setsockopt IP_ADD_MEMBERSHIP, " << strerror(errno);
-       return ;
+     if (setsockopt(listen_fd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
+         perror("setsockopt failed");
+         exit(EXIT_FAILURE);
      }
 
+     mbRun = true;
      return ;
 
 }
@@ -78,7 +93,9 @@ void ars548recv::InitSock()
 void ars548recv::threadrecv()
 {
     int timeout_ms = 100;
-    while(1)
+    const int MAX_LEN = 65536;
+    std::shared_ptr<char >  pdata_ptr = std::shared_ptr<char >(new char[MAX_LEN]);
+    while(mbRun)
     {
         int nrtn = 0;
         struct pollfd fds;
@@ -87,13 +104,27 @@ void ars548recv::threadrecv()
         int ready_num = poll(&fds, 1, timeout_ms);
         if (ready_num > 0) {
             char buf[32] = {0};  // larger than ReadableInfo::kSize
-            ssize_t nbytes = recvfrom(listen_fd_, buf, 32, 0, nullptr, nullptr);
+            ssize_t nbytes = recvfrom(listen_fd_, pdata_ptr.get(),MAX_LEN, 0, nullptr, nullptr);
             if (nbytes == -1) {
                 std::cout << "fail to recvfrom, " << strerror(errno)<<std::endl;
                 continue;
             }
             if(nbytes >0)
             {
+                mmutexbuf.lock();
+                if(mvectorbuf.size()>=10)
+                {
+                    mvectorbuf.erase(mvectorbuf.begin());
+                    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv buffer full, erase one."<<std::endl;
+                }
+                iv::ars548recv_pac ap;
+                ap.mpdata_ptr = pdata_ptr;
+                ap.mrecvtime = std::chrono::system_clock::now().time_since_epoch().count();
+                ap.ndatalen = nbytes;
+                mvectorbuf.push_back(ap);
+                mmutexbuf.unlock();
+                mcv.notify_all();
+                pdata_ptr = std::shared_ptr<char>(new char[MAX_LEN]);
                 std::cout<<" recv data: "<<nbytes<<std::endl;
             }
     //          return info->DeserializeFrom(buf, nbytes);
@@ -110,5 +141,142 @@ void ars548recv::threadrecv()
 
 
     }
+
+    close(listen_fd_);
+
+    std::cout<<"socket recv thread close."<<std::endl;
+}
+
+void ars548recv::threaddecode()
+{
+    while(mbRun)
+    {
+        std::unique_lock<std::mutex> lk(mmutexcv);
+        if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout)
+        {
+            lk.unlock();
+        }
+        else
+        {
+            lk.unlock();
+            while(mvectorbuf.size()>0)
+            {
+                iv::ars548recv_pac ap;
+                bool bgetpac = false;
+                mmutexbuf.lock();
+                if(mvectorbuf.size()>0)
+                {
+                    ap = mvectorbuf[0];
+                    mvectorbuf.erase(mvectorbuf.begin());
+                    bgetpac = true;
+                }
+                mmutexbuf.unlock();
+                if(bgetpac)
+                {
+                    DecodePac(ap);
+                }
+            }
+        }
+    }
+}
+
+void ars548recv::DecodePac(iv::ars548recv_pac &ap)
+{
+    ars548pac pac(ap.mpdata_ptr.get(),ap.ndatalen);
+    pac.Decode();
+
+    if(pac.GetPacType() == ars548pactype::TypeDectect)
+    {
+        std::cout<<"detect. "<<std::endl;
+        iv::radar::radar4ddetectarray * pdetarray = pac.GetDetArray();
+        SetDetect(pdetarray);
+    }
+
+    if(pac.GetPacType() == ars548pactype::TypeObject)
+    {
+        std::cout<<"object. "<<std::endl;
+        iv::radar::radar4dobjectarray * pobjarray = pac.GetObjArray();
+        SetObject(pobjarray);
+    }
+}
+
+void  ars548recv::SetDetect(iv::radar::radar4ddetectarray * pdetarray)
+{
+    mmutexdet.lock();
+    mdetarray.CopyFrom(*pdetarray);
+    mmutexdet.unlock();
+    mbdetupdate = true;
+    mcvdet.notify_all();
+
+}
+
+void  ars548recv::SetObject(iv::radar::radar4dobjectarray * pobjarray)
+{
+    mmutexobj.lock();
+    mobjarray.CopyFrom(*pobjarray);
+    mmutexobj.unlock();
+    mbobjupdate = true;
+    mcvobj.notify_all();
+}
+
+int ars548recv::GetDetect(int waitms,iv::radar::radar4ddetectarray & xdetarray)
+{
+    std::unique_lock<std::mutex> lk(mmutexcvdet);
+    if(mbdetupdate)
+    {
+        mmutexdet.lock();
+        xdetarray.CopyFrom(mdetarray);
+        mbdetupdate = false;
+        mmutexdet.unlock();
+        return 1;
+    }
+    if(mcvdet.wait_for(lk,std::chrono::milliseconds(waitms)) == std::cv_status::timeout)
+    {
+        lk.unlock();
+    }
+    else
+    {
+        lk.unlock();
+    }
+    if(mbdetupdate)
+    {
+        mmutexdet.lock();
+        xdetarray.CopyFrom(mdetarray);
+        mbdetupdate = false;
+        mmutexdet.unlock();
+        return 1;
+    }
+    return 0;
+
+}
+
+int ars548recv::GetObj(int waitms, iv::radar::radar4dobjectarray & xobjarray)
+{
+    std::unique_lock<std::mutex> lk(mmutexcvobj);
+    if(mbobjupdate)
+    {
+        mmutexobj.lock();
+        xobjarray.CopyFrom(mobjarray);
+        mbobjupdate = false;
+        mmutexobj.unlock();
+        return 1;
+    }
+    if(mcvobj.wait_for(lk,std::chrono::milliseconds(waitms)) == std::cv_status::timeout)
+    {
+        lk.unlock();
+    }
+    else
+    {
+        lk.unlock();
+    }
+    if(mbobjupdate)
+    {
+        mmutexobj.lock();
+        xobjarray.CopyFrom(mobjarray);
+        mbobjupdate = false;
+        mmutexobj.unlock();
+        return 1;
+    }
+    return 0;
 }
 

src/driver/driver_radar_4d_ars548/ars548recv.h

@@ -14,19 +14,80 @@
 #include <iostream>
 
 
+#include <vector>
+#include <memory>
+#include <mutex>
+#include <condition_variable>
+
+#include <ar548pac.h>
+
+
+namespace iv {
+struct ars548recv_pac
+{
+  std::shared_ptr<char> mpdata_ptr;
+  int64_t mrecvtime;  //nano seconds from epoch
+  int ndatalen;
+};
+
+}
+
 class ars548recv
 {
 public:
     ars548recv();
     void InitSock();
 
+    ~ars548recv();
+
+private:
+    std::vector<iv::ars548recv_pac> mvectorbuf;
+    std::mutex mmutexbuf;
+    const int MAXBUFSIZE = 10;
+
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+
+private:
+    bool mbRun = false;
+
 private:
     int notify_fd_ = -1;
     struct sockaddr_in notify_addr_;
     int listen_fd_ = -1;
     struct sockaddr_in listen_addr_;
 
+    std::thread * mpthreadsock;
+
     void threadrecv();
+
+private:
+    std::thread * mpthreaddecode;
+    void threaddecode();
+
+private:
+    void DecodePac(iv::ars548recv_pac & ap);
+
+    iv::radar::radar4ddetectarray mdetarray;
+    bool mbdetupdate = false;
+    iv::radar::radar4dobjectarray mobjarray;
+    bool mbobjupdate = false;
+
+    std::mutex mmutexdet;
+    std::mutex mmutexcvdet;
+    std::condition_variable mcvdet;
+
+    std::mutex mmutexobj;
+    std::mutex mmutexcvobj;
+    std::condition_variable mcvobj;
+
+    void  SetDetect(iv::radar::radar4ddetectarray * pdetarray);
+    void  SetObject(iv::radar::radar4dobjectarray * pobjarray);
+
+public:
+    int GetDetect(int waitms,iv::radar::radar4ddetectarray & xdetarray);
+    int GetObj(int waitms, iv::radar::radar4dobjectarray & xobjarray);
+
 };
 
 #endif // ARS548RECV_H

src/driver/driver_radar_4d_ars548/mainwindow.cpp

@@ -35,16 +35,21 @@ MainWindow::MainWindow(QWidget *parent)
 
     QTimer * timer1 = new QTimer();
     connect(timer1,SIGNAL(timeout()),this,SLOT(onTimer()));
-    timer1->start(10);
+ //   timer1->start(10);
 
     mprecv = new ars548recv();
 
+    connect(this,SIGNAL(detupdate()),this,SLOT(onDetUpdate()));
+
+    std::thread * pthread = new std::thread(&MainWindow::threaddet,this);
+
     setWindowTitle("ARS548 Viewer");
 
 }
 
 MainWindow::~MainWindow()
 {
+    delete mprecv;
     delete ui;
 }
 
@@ -184,6 +189,70 @@ void MainWindow::onTimer()
 #endif
 }
 
+void MainWindow::onDetUpdate()
+{
+    iv::radar::radar4ddetectarray xdetarray;
+    mmutexdet.lock();
+    xdetarray.CopyFrom(mdetarray);
+    mbdetupdate = false;
+    mmutexdet.unlock();
+
+    static int nskip = 0;
+    nskip++;
+    if(nskip <10)
+    {
+        return;
+    }
+    nskip = 0;
+
+    static bool bSetCam = false;
+    if(bSetCam == false)
+    {
+        bSetCam = true;
+        view->setCameraPosition(0,30,130,0,30,0,0,1,0);
+//        view->resetCamera();    //视角
+    }
+
+    int nsize = static_cast<int>(mvectordetname.size());
+    int i;
+    for(i=0;i<nsize;i++){
+        view->removeShape(mvectordetname[i]);
+    }
+    mvectordetname.clear();
+
+    double base_size = 0.5;
+    double base_height = 0.5;
+    double sigx = 15;
+    double sigy = 15 ;
+    double sigz = 1.0;
+
+    nsize = static_cast<int>(xdetarray.mdet_size());
+
+    for(i=0;i<nsize;i++)
+    {
+        iv::radar::radar4ddetect * pdet = xdetarray.mutable_mdet(i);
+        if(pdet->detection_radial_distance()<0.001)continue;
+        sigx = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * cos(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
+        sigy = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * sin(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
+        sigz = pdet->detection_radial_distance() * sin(pdet->unaligned_detection_elevation_angle());
+        char strname[256];
+        snprintf(strname,256,"%det%d",i);
+        view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,strname);
+        mvectordetname.push_back(std::string(strname));
+    }
+
+    std::cout<<"update det."<<std::endl;
+
+#if VTK890
+    ui->vtk->renderWindow()->Render();
+#else
+    ui->vtk->GetRenderWindow()->Render();
+#endif
+
+
+
+
+}
 
 void MainWindow::on_actionReset_Camera_triggered()
 {
@@ -217,3 +286,19 @@ void MainWindow::keyReleaseEvent(QKeyEvent *event)
     }
 }
 
+void MainWindow::threaddet()
+{
+    while(1)
+    {
+        iv::radar::radar4ddetectarray xdetarray;
+        if(mprecv->GetDetect(10,xdetarray) == 1)
+        {
+            mmutexdet.lock();
+            mdetarray.CopyFrom(xdetarray);
+            mbdetupdate = true;
+            mmutexdet.unlock();
+            emit detupdate();
+        }
+    }
+}
+

src/driver/driver_radar_4d_ars548/mainwindow.h

@@ -52,6 +52,9 @@ public:
      void resizeEvent(QResizeEvent *event);
      void keyReleaseEvent(QKeyEvent *event) Q_DECL_OVERRIDE;
 
+signals:
+     void detupdate();
+
 private slots:
 
     void onTimer();
@@ -60,6 +63,8 @@ private slots:
 
     void on_actionFull_Screem_triggered();
 
+    void onDetUpdate();
+
 private:
     Ui::MainWindow *ui;
 
@@ -71,5 +76,17 @@ private:
 
     ars548recv * mprecv;
 
+private:
+    void threaddet();
+
+    iv::radar::radar4ddetectarray mdetarray;
+    std::mutex mmutexdet;
+    bool mbdetupdate = false;
+    iv::radar::radar4dobjectarray mobjarray;
+    bool mbobjupdate = false;
+    std::mutex mmutexobj;
+
+    std::vector<std::string> mvectordetname;
+
 };
 #endif // MAINWINDOW_H

src/include/proto/radar4ddetect.proto

@@ -7,6 +7,7 @@ message radar4ddetect
   optional float Unaligned_Detection_Azimuth_Angle = 1; //Unaligned Detection Azimuth Angle，rad
   optional float Azimuth_Angle_Std = 2; //Azimuth Angle Std(Detection目标方位角标准差)，rad
   enum DetectionInvalidFlagsType{
+  	    Valid = 0;
             InValidDistance = 1;
             InvalidDistanceStd = 2;
             InvalidAzimuth = 4;