4D避障

Inc/soft_obstacle.h

@@ -56,6 +56,26 @@ extern bool uavrhup_getr1_ack;
 extern short obsfradar_sensitivity;
 extern short obsbradar_sensitivity;
 
+#pragma pack(1)
+typedef struct
+{
+    short angel_4DF;
+    short ground_filter_4DF;
+    short dotcloud_switch_4DF;
+    short angel_4DB;
+    short ground_filter_4DB;
+    short dotcloud_switch_4DB;
+
+    bool get_angel_4DF;
+    bool get_ground_filter_4DF;
+    bool get_angel_4DB;
+    bool get_ground_filter_4DB;
+    bool get_dotcloud_switch_4DF;
+    bool get_dotcloud_switch_4DB;
+}_dev_par;
+#pragma pack()
+
+extern _dev_par DM_4DRADARMAG;
 //避障雷达
 #pragma pack(1)
 typedef struct 

Inc/soft_p_2_c.h

@@ -37,12 +37,15 @@ enum vklink_msgid
 	_MSGID_REQ = 20,	   // 请求信息
 	_MSGID_ACK = 21,	   // 主控应答
 	_MSGID_SET = 22,	   // 设置信息
+	_MSGID_GET4D = 23,     // 4D雷达参数回复fmu相关
+	_MSGID_SET4D = 24,   // 4D雷达设置相关
 	_MSGID_SHA1 = 25,	   // 智能电池SHA1验证
 	_MSGID_DEV_LIST = 26,  // 设备SN,软硬件管理
 	_MSGID_HEART = 27,     // 心跳检测
 	_MSGID_360RADAR = 30,     //360雷达
 	_MSGID_DMRADAR = 31,     //电目雷达测试
-	_MSGID_F4DRADAR = 32,     //电目4d雷达测试
+	_MSGID_F4DRADAR = 32,     //电目4d前避障雷达测试
+	_MSGID_B4DRADAR = 33,     //电目4d后避障雷达测试
 	_MSGID_UPDATA = 200,   // 升级信息
 	_MSGID_CANDEBUG = 213, // CAN调试
 };

Inc/soft_seed_device.h

@@ -366,7 +366,8 @@ typedef struct
 
     _Temp_sensor Temp_sensor;
     Connect_check Temp_Sensor_Link;
-		
+
+	Part_Fradar Part_radarB;	
     Connect_check Part_Bradar_Link;
 } Device_type;
 extern Device_type Dev;

Inc/soft_terrain.h

@@ -15,7 +15,8 @@ extern UAVH30 uavh30_dist;
 extern bool terrain_is_link;
 extern uint16_t terrain_height;
 extern bool F4d_send_flag;
-
+extern bool B4d_send_flag;
+extern bool DM4Dmsg_send_fmu;
 void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len);
 
 void can_recv_muniu_terrain(uint8_t *data);
@@ -60,7 +61,7 @@ extern uavr_terrain uavr56_info;
 extern uavr_terrain mimo_ter_info; 
 extern uavr_terrain DM_ter_info; 
 extern int16_t F_4DRadar[3][3];
-
+extern int16_t B_4DRadar[3][3];
 // //木牛仿地
 // #pragma pack(1)
 // typedef struct 
@@ -111,7 +112,7 @@ typedef struct
   uint8_t buf[150 * 5];
 }DM_4DRADAR;
 extern DM_4DRADAR FMU_4D_info; 
-
+//extern DM_4DRADAR FMU_4DB_info;
 #pragma pack(1)
 typedef struct 
 {
@@ -125,7 +126,7 @@ typedef struct
 }DM_4dFRADAR;
 #pragma pack()
 extern DM_4dFRADAR DM_F4d;
-
+extern DM_4dFRADAR DM_B4d;
 
 extern Connect_check DM_status;
 extern Connect_check DM_4dstatus;
@@ -133,6 +134,8 @@ extern DM_RADAR DM_T_info,FMU_DM_info;
 extern uint8_t DM_recv_flag;
 extern uint8_t DM4d_recv_flag;
 extern uint8_t DM4d_to_fmu_flag;
+extern uint8_t DM4dB_recv_flag; 
 void DM_terrain_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len);
 void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len);
+void DM_Bobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len);
 #endif 

Inc/soft_timer.h

@@ -20,6 +20,7 @@ typedef struct
     bool part_radar;
     bool tempSensor;
     bool part_Fradar;
+    bool part_Bradar;
 }Dev_timer;
 extern Dev_timer devinfo_time;
 

Src/soft_can.c

@@ -432,6 +432,9 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
         case CAN_MSGID_FRADAR_INFO ... CAN_MSGID_FRADAR_VERSION_INFO:
           DM_Fobs_recieved_hookfuction(RxHeader.ExtId, RxData, RxHeader.DLC);
           break;
+          case CAN_MSGID_BRADAR_INFO ... CAN_MSGID_BRADAR_VERSION_INFO:
+          DM_Bobs_recieved_hookfuction(RxHeader.ExtId, RxData, RxHeader.DLC);
+          break;
         default:
           break;
         }
@@ -808,6 +811,7 @@ void check_radar_link_status()
   Check_dev_link(&DM_status,3000,NULL,sizeof(DM_RADAR));
   Check_dev_link(&Dev.Part_Tradar_Link,3000,(char *)&Dev.Part_radarT,sizeof(Part_Tradar));
   Check_dev_link(&Dev.Part_Fradar_Link,3000,(char *)&Dev.Part_radarF,sizeof(Part_Fradar));
