soft_terrain.c 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608
  1. #include "soft_terrain.h"
  2. #include "stdint.h"
  3. #include "stdbool.h"
  4. #include "tim.h"
  5. #include "can.h"
  6. #include "soft_can.h"
  7. #include "common.h"
  8. #include "soft_seed_device.h"
  9. #include "soft_crc.h"
  10. #include "string.h"
  11. #include "soft_obstacle.h"
  12. #include "soft_seed_device.h"
  13. #include "soft_version.h"
  14. #include "soft_p_2_c.h"
  15. UAVH30 uavh30_dist;
  16. uavr_terrain uavr56_info = {.get_radar_sensi = 50};
  17. uavr_terrain mimo_ter_info;
  18. _dev_par DM_4DRADARMAG;
  19. // muniu muniu_ter_info;
  20. bool terrain_is_link = false;
  21. uint16_t terrain_height = 0;
  22. /**
  23. * @file can_recv_mocib_terrain
  24. * @brief 莫之比防地解析
  25. * @param none
  26. * @details
  27. * @author Zhang Sir
  28. **/
  29. void can_recv_mocib_terrain(uint8_t *data)
  30. {
  31. uavh30_dist.powerful = (data[0] << 8) + data[1];
  32. uavh30_dist.near = (data[2] << 8) + data[3];
  33. uavh30_dist.far = (data[4] << 8) + data[5];
  34. uavr56_info.height = uavh30_dist.powerful;
  35. uavr56_info.Link.recv_time = HAL_GetTick();
  36. uavr56_info.Link.connect_status = COMP_NORMAL;
  37. }
  38. /**
  39. * @file can_recv_enzhao_terrain
  40. * @brief 恩曌防地解析
  41. * @param none
  42. * @details
  43. * @author Zhang Sir
  44. **/
  45. mimo_part_radar T_radar[3];
  46. void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
  47. {
  48. switch (CanID)
  49. {
  50. case CAN_MIMO_T_ID: // 单点雷达
  51. mimo_ter_info.crc = data[3] + data[4] + data[5] + data[6];
  52. if (data[7] == mimo_ter_info.crc)
  53. {
  54. mimo_ter_info.height = (256 * data[4]) + (data[3]); // cm
  55. // printf("%d %d\n", mimo_ter_info.height, m.muniu_hight); //输出到串口助手上 需要pringtf重定向
  56. }
  57. mimo_ter_info.Link.recv_time = HAL_GetTick();
  58. mimo_ter_info.Link.connect_status = COMP_NORMAL;
  59. Dev.Radar.facid_T = FAC_MIMO_RT;
  60. break;
  61. case CAN_MIMO_T_ID1:
  62. memcpy(&T_radar[0], data, 8);
  63. Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
  64. Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
  65. Dev.Part_radarT.facid = FAC_MIMO_RT;
  66. Dev.Radar.facid_T = FAC_MIMO_RT;
  67. break;
  68. case CAN_MIMO_T_ID2:
  69. memcpy(&T_radar[1], data, 8);
  70. break;
  71. case CAN_MIMO_T_ID3:
  72. memcpy(&T_radar[2], data, 8);
  73. break;
  74. default:
  75. break;
  76. }
  77. }
  78. // 木牛仿地雷达
  79. // void can_recv_muniu_terrain(uint8_t *data)
  80. // {
  81. // muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
  82. // muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];
  83. // muniu_ter_info.Link.connect_status = COMP_NORMAL;
  84. // muniu_ter_info.Link.recv_time = HAL_GetTick();
  85. // }
  86. //电目雷达
  87. DM_RADAR DM_T_info,FMU_DM_info; //原始数据
  88. uint8_t dm_i = 0;
  89. uint8_t DM_recv_flag = 0;
  90. uint8_t DM4d_recv_flag = 0;
  91. Connect_check DM_status;
  92. Connect_check DM_4dstatus;
  93. uavr_terrain DM_ter_info;
  94. void DM_terrain_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
  95. {
  96. if (cellCanID == 0x901300) // 多点协议
  97. {
  98. DM_T_info.byte7.frame_flag = data[7];
  99. DM_status.connect_status = COMP_NORMAL;
  100. DM_status.recv_time = HAL_GetTick();
  101. if (DM_T_info.byte7.flag.head != 0) // 头
  102. {
  103. memcpy(&DM_T_info.target_num, &data[0], 7);
  104. if( DM_T_info.target_num > 30 )
  105. DM_T_info.target_num = 30;
  106. dm_i = 0;
  107. if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
  108. {
  109. FMU_DM_info.target_num = 0;
  110. }
  111. }
  112. else if (DM_T_info.byte7.flag.tail != 0) // 尾
  113. {
  114. if (DM_T_info.target_num != 1)
  115. {
  116. memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
  117. }
  118. else
  119. {
  120. memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
  121. }
  122. if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
  123. {
  124. memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
  125. // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
  126. }
  127. }
  128. else
  129. {
  130. memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
  131. dm_i += 7;
  132. }
  133. if (dm_i >= 255 - 7)
  134. {
  135. dm_i = 0;
  136. }
  137. }
  138. else if (cellCanID == 0x901301) // 单点协议
  139. {
  140. DM_ter_info.Link.connect_status = COMP_NORMAL;
  141. DM_ter_info.Link.recv_time = HAL_GetTick();
  142. Dev.Radar.facid_T = FAC_DM_RT;
  143. DM_ter_info.height = data[3] + data[4] * 256;
  144. }
  145. // 版本信息
  146. if (cellCanID == 0x981301 && data[0] == 0x1)
  147. {
  148. uint32_t version_temp = 0;
  149. DM_T_info.byte7.frame_flag = data[7];
  150. if (DM_T_info.byte7.flag.head != 0) // 头
  151. {
  152. memcpy(&version_temp, &data[1], 4);
  153. Int2String(version_temp, DM_ter_info.sn, 9);
  154. // 通过SN序号判断新旧boot
  155. if ((version_temp % 10000000) < 2502999)
  156. DM_ter_info.version[3] = 'O';
  157. else
  158. DM_ter_info.version[3] = 'N';
  159. regist_dev_info(&dev_ter, DEVICE_TERRA, false, DM_ter_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
  160. }
  161. else if (DM_T_info.byte7.flag.tail != 0) // 尾
  162. {
  163. memcpy(&version_temp, &data[1], 4);
  164. Int2String(version_temp, &DM_ter_info.version[4], 6);
  165. DM_ter_info.version[0] = 'D';
  166. DM_ter_info.version[1] = 'S';
  167. DM_ter_info.version[2] = '1';
  168. regist_dev_info(&dev_ter, DEVICE_TERRA, false, NULL, 0, DM_ter_info.version, 10, NULL, 0, "dmter", 6);
  169. DM_ter_info.get_radar_ver_flag = true;
  170. pmu_send = PMU_SEND_VERSION; // 旧版APP
  171. }
  172. }
  173. else if (cellCanID == 0x981301 && (data[0] == 0x8 || data[0] == 0x5))
  174. {
  175. if (data[0] == 0x8)
  176. DM_ter_info.get_radar_blind_flag = true;
  177. pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x56, data[1] + data[2] * 256);
  178. }
  179. else if (cellCanID == 0x981301 && (data[0] == 0x9 || data[0] == 0x7))
  180. {
  181. if (data[0] == 0x9)
  182. DM_ter_info.get_radar_power_flag = true;
  183. pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x56, data[1] + data[2] * 256);
  184. }
  185. else if (cellCanID == 0x981301 && (data[0] == 0xA || data[0] == 0xB))
  186. {
  187. if (data[0] == 0xB)
  188. DM_ter_info.get_radar_rawSwitch_flag = true;
  189. pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x56, data[1] + data[2] * 256);
  190. }
  191. else if (cellCanID == 0x981301 && data[0] == 0x4)
  192. {
  193. pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
  194. }
  195. }
  196. int16_t F_4DRadar[3][3] = {0}; // X Y Z
  197. DM_4dFRADAR DM_F4d;
  198. int dm_4df_i = 0;
  199. DM_4DRADAR FMU_4D_info;
  200. bool F4d_send_flag = false;
  201. bool DM4Dmsg_send_fmu=false;
  202. void DM_Fobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
  203. {
  204. if (cellCanID == 0XA01300) // 多点协议
  205. {
  206. DM_T_info.byte7.frame_flag = data[7];
  207. DM_status.connect_status = COMP_NORMAL;
  208. DM_status.recv_time = HAL_GetTick();
  209. if (DM_T_info.byte7.flag.head != 0) // 头
  210. {
  211. memcpy(&DM_T_info.target_num, &data[0], 7);
  212. if( DM_T_info.target_num > 30 )
  213. DM_T_info.target_num = 30;
  214. dm_i = 0;
  215. if (DM_T_info.target_num == 0 && DM_recv_flag == 0)
  216. {
  217. FMU_DM_info.target_num = 0;
  218. }
  219. }
  220. else if (DM_T_info.byte7.flag.tail != 0) // 尾
  221. {
  222. if (DM_T_info.target_num != 1)
  223. {
  224. memcpy(&DM_T_info.buf[dm_i], &data[0], DM_T_info.target_num * 4 % 7);
  225. }
  226. else
  227. {
  228. memcpy(&DM_T_info.buf[dm_i], &data[0], 4);
  229. }
  230. if (DM_T_info.crc == Get_Crc16(&DM_T_info.buf[0], DM_T_info.target_num * 4) && DM_recv_flag == 0)
  231. {
  232. memcpy(&FMU_DM_info.target_num, &DM_T_info.target_num, DM_T_info.target_num * 4 + 8);
  233. // memcpy(&FMU_DM_info.buf[0], &DM_T_info.buf[0], DM_T_info.target_num * 4);
  234. }
  235. }
  236. else
  237. {
  238. memcpy(&DM_T_info.buf[dm_i], &data[0], 7);
  239. dm_i += 7;
  240. }
  241. if (dm_i >= 255 - 7)
  242. {
  243. dm_i = 0;
  244. }
  245. }
  246. else if (cellCanID == 0XA01301) // 单点协议
  247. {
  248. DM_f_info.Link.connect_status = COMP_NORMAL;
  249. DM_f_info.Link.recv_time = HAL_GetTick();
  250. Dev.Radar.facid_F = FAC_DM_RF;
  251. DM_f_info.distance_y = data[3] + data[4] * 256;
  252. }
  253. // 4D前避障雷达协议 点云
  254. if (cellCanID == 0XA01310)
  255. {
  256. DM_4dstatus.connect_status = COMP_NORMAL;
  257. DM_4dstatus.recv_time = HAL_GetTick();
  258. Dev.Part_radarF.facid = FAC_DM_RF_4D;
  259. DM_F4d.byte7.frame_flag = data[7];
  260. if (DM_F4d.byte7.flag.head != 0) // 头
  261. {
  262. memcpy(&DM_F4d.target_num, &data[0], 7);
  263. if(DM_F4d.target_num > 130)
  264. DM_F4d.target_num = 130;
  265. dm_4df_i = 0;
  266. }
  267. else if (DM_F4d.byte7.flag.tail != 0) // 尾
  268. {
  269. if (DM_F4d.target_num != 1)
  270. {
  271. if ((DM_F4d.target_num * 5) % 7 != 0)
  272. {
  273. memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], (DM_F4d.target_num * 5) % 7);
  274. dm_4df_i += (DM_F4d.target_num * 5) % 7;
  275. }
  276. else
  277. {
  278. memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
  279. dm_4df_i += 7;
  280. }
  281. }
  282. else
  283. {
  284. memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 5);
  285. dm_4df_i += 5;
  286. }
  287. if (DM_F4d.crc == Get_Crc16(&DM_F4d.RawData[0], DM_F4d.target_num * 5) && DM4d_recv_flag == 0)
  288. {
  289. FMU_4D_info.target_num = DM_F4d.target_num;
  290. memcpy(&FMU_4D_info.buf, &DM_F4d.RawData, DM_F4d.target_num * 5);
  291. F4d_send_flag = true;
  292. }
  293. }
  294. else
  295. {
  296. memcpy(&DM_F4d.RawData[dm_4df_i], &data[0], 7);
  297. dm_4df_i += 7;
  298. }
  299. if (dm_4df_i >= 145 * 5)
  300. {
  301. dm_4df_i = 0;
  302. }
  303. }
  304. // 4D前避障雷达协议 单点
  305. if (cellCanID == 0XA01302)
  306. {
  307. F_4DRadar[0][0] = data[1] + data[2] * 256;
  308. F_4DRadar[0][1] = data[3] + data[4] * 256;
  309. F_4DRadar[0][2] = data[5] + data[6] * 256;
  310. Dev.Part_Fradar_Link.recv_time = HAL_GetTick();
  311. Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
  312. Dev.Part_radarF.facid = FAC_DM_RF_4D;
  313. }
  314. else if (cellCanID == 0XA01303)
  315. {
  316. F_4DRadar[1][0] = data[1] + data[2] * 256;
  317. F_4DRadar[1][1] = data[3] + data[4] * 256;
  318. F_4DRadar[1][2] = data[5] + data[6] * 256;
  319. }
  320. else if (cellCanID == 0XA01304)
  321. {
  322. F_4DRadar[2][0] = data[1] + data[2] * 256;
  323. F_4DRadar[2][1] = data[3] + data[4] * 256;
  324. F_4DRadar[2][2] = data[5] + data[6] * 256;
  325. }
  326. // 版本信息
  327. if (cellCanID == 0XA81301 && data[0] == 0x1)
  328. {
  329. uint32_t version_temp = 0;
  330. DM_T_info.byte7.frame_flag = data[7];
  331. if (DM_T_info.byte7.flag.head != 0) // 头
  332. {
  333. memcpy(&version_temp, &data[1], 4);
  334. Int2String(version_temp, DM_f_info.sn, 9);
  335. // 通过SN序号判断新旧boot
  336. if ((version_temp % 10000000) < 2502999)
  337. DM_f_info.version[3] = 'O';
  338. else
  339. DM_f_info.version[3] = 'N';
  340. regist_dev_info(&dev_obsf, DEVICE_OBSF, false, DM_f_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
  341. }
  342. else if (DM_T_info.byte7.flag.tail != 0) // 尾
  343. {
  344. memcpy(&version_temp, &data[1], 4);
  345. Int2String(version_temp, &DM_f_info.version[4], 6);
  346. DM_f_info.version[0] = 'D';
  347. DM_f_info.version[1] = 'W';
  348. DM_f_info.version[2] = '1';
  349. regist_dev_info(&dev_obsf, DEVICE_OBSF, false, NULL, 0, DM_f_info.version, 10, NULL, 0, "dmter", 6);
  350. DM_f_info.get_radar_ver_flag = true;
  351. pmu_send = PMU_SEND_VERSION; // 旧版APP
  352. }
  353. }
  354. else if (cellCanID == 0xA81301 && (data[0] == 0x8 || data[0] == 0x5))
  355. {
  356. if (data[0] == 0x8)
  357. DM_f_info.get_radar_blind_flag = true;
  358. pmu_set_ack(_MSGID_SET, MSGID_SET_TR_BLIND, 0x11, data[1] + data[2] * 256);
  359. }
  360. else if (cellCanID == 0xA81301 && (data[0] == 0x9 || data[0] == 0x7))
  361. {
  362. if (data[0] == 0x9)
  363. DM_f_info.get_radar_power_flag = true;
  364. pmu_set_ack(_MSGID_SET, MSGID_SET_BR_POWER, 0x11, data[1] + data[2] * 256);
  365. }
  366. else if (cellCanID == 0xA81301 && (data[0] == 0xA || data[0] == 0xB))
  367. {
  368. if (data[0] == 0xB)
  369. DM_f_info.get_radar_rawSwitch_flag = true;
  370. pmu_set_ack(_MSGID_SET, MSGID_SET_RAW_SWITCH, 0x11, data[1] + data[2] * 256);
  371. }
  372. else if (cellCanID == 0xA81301 && data[0] == 0x4)
  373. {
  374. pmu_set_ack(_MSGID_SET, MSGID_SET_R_FUNC, 0, data[1] + data[2] * 256);
  375. }
  376. else if (cellCanID == 0XA81302 && data[0] == 0x1)
  377. {
  378. uint32_t version_temp = 0;
  379. DM_F4d.byte7.frame_flag = data[7];
  380. if (DM_F4d.byte7.flag.head != 0) // 头
  381. {
  382. memcpy(&version_temp, &data[1], 4);
  383. Int2String(version_temp, DM_f_info.sn, 9);
  384. // 通过SN序号判断新旧boot
  385. DM_f_info.version[3] = 'N';
  386. regist_dev_info(&dev_obsf, DEVICE_OBSF, false, DM_f_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
  387. }
  388. else if (DM_F4d.byte7.flag.tail != 0) // 尾
  389. {
  390. memcpy(&version_temp, &data[1], 4);
  391. Int2String(version_temp, &DM_f_info.version[4], 6);
  392. DM_f_info.version[0] = 'D';
  393. DM_f_info.version[1] = '4';
  394. DM_f_info.version[2] = 'F';
  395. regist_dev_info(&dev_obsf, DEVICE_OBSF, false, NULL, 0, DM_f_info.version, 10, NULL, 0, "dmter", 6);
  396. DM_f_info.get_radar_ver_flag = true;
  397. pmu_send = PMU_SEND_VERSION; // 旧版APP
  398. }
  399. }
  400. else if (cellCanID == 0xA81302 && (data[0] == 0xD || data[0] == 0xC))
  401. {
  402. if (data[0] == 0xD)
  403. DM_4DRADARMAG.get_angel_4DF = true;
  404. DM_4DRADARMAG.angel_4DF = data[1] + data[2] * 256;
  405. DM4Dmsg_send_fmu = true;
  406. }
  407. else if (cellCanID == 0xA81302 && (data[0] == 0xF || data[0] == 0xE))
  408. {
  409. if (data[0] == 0xF)
  410. DM_4DRADARMAG.get_ground_filter_4DF = true;
  411. DM_4DRADARMAG.ground_filter_4DF = data[1] + data[2] * 256;
  412. DM4Dmsg_send_fmu = true;
  413. }
  414. else if (cellCanID == 0xA81302 && (data[0] == 0xA || data[0] == 0xB))
  415. {
  416. if (data[0] == 0xB)
  417. DM_4DRADARMAG.get_dotcloud_switch_4DF = true;
  418. DM_4DRADARMAG.dotcloud_switch_4DF = data[1] + data[2] * 256;
  419. DM4Dmsg_send_fmu = true;
  420. }
  421. else if (cellCanID == 0xA81302 && (data[0] == 0x8 || data[0] == 0x5))
  422. {
  423. if (data[0] == 0x8)
  424. DM_4DRADARMAG.get_DM4DF_Blind_spot_distance = true;
  425. DM_4DRADARMAG.DM4DF_Blind_spot_distance = data[1] + data[2] * 256;
  426. DM4Dmsg_send_fmu = true;
  427. }
  428. /*else if(cellCanID == 0xA81302 && (data[0] == 0x1))
  429. {
  430. }*/
  431. }
  432. int16_t B_4DRadar[3][3] = {0}; // X Y Z
  433. DM_4dFRADAR DM_B4d;
  434. int dm_4dB_i = 0;
  435. // DM_4DRADAR FMU_4DB_info;
  436. uint8_t DM4dB_recv_flag = 0;
  437. void DM_Bobs_recieved_hookfuction(uint32_t cellCanID, uint8_t data[], uint8_t len)
  438. {
  439. // 4D后避障雷达协议 点云
  440. if (cellCanID == 0XB01310)
  441. {
  442. // DM_4dstatus.connect_status = COMP_NORMAL;
  443. // DM_4dstatus.recv_time = HAL_GetTick();
  444. Dev.Part_radarB.facid = FAC_DM_RB_4D;
  445. DM_B4d.byte7.frame_flag = data[7];
  446. if (DM_B4d.byte7.flag.head != 0) // 头
  447. {
  448. memcpy(&DM_B4d.target_num, &data[0], 7);
  449. dm_4dB_i = 0;
  450. }
  451. else if (DM_B4d.byte7.flag.tail != 0) // 尾
  452. {
  453. if (DM_B4d.target_num != 1)
  454. {
  455. if ((DM_B4d.target_num * 5) % 7 != 0)
  456. {
  457. memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], (DM_B4d.target_num * 5) % 7);
  458. dm_4dB_i += (DM_B4d.target_num * 5) % 7;
  459. }
  460. else
  461. {
  462. memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
  463. dm_4dB_i += 7;
  464. }
  465. }
  466. else
  467. {
  468. memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 5);
  469. dm_4dB_i += 5;
  470. }
  471. if (DM_4dstatus.connect_status != COMP_NORMAL)
  472. {
  473. if (DM_B4d.crc == Get_Crc16(&DM_B4d.RawData[0], DM_B4d.target_num * 5) && DM4dB_recv_flag == 0)
  474. {
  475. FMU_4D_info.target_num = DM_B4d.target_num;
  476. memcpy(&FMU_4D_info.buf, &DM_B4d.RawData, DM_B4d.target_num * 5);
  477. F4d_send_flag = true;
  478. }
  479. }
  480. }
  481. else
  482. {
  483. memcpy(&DM_B4d.RawData[dm_4dB_i], &data[0], 7);
  484. dm_4dB_i += 7;
  485. }
  486. if (dm_4dB_i >= 254 * 5)
  487. {
  488. dm_4dB_i = 0;
  489. }
  490. }
  491. // 4D后避障雷达协议 单点
  492. if (cellCanID == 0XB01302)
  493. {
  494. B_4DRadar[0][0] = data[1] + data[2] * 256;
  495. B_4DRadar[0][1] = data[3] + data[4] * 256;
  496. B_4DRadar[0][2] = data[5] + data[6] * 256;
  497. Dev.Part_Bradar_Link.recv_time = HAL_GetTick();
  498. Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
  499. Dev.Part_radarB.facid = FAC_DM_RB_4D;
  500. }
  501. else if (cellCanID == 0XB01303)
  502. {
  503. B_4DRadar[1][0] = data[1] + data[2] * 256;
  504. B_4DRadar[1][1] = data[3] + data[4] * 256;
  505. B_4DRadar[1][2] = data[5] + data[6] * 256;
  506. }
  507. else if (cellCanID == 0XB01304)
  508. {
  509. B_4DRadar[2][0] = data[1] + data[2] * 256;
  510. B_4DRadar[2][1] = data[3] + data[4] * 256;
  511. B_4DRadar[2][2] = data[5] + data[6] * 256;
  512. }
  513. else if (cellCanID == 0xB81302 && (data[0] == 0x1 || data[0] == 0x2))
  514. {
  515. uint32_t version_temp = 0;
  516. DM_B4d.byte7.frame_flag = data[7];
  517. if (DM_B4d.byte7.flag.head != 0) // 头
  518. {
  519. memcpy(&version_temp, &data[1], 4);
  520. Int2String(version_temp, DM_4DB_info.sn, 9);
  521. DM_4DB_info.version[3] = 'N';
  522. regist_dev_info(&dev_obsb, DEVICE_OBSB, false, DM_4DB_info.sn, 9, NULL, 0, NULL, 0, "dmter", 6);
  523. }
  524. else if (DM_B4d.byte7.flag.tail != 0) // 尾
  525. {
  526. memcpy(&version_temp, &data[1], 4);
  527. Int2String(version_temp, &DM_4DB_info.version[4], 6);
  528. DM_4DB_info.version[0] = 'D';
  529. DM_4DB_info.version[1] = '4';
  530. DM_4DB_info.version[2] = 'B';
  531. regist_dev_info(&dev_obsb, DEVICE_OBSB, false, NULL, 0, DM_4DB_info.version, 10, NULL, 0, "dmter", 6);
  532. DM_4DB_info.get_radar_ver_flag = true;
  533. pmu_send = PMU_SEND_VERSION; // 旧版APP
  534. }
  535. }
  536. else if (cellCanID == 0xB81302 && (data[0] == 0xA || data[0] == 0xB))
  537. {
  538. if (data[0] == 0xB)
  539. DM_4DRADARMAG.get_dotcloud_switch_4DB = true;
  540. DM_4DRADARMAG.dotcloud_switch_4DB = data[1] + data[2] * 256;
  541. DM4Dmsg_send_fmu = true;
  542. }
  543. else if (cellCanID == 0xB81302 && (data[0] == 0xD || data[0] == 0xC))
  544. {
  545. if (data[0] == 0xD)
  546. DM_4DRADARMAG.get_angel_4DB = true;
  547. DM_4DRADARMAG.angel_4DB = data[1] + data[2] * 256;
  548. DM4Dmsg_send_fmu = true;
  549. }
  550. else if (cellCanID == 0xB81302 && (data[0] == 0xF || data[0] == 0xE))
  551. {
  552. if (data[0] == 0xF)
  553. DM_4DRADARMAG.get_ground_filter_4DB = true;
  554. DM_4DRADARMAG.ground_filter_4DB = data[1] + data[2] * 256;
  555. DM4Dmsg_send_fmu = true;
  556. }
  557. else if (cellCanID == 0xB81302 && (data[0] == 0x8 || data[0] == 0x5))
  558. {
  559. if (data[0] == 0x8)
  560. DM_4DRADARMAG.get_DM4DB_Blind_spot_distance = true;
  561. DM_4DRADARMAG.DM4DB_Blind_spot_distance = data[1] + data[2] * 256;
  562. DM4Dmsg_send_fmu = true;
  563. }
  564. }