智能大会 v2x 更新，修复数据长度问题

src/v2x/platform/common/car_status.h

@@ -36,7 +36,7 @@ namespace iv {
         float speed;			//车速
         double mfCalAcc = 0;
         std::int16_t wheel_angle;		//方向盘转角
-        std::uint8_t braking_pressure;	//刹车压力
+        std::u8 braking_pressure;	//刹车压力
         bool mbRunPause = true;
         bool mbBrainCtring = false;
         bool status[6] = { true, false, false, false, true, false };	//bool arrive = false;	//  x4:是否到达站点（0:未到 1:到达）

src/v2x/platform/common/constants.h

@@ -6,14 +6,14 @@
 namespace iv {
 
 
-    const std::uint8_t SPEED_MAX = 30;						//车速上限
-    const std::uint8_t BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
-    const std::uint8_t ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
+    const std::u8 SPEED_MAX = 30;						//车速上限
+    const std::u8 BRAKING_PRESSURE_PERCENT_MAX = 50;	//刹车压力上限
+    const std::u8 ACCELERATE_PERCENT_MAX = 1;			//油门开合度上限
 
-    const std::uint8_t CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
-    const std::uint8_t CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
+    const std::u8 CONTROL_SPEED_TOLERANCE = 5;		//控制车速时容忍误差范围
+    const std::u8 CONTROL_WHEEL_ANGLE_TOLERANCE = 5;	//控制方向盘时容忍误差范围
 
-    const std::uint16_t WHEEL_ZERO = 0x1F4A;
+    const std::u16 WHEEL_ZERO = 0x1F4A;
     //static iv::CarStatus CarStatus_(new iv::CarStatusBasic);
 
     const float SPEED_RATIO = 0.08;		//实际车速和can中读出的车速比率
@@ -25,7 +25,7 @@ namespace iv {
 
     const int Send_Period = 20;   //发送周期
 
-    const std::uint8_t THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
+    const std::u8 THREAD_EXECUTECONTROL_MAXNUM = 1;	//decition_executer 执行线程最大数量
 
 
     const double MaxValue = 1.7976931348623157E+308;  //     Represents the largest possible value of a System.Double. This field is constant.

src/v2x/v2xTcpClientWL/data_type.h

@@ -13,9 +13,9 @@ enum DownID { StopCommand = 1, AutoPilotControl, StationCommand };
 //1目标物数据
 struct ObsTypeData
 {
-    u8 msgSig = 1;
-    u8 msgRate = 1;
-    u8 msgSize = 16; //0x20
+//    u8 msgSig = 1;
+//    u8 msgRate = 10;
+//    u8 msgSize = 16; //0x20
     u8 obsType = 0xFF;
     u8 obsSt = 0xFF;
     u8 hDisH = 0xFF;
@@ -34,12 +34,15 @@ struct ObsTypeData
     u8 obsWigthL = 0xFF;
 };
 //2位置数据 @
+struct DataHead
+{
+    u8 msgSig = 0;
+    u8 msgRate = 0;
+    u8 msgSizeH = 0; //0x8
+    u8 msgSizeL = 0; //0x8
+};
 struct LocationData
 {
-    u8 msgSig = 2;
-    u8 msgRate = 1;
-
-    u8 msgSize = 8; //0x8
     u8 lonH1 = 0;
     u8 lonH0 = 0;
     u8 lonL1 = 0;
@@ -52,9 +55,9 @@ struct LocationData
 //3决策数据 @
 struct DecisionData
 {
-    u8 msgSig = 3;
-    u8 msgRate = 1;
-    u8 msgSize = 22;
+//    u8 msgSig = 3;
+//    u8 msgRate = 10;
+//    u8 msgSize = 23;
     u8 gear = 0;
     u8 accPad = 0xFF;
     u8 brakePad = 0xFF;
@@ -75,22 +78,21 @@ struct DecisionData
     u8 vPowerAdReqL = 0;
     u8 lightAdReqH = 0;
     u8 lightAdReqL = 0;
-    u8 wiperAdReqH = 0;
-    u8 wiperAdReqL = 0;
+    u8 wiperAdReq = 0;
     u8 controllOver = 0;
 };
 //4整车性能数据
 struct VehiclePerfData
 {
-    u8 msgSig = 4;
-    u8 msgRate = 1;
-    u8 msgSize = 16;
+//    u8 msgSig = 4;
+//    u8 msgRate = 10;
+//    u8 msgSize = 16;
     u8 speedH = 0xFF; //@
     u8 speedL = 0xFF; //@
-    u8 hAccH = 0; //@
-    u8 hAccL = 0; //@
+    u8 hAccH = 2; //@
+    u8 hAccL = 0x08; //@
     u8 vAccH = 0; //@
-    u8 vAccL = 20; //@
+    u8 vAccL = 0; //@
     u8 headingH1 = 0;//@
     u8 headingH0 = 0;//@
     u8 headingL1 = 0;//@
@@ -106,9 +108,9 @@ struct VehiclePerfData
 //5整车状态数据
 struct VehicleStData
 {
-    u8 msgSig = 5;
-    u8 msgRate = 1;
-    u8 msgSize = 24;
+//    u8 msgSig = 5;
+//    u8 msgRate = 1;
+//    u8 msgSize = 23;
     u8 powerSt = 0xFF;
     u8 ctrlMode = 0xFF; //@
     u8 driMode = 0xFF;
@@ -124,7 +126,8 @@ struct VehicleStData
     u8 meilageL1 = 0;//@
     u8 meilageL0 = 0;//@
     u8 wiperSt = 0xFF;
-    u8 netSt = 0xFF;
+    u8 netStH = 0xFF;
+    u8 netStL = 0xFF;
     u8 signalSt = 0xFF;
     u8 upSpeedH = 0xF0;
     u8 upSpeedL = 0;
@@ -165,9 +168,9 @@ struct VehicleStData
 //10部件数据
 struct ComponsData
 {
-    u8 msgSig = 0x0A;
-    u8 msgRate = 1;
-    u8 msgSize = 11;
+//    u8 msgSig = 0x0A;
+//    u8 msgRate = 1;
+//    u8 msgSize = 11;
     u8 SRSSt = 0xFF;
     u8 GNSSST = 0xFF;
     u8 IMUSt = 0xFF;
@@ -182,7 +185,7 @@ struct ComponsData
 };
 
 //数据包
-struct UpdatePack
+struct UpdatePackHead
 {
     u8 head1 = 0x23;
     u8 head2 = 0x23;
@@ -195,9 +198,9 @@ struct UpdatePack
 //    std::string msgId = "000000000";
     u8 msgID[9] = {0};
     u8 encryType = 0x01;
-    u8 dataRate = 0;
-    u8 dataSizeL = 0;
+    u8 dataRate = 10;
     u8 dataSizeH = 0;
+    u8 dataSizeL = 0;
 };
 
 #endif // DATA_TYPE_H

src/v2x/v2xTcpClientWL/mainwindow.cpp

@@ -151,7 +151,7 @@ void MainWindow::on_pushButton_connect_clicked()
 
         //获取IP地址
         IP = ui->lineEdit_ip->text();
-        //获取端口号
+        //获取端口号qDebug("%x ",sendBuf);
         port = ui->lineEdit_port->text().toInt();
         ui->textEdit_messages->setText("");
         ui->textEdit_messages->insertPlainText("正在连接"+ui->lineEdit_ip->text()+":"+ui->lineEdit_port->text()+"\n");
@@ -203,6 +203,23 @@ void MainWindow::socket_Read_Data()
 }
 void MainWindow::upVehicleStatus()
 {
+    static int counter = 0;
+    static char *sendBuf = new char[10000];
+    int lenHead = sizeof(UpdatePackHead)/sizeof(unsigned char);
+    int lenLocation = sizeof(LocationData)/sizeof(unsigned char);
+    int lenDecision = sizeof(DecisionData)/sizeof(unsigned char);
+    int lenVehPerfData = sizeof(VehiclePerfData)/sizeof(unsigned char);
+    int lenVehStData = sizeof(VehicleStData)/sizeof(unsigned char);
+    int lenComponsData = sizeof(ComponsData)/sizeof(unsigned char);
+    int lenHeadData = sizeof(DataHead)/sizeof(unsigned char);
+    int lenLocStart = lenHead + lenHeadData;
+    int lenDecStart = lenLocStart + lenHeadData + lenLocation * 10;
+    int lenVPeStart = lenDecStart + lenHeadData + lenDecision * 10;
+    int lenVStStart = lenVPeStart + lenHeadData + lenVehPerfData * 10;
+    int lenComStart = lenVStStart + lenHeadData + lenVehStData;
+    int length = (lenLocation + lenDecision + lenVehPerfData) * 10 + lenVehStData \
+            + lenComponsData + lenHead + lenHeadData * 5;
+
     QByteArray time_stamp=getTimeStamp().toLatin1();
     memcpy(upPack.timeStamp, time_stamp.data(), sizeof(upPack.timeStamp));
     int tmp = mflon * 10000000;
@@ -218,61 +235,92 @@ void MainWindow::upVehicleStatus()
     tmp = (int)(mfEpsAngle/500.0 + 250);
     mdecision.epsAngleL = tmp & 0xFF;
     mdecision.epsAngleH = tmp>>8 & 0xFF;
-    tmp = (int)mfspeed * 100;
+    tmp = (int)mfspeed;
     mvehPerfData.speedL = tmp & 0xFF;
     mvehPerfData.speedH = tmp>>8 & 0xFF;
-    tmp = (int)(mfAcc * 100 / 980);
-    mvehPerfData.hAccL = tmp & 0xFF;
-    mvehPerfData.hAccH = tmp>>8 & 0xFF;
-    tmp = -1 * mvehPerfData.vAccL;
-    mvehPerfData.vAccL = tmp & 0xFF; //临时
-    mvehPerfData.vAccH = tmp>>8 & 0xFF; //临时
+
+    tmp = (int)(mfAccH) * 100 % 490 + 490;
+    mvehPerfData.vAccL = tmp & 0xFF;
+    mvehPerfData.vAccH = tmp>>8 & 0xFF;
+    tmp = (int)(mfAccV) * 100 % 490 + 490;
+    mvehPerfData.hAccL = tmp & 0xFF; //临时
+    mvehPerfData.hAccH = tmp>>8 & 0xFF; //临时
     tmp = (int)mfheading*80;
     mvehPerfData.headingL0 = tmp & 0xFF;
     mvehPerfData.headingL1 = tmp>>8 & 0xFF;
     mvehPerfData.headingH0 = tmp>>16 & 0xFF;
     mvehPerfData.headingH1 = tmp>>24 & 0xFF;
-    mvehStData.ctrlMode = 1;
-    mvehStData.meilageL0 = 0xE3;
-    mvehStData.meilageL1 = 0xFF;
-    mvehStData.meilageH0 = 0;
-    mvehStData.meilageH1 = 0;
-    mcomponsData.ADSysSt = 1;
-//    char str[10000];
-//    memcpy(str,mlocation,sizeof(LocationData));
-    char *sendBuf = new char[1000];
-    int length = 0;
-    int lenHead = sizeof(UpdatePack)/sizeof(unsigned char);
-    int lenLocation = sizeof(mlocation)/sizeof(unsigned char);
-    int lenDecision = sizeof(mdecision)/sizeof(unsigned char);
-    int lenVehPerfData = sizeof(mvehPerfData)/sizeof(unsigned char);
-    int lenVehStData = sizeof(mvehStData)/sizeof(unsigned char);
-    int lenComponsData = sizeof(mcomponsData)/sizeof(unsigned char);
-    length = lenLocation + lenDecision + lenVehPerfData + lenVehStData +lenComponsData;
-    upPack.dataSizeH = length>>8 & 0xFF;
-    upPack.dataSizeL = length & 0xFF;
-    memcpy(sendBuf,&upPack,lenHead);
-    length = lenHead;
-    memcpy(sendBuf+length,&mlocation,lenLocation);
-    length = length + lenLocation;
-    memcpy(sendBuf+length,&mdecision,lenDecision);
-    length = length + lenDecision;
-    memcpy(sendBuf+length,&mvehPerfData,lenVehPerfData);
-    length = length + lenVehPerfData;
-    memcpy(sendBuf+length,&mvehStData,lenVehStData);
-    length = length + lenVehStData;
-    memcpy(sendBuf+length,&mcomponsData,lenComponsData);
-    length = length + lenComponsData;
-
-    socket->write(sendBuf,length);
-    if(!socket->waitForBytesWritten(3000))
+    if(counter == 0)
     {
-        qDebug()<<"realtimeinfo send error";
+        upPack.dataSizeH = (length - lenHead)>>8 & 0xFF;
+        upPack.dataSizeL = (length - lenHead)& 0xFF;
+        memcpy(sendBuf,&upPack,lenHead);
+        DataHead head;
+        int dataSize = 0;
+
+        dataSize = 80;
+        head.msgSig = 2;
+        head.msgRate = 10;
+        head.msgSizeH = dataSize >> 8 & 0xFF;
+        head.msgSizeL = dataSize & 0xFF;
+        memcpy(sendBuf + lenLocStart - lenHeadData,&head, lenHeadData);
+
+        dataSize = 220;
+        head.msgSig = 3;
+        head.msgRate = 10;
+        head.msgSizeH = dataSize >> 8 & 0xFF;
+        head.msgSizeL = dataSize & 0xFF;
+        memcpy(sendBuf + lenDecStart - lenHeadData,&head, lenHeadData);
+
+        dataSize = 160;
+        head.msgSig = 4;
+        head.msgRate = 10;
+        head.msgSizeH = dataSize >> 8 & 0xFF;
+        head.msgSizeL = dataSize & 0xFF;
+        memcpy(sendBuf + lenVPeStart - lenHeadData,&head, lenHeadData);
+
+        dataSize = 24;
+        head.msgSig = 5;
+        head.msgRate = 1;
+        head.msgSizeH = dataSize >> 8 & 0xFF;
+        head.msgSizeL = dataSize & 0xFF;
+        memcpy(sendBuf + lenVStStart - lenHeadData,&head, lenHeadData);
+
+        dataSize = 11;
+        head.msgSig = 10;
+        head.msgRate = 1;
+        head.msgSizeH = dataSize >> 8 & 0xFF;
+        head.msgSizeL = dataSize & 0xFF;
+        memcpy(sendBuf + lenComStart - lenHeadData, &head, lenHeadData);
+    }
+
+    memcpy(sendBuf+lenLocStart + lenLocation * counter, &mlocation,lenLocation);
+    memcpy(sendBuf+lenDecStart + lenDecision * counter,&mdecision,lenDecision);
+    memcpy(sendBuf+lenVPeStart + lenVehPerfData * counter,&mvehPerfData,lenVehPerfData);
+    counter++;
+    if(counter == 10)
+    {
+        mvehStData.ctrlMode = 1;
+        mvehStData.meilageL0 = 0xE3;
+        mvehStData.meilageL1 = 0xFF;
+        mvehStData.meilageH0 = 0;
+        mvehStData.meilageH1 = 0;
+        mcomponsData.ADSysSt = 1;
+        memcpy(sendBuf+lenVStStart, &mvehStData, lenVehStData);
+        memcpy(sendBuf+lenComStart, &mcomponsData, lenComponsData);
+        socket->write(sendBuf,length);
+        if(!socket->waitForBytesWritten(3000))
+        {
+            qDebug()<<"realtimeinfo send error";
+        }
+        qDebug()<<QDateTime::currentDateTimeUtc() <<"lon"<<QString::number(mflon,10,7) \
+              <<"lat"<<QString::number(mflat,10,7)<<"eps"<<mfEpsAngle <<"speed"<<mfspeed \
+              <<"Hacc"<<mfAccH<<"Vacc"<<mfAccV<<"heading"<<mfheading<<"length" \
+              <<length<<"dataSize"<<length-lenHead;
+
+        counter = 0;
     }
-    //debug
 
-    qDebug()<<QDateTime::currentDateTimeUtc() <<"lon"<<mflon<<"lat"<<mflat<<"eps"<<mfEpsAngle \
-           <<"speed"<<mfspeed<<"acc"<<mfAcc<<"heading"<<mfheading;
 }
 
 
@@ -307,7 +355,7 @@ void MainWindow::UpdateChassis(const char *strdata, const unsigned int nSize, co
 void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,\
                               const QDateTime * dt,const char * strmemname)
 {
-
+    double accX,accY;
     iv::gps::gpsimu  xgpsimu;
     if(!xgpsimu.ParseFromArray(strdata,nSize))
     {
@@ -326,6 +374,15 @@ void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,cons
     mpdata.vel_D = xgpsimu.vd();    //地向速度,单位(米/秒)
     mpdata.vel_E = xgpsimu.ve();    //东向速度,单位(米/秒)
     mpdata.vel_N = xgpsimu.vn();    //北向速度,单位(米/秒)
+    if(xgpsimu.has_acce_x())
+    {
+        mfAccH = xgpsimu.acce_x();
+    }
+    if(xgpsimu.has_acce_y())
+    {
+        mfAccV = xgpsimu.acce_y();
+    }
+//     mfAcc = (float)sqrt(pow(accX,2)+pow(accY,2));
 
  //   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;

src/v2x/v2xTcpClientWL/mainwindow.h

@@ -26,6 +26,7 @@
 #include <QTimer>
 #include <iostream>
 #include <QMutex>
+#include <math.h>
 namespace Ui {
 class MainWindow;
 }
@@ -112,12 +113,14 @@ private:
     float mfEpsAngle = 0;
     int miSOC = 0;
     float mfAcc=0;
+    double mfAccH = 0;//水平加速度
+    double mfAccV = 0;
     int micarError = 0;
     int mierrorNum = 0;
     double mflat = 0;//经度
     double mflon = 0;//纬度
     double mfheading = 0;//航向
-    UpdatePack upPack;
+    UpdatePackHead upPack;
     LocationData mlocation;
     DecisionData mdecision;
     VehiclePerfData mvehPerfData;