+  Check_dev_link(&Dev.Part_Bradar_Link,3000,(char *)&Dev.Part_radarB,sizeof(Part_Fradar));
 }
 
 void put_date_to_can(uint8_t *buf,uint8_t par1,uint8_t par2,uint8_t par3,uint8_t par4,uint8_t par5,

Src/soft_obstacle.c

@@ -1390,13 +1390,19 @@ void get_radar_blindAndPower_function( void )
         put_date_to_can(can_buf,0x9,0,0,0,0,0,0,0X7);
         can_send_msg_normal(&can_buf[0], 8, can_id);
     }
-    else if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_rawSwitch_flag == false)
+    else if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_rawSwitch_flag == false )
+    {
+        can_id = 0xA81300;
+        put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
+    else if((Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RF_4D) && 
+            (DM_4DRADARMAG.get_dotcloud_switch_4DF == false && DM4Dmsg_send_fmu == false))
     {
         can_id = 0xA81300;
         put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
         can_send_msg_normal(&can_buf[0], 8, can_id);
     }
-
     if(DM_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_blind_flag == false)
     {
         can_id = 0x981300;
@@ -1415,6 +1421,41 @@ void get_radar_blindAndPower_function( void )
         put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
         can_send_msg_normal(&can_buf[0], 8, can_id);
     }
+    if((Dev.Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarB.facid == FAC_DM_RB_4D) && 
+            (DM_4DRADARMAG.get_dotcloud_switch_4DB == false && DM4Dmsg_send_fmu == false))
+    {
+        can_id = 0xB81300;
+        put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
+    if ((Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RF_4D) &&
+        (DM_4DRADARMAG.get_angel_4DF == false && DM4Dmsg_send_fmu == false))
+    {
+        can_id = 0xA81300;
+        put_date_to_can(can_buf, 0xD, 0, 0, 0, 0, 0, 0, 0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
+    else if ((Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RF_4D) &&
+             (DM_4DRADARMAG.get_ground_filter_4DF == false && DM4Dmsg_send_fmu == false))
+    {
+        can_id = 0xA81300;
+        put_date_to_can(can_buf, 0xF, 0, 0, 0, 0, 0, 0, 0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
+    if ((Dev.Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarB.facid == FAC_DM_RB_4D) &&
+        (DM_4DRADARMAG.get_angel_4DB == false && DM4Dmsg_send_fmu == false))
+    {
+        can_id = 0xB81300;
+        put_date_to_can(can_buf, 0xD, 0, 0, 0, 0, 0, 0, 0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
+    else if ((Dev.Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarB.facid == FAC_DM_RB_4D) &&
+             (DM_4DRADARMAG.get_ground_filter_4DB == false && DM4Dmsg_send_fmu == false))
+    {
+        can_id = 0xB81300;
+        put_date_to_can(can_buf, 0xF, 0, 0, 0, 0, 0, 0, 0X7);
+        can_send_msg_normal(&can_buf[0], 8, can_id);
+    }
 }
 // mimo_360info mimo360_info[MIMO_360_TotalSect];
 

Src/soft_p_2_c.c

@@ -624,8 +624,86 @@ void pmu_to_con_DM4DFradar_data(void)
     DM4d_recv_flag = 0;
     
 }
+//电目4D后避障雷达
+void pmu_to_con_DM4DBradar_data(void)
+{
+    uint8_t index = 0;
+    int8_t pack_count = 0;//需要发送包数
+    int8_t remain_lastcount = 0; //余数
+    uint8_t send_bytes = 0; //需要发送字节
+    uint8_t send_pack = 1; //当前发送包数
+    int16_t remain_target_num = 0; //剩余发送目标点
+    int16_t complete_bytes_i= 0;
+    
+    DM4dB_recv_flag = 1;
+
+    remain_target_num = FMU_4D_info.target_num;
+    pack_count = calculate_packages(remain_target_num,48);
+    while (remain_target_num > 0)
+    {
+        index = 0;
+        msg_buf[index++] = 0xFE;
+        msg_buf[index++] = 0;
+        msg_buf[index++] = 0;
+        msg_buf[index++] = 0x00;
+        msg_buf[index++] = 0x00;
+        msg_buf[index++] = _MSGID_B4DRADAR;
+
+        remain_lastcount = remain_target_num % 48;
+        if(remain_target_num > 0 && remain_lastcount == 0)
+            remain_lastcount = 48;
+
+        if(pack_count > send_pack)
+            send_bytes = 48 * 5;
+        else
+            send_bytes = remain_lastcount * 5;
+
+        msg_buf[index++] = send_pack; 
+        msg_buf[index++] = FMU_4D_info.target_num;
+        memcpy(&msg_buf[index],&DM_B4d.time_delay,2);
+        index += 2;
 
+        memcpy(&msg_buf[index],&FMU_4D_info.buf[complete_bytes_i],send_bytes);
+        index += send_bytes;
+        complete_bytes_i += send_bytes;
+
+        msg_buf[1] = index - 6;
+
+        crc = Get_Crc16(msg_buf, index);
+        msg_buf[index++] = crc;
+        msg_buf[index++] = (crc >> 8) & 0xff;
+
+        uart2_send_msg(msg_buf, index);
 
+        remain_target_num -= send_bytes / 5;
+        if(send_bytes < 48 * 5)
+            break;
+        send_pack++;
+    }
+    DM4dB_recv_flag = 0;
+}
+//4D雷达回复fmu
+void pmu_to_con_DM4DBradar_msg(void)
+{
+    uint8_t index = 0;
+    msg_buf[index++] = 0xFE;
+    msg_buf[index++] = 0;
+    msg_buf[index++] = 0;
+    msg_buf[index++] = 0x00;
+    msg_buf[index++] = 0x00;
+    msg_buf[index++] = _MSGID_GET4D;
+    msg_buf[index++] = 'V';
+    msg_buf[index++] = 'K';
+    msg_buf[index++] = 'Z';
+    msg_buf[index++] = '1';
+     memcpy(&msg_buf[index],&DM_4DRADARMAG.angel_4DF,12);
+     index += 12;
+     msg_buf[1] = index - 6;
+    crc = Get_Crc16(msg_buf, index);
+    msg_buf[index++] = crc;
+    msg_buf[index++] = (crc >> 8) & 0xff;
+    uart2_send_msg(msg_buf, index);
+}
 /**
   * @file    pmu_to_con_heart_data
   * @brief   PMU_发送心跳
@@ -740,13 +818,20 @@ void pmu_to_con_devtype_data(void)
         index += sizeof(Part_Tradar);
         devinfo_time.part_radar = false;
     }
-    else if(Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && devinfo_time.part_Fradar == true)
+    else if((Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RF_4D) && devinfo_time.part_Fradar == true)
     {
         msg_buf[index++] = DEV_PART_FRADAR;
         memcpy(&msg_buf[index],&Dev.Part_radarF.facid,sizeof(Part_Fradar));
         index += sizeof(Part_Fradar);
         devinfo_time.part_Fradar = false;
     }
+    else if((Dev.Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RB_4D) && devinfo_time.part_Bradar == true)
+    {
+        msg_buf[index++] = DEV_PART_BRADAR;
+        memcpy(&msg_buf[index],&Dev.Part_radarB.facid,sizeof(Part_Fradar));
+        index += sizeof(Part_Fradar);
+        devinfo_time.part_Bradar = false;
+    }
     // else if(Dev.Bms_Link.connect_status == COMP_NORMAL && devinfo_time.bms == true)
     // {
     //     msg_buf[index++] = DEV_BMS;
@@ -1227,6 +1312,16 @@ void pmu_to_fcu()
             pmu_to_con_DM4DFradar_data();
             F4d_send_flag = false;
         }
+        if(B4d_send_flag == true)
+        {
+            pmu_to_con_DM4DBradar_data();
+            B4d_send_flag = false;
+        }
+        if(DM4Dmsg_send_fmu == true)
+        {
+            pmu_to_con_DM4DBradar_msg();
+            DM4Dmsg_send_fmu = false;
+        }
     }
 }
 
@@ -1518,10 +1613,12 @@ void uart_recv_con_msg()
                 {
                     uint8_t can_buf[8] = {0};
                     uint32_t can_id = 0;
-                    if(msgidset.content1 == 3)
+                    if(msgidset.content1 == 3 )
                         can_id = 0x981300;
-                    else if(msgidset.content1 == 5)
+                    else if(msgidset.content1 == 5 || msgidset.content1 == 7)
                         can_id = 0xA81300;
+                    else if(msgidset.content1 == 6)
+                        can_id = 0xB81300;
                     put_date_to_can(can_buf,0x4,(msgidset.content1 & 0xff),((msgidset.content1 >> 8) & 0xff),0,0,0,0,0X7);
                     can_send_msg_normal(&can_buf[0], 8, can_id);
                 }
@@ -1837,6 +1934,56 @@ void uart_recv_con_msg()
                 break;
             }
             break;
+        case  _MSGID_SET4D:
+            {
+                uint16_t radar_id = 0;
+                uint8_t can_buf[8] = {0};
+                int radar_msg = 0;
+
+                radar_id = fcu_protocol.payload[6] + fcu_protocol.payload[7]*256;
+                memcpy(&radar_msg,&fcu_protocol.payload[8],4);
+                switch (radar_id)
+                {
+                case 1:
+                    can_buf[0] = 0XC;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XA81300);
+                    break;
+                case 2:
+                    can_buf[0] = 0XE;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XA81300);
+                    break;
+                case 3:
+                    can_buf[0] = 0XA;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XA81300);
+                    break;
+                case 4:
+                    can_buf[0] = 0XC;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XB81300);
+                    break;
+                case 5:
+                    can_buf[0] = 0XE;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XB81300);
+                    break;
+                case 6:
+                    can_buf[0] = 0XB;
+                    memcpy(&can_buf[1],&radar_msg,4);
+                    can_buf[7] = 7;
+                    can_send_msg_normal(can_buf,8,0XB81300);
+                    break;   
+                default:
+                    break;
+                }
+            }
         case _MSGID_HEART:
             time_heart[1] = HAL_GetTick();
             time_count[1]++;

Src/soft_seed_device.c

@@ -648,7 +648,8 @@ void check_dev_type_link(void )
 
 void DM_obs_test( void )
 {
-    if((Dev.Part_Fradar_Link.connect_status != COMP_NORMAL) || (Dev.Part_radarF.facid != FAC_DM_RF_4D))
+    if(((Dev.Part_Fradar_Link.connect_status != COMP_NORMAL) || (Dev.Part_radarF.facid != FAC_DM_RF_4D)) && 
+        ((Dev.Part_Bradar_Link.connect_status != COMP_NORMAL) || (Dev.Part_radarB.facid != FAC_DM_RB_4D)))
         return;
 
     static uint32_t time_50hz = 0;
@@ -1154,7 +1155,7 @@ void  update_device_type_data(void)
             Dev.Part_radarT.height_part3 = T_radar[2].Distance * 0.05f * 100 * cos(T_radar[2].Amuzith * 0.1f / RAD);
         }
 
-        if(Dev.Part_Fradar_Link.connect_status == COMP_NORMAL)
+        if(Dev.Part_Fradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarF.facid == FAC_DM_RF_4D)
         {
             Dev.Part_radarF.X1 = F_4DRadar[0][0];
             Dev.Part_radarF.Y1 = F_4DRadar[0][1];
@@ -1166,7 +1167,18 @@ void  update_device_type_data(void)
             Dev.Part_radarF.Y3 = F_4DRadar[2][1];
             Dev.Part_radarF.Z3 = F_4DRadar[2][2];
         }
-
+        if(Dev.Part_Bradar_Link.connect_status == COMP_NORMAL && Dev.Part_radarB.facid == FAC_DM_RB_4D)
+        {
+            Dev.Part_radarB.X1 = B_4DRadar[0][0];
+            Dev.Part_radarB.Y1 = B_4DRadar[0][1];
+            Dev.Part_radarB.Z1 = B_4DRadar[0][2];
+            Dev.Part_radarB.X2 = B_4DRadar[1][0];
+            Dev.Part_radarB.Y2 = B_4DRadar[1][1];
+            Dev.Part_radarB.Z2 = B_4DRadar[1][2];
+            Dev.Part_radarB.X3 = B_4DRadar[2][0];
+            Dev.Part_radarB.Y3 = B_4DRadar[2][1];
+            Dev.Part_radarB.Z3 = B_4DRadar[2][2];
+        }
         if(Dev.Temp_Sensor_Link.connect_status == COMP_NORMAL)
         {
             switch (Dev.Temp_sensor.facid)

Src/soft_terrain.c

@@ -13,53 +13,52 @@
 #include "soft_version.h"
 #include "soft_p_2_c.h"
 
-
 UAVH30 uavh30_dist;
 
 uavr_terrain uavr56_info = {.get_radar_sensi = 50};
-uavr_terrain mimo_ter_info; 
+uavr_terrain mimo_ter_info;
 
-//muniu muniu_ter_info;
+_dev_par DM_4DRADARMAG;
+// muniu muniu_ter_info;
 
 bool terrain_is_link = false;
 uint16_t terrain_height = 0;
 
 /**
-  * @file    can_recv_mocib_terrain
-  * @brief   莫之比防地解析
-  * @param   none
-  * @details 
-  * @author  Zhang Sir 
+ * @file    can_recv_mocib_terrain
+ * @brief   莫之比防地解析
+ * @param   none
+ * @details
+ * @author  Zhang Sir
  **/
 void can_recv_mocib_terrain(uint8_t *data)
 {
     uavh30_dist.powerful = (data[0] << 8) + data[1];
     uavh30_dist.near = (data[2] << 8) + data[3];
     uavh30_dist.far = (data[4] << 8) + data[5];
-    
+
     uavr56_info.height = uavh30_dist.powerful;
     uavr56_info.Link.recv_time = HAL_GetTick();
     uavr56_info.Link.connect_status = COMP_NORMAL;
 }
 
-
 /**
-  * @file    can_recv_enzhao_terrain
-  * @brief   恩曌防地解析
-  * @param   none
-  * @details 
-  * @author  Zhang Sir 
+ * @file    can_recv_enzhao_terrain
+ * @brief   恩曌防地解析
+ * @param   none
+ * @details
+ * @author  Zhang Sir
  **/
 mimo_part_radar T_radar[3];
 void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
 {
     switch (CanID)
     {
-    case CAN_MIMO_T_ID: //单点雷达
+    case CAN_MIMO_T_ID: // 单点雷达
         mimo_ter_info.crc = data[3] + data[4] + data[5] + data[6];
         if (data[7] == mimo_ter_info.crc)
         {
-            mimo_ter_info.height = (256 * data[4]) + (data[3]); //cm
+            mimo_ter_info.height = (256 * data[4]) + (data[3]); // cm
             //  printf("%d          %d\n", mimo_ter_info.height, m.muniu_hight);   //输出到串口助手上 需要pringtf重定向
         }
 
@@ -83,14 +82,13 @@ void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
     default:
         break;
     }
-
 }
 
-//木牛仿地雷达
-// void can_recv_muniu_terrain(uint8_t *data)
-// {
-//     muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
-//     muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];
+// 木牛仿地雷达
+//  void can_recv_muniu_terrain(uint8_t *data)
+//  {
+//      muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
+//      muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];
 
 //     muniu_ter_info.Link.connect_status = COMP_NORMAL;
 //     muniu_ter_info.Link.recv_time = HAL_GetTick();
@@ -104,55 +102,55 @@ uint8_t DM_recv_flag = 0;
 uint8_t DM4d_recv_flag = 0;
 Connect_check DM_status;
 Connect_check DM_4dstatus;
-uavr_terrain DM_ter_info; 
-void DM_terrain_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len)
+uavr_terrain DM_ter_info;
+void DM_terrain_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
 {
-    if(cellCanID == 0x901300) //多点协议
+    if (cellCanID == 0x901300) // 多点协议
     {
         DM_T_info.byte7.frame_flag = data[7];
 
         DM_status.connect_status = COMP_NORMAL;
         DM_status.recv_time = HAL_GetTick();
-        
-        if(DM_T_info.byte7.flag.head != 0 ) //头
+
+        if (DM_T_info.byte7.flag.head != 0) // 头
         {
-            memcpy(&DM_T_info.target_num,&data[0],7);
+            memcpy(&DM_T_info.target_num, &data[0], 7);
             if( DM_T_info.target_num > 30 )
                     DM_T_info.target_num = 30;
             dm_i = 0;
-            if(DM_T_info.target_num == 0 && DM_recv_flag == 0)
+            if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
             {
                 FMU_DM_info.target_num = 0;
             }
         }
-        else if(DM_T_info.byte7.flag.tail != 0) //尾
+        else if (DM_T_info.byte7.flag.tail != 0) // 尾
         {
-            if(DM_T_info.target_num != 1)
+            if (DM_T_info.target_num != 1)
             {
-                memcpy(&DM_T_info.buf[dm_i],&data[0],DM_T_info.target_num*4%7);
+                memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
             }
             else
             {
-                memcpy(&DM_T_info.buf[dm_i],&data[0],4);
+                memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
             }
 
-            if(DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0],DM_T_info.target_num * 4) &&  DM_recv_flag == 0)
+            if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
             {
-                memcpy(&FMU_DM_info.target_num,&DM_T_info.target_num,DM_T_info.target_num * 4 + 8);
-                //memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
+                memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
+                // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
             }
         }
         else
         {
-            memcpy(&DM_T_info.buf[dm_i],&data[0],7);
+            memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
             dm_i += 7;
         }
-        if(dm_i >= 255 -7)
+        if (dm_i >= 255 - 7)
         {
             dm_i = 0;
         }
     }
-    else if(cellCanID == 0x901301)//单点协议
+    else if (cellCanID == 0x901301) // 单点协议
     {
         DM_ter_info.Link.connect_status = COMP_NORMAL;
         DM_ter_info.Link.recv_time = HAL_GetTick();
@@ -161,69 +159,70 @@ void DM_terrain_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t
         DM_ter_info.height = data[3] + data[4] * 256;
     }
 
-    //版本信息
-    if(cellCanID == 0x981301 && data[0] == 0x1)
+    // 版本信息
+    if (cellCanID == 0x981301 && data[0] == 0x1)
     {
         uint32_t version_temp = 0;
         DM_T_info.byte7.frame_flag = data[7];
 
-        if(DM_T_info.byte7.flag.head != 0 ) //头
+        if (DM_T_info.byte7.flag.head != 0) // 头
         {
-            memcpy(&version_temp,&data[1],4);
-            Int2String(version_temp,DM_ter_info.sn,9);
-            //通过SN序号判断新旧boot
-            if((version_temp % 10000000) < 2502999)    
+            memcpy(&version_temp, &data[1], 4);
+            Int2String(version_temp, DM_ter_info.sn, 9);
+            // 通过SN序号判断新旧boot
+            if ((version_temp % 10000000) < 2502999)
                 DM_ter_info.version[3] = 'O';
             else
                 DM_ter_info.version[3] = 'N';
 
-            regist_dev_info(&dev_ter,DEVICE_TERRA,false,DM_ter_info.sn,9,NULL,0,NULL,0,"dmter",6);
+            regist_dev_info(&dev_ter, DEVICE_TERRA, false, DM_ter_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
         }
-        else if(DM_T_info.byte7.flag.tail != 0) //尾
+        else if (DM_T_info.byte7.flag.tail != 0) // 尾
         {
-           memcpy(&version_temp,&data[1],4);
-           Int2String(version_temp,&DM_ter_info.version[4],6);
-           DM_ter_info.version[0] = 'D';
-           DM_ter_info.version[1] = 'S';
-           DM_ter_info.version[2] = '1';
+            memcpy(&version_temp, &data[1], 4);
+            Int2String(version_temp, &DM_ter_info.version[4], 6);
+            DM_ter_info.version[0] = 'D';
+            DM_ter_info.version[1] = 'S';
+            DM_ter_info.version[2] = '1';
 
-           regist_dev_info(&dev_ter,DEVICE_TERRA,false,NULL,0,DM_ter_info.version,10,NULL,0,"dmter",6);
+            regist_dev_info(&dev_ter, DEVICE_TERRA, false, NULL, 0, DM_ter_info.version, 10, NULL, 0, "dmter", 6);
 
-           DM_ter_info.get_radar_ver_flag = true;
-           pmu_send = PMU_SEND_VERSION; //旧版APP
+            DM_ter_info.get_radar_ver_flag = true;
+            pmu_send = PMU_SEND_VERSION; // 旧版APP
         }
     }
-    else if(cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5))
+    else if (cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5))
     {
-        if(data[0] == 0x8)
+        if (data[0] == 0x8)
             DM_ter_info.get_radar_blind_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_TR_BLIND,0x56,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x56, data[1] + data[2] * 256);
     }
-    else if(cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7))
+    else if (cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7))
     {
-        if(data[0] == 0x9)
+        if (data[0] == 0x9)
             DM_ter_info.get_radar_power_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_BR_POWER,0x56,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x56, data[1] + data[2] * 256);
     }
