change driver_radar_4d_ars548. add object decode. not complete.

src/driver/driver_radar_4d_ars548/ar548pac.cpp

@@ -141,7 +141,7 @@ void ars548pac::DecodeDetect()
     memcpy(&CRC,mPayload.CRC,8);
     mdetarray.set_crc(CRC);
     mdetarray.set_length(mPayload.Length);
-    mdetarray.set_soc(mPayload.SOC);
+    mdetarray.set_sqc(mPayload.SOC);
     mdetarray.set_dataid(mPayload.DataID);
 
 
@@ -197,6 +197,119 @@ void ars548pac::DecodeDetect()
     SetMotoData(ppayload,nPos, (char *)&Origin_Yawstd,4); nPos = nPos + 4;
     SetMotoData(ppayload,nPos, (char *)&List_InvalidFlags,1); nPos = nPos + 1;
 
+    int j;
+    int nPos2 = nPos;
+    for(j=0;j<800;j++)
+    {
+        float fAngle;
+        float fAngleStd;
+        unsigned char InvalidFlags;
+        float EleAngle;
+        float EleAngleStd;
+        float fRadialDis;
+        float fRadialDisStd;
+        float fRadialVel;
+        float fRadialVelStd;
+        unsigned char DetRCS;
+        unsigned short DetID;
+        unsigned char ExistProb;
+        unsigned char DetClass;
+        unsigned char MultiProb;
+        unsigned short AssocObjID;
+        unsigned char ProbVelAmbig;
+        unsigned short tbd;
+        SetMotoData(ppayload,nPos2,(char *)&fAngle,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&fAngleStd,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&InvalidFlags,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&EleAngle,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&EleAngleStd,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&fRadialDis,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&fRadialDisStd,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&fRadialVel,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&fRadialVelStd,4);nPos2 = nPos2 + 4;
+        SetMotoData(ppayload,nPos2,(char *)&DetRCS,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&DetID,2);nPos2 = nPos2 + 2;
+        SetMotoData(ppayload,nPos2,(char *)&ExistProb,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&DetClass,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&MultiProb,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&AssocObjID,2);nPos2 = nPos2 + 2;
+        SetMotoData(ppayload,nPos2,(char *)&ProbVelAmbig,1);nPos2 = nPos2 + 1;
+        SetMotoData(ppayload,nPos2,(char *)&tbd,2);nPos2 = nPos2 + 2;
+        iv::radar::radar4ddetect xdet;
+        xdet.set_unaligned_detection_azimuth_angle(fAngle);
+        xdet.set_azimuth_angle_std(fAngleStd);
+        switch (InvalidFlags) {
+        case 1:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InValidDistance);
+            break;
+        case 0x02:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidDistanceStd);
+            break;
+        case 0x04:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidAzimuth);
+            break;
+        case 0x08:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidAzimuthStd);
+            break;
+        case 0x10:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidElevation);
+            break;
+        case 0x20:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidElevationStd);
+            break;
+        case 0x40:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidRangeRate);
+            break;
+        case 0x80:
+            xdet.set_detection_invalid_flags(iv::radar::radar4ddetect::InvalidRangeRateStd);
+            break;
+        default:
+            std::cout<<" Detection Invalid Flags error.  flag value: "<<InvalidFlags<<std::endl;
+            break;
+        }
+        xdet.set_unaligned_detection_elevation_angle(EleAngle);
+        xdet.set_elevation_angle_std(EleAngleStd);
+        xdet.set_detection_radial_distance(fRadialDis);
+        xdet.set_radial_distance_std(fRadialDisStd);
+        xdet.set_detection_radial_velocity(fRadialVel);
+        xdet.set_radial_velocity_std(fRadialDisStd);
+        xdet.set_detecion_rcs(DetRCS);
+        xdet.set_detection_id(DetID);
+        switch (DetClass) {
+        case 0:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::NoClassification);
+            break;
+        case 1:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::Noise);
+            break;
+        case 2:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::Ground);
+            break;
+        case 3:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::TraversableUnder);
+            break;
+        case 4:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::Obstacle);
+            break;
+        case 255:
+            xdet.set_detection_classification(iv::radar::radar4ddetect::Invalid);
+            break;
+        default:
+            std::cout<<" Dectection Class Inavlid: valid: "<<DetClass<<std::endl;
+            xdet.set_detection_classification(iv::radar::radar4ddetect::Invalid);
+            break;
+        }
+        xdet.set_existence_probability(ExistProb);
+        xdet.set_multi_target_probability(MultiProb);
+        xdet.set_associated_objectid(AssocObjID);
+        xdet.set_probability_for_resolved_velocity_ambiguity(ProbVelAmbig);
+        xdet.set_tbd(tbd);
+
+        iv::radar::radar4ddetect * pdet =  mdetarray.add_mdet();
+        pdet->CopyFrom(xdet);
+
+    }
+
     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;
