v2x sensor data collect

src/v2x/v2xTcpClient/boost.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/v2x/v2xTcpClient/data_type.h

@@ -14,8 +14,8 @@ struct VehicleStatus
     int soc=0;
     int carError=0;
     int errorNum=0;
-    float lon=0.0;
-    float lat=0.0;
+    double lon=0.0;
+    double lat=0.0;
     float yaw=0.0;
 };
 struct HardwareStatus

src/v2x/v2xTcpClient/decition_type.h

@@ -0,0 +1,61 @@
+#pragma once
+#ifndef _IV_DECITION_DECITION_TYPE_
+#define _IV_DECITION_DECITION_TYPE_
+#include "boost.h"
+namespace iv {
+namespace decition {
+struct DecitionBasic {
+    float speed;				//车速
+    float wheel_angle;			//转向角度
+    float brake;				//刹车
+    float accelerator;			//油门
+    float torque;               //力矩
+    bool leftlamp;				//左转向灯
+    bool rightlamp;				//右转向灯
+
+    int  engine;
+    int  grade;
+    int  mode;
+    int handBrake;
+    bool speak;
+    bool door;
+    bool bright;
+    int dangWei;
+
+    float angSpeed;
+    int brakeType :1;
+    char brakeEnable;  //add by fjk
+    bool left;         //add by fjk
+    bool right;        //add by fjk
+
+    bool angleEnable;
+    bool speedEnable;
+    bool dangweiEnable;
+    int  driveMode;
+
+    int directLight;
+    int brakeLight;
+    int backLight;
+    int topLight;
+
+    int farLight;
+    int nearLight;
+
+
+
+    bool   air_enable ;                  //空调使能
+    bool   air_on;
+    float   air_temp ;                  //空调温度
+    float   air_mode ;                  //空调模式
+    float   wind_level ;                  //空调风力
+    float   roof_light ;                  //顶灯
+    float   home_light ;                  //日光灯
+    float   air_worktime ;                  //空调工作时间
+    float   air_offtime ;                  //空调关闭时间
+
+};
+typedef boost::shared_ptr<DecitionBasic> Decition;	//决策
+}
+}
+#endif // !_IV_DECITION_DECITION_TYPE_
+

src/v2x/v2xTcpClient/gnss_coordinate_convert.cpp

@@ -0,0 +1,154 @@
+#include <gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+/*
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void GaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+*/
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/v2x/v2xTcpClient/gnss_coordinate_convert.h

@@ -0,0 +1,26 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/v2x/v2xTcpClient/gps_type.h

