|
|
@@ -1,10 +1,11 @@
|
|
|
#include "soft_terrain.h"
|
|
|
#include "soft_device.h"
|
|
|
-#include "soft_obstacle.h"
|
|
|
+
|
|
|
#include "soft_can.h"
|
|
|
#include "string.h"
|
|
|
-
|
|
|
-
|
|
|
+#include "soft_version.h"
|
|
|
+#include "main_task.h"
|
|
|
+#include "usart_data_handle.h"
|
|
|
uavr_terrain uavr56_info = {.get_radar_sensi = 50};
|
|
|
uavr_terrain mimo_ter_info;
|
|
|
moocib_part_radar_msg Mocib_radar_part;
|
|
|
@@ -95,5 +96,601 @@ void can_recv_mocib_T_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
+}\
|
|
|
+DM_RADAR DM_T_info,FMU_DM_info; //原始数据
|
|
|
+Connect_check DM_status;
|
|
|
+Connect_check DM_4dstatus;
|
|
|
+uint8_t dm_i = 0;
|
|
|
+uint8_t DM_recv_flag = 0;
|
|
|
+uavr_obs DM_f_info;
|
|
|
+int16_t F_4DRadar[3][3] = {0}; // X Y Z
|
|
|
+DM_4dTRADAR DM_T4d;
|
|
|
+DM_4dTRADAR DM_BT4d;
|
|
|
+int dm_4dt_i = 0;
|
|
|
+uint8_t DM4dt_recv_flag = 0;
|
|
|
+uint8_t DM4dbt_recv_flag = 0;
|
|
|
+DM_4dTRADAR_tofmu D4T_tofmu;
|
|
|
+DM_4dTRADAR_tofmu D4BT_tofmu;
|
|
|
+bool F4DT_send_flag = false;
|
|
|
+DM_4dFRADAR DM_F4d;
|
|
|
+bool DM4Dmsg_send_fmu=false;
|
|
|
+uavr_terrain DM_ter_info;
|
|
|
+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) // 头
|
|
|
+ {
|
|
|
+ memcpy(&DM_F4d.target_num, &data[0], 7);
|
|
|
+ if(DM_F4d.target_num > 130)
|
|
|
+ DM_F4d.target_num = 130;
|
|
|
+ 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 >= 145 * 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.DM_Part_Fradar_Link.recv_time = HAL_GetTick();
|
|
|
+ Dev.DM_Part_Fradar_Link.connect_status = COMP_NORMAL;
|
|
|
+ Dev.DM_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;
|
|
|
+ }
|
|
|
+ else if(cellCanID == 0XA01305)
|
|
|
+ {
|
|
|
+ DM_T4d.byte7.frame_flag = data[7];
|
|
|
+ if(DM_T4d.byte7.flag.head != 0 && data[0] == 25)
|
|
|
+ {
|
|
|
+ memcpy(&DM_T4d.target_num, &data[0], 7);
|
|
|
+ dm_4dt_i = 0;
|
|
|
+ }
|
|
|
+ else if(DM_T4d.byte7.flag.tail != 0)
|
|
|
+ {
|
|
|
+ if(data[0] == 0xFE)
|
|
|
+ {
|
|
|
+ memcpy(&DM_T4d.RawData[dm_4dt_i], &data[1], 2);
|
|
|
+ dm_4dt_i += 2;
|
|
|
+ }
|
|
|
+ if(DM_T4d.crc == Get_Crc16(&DM_T4d.RawData[0], DM_T4d.target_num * 2) && DM4dt_recv_flag == 0)
|
|
|
+ {
|
|
|
+ D4T_tofmu.target_num = DM_T4d.target_num;
|
|
|
+ D4T_tofmu.time_delay = DM_T4d.time_delay;
|
|
|
+ D4T_tofmu.err_num = DM_T4d.err_num;
|
|
|
+ memcpy(&D4T_tofmu.RawData, &DM_T4d.RawData, DM_T4d.target_num * 2);
|
|
|
+ F4DT_send_flag = true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ static uint32_t tmpnum = 0;
|
|
|
+ tmpnum++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(data[0] == 0xFE)
|
|
|
+ {
|
|
|
+ memcpy(&DM_T4d.RawData[dm_4dt_i], &data[1], 6);
|
|
|
+ dm_4dt_i += 6;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (dm_4dt_i >= 25 * 2)
|
|
|
+ {
|
|
|
+ dm_4dt_i = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // 版本信息
|
|
|
+ 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);
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0XA81302 && data[0] == 0x1)
|
|
|
+ {
|
|
|
+ uint32_t version_temp = 0;
|
|
|
+ DM_F4d.byte7.frame_flag = data[7];
|
|
|
+ if (DM_F4d.byte7.flag.head != 0) // 头
|
|
|
+ {
|
|
|
+ memcpy(&version_temp, &data[1], 4);
|
|
|
+ Int2String(version_temp, DM_f_info.sn, 9);
|
|
|
+ // 通过SN序号判断新旧boot
|
|
|
+ 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_F4d.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] = '4';
|
|
|
+ DM_f_info.version[2] = 'F';
|
|
|
+ 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 == 0xA81302 && (data[0] == 0xD || data[0] == 0xC))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xD)
|
|
|
+ DM_4DRADARMAG.get_angel_4DF = true;
|
|
|
+ DM_4DRADARMAG.angel_4DF = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xA81302 && (data[0] == 0xF || data[0] == 0xE))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xF)
|
|
|
+ DM_4DRADARMAG.get_ground_filter_4DF = true;
|
|
|
+ DM_4DRADARMAG.ground_filter_4DF = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xA81302 && (data[0] == 0xA || data[0] == 0xB))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xB)
|
|
|
+ DM_4DRADARMAG.get_dotcloud_switch_4DF = true;
|
|
|
+ DM_4DRADARMAG.dotcloud_switch_4DF = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xA81302 && (data[0] == 0x8 || data[0] == 0x5))
|
|
|
+ {
|
|
|
+ if (data[0] == 0x8)
|
|
|
+ DM_4DRADARMAG.get_DM4DF_Blind_spot_distance = true;
|
|
|
+ DM_4DRADARMAG.DM4DF_Blind_spot_distance = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ /*else if(cellCanID == 0xA81302 && (data[0] == 0x1))
|
|
|
+ {
|
|
|
+
|
|
|
+ }*/
|
|
|
}
|
|
|
+int16_t B_4DRadar[3][3];
|
|
|
+int dm_4dbt_i = 0;
|
|
|
+bool F4DB_send_flag = false;
|
|
|
+DM_4dFRADAR DM_B4d;
|
|
|
+DM_4D_REDARVER DM_4DB_info;
|
|
|
+void DM_Bobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
|
|
|
+{
|
|
|
+ // 4D后避障雷达协议 点云
|
|
|
+ /*if (cellCanID == 0XB01310)
|
|
|
+ {
|
|
|
+ // DM_4dstatus.connect_status = COMP_NORMAL;
|
|
|
+ // DM_4dstatus.recv_time = HAL_GetTick();
|
|
|
+ Dev.Part_radarB.facid = FAC_DM_RB_4D;
|
|
|
+
|
|
|
+ DM_B4d.byte7.frame_flag = data[7];
|
|
|
+
|
|
|
+ if (DM_B4d.byte7.flag.head != 0) // 头
|
|
|
+ {
|
|
|
+ memcpy(&DM_B4d.target_num, &data[0], 7);
|
|
|
+ dm_4dB_i = 0;
|
|
|
+ }
|
|
|
+ else if (DM_B4d.byte7.flag.tail != 0) // 尾
|
|
|
+ {
|
|
|
+ if (DM_B4d.target_num != 1)
|
|
|
+ {
|
|
|
+ if ((DM_B4d.target_num * 5) % 7 != 0)
|
|
|
+ {
|
|
|
+ memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], (DM_B4d.target_num * 5) % 7);
|
|
|
+ dm_4dB_i += (DM_B4d.target_num * 5) % 7;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
|
|
|
+ dm_4dB_i += 7;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 5);
|
|
|
+ dm_4dB_i += 5;
|
|
|
+ }
|
|
|
+ if (DM_4dstatus.connect_status != COMP_NORMAL)
|
|
|
+ {
|
|
|
+ if (DM_B4d.crc == Get_Crc16(&DM_B4d.RawData[0], DM_B4d.target_num * 5) && DM4dB_recv_flag == 0)
|
|
|
+ {
|
|
|
+ FMU_4D_info.target_num = DM_B4d.target_num;
|
|
|
+ memcpy(&FMU_4D_info.buf, &DM_B4d.RawData, DM_B4d.target_num * 5);
|
|
|
+ F4d_send_flag = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
|
|
|
+ dm_4dB_i += 7;
|
|
|
+ }
|
|
|
+ if (dm_4dB_i >= 254 * 5)
|
|
|
+ {
|
|
|
+ dm_4dB_i = 0;
|
|
|
+ }
|
|
|
+ }*/
|
|
|
+ // 4D后避障雷达协议 单点
|
|
|
+ if (cellCanID == 0XB01302)
|
|
|
+ {
|
|
|
+ B_4DRadar[0][0] = data[1] + data[2] * 256;
|
|
|
+ B_4DRadar[0][1] = data[3] + data[4] * 256;
|
|
|
+ B_4DRadar[0][2] = data[5] + data[6] * 256;
|
|
|
+
|
|
|
+ Dev.DM_Part_Bradar_Link.recv_time = HAL_GetTick();
|
|
|
+ Dev.DM_Part_Bradar_Link.connect_status = COMP_NORMAL;
|
|
|
+ Dev.DM_Part_radarB.facid = FAC_DM_RB_4D;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0XB01303)
|
|
|
+ {
|
|
|
+ B_4DRadar[1][0] = data[1] + data[2] * 256;
|
|
|
+ B_4DRadar[1][1] = data[3] + data[4] * 256;
|
|
|
+ B_4DRadar[1][2] = data[5] + data[6] * 256;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0XB01304)
|
|
|
+ {
|
|
|
+ B_4DRadar[2][0] = data[1] + data[2] * 256;
|
|
|
+ B_4DRadar[2][1] = data[3] + data[4] * 256;
|
|
|
+ B_4DRadar[2][2] = data[5] + data[6] * 256;
|
|
|
+ }
|
|
|
+ else if(cellCanID == 0XB01305)
|
|
|
+ {
|
|
|
+ DM_BT4d.byte7.frame_flag = data[7];
|
|
|
+ if(DM_BT4d.byte7.flag.head != 0 && data[0] == 25)
|
|
|
+ {
|
|
|
+ memcpy(&DM_BT4d.target_num, &data[0], 7);
|
|
|
+ dm_4dbt_i = 0;
|
|
|
+ }
|
|
|
+ else if(DM_BT4d.byte7.flag.tail != 0)
|
|
|
+ {
|
|
|
+ if(data[0] == 0xFE)
|
|
|
+ {
|
|
|
+ memcpy(&DM_BT4d.RawData[dm_4dbt_i], &data[1], 2);
|
|
|
+ dm_4dbt_i += 2;
|
|
|
+ }
|
|
|
+ if(DM_BT4d.crc == Get_Crc16(&DM_BT4d.RawData[0], DM_BT4d.target_num * 2) && DM4dbt_recv_flag == 0)
|
|
|
+ {
|
|
|
+ D4BT_tofmu.target_num = DM_BT4d.target_num;
|
|
|
+ D4BT_tofmu.time_delay = DM_BT4d.time_delay;
|
|
|
+ D4BT_tofmu.err_num = DM_BT4d.err_num;
|
|
|
+ memcpy(&D4BT_tofmu.RawData, &DM_BT4d.RawData, DM_BT4d.target_num * 2);
|
|
|
+ F4DB_send_flag = true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ static uint32_t tmpnum = 0;
|
|
|
+ tmpnum++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(data[0] == 0xFE)
|
|
|
+ {
|
|
|
+ memcpy(&DM_BT4d.RawData[dm_4dbt_i], &data[1], 6);
|
|
|
+ dm_4dbt_i += 6;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (dm_4dbt_i >= 25 * 2)
|
|
|
+ {
|
|
|
+ dm_4dbt_i = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xB81302 && (data[0] == 0x1 || data[0] == 0x2))
|
|
|
+ {
|
|
|
+ uint32_t version_temp = 0;
|
|
|
+ DM_B4d.byte7.frame_flag = data[7];
|
|
|
+
|
|
|
+ if (DM_B4d.byte7.flag.head != 0) // 头
|
|
|
+ {
|
|
|
+ memcpy(&version_temp, &data[1], 4);
|
|
|
+ Int2String(version_temp, DM_4DB_info.sn, 9);
|
|
|
+ DM_4DB_info.version[3] = 'N';
|
|
|
+ regist_dev_info(&dev_obsb, DEVICE_OBSB, false, DM_4DB_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
|
|
|
+ }
|
|
|
+ else if (DM_B4d.byte7.flag.tail != 0) // 尾
|
|
|
+ {
|
|
|
+ memcpy(&version_temp, &data[1], 4);
|
|
|
+ Int2String(version_temp, &DM_4DB_info.version[4], 6);
|
|
|
+ DM_4DB_info.version[0] = 'D';
|
|
|
+ DM_4DB_info.version[1] = '4';
|
|
|
+ DM_4DB_info.version[2] = 'B';
|
|
|
+
|
|
|
+ regist_dev_info(&dev_obsb, DEVICE_OBSB, false, NULL, 0, DM_4DB_info.version, 10, NULL, 0, "dmter", 6);
|
|
|
+
|
|
|
+ DM_4DB_info.get_radar_ver_flag = true;
|
|
|
+ pmu_send = PMU_SEND_VERSION; // 旧版APP
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xB81302 && (data[0] == 0xA || data[0] == 0xB))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xB)
|
|
|
+ DM_4DRADARMAG.get_dotcloud_switch_4DB = true;
|
|
|
+ DM_4DRADARMAG.dotcloud_switch_4DB = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xB81302 && (data[0] == 0xD || data[0] == 0xC))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xD)
|
|
|
+ DM_4DRADARMAG.get_angel_4DB = true;
|
|
|
+ DM_4DRADARMAG.angel_4DB = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xB81302 && (data[0] == 0xF || data[0] == 0xE))
|
|
|
+ {
|
|
|
+ if (data[0] == 0xF)
|
|
|
+ DM_4DRADARMAG.get_ground_filter_4DB = true;
|
|
|
+ DM_4DRADARMAG.ground_filter_4DB = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+ else if (cellCanID == 0xB81302 && (data[0] == 0x8 || data[0] == 0x5))
|
|
|
+ {
|
|
|
+ if (data[0] == 0x8)
|
|
|
+ DM_4DRADARMAG.get_DM4DB_Blind_spot_distance = true;
|
|
|
+ DM_4DRADARMAG.DM4DB_Blind_spot_distance = data[1] + data[2] * 256;
|
|
|
+ DM4Dmsg_send_fmu = true;
|
|
|
+ }
|
|
|
+}
|
|
|
+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);
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
redaiyuyuyu