1、版本发布

Inc/rkfifo.h

@@ -22,6 +22,7 @@ typedef struct rkfifo
     uint32_t mask;
     uint32_t esize;
     void* data;
+    int error;
 }rkfifo_t;
 
 

Inc/soft_bms.h

@@ -45,7 +45,7 @@ struct BMS_DATA
     uint16_t bms_cycle; //循环次数
     uint16_t bms_volt;  //电压 mv
     int16_t bms_ac;     //电流 10ma
-    uint16_t bms_temp;  //温度 0.1℃
+    int16_t bms_temp;  //温度 0.1℃
     uint8_t bms_vs;     //充电百分比 1%
     uint16_t bms_ss;    //状态
     uint16_t serial_num; //VK3厂商编号

Inc/soft_obstacle.h

@@ -79,7 +79,7 @@ typedef struct
 
   //版本信息
   int soft_verison; //软件版本 区分新旧雷达，旧版本设置灵敏度进BOOT
-  char version[10];
+  char version[11];
   int hard_version;
 
   bool get_radar_ver_flag; //检测到雷达一直发，上电发几次可能收不到

Inc/soft_p_2_c.h

@@ -19,7 +19,8 @@ typedef enum
 } Msg_Rx_Stage;
 
 extern Msg_Rx_Stage recv_step;
-
+extern int time_heart[3];
+extern int time_count[3];
 enum vklink_msgid
 {
 	_MSGID_VOL = 1,		   // 电压
@@ -41,6 +42,7 @@ enum vklink_msgid
 	_MSGID_HEART = 27,     // 心跳检测
 	_MSGID_360RADAR = 30,     //360雷达
 	_MSGID_DMRADAR = 31,     //电目雷达测试
+	_MSGID_F4DRADAR = 32,     //电目4d雷达测试
 	_MSGID_UPDATA = 200,   // 升级信息
 	_MSGID_CANDEBUG = 213, // CAN调试
 };
@@ -176,8 +178,16 @@ typedef struct
 	short UAV_type;
 	short thr_pwm;
 	short Candebug_flag; // can调试 0开 1关
-	int pos_x;           //经度
-	int pos_y;			 //纬度
+	int pos_x1;           //经度
+	int pos_y1;			 //纬度
+	float QuaterQ0;
+	float QuaterQ1;
+	float QuaterQ2;
+	float QuaterQ3;
+	float pos_x;
+	float pos_y;
+	float pos_z;
+	float pos_flag;
 } plane_para;
 #pragma pack()
 extern plane_para planep;

Inc/soft_seed_device.h

@@ -297,6 +297,22 @@ typedef struct
 } Part_Tradar;
 #pragma pack()
 
+#pragma pack(1)
+typedef struct
+{
+    uint8_t facid;
+    int16_t X1;
+    int16_t Y1;
+    int16_t Z1;
+    int16_t X2;
+    int16_t Y2;
+    int16_t Z2;
+    int16_t X3;
+    int16_t Y3;
+    int16_t Z3;
+} Part_Fradar;
+#pragma pack()
+
 typedef struct
 {
     Seed_info Seed;
@@ -341,6 +357,9 @@ typedef struct
     Part_Tradar Part_radarT;
     Connect_check Part_Tradar_Link;
 
+    Part_Fradar Part_radarF;
+    Connect_check Part_Fradar_Link;
+
     _Temp_sensor Temp_sensor;
     Connect_check Temp_Sensor_Link;
 

Inc/soft_terrain.h

@@ -43,7 +43,7 @@ typedef struct
   //获取版本信息
   bool get_radar_ver_flag;
   bool get_radar_sn_flag;
-  char version[10];
+  char version[11];
   int soft_verison;
   int hard_version;
 
@@ -58,7 +58,7 @@ typedef struct
 extern uavr_terrain uavr56_info;
 extern uavr_terrain mimo_ter_info; 
 extern uavr_terrain DM_ter_info; 
-
+extern int16_t F_4DRadar[3][3];
 
 // //木牛仿地
 // #pragma pack(1)
@@ -101,9 +101,36 @@ typedef struct
   uint8_t buf[240];
 }DM_RADAR;
 #pragma pack()
+
+typedef struct 
+{
+  uint8_t pack_num;
+  uint8_t target_num;
+
+  uint8_t buf[256 * 5];
+}DM_4DRADAR;
+extern DM_4DRADAR FMU_4D_info; 
+
+#pragma pack(1)
+typedef struct 
+{
+  uint8_t target_num;
+  uint16_t time_delay;
+  uint16_t crc;
+  uint16_t warn;
+  union dm_byte byte7;
+
+  uint8_t RawData[256 * 5];
+}DM_4dFRADAR;
+#pragma pack()
+extern DM_4dFRADAR DM_F4d;
+
+
 extern Connect_check DM_status;
+extern Connect_check DM_4dstatus;
 extern DM_RADAR DM_T_info,FMU_DM_info;
 extern uint8_t DM_recv_flag;
