fix(radar_esr_send):fix radar broken by send massage to it (wait to test)

src/detection/detection_radar_delphi_esr_send/can_send_data_struct.h

@@ -53,13 +53,13 @@ struct canBit4F1
     //5
     unsigned char canLateralMountingOffset : 8;
     //6
-    unsigned char canMaximumTracks : 6; // <64
+    unsigned char canMaximumTracks : 6; //< max 64
     unsigned char canBlockageDisable : 1;
-    unsigned char canRadarCmdRadiate : 1; //<1
+    unsigned char canRadarCmdRadiate : 1; //< 1
     //7
     unsigned char canRawDataEnable : 1;
     unsigned char canWiperStatus : 1;
-    unsigned char canGroupingMode : 2;//<3
+    unsigned char canGroupingMode : 2;//< 3
     unsigned char canMmrUpsideDown : 1;
     unsigned char canVehicleSpeedValid : 1;
     unsigned char canTurnSignalStatus : 2;
@@ -73,25 +73,25 @@ union canSend4F1
 struct canBit5F2
 {
     //0
-    unsigned char canLatAccH : 5;
+    unsigned char canLatAccH : 6;
     unsigned char canLatAAccValid : 1;
     unsigned char canLonAccValid : 1;
     //1
-    unsigned char canLongAccH : 5;
+    unsigned char canLonAccH : 5;
     unsigned char canLatAccL : 3;
     //2
     unsigned char canRadarFovLrH : 4;
-    unsigned char canongAccL: 4;
+    unsigned char canLonAccL: 4;
     //3
     unsigned char canRadarFovMr : 7;
-    unsigned char canRadarFovLr : 1;
+    unsigned char canRadarFovLrL : 1;
     //4
     unsigned char canRadarHeight : 7;
     unsigned char canAutoAlignDisable : 1;
     //5
     unsigned char canWheelSlip : 2;
     unsigned char canAutoAlignConverged : 1;
-    unsigned char canAalignAvgCtrTotal : 3;
+    unsigned char canAalignAvgCtrTotal : 3; //< defalt 0x03
     unsigned char canServAlignEn : 1;
     unsigned char canServAlignType : 1;
     //6
@@ -110,7 +110,7 @@ struct canBit5F4
     //0
     unsigned char canOversteerUndersteer : 8;
     //1
-    unsigned char canBeamwidthVert : 7;
+    unsigned char canBeamwidthVert : 7; // default 0x71
     unsigned char canYawRateBiasShift : 1;
     //2
     unsigned char canFunnelOffsetLeft : 8;

src/detection/detection_radar_delphi_esr_send/main.cpp

@@ -27,14 +27,18 @@
 
 std::string gstrmemgpsimu;
 std::string gstrmemcancar;
+
 static uint16_t CAN_Scan_Index;
 static double Vehicle_Yaw_Rate;
-
-static double Vehicle_Speed;        //<
-static uint8_t CAN_Vehicle_Speed_Valid; //<(1)valid
-
-static double Steering_Angle;
-static int16_t CAN_Radius_Curvature = 8191;
+static double Vehicle_Speed;        //< m/s
+static double Steering_Angle; //< optional signal, ready to use
+static int16_t Radius_Curvature = 8191; //< optional signal, 8191 meter is default
+static uint8_t Radar_FOV_Long_Range = 30; //< 0-30 degree
+static uint8_t Radar_FOV_Mid_Range = 120; //< 0-120 degree
+static uint8_t Radar_Height = 50;//< 0-125 cm
+static uint8_t Radar_to_Rear_Axle = 370;//< 0-710 cm
+static uint8_t Car_Wheel_Base = 270;//< 0-710 cm
+static uint8_t Car_Steering_Gear_Ratio = 18;//< 0-31.875
 /******************************************/
 
 canSend4F0 canData4f0;
@@ -56,13 +60,13 @@ QTime gTime;
 int gnRadarState = 0;
 
 #ifdef fujiankuan
-/****==================================================================================***/
+/***==================================================================================***/
 static int gnIndex = 0;
 
 //static unsigned char tmp1[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0xBF,0x00};
 static unsigned char canbyte[8];
 
-void ExecSend1(uint16_t id, unsigned char canData[8],int size)
+void ExecSend(uint16_t id, unsigned char canData[8],int size)
 {
     iv::can::canmsg xmsg;
     iv::can::canraw xraw;
@@ -104,7 +108,7 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
     Vehicle_Yaw_Rate = xgpsimu.gyro_z();
-    Vehicle_Speed = sqrt(pow(xgpsimu.vn(),2) + pow(xgpsimu.ve(),2));
+    Vehicle_Speed = sqrt(xgpsimu.vn()*xgpsimu.vn() + xgpsimu.ve()*xgpsimu.ve() + xgpsimu.vd()*xgpsimu.vd());
 }
 
 void Listencancar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -129,19 +133,50 @@ void signalHandler(int num)
 {
     unsigned int speed = (unsigned int)(Vehicle_Speed / 0.0625);
     int yawRate = (int)(Vehicle_Yaw_Rate / 0.0625);
+
     canData4f0.bit.canVehicleSpeedH = speed >> 8;
     canData4f0.bit.canVehicleSpeedL = speed & 0xFF;
+    if(Vehicle_Speed >= 0.0)
+        canData4f0.bit.can_VehicleSpeedDirection = 0x00;
+    else
+        canData4f0.bit.can_VehicleSpeedDirection = 0x01;
+
     canData4f0.bit.canYawRateH = yawRate >> 8;
     canData4f0.bit.canYawRateL = yawRate & 0xFF;
-    canData4f0.bit.canRadiusCurvatureH = CAN_Radius_Curvature >> 8;
-    canData4f0.bit.canRadiusCurvatureL = CAN_Radius_Curvature & 0x3F;
-    ExecSend1(0x4F0,canData4f0.byte,8);
+    canData4f0.bit.canYawRateValid = 0x01;
+
+    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;
-    ExecSend1(0x4F1,canData4f1.byte,8);
-    ExecSend1(0x5F2,canData5f2.byte,8);
-    ExecSend1(0x5F4,canData5f4.byte,8);
+
+    canData4f1.bit.canMaximumTracks = 0x3F;
+    canData4f1.bit.canRadarCmdRadiate = 0x01;
+
+    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 = 0x71;
+    canData5f4.bit.canDistanceRearAxle = (uint8_t)(Radar_to_Rear_Axle / 2.0);
+    canData5f4.bit.canWheelBase = (uint8_t)(Car_Wheel_Base / 2.0);
+    canData5f4.bit.canSteeringGearRatio = (uint8_t)(Car_Steering_Gear_Ratio / 0.125);
+
+    ExecSend(0x5F4,canData5f4.byte,8);
     qDebug()<<QTime::currentTime();
 }
 
@@ -396,7 +431,6 @@ void DecodeRadar(iv::can::canmsg xmsg)
             gobj.set_mstime(QDateTime::currentMSecsSinceEpoch());
             ShareResult();
             gntemp = 0;
-
         }
 
         if(gntemp > 100)