-    else if(cellCanID == 0x981301 && (data[0] == 0xA || data[0] == 0xB))
+    else if (cellCanID == 0x981301 && (data[0] == 0xA || data[0] == 0xB))
     {
-        if(data[0] == 0xB)
+        if (data[0] == 0xB)
             DM_ter_info.get_radar_rawSwitch_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_RAW_SWITCH,0x56,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x56, data[1] + data[2] * 256);
     }
-    else if(cellCanID == 0x981301 && data[0] == 0x4 )
+    else if (cellCanID == 0x981301 && data[0] == 0x4)
     {
-        pmu_set_ack(_MSGID_SET,MSGID_SET_R_FUNC,0,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
     }
+   
 }
 
-int16_t F_4DRadar[3][3]={0};  // X Y Z
+int16_t F_4DRadar[3][3] = {0}; // X Y Z
 DM_4dFRADAR DM_F4d;
 int dm_4df_i = 0;
 DM_4DRADAR FMU_4D_info;
 bool F4d_send_flag = false;
-
-void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len)
+bool DM4Dmsg_send_fmu=false;
+void DM_Fobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
 {
     if (cellCanID == 0XA01300) // 多点协议
     {
@@ -270,7 +269,7 @@ void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len
             dm_i = 0;
         }
     }
