#include "soft_radar_handle.h" #include "string.h" #include "main.h" #include "soft_obstacle.h" #include "soft_terrain.h" #include "main_task.h" #include "usart_data_handle.h" #include "soft_can.h" #include "math.h" #include "soft_update.h" #include "soft_device.h" /** * @file get_radar_info * @brief 获取雷达信息 * @param Info_Type:避障雷达 X：1 Y：2 * @details * @author Zhang Sir **/ short get_radar_info(uint8_t Radar_Type,uint8_t Info_Type) { uavr_terrain *Ptr_T = NULL; uavr_obs *Ptr_O = NULL; if(Radar_Type == T_RADAR) { if(mimo_ter_info.Link.connect_status != COMP_NOEXIST) Ptr_T = &mimo_ter_info; else if(uavr56_info.Link.connect_status != COMP_NOEXIST) Ptr_T = &uavr56_info; if(Ptr_T->Link.connect_status == COMP_LOST) {return -2;} else if(Ptr_T == NULL) {return -1;} else {return Ptr_T->height;} } else if(Radar_Type == F_RADAR) { if(uavr11_info.Link.connect_status != COMP_NOEXIST) Ptr_O = &uavr11_info; else if(mimo_f_info.Link.connect_status != COMP_NOEXIST) Ptr_O = &mimo_f_info; if(Ptr_O->Link.connect_status == COMP_LOST) {return -2;} else if(Ptr_O == NULL) {return -1;} else { if(Info_Type == OBS_X) return Ptr_O->distance_x; else if((Info_Type == OBS_Y)) return Ptr_O->distance_y; } } else if(Radar_Type == B_RADAR) { if(uavr12_info.Link.connect_status != COMP_NOEXIST) Ptr_O = &uavr12_info; else if(mimo_b_info.Link.connect_status != COMP_NOEXIST) Ptr_O = &mimo_b_info; if(Ptr_O->Link.connect_status == COMP_LOST) {return -2;} else if(Ptr_O == NULL) {return -1;} else { if(Info_Type == OBS_X) return Ptr_O->distance_x; else if((Info_Type == OBS_Y)) return Ptr_O->distance_y; } } return 0; } /** * @file radar_version_check * @brief 更改雷达版本格式 * @param none * @details * @author Zhang Sir **/ char radar_version[3][10] = {0}; //0 前避障 1后避障 2仿地 void radar_version_check(void) { //前避障 if(uavr11_info.Link.connect_status == COMP_NORMAL || uavr11_info.Link.boot_flag == true) { if(uavr11_info.soft_verison == 0 && uavr11_info.get_radar_ver_flag == false) { uavr11_info.version[0] = 'M'; uavr11_info.version[1] = '1'; for(uint8_t i = 2;i < 10; i++) { uavr11_info.version[i] = '0'; } } memcpy(&radar_version[0][0],&uavr11_info.version[0],10); } else if (mimo_f_info.Link.connect_status == COMP_NORMAL) { if(mimo_f_info.Link.boot_flag == true) { memcpy(&mimo_f_info.version,"E100000000",10); } memcpy(&radar_version[0][0],&mimo_f_info.version[0],10); } //后避障 if(uavr12_info.Link.connect_status == COMP_NORMAL ) { if(uavr12_info.soft_verison == 0 && uavr12_info.get_radar_ver_flag == false) { uavr12_info.version[0] = 'M'; uavr12_info.version[1] = '2'; for(uint8_t i = 2;i < 10; i++) { uavr12_info.version[i] = '0'; } } memcpy(&radar_version[1][0],&uavr12_info.version[0],10); } else if(mimo_b_info.Link.connect_status == COMP_NORMAL) { if(mimo_b_info.Link.boot_flag == true) { memcpy(&mimo_b_info.version,"E200000000",10); } memcpy(&radar_version[1][0],&mimo_b_info.version[0],10); } if(uavr56_info.Link.connect_status == COMP_NORMAL ) { if(uavr56_info.soft_verison == 0 && uavr56_info.get_radar_ver_flag == false) { uavr56_info.version[0] = 'M'; uavr56_info.version[1] = 'B'; for(uint8_t i = 2;i < 10; i++) { uavr56_info.version[i] = '0'; } } memcpy(&radar_version[2][0],&uavr56_info.version[0],10); } else if(mimo_ter_info.Link.connect_status == COMP_NORMAL) { if(mimo_ter_info.Link.boot_flag == true) { memcpy(&mimo_ter_info.version,"EB00000000",10); } memcpy(&radar_version[2][0],&mimo_ter_info.version[0],10); } } /** * @file check_radar_link_status * @brief 检查雷达连接函数 * @param * @details * @author Zhang Sir **/ void check_radar_link_status(void) { Check_dev_link(&mimo_ter_info.Link,3000,(char *)&mimo_ter_info,sizeof(uavr_terrain)); Check_dev_link(&uavr56_info.Link,3000,(char *)&uavr56_info,sizeof(uavr_terrain)); Check_dev_link(&uavr11_info.Link,3000,(char *)&uavr11_info,sizeof(uavr_obs)); Check_dev_link(&uavr12_info.Link,3000,(char *)&uavr12_info,sizeof(uavr_obs)); Check_dev_link(&mimo_f_info.Link,3000,(char *)&mimo_f_info,sizeof(uavr_obs)); Check_dev_link(&mimo_b_info.Link,3000,(char *)&mimo_b_info,sizeof(uavr_obs)); Check_dev_link(&mimo_360_info,3000,NULL,0); } /** * @file lidar_function * @brief 雷达相关函数 * @param none * @details * @author Zhang Sir **/ void send_mocib_radar_sensi(void) { //给FMU发送雷达灵敏度 //上电后 检测到有雷达连接，向飞控发送雷达灵敏度信息 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++; } } /** * @file can_send_info_to_mimo * @brief 给恩曌避障发送姿态信息 * @param none * @details * @author Zhang Sir **/ void can_send_info_to_mimo() { //static int mimofb_10HZ = 0; static int mimoatti_50HZ = 0; static int mimoatti2_50HZ = 0; int16_t index = 0; short tmpShort = 0; int8_t tmpChar = 0; uint8_t send_mimo_data[8] = {0}; if(update_info.vk_dev_update_flag == true) return; if( mimo_f_info.Link.connect_status == COMP_NORMAL || mimo_b_info.Link.connect_status == COMP_NORMAL || mimo_360_info.connect_status == COMP_NORMAL || Dev.Part_Tradar_Link.connect_status == COMP_NORMAL) { if (HAL_GetTick() - mimoatti_50HZ > 19) { mimoatti_50HZ = HAL_GetTick(); 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_Func(CANID1, send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO1, CAN_ID_EXT); } if(HAL_GetTick() - mimoatti2_50HZ > 21) { mimoatti2_50HZ = HAL_GetTick(); 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_Func(CANID1, send_mimo_data, sizeof(send_mimo_data), CAN_MIMO_ATTI_INFO2, CAN_ID_EXT); } } } /** * @file can_set_radar_sensi * @brief 设置雷达灵敏度 * @param none * @details * @author Zhang Sir **/ uint32_t uavr20_sensi_time = 0; short obsfradar_sensitivity = 50; short obsbradar_sensitivity = 50; 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT); } 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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT); } 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT); } 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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT); } 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_UPDATE_S1, CAN_ID_EXT); } 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_Func(CANID1, send_uavr20_sensi, sizeof(send_uavr20_sensi), CAN_UAVRH_SENSI_SA, CAN_ID_EXT); } 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; } } } } /** * @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 = 0; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 仿地最远距离 tmpShort = 0; short2buf(&send_uavr20_data[index], &tmpShort); index += 2; // 结束 send_uavr20_data[index++] = 0X5A; Can_Send_Msg_Func(CANID1, send_uavr20_data, sizeof(send_uavr20_data), SEND_UAV20_MSG, CAN_ID_EXT); } //读取前雷达版本 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT); 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT); 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_Func(CANID1, send_uavr20_sensi, 1, CAN_UAVRH_SENSI_RA, CAN_ID_EXT); uavr20_sensi_time = HAL_GetTick(); uavr56_info.get_radar_sensi_count++; } } } } } } /** * @file check_radar_update * @brief 雷达升级检测 * @param none * @details * @author Zhang Sir **/ 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; } /** * @file lidar_function * @brief 设置360雷达分区数 * @param none * @details * @author Zhang Sir **/ void lidar360_set_TotalSect(void) { if(mimo_360_info.connect_status == COMP_NORMAL && (mimo360_status.get_TotalSect_flag == true || mimo360_status.set_TotalSect_flag == true)) { uint8_t can_buf[8] = {0}; static uint32_t cur_time = 0; if(Check_Timer_Ready(&cur_time,_1_HZ_)) { if(mimo360_status.get_TotalSect_flag == true) { put_date_to_can(can_buf,0xAA,0x24,0x03,0x02,0x07,0x00,0x00,(0x24+0x03+0x02+0x07)&0xff); Can_Send_Msg_Func(CANID1, can_buf, 8, 0xFA, CAN_ID_STD); } if(mimo360_status.set_TotalSect_flag == true) { //设置12分区 put_date_to_can(can_buf,0xAA,0x24,0x03,0x82,0x07,0xC,0x00,(0x24+0x03+0x82+0x07+0xC)&0xff); Can_Send_Msg_Func(CANID1, can_buf, 8, 0xFA, CAN_ID_STD); } } } } /** * @file lidar_function * @brief 雷达 * @param none * @details * @author Zhang Sir **/ 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(); } //设置莫之比避障雷达灵敏度 can_set_radar_sensi(); //雷达升级不再给雷达发送信息,莫之比雷达发送姿态信息 if (radar_update_flag == false) { can_sendmsg_uavr20(); } //给恩曌雷达发送姿态信息数据 can_send_info_to_mimo(); //设置360雷达分区数 lidar360_set_TotalSect(); //给恩曌雷达设置灵敏度信息 //can_set_sensi_to_mimo(); }