add driver_lidar_rs16p.

src/driver/driver_lidar_leishen32/driver_lidar_leishen32.pro

@@ -18,14 +18,14 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp \
-    lidarudp.cpp \
     leishen32.cpp \
-    lidardecode.cpp
+    ../common/lidar/lidardecode.cpp \
+    ../common/lidar/lidarudp.cpp
 
 HEADERS += \
-    lidarudp.h \
     leishen32.h \
-    lidardecode.h
+    ../common/lidar/lidardecode.h \
+    ../common/lidar/lidarudp.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -38,3 +38,5 @@ HEADERS += \
 !include(../../../include/ivyaml-cpp.pri ) {
     error( "Couldn't find the ivyaml-cpp.pri file!" )
 }
+
+INCLUDEPATH += $$PWD/../common/lidar

src/driver/driver_lidar_rs16p/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_rs16p/driver_lidar_rs16p.pro

@@ -0,0 +1,41 @@
+QT -= gui
+
+QT += network
+
+CONFIG += c++14 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    ../common/lidar/lidardecode.cpp \
+    ../common/lidar/lidarudp.cpp \
+    rs16p.cpp
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../common/lidar
+
+HEADERS += \
+    ../common/lidar/lidardecode.h \
+    ../common/lidar/lidarudp.h \
+    rs16p.h

src/driver/driver_lidar_rs16p/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}

src/driver/driver_lidar_rs16p/rs16p.cpp

@@ -0,0 +1,217 @@
+#include "rs16p.h"
+
+rs16p::rs16p(char * strmemname, double froll ,double finclinationang_xaxis ,
+             double finclinationang_yaxis , unsigned short nDataPort )
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+
+    RegisterSend(strmemname);
+    mfrollang = froll;
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+
+    mprs16p_vertical = &rs16p_vertical[0];
+
+    int i;
+    for(i=0;i<16;i++)
+    {
+        mthetacos[i] = cos(mprs16p_vertical[i]*M_PI/180.0);
+        mthetasin[i] = sin(mprs16p_vertical[i]*M_PI/180.0);
+        std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
+    }
+
+    mplidarudp = new lidarudp(nDataPort);
+    mpthread = new std::thread(&rs16p::threaddecode,this);
+}
+
+rs16p::~rs16p()
+{
+    mbrun = false;
+    mpthread->join();
+    delete mplidarudp;
+}
+
+void rs16p::threaddecode( )
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidarudp->ComsumeRecv(10);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeUDPPac(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1206)
+    {
+        std::cout<<" DecodeUDPPac: "<<" pac size is not 1206.  pac size: "<<xpac.mndatasize<<std::endl;
+        return  -1;
+    }
+
+    double finclinationang_xaxis = mfinclinationang_xaxis * M_PI/180.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = mfinclinationang_yaxis * M_PI/180.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    bool binclix = false;
+    bool bincliy = false;
+    if(fabs(finclinationang_xaxis)>0.00000001)binclix = true;
+    if(fabs(finclinationang_yaxis)>0.00000001)bincliy = true;
+
+    float cos_finclinationang_xaxis = static_cast<float>(cos(finclinationang_xaxis)) ;
+    float sin_finclinationang_xaxis = static_cast<float>(sin(finclinationang_xaxis));
+    float cos_finclinationang_yaxis = static_cast<float>(cos(finclinationang_yaxis));
+    float sin_finclinationang_yaxis = static_cast<float>(sin(finclinationang_yaxis));
+
+    double frollang = mfrollang *M_PI/180.0;
+
+    char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
+
+    static double gAngleTotal = 0;
+    static double gAngleOld = 0;
+    static unsigned int g_seq = 1;
+
+
+
+    int i;
+    for(i=0;i<12;i++)
+    {
+        char * pstrpac = pstr+i*100;
+        unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
+        double fazu = *pazu;
+        fazu = fazu * 0.01;
+
+        pcl::PointXYZI point;
+ //       std::cout<<" fazu: "<<fazu<<std::endl;
+
+
+        if(fabs(fazu-gAngleOld)>=300)
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld)-360);
+ //           std::cout<<" add : "<<fabs(fabs(fazu-gAngleOld)-360)<<std::endl;
+        }
+        else
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld));
+        }
+
+        gAngleOld = fazu;
+
+        fazu = fazu * M_PI/180.0;
+
+        if(gAngleTotal>=360.0)
+        {
+            gAngleTotal = 0.0;
+            std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
+            ProduceCloud(mpoint_cloud_temp);
+            std::cout<<" produce. "<<std::endl;
+            //share point cloud.
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                        new pcl::PointCloud<pcl::PointXYZI>());
+
+            point_cloud->header.frame_id = "velodyne";
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+            point_cloud->width = 0;
+            point_cloud->header.seq =g_seq;
+            g_seq++;
+            mpoint_cloud_temp = point_cloud;
+        }
+
+        int j;
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
+            Range=Range* 0.0025;
+
+
+            pcl::PointXYZI point;
+ //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu)) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu ));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float y,z;
+                y = point.y;z = point.z;
+                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+            }
+            if(bincliy)
+            {
+                float z,x;
+                z = point.z;x = point.x;
+                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+        fazu = fazu + 0.18 * M_PI/180.0;
+
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1 + 48] << 8) + pstrpac[4+j*3+0 + 48]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2 + 48]) ;
+            Range=Range* 0.0025;
+
+
+            pcl::PointXYZI point;
+ //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float y,z;
+                y = point.y;z = point.z;
+                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+            }
+            if(bincliy)
+            {
+                float z,x;
+                z = point.z;x = point.x;
+                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+
+    }
+
+    return  0;
+}

