fix(driver_radar_ARS408):fix error when exit the process by ctrl+c. add rotation and translation function.

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.cpp

@@ -50,7 +50,7 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
     switch (can_id) {
@@ -122,13 +122,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
     case 0x60B:
         tempObjID = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DynProp);
@@ -148,9 +152,15 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
         break;
     case 0x60D:
         tempObjID = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Width_phys);
@@ -165,11 +175,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x211:
+    switch (can_id - 0x010) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Interference;
         temperature_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Temperature_Error;
@@ -178,13 +188,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
         speed_Loss = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch1_rx.RadarState_ch1.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x710:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x61A:
+    case 0x60A:
         if(ars408_can_database_ch1_rx.Obj_0_Status_ch1.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -227,28 +237,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x61B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_RCS_phys);
         break;
-    case 0x61C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_DistLat_rms));
@@ -260,11 +275,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ProbOfExist));
         break;
-    case 0x61D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Width_phys);
@@ -279,11 +300,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x221:
+    switch (can_id - 0x020) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Interference;
         temperature_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Temperature_Error;
@@ -292,13 +313,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
         speed_Loss = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch2_rx.RadarState_ch2.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x720:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x62A:
+    case 0x60A:
         if(ars408_can_database_ch2_rx.Obj_0_Status_ch2.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -341,28 +362,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x62B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_RCS_phys);
         break;
-    case 0x62C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_DistLat_rms));
@@ -374,11 +400,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ProbOfExist));
         break;
-    case 0x62D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Width_phys);
@@ -393,11 +425,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x231:
+    switch (can_id - 0x030) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Interference;
         temperature_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Temperature_Error;
@@ -406,13 +438,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
         speed_Loss = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch3_rx.RadarState_ch3.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x730:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x63A:
+    case 0x60A:
         if(ars408_can_database_ch3_rx.Obj_0_Status_ch3.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -455,28 +487,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x63B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_RCS_phys);
         break;
-    case 0x63C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_DistLat_rms));
@@ -488,11 +525,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ProbOfExist));
         break;
-    case 0x63D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Width_phys);
@@ -507,11 +550,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x241:
+    switch (can_id - 0x040) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Interference;
         temperature_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Temperature_Error;
@@ -520,13 +563,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
         speed_Loss = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch4_rx.RadarState_ch4.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x740:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x64A:
+    case 0x60A:
         if(ars408_can_database_ch4_rx.Obj_0_Status_ch4.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -569,28 +612,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x64B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_RCS_phys);
         break;
-    case 0x64C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_DistLat_rms));
@@ -602,11 +650,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ProbOfExist));
         break;
-    case 0x64D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Width_phys);
@@ -621,11 +675,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x251:
+    switch (can_id - 0x050) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Interference;
         temperature_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Temperature_Error;
@@ -634,13 +688,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
         speed_Loss = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch5_rx.RadarState_ch5.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x750:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x65A:
+    case 0x60A:
         if(ars408_can_database_ch5_rx.Obj_0_Status_ch5.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -683,28 +737,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x65B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_RCS_phys);
         break;
-    case 0x65C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_DistLat_rms));
@@ -716,11 +775,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ProbOfExist));
         break;
-    case 0x65D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Width_phys);
@@ -735,11 +800,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x261:
+    switch (can_id - 0x060) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Interference;
         temperature_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Temperature_Error;
@@ -748,13 +813,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
         speed_Loss = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch6_rx.RadarState_ch6.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x760:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x66A:
+    case 0x60A:
         if(ars408_can_database_ch6_rx.Obj_0_Status_ch6.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -797,28 +862,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x66B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_RCS_phys);
         break;
-    case 0x66C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_DistLat_rms));
@@ -830,11 +900,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ProbOfExist));
         break;
-    case 0x66D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Width_phys);
@@ -849,11 +925,11 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
     std::string major,minor,patch;
     iv::radar::radarobject *pobj;
     iv::radar::radarobject xradarObj_null;
-    double tempVLat,tempVLong;
+    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
     uint8_t tempObjID;
 
-    switch (can_id) {
-    case 0x271:
+    switch (can_id - 0x070) {
+    case 0x201:
         persistent_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Persistent_Error;
         interference_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Interference;
         temperature_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Temperature_Error;
@@ -862,13 +938,13 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
         speed_Loss = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_MotionRxState & 0x01;
         yaw_Rate_Loss = (ars408_can_database_ch7_rx.RadarState_ch7.RadarState_MotionRxState >> 1) & 0x01;
         break;
-    case 0x770:
+    case 0x700:
         major = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_MajorRelease);
         minor = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_MinorRelease);
         patch = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_PatchLevel);
         radar_Version = major + "." + minor + "." + patch;
         break;
