change detection_chassis. add Changan Shenlan Support.

src/controller/controller_changan_shenlan/main.cpp

@@ -41,6 +41,9 @@ int gnDecitionNum = 0; //when is zero,send default;
 const int gnDecitionNumMax = 100;
 int gnIndex = 0;
 
+static bool gbHaveVehSpd = false;
+static double gfVehSpd = 0.0;
+
 boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 static QMutex gMutex;
@@ -119,7 +122,7 @@ void ExecSend();
 void executeDecition(const iv::brain::decition &decition)
 {
 
-    static int xieya = 0;
+    static int xieya = 50;
 //     std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
 //     std::cout<<"angle_mode is "<<decition.angle_mode()<<" angle_active is "<<decition.angle_active()<<std::endl;
 //     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
@@ -144,7 +147,7 @@ void executeDecition(const iv::brain::decition &decition)
          _m24B.ACC_DecToStop =1;
     }
 
-    std::cout<<" type: "<<(int)_m144.ACC_ADCReqType<<std::endl;
+//    std::cout<<" type: "<<(int)_m144.ACC_ADCReqType<<std::endl;
 
 
     /*制动过程用的减速度，加速用扭矩*/
@@ -158,8 +161,8 @@ void executeDecition(const iv::brain::decition &decition)
         _m24B.ACC_ACCTargetAcceleration = ECU_24B_ACC_ACCTargetAcceleration_toS(decition.brake());
 
 
-    std::cout<<" brake : "<<decition.brake()<<std::endl;
-    std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
+ //   std::cout<<" brake : "<<decition.brake()<<std::endl;
+ //   std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
     if(decition.brake()>(-0.0000001))
     {
 
@@ -174,6 +177,7 @@ void executeDecition(const iv::brain::decition &decition)
             _m24B.ADCReqMode = 1;
              _m24B.ACC_DecToStop =1;
             xieya--;
+            std::cout<<" xieya. now veh speed: "<<gfVehSpd<<std::endl;
         }
         else
         {
@@ -187,7 +191,8 @@ void executeDecition(const iv::brain::decition &decition)
         _m24B.ACC_CDDActive = 1;
 
         _m24B.ACC_Driveoff_Request = 0;
-        xieya = 50;
+
+
     }
  //       _m24B.ACC_CDDActive = decition.brake_active();
 
@@ -195,7 +200,7 @@ void executeDecition(const iv::brain::decition &decition)
 
 //    std::cout<<" req mode: "<<_m144.ADS_Reqmode<<std::endl;
     byte_144[0] = ((_m144.ACC_LatAngReq >> 9) & (0x1FU)) | ((_m144.ADS_Reqmode & (0x07U)) << 5);
-    qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
+ //   qDebug(" req mode: %d byte: %02X  ",_m144.ADS_Reqmode, byte_144[0]);
     byte_144[1] = ((_m144.ACC_LatAngReq >> 1) & (0xFFU));
     byte_144[2] = ((_m144.ACC_LatAngReq & (0x01U)) << 7) | ((_m144.ACC_MotorTorqueMaxLimitRequest >> 5) & (0x3FU)) | ((_m144.ACC_LatAngReqActive & (0x01U)) << 6);
     byte_144[3] = ((_m144.ACC_MotorTorqueMaxLimitRequest & (0x1FU)) << 3) | ((_m144.ACC_MotorTorqueMinLimitRequest >> 8) & (0x07U));
@@ -208,8 +213,8 @@ void executeDecition(const iv::brain::decition &decition)
     //    _m24B.ACC_AEBActive = 0;
     //    _m24B.ACC_Driveoff_Request = 0;
     //    _m24B.ACC_DecToStop = 0;
-    std::cout<<" brake : "<<decition.brake()<<std::endl;
-    std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
+//    std::cout<<" brake : "<<decition.brake()<<std::endl;
+//    std::cout<<"brake acctive. "<<decition.brake_active()<<std::endl;
     if(decition.brake()>(-0.0000001))
     {
         _m24B.ACC_CDDActive = 0;
@@ -229,7 +234,16 @@ void executeDecition(const iv::brain::decition &decition)
         _m24B.ACC_CDDActive = 1;
 
         _m24B.ACC_Driveoff_Request = 0;
-        xieya = 50;
+
+        if(gbHaveVehSpd)
+        {
+            if(gfVehSpd < 0.01)
+            {
+                if(xieya != 50)std::cout<<"need xieya. "<<std::endl;
+                xieya = 50;
+
+            }
+        }
     }
  //       _m24B.ACC_CDDActive = decition.brake_active();
     _m24B.ACC_ACCMode = decition.auto_mode();