@@ -0,0 +1,136 @@
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include <boost.h>
+namespace iv {
+    struct GPS_INS
+    {
+        int valid = 0xff;
+        int index = 0;	//gps点序号
+
+        double gps_lat = 0;//纬度
+        double gps_lng = 0;//经度
+
+        double gps_x = 0;
+        double gps_y = 0;
+        double gps_z = 0;
+
+        double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+        double ins_pitch_angle = 0;	//俯仰角
+        double ins_heading_angle = 0;	//航向角
+
+        int ins_status = 0;	//惯导状态 4
+        int rtk_status = 0;	//rtk状态 6 -5 -3
+        int gps_satelites_num = 0;
+
+        //-----加速度--------------
+        double accel_x = 0;
+        double accel_y = 0;
+        double accel_z = 0;
+
+        //-------角速度------------
+        double ang_rate_x = 0;
+        double ang_rate_y = 0;
+        double ang_rate_z = 0;
+
+        //-----------方向速度--------------
+        double vel_N = 0;
+        double vel_E = 0;
+        double vel_D = 0;
+
+        int speed_mode = 0;
+        int mode2 = 0;
+        double speed = 3.5;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+        double mfRoadWidth = 3.5; // Current Road Width
+
+
+
+    };
+
+//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+
+   struct Station
+   {
+       int index;
+       GPS_INS station_location;
+       int map_index;
+   };
+
+
+
+     class Point2D
+    {
+      public:
+          double x = 0, y = 0, speed=0;
+         int v1 = 0, v2 = 0;
+         int roadMode = 0;
+
+         Point2D()
+        {
+            x = y = v1 = 0;
+        }
+
+         Point2D(double _x, double _y)
+        {
+            x = _x; y = _y;
+        }
+
+
+     };
+
+
+     class TrafficLight
+    {
+      public:
+         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+
+         TrafficLight()
+        {
+            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+        }
+
+     };
+
+     class StationCmd
+     {
+     public:
+         bool received;
+         uint32_t carID,carMode,emergencyStop,stationStop;
+         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
+         uint32_t stationID[20];
+         GPS_INS  stationGps[20];
+         uint32_t stationTotalNum;
+         StationCmd()
+         {
+             received=false;
+             has_carID=false;
+             has_carMode=false;
+             has_emergencyStop=false;
+             has_stationStop=false;
+             mode_manual_drive=false;
+             carID=0;
+             carMode=0;
+             emergencyStop=0;
+             stationStop=0;
+             stationTotalNum=0;
+         }
+     };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src/v2x/v2xTcpClient/mainwindow.cpp

@@ -28,7 +28,6 @@ MainWindow::MainWindow(QWidget *parent) :
     ui(new Ui::MainWindow)
 {
     ui->setupUi(this);
-
     //Start Get init param
     QString strpath = QCoreApplication::applicationDirPath();
     qDebug()<<strpath;
@@ -37,11 +36,12 @@ MainWindow::MainWindow(QWidget *parent) :
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
     std::string strCarVIN = xp.GetParam("carVIN","catarc001");
+    mscarID = QString::fromStdString(strCarVIN);
     std::string strHostIP = xp.GetParam("hostIP","47.95.196.28");
     std::string strHostPort = xp.GetParam("hostPort","12123");
     std::string strStationCount = xp.GetParam("stationCount","20");
     int count = std::stoi(strStationCount,nullptr,10);
-    stationGps location;
+    StationGps location;
     for(int i = 0; i < count; i++)
     {
         std::string strLatName = "lat" + std::to_string(i);
@@ -61,14 +61,37 @@ MainWindow::MainWindow(QWidget *parent) :
     ui->lineEdit_port->setText(QString::fromStdString(strHostPort));
     ui->pushButton_connect->setEnabled(false);
 
-    //连接信号槽
     givlog = new iv::Ivlog("v2x");
     connect(socket, &QTcpSocket::readyRead, this, &MainWindow::socket_Read_Data);
     connect(socket, &QTcpSocket::disconnected, this, &MainWindow::socket_Disconnected);
 
-    mp_v2xSend = iv::modulecomm::RegisterSend("v2x",1000,3);
+    /* ShareMemory Register BEGIN */
+    //ShareMem: v2x
+    mpMemV2xSend = iv::modulecomm::RegisterSend("v2x",1000,3);
+    //ShareMem: v2x使能状态
     iv::modulecomm::RegisterRecv("v2xStEn", ListenV2xStEn);
-    mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStReq",1000,1);
+    //ShareMem: v2x使能状态请求
+    mpMemV2xStSend = iv::modulecomm::RegisterSend("v2xStReq",1000,1);
+    //ShareMem: 车辆地盘状态
+    ModuleFun funchassis =std::bind(&MainWindow::UpdateChassis,this,std::placeholders::_1,\
+                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,\
+                                    std::placeholders::_5);
+    mpMemchassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);
+    //ShareMem: gps
+    ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1, \
+                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpMemGps = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+
+    ModuleFun funlidarObs =std::bind(&MainWindow::UpdateLidarObs,this,std::placeholders::_1, \
+                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpMemLidarObs = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",funlidarObs);
+
+    ModuleFun funRadarObs =std::bind(&MainWindow::UpdateRadarObs,this,std::placeholders::_1, \
+                                   std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpMemRadarObs = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",funRadarObs);
+
+    /* ShareMemory Register END */
+
     shareV2xStReqMsg();
     //heart beat,jiaolili,20210207
     m_bEnablePlatform=false;
@@ -364,7 +387,7 @@ void MainWindow::shareV2xProtoMsg(iv::v2x::v2x msgV2xProto)
     char * strdata = new char[msgV2xProto.ByteSize()];
     if(msgV2xProto.SerializePartialToArray(strdata,nsize))
     {
-        iv::modulecomm::ModuleSendMsg(mp_v2xSend,strdata,nsize);
+        iv::modulecomm::ModuleSendMsg(mpMemV2xSend,strdata,nsize);
     }
     givlog->info("v2x","share v2x controll Msg");
     delete strdata;
@@ -378,7 +401,7 @@ void MainWindow::shareV2xStReqMsg()
     char * str = new char[nsize];
     if(x.SerializeToArray(str,nsize))
     {
-        iv::modulecomm::ModuleSendMsg(mp_v2xStSend,str,nsize);
+        iv::modulecomm::ModuleSendMsg(mpMemV2xStSend,str,nsize);
     }
     else
     {
@@ -401,10 +424,22 @@ void MainWindow::shareV2xStReqMsg()
 void MainWindow::upVehicleStatus()
 {
     QString time_stamp=getTimeStamp();
-    QString test="['CCFF',"+time_stamp+",'catarc001',1,0,0,100,0,0,0,0,0]";
+    QString test="['CCFF',"+time_stamp+mscarID+","+QString::number(micarMode)+","+QString::number(mfspeed)+","+ \
+            QString::number(mfsteerAngle)+","+ QString::number(miSOC)+","+ QString::number(micarError)+","+ \
+            QString::number(mierrorNum)+","+QString::number(mflon)+","+ QString::number(mflat)+","+ \
+            QString::number(mfheading)+"]";
     //QByteArray bytes = test.toUtf8();
     QByteArray bytes = test.toLatin1();
     socket->write(bytes);
+    micarMode = 0;
+    mfspeed = 0;
+    mfsteerAngle = 0;
+    miSOC = 0;
+    micarError = 0;
+    mierrorNum = 0;
+    mflon = 0;
+    mflat = 0;
+    mfheading = 0;
 }
 void MainWindow::upHardwareStatus()
 {
@@ -413,6 +448,15 @@ void MainWindow::upHardwareStatus()
     //QByteArray bytes = test.toUtf8();
     QByteArray bytes = test.toLatin1();
     socket->write(bytes);
+    mistRadar = 1;
+    mistLidar = 1;
+    mistSonic = 0;//超声
+    mistCamera = 0;
+    mistmic = 0;
+    mistGPS = 1;
+    mistCanCar = 1;
+    mistCanRadar = 1;
+    mistlight = 0;//红绿灯
 }
 void MainWindow::upObstacleDataStatus()
 {
@@ -430,3 +474,153 @@ void MainWindow::upSoftwareStatus()
     QByteArray bytes = test.toLatin1();
     socket->write(bytes);
 }
+
+void MainWindow::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    mistCanCar = 0;
+    iv::chassis xchassis;
+    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"v2xTcpClient::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+    if(xchassis.has_drivemode())
+    {
+        micarMode = xchassis.drivemode();
+    }
+    if(xchassis.has_vel())
+        mfspeed = xchassis.vel();
+    if(xchassis.has_angle_feedback())
+        mfsteerAngle = xchassis.angle_feedback();
+    if((xchassis.has_soc()))
+    {
+        miSOC = xchassis.soc();
+    }
+}
+
+//接收GPS数据
+void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,\
+                              const QDateTime * dt,const char * strmemname)
+{
+
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
+        return;
+    }
+
+    mflat = xgpsimu.lat();
+    mflon = xgpsimu.lon();
+    mfheading = xgpsimu.heading();
+    mpdata.gps_lat = xgpsimu.lat();
+    mpdata.gps_lng = xgpsimu.lon();
+    mpdata.ins_heading_angle = xgpsimu.heading();
+    mpdata.rtk_status = xgpsimu.rtk_state();
+    mpdata.ins_status = xgpsimu.ins_state();
+    mpdata.vel_D = xgpsimu.vd();    //地向速度,单位(米/秒)
+    mpdata.vel_E = xgpsimu.ve();    //东向速度,单位(米/秒)
+    mpdata.vel_N = xgpsimu.vn();    //北向速度,单位(米/秒)
+
+ //   mfspeed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6;  //double pow(double x, double y) 返回 x 的 y 次幂，即 xy。
+    mistGPS = 0;
+}
+
+int toUnicode(const char* str)
+{
+    return str[0] + (str[1] ? toUnicode(str + 1) : 0);
+}
+
+constexpr inline int U(const char* str)
+{
+    return str[0] + (str[1] ? U(str + 1) : 0);
+}
+int GetTypeString(const char* obsType)
+{
+  switch (toUnicode(obsType))
+  {
+    case U("unknown"):
+      return 0;
+    case U("car"):
+      return 1;
+    case U("bike"):
+      return 5;
+    case U("pedestrian"):
+      return 3;
+    default:
+      return 0;
+  }
+}
+void MainWindow::UpdateLidarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
+                           const QDateTime * dt,const char * strmemname)
+{
+
+//    std::vector<iv::lidar::object>  lidarobjvec;
+//    strtolidarobj(lidarobjvec,strdata,nSize);
+    mqLidarObsInfo.clear();
+    iv::lidar::objectarray lidarobjvec;
+    std::string in;
+    in.append(strdata,nSize);
+    lidarobjvec.ParseFromString(in);
+    givlog->verbose("v2xLidarObs","obj size is %d ",lidarobjvec.obj_size());
+    miLidarObsCount = lidarobjvec.obj_size();
+    ObsInfo xobsInfo;
+    double x,y;
+    iv::GPS_INS gps_ins;
+    for(int i = 0; i < miLidarObsCount; i++)
+    {
+        x = lidarobjvec.obj(i).centroid().x();
+        y = lidarobjvec.obj(i).centroid().y();
+        gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
+        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &(gps_ins.gps_lng), &(gps_ins.gps_lat));
+        xobsInfo.lat = gps_ins.gps_lat;
+        xobsInfo.lon = gps_ins.gps_lng;
+        std::string str = lidarobjvec.obj(i).type_name();
+        xobsInfo.type = GetTypeString(str.data());
+        mqLidarObsInfo.append(xobsInfo);
+    }
+    mistLidar = 0;
+}
+
+void MainWindow::UpdateRadarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
+                           const QDateTime * dt,const char * strmemname)
+{
+    mqRadarObsInfo.clear();
+    static qint64 oldrecvtime;
+    iv::radar::radarobjectarray xradararray;
+    if(!xradararray.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error.");
+        return;
+    }
+
+    //   gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch());
+
+    if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
+    {
+        givlog->warn("radar interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
+    }
+
+    oldrecvtime = QDateTime::currentMSecsSinceEpoch();
+
+    miRadarObsCount = xradararray.obj_size();
+    ObsInfo xobsInfo;
+    double x,y;
+    iv::GPS_INS gps_ins;
+    for(int i = 0; i < miRadarObsCount; i++)
+    {
+        x = xradararray.obj(i).x();
+        y = xradararray.obj(i).y();
+        gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
+        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &(gps_ins.gps_lng), &(gps_ins.gps_lat));
+        xobsInfo.lat = gps_ins.gps_lat;
+        xobsInfo.lon = gps_ins.gps_lng;
+        xobsInfo.type = 0;
+        mqRadarObsInfo.append(xobsInfo);
+    }
+    mistRadar = 0;
+    mistCanRadar = 0;
+    mistLidar = 0;
+}