src/driver/driver_lidar_rs16p/rs16p.h

@@ -0,0 +1,53 @@
+#ifndef RS16P_H
+#define RS16P_H
+
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+
+#include "lidarudp.h"
+
+#include "lidardecode.h"
+
+class rs16p : public lidardecode
+{
+public:
+    rs16p(char * strmemname, double froll = 0.0,double finclinationang_xaxis = 0.0,
+          double finclinationang_yaxis = 0.0, unsigned short nDataPort = 6699);
+    ~rs16p();
+
+private:
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+
+    const double * mprs16p_vertical;
+    double  mprs16p_A[16];
+
+    double mthetacos[16];
+    double mthetasin[16];
+
+    std::thread * mpthread;
+    lidarudp * mplidarudp;
+
+    bool mbrun = true;
+
+    double mfinclinationang_xaxis = 0.0;
+    double mfinclinationang_yaxis = 0.0;
+    double mfrollang = 0.0;
+
+private:
+    const double rs16p_vertical[16]={13,15,9,11,5,7,1,3,
+                                    -3,-1,-7,-5,-11,-9,-15,-13};
+
+private:
+    void threaddecode( );
+    int DecodeUDPPac(iv::lidarudppac & xpac);
+};
+
+#endif // RS16P_H

src/driver/driver_service_maintain/Readme.md

@@ -1,4 +1,4 @@
 driver_service_maintain用来售后维护。
 使用方法，要结合tool_service_maintain来进行。
-driver_service_maintain需要机器上安装上nomachine，并在/home/nvidia/frp目录防止穿透工具frp的frpc程序。
+driver_service_maintain需要机器上安装上nomachine，并在/home/nvidia/frp目录放上穿透工具frp的frpc程序。
 用户连接车辆的wifi，打开tool_service_maintain会获得一个服务码，将这个服务码告诉售后工程师，售后工程师使用nomachine连接到车辆上的计算单元解决ota不能解决的问题。

src1/decision/common/adc_decision_function/ivpark_simple.cpp

@@ -123,9 +123,16 @@ bool ivpark_simple::IsBocheEnable(double fLon, double fLat, double fHeading)
     return true;
 }
 
