add driver/driver_ntrip_client for use serial port set ntrip data to serialport.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -624,6 +624,184 @@ int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoa
     return 0;
 }
 
+
+int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh)
+{
+
+    double dismin = std::numeric_limits<double>::infinity();
+    s = dismin;
+    int i;
+    std::vector<Road * > xvectorroad;
+    std::vector<double > xvectordismin;
+    std::vector<double > xvecotrnearx;
+    std::vector<double > xvectorneary;
+    std::vector<double > xvectornearhead;
+    std::vector<double > xvectors;
+    std::vector<GeometryBlock *> xvectorgeob;
+    double VECTORTHRESH = 5;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        bool bchange = false;
+        double localdismin = std::numeric_limits<double>::infinity();
+
+        double localnearx = 0;
+        double localneary = 0;
+        double localnearhead = 0;
+        double locals = 0;
+        GeometryBlock * pgeolocal;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                break;
+            case 1:
+                dis = GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh);
+                break;
+            case 2:  //arc
+                dis = GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh);
+                break;
+
+            case 3:
+                dis = 100000.0;
+                break;
+            case 4:
+                dis = GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+            if(dis < dismin)
+            {
+                dismin = dis;
+                nearx = nx;
+                neary = ny;
+                nearhead = nh;
+                s = dis;
+                *pObjRoad = proad;
+                *pgeo = pgb;
+
+            }
+
+            if((dis<VECTORTHRESH) &&(dis<localdismin))
+            {
+                localdismin = dis;
+                localnearx = nx;
+                localneary = ny;
+                localnearhead = nh;
+                locals = dis;
+                pgeolocal = pgb;
+                bchange = true;
+            }
+
+
+
+
+        }
+
+        if(bchange)
+        {
+            xvectorroad.push_back(proad);
+            xvecotrnearx.push_back(localnearx);
+            xvectorneary.push_back(localneary);
+            xvectordismin.push_back(localdismin);
+            xvectors.push_back(locals);
+            xvectorgeob.push_back(pgeolocal);
+            xvectornearhead.push_back(localnearhead);
+        }
+    }
+
+
+    if(s > nearthresh)return -1;
+
+    if((*pObjRoad)->GetLaneSectionCount()>0)
+    {
+        LaneSection * pLS = (*pObjRoad)->GetLaneSection(0);
+        if((*pObjRoad)->GetLaneSection(0)->GetLeftLaneCount()&&(*pObjRoad)->GetLaneSection(0)->GetRightLaneCount()>0)
+        {
+            return 0;
+        }
+        else
+        {
+            double headdiff = hdg - nearhead;
+            while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+            while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+            if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+            {
+               return 0;
+            }
+            else
+            {
+                if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0)&&(pLS->GetLeftLaneCount()>0))
+                {
+                    return 0;
+                }
+                else
+                {
+                    bool bHaveSame = false;
+                    for(i=0;i<xvectorroad.size();i++)
+                    {
+                        if(xvectorroad[i]->GetLaneSectionCount()<1)continue;
+                        bool bNeedFind = false;
+                        if(bHaveSame == false)bNeedFind = true;
+                        else
+                        {
+                            if(xvectordismin[i]<dismin)bNeedFind = true;
+                        }
+                        if(bNeedFind)
+                        {
+                            double fdiff = hdg - xvectornearhead[i];
+                            while(fdiff < 2.0*M_PI) fdiff = fdiff + 2.0*M_PI;
+                            while(fdiff >= 2.0*M_PI)fdiff = fdiff - 2.0*M_PI;
+
+                            bool bUse = false;
+                            LaneSection * pLS = xvectorroad[i]->GetLaneSection(0);
+                            if(((fdiff <M_PI/2.0)||(fdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                            {
+                                bUse = true;
+                            }
+                            if(((fdiff >=M_PI/2.0)&&(fdiff <= M_PI*3.0/2.0))&&(pLS->GetLeftLaneCount()>0))
+                            {
+                                bUse = true;
+                            }
+
+                            if(bUse)
+                            {
+                                dismin = xvectordismin[i];
+                                nearx = xvecotrnearx[i];
+                                neary = xvectorneary[i];
+                                nearhead = xvectornearhead[i];
+                                s = xvectors[i];
+                                *pObjRoad = xvectorroad[i];
+                                *pgeo = xvectorgeob[i];
+                                bHaveSame = true;
+                                std::cout<<"change road. "<<std::endl;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+
+
 /**
   * @brief SelectRoadLeftRight
   * 选择左侧道路或右侧道路
@@ -2026,7 +2204,9 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
     double  s;double nearx;
     double  neary;double  nearhead;
     Road * pRoad;GeometryBlock * pgeob;
-    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+
+    int nfind = GetNearPointWithHead(x_now,y_now,head,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
 
     if(nfind < 0)return -1;
 

src/driver/driver_ntrip_client/.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_ntrip_client/driver_ntrip_client.pro

@@ -0,0 +1,35 @@
+QT -= gui
+
+QT += network  serialport
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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
+
+#DEFINES += DEBUG_NTRIP_MSG
+
+# You can also make your code fail to compile if it uses 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 \
+        ntrip_client.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+HEADERS += \
+    ntrip_client.h

src/driver/driver_ntrip_client/driver_ntrip_client.xml

@@ -0,0 +1,11 @@
+<xml>	
+	<node name="driver_ntrip_client">
+		<param name="ip" value="60.205.8.49" />
+		<param name="port" value="8003" />
+		<param name="mountpoint" value="RTCM32_GGB" />
+		<param name="username" value="xxxx" />
+		<param name="password" value="xxxx" />
+		<param name="devname" value="/dev/ttyUSB0" />
+		<param name="baudrate" value="115200" />
+	</node>
+</xml>

src/driver/driver_ntrip_client/main.cpp

@@ -0,0 +1,31 @@
+#include <QCoreApplication>
+
+#include "ntrip_client.h"
+
+#include "xmlparam.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    std::string  strip,strport,strmountpoint,strusername,strpassword,strdevname,strbuadrate;
+    QString strpath = QCoreApplication::applicationDirPath();
+    if(argc < 2)
+        strpath = strpath + "/driver_ntrip_client.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+    strip = xp.GetParam("ip","60.205.8.49");
+    strport = xp.GetParam("port","8003");
+    strmountpoint = xp.GetParam("mountpoint","RTCM32_GGB");
+    strusername = xp.GetParam("username","xxxxx");
+    strpassword = xp.GetParam("password","xxxxx");
+    strdevname = xp.GetParam("devname","/dev/ttyUSB0");
+    strbuadrate = xp.GetParam("baudrate","115200");
+
+    ntrip_client xntrip(strip,strport,strmountpoint,strusername,strpassword,strdevname,atoi(strbuadrate.data()));
+    (void)&xntrip;
+
+    return a.exec();
+}

src/driver/driver_ntrip_client/ntrip_client.cpp

@@ -0,0 +1,253 @@
+#include "ntrip_client.h"
+
+ntrip_client::ntrip_client(std::string strip,std::string strport,std::string strMountPoint,
+                           std::string strUserName,std::string strPassWord,
+                           std::string strCOMName,int nBaudRate )
+{
+    mstrip = strip;
+    mstrport = strport;
+    mstrMountPoint = strMountPoint;
+    mstrUserName = strUserName;
+    mstrPassWord = strPassWord;
+    mstrCOMName = strCOMName;
+    mnBaudRate = nBaudRate;
+
+    socket_ = new QTcpSocket(this);
+//    socket_->connectToHost("rtk.ntrip.qxwz.com",8002);
+    connect(socket_, SIGNAL(connected()), this, SLOT(connectedSlot()));
+    connect(socket_, SIGNAL(disconnected()), this, SLOT(disconnectedSlot()));
+    connect(socket_, SIGNAL(readyRead()), this, SLOT(readyReadSlot()));
+    connect(socket_, SIGNAL(error(QAbstractSocket::SocketError)), this,
+            SLOT(errorSlot(QAbstractSocket::SocketError)));
+
+    mpTimerNtrip = new QTimer(this);
+    connect(mpTimerNtrip,SIGNAL(timeout()),this,SLOT(onTimerNtrip()));
+    mpTimerNtrip->setInterval(1000);
+    mpTimerNtrip->start();
+
+    mpTimerPort = new QTimer(this);
+    connect(mpTimerPort,SIGNAL(timeout()),this,SLOT(onTimerPortOpen()));
+    mpTimerPort->start(100);
+}
+
+void ntrip_client::run()
+{
+
+}
+
+
+void ntrip_client::connectedSlot()
+{
+    if(socket_->state() == QAbstractSocket::ConnectedState)
+    {
+        qDebug("main is connected.\n");
+        mFindCMState = 3;
+    }
+    else
+    {
+        qDebug("main is not connected.\n");
+    }
+    isConnected_ = true;
+}
+
+void ntrip_client::disconnectedSlot()
+{
+//    qDebug("connect server fail.");
+    if(mFindCMState != -1)mFindCMState = 0;
+    isConnected_ = false;
+    socket_->close();
+
+}
+
+void ntrip_client::readyReadSlot()
+{
+    char * str;
+
+    QByteArray message = socket_->readAll();
+//    qDebug(message.data());
+
+    switch (mFindCMState) {
+
+    case 4:
+        str = (char *)message.data();
+        if(strstr(str,"ICY 200 OK") > 0)
+        {
+            mFindCMState = 5;
+            qDebug("Login succcess");
+        }
+        else
+        {
+            if(strstr(str,"HTTP 401 Unauthorized") > 0)
+            {
+                mFindCMState = -1;
+                qDebug("Authorize Fail. message is %s",message.data());
+            }
+         //   mFindCMState = 0;
+        }
+        break;
+    case 5:
+#ifdef  DEBUG_NTRIP_MSG
+        ShowNtripMsg(message);
+#else
+        qDebug("ntrip msg len is %d",message.length());
+#endif
+
+        if(mbPortOpen)
+        {
+            if(-1 == m_serialPort_RTK->write(message))
+            {
+                qDebug("Write Serial Port Fail.");
+                m_serialPort_RTK->close();
+                mbPortOpen = false;
+                mpTimerPort->start(100);
+            }
+        }
+
+        break;
+    default:
+        break;
+    }
+
+
+//    qDebug("len = %d",message.length());
+/*
+    char strX[3000],strX2[10];
+    int i;
+    int nLen = message.length();
+    snprintf(strX,3000,"");
+    for(i=0;i<nLen;i++)
+    {
+        unsigned char * p = (unsigned char *)(message.data()+i);
+        snprintf(strX2,10,"%02X ",*p);strncat(strX,strX2,3000);
+    }
+    qDebug(strX);*/
+//    qDebug("TIME = %d read meesage length = %d",mTime.elapsed() ,message.length());
+
+
+
+}
+
+void ntrip_client::errorSlot(QAbstractSocket::SocketError)
+{
+//    QMessageBox::information(this, "show", socket_->errorString());
+    disconnectedSlot();
+}
+
+
+void ntrip_client::onTimerNtrip()
+{
+    switch (mFindCMState) {
+    case -1:
+        mpTimerPort->stop();
+        mpTimerNtrip->stop();
+        qDebug("User Pass Word Error.");
+        if(mbPortOpen)m_serialPort_RTK->close();
+        break;
+    case 0:
+        socket_->connectToHost(mstrip.data(),atoi(mstrport.data()));
+        mFindCMState = 1;
+        break;
+    case 2:
+        break;
+    case 3:
+        SendUserPass();
+        mFindCMState = 4;
+        break;
+    case 4:
+        break;
+    case 5:
+        SendGPGGA();
+        break;
+    default:
+        break;
+    }
+}
+
+void ntrip_client::SendUserPass()
+{
+    char strSend[1000];
+ //   snprintf(strSend,1000,"GET / HTTP/1.0\r\nUser-Agent: NTRIP GNSSInternetRadio/1.4.10\r\nAccept: */*\r\nConnection: close\r\n\r\n");
+    char strLine[100];
+    char strX[1000];
+    snprintf(strX,1000,"%s:%s",mstrUserName.data(),mstrPassWord.data());
+    QByteArray ba; ba.append(strX);qDebug(ba.toBase64().data());
+    snprintf(strSend,1000,"GET /%s HTTP/1.0\r\n",mstrMountPoint.data());
+    snprintf(strLine,100,"User-Agent: NTRIP GNSSInternetRadio/1.4.10\r\n");strncat(strSend,strLine,1000);
+    snprintf(strLine,100,"Accept: */*\r\n");strncat(strSend,strLine,1000);
+    snprintf(strLine,100,"Connection: close\r\n");strncat(strSend,strLine,1000);
+    snprintf(strLine,100,"Authorization: Basic %s\r\n",ba.toBase64().data());strncat(strSend,strLine,1000);
+    snprintf(strLine,100,"\r\n");strncat(strSend,strLine,1000);
+    socket_->write(strSend,strlen(strSend));
+    socket_->flush();
+}
+
+void ntrip_client::SendGPGGA()
+{
+
+
+    if(mbPortOpen)
+    {
+        QByteArray ba = m_serialPort_RTK->read(1000);
+        if(ba.length() > 0)
+        {
+            socket_->write(ba);
+            socket_->flush();
+        }
+    }
+    else
+    {
+#ifdef DEBUG_NTRIP_MSG
+        char strSend[1000];
+        snprintf(strSend,1000,"$GPGGA,102342.00,3948.6248006,N,11633.1924828,E,1,13,1.5,46.4085,M,-9.452,M,99,AAAA*72\r\n");
+        socket_->write(strSend,strlen(strSend));
+        socket_->flush();
+#endif
+    }
+
+/*    char strSend[1000];
+    snprintf(strSend,1000,"$GPGGA,102342.00,3948.6248006,N,11633.1924828,E,1,13,1.5,46.4085,M,-9.452,M,99,AAAA*72\r\n");
+    socket_->write(strSend,strlen(strSend));
+    socket_->flush();*/
+}
+
+void ntrip_client::onTimerPortOpen()
+{
+    if(mbPortOpen)return;
+    m_serialPort_RTK = new QSerialPort();
+    m_serialPort_RTK->setPortName(mstrCOMName.data());
+    m_serialPort_RTK->setBaudRate(mnBaudRate);
+    m_serialPort_RTK->setParity(QSerialPort::NoParity);
+    m_serialPort_RTK->setDataBits(QSerialPort::Data8);
+    m_serialPort_RTK->setStopBits(QSerialPort::OneStop);
+    m_serialPort_RTK->setFlowControl(QSerialPort::NoFlowControl);
+    m_serialPort_RTK->setReadBufferSize(0);
+    if(m_serialPort_RTK->open(QSerialPort::ReadWrite))
+    {
+        mbPortOpen = true;
+        mpTimerPort->stop();
+    }
+    else
+    {
+        qDebug("Open RTK Port Fail. Port Name is %s BaudRate is %d ",mstrCOMName.data(),mnBaudRate);
+        mbPortOpen = false;
+        delete m_serialPort_RTK;
+        mpTimerPort->setInterval(10000);
+
+    }
+
+}
+
+void ntrip_client::ShowNtripMsg(QByteArray ba)
+{
+    char strout[3000];
+    snprintf(strout,3000,"Ntrip msg len is %d info is:",ba.length());
+    char strtem[10];
+    int nlen = ba.length();
+    int i;
+    for(i=0;i<nlen;i++)
+    {
+        snprintf(strtem,10,"%02X ",(unsigned char )ba.at(i));
+        strncat(strout,strtem,3000);
+    }
+    qDebug(strout);
+}

src/driver/driver_ntrip_client/ntrip_client.h

@@ -0,0 +1,71 @@
+#ifndef NTRIP_CLIENT_H
+#define NTRIP_CLIENT_H
+
+
+#include <QObject>
+#include <QThread>
+#include <QDateTime>
+#include <QTimer>
+#include <iostream>
+
+#include <QSerialPort>
+
+#include <QUdpSocket>
+#include <QNetworkDatagram>
+#include <QtNetwork/QTcpSocket>
+
+
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+
+
+class ntrip_client : public QThread
+{
+        Q_OBJECT
+public:
+    ntrip_client(std::string strip,std::string strport,std::string strMountPoint,
+                 std::string strUserName,std::string strPassWord,
+                 std::string strCOMName,int nBaudRate = 115200);
+
+private slots:
+    void connectedSlot();
+    void disconnectedSlot();
+    void readyReadSlot();
+    void errorSlot(QAbstractSocket::SocketError);
+
+    void onTimerNtrip();
+    void onTimerPortOpen();
+
+private:
+    std::string mstrip;
+    std::string mstrport;
+    std::string mstrMountPoint;
+    std::string mstrUserName;
+    std::string mstrPassWord;
+    std::string mstrCOMName;
+    int mnBaudRate;
+
+    int mFindCMState = 0; //-1 Pass Error 0 not connected 1 connecting 2 connected 3 need send pass 4 had send usernamepassword 5 connected
+    QTcpSocket *socket_;
+    bool isConnected_;
+    int mSerialState;
+
+    QSerialPort *m_serialPort_RTK;
+    bool mbPortOpen = false;
+
+    QTimer * mpTimerNtrip;
+    QTimer * mpTimerPort;
+
+
+private:
+    void SendUserPass();
+    void SendGPGGA();
+
+    void ShowNtripMsg(QByteArray ba);
+
+private:
+    void run();
+};
+
+#endif // NTRIP_CLIENT_H