-    case 0x67A:
+    case 0x60A:
         if(ars408_can_database_ch7_rx.Obj_0_Status_ch7.Obj_NofObjects == 0)
         {
             xradarObjArray.Clear();
@@ -911,28 +987,33 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
             radarObjArraySend_lock.lock();
             radarObjArray_send.CopyFrom(xradarObjArray);
             radarObjArraySend_lock.unlock();
+//            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
         //clear the obj vector and flag vector
         vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
         radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
         break;
-    case 0x67B:
+    case 0x60B:
         tempObjID = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_ID;
         radarObjFreshFlag[tempObjID] = true;
-        vradarObj[tempObjID].set_x(-ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLat_phys);
-        vradarObj[tempObjID].set_y(ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLong_phys);
-        tempVLat = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLat_phys;
-        tempVLong = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLong_phys;
-        vradarObj[tempObjID].set_vx(-tempVLat);
-        vradarObj[tempObjID].set_vy(tempVLong);
-        vradarObj[tempObjID].set_vel(sqrt(tempVLat*tempVLat+tempVLong*tempVLong));
+        tempDX = -ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLat_phys;
+        tempDY = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLong_phys;
+        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_x(tempDX);
+        vradarObj[tempObjID].set_y(tempDY);
+        tempVX = -ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLat_phys;
+        tempVY = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLong_phys;
+        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_vx(tempVX);
+        vradarObj[tempObjID].set_vy(tempVY);
+        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
         vradarObj[tempObjID].set_bvalid(true);
         vradarObj[tempObjID].set_id(tempObjID);
         vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DynProp);
         vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_RCS_phys);
         break;
-    case 0x67C:
+    case 0x60C:
         tempObjID = ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ID;
         vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_DistLong_rms));
         vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_DistLat_rms));
@@ -944,11 +1025,17 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
         vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_MeasState);
         vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ProbOfExist));
         break;
-    case 0x67D:
+    case 0x60D:
         tempObjID = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ID;
-        vradarObj[tempObjID].set_object_arellong(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLong_phys);
-        vradarObj[tempObjID].set_object_arellat(-ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLat_phys);
-        vradarObj[tempObjID].set_object_orientationangle(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_OrientationAngle_phys);
+        tempAX = -ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLat_phys;
+        tempAY = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLong_phys;
+        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
+        vradarObj[tempObjID].set_object_arellong(tempAY);
+        vradarObj[tempObjID].set_object_arellat(tempAX);
+        tempOri = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
+        if(tempOri > 180.0)tempOri = tempOri - 360.0;
+        if(tempOri < -180.0)tempOri = tempOri + 360.0;
+        vradarObj[tempObjID].set_object_orientationangle(tempOri);
         vradarObj[tempObjID].set_object_class(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Class);
         vradarObj[tempObjID].set_object_length(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Length_phys);
         vradarObj[tempObjID].set_object_width(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Width_phys);
@@ -1082,6 +1169,18 @@ double CANRecv_Consumer::Trans_ProbOfExist_Ro_To_Phy(uint8_t data)
     return phy;
 }
 