+extern uint8_t DM4d_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);
 #endif 

Inc/soft_timer.h

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

Inc/soft_uart.h

@@ -24,8 +24,6 @@
 
 #define GROUP_ID_WEIGHT_UPDATE  222
 
-
-
 #pragma pack(1)
 typedef struct
 {

Src/can_debug.c

@@ -89,11 +89,6 @@ void Can_send_debug_to_app(void)
 
         uart2_send_msg(msg_buf, index);
         //uart3_send_msg(msg_buf, index);
-
-        if(send_time > 1)
-        {
-            HAL_Delay(50);
-        }
     }
 
 }

Src/main.c

@@ -159,7 +159,7 @@ int main(void)
     //更新播撒，称重，水泵，离心喷头,流量计等设备信息
     update_device_type_data(); 
 
-    //DM_obs_test();//test 
+    DM_obs_test();//test 
 //debug test
 #ifdef Debug_Mode
 

Src/rkfifo.c

@@ -143,8 +143,12 @@ unsigned int rkfifo_in(rkfifo_t *fifo, const void *buf, unsigned int len)
     unsigned int l;
 
     l = kfifo_unused(fifo);
+    
     if (len > l)
+    {
         len = l;
+        fifo->error++;
+    }
 
     __rkfifo_copy_in(fifo, buf, len, fifo->in);
     fifo->in += len;

Src/soft_bms.c

@@ -337,7 +337,6 @@ void update_bms_data( void )
             bms_data.bms_vs = Device1.vk_bms2.persent / 10;
             bms_data.bms_ss = Device1.vk_bms2.warn_flag;
             bms_data.bms_num = Device1.vk_bms2.bms_num;
-
             memcpy(&bms_data.bms_v1,&Device1.vk_bms2.bms_v1,26 * 2);
             
             memcpy(&bms_data.bms_ids[0],&Device1.vk_bms3.bms_id[0],20);

Src/soft_can.c

@@ -64,11 +64,14 @@ void Can_decode_data_function(CAN_RxHeaderTypeDef Rxhead)
         break;
 
       //恩曌360雷达 新协议
-      case CAN_360MIMO_1ID ... CAN_360MIMO_2ID:
-        //can_recv_mocib_new360_obstacle(RxHeader.StdId, RxData, RxHeader.DLC);
-        break;
+      // case CAN_360MIMO_1ID ... CAN_360MIMO_2ID:
+      //   //can_recv_mocib_new360_obstacle(RxHeader.StdId, RxData, RxHeader.DLC);
+      //   break;
       //恩曌前后避障雷达(多点)
-      case CAN_MIMO_FOBS_ID1 ... CAN_MIMO_BOBS_ID3:
+      case CAN_MIMO_FOBS_ID1 ... CAN_MIMO_FOBS_ID3:
+        can_recv_enzhao_obstacle(RxHeader.StdId, RxData, RxHeader.DLC);
+        break;
+      case CAN_MIMO_BOBS_ID1 ... CAN_MIMO_BOBS_ID3:
         can_recv_enzhao_obstacle(RxHeader.StdId, RxData, RxHeader.DLC);
         break;
       
@@ -521,7 +524,7 @@ void Can_obstacle_update()
       obstacle_buf[0] = 0x0B;
     }
 
-    HAL_Delay(100);
+    HAL_Delay(5);
     can_send_msg_normal(obstacle_buf, 1, 0x7E0);
 
     update_count = 1;
@@ -561,7 +564,7 @@ void Can_obstacle_update()
     obstacle_buf[6] = 0x02;
     obstacle_buf[7] = 0x06;
 
-    HAL_Delay(100);
+    HAL_Delay(5);
     can_send_msg_normal(obstacle_buf, 8, 0x7E2);
 
     update_count = 3;
@@ -730,8 +733,9 @@ void can_send_msg_normal(unsigned char *data, unsigned char length, unsigned int
     //等待有可使用的邮箱，并发送出去
     uint32_t time2 = HAL_GetTick();
     uint32_t TxMailBox0 = 0;
-    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) == 0 && HAL_GetTick() - time2 < 5)
-      ;
+    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) == 0 && HAL_GetTick() - time2 < 2)
+    {
+    }
     HAL_CAN_AddTxMessage(&hcan, &TxHeader, Data, &TxMailBox0);
   }
 }
@@ -776,8 +780,9 @@ void can_send_msg_normalstd(unsigned char *data, unsigned char length, unsigned
     //等待有可使用的邮箱，并发送出去
     uint32_t time2 = HAL_GetTick();
     uint32_t TxMailBox0 = 0;
-    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) == 0 && HAL_GetTick() - time2 < 5)
-      ;
+    while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan) == 0 && HAL_GetTick() - time2 < 2)
+    {
+    }
     HAL_CAN_AddTxMessage(&hcan, &TxHeader, Datas, &TxMailBox0);
   }
 }