-    else if(cellCanID == 0XA01301)//单点协议
+    else if (cellCanID == 0XA01301) // 单点协议
     {
         DM_f_info.Link.connect_status = COMP_NORMAL;
         DM_f_info.Link.recv_time = HAL_GetTick();
@@ -321,7 +320,6 @@ void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len
                 memcpy(&FMU_4D_info.buf, &DM_F4d.RawData, DM_F4d.target_num * 5);
                 F4d_send_flag = true;
             }
-            
         }
         else
         {
@@ -333,8 +331,8 @@ void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len
             dm_4df_i = 0;
         }
     }
-    //4D前避障雷达协议 单点
-    if(cellCanID == 0XA01302)
+    // 4D前避障雷达协议 单点
+    if (cellCanID == 0XA01302)
     {
         F_4DRadar[0][0] = data[1] + data[2] * 256;
         F_4DRadar[0][1] = data[3] + data[4] * 256;
@@ -344,75 +342,201 @@ void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len
         Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
         Dev.Part_radarF.facid = FAC_DM_RF_4D;
     }
-    else if(cellCanID == 0XA01303)
+    else if (cellCanID == 0XA01303)
     {
         F_4DRadar[1][0] = data[1] + data[2] * 256;
         F_4DRadar[1][1] = data[3] + data[4] * 256;
         F_4DRadar[1][2] = data[5] + data[6] * 256;
     }
