#include "soft_terrain.h" #include "stdint.h" #include "stdbool.h" #include "tim.h" #include "can.h" #include "soft_can.h" #include "common.h" #include "soft_seed_device.h" #include "soft_crc.h" #include "string.h" #include "soft_obstacle.h" #include "soft_seed_device.h" #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; //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 **/ 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 **/ 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: //单点雷达 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 // printf("%d %d\n", mimo_ter_info.height, m.muniu_hight); //输出到串口助手上 需要pringtf重定向 } mimo_ter_info.Link.recv_time = HAL_GetTick(); mimo_ter_info.Link.connect_status = COMP_NORMAL; Dev.Radar.facid_T = FAC_MIMO_RT; break; case CAN_MIMO_T_ID1: memcpy(&T_radar[0], data, 8); Dev.Part_Tradar_Link.recv_time = HAL_GetTick(); Dev.Part_Tradar_Link.connect_status = COMP_NORMAL; Dev.Part_radarT.facid = FAC_MIMO_RT; Dev.Radar.facid_T = FAC_MIMO_RT; break; case CAN_MIMO_T_ID2: memcpy(&T_radar[1], data, 8); break; case CAN_MIMO_T_ID3: memcpy(&T_radar[2], data, 8); break; 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]; // muniu_ter_info.Link.connect_status = COMP_NORMAL; // muniu_ter_info.Link.recv_time = HAL_GetTick(); // } //电目雷达 DM_RADAR DM_T_info,FMU_DM_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) { 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 ) //头 { 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) { FMU_DM_info.target_num = 0; } } else if(DM_T_info.byte7.flag.tail != 0) //尾 { if(DM_T_info.target_num != 1) { 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); } 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); } } else { memcpy(&DM_T_info.buf[dm_i],&data[0],7); dm_i += 7; } if(dm_i >= 255 -7) { dm_i = 0; } } else if(cellCanID == 0x901301)//单点协议 { DM_ter_info.Link.connect_status = COMP_NORMAL; DM_ter_info.Link.recv_time = HAL_GetTick(); Dev.Radar.facid_T = FAC_DM_RT; DM_ter_info.height = data[3] + data[4] * 256; } //版本信息 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 ) //头 { 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); } 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'; 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 } } else if(cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5)) { 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); } else if(cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7)) { 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); } else if(cellCanID == 0x981301 && (data[0] == 0xA || 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); } else if(cellCanID == 0x981301 && data[0] == 0x4 ) { 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 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) { if (cellCanID == 0XA01300) // 多点协议 { 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) // 头 { 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) { FMU_DM_info.target_num = 0; } } else if (DM_T_info.byte7.flag.tail != 0) // 尾 { if (DM_T_info.target_num != 1) { 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); } 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); } } else { memcpy(&DM_T_info.buf[dm_i], &data[0], 7); dm_i += 7; } if (dm_i >= 255 - 7) { dm_i = 0; } } else if(cellCanID == 0XA01301)//单点协议 { DM_f_info.Link.connect_status = COMP_NORMAL; DM_f_info.Link.recv_time = HAL_GetTick(); Dev.Radar.facid_F = FAC_DM_RF; DM_f_info.distance_y = data[3] + data[4] * 256; } // 4D前避障雷达协议 点云 if (cellCanID == 0XA01310) { DM_4dstatus.connect_status = COMP_NORMAL; DM_4dstatus.recv_time = HAL_GetTick(); Dev.Part_radarF.facid = FAC_DM_RF_4D; DM_F4d.byte7.frame_flag = data[7]; if (DM_F4d.byte7.flag.head != 0) // 头 { if(DM_F4d.target_num > 130) DM_F4d.target_num = 130; 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) { if ((DM_F4d.target_num * 5) % 7 != 0) { memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], (DM_F4d.target_num * 5) % 7); dm_4df_i += (DM_F4d.target_num * 5) % 7; } else { memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7); dm_4df_i += 7; } } else { memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 5); dm_4df_i += 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); F4d_send_flag = true; } } 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_4D; } 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) { uint32_t version_temp = 0; DM_T_info.byte7.frame_flag = data[7]; 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) 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); } else if(DM_T_info.byte7.flag.tail != 0) //尾 { 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 } } else if(cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5)) { 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); } else if(cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7)) { 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); } 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); } }