@@ -803,6 +808,7 @@ void check_radar_link_status()
   //Check_dev_link(&mimo_360_info,3000,NULL,sizeof(uavr_obs));
   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));
 }
 
 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_engine.c

@@ -115,7 +115,7 @@ GEELY_MONI geely_moni = {0};
 
 
 Connect_check geely_engin_link;
-
+int geely_version2_count = 0;
 void EngGeelyCanRecvHookFunction(uint32_t cellCanID, uint8_t data[], uint8_t len)
 {
     if(cellCanID == 0x100) //第二版协议识别
@@ -218,7 +218,8 @@ void EngGeelyCanRecvHookFunction(uint32_t cellCanID, uint8_t data[], uint8_t len
         if(cellCanID >= 0x1E1 && cellCanID <= 0x1EF)
         {
             geely_engin_link.connect_status = COMP_NORMAL;
-            if(geely_data2.version2_flag != true)
+            geely_version2_count++;
+            if(geely_data2.version2_flag != true && (geely_version2_count > 10))
             {
              geely_data2.version2_flag = true;
             } //作为第二版协议标志
@@ -305,9 +306,10 @@ void Geely_version2_init(void)
     static uint8_t geely_init_count = 15;
     if(geely_data2.version2_flag != true && geely_init_count > 1)
     {
-        geely_sendinfo2.client_cmd = 0x10;
+        geely_sendinfo2.client_cmd = 0x12;
         geely_sendinfo2.syscontrol = 0;
         geely_sendinfo2.check_sum1 = geely_sendinfo2.client_cmd + geely_sendinfo2.syscontrol + (geely_sendinfo2.message_conut1 << 4);
+        geely_sendinfo2.check_sum1 = 0 - geely_sendinfo2.check_sum1;
         memcpy(&can_buf[0],&geely_sendinfo2.client_cmd,8);
         geely_sendinfo2.message_conut1++;
 
@@ -353,7 +355,7 @@ void Geely_version2_send_info( void )
         } 
 
         geely_sendinfo2.syscontrol = 0;
-        geely_sendinfo2.check_sum1 = geely_sendinfo2.client_cmd + geely_sendinfo2.syscontrol + geely_sendinfo2.message_conut1;
+        geely_sendinfo2.check_sum1 = geely_sendinfo2.client_cmd + geely_sendinfo2.syscontrol + (geely_sendinfo2.message_conut1 << 4);
         geely_sendinfo2.check_sum1 = 0 - geely_sendinfo2.check_sum1;
         memcpy(&can_buf[0],&geely_sendinfo2.client_cmd,8);
         geely_sendinfo2.message_conut1++;
@@ -469,8 +471,8 @@ void send_msg_to_geely(void)
 
         can_send_msg_normalstd(&can_buf[0],8,0x1F0);
 
-        memcpy(&can_buf[0],&planep.pos_x,4);
-        memcpy(&can_buf[4],&planep.pos_y,4);
+        memcpy(&can_buf[0],&planep.pos_x1,4);
+        memcpy(&can_buf[4],&planep.pos_y1,4);
         can_send_msg_normalstd(&can_buf[0],8,0x1F1);
 
     }

Src/soft_obstacle.c

@@ -53,7 +53,7 @@ void can_recv_enzhao_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len)
     case CAN_MIMO_BOBS_ID1:
         memcpy(&B_radar[0], data, 8);
         recv_comB_flag = recv_comB_flag | 1;
-        Dev.Radar.facid_F = FAC_MIMO_RB;
+        Dev.Radar.facid_B = FAC_MIMO_RB;
         break;
     case CAN_MIMO_BOBS_ID2:
         memcpy(&B_radar[1], data, 8);
@@ -201,7 +201,7 @@ void can_recv_mimo_radar_version(uint32_t cellCanID, uint8_t data[], uint8_t len
         else if(data[0] == 0xFB && data[1] == 0x03 && data[2] == 0)
         {   
             //设置距离最大值反馈
-            if(data[3] == 0xB1 && data[4] == 0xc1 && data[5] == 0)
+            if(data[3] == 0xB2 && data[4] == 0xE1)
             {
                 pmu_set_ack(_MSGID_SET,MSGID_SET_LACKLOSS_CAL,mimo_lackloss.cal_distance,mimo_lackloss.strength);
             }
@@ -1056,41 +1056,44 @@ void can_recv_mocib_updata_read_set_hookfunction(uint32_t cellCanID, uint8_t dat
 void can_send_info_to_mimo()
 {
     static int mimo_50HZ = 0;
+    static int mimo_49HZ = 0;
     if (mimo_f_info.Link.connect_status == COMP_NORMAL || mimo_b_info.Link.connect_status == COMP_NORMAL /*|| 
-        mimo_360_info.connect_status == COMP_NORMAL*/)
+        mimo_360_info.connect_status == COMP_NORMAL*/ || Dev.Part_Fradar_Link.connect_status == COMP_NORMAL)
     {
+        int16_t index = 0;
+        short tmpShort = 0;
+        int8_t tmpChar = 0;
+        uint8_t send_mimo_data[8] = {0};
+        
         if (HAL_GetTick() - mimo_50HZ > 20) 
         {
             mimo_50HZ = HAL_GetTick();
 
-            int16_t index = 0;
-            short tmpShort = 0;
-            int8_t tmpChar = 0;
-            uint8_t send_mimo_data[8] = {0};
-
-            tmpShort = 1;
-            short2buf(&send_mimo_data[index], &tmpShort);
-            index += 2;
-            // 俯仰
-            tmpShort = planep.pitch_angle;
-            short2buf(&send_mimo_data[index], &tmpShort);
-            index += 2;
-
-            // 横滚
-            tmpShort = planep.roll_angle;
-            short2buf(&send_mimo_data[index], &tmpShort);
-            index += 2;
+            
 
-            //航向
-            if(planep.yaw < 0)
-                tmpShort = planep.yaw + 360;
-            else
-                tmpShort = planep.yaw;
+            // tmpShort = 1;
+            // short2buf(&send_mimo_data[index], &tmpShort);
+            // index += 2;
+            // // 俯仰
+            // tmpShort = planep.pitch_angle;
+            // short2buf(&send_mimo_data[index], &tmpShort);
+            // index += 2;
+
+            // // 横滚
+            // tmpShort = planep.roll_angle;
+            // short2buf(&send_mimo_data[index], &tmpShort);
+            // index += 2;
+
+            // //航向
+            // if(planep.yaw < 0)
+            //     tmpShort = planep.yaw + 360;
+            // else
+            //     tmpShort = planep.yaw;
             
-            short2buf(&send_mimo_data[index], &tmpShort);
-            index += 2;
+            // short2buf(&send_mimo_data[index], &tmpShort);
+            // index += 2;
 
-            can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), 0x3740403);
+            // can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), 0x3740403);
 
             index = 0;
             // 俯仰
@@ -1122,6 +1125,11 @@ void can_send_info_to_mimo()
             send_mimo_data[index] = tmpChar;
 
             can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO1);
+        }
+
+        if (HAL_GetTick() - mimo_49HZ > 21)
+        {
+            mimo_49HZ = HAL_GetTick();
 
             index = 0;
             //高度

Src/soft_p_2_c.c

@@ -544,6 +544,68 @@ void pmu_to_con_DMradar_data(void)
 
         uart2_send_msg(msg_buf, index);
     }
+    
+    if(DM_4dstatus.connect_status == COMP_NORMAL)
+    {
+        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;
+        
+        DM4d_recv_flag = 1;
+
+        remain_target_num = FMU_4D_info.target_num;
+        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_F4DRADAR;
+
+            remain_lastcount = remain_target_num % 48;
+            if( remain_lastcount != 0)
+            {
+                pack_count = remain_target_num / 48 + 1;
+                if(pack_count == 1)
+                    send_bytes = remain_lastcount * 5;
+                else
+                    send_bytes = 48 * 5;
+            }
+            else
+            {
+                if(remain_target_num == 0)
+                    send_bytes = 0;
+                else
+                    send_bytes = 48 * 5;
+            }
+
+            msg_buf[index++] = send_pack; 
+            msg_buf[index++] = FMU_4D_info.target_num;
+
+            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++;
+        }
+        DM4d_recv_flag = 0;
+    }
 }
 
 