+void CANRecv_Consumer::Rotate_Translate(double &x,double &y,double theta,double offset_x,double offset_y) //RFU,deg
+{
+    double tempX = x;
+    double tempY = y;
+    double tempTheta = theta / 180.0 * M_PI;
+    double tempSinTheta = sin(tempTheta);
+    double tempCosTheta = cos(tempTheta);
+
+    x = (tempX * tempCosTheta + tempY * tempSinTheta) + offset_x;
+    y = (tempY * tempCosTheta - tempX * tempSinTheta) + offset_y;
+}
+
 void CANRecv_Consumer::run()
 {
     radarType = setupConfig.radarType;

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.h

@@ -38,6 +38,8 @@ protected:
     double Trans_DVA_RMS_Ro_To_Phy(uint8_t data);
     double Trans_Ori_RMS_Ro_To_Phy(uint8_t data);
     double Trans_ProbOfExist_Ro_To_Phy(uint8_t data);
+    //rotation and translation
+    void Rotate_Translate(double &x,double &y,double theta,double offset_x,double offset_y); //RFU,deg
 
 signals:
     void RadarObjectArray_Ready();

src/driver/driver_radar_continental_ARS408_SRR308/main.cpp

@@ -56,135 +56,9 @@ double GPSIMUVelocity = 0; // m/s
 double GPSIMUYawRate = 0; // deg/s
 QMutex GPSIMUDataLock;
 
-
-//------------------------------------
-
-iv::radar::radarobjectarray gobj;
-int gntemp = 0;
-
-static int gnNotSend = 10;
-
-static bool gbinit = false;
-
-iv::radar::radarobjectarray mradararray;
-
-void ShareResult()
-{
-    char * str = new char[mradararray.ByteSize()];
-    int nsize = mradararray.ByteSize();
-    if(mradararray.SerializeToArray(str,nsize))
-    {
-        iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,str,nsize);
-    }
-
-    givlog->verbose("obj size is %d ",mradararray.obj_size());
-
-//    qDebug("share time is %d ",gTime.elapsed());
-    delete str;
-}
-
-void ProcRadarMsg(iv::can::canraw xmsg)
-{
-    if(gbinit == false)gnNotSend = 10;
-
-    if(gnNotSend > 0)
-    {
-        givlog->verbose("send valid data.");
-        iv::can::canraw x;
-        x.set_id(0x200);
-        x.set_bext(false);
-        x.set_bremote(false);
-        x.set_len(8);
-        unsigned char strdata[8];
-        strdata[0] = 0x08;
-        strdata[4] = 0x08;
-        x.set_data(strdata,8);
-        iv::can::canmsg xsend;
-        iv::can::canraw * praw = xsend.add_rawmsg();
-        praw->CopyFrom(x);
-        int nsize = xsend.ByteSize();
-        char * str = new char[nsize];
-        if(xsend.SerializeToArray(str,nsize))
-        {
-            iv::modulecomm::ModuleSendMsg(shmRadar.mpa,str,nsize);
-        }
-        else
-        {
-
-        }
-        delete str;
-        gnNotSend--;
-    }
-    int i;
-    if(xmsg.id() == 0x60A)
-    {
-        gbinit = true;
-        mradararray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-        ShareResult();
-        mradararray.clear_obj();
-    }
-    if(xmsg.id() == 0x60B)
-    {
-        if(xmsg.len()<8)
-        {
-            std::cout<<"ProcRadarMsg can message length is small "<<xmsg.len()<<std::endl;
-            return;
-        }
-        char data[8];
-        memcpy(data,xmsg.data().data(),8);
-        unsigned char id;
-        id = data[0];
-        float y;
-        float x;
-        float vx,vy;
-
-        unsigned char a,b;
-        a = data[1];
-        b = data[2];
-        b = b&0xf8;
-        y = a;
-        y = y*256 + b;
-        y = y/8;
-        y = y*0.2 - 500;
-        a = data[2];b = data[3];
-        a = a&0x07;
-        x = a;
-        x = x*256 + b;
-        x = x*0.2 -204.6;
-        a = data[4];b = data[5];
-        b = b&0xc0;b = b/64;
-        vy = a; vy = vy*4 + b;
-        vy = vy *0.25 - 128;
-        a = data[5];b = data[6];
-        a = a&0x3f;b = b&0xe0;b = b/32;
-        vx = a; vx = vx*8 + b;
-        vx = vx *0.25 - 64;
-
-        iv::radar::radarobject xobj;
-        xobj.set_bvalid(true);
-        xobj.set_x(x*(-1));
-        xobj.set_y(y);
-        xobj.set_vx(vx*(-1));
-        xobj.set_vy(vy);
-        xobj.set_vel(vy);
-
-        iv::radar::radarobject * pxobj = mradararray.add_obj();
-        pxobj->CopyFrom(xobj);
-    }
-
-}
-
-void DecodeRadar(iv::can::canmsg xmsgvetor)
-{
-
-    int i;
-
-    for(i=0;i<xmsgvetor.rawmsg_size();i++)
-    {
-        ProcRadarMsg(xmsgvetor.rawmsg(i));
-    }
-
-}
+CANSend_Producer* can_send_producer;
+CANSend_Consumer* can_send_consumer;
+CANRecv_Consumer* can_recv_consumer;
 
 void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -246,7 +120,7 @@ void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsign
 void ExitFunc()
 {
     gApp->quit();
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
 }
 
 void signal_handler(int sig)
@@ -313,15 +187,19 @@ int main(int argc, char *argv[])
     shmChassis.mnBufferCount = 3;
     shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisMsg);
 
-    CANSend_Producer can_send_producer(&CANSend);
-    can_send_producer.start();
-    CANSend_Consumer can_send_consumer(&CANSend);
-    can_send_consumer.start();
-    CANRecv_Consumer can_recv_consumer(&CANRecv);
-    can_recv_consumer.start();
+    can_send_producer = new CANSend_Producer(&CANSend);
+    can_send_producer->start();
+    can_send_consumer = new CANSend_Consumer(&CANSend);
+    can_send_consumer->start();
+    can_recv_consumer = new CANRecv_Consumer(&CANRecv);
+    can_recv_consumer->start();
 
     int rtn = a.exec();
 
+    can_send_producer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
+    can_send_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
+    can_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
+
     if(gfault != nullptr)delete gfault;
     if(givlog != nullptr)delete givlog;
     if(shmCANRecv.mpa != nullptr)iv::modulecomm::Unregister(shmCANRecv.mpa);