-    else if(cellCanID == 0XA01304)
+    else if (cellCanID == 0XA01304)
     {
         F_4DRadar[2][0] = data[1] + data[2] * 256;
         F_4DRadar[2][1] = data[3] + data[4] * 256;
         F_4DRadar[2][2] = data[5] + data[6] * 256;
     }
 
-
-
-
-    //版本信息
-    if(cellCanID == 0XA81301 && data[0] == 0x1)
+    // 版本信息
+    if (cellCanID == 0XA81301 && data[0] == 0x1)
     {
         uint32_t version_temp = 0;
         DM_T_info.byte7.frame_flag = data[7];
 
-        if(DM_T_info.byte7.flag.head != 0 ) //头
+        if (DM_T_info.byte7.flag.head != 0) // 头
         {
-            memcpy(&version_temp,&data[1],4);
-            Int2String(version_temp,DM_f_info.sn,9);
-            //通过SN序号判断新旧boot
-            if((version_temp % 10000000) < 2502999)   
+            memcpy(&version_temp, &data[1], 4);
+            Int2String(version_temp, DM_f_info.sn, 9);
+            // 通过SN序号判断新旧boot
+            if ((version_temp % 10000000) < 2502999)
                 DM_f_info.version[3] = 'O';
             else
                 DM_f_info.version[3] = 'N';
 
-            regist_dev_info(&dev_obsf,DEVICE_OBSF,false,DM_f_info.sn,9,NULL,0,NULL,0,"dmter",6);
+            regist_dev_info(&dev_obsf, DEVICE_OBSF, false, DM_f_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
         }
-        else if(DM_T_info.byte7.flag.tail != 0) //尾
+        else if (DM_T_info.byte7.flag.tail != 0) // 尾
         {
-            memcpy(&version_temp,&data[1],4);
-            Int2String(version_temp,&DM_f_info.version[4],6);
+            memcpy(&version_temp, &data[1], 4);
+            Int2String(version_temp, &DM_f_info.version[4], 6);
             DM_f_info.version[0] = 'D';
             DM_f_info.version[1] = 'W';
             DM_f_info.version[2] = '1';
-            
-           regist_dev_info(&dev_obsf,DEVICE_OBSF,false,NULL,0,DM_f_info.version,10,NULL,0,"dmter",6);
 
-           DM_f_info.get_radar_ver_flag = true;
-           pmu_send = PMU_SEND_VERSION; //旧版APP
+            regist_dev_info(&dev_obsf, DEVICE_OBSF, false, NULL, 0, DM_f_info.version, 10, NULL, 0, "dmter", 6);
+
+            DM_f_info.get_radar_ver_flag = true;
+            pmu_send = PMU_SEND_VERSION; // 旧版APP
         }
     }
-    else if(cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5))
+    else if (cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5))
     {
-        if(data[0] == 0x8)
+        if (data[0] == 0x8)
             DM_f_info.get_radar_blind_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_TR_BLIND,0x11,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x11, data[1] + data[2] * 256);
     }