@@ -661,6 +723,13 @@ 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)
+    {
+        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.Bms_Link.connect_status == COMP_NORMAL && devinfo_time.bms == true)
     // {
     //     msg_buf[index++] = DEV_BMS;
@@ -1183,6 +1252,8 @@ void Check_Rst(void)
 /******************void check_fmu_link()************************
  * ****************检查是否收到FMU信息************************************
  * ****************************************************************/
+int time_heart[3] = {0};
+int time_count[3] = {0};//test
 static uint32_t fmu_link_time = 0;
 void check_fmu_link()
 {
@@ -1212,7 +1283,6 @@ void check_fmu_link()
 _pmu_pin pmu_pin;
 bool recv_fmu_data = false;
 Set_info msgidset;
-uint32_t DMJZ=0;
 void uart_recv_con_msg()
 {
     check_fmu_link();
@@ -1660,10 +1730,7 @@ void uart_recv_con_msg()
                 uint8_t can_buf[8] = {0};
                 if(Dev.Lackloss_Link.connect_status == COMP_NORMAL && Dev.Checklow.facid == FAC_VK)
                 {
-                    mimo_lackloss.cal_distance = mimo_lackloss.distance / 10 + 5;
-                    put_date_to_can(can_buf,0xFA,0x03,0x00,0xB1,0XC1,0x00,0X00,0X00);
-                    can_buf[6] = mimo_lackloss.cal_distance & 0xff;
-                    can_buf[5] = (mimo_lackloss.cal_distance >> 8) & 0xff;
+                    put_date_to_can(can_buf,0xFA,0x03,0x00,0xB2,0XE1,0x00,0X00,0X00);
                     can_buf[7] = (can_buf[1]+can_buf[2]+can_buf[3]+can_buf[4]+can_buf[5]+can_buf[6]) & 0xff;
                     can_send_msg_normalstd(can_buf,8,0xFA); 
                 }
@@ -1741,6 +1808,8 @@ void uart_recv_con_msg()
             }
             break;
         case _MSGID_HEART:
+            time_heart[1] = HAL_GetTick();
+            time_count[1]++;
             pmu_heart_flag = true;
             break;
         case _MSGID_SHA1:

Src/soft_seed_device.c

@@ -41,7 +41,7 @@ void Weight_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
 
         Dev.Weight.facid = FAC_VK;
         Dev.Weight_Link.connect_status = COMP_NORMAL;
-        Dev.Weight_Link.recv_time = HAL_GetTick();
+        Dev.Weight_Link.recv_time = HAL_GetTick(); 
 
         break;
     //称重传感器设置回馈
@@ -64,7 +64,7 @@ void Weight_recieved_hookfuction(uint32_t CanID, uint8_t data[], uint8_t len)
             break;
         case 0xf6:
         {
-            char ver_buf[8] = {0};
+            char ver_buf[10] = {0};
             int ver_temp32 = 0;
             memcpy(&ver_temp32,&data[1],4);
             Int2String(ver_temp32,&ver_buf[2],6);
@@ -422,7 +422,6 @@ void Set_Seed_Weight_Par(uint8_t device_type,char *factory)
                 can_buf[7] = 0;
 
                 can_send_msg_normal((unsigned char *)&can_buf, 8, SEND_EFT_INFO);
-                HAL_Delay(10);
             }        
             break;
         case WEIGHT_DEVICE:
@@ -471,7 +470,6 @@ void Set_Seed_Weight_Par(uint8_t device_type,char *factory)
                     break;
                 }
                 can_send_msg_normal((unsigned char *)&can_buf, 8, 0x88BB);