src/v2x/v2xTcpClient/mainwindow.h

@@ -11,20 +11,35 @@
 #include "data_type.h"
 #include "QMessageBox"
 #include "v2x.pb.h"
+#include "chassis.pb.h"
+#include "gpsimu.pb.h"
+#include "object.pb.h"
+#include "objectarray.pb.h"
+#include "radarobject.pb.h"
+#include "radarobjectarray.pb.h"
+#include "gps_type.h"
 #include "modulecomm.h"
 #include "xmlparam.h"
 #include "ivlog.h"
+#include "transfer.h"
+#include "gnss_coordinate_convert.h"
 #include <QTimer>
 #include <iostream>
 namespace Ui {
 class MainWindow;
 }
 
-struct stationGps
+struct StationGps
 {
     double lon;
     double lat;
 };
+struct ObsInfo
+{
+    int    type;
+    double lon;
+    double lat;
+};
 
 class MainWindow : public QMainWindow
 {
@@ -40,11 +55,24 @@ public:
     void ProStationCommand(QString str);
     bool checkVehicle(QString str);
     int getDownStreamId(QString str);
+//    int toUnicode(const char* str);
+//    constexpr inline int U(const char* str);
+//    int GetTypeString(const char*  obsType) const;
     void upVehicleStatus();
     void upHardwareStatus();
     void upObstacleDataStatus();
     void upSoftwareStatus();
     void upDataStream();
+    void shareV2xStReqMsg();
+    void shareV2xProtoMsg(iv::v2x::v2x msgV2xProto);
+    void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, \
+                       const QDateTime *dt, const char *strmemname);
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,\
+                                  const QDateTime * dt,const char * strmemname);
+    void UpdateLidarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
+                               const QDateTime * dt,const char * strmemname);
+    void UpdateRadarObs(const char * strdata,const unsigned int nSize,const unsigned int index, \
+                               const QDateTime * dt,const char * strmemname);
 private slots:
 
     void socket_Read_Data();