@@ -256,7 +369,103 @@ void ars548pac::DecodeDetect()
 
 void ars548pac::DecodeObj()
 {
+    uint64_t CRC;
+    memcpy(&CRC,mPayload.CRC,8);
+    mobjarray.set_crc(CRC);
+    mobjarray.set_length(mPayload.Length);
+    mobjarray.set_sqc(mPayload.SOC);
+    mobjarray.set_dataid(mPayload.DataID);
+
+
+    int ndatalen = mPayload.nDataLen;
+    (void)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 char ObjectList_NumOfObject;
+
+    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 *)&ObjectList_NumOfObject,1); nPos = nPos + 1;
 
+    int nPos2 = nPos;
+    int i;
+    for(i=0;i<50;i++)
+    {
+        unsigned short u_StatusSensor; SetMotoData(ppayload,nPos2,(char *)&u_StatusSensor,2);nPos2 = nPos2 + 2;
+        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 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_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_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_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;
+        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_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;
+        unsigned char u_Classification_Animal;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Animal,1);nPos2 = nPos2 + 1;
+        unsigned char u_Classification_Hazard;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Hazard,1);nPos2 = nPos2 + 1;
+        unsigned char u_Classification_Unknown;SetMotoData(ppayload,nPos2,(char *)&u_Classification_Unknown,1);nPos2 = nPos2 + 1;
+        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_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_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;
+        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_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;
+        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_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;
+        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_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;
+        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;
+        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;
+        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 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;
+    }
 }
 
 void ars548pac::SetMotoData(char * pdata, int npos, char * pvalue,int nvaluelen)

src/include/proto/radar4ddetect.proto

@@ -4,8 +4,8 @@ package iv.radar;
 
 message radar4ddetect
 {
-  optional float mfAziAngle = 1; //Unaligned Detection Azimuth Angle，rad
-  optional float mfAziAngleStd = 2; //Azimuth Angle Std(Detection目标方位角标准差)，rad
+  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{
             InValidDistance = 1;
             InvalidDistanceStd = 2;
@@ -16,16 +16,16 @@ message radar4ddetect
             InvalidRangeRate = 64;
             InvalidRangeRateStd = 128;
         }
-  optional DetectionInvalidFlagsType mdetinvalidtype = 3; //Detection Invalid Flags
-  optional float mfEleAngle = 4; //Unaligned Detection Elevation Angle,rad
-  optional float mfEleAngleStd = 5; //Elevation Angle Std,rad
-  optional float mfRadialDis = 6; //Detection Radial Distance,m
-  optional float mfRadialDisStd =7; //Radial Distance Std,m
-  optional float mfRadialVel =8; //Detection Radial Velocity,m/s
-  optional float mfRadialVelStd =9; //Radial Velocity Std,m/s
-  optional int32 mDectectionRCS =10; //Detecition RCS,dBm²
-  optional uint32 mID =11; //Detection ID
-  optional uint32 mnProb =12; //Existence Probability, 0~100%
+  optional DetectionInvalidFlagsType Detection_Invalid_Flags = 3; //Detection Invalid Flags
+  optional float Unaligned_Detection_Elevation_Angle = 4; //Unaligned Detection Elevation Angle,rad
+  optional float Elevation_Angle_Std = 5; //Elevation Angle Std,rad
+  optional float Detection_Radial_Distance = 6; //Detection Radial Distance,m
+  optional float Radial_Distance_Std =7; //Radial Distance Std,m
+  optional float Detection_Radial_Velocity =8; //Detection Radial Velocity,m/s
+  optional float Radial_Velocity_Std =9; //Radial Velocity Std,m/s
+  optional int32 Detecion_RCS =10; //Detecition RCS,dBm²
+  optional uint32 Detection_ID =11; //Detection ID
+  optional uint32 Existence_Probability =12; //Existence Probability, 0~100%
   enum DetectionClassficationType{
   		NoClassification = 0;
   		Noise = 1;
@@ -34,11 +34,11 @@ message radar4ddetect
   		Obstacle = 4;
   		Invalid = 5;
   	}
-  optional DetectionClassficationType mClass = 13; //Detection Classification
-  optional uint32 mnMultiTargetProb =14; //Multi-Target Probability, 0~100%
-  optional uint32 mnAssocObjID =15; //Associated Object ID
-  optional uint32 mnProbVelAmbig =16; //Probability for resolved velocity ambiguity, 0~100%
-  optional uint32 mntbd =17; //tbd
+  optional DetectionClassficationType Detection_Classification = 13; //Detection Classification
+  optional uint32 Multi_Target_Probability =14; //Multi-Target Probability, 0~100%
+  optional uint32 Associated_ObjectID =15; //Associated Object ID
+  optional uint32 Probability_for_resolved_velocity_ambiguity =16; //Probability for resolved velocity ambiguity, 0~100%
+  optional uint32 tbd =17; //tbd
 
 };
 

src/include/proto/radar4ddetectarray.proto

@@ -8,10 +8,10 @@ import "radar4ddetect.proto";
 message radar4ddetectarray
 {
   optional int64 mnsTime = 1; //nano seconds since 1970
-  repeated radar4ddetect mdetarray = 2;
+  repeated radar4ddetect mdet = 2;
   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 SQC =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