-    else if(cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7))
+    else if (cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7))
     {
-        if(data[0] == 0x9)
+        if (data[0] == 0x9)
             DM_f_info.get_radar_power_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_BR_POWER,0x11,data[1] + data[2] * 256);
+        pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x11, data[1] + data[2] * 256);
+    }
+    else if (cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
+    {
+        
+            if (data[0] == 0xB)
+                DM_f_info.get_radar_rawSwitch_flag = true;
+            pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x11, data[1] + data[2] * 256);
+        
+    }
+    else if (cellCanID == 0xA81301 && data[0] == 0x4)
+    {
+        pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
+    }
+    else if (cellCanID == 0xA81302 && (data[0] == 0xD || data[0] == 0xC))
+    {
+        if (data[0] == 0xD)
+            DM_4DRADARMAG.get_angel_4DF = true;
+        DM_4DRADARMAG.angel_4DF = data[1] + data[2] * 256;
+        DM4Dmsg_send_fmu = true;
+    }
+    else if (cellCanID == 0xA81302 && (data[0] == 0xF || data[0] == 0xE))
+    {
+        if (data[0] == 0xF)
+            DM_4DRADARMAG.get_ground_filter_4DF = true;
+        DM_4DRADARMAG.ground_filter_4DF = data[1] + data[2] * 256;
+        DM4Dmsg_send_fmu = true;
+    }
+    else if (cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
+    {
+        if (data[0] == 0xB)
+                DM_4DRADARMAG.get_dotcloud_switch_4DF = true;
+            DM_4DRADARMAG.dotcloud_switch_4DF = data[1] + data[2] * 256;
+            DM4Dmsg_send_fmu = true;
+    }
+}
+int16_t B_4DRadar[3][3] = {0}; // X Y Z
+DM_4dFRADAR DM_B4d;
+int dm_4dB_i = 0;
+// DM_4DRADAR FMU_4DB_info;
+bool B4d_send_flag = false;
+uint8_t DM4dB_recv_flag = 0;
+void DM_Bobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
+{
+    // 4D后避障雷达协议 点云
+    if (cellCanID == 0XB01310)
+    {
+        // DM_4dstatus.connect_status = COMP_NORMAL;
+        // DM_4dstatus.recv_time = HAL_GetTick();
+        Dev.Part_radarB.facid = FAC_DM_RB_4D;
+
+        DM_B4d.byte7.frame_flag = data[7];
+
+        if (DM_B4d.byte7.flag.head != 0) // 头
+        {
+            memcpy(&DM_B4d.target_num, &data[0], 7);
+            dm_4dB_i = 0;
+        }
+        else if (DM_B4d.byte7.flag.tail != 0) // 尾
+        {
+            if (DM_B4d.target_num != 1)
+            {
+                if ((DM_B4d.target_num * 5) % 7 != 0)
+                {
+                    memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], (DM_B4d.target_num * 5) % 7);
+                    dm_4dB_i += (DM_B4d.target_num * 5) % 7;
+                }
+                else
+                {
+                    memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
+                    dm_4dB_i += 7;
+                }
+            }
+            else
+            {
+                memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 5);
+                dm_4dB_i += 5;
+            }
+            if (DM_4dstatus.connect_status != COMP_NORMAL)
+            {
+                if (DM_B4d.crc == Get_Crc16(&DM_B4d.RawData[0], DM_B4d.target_num * 5) && DM4dB_recv_flag == 0)
+                {
+                    FMU_4D_info.target_num = DM_B4d.target_num;
+                    memcpy(&FMU_4D_info.buf, &DM_B4d.RawData, DM_B4d.target_num * 5);
+                    B4d_send_flag = true;
+                }
+            }
+        }
+        else
+        {
+            memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
+            dm_4dB_i += 7;
+        }
+        if (dm_4dB_i >= 254 * 5)
+        {
+            dm_4dB_i = 0;
+        }
+    }
+    // 4D后避障雷达协议 单点
+    if (cellCanID == 0XB01302)
+    {
+        B_4DRadar[0][0] = data[1] + data[2] * 256;
+        B_4DRadar[0][1] = data[3] + data[4] * 256;
+        B_4DRadar[0][2] = data[5] + data[6] * 256;
+
+        Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
+        Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
+        Dev.Part_radarB.facid = FAC_DM_RB_4D;
+    }
+    else if (cellCanID == 0XB01303)
+    {
+        B_4DRadar[1][0] = data[1] + data[2] * 256;
+        B_4DRadar[1][1] = data[3] + data[4] * 256;
+        B_4DRadar[1][2] = data[5] + data[6] * 256;
+    }
+    else if (cellCanID == 0XB01304)
+    {
+        B_4DRadar[2][0] = data[1] + data[2] * 256;
+        B_4DRadar[2][1] = data[3] + data[4] * 256;
+        B_4DRadar[2][2] = data[5] + data[6] * 256;
+    }
+    else if (cellCanID == 0xB81302 && (data[0] == 0xA || data[0] == 0xB))
+    {
+            if (data[0] == 0xB)
+                DM_4DRADARMAG.get_dotcloud_switch_4DB = true;
+            DM_4DRADARMAG.dotcloud_switch_4DB = data[1] + data[2] * 256;
+            DM4Dmsg_send_fmu = true;
     }
