|
|
@@ -0,0 +1,1266 @@
|
|
|
+#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;
|
|
|
+}
|
pengcheng