-                HAL_Delay(10);
                 weight_order.type = 0;
             }
             else
@@ -515,7 +513,6 @@ void Set_Seed_Weight_Par(uint8_t device_type,char *factory)
                         break;
                     }
                     can_send_msg_normal((unsigned char *)&can_buf, 8, SEND_EFT_INFO);
-                    HAL_Delay(10);
                     weight_order.type = 0;
                 }
             }
@@ -640,43 +637,44 @@ void check_dev_type_link(void )
 
 }
 
-// void DM_obs_test( void )
-// {
-//     static uint32_t time_50hz = 0;
-//     static uint32_t time_49hz = 0;
-//     static uint32_t time_48hz = 0;
-//     static uint32_t time_47hz = 0;
-//     uint8_t can_buf[8] = {0};
-//     if(Check_Timer_Ready(&time_50hz,20))
-//     {
-//         memcpy(&can_buf[0],&planep.QuaterQ0,4);
-//         memcpy(&can_buf[4],&planep.QuaterQ1,4);
-//         can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2345);   
-//     }
-
-//     if(Check_Timer_Ready(&time_49hz,20))
-//     {
-//         memcpy(&can_buf[0],&planep.QuaterQ2,4);
-//         memcpy(&can_buf[4],&planep.QuaterQ3,4);
-//         can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2346);
-//     }
-
-//     if(Check_Timer_Ready(&time_48hz,50))
-//     {
-//         memcpy(&can_buf[0],&planep.pos_x,4);
-//         memcpy(&can_buf[4],&planep.pos_y,4);
-//         can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2347);
-//     }
-
-//     if(Check_Timer_Ready(&time_47hz,50))
-//     {
-//         memcpy(&can_buf[0],&planep.pos_z,4);
-//         memcpy(&can_buf[4],&planep.pos_flag,4);
-//         can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2348);
-//     }
-
-
-// }
+void DM_obs_test( void )
+{
+    if(Dev.Part_Fradar_Link.connect_status != COMP_NORMAL)
+        return;
+
+    static uint32_t time_50hz = 0;
+    static uint32_t time_49hz = 0;
+    static uint32_t time_48hz = 0;
+    static uint32_t time_47hz = 0;
+    uint8_t can_buf[8] = {0};
+    if(Check_Timer_Ready(&time_50hz,20))
+    {
+        memcpy(&can_buf[0],&planep.QuaterQ0,4);
+        memcpy(&can_buf[4],&planep.QuaterQ1,4);
+        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2345);   
+    }
+
+    if(Check_Timer_Ready(&time_49hz,20))
+    {
+        memcpy(&can_buf[0],&planep.QuaterQ2,4);
+        memcpy(&can_buf[4],&planep.QuaterQ3,4);
+        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2346);
+    }
+
+    if(Check_Timer_Ready(&time_48hz,19))
+    {
+        memcpy(&can_buf[0],&planep.pos_x,4);
+        memcpy(&can_buf[4],&planep.pos_y,4);
+        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2347);
+    }
+
+    if(Check_Timer_Ready(&time_47hz,19))
+    {
+        memcpy(&can_buf[0],&planep.pos_z,4);
+        memcpy(&can_buf[4],&planep.pos_flag,4);
+        can_send_msg_normal((unsigned char *)&can_buf, 8, 0x2348);
+    }
+}
 /**
   * @file    update_device_type_data
   * @brief   更新设备信息
@@ -1147,6 +1145,19 @@ 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)
+        {
+            Dev.Part_radarF.X1 = F_4DRadar[0][0];
+            Dev.Part_radarF.Y1 = F_4DRadar[0][1];
+            Dev.Part_radarF.Z1 = F_4DRadar[0][2];
+            Dev.Part_radarF.X2 = F_4DRadar[1][0];
+            Dev.Part_radarF.Y2 = F_4DRadar[1][1];
+            Dev.Part_radarF.Z2 = F_4DRadar[1][2];
+            Dev.Part_radarF.X3 = F_4DRadar[2][0];
+            Dev.Part_radarF.Y3 = F_4DRadar[2][1];
+            Dev.Part_radarF.Z3 = F_4DRadar[2][2];
+        }
+
         if(Dev.Temp_Sensor_Link.connect_status == COMP_NORMAL)
         {
             switch (Dev.Temp_sensor.facid)

Src/soft_terrain.c

@@ -102,7 +102,9 @@ DM_RADAR DM_T_info,FMU_DM_info;
 DM_RADAR DM_F_info;
 uint8_t dm_i = 0;
 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)
 {
@@ -214,7 +216,10 @@ void DM_terrain_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t
     }
 }
 
-
+int16_t F_4DRadar[3][3]={0};  // X Y Z
+DM_4dFRADAR DM_F4d;
+int dm_4df_i = 0;
+DM_4DRADAR FMU_4D_info;
 void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len)
 {
     if(cellCanID == 0XA01300) //多点协议
@@ -268,6 +273,77 @@ void DM_Fobs_recieved_hookfuction(uint32_t cellCanID,uint8_t data[], uint8_t len
 
         DM_f_info.distance_y = data[3] + data[4] * 256;
     }
+
+    //4D前避障雷达协议 点云
+    if(cellCanID == 0XA01310)
+    {
+        DM_4dstatus.connect_status = COMP_NORMAL;
+        DM_status.recv_time = HAL_GetTick();
+        Dev.Part_radarF.facid = FAC_DM_RF;
+
+        DM_F4d.byte7.frame_flag = data[7];
+        
+        if(DM_F4d.byte7.flag.head!= 0 ) //头
+        {
+            memcpy(&DM_F4d.target_num,&data[0],7);
+            dm_4df_i = 0;
+        }
+        else if(DM_F4d.byte7.flag.tail != 0) //尾
+        {
+            if(DM_F4d.target_num != 1)
+            {
+                memcpy(&DM_F4d.RawData[dm_4df_i],&data[0],(DM_F4d.target_num * 5) % 7);
+            }
+            else
+            {
+                memcpy(&DM_F4d.RawData[dm_4df_i],&data[0],5);
+            }
+
+            if(DM_F4d.crc == Get_Crc16(&DM_F4d.RawData[0],DM_F4d.target_num * 5) &&  DM4d_recv_flag == 0)
+            {
+                FMU_4D_info.target_num = DM_F4d.target_num;
+                memcpy(&FMU_4D_info.buf,&DM_F4d.RawData,DM_F4d.target_num *5);
+            }
+        }
+        else
+        {
+            memcpy(&DM_F4d.RawData[dm_4df_i],&data[0],7);
+            dm_4df_i += 7;
+        }
+        if(dm_4df_i >= 254 * 5)
+        {
+            dm_4df_i = 0;
+        }
+    }
+
+    
+    //4D前避障雷达协议 单点
+    if(cellCanID == 0XA01302)
+    {
+        F_4DRadar[0][0] = data[1] + data[2] * 256;
+        F_4DRadar[0][1] = data[3] + data[4] * 256;
+        F_4DRadar[0][2] = data[5] + data[6] * 256;
+
+        Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
+        Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
+        Dev.Part_radarF.facid = FAC_DM_RF;
+    }
+    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)
+    {
+        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)
     {

Src/soft_test.c

@@ -184,7 +184,7 @@ void pmu_inside_led()
  **/
 uint8_t recv_vk_protocol[MAX_UART_BUF] = {0};
 uint8_t candebug_protocol[MAX_UART_BUF] = {0};