-    else if(cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
+    else if (cellCanID == 0xB81302 && (data[0] == 0xD || data[0] == 0xC))
     {
-        if(data[0] == 0xB)
-            DM_f_info.get_radar_rawSwitch_flag = true;
-        pmu_set_ack(_MSGID_SET,MSGID_SET_RAW_SWITCH,0x11,data[1] + data[2] * 256);
+        if (data[0] == 0xD)
+            DM_4DRADARMAG.get_angel_4DB = true;
+        DM_4DRADARMAG.angel_4DB = data[1] + data[2] * 256;
+        DM4Dmsg_send_fmu = true;
     }
-    else if(cellCanID == 0xA81301 && data[0] == 0x4 )
+    else if (cellCanID == 0xB81302 && (data[0] == 0xF || data[0] == 0xE))
     {
-        pmu_set_ack(_MSGID_SET,MSGID_SET_R_FUNC,0,data[1] + data[2] * 256);
+        if (data[0] == 0xF)
+            DM_4DRADARMAG.get_ground_filter_4DB = true;
+        DM_4DRADARMAG.ground_filter_4DB = data[1] + data[2] * 256;
+        DM4Dmsg_send_fmu = true;
     }
-    
 }

Src/soft_timer.c

@@ -76,6 +76,7 @@ void timer_function()
         devinfo_time.part_radar = true;
         devinfo_time.part_Fradar = true;
         devinfo_time.flow = true;