+double ivpark_simple::GetDistance(iv::GPS_INS p1,iv::GPS_INS p2)
+{
+    return sqrt(pow(p1.gps_x - p2.gps_x,2)+pow(p1.gps_y-p2.gps_y,2));
+}
+
+
 
 int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate,bool bbocheMode)
 {
+    (void)fHeading;
     if(bbocheMode == false)
     {
         return 0; //Not in boche mode
@@ -195,6 +202,8 @@ int ivpark_simple::GetBocheDecision(double fLon,double fLat,double fHeading,doub
 
     }
 
+    return 0;
+
 }
 
 void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
@@ -239,9 +248,10 @@ void ivpark_simple::reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fA
 void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
 {
 
+    (void)fSpeed;
     static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
 
-    int64_t nstoptime_ms = 1000; //every stop time is 3
+    int64_t nstoptime_ms = 3000; //every stop time is 3
 
     if(mlastvehstate != xvehstate)
     {
@@ -292,9 +302,10 @@ void ivpark_simple::reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & f
 
 void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
 {
+    (void)fSpeed;
     static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
 
-    int64_t nstoptime_ms = 1000; //every stop time is 3
+    int64_t nstoptime_ms = 3000; //every stop time is 3
 
     if(mlastvehstate != xvehstate)
     {
@@ -341,9 +352,11 @@ void ivpark_simple::reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & f
 
 void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
 {
+    (void)nowgps;
+    (void)fSpeed;
     static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
 
-    int64_t nstoptime_ms = 1000; //every stop time is 3
+    int64_t nstoptime_ms = 3000; //every stop time is 3
 
     if(mlastvehstate != xvehstate)
     {
@@ -374,5 +387,228 @@ void ivpark_simple::reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc
     fAcc = -0.5;
     fWheel = 0.0;
 
-    xvehstate = reverseComplete;
+    xvehstate = normalRun;
 }
+
+void ivpark_simple::dRever0Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+
+    mlastvehstate = xvehstate;
+
+    Point2D pt = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+
+    Point2D ptnear = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
+
+    double fdistonear = fabs(pt.y - ptnear.y);
+
+    nshift = 2;
+    if(fdistonear>1.0)
+    {
+        fAcc = 0.0;  //acc calcutale by pid
+        fWheel = 0.0;
+        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
+
+    }
+    else
+    {
+        if((fSpeed>0.3)&&(fdistonear>0.3))
+        {
+            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdistonear);
+            fdSpeed = 0.0;
+            fdSecSpeed = fdSpeed/3.6;
+        }
+        else
+        {
+                fAcc = -0.5;
+                xvehstate = dRever2;
+                fdSpeed = 0.0;
+                fdSecSpeed = fdSpeed/3.6;
+        }
+    }
+
+
+
+    return;
+}
+
+void ivpark_simple::dRever1Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+    (void)fSpeed;
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 3000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = iv::decition::Compute00().bocheAngle*16.5 *(-1.0);
+        mCircleWheel = fWheel;
+        return;
+
+    }
+
+    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint1.gps_x,iv::decition::Compute00().dTpoint1.gps_y, maimgps);
+
+    double fdis = fabs(pt1.x - pt2.x);
+
+    if(fdis>1.0)
+    {
+        fAcc = 0.0;  //acc calcutale by pid
+
+        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
+    }
+    else
+    {
+        if((fSpeed>0.3)&&(fdis>0.3))
+        {
+            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fdis);
+            fdSpeed = 0.0;
+            fdSecSpeed = fdSpeed/3.6;
+        }
+        else
+        {
+                fAcc = -0.5;
+                fdSpeed = 0.0;
+                fdSecSpeed = fdSpeed/3.6;
+                xvehstate = dRever2;
+        }
+    }
+    fWheel = mCircleWheel;
+}
+
+void ivpark_simple::dRever2Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+    (void)fSpeed;
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 3000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = 0;
+        return;
+
+    }
+
+    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint2.gps_x,iv::decition::Compute00().dTpoint2.gps_y, maimgps);
+
+    double fdis = pt1.x - pt2.x;
+
+    if(fdis>-1.0)
+    {
+        fAcc = 0.0;  //acc calcutale by pid
+
+        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
+    }
+    else
+    {
+        if((fSpeed>0.3)&&(fabs(fdis)>0.3))
+        {
+            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
+            fdSpeed = 0.0;
+            fdSecSpeed = fdSpeed/3.6;
+        }
+        else
+        {
+                fAcc = -0.5;
+                fdSpeed = 0.0;
+                fdSecSpeed = fdSpeed/3.6;
+                xvehstate = dRever3;
+        }
+    }
+    fWheel = 0;
+}
+
+void ivpark_simple::dRever3Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate)
+{
+    (void)fSpeed;
+    static int64_t nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    int64_t nstoptime_ms = 3000; //every stop time is 3
+
+    if(mlastvehstate != xvehstate)
+    {
+        nstoptiming_ms = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+    mlastvehstate = xvehstate;
+
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+
+    nshift = 2;
+
+    if(abs(nnow - nstoptiming_ms) < nstoptime_ms)  //stop 3s, change wheel when stop
+    {
+        fdSpeed = 0;
+        fdSecSpeed = 0;
+        fAcc = -0.5;
+        fWheel = iv::decition::Compute00().bocheAngle*16.5 *1.0;
+        mCircleWheel = fWheel;
+        return;
+
+    }
+
+    Point2D pt1 = iv::decition::Coordinate_Transfer(nowgps.gps_x,nowgps.gps_y, maimgps);
+    Point2D pt2 = iv::decition::Coordinate_Transfer(iv::decition::Compute00().dTpoint3.gps_x,iv::decition::Compute00().dTpoint3.gps_y, maimgps);
+
+    double fdis = fabs(pt1.x - pt2.x);
+
+    if(fdis>1.0)
+    {
+        fAcc = 0.0;  //acc calcutale by pid
+
+        fdSpeed = 2.0; fdSecSpeed = fdSpeed/3.6;
+    }
+    else
+    {
+        if((fSpeed>0.3)&&(fabs(fdis)>0.3))
+        {
+            fAcc = (-1.0)*pow(fSpeed/3.6,2)/(2.0*fabs(fdis));
+            fdSpeed = 0.0;
+            fdSecSpeed = fdSpeed/3.6;
+        }
+        else
+        {
+                fAcc = -0.5;
+                fdSpeed = 0.0;
+                fdSecSpeed = fdSpeed/3.6;
+                xvehstate = reverseDirect;
+        }
+    }
+    fWheel = mCircleWheel;
+}
+
+
+

src1/decision/common/adc_decision_function/ivpark_simple.h

@@ -24,11 +24,28 @@ private:
     double mCircleWheel = 0;
 
 private:
-    void reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
-    void reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
-    void reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
-    void reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);
 
+/*
+    垂直泊车过程：
+    直倒-圆弧倒车-直倒-抵达
+
+    侧方泊车过程：
+    直倒-第一段圆弧倒车-直倒-第二段圆弧倒车-直倒-抵达
+*/
+
+    void reversingcarFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);  //直倒
+    void reverseCircleFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate);// 圆弧
+    void reverseDirectFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 直倒
+    void reverseArrFun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 抵达
+
+    void dRever0Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 直倒
+    void dRever1Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 第一段弧
+    void dRever2Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 连接直倒
+    void dRever3Fun(iv::GPS_INS nowgps,double fSpeed,double & fAcc,double & fWheel,int & nshift,double & fdSpeed,double & fdSecSpeed,VehState & xvehstate); // 第二段弧
+
+
+private:
+    double GetDistance(iv::GPS_INS p1,iv::GPS_INS p2);
 
 };