#include "soft_obstacle.h" #include "tim.h" #include "string.h" #include "math.h" #include "soft_terrain.h" #include "soft_uart.h" #include "common.h" #include "soft_can.h" #include "soft_p_2_c.h" #include "stdlib.h" #include "soft_flow.h" #include "common.h" #include "soft_seed_device.h" #include "soft_water_device.h" #include "soft_version.h" #include "soft_eft.h" #include "common.h" uavr_obs uavr11_info = {.get_radar_sensi = 50}; uavr_obs uavr12_info= {.get_radar_sensi = 50}; uavr_obs mimo_f_info = {.signal_qulity = 0}; uavr_obs mimo_b_info = {.signal_qulity = 0}; uavr_obs DM_f_info; /** * @file can_recv_enzhao_obstacle * @brief 恩曌多点避障解析 * @param none * @details * @author Zhang Sir **/ mimo_part_radar F_radar[3]; mimo_part_radar B_radar[3]; uint8_t recv_comF_flag = 0,recv_comB_flag = 0; void can_recv_enzhao_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len) { switch (cellCanID) { case CAN_MIMO_FOBS_ID1: memcpy(&F_radar[0], data, 8); recv_comF_flag = recv_comF_flag | 1; Dev.Radar.facid_F = FAC_MIMO_RF; break; case CAN_MIMO_FOBS_ID2: memcpy(&F_radar[1], data, 8); recv_comF_flag = recv_comF_flag | 2; break; case CAN_MIMO_FOBS_ID3: memcpy(&F_radar[2], data, 8); recv_comF_flag = recv_comF_flag | 4; break; 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; break; case CAN_MIMO_BOBS_ID2: memcpy(&B_radar[1], data, 8); recv_comB_flag = recv_comB_flag | 2; break; case CAN_MIMO_BOBS_ID3: memcpy(&B_radar[2], data, 8); recv_comB_flag = recv_comB_flag | 4; break; default: break; } if(recv_comF_flag == 7) { recv_comF_flag = 0; mimo_buf_sort(&F_radar[0], 3); for (uint8_t i = 0; i < 3; i++) { //X轴小于4M内数据 if (/*(abs(F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD)) < 400) &&*/ F_radar[i].Distance > 0) { mimo_f_info.distance_x = F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD); mimo_f_info.distance_y = F_radar[i].Distance * 0.05f * 100 * cos(F_radar[i].Amuzith * 0.1f / RAD); break; } if(i == 2) { mimo_f_info.distance_x = 0; mimo_f_info.distance_y = 0; } } mimo_f_info.Link.connect_status = COMP_NORMAL; mimo_f_info.Link.recv_time = HAL_GetTick(); } if(recv_comB_flag == 7) { recv_comB_flag = 0; mimo_buf_sort(&B_radar[0], 3); for (uint8_t i = 0; i < 3; i++) { //X轴小于4M内数据 if (/*(abs(B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD)) < 400) &&*/ B_radar[i].Distance > 0) { mimo_b_info.distance_x = B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD); mimo_b_info.distance_y = B_radar[i].Distance * 0.05f * 100 * cos(B_radar[i].Amuzith * 0.1f / RAD); break; } if(i == 2) { mimo_b_info.distance_x = 0; mimo_b_info.distance_y = 0; } } mimo_b_info.Link.recv_time = HAL_GetTick(); mimo_b_info.Link.connect_status = COMP_NORMAL; } } /** * @file can_recv_enzhao_obstacle * @brief 恩曌单避障解析 * @param none * @details * @author Zhang Sir **/ void can_recv_mimo_signal_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len) { uint16_t frame_header = 0; memcpy(&frame_header,&data[0],2); if(frame_header == 0x5AA5 && data[2] == 0x04) { switch (cellCanID) { case CAN_MIMO_FOBS_SIG: mimo_f_info.distance_y = data[3] + data[4] * 256; mimo_f_info.signal_qulity = data[5]; mimo_f_info.Link.recv_time = HAL_GetTick(); mimo_f_info.Link.connect_status = COMP_NORMAL; Dev.Radar.facid_F = FAC_MIMO_RF; break; case CAN_MIMO_BOBS_SIG: mimo_b_info.distance_y = data[3] + data[4] * 256; mimo_b_info.signal_qulity = data[5]; mimo_b_info.Link.recv_time = HAL_GetTick(); mimo_b_info.Link.connect_status = COMP_NORMAL; Dev.Radar.facid_B = FAC_MIMO_RB; break; default: break; } } } /** * @file can_recv_mimo_radar_version * @brief 恩曌设备版本获取 * @param none * @details * @author Zhang Sir **/ void can_recv_mimo_radar_version(uint32_t cellCanID, uint8_t data[], uint8_t len) { static uint8_t mimo_version[28] = {0}; static uint8_t frame_num = 0; static bool frame_head = false; uint8_t i = 0; uint32_t checksum = 0; uint32_t soft_ver = 0; switch (cellCanID) { case 0XFB: if(data[0] == 0XAA && data[1] == 0X55) { if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X03 && data[5] == 0X01 && data[6] == 0X65) { pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x11,0); } else if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X03 && data[5] == 0X02 && data[6] == 0X66) { pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x12,0); } else if(data[2] == 0X03 && data[3] == 0X61 && data[4] == 0X00 && data[5] == 0X03 && data[6] == 0X64) { pmu_set_ack(_MSGID_SET,MSGID_SET_RADAR_FB,0x100,0); } else { frame_num = 0; memcpy(&mimo_version[frame_num],&data[4],4); frame_num += 4; frame_head = true; } } //断料记 else if(data[0] == 0xFB && data[1] == 0x03 && data[2] == 0) { //设置距离最大值反馈 if(data[3] == 0xB1 && data[4] == 0xc1 && data[5] == 0) { pmu_set_ack(_MSGID_SET,MSGID_SET_LACKLOSS_CAL,mimo_lackloss.cal_distance,mimo_lackloss.strength); } } //流量计 else if(data[0] == 0xFB && data[1] == 0x10) //雷达版本和流量计协议有冲突 { switch (data[2]) { case 0: if(data[4] == 0XD1) //流速K { flow_mimo1.flow_k = data[5] * 256 + data[6]; } else if(data[4] == 0XC1)//设置流速K ack { if(data[6] == 0) { flow_mimo1.flow_k = flow_mimo1.flow_calk; if(Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status != COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,flow_mimo1.flow_k,0); } } } else if(data[4] == 0xE2) { if(data[6] == 0 && Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status != COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_FLOW_BACKGROUND,0,0); } flow_inf.ch1.clear_background = false; } else if (data[4] == 0XEB)//sn 两包 协议冲突 协议有问题 { for( i= 0;i<2;i++) //内容第一自字节0X0F?先舍弃一字节, { flow_mimo1.sn[2*i] = ((data[6 + i] >> 4) & 0xf) + '0'; flow_mimo1.sn[2*i+1] = (data[6 + i] & 0xf )+ '0'; } frame_num = 21;//流量序列号的标记 } else if(data[4] == 0XEC)//软件号 { flow_mimo1.soft_version[0] = data[5] + '0'; flow_mimo1.soft_version[1] = data[6] + '0'; } break; case 1: if(data[4] == 0XD1) { flow_mimo2.flow_k = data[5] * 256 + data[6]; } else if(data[4] == 0XC1) { if(data[6] == 0) { flow_mimo2.flow_k = flow_mimo2.flow_calk; if(Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,flow_mimo1.flow_k,flow_mimo2.flow_k); } else if(Dev.Flow_Link1.connect_status != COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_MIMO_FLOW,0,flow_mimo2.flow_k); } } } else if(data[4] == 0xE2) { if(data[6] == 0 && Dev.Flow_Link1.connect_status == COMP_NORMAL && Dev.Flow_Link2.connect_status == COMP_NORMAL) { pmu_set_ack(_MSGID_SET,MSGID_SET_FLOW_BACKGROUND,0,0); } flow_inf.ch1.clear_background = false; } break; default: break; } } else { if(frame_num == 21) { for( i=0;i<5;i++) { flow_mimo1.sn[4+2*i] = ((data[i] >> 4) & 0xf) + '0'; flow_mimo1.sn[4+2*i+1] = (data[i] & 0xf )+ '0'; } //flow_mimo1.sn[1] = 'F'; frame_num = 0; } if(frame_num <= 20) { if(frame_head == true) { memcpy(&mimo_version[frame_num],&data[0],8); frame_num += 8; } if(frame_num == 0x1c) //恩曌SN取中间日期12位,其他有问题 { checksum = 0x01; for(uint8_t i = 0; i < 27; i++) { checksum += mimo_version[i]; } if((checksum & 0xFF) == mimo_version[27]) { memcpy(&soft_ver,&mimo_version[23],4); switch (mimo_version[18]) { case 0x00: mimo_ter_info.version[0] = 'E'; mimo_ter_info.version[1] = 'B'; mimo_ter_info.version[2] = '0'; mimo_ter_info.version[3] = '0'; Int2String(soft_ver,&mimo_ter_info.version[4],6); memcpy(&mimo_ter_info.hard_version,&mimo_version[19],4); if(mimo_ter_info.hard_version == 210221) { mimo_ter_info.version[3] = 'J'; } for( i= 0;i<9;i++) { mimo_ter_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0'; mimo_ter_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0'; } //memcpy(&mimo_ter_info.sn[0],&mimo_version[4],18); regist_dev_info(&dev_ter,DEVICE_TERRA,false,mimo_ter_info.sn,18,mimo_ter_info.version,10,NULL,0,"mimo",5); mimo_ter_info.get_radar_ver_flag = true; mimo_ter_info.Link.boot_flag = false; break; case 0x03: mimo_f_info.version[0] = 'E'; mimo_f_info.version[1] = '1'; mimo_f_info.version[2] = '0'; mimo_f_info.version[3] = '0'; Int2String(soft_ver,&mimo_f_info.version[4],6); memcpy(&mimo_f_info.hard_version,&mimo_version[19],4); if(mimo_f_info.hard_version == 190302) //恩曌协议定义 { mimo_f_info.version[2] = '1'; //极翼 mimo_f_info.version[3] = 'J'; } else { mimo_f_info.version[2] = '0'; //vk mimo_f_info.version[3] = 'V'; } for( i= 0;i<9;i++) { mimo_f_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0'; mimo_f_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0'; } //memcpy(&mimo_f_info.sn[0],&mimo_version[4],12); regist_dev_info(&dev_obsf,DEVICE_OBSF,false,mimo_f_info.sn,18,mimo_f_info.version,10,mimo_f_info.version,10,"mimo",5); mimo_f_info.get_radar_ver_flag = true; mimo_f_info.Link.boot_flag = false; break; case 0x04: mimo_b_info.version[0] = 'E'; mimo_b_info.version[1] = '2'; mimo_b_info.version[2] = '0'; mimo_b_info.version[3] = '0'; Int2String(soft_ver,&mimo_b_info.version[4],6); memcpy(&mimo_b_info.hard_version,&mimo_version[19],4); if(mimo_b_info.hard_version == 190302) //恩曌协议定义 { mimo_b_info.version[2] = '1'; //极翼 mimo_b_info.version[3] = 'J'; } else { mimo_b_info.version[2] = '0'; //vk mimo_b_info.version[3] = 'V'; } for( i= 0;i<9;i++) { mimo_b_info.sn[2*i] = ((mimo_version[7 + i] >> 4) & 0xf) + '0'; mimo_b_info.sn[2*i+1] = (mimo_version[7 + i] & 0xf )+ '0'; } //memcpy(&mimo_b_info.sn[0],&mimo_version[4],12); regist_dev_info(&dev_obsb,DEVICE_OBSB,false,mimo_b_info.sn,18,mimo_b_info.version,10,mimo_b_info.version,10,"mimo",5); mimo_b_info.get_radar_ver_flag = true; mimo_b_info.Link.boot_flag = false; break; default: break; } frame_num = 0; frame_head = false; pmu_send = PMU_SEND_VERSION; } } } else { } } // char_to_hex_string(&data[5], 3, &mimo_ter_info.version[4], 6, "00"); // mimo_ter_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff); break; default: break; } } /** * @file can_recv_mocib_F_obstacle * @brief 莫之比前避障解析 * @param none * @details * @author Zhang Sir **/ bool obs_f_is_link = false; void can_recv_mocib_F_obstacle(uint8_t *data) { uavr11_info.distance_x= (data[0] << 8) + data[1] - 32768; uavr11_info.distance_y = (data[2] << 8) + data[3]; // if(abs(uavr11_info.distance_x) > 400) // { // uavr11_info.distance_y = 0; // uavr11_info.distance_x = 0; // } uavr11_info.Link.connect_status = COMP_NORMAL; uavr11_info.Link.recv_time = HAL_GetTick(); } // /** // * @brief 恩曌360雷达 // */ // #pragma pack(push) // #pragma pack(1) // typedef struct // { // uint8_t totalSect; //总分区数 // uint8_t validSect; // 有效分区数 // uint8_t cycleCounter; // uint8_t reserve0; // uint32_t reserve : 20; // uint32_t height : 12; // 精度 0.01m // } FrameHeader1S; // typedef struct // { // uint8_t sectID; //分区 ID // uint16_t dis; //距离 // int16_t ele; // uint8_t rcs; //目标雷达截面积 // int16_t reserve1; // } SectionPackS; // typedef struct // { // uint32_t ID; // uint8_t data[8]; // } CanMessage_t; // #pragma pack(pop) // #define MAX_SECTNUM 180 // typedef struct // { // uint32_t totalSect; //总分区个数,数组中对应位置对应目标所在分区 // uint32_t validSect; //总分区个数,数组中对应位置对应目标所在分区 // uint32_t completeQ; // 0 代表无数据 1 代表数据接收到但不全 2 代表全部接收成功 // float height; // float dis[MAX_SECTNUM]; // 雷达到目标距离,当目标不存在时距离为 // float ele[MAX_SECTNUM]; // 目标的 RCS // float rcs[MAX_SECTNUM]; // 目标的 RCS // } TargetInfoS; // 原始目标结构信息 // TargetInfoS Targetbuffer = {0}; // 用于缓存 // TargetInfoS TargetOut = {0}; // static int LastSection = -1; // int ValidSect = 0; // comp_status mimo360_link_status = COMP_NOEXIST; // void can_recv_mocib_360_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len) // { // int index; // CanMessage_t *can_tmp_ptr = NULL; // CanMessage_t can_tmp; // FrameHeader1S *frameHead_ptr; // SectionPackS *pack_ptr; // can_tmp_ptr = &can_tmp; // can_tmp_ptr->ID = cellCanID; // memcpy(&can_tmp_ptr->data[0], data, len); // mimo360_link_status = COMP_NORMAL; // if (can_tmp_ptr->ID == 0x301) // 检测到帧头 // { // if (Targetbuffer.completeQ == 1) // 如果数据未接完 // { // memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer)); // } // memset(&Targetbuffer, 0, sizeof(Targetbuffer)); // frameHead_ptr = (FrameHeader1S *)can_tmp_ptr->data; // Targetbuffer.totalSect = frameHead_ptr->totalSect; // Targetbuffer.height = frameHead_ptr->height / 100.0F; // if (frameHead_ptr->validSect == 0) // 如果不存在分区数据 // { // Targetbuffer.completeQ = 2; // Targetbuffer.validSect = 0; // memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer)); // } // else // 如果存在分区数据 // { // ValidSect = frameHead_ptr->validSect; // Targetbuffer.completeQ = 1; // } // LastSection = -1; // } // else if (can_tmp_ptr->ID == 0x302) // 检测到目标数据 // { // pack_ptr = (SectionPackS *)can_tmp_ptr->data; // if (Targetbuffer.completeQ == 1) //如果数据未接收完 // { // index = pack_ptr->sectID; //获取分区 ID // if (LastSection == -1) // { // Targetbuffer.dis[index] = pack_ptr->dis * 0.01F; // Targetbuffer.ele[index] = pack_ptr->ele * 0.01F; // Targetbuffer.rcs[index] = pack_ptr->rcs; // Targetbuffer.validSect++; // LastSection = index; // } // else if (LastSection < index) //分区数据是按照从小到大输出才是正确的 // { // Targetbuffer.dis[index] = pack_ptr->dis * 0.01F; // Targetbuffer.ele[index] = pack_ptr->ele * 0.1F; // Targetbuffer.rcs[index] = pack_ptr->rcs; // Targetbuffer.validSect++; // } // else // 如果不是则存在丢失包含帧 ID 的多个数据包 // { // Targetbuffer.completeQ = 0; //数据不保存 // } // if (Targetbuffer.completeQ == 1) // { // if (ValidSect == Targetbuffer.validSect) //有效个数等于实际接收个数 // { // Targetbuffer.completeQ = 2; // memcpy(&TargetOut, &Targetbuffer, sizeof(Targetbuffer)); // memset(&Targetbuffer, 0, sizeof(Targetbuffer)); // } // } // } // } // } /** * @file can_recv_mocib_B_obstacle * @brief 莫之比后避障障解析 * @param none * @details * @author Zhang Sir **/ bool obs_b_is_link = false; void can_recv_mocib_B_obstacle(uint8_t *data) { uavr12_info.distance_x = (data[0] << 8) + data[1] - 32768; uavr12_info.distance_y = (data[2] << 8) + data[3]; // if(abs(uavr12_info.distance_x) > 400) // { // uavr12_info.distance_x = 0; // uavr12_info.distance_y = 0; // } uavr12_info.Link.connect_status = COMP_NORMAL; uavr12_info.Link.recv_time = HAL_GetTick(); } uint32_t uavr20_ver_time = 0; uint32_t uavr20_sensi_time = 0; char can_get_uavr21_ver_comp = 0; /** * @file can_sendmsg_uavr20 * @brief 给墨汁比雷达发送无人机姿态信息 * @param none * @details * @author Zhang Sir **/ uint32_t uavr20_send_time = 0; void can_sendmsg_uavr20(void) { if (uavr12_info.Link.connect_status == COMP_NORMAL || uavr11_info.Link.connect_status == COMP_NORMAL || uavr56_info.Link.connect_status == COMP_NORMAL) { //10hz发送 if ((HAL_GetTick() - uavr20_send_time > 100) && planep.lock_status == 1) { uavr20_send_time = HAL_GetTick(); int16_t index = 0; short tmpShort = 0; uint8_t send_uavr20_data[16] = {0}; // 开头 send_uavr20_data[index++] = 0XA5; // 俯仰 tmpShort = planep.pitch_angle; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 前后速度 tmpShort = planep.E_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD) + planep.N_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD); short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 横滚 tmpShort = planep.roll_angle; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 左右速度 tmpShort = planep.E_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD) + planep.N_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD); short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 后边的都没用上 // 上下加速度 tmpShort = planep.alt_vel; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 仿地最近距离 tmpShort = uavh30_dist.near; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 仿地最远距离 tmpShort = uavh30_dist.far; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 结束 send_uavr20_data[index++] = 0X5A; can_send_msg_normal(send_uavr20_data, sizeof(send_uavr20_data), SEND_UAV20_MSG); } //读取前雷达版本 else { //读取前雷达灵敏度 if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == false && uavr11_info.get_radar_sensi_count < 5) { if (HAL_GetTick() - uavr20_sensi_time > 1000) { // 开头 uint8_t send_uavr20_sensi[1] = {0}; send_uavr20_sensi[0] = 0x11; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA); uavr20_sensi_time = HAL_GetTick(); uavr11_info.get_radar_sensi_count++; } } else { //读取后雷达灵敏度 if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == false && uavr12_info.get_radar_sensi_count < 5) { if (HAL_GetTick() - uavr20_sensi_time > 1000) { // 开头 uint8_t send_uavr20_sensi[1] = {0}; send_uavr20_sensi[0] = 0x12; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA); uavr20_sensi_time = HAL_GetTick(); uavr12_info.get_radar_sensi_count++; } } else if(uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == false && uavr56_info.get_radar_sensi_count < 5) { if (HAL_GetTick() - uavr20_sensi_time > 1000) { // 开头 uint8_t send_uavr20_sensi[1] = {0}; send_uavr20_sensi[0] = 0x0B; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA); uavr20_sensi_time = HAL_GetTick(); uavr56_info.get_radar_sensi_count++; } } } } } } bool uavrhup_getr1_ack = false; short obsfradar_sensitivity = 50; short obsbradar_sensitivity = 50; /** * @file can_set_radar_sensi * @brief 设置雷达灵敏度 * @param none * @details * @author Zhang Sir **/ void can_set_radar_sensi() { static int radar_sensi_ack_time = 0; // 设置前雷达灵敏度 if (uavr11_info.get_radar_sensi_flag == true && uavr11_info.fcu_set_sensi_flag == true && uavr11_info.set_radar_sensi_count < 5 && HAL_GetTick() - uavr20_sensi_time > 1000 && uavr11_info.Link.connect_status == COMP_NORMAL) { uint8_t send_uavr20_sensi[3] = {0}; uavr11_info.set_radar_sensi_count++; uavr20_sensi_time = HAL_GetTick(); if (uavrhup_getr1_ack == false) { //设置灵敏度先进入boot模式 新版本不进入boot if(uavr11_info.soft_verison >= RADAR_NER_VERSION ) { uavrhup_getr1_ack = true; } else { send_uavr20_sensi[0] = 0x11; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1); } uavr11_info.set_radar_sensi_ack = false; } else { if (uavr11_info.set_radar_sensi_ack == false) { send_uavr20_sensi[0] = 0x11; //大端方式发送 send_uavr20_sensi[1] = (obsfradar_sensitivity >> 8) & 0xff; send_uavr20_sensi[2] = (obsfradar_sensitivity)&0xff; can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA); } else { uavr11_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr11_info.set_radar_sensi_ack = false; uavr11_info.set_radar_sensi_count = 0; } } //超过5次失败后恢复 if (uavr11_info.set_radar_sensi_count >= 5) { uavr11_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr11_info.set_radar_sensi_ack = false; uavr11_info.set_radar_sensi_count = 0; } } //设置后雷达灵敏度 else if (uavr12_info.get_radar_sensi_flag == true && uavr12_info.fcu_set_sensi_flag == true && uavr12_info.set_radar_sensi_count < 5 && HAL_GetTick() - uavr20_sensi_time > 1000 && uavr12_info.Link.connect_status == COMP_NORMAL) { uint8_t send_uavr20_sensi[3] = {0}; uavr12_info.set_radar_sensi_count++; uavr20_sensi_time = HAL_GetTick(); if (uavrhup_getr1_ack == false) { //设置灵敏度先进入boot模式 新版本不进入boot if(uavr12_info.soft_verison >= RADAR_NER_VERSION ) { uavrhup_getr1_ack = true; } else { //设置灵敏度先进入boot模式 send_uavr20_sensi[0] = 0x12; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1); } uavr12_info.set_radar_sensi_ack = false; } else { if (uavr12_info.set_radar_sensi_ack == false) { send_uavr20_sensi[0] = 0x12; send_uavr20_sensi[1] = (obsbradar_sensitivity >> 8) & 0xff; send_uavr20_sensi[2] = (obsbradar_sensitivity)&0xff; can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA); } else { uavr12_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr12_info.set_radar_sensi_ack = false; uavr12_info.set_radar_sensi_count = 0; } } //超过5次失败后恢复 if (uavr12_info.set_radar_sensi_count >= 5) { uavr12_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr12_info.set_radar_sensi_ack = false; uavr12_info.set_radar_sensi_count = 0; } } //设置仿地雷达灵敏度 else if (uavr56_info.get_radar_sensi_flag == true && uavr56_info.fcu_set_sensi_flag == true && uavr56_info.set_radar_sensi_count < 5 && HAL_GetTick() - uavr20_sensi_time > 1000 && uavr56_info.Link.connect_status == COMP_NORMAL) { uint8_t send_uavr20_sensi[3] = {0}; uavr56_info.set_radar_sensi_count++; uavr20_sensi_time = HAL_GetTick(); if (uavrhup_getr1_ack == false) { if(uavr56_info.soft_verison >= RADAR_NER_VERSION ) { uavrhup_getr1_ack = true; } else { //设置灵敏度先进入boot模式 send_uavr20_sensi[0] = 0x0B; can_send_msg_normal(send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1); } uavr56_info.set_radar_sensi_ack = false; } else { if (uavr56_info.set_radar_sensi_ack == false) { send_uavr20_sensi[0] = 0x0B; send_uavr20_sensi[1] = (uavr56_info.fcu_set_sensi >> 8) & 0xff; send_uavr20_sensi[2] = (uavr56_info.fcu_set_sensi)&0xff; can_send_msg_normal(send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA); } else { uavr56_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr56_info.set_radar_sensi_ack = false; uavr56_info.set_radar_sensi_count = 0; } } //超过5次失败后恢复 if (uavr56_info.set_radar_sensi_count >= 5) { uavr56_info.fcu_set_sensi_flag = false; uavrhup_getr1_ack = false; uavr56_info.set_radar_sensi_ack = false; uavr56_info.set_radar_sensi_count = 0; } } //设置莫之比避障灵敏度成功后ACK主控 if (uavr11_info.set_radar_sensi_ack == true || uavr12_info.set_radar_sensi_ack == true || uavr56_info.set_radar_sensi_ack == true) { //同时设置有个1.5s间隔 if(HAL_GetTick() - radar_sensi_ack_time > 1500) { radar_sensi_ack_time = HAL_GetTick(); if (uavr11_info.set_radar_sensi_ack == true) { pmu_set_ack(22, 1, uavr11_info.get_radar_sensi,0); uavr11_info.set_radar_sensi_ack = false; uavr11_info.fcu_set_sensi_flag = false; } else if (uavr12_info.set_radar_sensi_ack == true) { pmu_set_ack(22, 2, uavr12_info.get_radar_sensi,0); uavr12_info.set_radar_sensi_ack = false; uavr12_info.fcu_set_sensi_flag = false; } else if(uavr56_info.set_radar_sensi_ack == true) { pmu_set_ack(22, 6, uavr56_info.get_radar_sensi,0); uavr56_info.set_radar_sensi_ack = false; uavr56_info.fcu_set_sensi_flag = false; } } } } void can_recv_mocib_updata_read_set_hookfunction(uint32_t cellCanID, uint8_t data[]) { //AG代码 和雷达升级不兼容,优先升级 if (Rupdate.update_flag == true) { //避障雷达升级 if (Rupdate.buf_flag == false) { memcpy(Rupdate.update_buf, data, 8); Rupdate.buf_flag = true; switch (cellCanID) { case 0x7E1: Rupdate.U7E1 = true; break; case 0x7E3: Rupdate.U7E3 = true; break; case 0x7E6: Rupdate.U7E6 = true; break; default: break; } } } else { switch (cellCanID) { //莫之比雷达反馈版本信息 case CAN_UAVRH_UPDATE_R1: uavrhup_getr1_ack = true; break; //case CAN_UAVRH_VER_R: // if (uavr11_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 1) // { // //char_to_hex_string(data, 4, uavr11_info.version, 10, "11"); // uavr11_info.get_radar_ver_flag = true; // //升完级发送版本信息 // if(HAL_GetTick() > 10000) // { // pmu_send = VERSION; // } // } // else if (uavr12_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 2) // { // //char_to_hex_string(data, 4, uavr12_info.version, 10, "12"); // uavr12_info.get_radar_ver_flag = true; // if(HAL_GetTick() > 10000) // { // pmu_send = VERSION; // } // } // else if (uavr56_info.get_radar_ver_flag == false && can_get_uavr21_ver_comp == 3) // { // //char_to_hex_string(data, 4, uavr56_info.version, 10, "56"); // uavr56_info.get_radar_ver_flag = true; // if(HAL_GetTick() > 10000) // { // pmu_send = VERSION; // } // } // break; //莫之比雷达设置灵敏度及反馈 case CAN_UAVRH_SENSI_SA: if (data[0] == 0x11) { uavr11_info.set_radar_sensi_ack = true; //莫之比大端模式 uavr11_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } else if (data[0] == 0x12) { uavr12_info.set_radar_sensi_ack = true; //莫之比大端模式 uavr12_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } else if(data[0] == 0x0B) { uavr56_info.set_radar_sensi_ack = true; uavr56_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } break; //莫之比雷达读取灵敏度及反馈 case CAN_UAVRH_SENSI_RA: if (data[0] == 0x11) { uavr11_info.get_radar_sensi_flag = true; //莫之比大端模式 uavr11_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } else if (data[0] == 0x12) { uavr12_info.get_radar_sensi_flag = true; //莫之比大端模式 uavr12_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } else if(data[0] == 0x0B) { uavr56_info.get_radar_sensi_flag = true; uavr56_info.get_radar_sensi = ((data[1] << 8) & 0xff00) + data[2]; } default: break; } } } /** * @file can_send_info_to_mimo * @brief 给恩曌避障发送姿态信息 * @param none * @details * @author Zhang Sir **/ void can_send_info_to_mimo() { static int mimo_50HZ = 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*/) { 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; short2buf(&send_mimo_data[index], &tmpShort); index += 2; can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), 0x3740403); index = 0; // 俯仰 tmpShort = -planep.roll_angle; short2buf(&send_mimo_data[index], &tmpShort); index += 2; // 横滚 tmpShort = planep.pitch_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; // 前后速度 tmpChar = (planep.E_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD) + planep.N_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD)) / 10; //0.1m/s send_mimo_data[index++] = tmpChar; //雷达安装俯仰角 tmpChar = 0; send_mimo_data[index] = tmpChar; can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO1); index = 0; //高度 tmpShort = planep.alt; short2buf(&send_mimo_data[index], &tmpShort); index += 2; //俯仰角速度 tmpShort = 0; short2buf(&send_mimo_data[index], &tmpShort); index += 2; //横滚角速度 tmpShort = 0; short2buf(&send_mimo_data[index], &tmpShort); index += 2; // 左右速度 tmpChar = (planep.E_vel * cosf(planep.yaw / 100.0f * DEG_TO_RAD) + planep.N_vel * sinf(planep.yaw / 100.0f * DEG_TO_RAD)) / 10; send_mimo_data[index++] = tmpChar; //Z速度 tmpChar = planep.alt_vel / 10; //0.1m/s send_mimo_data[index] = tmpChar; can_send_msg_normal(send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO2); } } } /** * @file lidar_function * @brief 雷达相关函数 * @param none * @details * @author Zhang Sir **/ void send_mocib_radar_sensi(void) { static int radar_sensi_send_time = 0; //uint8_t radar_can_buf[8] = {0}; if(HAL_GetTick() > 7000 && HAL_GetTick() - radar_sensi_send_time > 300) { //给FMU发送雷达灵敏度 radar_sensi_send_time = HAL_GetTick(); //上电后 检测到有雷达连接,向飞控发送雷达灵敏度信息 if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == true && uavr11_info.send_fcu_sensi_count <= 3) { pmu_set_ack(22, 1, uavr11_info.get_radar_sensi,0); uavr11_info.send_fcu_sensi_count++; } else if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == true && uavr12_info.send_fcu_sensi_count <= 3) { pmu_set_ack(22, 2, uavr12_info.get_radar_sensi,0); uavr12_info.send_fcu_sensi_count++; } else if (uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == true && uavr56_info.send_fcu_sensi_count <= 3) { pmu_set_ack(22, 6, uavr56_info.get_radar_sensi,0); uavr56_info.send_fcu_sensi_count++; } } } void lidar_function(void) { //莫之比避障雷达升级 if (radar_update_flag == true && uavr11_info.fcu_set_sensi_flag != true && uavr12_info.fcu_set_sensi_flag != true && uavr56_info.fcu_set_sensi_flag != true) { Rupdate.update_flag = true; Can_obstacle_update(); } //上电给fcu发送雷达灵敏度 send_mocib_radar_sensi(); //设置莫之比避障雷达灵敏度 can_set_radar_sensi(); //获取电目雷达盲区、能量、原始数据开关 get_radar_blindAndPower_function(); //雷达升级不再给雷达发送信息,莫之比雷达发送姿态信息 if (radar_update_flag == false) { can_sendmsg_uavr20(); } //给恩曌雷达发送姿态信息数据 can_send_info_to_mimo(); } void can_recv_mocib_version_info(uint32_t cellCanID, uint8_t data[], uint8_t len) { static uint8_t frame_fi = 0,frame_bi = 0,frame_ti = 0; switch (cellCanID) { //SN号 case CAN_OBSTARCLE11_SN: if(frame_fi == 0) { memcpy(&uavr11_info.sn[0],&data[2],6);//要后六个字节 frame_fi += 6; } else if(frame_fi == 6) { memcpy(&uavr11_info.sn[frame_fi],&data[1],7);//要后7个字节 frame_fi += 7; } else if(frame_fi >= 13) { memcpy(&uavr11_info.sn[frame_fi],&data[1],3); frame_fi = 0; regist_dev_info(&dev_obsf,DEVICE_OBSF,false,uavr11_info.sn,20,NULL,0,NULL,0,"mocib",6); uavr11_info.get_radar_sn_flag = true; } break; case CAN_OBSTARCLE12_SN: if(frame_bi == 0) { memcpy(&uavr12_info.sn[0],&data[2],6);//要后六个字节 frame_bi += 6; } else if(frame_bi == 6) { memcpy(&uavr12_info.sn[frame_bi],&data[1],7);//要后7个字节 frame_bi += 7; } else if(frame_bi >= 13) { memcpy(&uavr12_info.sn[frame_bi],&data[1],3); frame_bi = 0; regist_dev_info(&dev_obsb,DEVICE_OBSB,false,uavr12_info.sn,20,NULL,0,NULL,0,"mocib",6); uavr12_info.get_radar_sn_flag = true; } break; case CAN_OBSTARCLE56_SN: if(frame_ti == 0) { memcpy(&uavr56_info.sn[0],&data[2],6);//要后六个字节 frame_ti += 6; } else if(frame_ti == 6) { memcpy(&uavr56_info.sn[frame_ti],&data[1],7);//要后7个字节 frame_ti += 7; } else if(frame_ti >= 13) { memcpy(&uavr56_info.sn[frame_ti],&data[1],3); frame_ti = 0; regist_dev_info(&dev_ter,DEVICE_TERRA,false,uavr56_info.sn,20,NULL,0,NULL,0,"mocib",6); uavr56_info.get_radar_sn_flag = true; } break; //版本信息 case 0x00eeff11: uavr11_info.version[0] = 'M'; uavr11_info.version[1] = '1'; uavr11_info.version[2] = (data[1] + data[2]) + '0'; uavr11_info.version[3] = (data[3] + data[4]) + '0'; char_to_hex_string(&data[5], 3, &uavr11_info.version[4], 6, "00"); uavr11_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff); uavr11_info.get_radar_ver_flag = true; regist_dev_info(&dev_obsf,DEVICE_OBSF,false,NULL,0,uavr11_info.version,10,NULL,0,"mocib",6); break; case 0x00eeff12: uavr12_info.version[0] = 'M'; uavr12_info.version[1] = '2'; uavr12_info.version[2] = (data[1] + data[2]) + '0'; uavr12_info.version[3] = (data[3] + data[4]) + '0'; char_to_hex_string(&data[5], 3, &uavr12_info.version[4], 6, "00"); uavr12_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff); uavr12_info.get_radar_ver_flag = true; regist_dev_info(&dev_obsb,DEVICE_OBSB,false,NULL,0,uavr12_info.version,10,NULL,0,"mocib",6); break; case 0x00eeff0b: uavr56_info.version[0] = 'M'; uavr56_info.version[1] = 'B'; uavr56_info.version[2] = (data[1] + data[2]) + '0'; uavr56_info.version[3] = (data[3] + data[4]) + '0'; char_to_hex_string(&data[5], 3, &uavr56_info.version[4], 6, "00"); uavr56_info.soft_verison = ((data[5] & 0xff) << 16) + ((data[6] & 0xff) << 8) + (data[7] & 0xff); uavr56_info.get_radar_ver_flag = true; regist_dev_info(&dev_ter,DEVICE_TERRA,false,NULL,0,uavr56_info.version,10,NULL,0,"mocib",6); break; default: break; } // if(HAL_GetTick() > 10000) // { pmu_send = PMU_SEND_VERSION; // } } bool check_radar_update(void) { if (uavr12_info.Link.connect_status == COMP_NORMAL && uavr12_info.get_radar_sensi_flag == false && uavr12_info.get_radar_sensi_count < 5) { return false; } if (uavr11_info.Link.connect_status == COMP_NORMAL && uavr11_info.get_radar_sensi_flag == false && uavr11_info.get_radar_sensi_count < 5) { return false; } if (uavr56_info.Link.connect_status == COMP_NORMAL && uavr56_info.get_radar_sensi_flag == false && uavr56_info.get_radar_sensi_count < 5) { return false; } if(uavr11_info.fcu_set_sensi_flag == true ||uavr12_info.fcu_set_sensi_flag == true || uavr56_info.fcu_set_sensi_flag == true) { return false; } return true; } void get_radar_blindAndPower_function( void ) { uint8_t can_buf[8] = {0}; uint32_t can_id = 0; static uint32_t time_1hz = 0; if(!Check_Timer_Ready(&time_1hz,_1_HZ_)) return; if(DM_f_info.Link.connect_status == COMP_NORMAL && DM_f_info.get_radar_blind_flag == false) { can_id = 0xA81300; put_date_to_can(can_buf,0x8,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_power_flag == false) { can_id = 0xA81300; 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) { 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; put_date_to_can(can_buf,0x8,0,0,0,0,0,0,0X7); can_send_msg_normal(&can_buf[0], 8, can_id); } else if(DM_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_power_flag == false) { can_id = 0x981300; 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_ter_info.Link.connect_status == COMP_NORMAL && DM_ter_info.get_radar_rawSwitch_flag == false) { can_id = 0x981300; put_date_to_can(can_buf,0xB,0,0,0,0,0,0,0X7); can_send_msg_normal(&can_buf[0], 8, can_id); } } // mimo_360info mimo360_info[MIMO_360_TotalSect]; // mimo_360status mimo360_status; // Connect_check mimo_360_info; // void mimo360_sort(mimo_360info *arr,uint16_t length) // { // if ( length < 2 ) // { // return; // } // uint16_t num = 0, num1 = 0; // mimo_360info tmp; // for ( num = length - 1; num >= 1; num-- ) // { // for ( num1 = 0; num1 <= num - 1; num1++ ) // { // if ( ( ( arr + num1 )->dis0 ) > ( ( arr + num1 + 1 )->dis0 ) ) // { // tmp = *( arr + num1 ); // *( arr + num1 ) = *( arr + num1 + 1 ); // *( arr + num1 + 1 ) = tmp; // } // } // } // } // uint16_t test_index = 0; // uint16_t compelte = 0; // uint8_t radar360_proflag = 0; // mimo_360_cont fmu_360info; // void can_recv_mocib_new360_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len) // { // mimo_360_info.connect_status = COMP_NORMAL; // mimo_360_info.recv_time = HAL_GetTick(); // switch (cellCanID) // { // case CAN_360MIMO_1ID: // memcpy(&mimo360_status,&data[0],8); // mimo360_status.index = 0; // if(mimo360_status.nTarget == 0) // { // fmu_360info.total_tar = 0; // memset(&fmu_360info.data[0],0,sizeof(mimo_360_data)); // } // break; // case CAN_360MIMO_2ID: // if(mimo360_status.nTarget <= MIMO_360_TotalSect && mimo360_status.nTarget > 0) // { // for(uint8_t i = 0;i < 2;i++) // { // if(i == 0) // { // memcpy(&mimo360_info[mimo360_status.index].sectId0,&data[0],sizeof(mimo_360info)); // } // else // { // memcpy(&mimo360_info[mimo360_status.index].sectId0,&data[4],sizeof(mimo_360info)); // } // if(mimo360_status.nTarget - 1 == mimo360_status.index && radar360_proflag == 0) // { // //mimo360_sort(&mimo360_info[0],mimo360_status.nTarget); // fmu_360info.TotalSect = mimo360_status.TotalSect; // fmu_360info.total_tar = mimo360_status.nTarget; // for(uint8_t j = 0;j < mimo360_status.nTarget;j++) // { // fmu_360info.data[j].sec_angle = mimo360_info[j].sectId0 * 360 / mimo360_status.TotalSect; // fmu_360info.data[j].distance = mimo360_info[j].dis0 * mimo360_status.RangeRes/100; //cm // fmu_360info.data[j].rcs0 = mimo360_info[j].rcs0; // fmu_360info.data[j].el0 = mimo360_info[j].el0 * 0.5; // } // } // mimo360_status.index++; // } // } // break; // default: // break; // } // }