@@ -332,10 +346,20 @@ void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned
         return;
     }
 
+    if(xchassis.has_epsmode())
+    {
     if(xchassis.epsmode() == 0)
     {
         gbChassisEPS = true;
     }
+    }
+
+    if(xchassis.has_vel())
+    {
+        gfVehSpd = xchassis.vel();
+        gbHaveVehSpd = true;
+  //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
+    }
 }
 
 
@@ -390,8 +414,8 @@ void ExecSend()
     xraw.set_len(8);
     iv::can::canraw * pxraw144 = xmsg.add_rawmsg();
     pxraw144->CopyFrom(xraw);
-    qDebug(" 0x144: %02X %02X %02X %02X %02X %02X %02X %02X",byte_144[0],byte_144[1],byte_144[2],byte_144[3],
-            byte_144[4],byte_144[5],byte_144[6],byte_144[7]);
+//    qDebug(" 0x144: %02X %02X %02X %02X %02X %02X %02X %02X",byte_144[0],byte_144[1],byte_144[2],byte_144[3],
+//            byte_144[4],byte_144[5],byte_144[6],byte_144[7]);
     xmsg.set_channel(0);
     xmsg.set_index(gnIndex);
 

src/detection/detection_chassis/decodechassis.cpp

@@ -447,6 +447,7 @@ int ProcShenLanChassis(void *pa, iv::can::canmsg *pmsg)
         {
             bHave0x147 = false;
             ShareChassis(pa,&xchassis);
+            std::cout<<"veh: "<<xchassis.vel()<<std::endl;
         }
 
 //        if(ghave0x13&&ghave0x14&&ghave0x15)

src/detection/detection_chassis/decodechassis.h

@@ -18,7 +18,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp \
-    hcp2.cpp
+    hcp2.cpp \
+    ../../common/common/math/gnss_coordinate_convert.cpp
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -30,6 +31,10 @@ SOURCES += main.cpp \
 }
 
 HEADERS += \
-    hcp2.h
+    hcp2.h \
+    ../../common/common/math/gnss_coordinate_convert.h
+
+
+INCLUDEPATH += $$PWD/../../common/common/math
 
 LIBS += -livprotoif

src/driver/driver_gps_hcp2/hcp2.cpp

@@ -1,5 +1,7 @@
 #include "hcp2.h"
 
+#include "gnss_coordinate_convert.h"
+
 extern iv::Ivlog * givlog;
 extern iv::Ivfault *gfault;
 
@@ -42,8 +44,10 @@ static bool checknmeasen(const char * strsen,const unsigned int nlen)
 }
 
 hcp2::hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                         const char * strmsgimu)
+                         const char * strmsgimu,double fmovex,double fmovey)
 {
+    mfmovex = fmovex;
+    mfmovey = fmovey;
     mTime.start();
     mfCalc_acc = 0;
     mfOldVel = 0;
@@ -591,6 +595,12 @@ void hcp2::SerialGPSDecodeSen(QString strsen)
    }
 
    iv::gps::gpsimu gpsimu;
+
+   if((fabs(mfmovex)>0.0000001)||(fabs(mfmovey)>0.0000001))
+   {
+       MoveLonLat(fLon,fLat,fheading,mfmovex,mfmovey);
+   }
+
    gpsimu.set_vd(vu);
    gpsimu.set_ve(ve);
    gpsimu.set_vn(vn);
@@ -708,3 +718,15 @@ void hcp2::BroadCastData()
 
     gfault->SetFaultState(1, 0, "ok");
 }
+
+void hcp2::MoveLonLat(double & flon,double & flat,const double fHeading,const double fMoveX,const double fMoveY)
+{
+    double fHdg = (90-fHeading)*M_PI/180.0;
+    double x,y;
+    GaussProjCal(flon,flat,&x,&y);
+    double xnew,ynew;
+    xnew = x+ fMoveY *cos(fHdg+ M_PI/2.0) + fMoveX*cos(fHdg);
+    ynew = y + fMoveY *sin(fHdg + M_PI/2.0) + fMoveX*sin(fHdg);
+    GaussProjInvCal(xnew,ynew,&flon,&flat);
+}
+

src/driver/driver_gps_hcp2/hcp2.h

@@ -35,7 +35,7 @@ class hcp2 : public QThread
     Q_OBJECT
 public:
     hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                const char * strmsgimu);
