initial commit,receive MMW radar data OK and visualization OK[VIP]

.gitignore

@@ -0,0 +1,5 @@
+#Add by Zhanghm in 2017-12-25
+build/
+build[1-9]/
+cmake-build-debug/
+.idea/

CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(delphi_radar)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+find_package(OpenCV 2.4 REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+set(SRC_LIST TypeDef.h object_detection_radar.h frontal_delphi_radar.cpp main.cpp object_detection_radar.cpp)
+add_executable(delphi_radar ${SRC_LIST})
+target_link_libraries(delphi_radar ${OpenCV_LIBS} libpthread.so)

TypeDef.h

@@ -0,0 +1,32 @@
+#pragma once
+
+//每一个毫米波雷达目标的数据结构体
+typedef struct _moving_object_millimeter{
+	unsigned short target_ID; //目标ID号，从1-64
+	double range;			//目标距离
+	double v;				//目标速度
+	double angle;			//目标方位角
+	double x;
+	double y;
+	bool valid;
+	//zhanghm:20170911 new add
+	unsigned short status;//目标状态,CAN_TX_TRACK_STATUS
+	unsigned short moving; //运动状态
+	bool moving_fast;
+	bool moving_slow;
+}moving_object_millimeter;
+
+typedef struct _delphi_radar_target{
+	moving_object_millimeter delphi_detection_array[64]; //64个目标数据
+	unsigned short ACC_Target_ID;//ACC目标的ID号,0-64,0意味着没有ACC目标
+}delphi_radar_target;
+
+
+
+typedef struct Vehicle_Info_
+{
+	float Yaw_Rate;
+	float yawrate_phi;
+	float Vehicle_Speed;
+	float Steering_Angle;
+}Vehicle_Info;

frontal_delphi_radar.cpp

@@ -0,0 +1,237 @@
+#include "frontal_delphi_radar.h"
+#include <arpa/inet.h>
+#include <algorithm>
+#include <math.h>
+
+const char* FRONTAL_RADAR_IP = "192.168.0.178";
+using std::vector;
+#define PI 3.1415926535898
+const double toRAD = PI/180.0;
+FrontalDelphiRadar::FrontalDelphiRadar(){
+  memset((char*)&myaddr_,0,sizeof(myaddr_));
+  //initialize the parsing parameters
+  radar_target_CAN_ID_vec_.resize(64);
+  for(vector<unsigned int>::iterator it=radar_target_CAN_ID_vec_.begin();it!=radar_target_CAN_ID_vec_.end();++it){
+    int count = 0x500+(it-radar_target_CAN_ID_vec_.begin());
+    *it = count;
+  }
+
+  //reset the radar struct
+  memset(&radar_target_data_,0,sizeof(radar_target_data_));
+  memset(radar_data_buf_,0,sizeof(radar_data_buf_));//reset buffer
+
+}
+
+FrontalDelphiRadar::~FrontalDelphiRadar(){
+
+}
+bool FrontalDelphiRadar::Init(){
+  //1)create radar socket
+  if((radar_socket_=socket(AF_INET,SOCK_DGRAM,0))<0){
+    perror("[ERROR]cannot create radar socket!");
+    return false;
+  }
+  //2)build connecion address
+  myaddr_.sin_family=AF_INET;
+  myaddr_.sin_port = htons(FRONTAL_RADAR_LISTEN_PORT);
+  myaddr_.sin_addr.s_addr = htonl(INADDR_ANY);
+  //enable address reuse
+  int ret,on;
+  ret = setsockopt(radar_socket_,SOL_SOCKET,SO_REUSEADDR,&on,sizeof(on));
+
+  //3)bind socket to specific address
+  if(bind(radar_socket_,(sockaddr*)&myaddr_,sizeof(myaddr_))<0){
+    perror("[ERROR]cannot bind the radar socket!");
+    close(radar_socket_);
+    return false;
+  }
+
+
+  return true;
+}
+
+bool FrontalDelphiRadar::Update(){
+  //send initial data to radar
+  Send_Triggle_Signal();
+
+  //receive radar socket data
+  remaddrlen_ = sizeof(remaddr_);
+  recv_len_ = recvfrom(radar_socket_,radar_data_buf_,RADAR_DATA_BUFFER,0,(struct sockaddr*)&remaddr_,&remaddrlen_);
+  //printf("recv length is %d \n",recv_len_);
+  //printf("remote address is %s and %d\n",inet_ntoa(remaddr_.sin_addr),ntohs(remaddr_.sin_port));
+  if(recv_len_<0){
+    perror("[ERROR]cannot receive radar data!");
+    close(radar_socket_);
+    return false;
+  }
+  else
+    Proc_Radar_Data();
+  //usleep(50000);
+}
+
+bool FrontalDelphiRadar::Send_Triggle_Signal(){
+  //1)specify remote address
+  sockaddr_in send_addr;
+  memset(&send_addr,0,sizeof(send_addr));
+  send_addr.sin_addr.s_addr = inet_addr(FRONTAL_RADAR_IP);//IP
+  send_addr.sin_family = AF_INET;
+  send_addr.sin_port = htons(FRONTAL_RADAR_PORT); //port
+  unsigned char FI=0b00001000;
+  unsigned char tmpCanID1=0b00000000;
+  unsigned char tmpCanID2=0b00000000;
+  unsigned char tmpCanID3=0b00000100;
+  unsigned char tmpCanID4=0b11110001;
+  unsigned char tmpNum=0b00000000;
+  //  quint8 tmpNum2 = 0b10111111;
+  unsigned char tmpNum2 = 0xbf;
+
+  //2)fill send buffer
+  //send data buffer
+  unsigned char send_buf[13];
+  /*ID = 0X04f1 DATA = 00 00 00 00 00 00 BF 00*/
+  send_buf[0]=FI;
+  send_buf[1]=tmpCanID1;
+  send_buf[2]=tmpCanID2;
+  send_buf[3]=tmpCanID3;
+  send_buf[4]=tmpCanID4;
+  send_buf[5]=tmpNum;
+  send_buf[6]=tmpNum;
+  send_buf[7]=tmpNum;
+  send_buf[8]=tmpNum;
+  send_buf[9]=tmpNum;
+  send_buf[10]=tmpNum;
+  send_buf[11]=tmpNum2;
+  send_buf[12]=tmpNum;
+  int send_len = sendto(radar_socket_,send_buf,sizeof(send_buf),0,(sockaddr*)&remaddr_,sizeof(remaddr_));
+  if(send_len<0){
+    perror("[ERROR]cannot send initializiton data!");
+    return false;
+  }
+  return true;
+}
+
+bool FrontalDelphiRadar::Send_Vehicle_Info(){
+  //TODO: vehicle info value assignment
+  int speed_can = 0;
+  short yawrate_can = 0;
+  int radius;
+  unsigned char steersign;
+  short steer_can = 0;
+  unsigned char bfsign = 0;
+
+  //Send info to ID 0x4F0
+  unsigned char FI=0b00001000;
+  unsigned char tmpCanID1=0b00000000;
+  unsigned char tmpCanID2=0b00000000;
+  unsigned char tmpCanID3=0b00000100; //04
+  unsigned char tmpCanID4=0b11110000; //F0
+  unsigned char send_buf_4F0[13];
+  memset(send_buf_4F0,0,sizeof(send_buf_4F0));
+
+  send_buf_4F0[0]=FI;
+  send_buf_4F0[1]=tmpCanID1;
+  send_buf_4F0[2]=tmpCanID2;
+  send_buf_4F0[3]=tmpCanID3;
+  send_buf_4F0[4]=tmpCanID4;
+  send_buf_4F0[5]=(speed_can>>3); //host vehicle spedd, m/s
+  send_buf_4F0[6]=(((speed_can&0x07)<<5)|((yawrate_can>>8)&0x0F)|(bfsign<<4));
+  send_buf_4F0[7]=((yawrate_can)&0xFF);
+  send_buf_4F0[8]=(0x80|(radius>>8));//横摆角速度有效位,转弯半径
+  send_buf_4F0[9]=(radius&0xFF);
+  send_buf_4F0[10]=(0x80|(steersign<<6)|(steer_can>>5));
+  send_buf_4F0[11]=((steer_can&0x1F)<<3);
+
+
+  //Send info to ID 0x4F1
+  unsigned char send_buf_4F1[13];
+  memset(send_buf_4F1,0,sizeof(send_buf_4F1));
+  send_buf_4F1[0]=0b00001000;
+  send_buf_4F1[1]=0x00;
+  send_buf_4F1[2]=0x00;
+  send_buf_4F1[3]=0x04;
+  send_buf_4F1[4]=0xF1;
+  send_buf_4F1[10]= 0;//横向安装偏差
+  send_buf_4F1[11]=(1<<7)|(1<<6)|63;
+  send_buf_4F1[12] =(1<<5);//速度有效位置
+
+  //Send info to ID 0x5F2
+
+
+}
+void FrontalDelphiRadar::Proc_Radar_Data(){
+  int can_frame_count = recv_len_/13;//each can frame contains 13 bytes
+  for(int i=0;i<can_frame_count;++i){//a udp data frame may contain numbers of CAN frames
+    unsigned char* buf = &(radar_data_buf_[i*13]);
+    unsigned int tmpCanID;
+    unsigned char tmpdata[8];
+    tmpCanID=buf[1]<<24|buf[2]<<16|buf[3]<<8|buf[4];
+    tmpdata[0]=buf[5];tmpdata[1]=buf[6];tmpdata[2]=buf[7];
+    tmpdata[3]=buf[8];tmpdata[4]=buf[9];tmpdata[5]=buf[10];
+    tmpdata[6]=buf[11];tmpdata[7]=buf[12];
+
+    //parsing the radar data we want
+    vector<unsigned int>::iterator iter = find(radar_target_CAN_ID_vec_.begin(),radar_target_CAN_ID_vec_.end(),tmpCanID);
+    if(iter!=radar_target_CAN_ID_vec_.end()){ //obtain valid radar target data
+      int m = iter-radar_target_CAN_ID_vec_.begin();//the m-th target
+      printf("mmmmmmmmm is %d \n",m);
+      unsigned short temp_D1;
+      unsigned short temp_D2;
+      unsigned short temp_V1;
+      unsigned short temp_V2;
+      unsigned short temp_V3;
+      //range  Unit: m
+      temp_D1 =tmpdata[2];
+      temp_D2 = tmpdata[3];
+      radar_target_data_.delphi_detection_array[m].range = ((temp_D2)|((temp_D1&0x0007)<<8))*0.1f;
+      printf("range is %f \n",radar_target_data_.delphi_detection_array[m].range);
+
+      //range_rate  Unit: m/s
+      temp_V1 = tmpdata[6];
+      temp_V2 = tmpdata[7];
+      unsigned short  temp_V=temp_V1&0x0020;
+      if (temp_V==0)
+      {
+        radar_target_data_.delphi_detection_array[m].v =(temp_V2|((temp_V1&0x003F)<<8))*0.01f;//Unit: m/s
+      }
+      if (temp_V==0x0020)
+      {
+        unsigned short temp_0=((temp_V1&0x003F) <<8)|temp_V2;
+        unsigned short temp_1=temp_0 - 1;
+        unsigned short temp_2=(~temp_1) & 0x1FFF;
+        radar_target_data_.delphi_detection_array[m].v=-(temp_2*0.01f);//Unit: m/s
+      }
+      printf("range rate is %f \n",radar_target_data_.delphi_detection_array[m].v);
+
+      //angle  Unit:degree
+      unsigned short temp_A1=tmpdata[1];
+      unsigned short temp_A2=tmpdata[2];
+      unsigned short temp_A3=temp_A1&0x0010;
+      if (temp_A3==0)
+      {
+        radar_target_data_.delphi_detection_array[m].angle=(((temp_A1&0x000F)<<5)|((temp_A2&0x00F8)>>3))*0.1f;
+      }
+      if(temp_A3==0x0010)
+      {
+        unsigned short temp_3=((temp_A1&0x000F)<<5)|((temp_A2&0x00F8)>>3);
+        unsigned short temp_4=temp_3 - 1;
+        unsigned short temp_5=(~temp_4) & 0x01FF;
+        radar_target_data_.delphi_detection_array[m].angle=-temp_5*0.1f;
+      }
+      printf("angle is %f \n",radar_target_data_.delphi_detection_array[m].angle);
+
+      //calculate x,y   Unit: m
+      radar_target_data_.delphi_detection_array[m].x=radar_target_data_.delphi_detection_array[m].range*sin(radar_target_data_.delphi_detection_array[m].angle*toRAD);
+      radar_target_data_.delphi_detection_array[m].y=radar_target_data_.delphi_detection_array[m].range*cos(radar_target_data_.delphi_detection_array[m].angle*toRAD);
+
+    }
+
+
+  }
+
+}
+
+
+delphi_radar_target FrontalDelphiRadar::radar_target_data(){
+  return radar_target_data_;
+}
+

frontal_delphi_radar.h

@@ -0,0 +1,68 @@
+/*==================================================================
+ *    Function    ：receive udp data of delphi radar and parse them
+ *    相关说明：
+ *    作者    ：  zhanghm
+ *    创建日期    ：20180307
+ *    修改记录：
+/*==================================================================*/
+#ifndef FRONTAL_DELPHI_RADAR_H_
+#define FRONTAL_DELPHI_RADAR_H_
+//C/C++
+#include <cstdio>
+#include <string.h>
+#include <iostream>
+#include <string>
+#include <vector>
+//UDP socket
+#include <dlfcn.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <netinet/in.h>
+//project headers
+#include "TypeDef.h" //radar struct
+
+//const definition
+const int FRONTAL_RADAR_PORT = 4001;
+const int FRONTAL_RADAR_LISTEN_PORT = 8010; //local listen port
+
+const short RADAR_DATA_BUFFER = 650; //buffer 1000 bytes
+
+class FrontalDelphiRadar{
+public:
+
+public:
+  //constructors
+  FrontalDelphiRadar();
+  ~FrontalDelphiRadar();
+
+  bool Init();//init socket and bind socket
+  bool Update();//update socket to receive data, should be call in a loop
+  delphi_radar_target radar_target_data(); //get radar target data
+private:
+  void Proc_Radar_Data();//parse radar data
+  bool Send_Vehicle_Info();//send vehicle information to radar
+  bool Send_Triggle_Signal();//send triggle signal to radar to get normal data
+
+private:
+  //udp socket
+  int radar_socket_; //socket used to communicate with radar,send or receive data from radar
+  sockaddr_in myaddr_; //listen port info
+  sockaddr_in remaddr_; //remote address
+  socklen_t remaddrlen_;
+  int recv_len_; //recvfrom function return, indicate receive how many bytes in a UDP package
+  //address struct for send vehicle info
+
+  //radar data parsing
+  unsigned char radar_data_buf_[RADAR_DATA_BUFFER];
+  std::vector<unsigned int> radar_target_CAN_ID_vec_;
+  delphi_radar_target radar_target_data_;
+
+
+
+
+
+
+};
+
+#endif /*FRONTAL_DELPHI_RADAR_H_*/

main.cpp

@@ -0,0 +1,30 @@
+#include <iostream>
+#include "frontal_delphi_radar.h"
+#include "object_detection_radar.h"
+using namespace std;
+int main(){
+    FrontalDelphiRadar frontal_delphi;
+    ObjectDetection object_detection_;
+    object_detection_.draw_basic_info();
+    bool flag = frontal_delphi.Init();
+    if(flag == true){
+        cout<<"hello world!"<<endl;
+    }
+    else{
+        cout<<"error"<<endl;
+        return -1;
+    }
+    while(1){
+    frontal_delphi.Update();
+    delphi_radar_target radar_data=frontal_delphi.radar_target_data();
+    object_detection_.get_radar_Data(radar_data);
+    object_detection_.main_function2();
+    IplImage* delphi_image = object_detection_.m_Delphi_img;
+    cvNamedWindow("delphi_image",CV_WINDOW_NORMAL);
+    cvShowImage("delphi_image", delphi_image);
+
+    int key = cvWaitKey(10);
+    if(key == 32)
+        cvWaitKey(0);
+    }
+}