feat(radar_srr and proto):add valid level to srr object

src/detection/detection_radar_delphi_srr/main.cpp

@@ -105,7 +105,7 @@ void ShareResult()
 
 void DecodeRadar(iv::can::canmsg xmsg)
 {
-    int32_t range, rate, angle,range_right,rate_right,angle_right;
+    int16_t range, rate, angle,range_right,rate_right,angle_right;
     if(xmsg.rawmsg_size() < 1)return;
 
     int i;
@@ -115,41 +115,25 @@ void DecodeRadar(iv::can::canmsg xmsg)
 
         if((canmsg.id() >= 0x500)&&(canmsg.id() <= 0x53f))
         {
-            int  data[8];
+            unsigned char  data[8];
             unsigned char cdata[8];
             memcpy(cdata,canmsg.data().data(),8);
 
             int radar_ID_index = canmsg.id()-0x500 ;
+            unsigned int detect_valid_level = 0;
+            bool detect_status = false;
+            unsigned int object_power = 0;
 
-            int j;
-            for(j=0;j<8;j++)data[j] = cdata[j];
+            for(int j=0;j<8;j++)data[j] = cdata[j];
 ////////////////////////////////RSDS_PCAN_v18.dbc
-            angle = (data[2]  << 8) + data[3];
-            range = (data[4]  << 8) + data[5];
-            rate =  (data[6]  << 8) + data[7];
-            if (angle & 0x8000) {
-                angle = angle | 0xFFFF0000;
-            }
-            if (rate & 0x8000) {
-                rate = rate | 0xFFFF0000;
-            }
-            if (range & 0x8000) {
-                range = range | 0xFFFF0000;
-           }
-///////////////////////////////RSDS_PCAN_v18.dbc
-
-//                       angle = (data[2]  << 8) + data[3];
-//                       range = (data[4]  << 3) + (((data[5])&(0xE0)) >>5);
-//                       rate =  ((data[5]&(0x1F))  << 9) + (data[6]<<1)+((data[7]&(0x80))>>7);
-//                       if (angle & 0x8000) {
-//                           angle = angle | 0xFFFF0000;
-//                       }
-//                       if (rate & 0x2000) {
-//                           rate = rate | 0xFFFFC000;
-
-//                       }
-
+            detect_valid_level = (data[0] >> 5) & 0x07;
+            detect_status = (data[0] >> 4) & 0x01;
+            object_power = ((((data[0] & 0x0F) << 8) | data[1]) << 4) >> 4;
+            angle = (data[2]  << 8) | data[3];
+            range = (data[4]  << 8) | data[5];
+            rate =  (data[6]  << 8) | data[7];
 
+///////////////////////////////RSDS_PCAN_v18.dbc
 
             //If angle and range both are 0, it is an invalid data.
             //qDebug("range = %d angle = %d ",range,angle);
@@ -157,7 +141,7 @@ void DecodeRadar(iv::can::canmsg xmsg)
             angle=angle*0.0078125;
             rate=rate*0.0078125;
 //            qDebug("range = %d angle = %d ",range,angle);
-            if (angle != 0 || range != 0) {
+            if ((angle != 0 || range != 0) && detect_status != 0 && detect_valid_level > 1) {
                 iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
                 pobj->set_bvalid(true);
                 pobj->set_x(range  * sin(angle  / 180.0 * M_PI));
@@ -165,12 +149,13 @@ void DecodeRadar(iv::can::canmsg xmsg)
                 pobj->set_vel(rate );
                 pobj->set_vx(pobj->vel() * sin(angle / 180.0 * M_PI));
                 pobj->set_vy(pobj->vel() * cos(angle / 180.0 * M_PI));
+                pobj->set_power(object_power);
+                pobj->set_detect_status(detect_status);
+                pobj->set_detect_valid_level(detect_valid_level);
                // pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
 
-
                // qDebug("x = %d angle = %d ",pobj->vel());
 
-
             }
             else {
                 iv::radar::radarobject * pobj = gobj.mutable_obj(radar_ID_index);
@@ -207,33 +192,29 @@ void DecodeRadar(iv::can::canmsg xmsg)
 ///////////////////////////////////////////radra_right
         if((canmsg.id() >= 0x600)&&(canmsg.id() <= 0x63f))
         {
-            int  data[8];
+            unsigned char  data[8];
             unsigned char cdata[8];
             memcpy(cdata,canmsg.data().data(),8);
 
-            int radar_ID_index = canmsg.id()-0x600 ;
+            int radar_ID_index = canmsg.id()-0x600;
+            unsigned int detect_valid_level = 0;
+            bool detect_status = false;
+            unsigned int object_power = 0;
 
-            int j;
-            for(j=0;j<8;j++)data[j] = cdata[j];
+            for(int j=0;j<8;j++)data[j] = cdata[j];
 ////////////////////////////////RSDS_PCAN_v18.dbc
-            angle_right = (data[2]  << 8) + data[3];
-            range_right = (data[4]  << 8) + data[5];
-            rate_right  =  (data[6]  << 8) + data[7];
-            if (angle_right & 0x8000) {
-                angle_right = angle_right | 0xFFFF0000;
-            }
-            if (rate_right & 0x8000) {
-                rate_right = rate_right | 0xFFFF0000;
-            }
-            if (range_right & 0x8000) {
-                range_right = range_right | 0xFFFF0000;
-           }
+            detect_valid_level = (data[0] >> 5) & 0x07;
+            detect_status = (data[0] >> 4) & 0x01;
+            object_power = ((((data[0] & 0x0F) << 8) | data[1]) << 4) >> 4;
+            angle_right = (data[2]  << 8) | data[3];
+            range_right = (data[4]  << 8) | data[5];
+            rate_right  =  (data[6]  << 8) | data[7];
 
             range_right=range_right*0.0078125;
             angle_right=angle_right*0.0078125;
             rate_right=rate_right*0.0078125;
 //            qDebug("range_right = %d angle_right = %d ",range_right,angle_right);
-            if (angle_right != 0 || range_right != 0) {
+            if ((angle_right != 0 || range_right != 0) && detect_status != 0 && detect_valid_level > 1) {
                 iv::radar::radarobject * pobj = gobjright.mutable_obj(radar_ID_index);
                 pobj->set_bvalid(true);
                 pobj->set_x(range_right  * sin(angle_right  / 180.0 * M_PI));
@@ -241,6 +222,9 @@ void DecodeRadar(iv::can::canmsg xmsg)
                 pobj->set_vel(rate_right );
                 pobj->set_vx(pobj->vel() * sin(angle_right/ 180.0 * M_PI));
                 pobj->set_vy(pobj->vel() * cos(angle_right / 180.0 * M_PI));
+                pobj->set_power(object_power);
+                pobj->set_detect_status(detect_status);
+                pobj->set_detect_valid_level(detect_valid_level);
                // pobj->set_rectime(QDateTime::currentMSecsSinceEpoch());
 
             }

src/include/proto/radarobject.proto

@@ -25,6 +25,8 @@ message radarobject
   optional bool Fast_Movable =19; //快速运动
   optional bool Slow_Movable =20; //慢速运动
   optional int32 Power =21; //信号反射强度，单位dB
+  optional int32 Detect_Valid_Level = 22;//srr目标置信度
+  optional bool Detect_Status = 23;//srr目标检测状态，为1目标有效，置信度有意义
 
 };