+                const char * strmsgimu,double fmovex,double fmovey);
 
 private slots:
     void onTimer();
@@ -43,6 +43,8 @@ private slots:
 private:
     void run();
 
+    void MoveLonLat(double & flon,double & flat,const double fHeading,const double fMoveX,const double fMoveY);
+
 private:
     QByteArray mbaGPSBuf;
     bool mbSerialOpen;
@@ -68,6 +70,9 @@ private:
 
     int mngpscount;
 
+    double mfmovex = 0;
+    double mfmovey = 0;
+
 private:
 
     double mhdtheading;

src/driver/driver_gps_hcp2/main.cpp

@@ -28,7 +28,11 @@ int main(int argc, char *argv[])
     std::string strgpsimumemname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
     std::string strgpsmemname = xp.GetParam("msg_gps","hcp2_gps");
     std::string strimumemname = xp.GetParam("msg_imu","hcp2_imu");
-    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data());
+    std::string strmovex = xp.GetParam("movefront","0.0");
+    std::string strmovey = xp.GetParam("moveleft","0");
+    double fmovex = atof(strmovex.data());
+    double fmovey = atof(strmovey.data());
+    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data(),fmovex,fmovey);
     x.start();
     return a.exec();
 }

src/ui/ADCIntelligentShow_grpc/adcintelligentshow.cpp

@@ -953,7 +953,7 @@ void ADCIntelligentShow::onUpdateGPSIMU()
         ui->local->setText(QStringLiteral("定位：")+QString::number(xgpsimu.rtk_state()));
         if(mnstylegps != 1)
         {
-            ui->local->setStyleSheet("color: rgb(220, 220, 220); font: 15pt 'Arial';");
+            ui->local->setStyleSheet("color: rgb(220, 220, 220); font: 30px 'Arial';");
             ui->localimg->setStyleSheet("background-image: url(:/new/pic/green.png);");
             mnstylegps = 1;
         }
@@ -963,7 +963,7 @@ void ADCIntelligentShow::onUpdateGPSIMU()
         ui->local->setText(QStringLiteral("定位：")+QString::number(xgpsimu.rtk_state()));
         if(mnstylegps != 2)
         {
-            ui->local->setStyleSheet("color: rgb(220, 220, 220); font: 15pt 'Arial';");
+            ui->local->setStyleSheet("color: rgb(220, 220, 220); font: 30px 'Arial';");
             ui->localimg->setStyleSheet("background-image: url(:/new/pic/red.png);");
             mnstylegps = 2;
         }
@@ -1146,6 +1146,7 @@ void ADCIntelligentShow::on_pushButton_go_clicked()
 {
     if(gvectorpos.size()<1)return;
 
+    qDebug("set plan.lon:%f lat:%f index:%d",gvectorpos[mnStationIndex].mflon,gvectorpos[mnStationIndex].mflat,mnStationIndex);
     xodrobj xo;
     xo.flon = gvectorpos[mnStationIndex].mflon;
     xo.flat = gvectorpos[mnStationIndex].mflat;

src/ui/ADCIntelligentShow_grpc/gps_nbtype.h

@@ -13,6 +13,92 @@ namespace iv {
         double gps_y = 0;
         double gps_z = 0;
 
+        double frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=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 = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        int roadMode;
+        int runMode;
+        int roadSum;
+        int roadOri;
+
+        double mfLaneWidth = 3.5; // Current Lane Width
+
+        double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+        int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+        double mfDisToRoadLeft = 1.8; //Distance to Road Left
+        double mfRoadWidth = 3.5; // Road Width
+
+        bool mbInLaneAvoid = false; //if true suport In Lane Avoid
+        double gps_lat_avoidleft;
+        double gps_lng_avoidleft;
+        double gps_lat_avoidright;
+        double gps_lng_avoidright;
+        double gps_x_avoidleft = 0;
+        double gps_y_avoidleft = 0;
+        double gps_x_avoidright = 0;
+        double gps_y_avoidright = 0;
+
+        bool mbnoavoid = false; //If true this point is no avoid.
+        double mfCurvature = 0.0;
+
+        char mcreserved[10];
+        int mnreserved[5];
+        double mfreserved[2];
+
+
+
+    };
+
+    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 frenet_s=0;
+        double frenet_s_dot=0;
+        double frenet_s_dot_dot=0;
+        double frenet_d=0;
+        double frenet_d_dot=0;
+        double frenet_d_dot_dot=0;
+
         double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
         double ins_pitch_angle = 0;	//俯仰角
         double ins_heading_angle = 0;	//航向角
@@ -72,5 +158,6 @@ namespace iv {
 
 
     };
+
 }
 #endif // GPS_NBTYPE_H