+        devinfo_time.part_Bradar = true;
 
     }
     if ( Check_Timer_Ready(&time_50hz,_50_HZ_) )

Src/soft_uart.c

@@ -418,9 +418,9 @@ void check_uart_data(rkfifo_t *fifo)
 					uart_info.fcu_buf_flag = true; 
 				}
 				//莫之比雷达升级
-				else if(((vk_data.group_id == GROUP_ID_F_UPDATE && uavr11_info.Link.connect_status != COMP_NOEXIST) ||
+				else if((vk_data.group_id == GROUP_ID_F_UPDATE && uavr11_info.Link.connect_status != COMP_NOEXIST) ||
 					 	(vk_data.group_id == GROUP_ID_B_UPDATE && uavr12_info.Link.connect_status != COMP_NOEXIST) ||
-					    (vk_data.group_id == GROUP_ID_T_UPDATE && uavr56_info.Link.connect_status != COMP_NOEXIST)))
+					    (vk_data.group_id == GROUP_ID_T_UPDATE && uavr56_info.Link.connect_status != COMP_NOEXIST))
 				{	
 					if(uart_info.use_update_buf_flag == false)
 					{

Src/soft_update.c

@@ -104,6 +104,10 @@ void Vk_Update_Device_Protocol(void)
             {
                 memcpy(&can_buf[5],"MZB",3);
             }
+            else if(Dev.Part_radarB.facid == FAC_DM_RB_4D)
+            {
+                memcpy(&can_buf[5],"D4B",3);
+            }
             break;
         case UPDATE_TERAIN:
             if(Dev.Radar.facid_T == FAC_MIMO_RT)
@@ -220,13 +224,13 @@ void Update_Dev_Bootversion_Function(uint8_t data[])
                 }
 				regist_dev_info(&dev_obsf,DEVICE_OBSF,false,NULL,0,DM_f_info.version,10,NULL,0,"dmf",6);
             }
-            else if(memcmp((char *)&data[1],"MZB",3) == 0)
+            /*else if(memcmp((char *)&data[1],"MZB",3) == 0)
             {
                 Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
                 Dev.Part_Fradar_Link.connect_status= COMP_NORMAL;
                 Dev.Part_Fradar_Link.boot_flag = true;
                 Dev.Radar.facid_F=FAC_MOCIB_RF;
-            }
+            }*/
             else if(memcmp((char *)&data[1],"D4F",3) == 0)
             {
                 Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
@@ -259,15 +263,18 @@ void Update_Dev_Bootversion_Function(uint8_t data[])
                 mimo_b_info.Link.boot_flag = true;
                 Dev.Radar.facid_B = FAC_MIMO_RB;
             }
-            else if(memcmp((char *)&data[1],"MZB",3) == 0)
+            /*else if(memcmp((char *)&data[1],"MZB",3) == 0)
             {
                 Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
                 Dev.Part_Bradar_Link.connect_status= COMP_NORMAL;
                 Dev.Part_Bradar_Link.boot_flag = true;
                 Dev.Radar.facid_B = FAC_MOCIB_RB;
-            }
+            }*/
             else if(memcmp((char *)&data[1],"D4B",3) == 0)
             {
+                Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
+                Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
+                Dev.Part_radarB.facid = FAC_DM_RB_4D;
                 // Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
                 // Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
                 // Dev.Part_radarT.facid = FAC_DM_RF;