-uint8_t send_uart_buffer[MAX_UART_BUF] = {0};
+uint8_t send_uart_buffer[ MAX_UART_BUF * 3 ] = {0};
 void user_init(void)
 {
   //can pwm uart 初始化

Src/soft_timer.c

@@ -74,6 +74,7 @@ void timer_function()
 
         devinfo_time.radar = true;
         devinfo_time.part_radar = true;
+        devinfo_time.part_Fradar = true;
         devinfo_time.flow = true;
 
     }

Src/soft_uart.c

@@ -176,7 +176,7 @@ void UART_Init(void)
 	HAL_UART_Receive_DMA(&huart2, (uint8_t *)uart_info.uart2_recv_buf, MAX_UART_BUF);
 	//开启错误处理机制
 	__HAL_UART_ENABLE_IT(&huart2, UART_IT_ERR);
-
+	
 	//uart3
 	//HAL_UART_Receive_IT(&huart3, (uint8_t *)uart_info.uart3_recv_buf, 1);
 
@@ -207,7 +207,14 @@ void USER_UART_IRQHandler(UART_HandleTypeDef *huart)
 			__HAL_UART_CLEAR_IDLEFLAG(&huart2); 		
 			HAL_UART_AbortReceive(&huart2);//HAL_UART_DMAStop 有问题
 			uart_info.uart_dma_recv_counts = MAX_UART_BUF - __HAL_DMA_GET_COUNTER(huart->hdmarx);
