| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608 |
- #include "soft_terrain.h"
- #include "stdint.h"
- #include "stdbool.h"
- #include "tim.h"
- #include "can.h"
- #include "soft_can.h"
- #include "common.h"
- #include "soft_seed_device.h"
- #include "soft_crc.h"
- #include "string.h"
- #include "soft_obstacle.h"
- #include "soft_seed_device.h"
- #include "soft_version.h"
- #include "soft_p_2_c.h"
- UAVH30 uavh30_dist;
- uavr_terrain uavr56_info = {.get_radar_sensi = 50};
- uavr_terrain mimo_ter_info;
- _dev_par DM_4DRADARMAG;
- // muniu muniu_ter_info;
- bool terrain_is_link = false;
- uint16_t terrain_height = 0;
- /**
- * @file can_recv_mocib_terrain
- * @brief 莫之比防地解析
- * @param none
- * @details
- * @author Zhang Sir
- **/
- void can_recv_mocib_terrain(uint8_t *data)
- {
- uavh30_dist.powerful = (data[0] << 8) + data[1];
- uavh30_dist.near = (data[2] << 8) + data[3];
- uavh30_dist.far = (data[4] << 8) + data[5];
- uavr56_info.height = uavh30_dist.powerful;
- uavr56_info.Link.recv_time = HAL_GetTick();
- uavr56_info.Link.connect_status = COMP_NORMAL;
- }
- /**
- * @file can_recv_enzhao_terrain
- * @brief 恩曌防地解析
- * @param none
- * @details
- * @author Zhang Sir
- **/
- mimo_part_radar T_radar[3];
- void can_recv_enzhao_terrain(uint32_t CanID, uint8_t data[], uint8_t len)
- {
- switch (CanID)
- {
- case CAN_MIMO_T_ID: // 单点雷达
- mimo_ter_info.crc = data[3] + data[4] + data[5] + data[6];
- if (data[7] == mimo_ter_info.crc)
- {
- mimo_ter_info.height = (256 * data[4]) + (data[3]); // cm
- // printf("%d %d\n", mimo_ter_info.height, m.muniu_hight); //输出到串口助手上 需要pringtf重定向
- }
- mimo_ter_info.Link.recv_time = HAL_GetTick();
- mimo_ter_info.Link.connect_status = COMP_NORMAL;
- Dev.Radar.facid_T = FAC_MIMO_RT;
- break;
- case CAN_MIMO_T_ID1:
- memcpy(&T_radar[0], data, 8);
- Dev.Part_Tradar_Link.recv_time = HAL_GetTick();
- Dev.Part_Tradar_Link.connect_status = COMP_NORMAL;
- Dev.Part_radarT.facid = FAC_MIMO_RT;
- Dev.Radar.facid_T = FAC_MIMO_RT;
- break;
- case CAN_MIMO_T_ID2:
- memcpy(&T_radar[1], data, 8);
- break;
- case CAN_MIMO_T_ID3:
- memcpy(&T_radar[2], data, 8);
- break;
- default:
- break;
- }
- }
- // 木牛仿地雷达
- // void can_recv_muniu_terrain(uint8_t *data)
- // {
- // muniu_ter_info.muniu_hight = (data[0] << 8) + data[1];
- // muniu_ter_info.muniu_SNR = (data[2] << 8) + data[3];
- // muniu_ter_info.Link.connect_status = COMP_NORMAL;
- // muniu_ter_info.Link.recv_time = HAL_GetTick();
- // }
- //电目雷达
- DM_RADAR DM_T_info,FMU_DM_info; //原始数据
- uint8_t dm_i = 0;
- uint8_t DM_recv_flag = 0;
- uint8_t DM4d_recv_flag = 0;
- Connect_check DM_status;
- Connect_check DM_4dstatus;
- uavr_terrain DM_ter_info;
- 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);
- }
-
- }
- int16_t F_4DRadar[3][3] = {0}; // X Y Z
- DM_4dFRADAR DM_F4d;
- int dm_4df_i = 0;
- DM_4DRADAR FMU_4D_info;
- bool F4d_send_flag = false;
- bool DM4Dmsg_send_fmu=false;
- 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.Part_Fradar_Link.recv_time = HAL_GetTick();
- Dev.Part_Fradar_Link.connect_status = COMP_NORMAL;
- Dev.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;
- }
- // 版本信息
- 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] = {0}; // X Y Z
- DM_4dFRADAR DM_B4d;
- int dm_4dB_i = 0;
- // DM_4DRADAR FMU_4DB_info;
- uint8_t DM4dB_recv_flag = 0;
- 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.Part_Bradar_Link.recv_time = HAL_GetTick();
- Dev.Part_Bradar_Link.connect_status = COMP_NORMAL;
- Dev.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 == 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;
- }
- }
|