@@ -57,13 +85,53 @@ private slots:
 private:
     Ui::MainWindow *ui;
     QTcpSocket *socket;
-    void * mp_v2xSend;
-    void * mp_v2xStSend;
+    void * mpMemV2xSend;
+    void * mpMemV2xStSend;
+    void * mpMemchassis;
+    void * mpMemGps;
+    void * mpMemLadarESRObs;//前毫米波
+    void * mpMemLadarSRRLObs;//角毫米波
+    void * mpMemRadarObs;
+    void * mpMemLidarObs;
+    void * mpMemMobleyeObs;//Mobiley
+    void * mpMemLidar_pc;
     bool m_bEnablePlatform;
     bool m_bIsConnect;
-    QVector<stationGps> mstationGps;
-    void shareV2xStReqMsg();
-    void shareV2xProtoMsg(iv::v2x::v2x msgV2xProto);
+    QVector<StationGps> mstationGps;
+    iv::GPS_INS mpdata;
+
+    /* 车辆状态数据 上行 BEGIN */
+    QString mscarID ;
+    int micarMode = 0;
+    float mfspeed = 0;
+    float mfsteerAngle = 0;
+    int miSOC = 0;
+    int micarError = 0;
+    int mierrorNum = 0;
+    double mflat = 0;//经度
+    double mflon = 0;//纬度
+    double mfheading = 0;//航向
+    /* 车辆状态数据 END */
+
+    /* 硬件状态数据 上行 BEGIN */
+    int mistRadar = 1;
+    int mistLidar = 1;
+    int mistSonic = 0;//超声
+    int mistCamera = 0;
+    int mistmic = 0;
+    int mistGPS = 1;
+    int mistCanCar = 1;
+    int mistCanRadar = 1;
+    int mistlight = 0;//红绿灯
+    /* 硬件状态数据 上行 END */
+
+    /* 障碍物状态数据 上行 START */
+    QVector<ObsInfo> mqLidarObsInfo;
+    QVector<ObsInfo> mqRadarObsInfo;
+    int miLidarObsCount = 0;
+    int miRadarObsCount = 0;
+    /* 障碍物状态数据 上行 END */
+
 
 };
 #endif // MAINWINDOW_H