src/ui/ADCIntelligentShow_grpc/ivmapview.cpp

@@ -5,19 +5,19 @@
 
 #include <math.h>
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-static void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
     int ProjNo = 0; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, X0, Y0, xval, yval;
+    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年西安坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
     ProjNo = (int)(longitude / ZoneWide);
     longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
     longitude0 = longitude0 * iPI;
-    //latitude0 = 0;
+    latitude0 = 0;
     longitude1 = longitude * iPI; //经度转换为弧度
     latitude1 = latitude * iPI; //纬度转换为弧度
     e2 = 2 * f - f * f;
@@ -26,13 +26,12 @@ static void GaussProjCal(double longitude, double latitude, double *X, double *Y
     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));
+    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);
+        + (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;
@@ -40,6 +39,8 @@ static void GaussProjCal(double longitude, double latitude, double *X, double *Y
     *Y = yval;
 }
 
+
+
 ivmapview::ivmapview()
 {
     image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300，使用32位颜色
@@ -150,6 +151,7 @@ void ivmapview::paint()
         double k1, k2;
 
 
+  //      qDebug("y: %f ",xnavigation_data[1].gps_y);
         //路径点的预处理
         for (int i = 0; i < sizeN; i++)
         {
@@ -196,9 +198,12 @@ void ivmapview::paint()
 //            lng[0] = DataUI->mInfo.gps_inshead;
 //        }
         GaussProjCal(xgpsimu.lon(), xgpsimu.lat(), &gps_x, &gps_y);
+ //       qDebug("now lon:%f lat:%f %f %f ",xgpsimu.lon(),xgpsimu.lat(),xnavigation_data[0].gps_lng,xnavigation_data[0].gps_lat);
         x0[0] = gps_x;
         y0[0] = gps_y;
         lng[0] = xgpsimu.heading();
+
+//        qDebug("%f %f %f %f",xnavigation_data[1].gps_x, xnavigation_data[1].gps_y,xnavigation_data[0].gps_lng,xnavigation_data[0].gps_lat);
         //根据标定原点的选取，对路径点进行转化
         for (int i = 1; i < sizeN; i++)
         {
@@ -216,6 +221,8 @@ void ivmapview::paint()
             y2[i] = y0[i] - k2 * 5;
         }
 
+
+
         double kx = (double)(600) / (abs(y_max - y_min));//x轴的系数
         double ky = (double)(600) / (abs(y_max - y_min));//y方向的比例系数
 
@@ -357,8 +364,19 @@ int ivmapview::GetType()
 
 void ivmapview::setmap(std::vector<iv::MAP_GPS_INS> xnavigation_data)
 {
+    unsigned int i;
+    for(i=0;i<xnavigation_data.size();i++)
+    {
+        double x,y;
+        GaussProjCal(xnavigation_data[i].gps_lng,xnavigation_data[i].gps_lat,&x,&y);
+        xnavigation_data[i].gps_x = x;
+        xnavigation_data[i].gps_y = y;
+ //       qDebug("x %f y %f ",xnavigation_data[i].gps_lng,xnavigation_data[i].gps_lat);
+    }
     mMutexMap.lock();
     mnavigation_data = xnavigation_data;
+    mnTimeGPSUpdate = QDateTime::currentMSecsSinceEpoch();
+    qDebug("map updata.");
     mMutexMap.unlock();
 }
 

src/ui/ADCIntelligentShow_grpc/main.cpp

@@ -1,3 +1,4 @@
+
 #include "adcintelligentshow.h"
 #include <QApplication>
 #include <QDebug>
@@ -93,14 +94,17 @@ void LoadPos(std::string strfilepath)
    //         QList<QByteArray> badata = baline[i].split('\t');
             QStringList badata = x.split(QRegExp("[\t ,;]"));
 
+            qDebug("size: %d ",badata.size());
             if(badata.size()>=3)
             {
                 iv::pos_def xposdef;
                 xposdef.mstrstationname = badata[0].toStdString();
                 xposdef.mflon = badata[1].toDouble();
                 xposdef.mflat = badata[2].toDouble();
+                qDebug("data1 : %s  string: %s",badata[2].toLatin1().data(),x.toLatin1().data());
                 gvectorpos.push_back(xposdef);
 
+
             }
 
         }