ESR send modify sendata

src/detection/detection_radar_delphi_esr_send/main.cpp

@@ -36,8 +36,8 @@ static int16_t Radius_Curvature = 8191; //< optional signal, 8191 meter is defau
 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 uint16_t Radar_to_Rear_Axle = 370;//< 0-710 cm
+static uint16_t Car_Wheel_Base = 270;//< 0-710 cm
 static uint8_t Car_Steering_Gear_Ratio = 18;//< 0-31.875
 /******************************************/
 
@@ -72,6 +72,8 @@ void ExecSend(uint16_t id, unsigned char canData[8],int size)
     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);
@@ -107,7 +109,7 @@ void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned i
         givlog->warn("detection_radar_delphi_esr_send--Listengpsimu parse errror. nSize is %d",nSize);
         return;
     }
-    Vehicle_Yaw_Rate = xgpsimu.gyro_z();
+    Vehicle_Yaw_Rate = xgpsimu.gyro_z() * -1;
     Vehicle_Speed = sqrt(xgpsimu.vn()*xgpsimu.vn() + xgpsimu.ve()*xgpsimu.ve() + xgpsimu.vd()*xgpsimu.vd());
 }
 
@@ -144,7 +146,7 @@ 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;
 
@@ -171,13 +173,14 @@ void signalHandler(int num)
 
     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.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);
-    qDebug()<<QTime::currentTime();
 }
 
 /*****************************************/
@@ -530,12 +533,12 @@ int main(int argc, char *argv[])
 
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
-    std::string strmemcan = xp.GetParam("canrecv","canrecv0");
-    std::string strmemsend = xp.GetParam("cansend","cansend0");
+    std::string strmemcan = xp.GetParam("canrecv","canrecv1");
+    std::string strmemsend = xp.GetParam("cansend","cansend1");
     std::string strmemradar = xp.GetParam("radar","radar");
     std::string strmodulename = xp.GetParam("modulename","detection_radar_delphi_esr");
 #ifdef fujiankuan
-    gstrmemgpsimu = xp.GetParam("gpsimu_name","hcp_gpsimu");
+    gstrmemgpsimu = xp.GetParam("gpsimu_name","hcp2_gpsimu");
     gstrmemcancar = xp.GetParam("cancar","can_car");
 #endif