src/v2x/v2xTcpClient/transfer.cpp

@@ -0,0 +1,138 @@
+#include <transfer.h>
+#include <decition_type.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+using namespace std;
+#define PI (3.1415926535897932384626433832795)
+
+
+
+///大地转车体
+iv::Point2D iv::decition::Coordinate_Transfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path - pos.gps_x, y_path - pos.gps_y);
+
+    double distance = sqrt((x_path - pos.gps_x) * (x_path - pos.gps_x) + (y_path - pos.gps_y) * (y_path - pos.gps_y));
+    double angle = theta - (-90 + pos.ins_heading_angle) / 180 * PI;
+
+    x_vehicle = -cos(angle) * distance;
+    y_vehicle = sin(angle) * distance;
+    return  iv::Point2D(x_vehicle, y_vehicle);
+}
+
+///车体转大地
+iv::GPS_INS iv::decition::Coordinate_UnTransfer(double x_path, double y_path, iv::GPS_INS pos)
+{
+    iv::GPS_INS gps_ins;
+    double x_vehicle, y_vehicle;
+    double theta = atan2(x_path, y_path);
+
+    double distance = sqrt(x_path * x_path + y_path * y_path);
+    double angle = (pos.ins_heading_angle / 180 * PI + theta);
+
+    x_vehicle = pos.gps_x + distance * sin(angle);
+    y_vehicle = pos.gps_y + distance * cos(angle);
+    gps_ins.gps_x = x_vehicle;
+    gps_ins.gps_y = y_vehicle;
+
+    return  gps_ins;
+}
+
+double iv::decition::GetDistance(iv::Point2D x1, iv::Point2D x2)
+{
+    return sqrt((x1.x - x2.x) * (x1.x - x2.x) + (x1.y - x2.y) * (x1.y - x2.y));
+}
+
+double iv::decition::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
+{
+    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
+}
+
+
+
+
+//GPS转大地坐标
+void iv::decition::BLH2XYZ(iv::GPS_INS gp)
+{
+    int nFlag = 2;
+
+    double B = gp.gps_lat;
+
+    double L = gp.gps_lng;
+
+
+
+    double N, E, h;
+    double L0 = GetL0InDegree(L);//根据经度求中央子午线经度
+
+    double a = 6378245.0;            //地球半径  北京6378245
+    double F = 298.257223563;        //地球扁率
+    double iPI = 0.0174532925199433; //2pi除以360，用于角度转换
+
+    double f = 1 / F;
+    double b = a * (1 - f);
+    double ee = (a * a - b * b) / (a * a);
+    double e2 = (a * a - b * b) / (b * b);
+    double n = (a - b) / (a + b), n2 = (n * n), n3 = (n2 * n), n4 = (n2 * n2), n5 = (n4 * n);
+    double al = (a + b) * (1 + n2 / 4 + n4 / 64) / 2;
+    double bt = -3 * n / 2 + 9 * n3 / 16 - 3 * n5 / 32;
+    double gm = 15 * n2 / 16 - 15 * n4 / 32;
+    double dt = -35 * n3 / 48 + 105 * n5 / 256;
+    double ep = 315 * n4 / 512;
+
+    B = B * iPI;
+    L = L * iPI;
+    L0 = L0 * iPI;
+
+    double l = L - L0, cl = (cos(B) * l), cl2 = (cl * cl), cl3 = (cl2 * cl), cl4 = (cl2 * cl2), cl5 = (cl4 * cl), cl6 = (cl5 * cl), cl7 = (cl6 * cl), cl8 = (cl4 * cl4);
+    double lB = al * (B + bt * sin(2 * B) + gm * sin(4 * B) + dt * sin(6 * B) + ep * sin(8 * B));
+    double t = tan(B), t2 = (t * t), t4 = (t2 * t2), t6 = (t4 * t2);
+    double Nn = a / sqrt(1 - ee * sin(B) * sin(B));
+    double yt = e2 * cos(B) * cos(B);
+    N = lB;
+    N += t * Nn * cl2 / 2;
+    N += t * Nn * cl4 * (5 - t2 + 9 * yt + 4 * yt * yt) / 24;
+    N += t * Nn * cl6 * (61 - 58 * t2 + t4 + 270 * yt - 330 * t2 * yt) / 720;
+    N += t * Nn * cl8 * (1385 - 3111 * t2 + 543 * t4 - t6) / 40320;
+
+    E = Nn * cl;
+    E += Nn * cl3 * (1 - t2 + yt) / 6;
+    E += Nn * cl5 * (5 - 18 * t2 + t4 + 14 * yt - 58 * t2 * yt) / 120;
+    E += Nn * cl7 * (61 - 479 * t2 + 179 * t4 - t6) / 5040;
+
+    E += 500000;
+    if (nFlag == 1)
+    {
+        //UTM投影
+        N = 0.9996 * N;
+        E = 0.9996 * (E - 500000.0) + 500000.0;//Get y
+    }
+    if (nFlag == 2)
+    {
+        //UTM投影
+        N = 0.9999 * N;
+        E = 0.9999 * (E - 500000.0) + 250000.0;//Get y
+    }
+
+    //原
+    //pt2d.x = N;
+    //pt2d.y = E;
+    //
+    gp.gps_x = E - 280000;
+    gp.gps_y = N - 4325000;
+
+}
+
+
+double iv::decition::GetL0InDegree(double dLIn)
+{
+    //3°带求带号及中央子午线经度(d的形式)
+    //具体公式请参阅《常用大地坐标系及其变换》朱华统，解放军出版社138页
+    double L = dLIn;//d.d
+    double L_ddd_Style = L;
+    double ZoneNumber = (int)((L_ddd_Style - 1.5) / 3.0) + 1;
+    double L0 = ZoneNumber * 3.0;//degree
+    return L0;
+}