-
+			for(uint8_t i = 0;i<uart_info.uart_dma_recv_counts;i++)
+			{
+				if(uart_info.uart2_recv_buf[i] == 0xfe && uart_info.uart2_recv_buf[i+5] == 27)
+				{
+					time_heart[0] = HAL_GetTick();
+					time_count[0]++;
+				}
+			}
 			rkfifo_in(&recv_rkfifo,uart_info.uart2_recv_buf,uart_info.uart_dma_recv_counts);
 			
 			memset(uart_info.uart2_recv_buf,0,MAX_UART_BUF);
@@ -309,7 +316,7 @@ Vk_protocol vk_data = {.head = 0XFE,
 						   .system_id = 0,
 						   .head_bytes = 6,
 						   .check_bytes = 2};
-
+						   
 void check_uart_data(rkfifo_t *fifo)
 {
 	uint8_t c = 0;
@@ -366,6 +373,11 @@ void check_uart_data(rkfifo_t *fifo)
 			break;
 		case RX_MSGID:
 			vk_data.msg_id = c;
+			if(c == 27)
+			{
+				time_heart[2] = HAL_GetTick();
+				time_count[2]++;
+			}
 			recv_step = RX_PAYLOAD;
 			i = 0;
 			break;
@@ -400,12 +412,9 @@ void check_uart_data(rkfifo_t *fifo)
 			{
 				if(vk_data.group_id == GROUP_ID_FCU || vk_data.group_id == GROUP_ID_PMU_UPDATE)
 				{
-					if(uart_info.fcu_buf_flag == false)
-					{
-						fcu_protocol.msg_id = vk_data.msg_id;
-						memcpy(&fcu_protocol.payload[0],&vk_data.payload[0],vk_data.len + vk_data.head_bytes + vk_data.check_bytes);
-						uart_info.fcu_buf_flag = true;  
-					}
+					fcu_protocol.msg_id = vk_data.msg_id;
+					memcpy(&fcu_protocol.payload[0],&vk_data.payload[0],vk_data.len + vk_data.head_bytes + vk_data.check_bytes);
+					uart_info.fcu_buf_flag = true; 
 				}
 				//莫之比雷达升级
 				else if((vk_data.group_id == GROUP_ID_F_UPDATE && uavr11_info.Link.connect_status != COMP_NOEXIST) ||
@@ -495,5 +504,7 @@ void check_uart_data(rkfifo_t *fifo)
 			//vk_protocol_test[5]++;
 			break;
 		}
+		if( uart_info.fcu_buf_flag == true)
+			break;
 	}
 }

Src/soft_update.c

@@ -86,6 +86,10 @@ void Vk_Update_Device_Protocol(void)
             {
                 memcpy(&can_buf[5],"DW1",3);
             }
+            else if(Dev.Part_radarF.facid == FAC_DM_RF)
+            {
+                memcpy(&can_buf[5],"D4F",3);
+            }
             break;
         case UPDATE_OBS_B:
             if(Dev.Radar.facid_B == FAC_MIMO_RB)
@@ -100,7 +104,10 @@ void Vk_Update_Device_Protocol(void)
             }
             else if(Dev.Radar.facid_T == FAC_DM_RT)
             {
-                memcpy(&can_buf[5],"DS1",3);
+                if(Dev.Part_Tradar_Link.connect_status == COMP_NORMAL)
+                    memcpy(&can_buf[5],"D4T",3);
+                else
+                    memcpy(&can_buf[5],"DS1",3);
             }
             break;
         case UPDATE_OBS360:
