esr send logic

src/detection/detection_radar_delphi_esr_send/main.cpp

@@ -64,19 +64,16 @@ int gnRadarState = 0;
 static int gnIndex = 0;
 
 //static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
-static unsigned char canbyte[8];
 
-void ExecSend(uint16_t id, unsigned char canData[8],int size)
+void ExecSend()
 {
     iv::can::canmsg xmsg;
     iv::can::canraw xraw;
-
-    memcpy(canbyte,canData,size);
 //    for(int i = 0 ; i< 8;i++)
 //        qDebug("%x",canbyte[i]);
 
-    xraw.set_id(id);
-    xraw.set_data(canbyte,8);
+    xraw.set_id(0x4F0);
+    xraw.set_data(canData4f0.byte,8);
     xraw.set_bext(false);
     xraw.set_bremote(false);
     xraw.set_len(8);
@@ -84,6 +81,38 @@ void ExecSend(uint16_t id, unsigned char canData[8],int size)
     pxraw->CopyFrom(xraw);
     xmsg.set_channel(1);
     xmsg.set_index(gnIndex);
+
+    xraw.set_id(0x4F1);
+    xraw.set_data(canData4f1.byte,8);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(8);
+    pxraw = xmsg.add_rawmsg();
+    pxraw->CopyFrom(xraw);
+    xmsg.set_channel(1);
+    xmsg.set_index(gnIndex);
+
+    xraw.set_id(0x5F2);
+    xraw.set_data(canData5f2.byte,8);
+    xraw.set_bext(false);
+
+    xraw.set_bremote(false);
+    xraw.set_len(8);
+    pxraw = xmsg.add_rawmsg();
+    pxraw->CopyFrom(xraw);
+    xmsg.set_channel(1);
+    xmsg.set_index(gnIndex);
+
+    xraw.set_id(0x5F4);
+    xraw.set_data(canData5f4.byte,8);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(8);
+    pxraw = xmsg.add_rawmsg();
+    pxraw->CopyFrom(xraw);
+    xmsg.set_channel(1);
+    xmsg.set_index(gnIndex);
+
     gnIndex++;
     xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
     int ndatasize = xmsg.ByteSize();
@@ -146,12 +175,9 @@ void signalHandler(int num)
     canData4f0.bit.canYawRateH = yawRate >> 8;
     canData4f0.bit.canYawRateL = yawRate & 0xFF;
     canData4f0.bit.canYawRateValid = 0x01;
-qDebug("speed %x %f, yaw %x %f",speed,Vehicle_Speed,yawRate,Vehicle_Yaw_Rate);
     canData4f0.bit.canRadiusCurvatureH = Radius_Curvature >> 8;
     canData4f0.bit.canRadiusCurvatureL = Radius_Curvature & 0x3F;
 
-    ExecSend(0x4F0,canData4f0.byte,8);
-
     canData4f1.bit.canScanIndexAckH = CAN_Scan_Index >> 8;
     canData4f1.bit.canScanIndexAckL = CAN_Scan_Index & 0xFF;
 
@@ -161,26 +187,20 @@ qDebug("speed %x %f, yaw %x %f",speed,Vehicle_Speed,yawRate,Vehicle_Yaw_Rate);
     canData4f1.bit.canGroupingMode = 0x03;
     canData4f1.bit.canVehicleSpeedValid = 0x01;
 
-    ExecSend(0x4F1,canData4f1.byte,8);
-
     canData5f2.bit.canRadarFovLrH = Radar_FOV_Long_Range >> 1;
     canData5f2.bit.canRadarFovLrL = Radar_FOV_Long_Range & 0x01;
     canData5f2.bit.canRadarFovMr = Radar_FOV_Mid_Range & 0x7F;
 
     canData5f2.bit.canRadarHeight = Radar_Height & 0x7F;
-
     canData5f2.bit.canAalignAvgCtrTotal = 0x03;
 
-    ExecSend(0x5F2,canData5f2.byte,8);
-
     canData5f4.bit.canBeamwidthVert = 0x47;
     canData5f4.bit.canDistanceRearAxle = (uint8_t)((Radar_to_Rear_Axle - 200) / 2.0);
 //    qDebug()<<canData5f4.bit.canDistanceRearAxle;
     canData5f4.bit.canWheelBase = (uint8_t)((Car_Wheel_Base - 200) / 2.0);
     canData5f4.bit.canSteeringGearRatio = (uint8_t)(Car_Steering_Gear_Ratio / 0.125);
 
-
-    ExecSend(0x5F4,canData5f4.byte,8);
+    ExecSend();
 }
 
 /*****************************************/