#include "soft_obstacle.h" #include "soft_device.h" #include "string.h" #include "usart_data_handle.h" #include "main_task.h" #include "soft_can.h" #include "math.h" #include "stdlib.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}; /** * @file can_recv_mocib_F_obstacle * @brief 莫之比前避障解析 * @param none * @details * @author Zhang Sir **/ 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]; uavr11_info.Link.connect_status = COMP_NORMAL; uavr11_info.Link.recv_time = HAL_GetTick(); } /** * @file can_recv_mocib_B_obstacle * @brief 莫之比后避障障解析 * @param none * @details * @author Zhang Sir **/ 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]; uavr12_info.Link.connect_status = COMP_NORMAL; uavr12_info.Link.recv_time = HAL_GetTick(); } /** * @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; } } } mimo_360info mimo360_info[MIMO_360_TotalSect]; mimo_360status mimo360_status = {.get_TotalSect_flag = true}; 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; } } //避障处理函数， 减少扫地的情况 int Fobs_delay_time = 0; uint8_t Fobs_angle[3] = {0}; uint8_t Fobs_time = 0; bool Fobs_handle_function(void) { if(planep.lock_status == STA_LOCK) { return true; } if(terrain_is_link == true) { //仿地雷达连接的情况下检测9度以下 if(planep.pitch_angle > -1300) { //从小到大赋值 if(Fobs_angle[2] != 0) { Fobs_time = Fobs_angle[2]; } else if (Fobs_angle[1] != 0) { Fobs_time = Fobs_angle[1]; } else if (Fobs_angle[0] != 0) { Fobs_time = Fobs_angle[0]; } else{ Fobs_time = 0; } if(HAL_GetTick() - Fobs_delay_time > 1000 + Fobs_time * 100) { memset(&Fobs_angle[0],0,3); return true; } } else { if(planep.pitch_angle < -1500) { Fobs_angle[2] = 1;//15; } else if(planep.pitch_angle < -1350) { Fobs_angle[1] = 1;//10; } else { Fobs_angle[0] = 1;//5; } Fobs_delay_time = HAL_GetTick(); return false; } } else { //无仿地或高于2m 12度 if(planep.pitch_angle > -1300) { if(Fobs_angle[2] != 0) { Fobs_time = Fobs_angle[2]; } else if (Fobs_angle[1] != 0) { Fobs_time = Fobs_angle[1]; } else if (Fobs_angle[0] != 0) { Fobs_time = Fobs_angle[0]; } else{ Fobs_time = 0; } if(HAL_GetTick() - Fobs_delay_time > 500 + Fobs_time * 100) { memset(&Fobs_angle[0],0,3); return true; } } else { if(planep.pitch_angle > -1300) { Fobs_angle[0] = 1;//5; } else if(planep.pitch_angle > -1500) { Fobs_angle[1] = 1;//10; } else { Fobs_angle[2] = 1;//15; } Fobs_delay_time = HAL_GetTick(); return false; } } return false; } int Bobs_delay_time = 0; uint8_t Bobs_angle[3] = {0}; uint8_t Bobs_time = 0; bool Bobs_handle_function(void) { if(planep.lock_status == STA_LOCK) { return true; } if(terrain_is_link == true) { //仿地雷达连接的情况下检测9度以下，增加延时到2.5s if(planep.pitch_angle < 1300) { if(Bobs_angle[2] != 0) { Bobs_time = Bobs_angle[2]; } else if (Bobs_angle[1] != 0) { Bobs_time = Bobs_angle[1]; } else if (Bobs_angle[0] != 0) { Bobs_time = Bobs_angle[0]; } else{ Bobs_time = 0; } if(HAL_GetTick() - Bobs_delay_time > 500 + Bobs_time * 100) { memset(&Bobs_angle[0],0,3); return true; } } else { if(planep.pitch_angle > 1500) { Bobs_angle[2] = 1;//15; } else if(planep.pitch_angle > 1350) { Bobs_angle[1] = 1;//10; } else { Bobs_angle[0] = 1;//5; } Bobs_delay_time = HAL_GetTick(); return false; } } else { //无仿地或高于2m 12度 if(planep.pitch_angle < 1300) { if(Bobs_angle[2] != 0) { Bobs_time = Bobs_angle[2]; } else if (Bobs_angle[1] != 0) { Bobs_time = Bobs_angle[1]; } else if (Bobs_angle[0] != 0) { Bobs_time = Bobs_angle[0]; } else{ Bobs_time = 0; } if(HAL_GetTick() - Bobs_delay_time > 1000 + Bobs_time * 100) { memset(&Bobs_angle[0],0,3); return true; } } else { if(planep.pitch_angle > 1500) { Bobs_angle[2] = 1;//15; } else if(planep.pitch_angle > 1350) { Bobs_angle[1] = 1;//10; } else { Bobs_angle[0] = 1;//5; } Bobs_delay_time = HAL_GetTick(); return false; } } return false; } /** * @file can_recv_enzhao_obstacle * @brief 恩曌多点避障解析 * @param none * @details * @author Zhang Sir **/ /*****************************恩曌雷达排序************************/ void mimomocib_buf_sort( mimo_part_radar *arr, uint16_t length ) { if ( length < 2 ) { return; } uint16_t num = 0, num1 = 0; mimo_part_radar tmp; for ( num = length - 1; num >= 1; num-- ) { for ( num1 = 0; num1 <= num - 1; num1++ ) { if ( ( ( arr + num1 )->Distance ) > ( ( arr + num1 + 1 )->Distance ) ) { tmp = *( arr + num1 ); *( arr + num1 ) = *( arr + num1 + 1 ); *( arr + num1 + 1 ) = tmp; } } } } 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.Part_Fradar_Link.connect_status = COMP_NORMAL; Dev.Part_Fradar_Link.recv_time = HAL_GetTick(); Dev.Part_radarF.facid = 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.Part_Bradar_Link.connect_status = COMP_NORMAL; Dev.Part_Bradar_Link.recv_time = HAL_GetTick(); Dev.Part_radarB.facid = 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; mimomocib_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; mimomocib_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; } } void can_recv_mocib_obstacle(uint32_t cellCanID, uint8_t data[], uint8_t len) { switch (cellCanID) { case CAN_UAVRH_FOBS_ID1: memcpy(&F_radar[0], data, 8); recv_comF_flag = recv_comF_flag | 1; Dev.Part_Fradar_Link.connect_status = COMP_NORMAL; Dev.Part_Fradar_Link.recv_time = HAL_GetTick(); Dev.Part_radarF.facid = FAC_MOCIB_RF; break; case CAN_UAVRH_FOBS_ID2: memcpy(&F_radar[1], data, 8); recv_comF_flag = recv_comF_flag | 2; break; case CAN_UAVRH_FOBS_ID3: memcpy(&F_radar[2], data, 8); recv_comF_flag = recv_comF_flag | 4; break; case CAN_UAVRH_BOBS_ID1: memcpy(&B_radar[0], data, 8); recv_comB_flag = recv_comB_flag | 1; Dev.Part_Bradar_Link.connect_status = COMP_NORMAL; Dev.Part_Bradar_Link.recv_time = HAL_GetTick(); Dev.Part_radarB.facid = FAC_MOCIB_RB; break; case CAN_UAVRH_BOBS_ID2: memcpy(&B_radar[1], data, 8); recv_comB_flag = recv_comB_flag | 2; break; case CAN_UAVRH_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; mimomocib_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) { uavr11_info.distance_x = F_radar[i].Distance * 0.05f * 100 * sin(F_radar[i].Amuzith * 0.1f / RAD); uavr11_info.distance_y = F_radar[i].Distance * 0.05f * 100 * cos(F_radar[i].Amuzith * 0.1f / RAD); break; } if(i == 2) { uavr11_info.distance_x = 0; uavr11_info.distance_y = 0; } } uavr11_info.Link.connect_status = COMP_NORMAL; uavr11_info.Link.recv_time = HAL_GetTick(); } if(recv_comB_flag == 7) { recv_comB_flag = 0; mimomocib_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) { uavr12_info.distance_x = B_radar[i].Distance * 0.05f * 100 * sin(B_radar[i].Amuzith * 0.1f / RAD); uavr12_info.distance_y = B_radar[i].Distance * 0.05f * 100 * cos(B_radar[i].Amuzith * 0.1f / RAD); break; } if(i == 2) { uavr12_info.distance_x = 0; uavr12_info.distance_y = 0; } } uavr12_info.Link.recv_time = HAL_GetTick(); uavr12_info.Link.connect_status = COMP_NORMAL; } }