change 4d radar code. not complete.

src/driver/driver_radar_4d_ars548/ar548pac.cpp

@@ -2,6 +2,7 @@
 
 #include <iostream>
 #include <string.h>
+#include <memory>
 
 #define POS_ServiceID 0
 #define POS_MethodID 2
@@ -12,10 +13,11 @@
 #define POS_InterfaceVersion 13
 #define POS_MessageType 14
 #define POS_ReturnCode 15
+#define POS_PayLoad 16
 
 ars548pac::ars548pac(const char * pdata,int ndatalen)
 {
-    if(ndatalen<16)
+    if(ndatalen<36)
     {
         std::cout<<" ARS548 Packet Len < 16, ndatalen:"<<ndatalen<<std::endl;
         mbPacValid = false;
@@ -44,6 +46,19 @@ ars548pac::ars548pac(const char * pdata,int ndatalen)
         return;
     }
 
+    memcpy(mPayload.CRC,pdata+POS_PayLoad + 0,8);
+    memcpy(&mPayload.Length,pdata+POS_PayLoad + 8,4);
+    memcpy(&mPayload.SOC,pdata+POS_PayLoad + 12,4);
+    memcpy(&mPayload.DataID,pdata+POS_PayLoad + 16,4);
+    mototointel_value((char *)&mPayload.Length,4);
+    mototointel_value((char *)&mPayload.SOC,4);
+    mototointel_value((char *)&mPayload.DataID,4);
+
+    mPayload.nDataLen = ndatalen - 36;
+    mPayload.pPayload_ptr = std::shared_ptr<char >(new char[mPayload.nDataLen]);
+    memcpy(mPayload.pPayload_ptr.get(),pdata + 36,mPayload.nDataLen);
+
+
 }
 
 
