can.cpp 58 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266
  1. #include <control/can.h>
  2. #include <control/control_status.h>
  3. #include <common/constants.h>
  4. #include <common/logout.h>
  5. #include <perception/sensor_radar.h>
  6. #include <QTime>
  7. #define VCI_USBCAN1 3
  8. #define VCI_USBCAN2 4
  9. #define VCI_USBCAN2A 4
  10. #define VCI_USBCAN_E_U 20
  11. #define VCI_USBCAN_2E_U 21
  12. #define VCI_ACCCODE_DEFAULT 0x00000000
  13. #define VCI_ACCMASK_DEFAULT 0xFFFFFFFF
  14. #define VCI_TIMER0_DEFAULT 0x00
  15. #define VCI_TIMER1_DEFAULT 0x1C
  16. #define VCI_FILTER_DEFAULT 1
  17. #define VCI_MODE_DEFAULT 0
  18. //函数调用返回状态值
  19. #define STATUS_OK 1
  20. #define STATUS_ERR 0
  21. #define LOG_ENABLE
  22. #ifndef M_PI
  23. #define M_PI (3.1415926535897932384626433832795)
  24. #endif
  25. iv::control::CanUtil::CanUtil() {
  26. obs_radar1 = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  27. obs_radar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  28. obs_radar_ui = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
  29. #ifdef WIN32
  30. m_hDLL = LoadLibrary(TEXT("ControlCAN.dll"));
  31. VCI_OpenDevice = (LPVCI_OpenDevice)GetProcAddress(m_hDLL, "VCI_OpenDevice");//取得函数地址
  32. VCI_CloseDevice = (LPVCI_CloseDevice)GetProcAddress(m_hDLL, "VCI_CloseDevice");
  33. VCI_InitCAN = (LPVCI_InitCan)GetProcAddress(m_hDLL, "VCI_InitCAN");
  34. VCI_ReadBoardInfo = (LPVCI_ReadBoardInfo)GetProcAddress(m_hDLL, "VCI_ReadBoardInfo");
  35. VCI_ReadErrInfo = (LPVCI_ReadErrInfo)GetProcAddress(m_hDLL, "VCI_ReadErrInfo");
  36. VCI_ReadCanStatus = (LPVCI_ReadCanStatus)GetProcAddress(m_hDLL, "VCI_ReadCANStatus");
  37. VCI_GetReference = (LPVCI_GetReference)GetProcAddress(m_hDLL, "VCI_GetReference");
  38. VCI_SetReference = (LPVCI_SetReference)GetProcAddress(m_hDLL, "VCI_SetReference");
  39. VCI_GetReceiveNum = (LPVCI_GetReceiveNum)GetProcAddress(m_hDLL, "VCI_GetReceiveNum");
  40. VCI_ClearBuffer = (LPVCI_ClearBuffer)GetProcAddress(m_hDLL, "VCI_ClearBuffer");
  41. VCI_StartCAN = (LPVCI_StartCAN)GetProcAddress(m_hDLL, "VCI_StartCAN");
  42. VCI_ResetCAN = (LPVCI_ResetCAN)GetProcAddress(m_hDLL, "VCI_ResetCAN");
  43. VCI_Transmit = (LPVCI_Transmit)GetProcAddress(m_hDLL, "VCI_Transmit");
  44. VCI_Receive = (LPVCI_Receive)GetProcAddress(m_hDLL, "VCI_Receive");
  45. VCI_GetReference2 = (LPVCI_GetReference2)GetProcAddress(m_hDLL, "VCI_GetReference2");
  46. VCI_SetReference2 = (LPVCI_SetReference2)GetProcAddress(m_hDLL, "VCI_SetReference2");
  47. VCI_ResumeConfig = (LPVCI_ResumeConfig)GetProcAddress(m_hDLL, "VCI_ResumeConfig");
  48. VCI_ConnectDevice = (LPVCI_ConnectDevice)GetProcAddress(m_hDLL, "VCI_ConnectDevice");
  49. VCI_UsbDeviceReset = (LPVCI_UsbDeviceReset)GetProcAddress(m_hDLL, "VCI_UsbDeviceReset");
  50. #endif
  51. }
  52. iv::control::CanUtil::~CanUtil() {
  53. }
  54. void iv::control::CanUtil::openCanCard() {
  55. VCI_INIT_CONFIG initconfig;
  56. initconfig.AccCode = VCI_ACCCODE_DEFAULT;
  57. initconfig.AccMask = VCI_ACCMASK_DEFAULT;
  58. initconfig.Timing0 = VCI_TIMER0_DEFAULT;
  59. initconfig.Timing1 = VCI_TIMER1_DEFAULT;
  60. initconfig.Filter = VCI_FILTER_DEFAULT;
  61. initconfig.Mode = VCI_MODE_DEFAULT;
  62. int status = VCI_OpenDevice(m_devtype, m_devind, 0); //打开设备
  63. m_connect = -1;
  64. if (status == 1)
  65. {
  66. int init = VCI_InitCAN(m_devtype, m_devind, m_cannum, &initconfig); //初始化设备
  67. if (init == 1)
  68. {
  69. if (VCI_StartCAN(m_devtype, m_devind, m_cannum) == 1) {
  70. send_connect = true;
  71. m_connect = 1;
  72. thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));
  73. thread_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive, this)));
  74. }
  75. else {
  76. throw 0;
  77. }
  78. }
  79. else
  80. {
  81. throw 1;
  82. }
  83. // 打开2通道,供毫米波雷达读取数据用
  84. init = VCI_InitCAN(m_devtype, m_devind, m_cannum_radar, &initconfig); //初始化设备
  85. if (init == 1)
  86. {
  87. if (VCI_StartCAN(m_devtype, m_devind, m_cannum_radar) == 1) {
  88. m_connect = 1;
  89. thread_radar_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_radar, this))); //启动毫米波雷达接收线程5
  90. }
  91. else {
  92. throw 2;
  93. }
  94. }
  95. else
  96. {
  97. throw 3;
  98. }
  99. }
  100. else if(status == 0) {
  101. throw 4;
  102. }
  103. else if (status == -1) {
  104. throw 5;
  105. }
  106. #ifdef USE_MOBILEYE
  107. status = VCI_OpenDevice(m_devtype, m_devindmob, 0); //打开设备
  108. if (status == 1)
  109. {
  110. int init = VCI_InitCAN(m_devtype, m_devindmob, m_cannum, &initconfig); //初始化设备
  111. if (init == 1)
  112. {
  113. if (VCI_StartCAN(m_devtype, m_devindmob, m_cannum) == 1) {
  114. m_connect = 1;
  115. thread_mobileye_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_mobileye, this)));
  116. }
  117. else {
  118. throw 0;
  119. }
  120. }
  121. else
  122. {
  123. throw 1;
  124. }
  125. }
  126. else if(status == 0) {
  127. throw 4;
  128. }
  129. else if (status == -1) {
  130. throw 5;
  131. }
  132. #endif
  133. }
  134. void iv::control::CanUtil::closeCanCard() {
  135. if(m_connect != 0)
  136. {
  137. thread_receive->interrupt();
  138. thread_receive->join();
  139. thread_send->interrupt();
  140. thread_send->join();
  141. thread_receive->interrupt();
  142. thread_receive->join();
  143. unsigned int rec = VCI_CloseDevice(m_devtype, m_devind);
  144. #ifdef USE_MOBILEYE
  145. rec = VCI_CloseDevice(m_devtype, m_devindmob);
  146. #endif
  147. std::cout << "close can:" << rec << std::endl;
  148. }
  149. }
  150. void iv::control::CanUtil::receive() {
  151. while (m_connect)
  152. {
  153. VCI_CAN_OBJ receivedata[2500];
  154. int res = VCI_Receive(m_devtype, m_devind, m_cannum, receivedata, 2500, 10);
  155. if (res > 0) //未能从can卡中读到数
  156. {
  157. // std::cout<<"reee"<<std::endl;
  158. int i;
  159. for(i=0;i<res;i++)
  160. {
  161. if(receivedata[i].ID == 0x282)
  162. {
  163. int i;
  164. unsigned int xvalue[8];
  165. float fvalue[8];
  166. unsigned int x;
  167. for(i=0;i<8;i++)
  168. {
  169. memcpy(&x,&receivedata[i].Data[0],4);
  170. x = (x<<((7-i)*4))>>28;
  171. xvalue[i] = x;
  172. fvalue[i] = x;
  173. if(x == 0)fvalue[i] = -1;
  174. else fvalue[i] = (fvalue[i] - 1.0)*0.2;
  175. }
  176. ServiceCarStatus.multrasonic_obs.mfront_leftmid = fvalue[0];
  177. ServiceCarStatus.multrasonic_obs.mfront_left = fvalue[1];
  178. ServiceCarStatus.multrasonic_obs.mfront_right = fvalue[2];
  179. ServiceCarStatus.multrasonic_obs.mfront_rightmid = fvalue[3];
  180. ServiceCarStatus.multrasonic_obs.mrear_leftmid = fvalue[4];
  181. ServiceCarStatus.multrasonic_obs.mrear_left = fvalue[5];
  182. ServiceCarStatus.multrasonic_obs.mrear_right = fvalue[6];
  183. ServiceCarStatus.multrasonic_obs.mrear_rightmid = fvalue[7];
  184. // std::cout<<"rec oooo value is back "<<(int)(receivedata[i].Data[3])<<" head "<<(int)receivedata[i].Data[1]<<std::endl;
  185. }
  186. }
  187. }
  188. else
  189. {
  190. usleep(1000);
  191. }
  192. }
  193. }
  194. void iv::control::CanUtil::initial()
  195. {
  196. ServiceControlStatus.command.byte[0] = 0xEB;
  197. ServiceControlStatus.command.byte[1] = 0;
  198. ServiceControlStatus.command.byte[2] = 0;
  199. ServiceControlStatus.command.byte[3] = 0;
  200. ServiceControlStatus.command.byte[4] = 0;
  201. ServiceControlStatus.command.byte[5] = 0;
  202. ServiceControlStatus.command.byte[6] = 0;
  203. ServiceControlStatus.command.byte[7] = 0;
  204. }
  205. void iv::control::CanUtil::pack_command()
  206. {
  207. ServiceControlStatus.command.bit.heartbreak++;
  208. if (ServiceControlStatus.command.bit.heartbreak >= 256)
  209. ServiceControlStatus.command.bit.heartbreak = 0;
  210. ServiceControlStatus.command.byte[7] = (ServiceControlStatus.command.byte[1] + ServiceControlStatus.command.byte[2]
  211. + ServiceControlStatus.command.byte[3] + ServiceControlStatus.command.byte[4]
  212. + ServiceControlStatus.command.byte[5] + ServiceControlStatus.command.byte[6]) % 256;
  213. }
  214. void iv::control::CanUtil::send()
  215. {
  216. VCI_CAN_OBJ Command_data;
  217. QTime time1;
  218. int LastTimeSend1;
  219. time1.start();
  220. LastTimeSend1 = time1.elapsed();
  221. int Send1Int = 10; //100ms
  222. Command_data.SendType = m_sendtype;
  223. Command_data.ExternFlag = m_frameflag;
  224. Command_data.RemoteFlag = m_frameformat;
  225. Command_data.ID = ServiceControlStatus.command_ID;
  226. Command_data.DataLen = 8;
  227. initial();
  228. while (true)
  229. {
  230. int print_time = time1.elapsed() - LastTimeSend1;
  231. if (print_time >= Send1Int)
  232. {
  233. LastTimeSend1 = time1.elapsed();
  234. pack_command();
  235. memcpy(Command_data.Data, ServiceControlStatus.command.byte, 8);
  236. VCI_Transmit(m_devtype, m_devind, m_cannum, &Command_data, 1);
  237. /*for (int c = 0; c < 8; c++)
  238. ODS("%x ", Command_data.Data[c]);
  239. ODS("\n");*/
  240. //ODS(" %d \n", print_time);
  241. }
  242. }
  243. }
  244. /******************************************************mobileye AWS display*****************************************************
  245. * message ID:0x700
  246. * member: type physical unit range note
  247. * dusk_time: double / 0,1
  248. * night_time double / 0,1
  249. *******************************************************************************************************************************/
  250. void iv::control::CanUtil::impl_0x700(VCI_CAN_OBJ receivedata)
  251. {
  252. ServiceCarStatus.aws_display.dusk_time = (receivedata.Data[0] >> 3) & 0x1;
  253. ServiceCarStatus.aws_display.night_time = (receivedata.Data[0] >> 4) & 0x1;
  254. ServiceCarStatus.aws_display.lanes_on = receivedata.Data[4] & 0x1;
  255. //std::cout << "lanes_on:" << ServiceCarStatus.aws_display.lanes_on << std::endl;
  256. }
  257. /******************************************************mobileye lane***********************************************************
  258. * message ID:0x737
  259. * member: type physical unit range note
  260. * curvature: double 1/meter [-0.12, 0.12] "a" in the equation:y = ax^2+bx+c
  261. * heading: double / [-1.0, 1.0] "b" in the equation:y = ax^2+bx+c
  262. * construction area: bool / / /
  263. * pitch angle: double radians [-0.05, 0.05] pitch of the host vehicle(derived from lanes analysis)
  264. * yaw angle: double radians / yaw of the host vehicle(derived from lanes analysis)
  265. * right_LDW: bit/bool / 0,1 0 stands for unavailable, 1 for available
  266. * left_LDW: bit/bool / 0,1 0 stands for unavailable, 1 for available
  267. *******************************************************************************************************************************/
  268. void iv::control::CanUtil::impl_0x737(VCI_CAN_OBJ receivedata)
  269. {
  270. //lane curvature
  271. int32_t tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  272. tmp <<= 16;
  273. tmp >>= 16;
  274. ServiceCarStatus.Lane.curvature = tmp * 3.81 * 1e-6;
  275. //std::cout<<"curvature:"<<ServiceCarStatus.Lane.curvature<<std::endl;
  276. tmp = 0;
  277. //lane heading
  278. tmp = ((receivedata.Data[3] & 0xf) << 8) | receivedata.Data[2];
  279. tmp <<= 20;
  280. tmp >>= 20;
  281. ServiceCarStatus.Lane.heading = tmp * 0.0005;
  282. tmp = 0;
  283. //construction area
  284. ServiceCarStatus.Lane.is_ca = (receivedata.Data[3] >> 4) & 0x1;
  285. //left_LDW
  286. ServiceCarStatus.Lane.is_left_LDW_available = (receivedata.Data[3] >> 5) & 0x1;
  287. //right_LDW
  288. ServiceCarStatus.Lane.is_right_LDW_available = (receivedata.Data[3] >> 6) & 0x1;
  289. //yaw
  290. tmp = (receivedata.Data[5] >> 8) | receivedata.Data[4];
  291. tmp <<= 16;
  292. tmp >>= 16;
  293. //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
  294. ServiceCarStatus.Lane.yaw = tmp * 0.000977;
  295. std::cout << "yaw:"<< ServiceCarStatus.Lane.yaw<< std::endl;
  296. tmp = 0;
  297. //pitch
  298. tmp = (receivedata.Data[7] >> 8) | receivedata.Data[6];
  299. tmp <<= 16;
  300. tmp >>= 16;
  301. //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0 / 512;
  302. ServiceCarStatus.Lane.yaw = tmp * 0.000002;
  303. std::cout << "pitch:" << ServiceCarStatus.Lane.pitch << std::endl;
  304. }
  305. /**************************************************** mobileye obstacle status ****************************************************************************
  306. * message ID:0x738
  307. * member: type physical unit range note
  308. * num_obstacles int / [0:255] /
  309. * timestamp int ms [0:255] only the lowest 8 bits of the timestamp is given
  310. * app_version: int / [0:255] vesion info consists of X.Y.Z.W
  311. * active_version int/2bits / [0:3] index of the active section of app_version signal
  312. * left_close_rang_cut_in bool / 0,1 0 false, 1 true
  313. * right_close_rang_cut_in bool / 0,1 0 false, 1 true
  314. * go enum / 0,1...15 0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
  315. * protocol version uchar/8bit / 0x00...0xff /
  316. * is_close_car bool / 0,1 0 no close, 1 close car exists
  317. * failsafe int/4bit / [0:7] failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
  318. *************************************************************************************************************************************************************/
  319. void iv::control::CanUtil::impl_0x738(VCI_CAN_OBJ receivedata)
  320. {
  321. //num_obstacles
  322. ServiceCarStatus.obstacleStatus.num_obstacles = receivedata.Data[0];
  323. //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;
  324. //timestamp
  325. ServiceCarStatus.obstacleStatus.timestamp = receivedata.Data[1];
  326. //app version
  327. ServiceCarStatus.obstacleStatus.app_version = receivedata.Data[2];
  328. //active version
  329. ServiceCarStatus.obstacleStatus.active_version = receivedata.Data[3] & 0x3;
  330. //is left close rang cut in
  331. ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (receivedata.Data[3] >> 2) & 0x1;
  332. //is right close rang cut in
  333. ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (receivedata.Data[3] >> 3) & 0x1;
  334. //go
  335. ServiceCarStatus.obstacleStatus.go = receivedata.Data[3] >> 4;
  336. //protocol version
  337. ServiceCarStatus.obstacleStatus.protocol_version = receivedata.Data[4];
  338. //close car
  339. ServiceCarStatus.obstacleStatus.is_close_car = receivedata.Data[5] & 0x1;
  340. //failsafe
  341. ServiceCarStatus.obstacleStatus.failsafe = (receivedata.Data[5] >> 1) & 0xf;
  342. }
  343. /**************************************************** mobileye obstacle data a ****************************************************************************
  344. * message ID:0x739
  345. * member: type physical unit range note
  346. * obstacle_ID int / [0:63] new obstacles are given the last used free ID
  347. * obstacle_pos_x double m [0,250] longtitude position of the obstacle relative to the ref point
  348. * obstacle_pos_y double m [-31.93,31.93] lateral position of the obstacle, error relative to pos_x below 10% or 2meters
  349. * blinker_info int/enum / [0:4] 0 unavailable, 1 off, 2 left, 3 right, 4 both indicated blinkers status
  350. * cut_in_and_out int / [0:4] 0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
  351. * cut_in: target entering the host lane, cut_out: exiting but not distinguish between sides
  352. * obstacle_rel_vel_x double m/s [-127.93,127.93] relative longtitude velocity of the obstacle
  353. * obstacle_status int/enum / [0:6] 0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
  354. * 4 oncoming, 5 parked(never moved, back lights off), 6 unused
  355. * is_obstacle_brake_lights bool / 0,1 0 object's brake light off or not identified, 1 object's brake light on
  356. * obstacle_valid int/enum / [1:2] 1 new valid(detected this frame), 2 older valid
  357. *************************************************************************************************************************************************************/
  358. obstacle_data_a iv::control::CanUtil::impl_0x739(VCI_CAN_OBJ receivedata)
  359. {
  360. int32_t tmp = 0;
  361. obstacle_data_a obs_tmp;
  362. //obstacle id
  363. obs_tmp.obstacle_ID = receivedata.Data[0];
  364. //ServiceCarStatus.obstacleStatusA.obstacle_ID = receivedata.Data[0];
  365. //obstacle pos x
  366. tmp = ((receivedata.Data[2] & 0xf) << 8) | receivedata.Data[1];
  367. obs_tmp.obstacle_pos_x = tmp * 0.0625;
  368. //ServiceCarStatus.obstacleStatusA.obstacle_pos_x = tmp * 0.0625;
  369. tmp = 0;
  370. //obstacle pos y
  371. tmp = ((receivedata.Data[4] & 0x3) << 8) | receivedata.Data[3];
  372. obs_tmp.obstacle_pos_y = tmp * 0.0625;
  373. //ServiceCarStatus.obstacleStatusA.obstacle_pos_y = tmp * 0.0625;
  374. tmp = 0;
  375. //blinker info
  376. obs_tmp.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
  377. //ServiceCarStatus.obstacleStatusA.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
  378. //cut in and out
  379. obs_tmp.cut_in_and_out = receivedata.Data[4] >> 5;
  380. //ServiceCarStatus.obstacleStatusA.cut_in_and_out = receivedata.Data[4] >> 5;
  381. //obstacle rel vel x
  382. tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
  383. obs_tmp.obstacle_rel_vel_x = tmp * 0.0625;
  384. //ServiceCarStatus.obstacleStatusA.obstacle_rel_vel_x = tmp * 0.0625;
  385. tmp = 0;
  386. //obstacle type
  387. obs_tmp.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
  388. //ServiceCarStatus.obstacleStatusA.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
  389. //obstacle status
  390. obs_tmp.obstacle_status = receivedata.Data[7] & 0x7;
  391. //ServiceCarStatus.obstacleStatusA.obstacle_status = receivedata.Data[7] & 0x7;
  392. //obstacle brake lights
  393. obs_tmp.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
  394. //ServiceCarStatus.obstacleStatusA.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
  395. //obstacle valid
  396. obs_tmp.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
  397. //ServiceCarStatus.obstacleStatusA.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
  398. return obs_tmp;
  399. }
  400. /**************************************************** mobileye obstacle data b ****************************************************************************
  401. * message ID:0x73a
  402. * member: type physical unit range note
  403. * obstacle_length double m [0,31] length of the obstacle(longitude axis)
  404. * obstacle_width double m [0,12.5] width of the obstacle(lateral axis)
  405. * obstacle_age int / [0:255] value starts at 1 when it is first detected, increaments in i each frame
  406. * obstacle_lane int/enum / [0:3] 0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
  407. * 3 invalid signal
  408. * is_cipv_flag int/enum / [0:1] 0 not cipv, 1 cipv(closest in path vehicle)
  409. * radar_pos_x double m [0,250] longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
  410. * if no radar target is matched, value = 0xfff
  411. * radar_vel_x double m/s [-127.93,127.93] longitude velocity of the radar target matched to the vision targets, if no radar target is matched value=0xfff
  412. * 4 oncoming, 5 parked(never moved, back lights off), 6 unused
  413. * radar_match_confidence int / [0:5] 0 no match, 1 multi match: radar doesn't describe well, 2-4 vision-radar match:with bounded error between
  414. * vision and radar measurements higher->smaller error, 5 high confidence match
  415. * matched_radar_id int / [0:127] ID of Primary radar target matched to the vision target if applicable
  416. *************************************************************************************************************************************************************/
  417. obstacle_data_b iv::control::CanUtil::impl_0x73a(VCI_CAN_OBJ receivedata)
  418. {
  419. int32_t tmp = 0;
  420. obstacle_data_b obs_tmp;
  421. //obstacle length
  422. tmp = receivedata.Data[0];
  423. obs_tmp.obstacle_length = tmp * 0.5;
  424. //ServiceCarStatus.obstacleStatusB.obstacle_length = tmp * 0.5;
  425. tmp = 0;
  426. //obstacle width
  427. tmp = receivedata.Data[1];
  428. obs_tmp.obstacle_width = tmp * 0.05;
  429. //ServiceCarStatus.obstacleStatusB.obstacle_width = tmp * 0.05;
  430. tmp = 0;
  431. //obstacle age
  432. obs_tmp.obstacle_age = receivedata.Data[2];
  433. //ServiceCarStatus.obstacleStatusB.obstacle_age = receivedata.Data[2];
  434. //obstacle lane
  435. obs_tmp.obstacle_lane = receivedata.Data[3] & 0x3;
  436. //ServiceCarStatus.obstacleStatusB.obstacle_lane = receivedata.Data[3] & 0x3;
  437. //is cipv flag
  438. obs_tmp.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
  439. //ServiceCarStatus.obstacleStatusB.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
  440. //radar pos x
  441. tmp = (receivedata.Data[4] << 4) | (receivedata.Data[3] >> 4);
  442. obs_tmp.radar_pos_x = tmp * 0.0625;
  443. //ServiceCarStatus.obstacleStatusB.radar_pos_x = tmp * 0.0625;
  444. tmp = 0;
  445. //radar vel x
  446. tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
  447. tmp <<= 20;
  448. tmp >>= 20;
  449. obs_tmp.radar_vel_x = tmp * 0.0625;
  450. //ServiceCarStatus.obstacleStatusB.radar_vel_x = tmp * 0.0625;
  451. tmp = 0;
  452. //radar match confidence
  453. obs_tmp.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
  454. //ServiceCarStatus.obstacleStatusB.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
  455. //matched radar ID
  456. obs_tmp.matched_radar_id = receivedata.Data[7] & 0x7f;
  457. //ServiceCarStatus.obstacleStatusB.matched_radar_id = receivedata.Data[7] & 0x7f;
  458. return obs_tmp;
  459. }
  460. /**************************************************** mobileye obstacle data c ****************************************************************************
  461. * message ID:0x73b
  462. * member: type physical unit range note
  463. * obstacle_angle_rate double degree/s [-327.68, 327.68] Angle rate of Center of Obstacle, negative->moved to left
  464. * obstcale_scale_change double pix/s [-6.5532, 6.5532] /
  465. * object_accel_x double m/s2 [-14.97, 14.97] longtitude acceleration of the object
  466. * obstacle_replaced bool / 0,1 0 not replaced in this frame, 1 replace in this frame
  467. * obstacle_angle double degree [-327.68,327.68] Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
  468. * exactly in front of us, a positive angle indicates that theobstacle is to the right
  469. *************************************************************************************************************************************************************/
  470. obstacle_data_c iv::control::CanUtil::impl_0x73b(VCI_CAN_OBJ receivedata)
  471. {
  472. int32_t tmp = 0;
  473. obstacle_data_c obs_tmp;
  474. //obstacle angle rate
  475. tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  476. tmp <<= 16;
  477. tmp >>= 16;
  478. obs_tmp.obstacle_angle_rate = tmp * 0.01;
  479. //ServiceCarStatus.obstacleStatusC.obstacle_angle_rate = tmp * 0.01;
  480. tmp = 0;
  481. //obstacle scale change
  482. tmp = (receivedata.Data[3] << 8) | receivedata.Data[2];
  483. tmp <<= 16;
  484. tmp >>= 16;
  485. obs_tmp.obstacle_scale_change = tmp * 0.0002;
  486. //ServiceCarStatus.obstacleStatusC.obstacle_scale_change = tmp * 0.0002;
  487. tmp = 0;
  488. //object accel x
  489. tmp = ((receivedata.Data[5] & 0x3) << 8) | receivedata.Data[4];
  490. tmp <<= 22;
  491. tmp >>= 22;
  492. obs_tmp.object_accel_x = tmp * 0.03;
  493. //ServiceCarStatus.obstacleStatusC.object_accel_x = tmp * 0.03;
  494. tmp = 0;
  495. //obstacle replaced
  496. obs_tmp.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
  497. //ServiceCarStatus.obstacleStatusC.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
  498. //obstacle angle
  499. tmp = (receivedata.Data[7] << 8) | receivedata.Data[6];
  500. tmp <<= 16;
  501. tmp >>= 16;
  502. obs_tmp.obstacle_angle = tmp * 0.01;
  503. //ServiceCarStatus.obstacleStatusC.obstacle_angle = tmp * 0.01;
  504. return obs_tmp;
  505. }
  506. /**************************************************** mobileye aftermarket lane ****************************************************************************
  507. * message ID:0x669
  508. * member: type physical unit range note
  509. * lane_conf_left int/2bit / [0:3] 0 lowest, 3 highest
  510. * is_ldw_availablity_left bool / 0,1 lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
  511. * lane_type_left int/enum / [0:6] 0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
  512. * dist_to_lane_l double m [-40:40] "c" in the equation:y = ax^2+bx+c
  513. * lane_conf_right int/2bit / [0:3] 0 lowest, 3 highest
  514. * is_ldw_availablity_right bool / 0,1 lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
  515. * lane_type_right int/enum / [0:6] 0 dashed, 1 solid, 2 none, 3 road edge, 4 double lane mark, 5 sbott's dots, 6 invalid
  516. * dist_to_lane_r double m [-40:40] "c" in the equation:y = ax^2+bx+c
  517. *************************************************************************************************************************************************************/
  518. void iv::control::CanUtil::impl_0x669(VCI_CAN_OBJ receivedata)
  519. {
  520. int32_t tmp;
  521. //lane confidence left
  522. tmp = receivedata.Data[0] & 0x3;
  523. tmp <<= 30;
  524. tmp >>= 30;
  525. ServiceCarStatus.aftermarketLane.lane_conf_left = tmp;
  526. //std::cout << "left confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_left << std::endl;
  527. tmp = 0;
  528. //is LDW availability left
  529. ServiceCarStatus.aftermarketLane.is_ldw_availablility_left = (receivedata.Data[0] >> 2) & 0x1;
  530. //lane type left
  531. tmp = (receivedata.Data[0] >> 4) & 0xf;
  532. tmp <<= 28;
  533. tmp >>= 28;
  534. ServiceCarStatus.aftermarketLane.lane_type_left = tmp;
  535. std::cout << "lane_type_left: " << ServiceCarStatus.aftermarketLane.lane_type_left << std::endl;
  536. tmp = 0;
  537. //dist to left lane
  538. tmp = (receivedata.Data[2] << 4) | ((receivedata.Data[1] >> 4) & 0xf);
  539. tmp <<= 20;
  540. tmp >>= 20;
  541. ServiceCarStatus.aftermarketLane.dist_to_lane_l = tmp * 0.02;
  542. std::cout << "dist_to_lane_l:" << ServiceCarStatus.aftermarketLane.dist_to_lane_l << std::endl;
  543. tmp = 0;
  544. //lane confidence right
  545. tmp = receivedata.Data[5] & 0x3;
  546. tmp <<= 30;
  547. tmp >>= 30;
  548. ServiceCarStatus.aftermarketLane.lane_conf_right = tmp;
  549. //std::cout << "right confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_right << std::endl;
  550. tmp = 0;
  551. //is LDW availability right
  552. ServiceCarStatus.aftermarketLane.is_ldw_availablility_right = (receivedata.Data[5] >> 2) & 0x1;
  553. //lane type right
  554. tmp = (receivedata.Data[5] >> 4) & 0xf;
  555. tmp <<= 28;
  556. tmp >>= 28;
  557. ServiceCarStatus.aftermarketLane.lane_type_right = tmp;
  558. std::cout<<"lane_type_right: " << ServiceCarStatus.aftermarketLane.lane_type_right << std::endl;
  559. tmp = 0;
  560. //dist to right lane
  561. tmp = (receivedata.Data[7] << 4) | ((receivedata.Data[6] >> 4) & 0xf);
  562. tmp <<= 20;
  563. tmp >>= 20;
  564. ServiceCarStatus.aftermarketLane.dist_to_lane_r = tmp * 0.02;
  565. std::cout << "dist_to_lane_r:" << ServiceCarStatus.aftermarketLane.dist_to_lane_r << std::endl;
  566. tmp = 0;
  567. }
  568. /**************************************************** mobileye LKA left lane a ****************************************************************************
  569. * message ID:0x766
  570. * member: type physical unit range note
  571. * lane_type enum / [0:6] 0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
  572. * quality enum / [0:3] 0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
  573. * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
  574. * position double m [-127,128] physical distance between lane mark and camera on the lateral position
  575. * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
  576. * curvature_derivative double / [-0.00012, 0.00012] /
  577. * width_left_marking double m [0, 2.5] left lane marking width
  578. *************************************************************************************************************************************************************/
  579. void iv::control::CanUtil::impl_0x766(VCI_CAN_OBJ receivedata)
  580. {
  581. int32_t tmp = 0;
  582. //lane type
  583. ServiceCarStatus.LKAleftLaneA.lane_type = receivedata.Data[0] & 0xf;
  584. //quality
  585. ServiceCarStatus.LKAleftLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
  586. //model degree
  587. ServiceCarStatus.LKAleftLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
  588. //position
  589. tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
  590. tmp <<= 16;
  591. tmp >>= 16;
  592. ServiceCarStatus.LKAleftLaneA.position = tmp / 256.0;
  593. //ServiceCarStatus.LKAleftLaneA.position = tmp * 0.003906;
  594. tmp = 0;
  595. //curvature
  596. tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
  597. ServiceCarStatus.LKAleftLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
  598. //ServiceCarStatus.LKAleftLaneA.curvature = tmp * 0.000001 + -0.031999;
  599. tmp = 0;
  600. //curvature derivative
  601. tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
  602. ServiceCarStatus.LKAleftLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
  603. //ServiceCarStatus.LKAleftLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
  604. tmp = 0;
  605. //width left marking
  606. tmp = receivedata.Data[7];
  607. ServiceCarStatus.LKAleftLaneA.width_left_marking = tmp * 0.01;
  608. }
  609. /**************************************************** mobileye LKA left lane b ****************************************************************************
  610. * message ID:0x767
  611. * member: type physical unit range note
  612. * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
  613. * view_range double m [0, 127.996] physical view range of lane mark
  614. * view_range_availability enum / 0,1 0 not valid, 1 valid
  615. *************************************************************************************************************************************************************/
  616. void iv::control::CanUtil::impl_0x767(VCI_CAN_OBJ receivedata)
  617. {
  618. int32_t tmp = 0;
  619. //heading angle
  620. tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  621. ServiceCarStatus.LKAleftLaneB.heading_angle = (tmp - 0x7fff) / 1024;
  622. //ServiceCarStatus.LKAleftLaneB.heading_angle = tmp * 0.000977 + -31.999023;
  623. tmp = 0;
  624. //view range
  625. tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
  626. ServiceCarStatus.LKAleftLaneB.view_range = tmp / 256.0;
  627. //ServiceCarStatus.LKAleftLaneB.view_range = tmp * 0.003906;
  628. tmp = 0;
  629. //view range availability
  630. ServiceCarStatus.LKAleftLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
  631. }
  632. /**************************************************** mobileye LKA right lane a ****************************************************************************
  633. * message ID:0x768
  634. * member: type physical unit range note
  635. * lane_type enum / [0:6] 0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
  636. * quality enum / [0:3] 0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
  637. * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
  638. * position double m [-127,128] physical distance between lane mark and camera on the lateral position
  639. * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
  640. * curvature_derivative double / [-0.00012, 0.00012] /
  641. * width_right_marking double m [0, 2.5] right lane marking width
  642. *************************************************************************************************************************************************************/
  643. void iv::control::CanUtil::impl_0x768(VCI_CAN_OBJ receivedata)
  644. {
  645. int32_t tmp = 0;
  646. //lane type
  647. ServiceCarStatus.LKArightLaneA.lane_type = receivedata.Data[0] & 0xf;
  648. //quality
  649. ServiceCarStatus.LKArightLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
  650. //model degree
  651. ServiceCarStatus.LKArightLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
  652. //position
  653. tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
  654. tmp <<= 16;
  655. tmp >>= 16;
  656. ServiceCarStatus.LKArightLaneA.position = tmp / 256.0;
  657. //ServiceCarStatus.LKArightLaneA.position = tmp * 0.003906;
  658. tmp = 0;
  659. //curvature
  660. tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
  661. ServiceCarStatus.LKArightLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
  662. //ServiceCarStatus.LKArightLaneA.curvature = tmp * 0.000001 + -0.031999;
  663. tmp = 0;
  664. //curvature derivative
  665. tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
  666. ServiceCarStatus.LKArightLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
  667. //ServiceCarStatus.LKArightLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
  668. tmp = 0;
  669. //width left marking
  670. tmp = receivedata.Data[7];
  671. ServiceCarStatus.LKArightLaneA.width_left_marking = tmp * 0.01;
  672. }
  673. /**************************************************** mobileye LKA right lane b ****************************************************************************
  674. * message ID:0x767
  675. * member: type physical unit range note
  676. * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
  677. * view_range double m [0, 127.996] physical view range of lane mark
  678. * view_range_availability enum / 0,1 0 not valid, 1 valid
  679. *************************************************************************************************************************************************************/
  680. void iv::control::CanUtil::impl_0x769(VCI_CAN_OBJ receivedata)
  681. {
  682. int32_t tmp = 0;
  683. //heading angle
  684. tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  685. ServiceCarStatus.LKArightLaneB.heading_angle = (tmp - 0x7fff) / 1024;
  686. //ServiceCarStatus.LKArightLaneB.heading_angle = tmp * 0.000977 + -31.999023;
  687. tmp = 0;
  688. //view range
  689. tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
  690. ServiceCarStatus.LKArightLaneB.view_range = tmp / 256.0;
  691. //ServiceCarStatus.LKArightLaneB.view_range = tmp * 0.003906;
  692. tmp = 0;
  693. //view range availability
  694. ServiceCarStatus.LKArightLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
  695. }
  696. /**************************************************** mobileye next lane a **********************************************************************************
  697. * message ID:0x76c
  698. * member: type physical unit range note
  699. * lane_type enum / [1:2] 1 solid, 2 undecided
  700. * quality / / / not yet implemented for next lanes
  701. * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
  702. * position double m [-127,128] physical distance between lane mark and camera on the lateral position
  703. * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
  704. * curvature_derivative double / [-0.00012, 0.00012] /
  705. * lane_mark_widt double m [0, 2.5] lane marking width
  706. *************************************************************************************************************************************************************/
  707. next_lane_a iv::control::CanUtil::impl_0x76c(VCI_CAN_OBJ receivedata)
  708. {
  709. int32_t tmp = 0;
  710. next_lane_a nextLane_tmp;
  711. //lane type
  712. nextLane_tmp.lane_type = receivedata.Data[0] & 0xf;
  713. //ServiceCarStatus.nextLaneA.lane_type = receivedata.Data[0] & 0xf;
  714. //quality
  715. nextLane_tmp.quality = (receivedata.Data[0] >> 4) & 0x3;
  716. //ServiceCarStatus.nextLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
  717. //model degree
  718. nextLane_tmp.model_degree = (receivedata.Data[0] >> 6) & 0x3;
  719. //ServiceCarStatus.nextLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
  720. //position
  721. tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
  722. tmp <<= 16;
  723. tmp >>= 16;
  724. nextLane_tmp.position = tmp * 0.003906;
  725. //ServiceCarStatus.nextLaneA.position = tmp / 256.0;
  726. //ServiceCarStatus.nextLaneA.position = tmp * 0.003906;
  727. tmp = 0;
  728. //curvature
  729. tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
  730. nextLane_tmp.curvature = tmp * 0.000001 + -0.031999;
  731. //ServiceCarStatus.nextLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
  732. //ServiceCarStatus.nextLaneA.curvature = tmp * 0.000001 + -0.031999;
  733. tmp = 0;
  734. //curvature derivative
  735. tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
  736. nextLane_tmp.curvature_derivative = tmp * 0.000000 + -0.000122;
  737. //ServiceCarStatus.nextLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
  738. //ServiceCarStatus.nextLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
  739. tmp = 0;
  740. //width left marking
  741. tmp = receivedata.Data[7];
  742. nextLane_tmp.lane_mark_width = tmp * 0.01;
  743. //ServiceCarStatus.nextLaneA.lane_mark_width = tmp * 0.01;
  744. return nextLane_tmp;
  745. }
  746. /**************************************************** mobileye LKA right lane b ****************************************************************************
  747. * message ID:0x76d
  748. * member: type physical unit range note
  749. * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
  750. * view_range double m [0, 127.996] physical view range of lane mark
  751. * view_range_availability enum / 0,1 0 not valid, 1 valid
  752. *************************************************************************************************************************************************************/
  753. next_lane_b iv::control::CanUtil::impl_0x76d(VCI_CAN_OBJ receivedata)
  754. {
  755. int32_t tmp = 0;
  756. next_lane_b nextLane_tmp;
  757. //heading angle
  758. tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  759. nextLane_tmp.heading_angle = tmp * 0.000977 + -31.999023;
  760. //ServiceCarStatus.nextLaneB.heading_angle = (tmp - 0x7fff) / 1024;
  761. //ServiceCarStatus.nextLaneB.heading_angle = tmp * 0.000977 + -31.999023;
  762. tmp = 0;
  763. //view range
  764. tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
  765. nextLane_tmp.view_range = tmp * 0.003906;
  766. //ServiceCarStatus.nextLaneB.view_range = tmp / 256.0;
  767. //ServiceCarStatus.nextLaneB.view_range = tmp * 0.003906;
  768. tmp = 0;
  769. //view range availability
  770. nextLane_tmp.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
  771. //ServiceCarStatus.nextLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
  772. return nextLane_tmp;
  773. }
  774. /**************************************************** mobileye reference points ******************************************************************************
  775. * message ID:0x76a
  776. * member: type physical unit range note
  777. * ref_point_1_position double m [-127.996, 127.996] physical distance between camera and reference point 1 on lateral axis.
  778. * The reference point defines the lateral location of the lane center at ref-Point distance.
  779. * ref Point 1 is this measurement at 1 second headway.
  780. * ref_point_1_dist double m [0, 127.99609376] physical distance between reference point and camera
  781. * ref_point_1_validity enum / 0,1 0 invalid, 1 valid
  782. * ref_point_2_position double m [-127.996, 127.996] empty! physical distance between camera and reference point 2 on lateral axis.
  783. * The reference point defines the lateral location of the lane center at ref-Point distance.
  784. * ref Point 2 is this measurement at 1 second headway.
  785. * ref_point_2_dist double m [0, 127.99609376] empty! physical distance between reference point and camera
  786. * ref_point_2_validity enum / 0,1 empty! 0 invalid, 1 valid
  787. *************************************************************************************************************************************************************/
  788. void iv::control::CanUtil::impl_0x76a(VCI_CAN_OBJ receivedata)
  789. {
  790. int32_t tmp = 0;
  791. //ref_point_1_position
  792. //tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
  793. ServiceCarStatus.refPoints.ref_point_1_position = (tmp - 0x7fff) / 256.0;
  794. ServiceCarStatus.refPoints.ref_point_1_position = tmp * 0.003906 + -127.996094;
  795. tmp = 0;
  796. //ref_point_1_dist
  797. tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
  798. //ServiceCarStatus.refPoints.ref_point_1_dist = tmp / 256.0;
  799. ServiceCarStatus.refPoints.ref_point_1_dist = tmp * 0.003906;
  800. tmp = 0;
  801. //is ref point 1 validity
  802. ServiceCarStatus.refPoints.is_ref_point_1_validity = (receivedata.Data[3] >> 7) & 0x1;
  803. //ref_point_2_position
  804. tmp = (receivedata.Data[5] << 8) | receivedata.Data[4];
  805. //ServiceCarStatus.refPoints.ref_point_2_position = (tmp - 0x7fff) / 256.0;
  806. ServiceCarStatus.refPoints.ref_point_2_position = tmp * 0.003906 + -127.996094;
  807. tmp = 0;
  808. //ref_point_2_dist
  809. tmp = ((receivedata.Data[7] & 0x7f) << 8) | receivedata.Data[6];
  810. //ServiceCarStatus.refPoints.ref_point_2_dist = tmp / 256.0;
  811. ServiceCarStatus.refPoints.ref_point_2_dist = tmp * 0.003906;
  812. tmp = 0;
  813. //is ref point 2 validity
  814. ServiceCarStatus.refPoints.is_ref_point_2_validity = (receivedata.Data[7] >> 7) & 0x1;
  815. }
  816. /********************************************** mobileye number of next lane markers reported **************************************************************
  817. * message ID:0x76b
  818. * member: type physical unit range note
  819. * num_of_next_lane_mark_reported enum / 1,2 indicates how many extra lane markers are also reported, on top of left
  820. * and right lane. Currently two lane marks are always reported one for left and one for
  821. * right, and they are valid only if the lane type is Solid.
  822. *************************************************************************************************************************************************************/
  823. void iv::control::CanUtil::impl_0x76b(VCI_CAN_OBJ receivedata)
  824. {
  825. //num_of_next_lane_mark_reported
  826. ServiceCarStatus.numOfNextLaneMarkReported.num_of_next_lane_mark_reported = receivedata.Data[0];
  827. }
  828. void iv::control::CanUtil::receive_radar() {
  829. int32_t range, rate, angle;
  830. while (m_connect)
  831. {
  832. VCI_CAN_OBJ receivedata[2500];
  833. int res = VCI_Receive(m_devtype, m_devind, m_cannum_radar, receivedata, 2500, 10);
  834. int radar_ID_index = -1;
  835. if (res <= 0) //未能从can卡中读到数
  836. {
  837. #ifdef linux
  838. usleep(1000);
  839. #endif
  840. #ifdef WIN32
  841. Sleep(1);
  842. #endif
  843. continue;
  844. }
  845. for (int i = 0; i < res; i++) {
  846. //对应毫米波雷达数据解析
  847. radar_ID_index = receivedata[i].ID - 0x500;
  848. if (radar_ID_index >= 0x00 && radar_ID_index <= 0x3f) {
  849. ServiceCarStatus.mRadarS = 10;
  850. angle = ((receivedata[i].Data[1] & 0x1F) << 5) + ((receivedata[i].Data[2] & 0xF8) / 8);
  851. range = ((receivedata[i].Data[2] & 0x07) << 8) + receivedata[i].Data[3];
  852. rate = ((receivedata[i].Data[6] & 0x3F) << 8) | receivedata[i].Data[7];
  853. if (angle & 0x200) {
  854. angle = angle | 0xFFFFFC00;
  855. }
  856. if (rate & 0x2000) {
  857. rate = rate | 0xFFFFC000;
  858. }
  859. //If angle and range both are 0, it is an invalid data.
  860. if (angle != 0 || range != 0) {
  861. obs_find_flag = true;
  862. ServiceCarStatus.obs_radar[radar_ID_index].valid = true;
  863. ServiceCarStatus.obs_radar[radar_ID_index].nomal_x = range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI);
  864. ServiceCarStatus.obs_radar[radar_ID_index].nomal_y = range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI);
  865. ServiceCarStatus.obs_radar[radar_ID_index].speed_relative = rate * 1.0 / 100.0;
  866. ServiceCarStatus.obs_radar[radar_ID_index].speed_x = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * sin(angle / 1800.0 * M_PI);
  867. ServiceCarStatus.obs_radar[radar_ID_index].speed_y = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * cos(angle / 1800.0 * M_PI);
  868. }
  869. else {
  870. ServiceCarStatus.obs_radar[radar_ID_index].valid = false;
  871. }
  872. }
  873. }
  874. }
  875. }
  876. void iv::control::CanUtil::obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC)
  877. {
  878. int vec_size = obstacleStatusA.size();
  879. if(vec_size == 0)
  880. {
  881. return;
  882. }
  883. std::cout<< "*****************************************************************************************"<<std::endl;
  884. for(int i = 0; i < vec_size; i++)
  885. {
  886. std::cout<< "obstacle ID: " << obstacleStatusA[i].obstacle_ID << std::endl;
  887. std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_x << std::endl;
  888. std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_y << std::endl;
  889. std::cout<< "obstacle_rel_vel_x: " << obstacleStatusA[i].obstacle_rel_vel_x << std::endl;
  890. std::cout<< "obstacle_type: " << obstacleStatusA[i].obstacle_type << std::endl;
  891. std::cout<< "obstacle_status: " << obstacleStatusA[i].obstacle_status << std::endl;
  892. std::cout<< "obstacle_brake_light: " << obstacleStatusA[i].is_obstacle_brake_lights << std::endl;
  893. std::cout<< "obstacle_valid: " << obstacleStatusA[i].obstacle_valid << std::endl;
  894. // //std::cout<< "obstacle_length: " << obstacleStatusB[i].obstacle_length << std::endl;
  895. // std::cout<< "obstacle_width: " << obstacleStatusB[i].obstacle_width << std::endl;
  896. // std::cout<< "obstacle_age: " << obstacleStatusB[i].obstacle_age << std::endl;
  897. // std::cout<< "obstacle_cipv: " << obstacleStatusB[i].is_cipv_flag << std::endl;
  898. // std::cout<< "obstacle_radar_pos_x: " << obstacleStatusB[i].radar_pos_x << std::endl;
  899. // std::cout<< "obstacle_radar_vel_x: " << obstacleStatusB[i].radar_vel_x << std::endl;
  900. // std::cout<< "obstacle_angle_rate: " << obstacleStatusC[i].obstacle_angle_rate << std::endl;
  901. // std::cout<< "obstacle_angle" << obstacleStatusC[i].obstacle_angle << std::endl;
  902. // std::cout<< "object_accel_x" << obstacleStatusC[i].object_accel_x << std::endl;
  903. // std::cout<< "obstacle_scale_change: " << obstacleStatusC[i].obstacle_scale_change << std::endl;
  904. // std::cout<< "is_obstacle_replaced: " << obstacleStatusC[i].is_obstacle_replaced<< std::endl;
  905. }
  906. std::cout<< "*****************************************************************************************"<<std::endl;
  907. }
  908. #ifdef USE_MOBILEYE
  909. void iv::control::CanUtil::receive_mobileye(){
  910. while(m_connect)
  911. {
  912. memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_a) * 15);
  913. memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_b) * 15);
  914. memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_c) * 15);
  915. int obs_a_count = 0, obs_b_count = 0, obs_c_count = 0;
  916. int left_lane_a_count = 0, right_lane_a_count = 0, left_lane_b_count = 0, right_lane_b_count = 0;
  917. obstacle_data_a a_tmp;
  918. obstacle_data_b b_tmp;
  919. obstacle_data_c c_tmp;
  920. next_lane_a left_tmp_a, right_tmp_a;
  921. next_lane_b left_tmp_b, right_tmp_b;
  922. VCI_CAN_OBJ receivedata[2500];
  923. int res = VCI_Receive(m_devtype, m_devindmob, m_cannum, receivedata, 2500, 10);
  924. if (res <= 0)
  925. {
  926. #ifdef linux
  927. usleep(1000);
  928. #endif
  929. #ifdef WIN32
  930. Sleep(1);
  931. #endif
  932. continue;
  933. }
  934. for (int i = 0; i < res; i++)
  935. {
  936. switch(receivedata[i].ID)
  937. {
  938. case 0x700:
  939. impl_0x700(receivedata[i]);
  940. break;
  941. case 0x737:
  942. impl_0x737(receivedata[i]);
  943. break;
  944. case 0x738:
  945. impl_0x738(receivedata[i]);
  946. break;
  947. case 0x739: case 0x73c: case 0x73f: case 0x742: case 0x745: case 0x748: case 0x74b: case 0x74e: case 0x751: case 0x754: case 0x757: case 0x75a: case 0x75d: case 0x760: case 0x763:
  948. a_tmp = impl_0x739(receivedata[i]);
  949. memcpy(&(ServiceCarStatus.obstacleStatusA[obs_a_count]), &a_tmp, sizeof(obstacle_data_a));
  950. obs_a_count++;
  951. break;
  952. case 0x73a: case 0x73d: case 0x740: case 0x743: case 0x746: case 0x749: case 0x74c: case 0x74f: case 0x752: case 0x755: case 0x758: case 0x75b: case 0x75e: case 0x761: case 0x764:
  953. b_tmp = impl_0x73a(receivedata[i]);
  954. memcpy(&(ServiceCarStatus.obstacleStatusB[obs_b_count]), &b_tmp, sizeof(obstacle_data_b));
  955. obs_b_count++;
  956. break;
  957. case 0x73b: case 0x73e: case 0x741: case 0x744: case 0x747: case 0x74a: case 0x74d: case 0x750: case 0x753: case 0x756: case 0x759: case 0x75c: case 0x75f: case 0x762: case 0x765:
  958. c_tmp = impl_0x73b(receivedata[i]);
  959. memcpy(&(ServiceCarStatus.obstacleStatusC[obs_a_count]), &c_tmp, sizeof(obstacle_data_c));
  960. obs_c_count++;
  961. break;
  962. case 0x669:
  963. impl_0x669(receivedata[i]);
  964. break;
  965. case 0x766:
  966. impl_0x766(receivedata[i]);
  967. break;
  968. case 0x767:
  969. impl_0x767(receivedata[i]);
  970. break;
  971. case 0x768:
  972. impl_0x768(receivedata[i]);
  973. break;
  974. case 0x769:
  975. impl_0x769(receivedata[i]);
  976. break;
  977. case 0x76c: case 0x76e: case 0x771: case 0x773: case 0x775: case 0x777: case 0x779: case 0x77b:
  978. if (receivedata[i].ID == 0x76c || receivedata[i].ID == 0x771 || receivedata[i].ID == 0x775 || receivedata[i].ID == 0x779)
  979. {
  980. left_tmp_a = impl_0x76c(receivedata[i]);
  981. memcpy(&(ServiceCarStatus.nextLaneA_left[left_lane_a_count]), &left_tmp_a, sizeof(next_lane_a));
  982. left_lane_a_count++;
  983. }
  984. else
  985. {
  986. right_tmp_a = impl_0x76c(receivedata[i]);
  987. memcpy(&(ServiceCarStatus.nextLaneA_right[right_lane_a_count]), &right_tmp_a, sizeof(next_lane_a));
  988. right_lane_a_count++;
  989. }
  990. //impl_0x76c(receivedata[i]);
  991. break;
  992. case 0x76d: case 0x76f: case 0x772: case 0x774: case 0x776: case 0x778: case 0x77a: case 0x77c:
  993. if (receivedata[i].ID == 0x76d || receivedata[i].ID == 0x772 || receivedata[i].ID == 0x776 || receivedata[i].ID == 0x77a)
  994. {
  995. left_tmp_b = impl_0x76d(receivedata[i]);
  996. memcpy(&(ServiceCarStatus.nextLaneB_left[left_lane_b_count]), &left_tmp_b, sizeof(next_lane_b));
  997. left_lane_b_count++;
  998. }
  999. else
  1000. {
  1001. right_tmp_b = impl_0x76d(receivedata[i]);
  1002. memcpy(&(ServiceCarStatus.nextLaneB_right[right_lane_b_count]), &right_tmp_b, sizeof(next_lane_b));
  1003. right_lane_b_count++;
  1004. }
  1005. break;
  1006. case 0x76a:
  1007. impl_0x76a(receivedata[i]);
  1008. break;
  1009. case 0x76b:
  1010. impl_0x76b(receivedata[i]);
  1011. break;
  1012. default:
  1013. break;
  1014. }
  1015. }
  1016. // obs_print(ServiceCarStatus.obstacleStatusA, ServiceCarStatus.obstacleStatusB, ServiceCarStatus.obstacleStatusC);
  1017. }
  1018. }
  1019. #endif
  1020. //void iv::control::CanUtil::send() {
  1021. // VCI_CAN_OBJ senddata;
  1022. // QTime time;
  1023. // int nTimeLastSend1;
  1024. // time.start();
  1025. // nTimeLastSend1 = time.elapsed();
  1026. // int nSend1Int = 10; //10ms
  1027. // senddata.SendType = m_sendtype;
  1028. // senddata.ExternFlag = m_frameflag;
  1029. // senddata.RemoteFlag = m_frameformat;
  1030. // ServiceControlStatus.control_cmd_.Head = 0xEB;
  1031. // while (true) {
  1032. // usleep(1000);
  1033. // if ((time.elapsed() - nTimeLastSend1) >= nSend1Int)
  1034. // {
  1035. // nTimeLastSend1 = time.elapsed();
  1036. // senddata.ID = ServiceControlStatus.id;
  1037. // senddata.DataLen = 8;
  1038. // ServiceControlStatus.control_cmd_.CommunicationCount++;
  1039. // ServiceControlStatus.control_cmd_.CheckSum = (ServiceControlStatus.control_cmd_.Acceleration_frac + ServiceControlStatus.control_cmd_.Acceleration_int + ServiceControlStatus.control_cmd_.CommunicationCount + ServiceControlStatus.control_cmd_.Control + ServiceControlStatus.control_cmd_.Wheel_Angel[0] + ServiceControlStatus.control_cmd_.Wheel_Angel[1])%256;
  1040. // memcpy(senddata.Data, &ServiceControlStatus.control_cmd_.Head, 8);
  1041. // VCI_Transmit(m_devtype, m_devind, m_cannum, &senddata, 1);
  1042. // ServiceControlStatus.control_cmd_.Control &= 0xCB;
  1043. // }
  1044. // }
  1045. //}
  1046. void iv::control::CanUtil::startsend(bool start_or_stop)
  1047. {
  1048. if(start_or_stop == true && send_connect == true && is_repeat == false)
  1049. {
  1050. is_repeat = true;
  1051. thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this))); //启动发送线程
  1052. }
  1053. else if(start_or_stop == false && send_connect == true && is_repeat == true)
  1054. {
  1055. thread_send->interrupt();
  1056. thread_send->join();
  1057. is_repeat = false;
  1058. }
  1059. }
  1060. bool iv::control::CanUtil::did_radar_ok()
  1061. {
  1062. bool temp = obs_find_flag;
  1063. obs_find_flag = false;
  1064. return temp;
  1065. }