src/v2x/v2xTcpClient/transfer.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_DECITION_TRANSFER_
+#define _IV_DECITION_TRANSFER_
+
+#include "gps_type.h"
+#include "decition_type.h"
+#include <vector>
+
+namespace iv {
+    namespace decition {
+        //根据传感器传输来的信息作出决策
+
+        double GetL0InDegree(double dLIn);
+
+        void BLH2XYZ(GPS_INS gps_ins);
+
+        ///大地转车体
+        Point2D Coordinate_Transfer(double x_path, double y_path, GPS_INS pos);
+
+        ///车体转大地
+        GPS_INS Coordinate_UnTransfer(double x_path, double y_path, GPS_INS pos);
+        double GetDistance(Point2D x1, Point2D x2);
+        double GetDistance(GPS_INS p1, GPS_INS p2);
+    }
+}
+
+#endif // ! _IV_DECITION_TRANSFER_

src/v2x/v2xTcpClient/v2xTcpClient.pro

@@ -32,17 +32,43 @@ CONFIG += c++11
 SOURCES += \
         main.cpp \
         mainwindow.cpp \
-        ../../include/msgtype/v2x.pb.cc
+        ../../include/msgtype/v2x.pb.cc \
+        ../../include/msgtype/chassis.pb.cc \
+        ../../include/msgtype/gpsimu.pb.cc \
+        ../../include/msgtype/object.pb.cc \
+        ../../include/msgtype/objectarray.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
+        transfer.cpp \
+    gnss_coordinate_convert.cpp
 
 HEADERS += \
         mainwindow.h \
         data_type.h\
-        ../../include/msgtype/v2x.pb.h
+        ../../include/msgtype/v2x.pb.h \
+        ../../include/msgtype/chassis.pb.h \
+        ../../include/msgtype/gpsimu.pb.h \
+        ../../include/msgtype/object.pb.h \
+        ../../include/msgtype/objectarray.pb.h \
+        ../../include/msgtype/radarobject.pb.h \
+        ../../include/msgtype/radarobjectarray.pb.h \
+        transfer.h \
+        decition_type.h \
+        gps_type.h \
+        boost.h \
+    gnss_coordinate_convert.h
 
 FORMS += \
         mainwindow.ui
 
+win32: INCLUDEPATH += C:/File/boost/boost_1_67_0
+win32: LIBS += -LC:/File/boost/boost_1_67_0/vc2017/lib -lboost_system-vc141-mt-x64-1_67 -lboost_thread-vc141-mt-x64-1_67
+unix:!macx: LIBS += -L$$PWD/./  -lboost_thread -lboost_system
+unix:LIBS += -lboost_thread -lboost_system -lboost_serialization
+
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
 else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
+
+