@@ -122,6 +137,119 @@ void ars548pac::Decode()
 
 void ars548pac::DecodeDetect()
 {
+    uint64_t CRC;
+    memcpy(&CRC,mPayload.CRC,8);
+    mdetarray.set_crc(CRC);
+    mdetarray.set_length(mPayload.Length);
+    mdetarray.set_soc(mPayload.SOC);
+    mdetarray.set_dataid(mPayload.DataID);
+
+
+    int ndatalen = mPayload.nDataLen;
+    char * ppayload = mPayload.pPayload_ptr.get();
+
+    int nPos = 0;
+    unsigned int Timestamp_Nanoseconds;
+    unsigned int Timestamp_Seconds;
+    unsigned char Timestamp_SyncStatus;
+    unsigned int EventDataQualifier;
+    unsigned char ExtendedQualifier;
+    unsigned short Origin_InvalidFlags;
+    float Origin_Xpos;
+    float Origin_Xstd;
+    float Origin_Ypos;
+    float Origin_Ystd;
+    float Origin_Zpos;
+    float Origin_Zstd;
+    float Origin_Roll;
+    float Origin_Rollstd;
+    float Origin_Pitch;
+    float Origin_Pitchstd;
+    float Origin_Yaw;
+    float Origin_Yawstd;
+    unsigned char List_InvalidFlags;
+//    List_Detections;
+    float List_RadVelDomain_Min;
+    float List_RadVelDomain_Max;
+    unsigned int List_NumOfDetections;
+    float Aln_AzimuthCorrection;
+    float Aln_ElevationCorrection;
+    unsigned char Aln_Status;
+
+    SetMotoData(ppayload,nPos, (char *)&Timestamp_Nanoseconds,4);nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Timestamp_Seconds,4); nPos = nPos +4;
+    SetMotoData(ppayload,nPos, (char *)&Timestamp_SyncStatus,1); nPos = nPos +1;
+    SetMotoData(ppayload,nPos, (char *)&EventDataQualifier,4);nPos = nPos +4;
+    SetMotoData(ppayload,nPos, (char *)&ExtendedQualifier,1); nPos = nPos + 1;
+    SetMotoData(ppayload,nPos, (char *)&Origin_InvalidFlags,2); nPos = nPos + 2;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Xpos,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Xstd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Ypos,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Ystd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Zpos,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Zstd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Roll,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Rollstd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Pitch,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Pitchstd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Yaw,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Origin_Yawstd,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&List_InvalidFlags,1); nPos = nPos + 1;
+
+    nPos = nPos + 44 * 800;
+    SetMotoData(ppayload,nPos, (char *)&List_RadVelDomain_Min,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&List_RadVelDomain_Max,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&List_NumOfDetections,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Aln_AzimuthCorrection,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Aln_ElevationCorrection,4); nPos = nPos + 4;
+    SetMotoData(ppayload,nPos, (char *)&Aln_Status,1); //nPos = nPos + 1;
+
+    mdetarray.set_timestamp_ns(Timestamp_Nanoseconds);
+    mdetarray.set_timestamp_s(Timestamp_Seconds);
+    switch (Timestamp_SyncStatus) {
+    case 1:
+        mdetarray.set_timestamp_syncstatus(iv::radar::radar4ddetectarray::SYNC_OK);
+        break;
+    case 2:
+        mdetarray.set_timestamp_syncstatus(iv::radar::radar4ddetectarray::SYNC_NEVERSYNC);
+        break;
+    default:
+        mdetarray.set_timestamp_syncstatus(iv::radar::radar4ddetectarray::SYNC_LOST);
+        break;
+    }
+    mdetarray.set_eventdataqualifier(EventDataQualifier);
+    mdetarray.set_extendedqualifier(ExtendedQualifier);
+    mdetarray.set_origin_invalidflags(Origin_InvalidFlags);
+    mdetarray.set_origin_xpos(Origin_Xpos);
+    mdetarray.set_origin_xstd(Origin_Xstd);
+    mdetarray.set_origin_ypos(Origin_Ypos);
+    mdetarray.set_origin_ystd(Origin_Ystd);
+    mdetarray.set_origin_zpos(Origin_Zpos);
+    mdetarray.set_origin_zstd(Origin_Zstd);
+    mdetarray.set_origin_roll(Origin_Roll);
+    mdetarray.set_origin_rollstd(Origin_Rollstd);
+    mdetarray.set_origin_pitch(Origin_Pitch);
+    mdetarray.set_origin_pitchstd(Origin_Pitchstd);
+    mdetarray.set_origin_yaw(Origin_Yaw);
+    mdetarray.set_origin_yawstd(Origin_Yawstd);
+    mdetarray.set_origin_invalidflags(List_InvalidFlags);
+
+    mdetarray.set_list_radveldomain_min(List_RadVelDomain_Min);
+    mdetarray.set_list_radveldomain_max(List_RadVelDomain_Max);
+    mdetarray.set_list_numofdetections(List_NumOfDetections);
+    mdetarray.set_aln_azimuthcorrection(Aln_AzimuthCorrection);
+    mdetarray.set_aln_elevationcorrection(Aln_ElevationCorrection);
+    switch (Aln_Status) {
+    case 0:
+        mdetarray.set_aln_status(iv::radar::radar4ddetectarray::ALIGNMENT_INIT);
+        break;
+    case 1:
+        mdetarray.set_aln_status(iv::radar::radar4ddetectarray::ALIGNMENT_OK);
+        break;
+    default:
+        mdetarray.set_aln_status(iv::radar::radar4ddetectarray::ALIGNMENT_NOTOK);
+        break;
+    }
 
 }
 
@@ -129,3 +257,9 @@ void ars548pac::DecodeObj()
 {
 
 }
+
+void ars548pac::SetMotoData(char * pdata, int npos, char * pvalue,int nvaluelen)
+{
+    memcpy(pvalue,pdata+npos,nvaluelen);
+    mototointel_value(pvalue,nvaluelen);
+}

src/driver/driver_radar_4d_ars548/ar548pac.h

@@ -27,7 +27,7 @@ struct ars548pac_Payload
     unsigned int SOC;
     unsigned int DataID;
     unsigned int nDataLen;
-    std::shared_ptr<char> pPayload_ptr;
+    std::shared_ptr<char > pPayload_ptr;
 };
 
 }
@@ -66,7 +66,8 @@ public:
 
 
 
-public:
+
+private:
     void mototointel_unsignedshort(unsigned short &value);
     void mototointel_unsignedint(unsigned int &value);
     void mototointel_value(char * pvalue, int nlen);
@@ -74,6 +75,8 @@ public:
     void DecodeDetect();
     void DecodeObj();
 
+    void SetMotoData(char * pdata, int npos, char * pvalue,int nvaluelen);
+
 };
 
 #endif // AR548PAC_H