@@ -206,6 +213,12 @@ 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],"D4F",3) == 0)
+            {
+                Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
+                Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
+                Dev.Part_radarF.facid = FAC_DM_RF;
+            }
             else
             {
                 DM_f_info.Link.connect_status = COMP_NORMAL;
@@ -232,6 +245,12 @@ 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],"D4B",3) == 0)
+            {
+                // Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
+                // Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
+                // Dev.Part_radarT.facid = FAC_DM_RF;
+            }
             break;
         case UPDATE_TERAIN:
             if(memcmp((char *)&data[1],"TR0",3) == 0)
@@ -257,6 +276,12 @@ void Update_Dev_Bootversion_Function(uint8_t data[])
                 }
                 regist_dev_info(&dev_ter,DEVICE_TERRA,false,NULL,0,DM_ter_info.version,10,NULL,0,"dmter",6);
             }
+            else if(memcmp((char *)&data[1],"D4T",3) == 0)
+            {
+                Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
+                Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
+                Dev.Part_radarT.facid = FAC_DM_RF;
+            }
             else
             {
                 DM_ter_info.Link.connect_status = COMP_NORMAL;

Src/soft_version.c

@@ -241,14 +241,14 @@ void get_radar_version_and_sn(void)
  **/
 void get_flowmeter_version_and_sn(void)
 {
-
+    static uint8_t send_interval = 0;
     if((Dev.Flow_Link1.connect_status == COMP_NORMAL || Dev.Flow_Link2.connect_status == COMP_NORMAL) && 
     (Dev.Flow.facid == FAC_MIMO_SIG || Dev.Flow.facid == FAC_MIMO_DOU))
     {
         uint8_t can_buf[8] = {0};
 
         //读取K值 和序列号
-        if(flow_mimo1.get_k_count  < 10 && Dev.Flow_Link1.connect_status == COMP_NORMAL )
+        if(flow_mimo1.get_k_count  < 5 && Dev.Flow_Link1.connect_status == COMP_NORMAL )
         {   
             //K值
             can_buf[0] = 0xfa;
@@ -263,10 +263,13 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 0)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 1;
+            }
             can_buf[7] = 0;
             
-            HAL_Delay(50);
             //序列号
             can_buf[0] = 0xfa;
             can_buf[1] = 0x10;
@@ -280,10 +283,13 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 1)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 2;
+            }
             can_buf[7] = 0;
 
-            HAL_Delay(50);
             //软件版本
             can_buf[0] = 0xfa;
             can_buf[1] = 0x10;
@@ -297,12 +303,17 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 2)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 0;
+            }
             can_buf[7] = 0;            
 
-            flow_mimo1.get_k_count++;
+            if(send_interval == 0)
+                flow_mimo1.get_k_count++;
         }
-        else if(flow_mimo2.get_k_count  < 10 && Dev.Flow_Link2.connect_status == COMP_NORMAL)
+        else if(flow_mimo2.get_k_count  < 5 && Dev.Flow_Link2.connect_status == COMP_NORMAL)
         {   
             can_buf[0] = 0xfa;
             can_buf[1] = 0x10;
@@ -316,7 +327,11 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 0)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 1;
+            }
             can_buf[7] = 0;
 
             //序列号
@@ -332,7 +347,11 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 1)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 2;
+            }
             can_buf[7] = 0;
 
             //软件版本
@@ -348,7 +367,11 @@ void get_flowmeter_version_and_sn(void)
             {
                 can_buf[7] += can_buf[i];
             }
-            can_send_msg_normalstd(can_buf, 8, 0xFA);
+            if(send_interval == 2)
+            {
+                can_send_msg_normalstd(can_buf, 8, 0xFA);
+                send_interval = 0;
+            }
             can_buf[7] = 0;  
 
             flow_mimo2.get_k_count++;
@@ -372,9 +395,9 @@ void get_flowmeter_version_and_sn(void)
 
             flow_mimo1.send_k_count--;
             can_send_msg_normalstd(can_buf, 8, 0xFA);
-            HAL_Delay(50);
+            HAL_Delay(5);
         }
-        if(flow_mimo2.send_k_count > 0 && Dev.Flow_Link2.connect_status == COMP_NORMAL)
+        else if(flow_mimo2.send_k_count > 0 && Dev.Flow_Link2.connect_status == COMP_NORMAL)
         {            
             can_buf[0] = 0xfa;
             can_buf[1] = 0x10;
@@ -481,7 +504,7 @@ void get_device_version_and_sn(void)
 {
     static uint32_t circu_time = 0;
 
-    if(Check_Timer_Ready(&circu_time,_1_HZ_))
+    if(Check_Timer_Ready(&circu_time,_2_HZ_))
     {
         //获取雷达版本和SN号
         get_radar_version_and_sn();

Src/soft_water_device.c

@@ -236,8 +236,6 @@ void can_sendmsg_flow(void)
         can_buf[7] = 0XFE;
         can_send_msg_normal(can_buf,8,0x216020);
 
-        HAL_Delay(100);
-
         can_buf[0] = 0XF3;
         can_buf[7] = 0XFE;
         can_send_msg_normal(can_buf,8,0x216020);