123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266 |
- #include <control/can.h>
- #include <control/control_status.h>
- #include <common/constants.h>
- #include <common/logout.h>
- #include <perception/sensor_radar.h>
- #include <QTime>
- #define VCI_USBCAN1 3
- #define VCI_USBCAN2 4
- #define VCI_USBCAN2A 4
- #define VCI_USBCAN_E_U 20
- #define VCI_USBCAN_2E_U 21
- #define VCI_ACCCODE_DEFAULT 0x00000000
- #define VCI_ACCMASK_DEFAULT 0xFFFFFFFF
- #define VCI_TIMER0_DEFAULT 0x00
- #define VCI_TIMER1_DEFAULT 0x1C
- #define VCI_FILTER_DEFAULT 1
- #define VCI_MODE_DEFAULT 0
- //函数调用返回状态值
- #define STATUS_OK 1
- #define STATUS_ERR 0
- #define LOG_ENABLE
- #ifndef M_PI
- #define M_PI (3.1415926535897932384626433832795)
- #endif
- iv::control::CanUtil::CanUtil() {
- obs_radar1 = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- obs_radar_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- obs_radar_ui = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
- #ifdef WIN32
- m_hDLL = LoadLibrary(TEXT("ControlCAN.dll"));
- VCI_OpenDevice = (LPVCI_OpenDevice)GetProcAddress(m_hDLL, "VCI_OpenDevice");//取得函数地址
- VCI_CloseDevice = (LPVCI_CloseDevice)GetProcAddress(m_hDLL, "VCI_CloseDevice");
- VCI_InitCAN = (LPVCI_InitCan)GetProcAddress(m_hDLL, "VCI_InitCAN");
- VCI_ReadBoardInfo = (LPVCI_ReadBoardInfo)GetProcAddress(m_hDLL, "VCI_ReadBoardInfo");
- VCI_ReadErrInfo = (LPVCI_ReadErrInfo)GetProcAddress(m_hDLL, "VCI_ReadErrInfo");
- VCI_ReadCanStatus = (LPVCI_ReadCanStatus)GetProcAddress(m_hDLL, "VCI_ReadCANStatus");
- VCI_GetReference = (LPVCI_GetReference)GetProcAddress(m_hDLL, "VCI_GetReference");
- VCI_SetReference = (LPVCI_SetReference)GetProcAddress(m_hDLL, "VCI_SetReference");
- VCI_GetReceiveNum = (LPVCI_GetReceiveNum)GetProcAddress(m_hDLL, "VCI_GetReceiveNum");
- VCI_ClearBuffer = (LPVCI_ClearBuffer)GetProcAddress(m_hDLL, "VCI_ClearBuffer");
- VCI_StartCAN = (LPVCI_StartCAN)GetProcAddress(m_hDLL, "VCI_StartCAN");
- VCI_ResetCAN = (LPVCI_ResetCAN)GetProcAddress(m_hDLL, "VCI_ResetCAN");
- VCI_Transmit = (LPVCI_Transmit)GetProcAddress(m_hDLL, "VCI_Transmit");
- VCI_Receive = (LPVCI_Receive)GetProcAddress(m_hDLL, "VCI_Receive");
- VCI_GetReference2 = (LPVCI_GetReference2)GetProcAddress(m_hDLL, "VCI_GetReference2");
- VCI_SetReference2 = (LPVCI_SetReference2)GetProcAddress(m_hDLL, "VCI_SetReference2");
- VCI_ResumeConfig = (LPVCI_ResumeConfig)GetProcAddress(m_hDLL, "VCI_ResumeConfig");
- VCI_ConnectDevice = (LPVCI_ConnectDevice)GetProcAddress(m_hDLL, "VCI_ConnectDevice");
- VCI_UsbDeviceReset = (LPVCI_UsbDeviceReset)GetProcAddress(m_hDLL, "VCI_UsbDeviceReset");
- #endif
- }
- iv::control::CanUtil::~CanUtil() {
- }
- void iv::control::CanUtil::openCanCard() {
- VCI_INIT_CONFIG initconfig;
- initconfig.AccCode = VCI_ACCCODE_DEFAULT;
- initconfig.AccMask = VCI_ACCMASK_DEFAULT;
- initconfig.Timing0 = VCI_TIMER0_DEFAULT;
- initconfig.Timing1 = VCI_TIMER1_DEFAULT;
- initconfig.Filter = VCI_FILTER_DEFAULT;
- initconfig.Mode = VCI_MODE_DEFAULT;
- int status = VCI_OpenDevice(m_devtype, m_devind, 0); //打开设备
- m_connect = -1;
- if (status == 1)
- {
- int init = VCI_InitCAN(m_devtype, m_devind, m_cannum, &initconfig); //初始化设备
- if (init == 1)
- {
- if (VCI_StartCAN(m_devtype, m_devind, m_cannum) == 1) {
- send_connect = true;
- m_connect = 1;
- thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this)));
- thread_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive, this)));
- }
- else {
- throw 0;
- }
- }
- else
- {
- throw 1;
- }
- // 打开2通道,供毫米波雷达读取数据用
- init = VCI_InitCAN(m_devtype, m_devind, m_cannum_radar, &initconfig); //初始化设备
- if (init == 1)
- {
- if (VCI_StartCAN(m_devtype, m_devind, m_cannum_radar) == 1) {
- m_connect = 1;
- thread_radar_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_radar, this))); //启动毫米波雷达接收线程5
- }
- else {
- throw 2;
- }
-
- }
- else
- {
- throw 3;
- }
- }
- else if(status == 0) {
- throw 4;
- }
- else if (status == -1) {
- throw 5;
- }
- #ifdef USE_MOBILEYE
- status = VCI_OpenDevice(m_devtype, m_devindmob, 0); //打开设备
- if (status == 1)
- {
- int init = VCI_InitCAN(m_devtype, m_devindmob, m_cannum, &initconfig); //初始化设备
- if (init == 1)
- {
- if (VCI_StartCAN(m_devtype, m_devindmob, m_cannum) == 1) {
- m_connect = 1;
- thread_mobileye_receive = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::receive_mobileye, this)));
- }
- else {
- throw 0;
- }
- }
- else
- {
- throw 1;
- }
- }
- else if(status == 0) {
- throw 4;
- }
- else if (status == -1) {
- throw 5;
- }
- #endif
- }
- void iv::control::CanUtil::closeCanCard() {
- if(m_connect != 0)
- {
- thread_receive->interrupt();
- thread_receive->join();
- thread_send->interrupt();
- thread_send->join();
- thread_receive->interrupt();
- thread_receive->join();
- unsigned int rec = VCI_CloseDevice(m_devtype, m_devind);
- #ifdef USE_MOBILEYE
- rec = VCI_CloseDevice(m_devtype, m_devindmob);
- #endif
- std::cout << "close can:" << rec << std::endl;
- }
- }
- void iv::control::CanUtil::receive() {
- while (m_connect)
- {
- VCI_CAN_OBJ receivedata[2500];
- int res = VCI_Receive(m_devtype, m_devind, m_cannum, receivedata, 2500, 10);
- if (res > 0) //未能从can卡中读到数
- {
- // std::cout<<"reee"<<std::endl;
- int i;
- for(i=0;i<res;i++)
- {
- if(receivedata[i].ID == 0x282)
- {
- int i;
- unsigned int xvalue[8];
- float fvalue[8];
- unsigned int x;
- for(i=0;i<8;i++)
- {
- memcpy(&x,&receivedata[i].Data[0],4);
- x = (x<<((7-i)*4))>>28;
- xvalue[i] = x;
- fvalue[i] = x;
- if(x == 0)fvalue[i] = -1;
- else fvalue[i] = (fvalue[i] - 1.0)*0.2;
- }
- ServiceCarStatus.multrasonic_obs.mfront_leftmid = fvalue[0];
- ServiceCarStatus.multrasonic_obs.mfront_left = fvalue[1];
- ServiceCarStatus.multrasonic_obs.mfront_right = fvalue[2];
- ServiceCarStatus.multrasonic_obs.mfront_rightmid = fvalue[3];
- ServiceCarStatus.multrasonic_obs.mrear_leftmid = fvalue[4];
- ServiceCarStatus.multrasonic_obs.mrear_left = fvalue[5];
- ServiceCarStatus.multrasonic_obs.mrear_right = fvalue[6];
- ServiceCarStatus.multrasonic_obs.mrear_rightmid = fvalue[7];
- // std::cout<<"rec oooo value is back "<<(int)(receivedata[i].Data[3])<<" head "<<(int)receivedata[i].Data[1]<<std::endl;
- }
- }
- }
- else
- {
- usleep(1000);
- }
- }
- }
- void iv::control::CanUtil::initial()
- {
- ServiceControlStatus.command.byte[0] = 0xEB;
- ServiceControlStatus.command.byte[1] = 0;
- ServiceControlStatus.command.byte[2] = 0;
- ServiceControlStatus.command.byte[3] = 0;
- ServiceControlStatus.command.byte[4] = 0;
- ServiceControlStatus.command.byte[5] = 0;
- ServiceControlStatus.command.byte[6] = 0;
- ServiceControlStatus.command.byte[7] = 0;
- }
- void iv::control::CanUtil::pack_command()
- {
- ServiceControlStatus.command.bit.heartbreak++;
- if (ServiceControlStatus.command.bit.heartbreak >= 256)
- ServiceControlStatus.command.bit.heartbreak = 0;
- ServiceControlStatus.command.byte[7] = (ServiceControlStatus.command.byte[1] + ServiceControlStatus.command.byte[2]
- + ServiceControlStatus.command.byte[3] + ServiceControlStatus.command.byte[4]
- + ServiceControlStatus.command.byte[5] + ServiceControlStatus.command.byte[6]) % 256;
- }
- void iv::control::CanUtil::send()
- {
- VCI_CAN_OBJ Command_data;
- QTime time1;
- int LastTimeSend1;
- time1.start();
- LastTimeSend1 = time1.elapsed();
- int Send1Int = 10; //100ms
- Command_data.SendType = m_sendtype;
- Command_data.ExternFlag = m_frameflag;
- Command_data.RemoteFlag = m_frameformat;
- Command_data.ID = ServiceControlStatus.command_ID;
- Command_data.DataLen = 8;
- initial();
- while (true)
- {
- int print_time = time1.elapsed() - LastTimeSend1;
- if (print_time >= Send1Int)
- {
- LastTimeSend1 = time1.elapsed();
- pack_command();
- memcpy(Command_data.Data, ServiceControlStatus.command.byte, 8);
- VCI_Transmit(m_devtype, m_devind, m_cannum, &Command_data, 1);
- /*for (int c = 0; c < 8; c++)
- ODS("%x ", Command_data.Data[c]);
- ODS("\n");*/
- //ODS(" %d \n", print_time);
- }
- }
- }
- /******************************************************mobileye AWS display*****************************************************
- * message ID:0x700
- * member: type physical unit range note
- * dusk_time: double / 0,1
- * night_time double / 0,1
- *******************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x700(VCI_CAN_OBJ receivedata)
- {
- ServiceCarStatus.aws_display.dusk_time = (receivedata.Data[0] >> 3) & 0x1;
- ServiceCarStatus.aws_display.night_time = (receivedata.Data[0] >> 4) & 0x1;
- ServiceCarStatus.aws_display.lanes_on = receivedata.Data[4] & 0x1;
- //std::cout << "lanes_on:" << ServiceCarStatus.aws_display.lanes_on << std::endl;
- }
- /******************************************************mobileye lane***********************************************************
- * message ID:0x737
- * member: type physical unit range note
- * curvature: double 1/meter [-0.12, 0.12] "a" in the equation:y = ax^2+bx+c
- * heading: double / [-1.0, 1.0] "b" in the equation:y = ax^2+bx+c
- * construction area: bool / / /
- * pitch angle: double radians [-0.05, 0.05] pitch of the host vehicle(derived from lanes analysis)
- * yaw angle: double radians / yaw of the host vehicle(derived from lanes analysis)
- * right_LDW: bit/bool / 0,1 0 stands for unavailable, 1 for available
- * left_LDW: bit/bool / 0,1 0 stands for unavailable, 1 for available
- *******************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x737(VCI_CAN_OBJ receivedata)
- {
- //lane curvature
- int32_t tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- tmp <<= 16;
- tmp >>= 16;
- ServiceCarStatus.Lane.curvature = tmp * 3.81 * 1e-6;
- //std::cout<<"curvature:"<<ServiceCarStatus.Lane.curvature<<std::endl;
- tmp = 0;
- //lane heading
- tmp = ((receivedata.Data[3] & 0xf) << 8) | receivedata.Data[2];
- tmp <<= 20;
- tmp >>= 20;
- ServiceCarStatus.Lane.heading = tmp * 0.0005;
- tmp = 0;
- //construction area
- ServiceCarStatus.Lane.is_ca = (receivedata.Data[3] >> 4) & 0x1;
- //left_LDW
- ServiceCarStatus.Lane.is_left_LDW_available = (receivedata.Data[3] >> 5) & 0x1;
- //right_LDW
- ServiceCarStatus.Lane.is_right_LDW_available = (receivedata.Data[3] >> 6) & 0x1;
- //yaw
- tmp = (receivedata.Data[5] >> 8) | receivedata.Data[4];
- tmp <<= 16;
- tmp >>= 16;
- //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0;
- ServiceCarStatus.Lane.yaw = tmp * 0.000977;
- std::cout << "yaw:"<< ServiceCarStatus.Lane.yaw<< std::endl;
- tmp = 0;
- //pitch
- tmp = (receivedata.Data[7] >> 8) | receivedata.Data[6];
- tmp <<= 16;
- tmp >>= 16;
- //ServiceCarStatus.Lane.yaw = (tmp - 0x7fff) / 1024.0 / 512;
- ServiceCarStatus.Lane.yaw = tmp * 0.000002;
- std::cout << "pitch:" << ServiceCarStatus.Lane.pitch << std::endl;
- }
- /**************************************************** mobileye obstacle status ****************************************************************************
- * message ID:0x738
- * member: type physical unit range note
- * num_obstacles int / [0:255] /
- * timestamp int ms [0:255] only the lowest 8 bits of the timestamp is given
- * app_version: int / [0:255] vesion info consists of X.Y.Z.W
- * active_version int/2bits / [0:3] index of the active section of app_version signal
- * left_close_rang_cut_in bool / 0,1 0 false, 1 true
- * right_close_rang_cut_in bool / 0,1 0 false, 1 true
- * go enum / 0,1...15 0 stop, 1 go, 2 undecided, 3 driver decisions is required, 4-14 unused, 15 not calculated
- * protocol version uchar/8bit / 0x00...0xff /
- * is_close_car bool / 0,1 0 no close, 1 close car exists
- * failsafe int/4bit / [0:7] failsafe situation, 0000 no failsafe, 0001 low sun, 0010 blur image, 0100 1000 unused
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x738(VCI_CAN_OBJ receivedata)
- {
- //num_obstacles
- ServiceCarStatus.obstacleStatus.num_obstacles = receivedata.Data[0];
- //std::cout << ServiceCarStatus.obstacleStatus.num_obstacles<< std::endl;
- //timestamp
- ServiceCarStatus.obstacleStatus.timestamp = receivedata.Data[1];
- //app version
- ServiceCarStatus.obstacleStatus.app_version = receivedata.Data[2];
- //active version
- ServiceCarStatus.obstacleStatus.active_version = receivedata.Data[3] & 0x3;
- //is left close rang cut in
- ServiceCarStatus.obstacleStatus.is_left_close_rang_cut_in = (receivedata.Data[3] >> 2) & 0x1;
- //is right close rang cut in
- ServiceCarStatus.obstacleStatus.is_right_close_rang_cut_in = (receivedata.Data[3] >> 3) & 0x1;
- //go
- ServiceCarStatus.obstacleStatus.go = receivedata.Data[3] >> 4;
- //protocol version
- ServiceCarStatus.obstacleStatus.protocol_version = receivedata.Data[4];
- //close car
- ServiceCarStatus.obstacleStatus.is_close_car = receivedata.Data[5] & 0x1;
- //failsafe
- ServiceCarStatus.obstacleStatus.failsafe = (receivedata.Data[5] >> 1) & 0xf;
- }
- /**************************************************** mobileye obstacle data a ****************************************************************************
- * message ID:0x739
- * member: type physical unit range note
- * obstacle_ID int / [0:63] new obstacles are given the last used free ID
- * obstacle_pos_x double m [0,250] longtitude position of the obstacle relative to the ref point
- * obstacle_pos_y double m [-31.93,31.93] lateral position of the obstacle, error relative to pos_x below 10% or 2meters
- * blinker_info int/enum / [0:4] 0 unavailable, 1 off, 2 left, 3 right, 4 both indicated blinkers status
- * cut_in_and_out int / [0:4] 0 undefined, 1 in_host_lane, 2 out_host_lane, 3 cut_in, 4 cut_out
- * cut_in: target entering the host lane, cut_out: exiting but not distinguish between sides
- * obstacle_rel_vel_x double m/s [-127.93,127.93] relative longtitude velocity of the obstacle
- * obstacle_status int/enum / [0:6] 0 undefined, 1 standing(never moved, back lights on), 2 stopped(moveable), 3 moving
- * 4 oncoming, 5 parked(never moved, back lights off), 6 unused
- * is_obstacle_brake_lights bool / 0,1 0 object's brake light off or not identified, 1 object's brake light on
- * obstacle_valid int/enum / [1:2] 1 new valid(detected this frame), 2 older valid
- *************************************************************************************************************************************************************/
- obstacle_data_a iv::control::CanUtil::impl_0x739(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- obstacle_data_a obs_tmp;
- //obstacle id
- obs_tmp.obstacle_ID = receivedata.Data[0];
- //ServiceCarStatus.obstacleStatusA.obstacle_ID = receivedata.Data[0];
- //obstacle pos x
- tmp = ((receivedata.Data[2] & 0xf) << 8) | receivedata.Data[1];
- obs_tmp.obstacle_pos_x = tmp * 0.0625;
- //ServiceCarStatus.obstacleStatusA.obstacle_pos_x = tmp * 0.0625;
- tmp = 0;
- //obstacle pos y
- tmp = ((receivedata.Data[4] & 0x3) << 8) | receivedata.Data[3];
- obs_tmp.obstacle_pos_y = tmp * 0.0625;
- //ServiceCarStatus.obstacleStatusA.obstacle_pos_y = tmp * 0.0625;
- tmp = 0;
- //blinker info
- obs_tmp.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
- //ServiceCarStatus.obstacleStatusA.blinker_info = (receivedata.Data[4] >> 2) & 0x7;
- //cut in and out
- obs_tmp.cut_in_and_out = receivedata.Data[4] >> 5;
- //ServiceCarStatus.obstacleStatusA.cut_in_and_out = receivedata.Data[4] >> 5;
- //obstacle rel vel x
- tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
- obs_tmp.obstacle_rel_vel_x = tmp * 0.0625;
- //ServiceCarStatus.obstacleStatusA.obstacle_rel_vel_x = tmp * 0.0625;
- tmp = 0;
- //obstacle type
- obs_tmp.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
- //ServiceCarStatus.obstacleStatusA.obstacle_type = (receivedata.Data[6] >> 4) & 0x7;
- //obstacle status
- obs_tmp.obstacle_status = receivedata.Data[7] & 0x7;
- //ServiceCarStatus.obstacleStatusA.obstacle_status = receivedata.Data[7] & 0x7;
- //obstacle brake lights
- obs_tmp.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
- //ServiceCarStatus.obstacleStatusA.is_obstacle_brake_lights = (receivedata.Data[7] >> 3) & 0x1;
- //obstacle valid
- obs_tmp.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
- //ServiceCarStatus.obstacleStatusA.obstacle_valid = (receivedata.Data[7] >> 6) & 0x7;
- return obs_tmp;
- }
- /**************************************************** mobileye obstacle data b ****************************************************************************
- * message ID:0x73a
- * member: type physical unit range note
- * obstacle_length double m [0,31] length of the obstacle(longitude axis)
- * obstacle_width double m [0,12.5] width of the obstacle(lateral axis)
- * obstacle_age int / [0:255] value starts at 1 when it is first detected, increaments in i each frame
- * obstacle_lane int/enum / [0:3] 0 not assigned, 1 ego lane, 2 next lane(left or right) or next next lane,
- * 3 invalid signal
- * is_cipv_flag int/enum / [0:1] 0 not cipv, 1 cipv(closest in path vehicle)
- * radar_pos_x double m [0,250] longtitude postion of the primary radar target matched to the vision target, dist=relative to ref point
- * if no radar target is matched, value = 0xfff
- * 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
- * 4 oncoming, 5 parked(never moved, back lights off), 6 unused
- * 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
- * vision and radar measurements higher->smaller error, 5 high confidence match
- * matched_radar_id int / [0:127] ID of Primary radar target matched to the vision target if applicable
- *************************************************************************************************************************************************************/
- obstacle_data_b iv::control::CanUtil::impl_0x73a(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- obstacle_data_b obs_tmp;
- //obstacle length
- tmp = receivedata.Data[0];
- obs_tmp.obstacle_length = tmp * 0.5;
- //ServiceCarStatus.obstacleStatusB.obstacle_length = tmp * 0.5;
- tmp = 0;
- //obstacle width
- tmp = receivedata.Data[1];
- obs_tmp.obstacle_width = tmp * 0.05;
- //ServiceCarStatus.obstacleStatusB.obstacle_width = tmp * 0.05;
- tmp = 0;
- //obstacle age
- obs_tmp.obstacle_age = receivedata.Data[2];
- //ServiceCarStatus.obstacleStatusB.obstacle_age = receivedata.Data[2];
- //obstacle lane
- obs_tmp.obstacle_lane = receivedata.Data[3] & 0x3;
- //ServiceCarStatus.obstacleStatusB.obstacle_lane = receivedata.Data[3] & 0x3;
- //is cipv flag
- obs_tmp.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
- //ServiceCarStatus.obstacleStatusB.is_cipv_flag = (receivedata.Data[3] >> 2) & 0x1;
- //radar pos x
- tmp = (receivedata.Data[4] << 4) | (receivedata.Data[3] >> 4);
- obs_tmp.radar_pos_x = tmp * 0.0625;
- //ServiceCarStatus.obstacleStatusB.radar_pos_x = tmp * 0.0625;
- tmp = 0;
- //radar vel x
- tmp = ((receivedata.Data[6] & 0xf) << 8) | receivedata.Data[5];
- tmp <<= 20;
- tmp >>= 20;
- obs_tmp.radar_vel_x = tmp * 0.0625;
- //ServiceCarStatus.obstacleStatusB.radar_vel_x = tmp * 0.0625;
- tmp = 0;
- //radar match confidence
- obs_tmp.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
- //ServiceCarStatus.obstacleStatusB.radar_match_confidence = (receivedata.Data[6] >> 4) & 0x7;
- //matched radar ID
- obs_tmp.matched_radar_id = receivedata.Data[7] & 0x7f;
- //ServiceCarStatus.obstacleStatusB.matched_radar_id = receivedata.Data[7] & 0x7f;
- return obs_tmp;
- }
- /**************************************************** mobileye obstacle data c ****************************************************************************
- * message ID:0x73b
- * member: type physical unit range note
- * obstacle_angle_rate double degree/s [-327.68, 327.68] Angle rate of Center of Obstacle, negative->moved to left
- * obstcale_scale_change double pix/s [-6.5532, 6.5532] /
- * object_accel_x double m/s2 [-14.97, 14.97] longtitude acceleration of the object
- * obstacle_replaced bool / 0,1 0 not replaced in this frame, 1 replace in this frame
- * obstacle_angle double degree [-327.68,327.68] Angle to Center of Obstacle in degrees, 0 indicates that the obstacle is in
- * exactly in front of us, a positive angle indicates that theobstacle is to the right
- *************************************************************************************************************************************************************/
- obstacle_data_c iv::control::CanUtil::impl_0x73b(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- obstacle_data_c obs_tmp;
- //obstacle angle rate
- tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- tmp <<= 16;
- tmp >>= 16;
- obs_tmp.obstacle_angle_rate = tmp * 0.01;
- //ServiceCarStatus.obstacleStatusC.obstacle_angle_rate = tmp * 0.01;
- tmp = 0;
- //obstacle scale change
- tmp = (receivedata.Data[3] << 8) | receivedata.Data[2];
- tmp <<= 16;
- tmp >>= 16;
- obs_tmp.obstacle_scale_change = tmp * 0.0002;
- //ServiceCarStatus.obstacleStatusC.obstacle_scale_change = tmp * 0.0002;
- tmp = 0;
- //object accel x
- tmp = ((receivedata.Data[5] & 0x3) << 8) | receivedata.Data[4];
- tmp <<= 22;
- tmp >>= 22;
- obs_tmp.object_accel_x = tmp * 0.03;
- //ServiceCarStatus.obstacleStatusC.object_accel_x = tmp * 0.03;
- tmp = 0;
- //obstacle replaced
- obs_tmp.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
- //ServiceCarStatus.obstacleStatusC.is_obstacle_replaced = (receivedata.Data[5] >> 4) & 0x1;
- //obstacle angle
- tmp = (receivedata.Data[7] << 8) | receivedata.Data[6];
- tmp <<= 16;
- tmp >>= 16;
- obs_tmp.obstacle_angle = tmp * 0.01;
- //ServiceCarStatus.obstacleStatusC.obstacle_angle = tmp * 0.01;
- return obs_tmp;
- }
- /**************************************************** mobileye aftermarket lane ****************************************************************************
- * message ID:0x669
- * member: type physical unit range note
- * lane_conf_left int/2bit / [0:3] 0 lowest, 3 highest
- * is_ldw_availablity_left bool / 0,1 lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
- * 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
- * dist_to_lane_l double m [-40:40] "c" in the equation:y = ax^2+bx+c
- * lane_conf_right int/2bit / [0:3] 0 lowest, 3 highest
- * is_ldw_availablity_right bool / 0,1 lane_conf>=2->on, speed>55km/h, configuration of LDW>=1
- * 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
- * dist_to_lane_r double m [-40:40] "c" in the equation:y = ax^2+bx+c
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x669(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp;
- //lane confidence left
- tmp = receivedata.Data[0] & 0x3;
- tmp <<= 30;
- tmp >>= 30;
- ServiceCarStatus.aftermarketLane.lane_conf_left = tmp;
- //std::cout << "left confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_left << std::endl;
- tmp = 0;
- //is LDW availability left
- ServiceCarStatus.aftermarketLane.is_ldw_availablility_left = (receivedata.Data[0] >> 2) & 0x1;
- //lane type left
- tmp = (receivedata.Data[0] >> 4) & 0xf;
- tmp <<= 28;
- tmp >>= 28;
- ServiceCarStatus.aftermarketLane.lane_type_left = tmp;
- std::cout << "lane_type_left: " << ServiceCarStatus.aftermarketLane.lane_type_left << std::endl;
- tmp = 0;
- //dist to left lane
- tmp = (receivedata.Data[2] << 4) | ((receivedata.Data[1] >> 4) & 0xf);
- tmp <<= 20;
- tmp >>= 20;
- ServiceCarStatus.aftermarketLane.dist_to_lane_l = tmp * 0.02;
- std::cout << "dist_to_lane_l:" << ServiceCarStatus.aftermarketLane.dist_to_lane_l << std::endl;
- tmp = 0;
- //lane confidence right
- tmp = receivedata.Data[5] & 0x3;
- tmp <<= 30;
- tmp >>= 30;
- ServiceCarStatus.aftermarketLane.lane_conf_right = tmp;
- //std::cout << "right confidence:" << ServiceCarStatus.aftermarketLane.lane_conf_right << std::endl;
- tmp = 0;
- //is LDW availability right
- ServiceCarStatus.aftermarketLane.is_ldw_availablility_right = (receivedata.Data[5] >> 2) & 0x1;
- //lane type right
- tmp = (receivedata.Data[5] >> 4) & 0xf;
- tmp <<= 28;
- tmp >>= 28;
- ServiceCarStatus.aftermarketLane.lane_type_right = tmp;
- std::cout<<"lane_type_right: " << ServiceCarStatus.aftermarketLane.lane_type_right << std::endl;
- tmp = 0;
- //dist to right lane
- tmp = (receivedata.Data[7] << 4) | ((receivedata.Data[6] >> 4) & 0xf);
- tmp <<= 20;
- tmp >>= 20;
- ServiceCarStatus.aftermarketLane.dist_to_lane_r = tmp * 0.02;
- std::cout << "dist_to_lane_r:" << ServiceCarStatus.aftermarketLane.dist_to_lane_r << std::endl;
- tmp = 0;
- }
- /**************************************************** mobileye LKA left lane a ****************************************************************************
- * message ID:0x766
- * member: type physical unit range note
- * lane_type enum / [0:6] 0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
- * quality enum / [0:3] 0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
- * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
- * position double m [-127,128] physical distance between lane mark and camera on the lateral position
- * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
- * curvature_derivative double / [-0.00012, 0.00012] /
- * width_left_marking double m [0, 2.5] left lane marking width
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x766(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- //lane type
- ServiceCarStatus.LKAleftLaneA.lane_type = receivedata.Data[0] & 0xf;
- //quality
- ServiceCarStatus.LKAleftLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
- //model degree
- ServiceCarStatus.LKAleftLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
- //position
- tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
- tmp <<= 16;
- tmp >>= 16;
- ServiceCarStatus.LKAleftLaneA.position = tmp / 256.0;
- //ServiceCarStatus.LKAleftLaneA.position = tmp * 0.003906;
- tmp = 0;
- //curvature
- tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
- ServiceCarStatus.LKAleftLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
- //ServiceCarStatus.LKAleftLaneA.curvature = tmp * 0.000001 + -0.031999;
- tmp = 0;
- //curvature derivative
- tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
- ServiceCarStatus.LKAleftLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
- //ServiceCarStatus.LKAleftLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
- tmp = 0;
- //width left marking
- tmp = receivedata.Data[7];
- ServiceCarStatus.LKAleftLaneA.width_left_marking = tmp * 0.01;
- }
- /**************************************************** mobileye LKA left lane b ****************************************************************************
- * message ID:0x767
- * member: type physical unit range note
- * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
- * view_range double m [0, 127.996] physical view range of lane mark
- * view_range_availability enum / 0,1 0 not valid, 1 valid
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x767(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- //heading angle
- tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- ServiceCarStatus.LKAleftLaneB.heading_angle = (tmp - 0x7fff) / 1024;
- //ServiceCarStatus.LKAleftLaneB.heading_angle = tmp * 0.000977 + -31.999023;
- tmp = 0;
- //view range
- tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
- ServiceCarStatus.LKAleftLaneB.view_range = tmp / 256.0;
- //ServiceCarStatus.LKAleftLaneB.view_range = tmp * 0.003906;
- tmp = 0;
- //view range availability
- ServiceCarStatus.LKAleftLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
- }
- /**************************************************** mobileye LKA right lane a ****************************************************************************
- * message ID:0x768
- * member: type physical unit range note
- * lane_type enum / [0:6] 0 dashed, 1 solid, 2 undecided, 3 road edge, 4 double lane mark, 5 bott's dots, 6 invalid
- * quality enum / [0:3] 0,1 low quality:lane measurements not valid and LDW not given, 2,3 high quality
- * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
- * position double m [-127,128] physical distance between lane mark and camera on the lateral position
- * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
- * curvature_derivative double / [-0.00012, 0.00012] /
- * width_right_marking double m [0, 2.5] right lane marking width
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x768(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- //lane type
- ServiceCarStatus.LKArightLaneA.lane_type = receivedata.Data[0] & 0xf;
- //quality
- ServiceCarStatus.LKArightLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
- //model degree
- ServiceCarStatus.LKArightLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
- //position
- tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
- tmp <<= 16;
- tmp >>= 16;
- ServiceCarStatus.LKArightLaneA.position = tmp / 256.0;
- //ServiceCarStatus.LKArightLaneA.position = tmp * 0.003906;
- tmp = 0;
- //curvature
- tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
- ServiceCarStatus.LKArightLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
- //ServiceCarStatus.LKArightLaneA.curvature = tmp * 0.000001 + -0.031999;
- tmp = 0;
- //curvature derivative
- tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
- ServiceCarStatus.LKArightLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
- //ServiceCarStatus.LKArightLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
- tmp = 0;
- //width left marking
- tmp = receivedata.Data[7];
- ServiceCarStatus.LKArightLaneA.width_left_marking = tmp * 0.01;
- }
- /**************************************************** mobileye LKA right lane b ****************************************************************************
- * message ID:0x767
- * member: type physical unit range note
- * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
- * view_range double m [0, 127.996] physical view range of lane mark
- * view_range_availability enum / 0,1 0 not valid, 1 valid
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x769(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- //heading angle
- tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- ServiceCarStatus.LKArightLaneB.heading_angle = (tmp - 0x7fff) / 1024;
- //ServiceCarStatus.LKArightLaneB.heading_angle = tmp * 0.000977 + -31.999023;
- tmp = 0;
- //view range
- tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
- ServiceCarStatus.LKArightLaneB.view_range = tmp / 256.0;
- //ServiceCarStatus.LKArightLaneB.view_range = tmp * 0.003906;
- tmp = 0;
- //view range availability
- ServiceCarStatus.LKArightLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
- }
- /**************************************************** mobileye next lane a **********************************************************************************
- * message ID:0x76c
- * member: type physical unit range note
- * lane_type enum / [1:2] 1 solid, 2 undecided
- * quality / / / not yet implemented for next lanes
- * model_degree enum / [1:3] 1 linear model, 2 parabolic model, 3 3rd-degree model
- * position double m [-127,128] physical distance between lane mark and camera on the lateral position
- * curvature double / [-0.02, 0.02] given a very low curvature derivative, positive curvature indicates a right hand side curve
- * curvature_derivative double / [-0.00012, 0.00012] /
- * lane_mark_widt double m [0, 2.5] lane marking width
- *************************************************************************************************************************************************************/
- next_lane_a iv::control::CanUtil::impl_0x76c(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- next_lane_a nextLane_tmp;
- //lane type
- nextLane_tmp.lane_type = receivedata.Data[0] & 0xf;
- //ServiceCarStatus.nextLaneA.lane_type = receivedata.Data[0] & 0xf;
- //quality
- nextLane_tmp.quality = (receivedata.Data[0] >> 4) & 0x3;
- //ServiceCarStatus.nextLaneA.quality = (receivedata.Data[0] >> 4) & 0x3;
- //model degree
- nextLane_tmp.model_degree = (receivedata.Data[0] >> 6) & 0x3;
- //ServiceCarStatus.nextLaneA.model_degree = (receivedata.Data[0] >> 6) & 0x3;
- //position
- tmp = (receivedata.Data[2] << 8) | receivedata.Data[1];
- tmp <<= 16;
- tmp >>= 16;
- nextLane_tmp.position = tmp * 0.003906;
- //ServiceCarStatus.nextLaneA.position = tmp / 256.0;
- //ServiceCarStatus.nextLaneA.position = tmp * 0.003906;
- tmp = 0;
- //curvature
- tmp = (receivedata.Data[4] << 8) | receivedata.Data[3];
- nextLane_tmp.curvature = tmp * 0.000001 + -0.031999;
- //ServiceCarStatus.nextLaneA.curvature = (tmp - 0x7fff) / 1024.0 / 1000;
- //ServiceCarStatus.nextLaneA.curvature = tmp * 0.000001 + -0.031999;
- tmp = 0;
- //curvature derivative
- tmp = (receivedata.Data[6] << 8) | receivedata.Data[5];
- nextLane_tmp.curvature_derivative = tmp * 0.000000 + -0.000122;
- //ServiceCarStatus.nextLaneA.curvature_derivative = (double)(tmp - 0x7fff) / (1 << 28);
- //ServiceCarStatus.nextLaneA.curvature_derivative = tmp * 0.000000 + -0.000122;
- tmp = 0;
- //width left marking
- tmp = receivedata.Data[7];
- nextLane_tmp.lane_mark_width = tmp * 0.01;
- //ServiceCarStatus.nextLaneA.lane_mark_width = tmp * 0.01;
- return nextLane_tmp;
- }
- /**************************************************** mobileye LKA right lane b ****************************************************************************
- * message ID:0x76d
- * member: type physical unit range note
- * heading_angle double radians [-0.357, 0.357] physical slope of the lane mark positive->steering towards the right
- * view_range double m [0, 127.996] physical view range of lane mark
- * view_range_availability enum / 0,1 0 not valid, 1 valid
- *************************************************************************************************************************************************************/
- next_lane_b iv::control::CanUtil::impl_0x76d(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- next_lane_b nextLane_tmp;
- //heading angle
- tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- nextLane_tmp.heading_angle = tmp * 0.000977 + -31.999023;
- //ServiceCarStatus.nextLaneB.heading_angle = (tmp - 0x7fff) / 1024;
- //ServiceCarStatus.nextLaneB.heading_angle = tmp * 0.000977 + -31.999023;
- tmp = 0;
- //view range
- tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
- nextLane_tmp.view_range = tmp * 0.003906;
- //ServiceCarStatus.nextLaneB.view_range = tmp / 256.0;
- //ServiceCarStatus.nextLaneB.view_range = tmp * 0.003906;
- tmp = 0;
- //view range availability
- nextLane_tmp.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
- //ServiceCarStatus.nextLaneB.is_view_range_availability = (receivedata.Data[3] >> 7) & 0x1;
- return nextLane_tmp;
- }
- /**************************************************** mobileye reference points ******************************************************************************
- * message ID:0x76a
- * member: type physical unit range note
- * ref_point_1_position double m [-127.996, 127.996] physical distance between camera and reference point 1 on lateral axis.
- * The reference point defines the lateral location of the lane center at ref-Point distance.
- * ref Point 1 is this measurement at 1 second headway.
- * ref_point_1_dist double m [0, 127.99609376] physical distance between reference point and camera
- * ref_point_1_validity enum / 0,1 0 invalid, 1 valid
- * ref_point_2_position double m [-127.996, 127.996] empty! physical distance between camera and reference point 2 on lateral axis.
- * The reference point defines the lateral location of the lane center at ref-Point distance.
- * ref Point 2 is this measurement at 1 second headway.
- * ref_point_2_dist double m [0, 127.99609376] empty! physical distance between reference point and camera
- * ref_point_2_validity enum / 0,1 empty! 0 invalid, 1 valid
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x76a(VCI_CAN_OBJ receivedata)
- {
- int32_t tmp = 0;
- //ref_point_1_position
- //tmp = (receivedata.Data[1] << 8) | receivedata.Data[0];
- ServiceCarStatus.refPoints.ref_point_1_position = (tmp - 0x7fff) / 256.0;
- ServiceCarStatus.refPoints.ref_point_1_position = tmp * 0.003906 + -127.996094;
- tmp = 0;
- //ref_point_1_dist
- tmp = ((receivedata.Data[3] & 0x7f) << 8) | receivedata.Data[2];
- //ServiceCarStatus.refPoints.ref_point_1_dist = tmp / 256.0;
- ServiceCarStatus.refPoints.ref_point_1_dist = tmp * 0.003906;
- tmp = 0;
- //is ref point 1 validity
- ServiceCarStatus.refPoints.is_ref_point_1_validity = (receivedata.Data[3] >> 7) & 0x1;
- //ref_point_2_position
- tmp = (receivedata.Data[5] << 8) | receivedata.Data[4];
- //ServiceCarStatus.refPoints.ref_point_2_position = (tmp - 0x7fff) / 256.0;
- ServiceCarStatus.refPoints.ref_point_2_position = tmp * 0.003906 + -127.996094;
- tmp = 0;
- //ref_point_2_dist
- tmp = ((receivedata.Data[7] & 0x7f) << 8) | receivedata.Data[6];
- //ServiceCarStatus.refPoints.ref_point_2_dist = tmp / 256.0;
- ServiceCarStatus.refPoints.ref_point_2_dist = tmp * 0.003906;
- tmp = 0;
- //is ref point 2 validity
- ServiceCarStatus.refPoints.is_ref_point_2_validity = (receivedata.Data[7] >> 7) & 0x1;
- }
- /********************************************** mobileye number of next lane markers reported **************************************************************
- * message ID:0x76b
- * member: type physical unit range note
- * num_of_next_lane_mark_reported enum / 1,2 indicates how many extra lane markers are also reported, on top of left
- * and right lane. Currently two lane marks are always reported one for left and one for
- * right, and they are valid only if the lane type is Solid.
- *************************************************************************************************************************************************************/
- void iv::control::CanUtil::impl_0x76b(VCI_CAN_OBJ receivedata)
- {
- //num_of_next_lane_mark_reported
- ServiceCarStatus.numOfNextLaneMarkReported.num_of_next_lane_mark_reported = receivedata.Data[0];
- }
- void iv::control::CanUtil::receive_radar() {
- int32_t range, rate, angle;
- while (m_connect)
- {
- VCI_CAN_OBJ receivedata[2500];
- int res = VCI_Receive(m_devtype, m_devind, m_cannum_radar, receivedata, 2500, 10);
- int radar_ID_index = -1;
- if (res <= 0) //未能从can卡中读到数
- {
- #ifdef linux
- usleep(1000);
- #endif
- #ifdef WIN32
- Sleep(1);
- #endif
- continue;
- }
- for (int i = 0; i < res; i++) {
- //对应毫米波雷达数据解析
- radar_ID_index = receivedata[i].ID - 0x500;
- if (radar_ID_index >= 0x00 && radar_ID_index <= 0x3f) {
- ServiceCarStatus.mRadarS = 10;
- angle = ((receivedata[i].Data[1] & 0x1F) << 5) + ((receivedata[i].Data[2] & 0xF8) / 8);
- range = ((receivedata[i].Data[2] & 0x07) << 8) + receivedata[i].Data[3];
- rate = ((receivedata[i].Data[6] & 0x3F) << 8) | receivedata[i].Data[7];
- if (angle & 0x200) {
- angle = angle | 0xFFFFFC00;
- }
- if (rate & 0x2000) {
- rate = rate | 0xFFFFC000;
- }
- //If angle and range both are 0, it is an invalid data.
- if (angle != 0 || range != 0) {
- obs_find_flag = true;
- ServiceCarStatus.obs_radar[radar_ID_index].valid = true;
- ServiceCarStatus.obs_radar[radar_ID_index].nomal_x = range * 0.1 * sin(angle * 1.0 / 1800.0 * M_PI);
- ServiceCarStatus.obs_radar[radar_ID_index].nomal_y = range * 0.1 * cos(angle * 1.0 / 1800.0 * M_PI);
- ServiceCarStatus.obs_radar[radar_ID_index].speed_relative = rate * 1.0 / 100.0;
- ServiceCarStatus.obs_radar[radar_ID_index].speed_x = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * sin(angle / 1800.0 * M_PI);
- ServiceCarStatus.obs_radar[radar_ID_index].speed_y = ServiceCarStatus.obs_radar[radar_ID_index].speed_relative * cos(angle / 1800.0 * M_PI);
- }
- else {
- ServiceCarStatus.obs_radar[radar_ID_index].valid = false;
- }
- }
- }
- }
- }
- void iv::control::CanUtil::obs_print(std::vector<obstacle_data_a> obstacleStatusA, std::vector<obstacle_data_b> obstacleStatusB, std::vector<obstacle_data_c> obstacleStatusC)
- {
- int vec_size = obstacleStatusA.size();
- if(vec_size == 0)
- {
- return;
- }
- std::cout<< "*****************************************************************************************"<<std::endl;
- for(int i = 0; i < vec_size; i++)
- {
- std::cout<< "obstacle ID: " << obstacleStatusA[i].obstacle_ID << std::endl;
- std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_x << std::endl;
- std::cout<< "obstacle_pos_x: " << obstacleStatusA[i].obstacle_pos_y << std::endl;
- std::cout<< "obstacle_rel_vel_x: " << obstacleStatusA[i].obstacle_rel_vel_x << std::endl;
- std::cout<< "obstacle_type: " << obstacleStatusA[i].obstacle_type << std::endl;
- std::cout<< "obstacle_status: " << obstacleStatusA[i].obstacle_status << std::endl;
- std::cout<< "obstacle_brake_light: " << obstacleStatusA[i].is_obstacle_brake_lights << std::endl;
- std::cout<< "obstacle_valid: " << obstacleStatusA[i].obstacle_valid << std::endl;
- // //std::cout<< "obstacle_length: " << obstacleStatusB[i].obstacle_length << std::endl;
- // std::cout<< "obstacle_width: " << obstacleStatusB[i].obstacle_width << std::endl;
- // std::cout<< "obstacle_age: " << obstacleStatusB[i].obstacle_age << std::endl;
- // std::cout<< "obstacle_cipv: " << obstacleStatusB[i].is_cipv_flag << std::endl;
- // std::cout<< "obstacle_radar_pos_x: " << obstacleStatusB[i].radar_pos_x << std::endl;
- // std::cout<< "obstacle_radar_vel_x: " << obstacleStatusB[i].radar_vel_x << std::endl;
- // std::cout<< "obstacle_angle_rate: " << obstacleStatusC[i].obstacle_angle_rate << std::endl;
- // std::cout<< "obstacle_angle" << obstacleStatusC[i].obstacle_angle << std::endl;
- // std::cout<< "object_accel_x" << obstacleStatusC[i].object_accel_x << std::endl;
- // std::cout<< "obstacle_scale_change: " << obstacleStatusC[i].obstacle_scale_change << std::endl;
- // std::cout<< "is_obstacle_replaced: " << obstacleStatusC[i].is_obstacle_replaced<< std::endl;
- }
- std::cout<< "*****************************************************************************************"<<std::endl;
- }
- #ifdef USE_MOBILEYE
- void iv::control::CanUtil::receive_mobileye(){
- while(m_connect)
- {
- memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_a) * 15);
- memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_b) * 15);
- memset(ServiceCarStatus.obstacleStatusA, 0, sizeof(obstacle_data_c) * 15);
- int obs_a_count = 0, obs_b_count = 0, obs_c_count = 0;
- int left_lane_a_count = 0, right_lane_a_count = 0, left_lane_b_count = 0, right_lane_b_count = 0;
- obstacle_data_a a_tmp;
- obstacle_data_b b_tmp;
- obstacle_data_c c_tmp;
- next_lane_a left_tmp_a, right_tmp_a;
- next_lane_b left_tmp_b, right_tmp_b;
- VCI_CAN_OBJ receivedata[2500];
- int res = VCI_Receive(m_devtype, m_devindmob, m_cannum, receivedata, 2500, 10);
- if (res <= 0)
- {
- #ifdef linux
- usleep(1000);
- #endif
- #ifdef WIN32
- Sleep(1);
- #endif
- continue;
- }
- for (int i = 0; i < res; i++)
- {
- switch(receivedata[i].ID)
- {
- case 0x700:
- impl_0x700(receivedata[i]);
- break;
- case 0x737:
- impl_0x737(receivedata[i]);
- break;
- case 0x738:
- impl_0x738(receivedata[i]);
- break;
- 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:
- a_tmp = impl_0x739(receivedata[i]);
- memcpy(&(ServiceCarStatus.obstacleStatusA[obs_a_count]), &a_tmp, sizeof(obstacle_data_a));
- obs_a_count++;
- break;
- 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:
- b_tmp = impl_0x73a(receivedata[i]);
- memcpy(&(ServiceCarStatus.obstacleStatusB[obs_b_count]), &b_tmp, sizeof(obstacle_data_b));
- obs_b_count++;
- break;
- 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:
- c_tmp = impl_0x73b(receivedata[i]);
- memcpy(&(ServiceCarStatus.obstacleStatusC[obs_a_count]), &c_tmp, sizeof(obstacle_data_c));
- obs_c_count++;
- break;
- case 0x669:
- impl_0x669(receivedata[i]);
- break;
- case 0x766:
- impl_0x766(receivedata[i]);
- break;
- case 0x767:
- impl_0x767(receivedata[i]);
- break;
- case 0x768:
- impl_0x768(receivedata[i]);
- break;
- case 0x769:
- impl_0x769(receivedata[i]);
- break;
- case 0x76c: case 0x76e: case 0x771: case 0x773: case 0x775: case 0x777: case 0x779: case 0x77b:
- if (receivedata[i].ID == 0x76c || receivedata[i].ID == 0x771 || receivedata[i].ID == 0x775 || receivedata[i].ID == 0x779)
- {
- left_tmp_a = impl_0x76c(receivedata[i]);
- memcpy(&(ServiceCarStatus.nextLaneA_left[left_lane_a_count]), &left_tmp_a, sizeof(next_lane_a));
- left_lane_a_count++;
- }
- else
- {
- right_tmp_a = impl_0x76c(receivedata[i]);
- memcpy(&(ServiceCarStatus.nextLaneA_right[right_lane_a_count]), &right_tmp_a, sizeof(next_lane_a));
- right_lane_a_count++;
- }
- //impl_0x76c(receivedata[i]);
- break;
- case 0x76d: case 0x76f: case 0x772: case 0x774: case 0x776: case 0x778: case 0x77a: case 0x77c:
- if (receivedata[i].ID == 0x76d || receivedata[i].ID == 0x772 || receivedata[i].ID == 0x776 || receivedata[i].ID == 0x77a)
- {
- left_tmp_b = impl_0x76d(receivedata[i]);
- memcpy(&(ServiceCarStatus.nextLaneB_left[left_lane_b_count]), &left_tmp_b, sizeof(next_lane_b));
- left_lane_b_count++;
- }
- else
- {
- right_tmp_b = impl_0x76d(receivedata[i]);
- memcpy(&(ServiceCarStatus.nextLaneB_right[right_lane_b_count]), &right_tmp_b, sizeof(next_lane_b));
- right_lane_b_count++;
- }
- break;
- case 0x76a:
- impl_0x76a(receivedata[i]);
- break;
- case 0x76b:
- impl_0x76b(receivedata[i]);
- break;
- default:
- break;
- }
- }
- // obs_print(ServiceCarStatus.obstacleStatusA, ServiceCarStatus.obstacleStatusB, ServiceCarStatus.obstacleStatusC);
- }
- }
- #endif
- //void iv::control::CanUtil::send() {
-
- // VCI_CAN_OBJ senddata;
- // QTime time;
- // int nTimeLastSend1;
- // time.start();
- // nTimeLastSend1 = time.elapsed();
- // int nSend1Int = 10; //10ms
- // senddata.SendType = m_sendtype;
- // senddata.ExternFlag = m_frameflag;
- // senddata.RemoteFlag = m_frameformat;
- // ServiceControlStatus.control_cmd_.Head = 0xEB;
- // while (true) {
- // usleep(1000);
- // if ((time.elapsed() - nTimeLastSend1) >= nSend1Int)
- // {
- // nTimeLastSend1 = time.elapsed();
- // senddata.ID = ServiceControlStatus.id;
- // senddata.DataLen = 8;
- // ServiceControlStatus.control_cmd_.CommunicationCount++;
- // 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;
- // memcpy(senddata.Data, &ServiceControlStatus.control_cmd_.Head, 8);
- // VCI_Transmit(m_devtype, m_devind, m_cannum, &senddata, 1);
- // ServiceControlStatus.control_cmd_.Control &= 0xCB;
- // }
- // }
- //}
- void iv::control::CanUtil::startsend(bool start_or_stop)
- {
- if(start_or_stop == true && send_connect == true && is_repeat == false)
- {
- is_repeat = true;
- thread_send = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&iv::control::CanUtil::send, this))); //启动发送线程
- }
- else if(start_or_stop == false && send_connect == true && is_repeat == true)
- {
- thread_send->interrupt();
- thread_send->join();
- is_repeat = false;
- }
- }
- bool iv::control::CanUtil::did_radar_ok()
- {
- bool temp = obs_find_flag;
- obs_find_flag = false;
- return temp;
- }
|