src/include/proto/radar4ddetectarray.proto

@@ -9,21 +9,21 @@ message radar4ddetectarray
 {
   optional int64 mnsTime = 1; //nano seconds since 1970
   repeated radar4ddetect mdetarray = 2;
-  optional uint64 mCRC =3; //Checksum (E2E Profile 7) (Reserved)
-  optional uint32 mLength =4; //Len (E2E Profile 7) (Reserved)
-  optional uint32 mSOC =5; //SQC (E2E Profile 7) (Reserved)
-  optional uint32 mDataID =6; //Data ID (E2E Profile 7) (Reserved)
-  optional uint32 mTimestamp_ns =7; //Timestamp Nanoseconds
-  optional uint32 mTimestamp_s =8; //Timestamp Seconds
+  optional uint64 CRC =3; //Checksum (E2E Profile 7) (Reserved)
+  optional uint32 Length =4; //Len (E2E Profile 7) (Reserved)
+  optional uint32 SOC =5; //SQC (E2E Profile 7) (Reserved)
+  optional uint32 DataID =6; //Data ID (E2E Profile 7) (Reserved)
+  optional uint32 Timestamp_ns =7; //Timestamp Nanoseconds
+  optional uint32 Timestamp_s =8; //Timestamp Seconds
   enum Timestamp_SyncStatus_Type{
   		SYNC_OK = 1;
   		SYNC_NEVERSYNC = 2;
   		SYNC_LOST = 3;
   	}
-  optional Timestamp_SyncStatus_Type mTimestamp_SyncStatus =9; //Timestamp_SyncStatus
-  optional uint32 mEventDataQualifier =10; //Event Data Qualifier (unused)
-  optional uint32 mExtendedQualifier = 11; //Extended Qualifier (unused)
-  optional uint32 mOrigin_InvalidFlags = 12; //Sensor Position Invalid flags (unused)
+  optional Timestamp_SyncStatus_Type Timestamp_SyncStatus =9; //Timestamp_SyncStatus
+  optional uint32 EventDataQualifier =10; //Event Data Qualifier (unused)
+  optional uint32 ExtendedQualifier = 11; //Extended Qualifier (unused)
+  optional uint32 Origin_InvalidFlags = 12; //Sensor Position Invalid flags (unused)
   optional float Origin_Xpos = 13; //Sensor X Position with reference to rear axle,m
   optional float Origin_Xstd = 14; //Sensor X Position STD (unused),m
   optional float Origin_Ypos = 15; //Sensor Y Position with reference to rear axle,m
@@ -36,16 +36,16 @@ message radar4ddetectarray
   optional float Origin_Pitchstd =22; //Sensor Pitch Angle STD,rad
   optional float Origin_Yaw =23; //Sensor Yaw Angle with alignment correction,rad
   optional float Origin_Yawstd = 24; //Sensor Yaw Angle STD,rad
-  optional uint32 mList_InvalidFlags =25; //Invalid flags (unused)
-  optional float mList_RadVelDomain_Min =26; //Ambiguity free Doppler velocity range Min,m/s
-  optional float mList_RadVelDomain_Max =27; //Ambiguity free Doppler velocity range Max,m/s
-  optional uint32 mList_NumOfDetections =28; //Number of Detections
-  optional float mAln_AzimuthCorrection =29; //Azimuth Alignment Correction
-  optional float mAln_ElevationCorrection =30; //Elevation Alignment Correction
+  optional uint32 List_InvalidFlags =25; //Invalid flags (unused)
+  optional float List_RadVelDomain_Min =26; //Ambiguity free Doppler velocity range Min,m/s
+  optional float List_RadVelDomain_Max =27; //Ambiguity free Doppler velocity range Max,m/s
+  optional uint32 List_NumOfDetections =28; //Number of Detections
+  optional float Aln_AzimuthCorrection =29; //Azimuth Alignment Correction
+  optional float Aln_ElevationCorrection =30; //Elevation Alignment Correction
   enum Aln_Status_Type{
   		ALIGNMENT_INIT = 0;
   		ALIGNMENT_OK = 1;
   		ALIGNMENT_NOTOK = 2;
 	}
-  optional Aln_Status_Type mAln_Status =31; //Status of alignment
+  optional Aln_Status_Type Aln_Status =31; //Status of alignment
 };