📄 gpsinterface_gpsd.cpp
字号:
/*
* Roadnav
* GPSInterface_GPSD.cpp
*
* Copyright (c) 2004 - 2006 Richard L. Lynch <rllynch@users.sourceforge.net>
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of version 2 of the GNU General Public License
* as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
//////////////////////////////////////////////////////////////////////////////
/// \file
///
/// gpsd interface module.
///
//////////////////////////////////////////////////////////////////////////////
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#ifdef _MSC_VER
#pragma warning(disable: 4786)
#pragma warning(disable: 4800)
#endif
#include <wx/wx.h>
#include <wx/socket.h>
#include <wx/datetime.h>
#include <wx/tokenzr.h>
#include "GPSInterface_GPSD.h"
#include "App.h"
#include "libroadnav/Debug.h"
#ifdef HAVE_LIBGPS
using namespace std;
GPSInterface_GPSD::GPSInterface_GPSD() : m_tLastGPSSentence((time_t) 0)
{
m_fGPSDLastTimestamp = 0;
m_iGPSDProtocolVersion = -1;
m_fMinimumSpeedForHeading = 5.0;
m_fLastHeading = -1;
}
GPSInterface_GPSD::~GPSInterface_GPSD()
{
}
wxString GPSInterface_GPSD::Name()
{
return wxT("gpsd");
}
IGPSInterface::EGPSStatus GPSInterface_GPSD::GetData(wxGPSEvent * pGPSEvent)
{
Point ptGPS;
double fSpeed = 0;
double fHeading = 0;
wxString strFixType;
vector<SSatelliteInfo> vSatelliteInfo;
vector<SSatelliteInfo> vSatelliteInfoTmp;
long nSatellites = 0;
wxString strError;
int iLockType = 0;
double fGPSDTimestamp = 0;
// throttle
wxThread::Sleep(1000);
if (!m_sockGPSD.IsConnected())
{
wxString strHost;
wxString strPort;
long lPort = 2947;
int iMajorGPSDVersion = 0;
int iMinorGPSDVersion = 0;
wxString strCommands;
strHost = g_pConfig->Read(wxT("gpsdHost"), wxT("localhost"));
strPort = g_pConfig->Read(wxT("gpsdPort"), wxT("2947"));
strPort.ToLong(&lPort, 10);
strError = GPSDDisconnect(&m_sockGPSD);
strError = GPSDConnect(&m_sockGPSD, strHost, lPort);
if (strError == wxT(""))
{
m_sockGPSD.SetTimeout(5);
if (g_pConfig->Read(wxT("gpsdCompatibilityCheck"), (long) 1))
{
strError = GPSDGetProtocolVersion(&m_sockGPSD, &m_iGPSDProtocolVersion, &iMajorGPSDVersion, &iMinorGPSDVersion, &strCommands);
}
else
{
m_iGPSDProtocolVersion = 3;
iMajorGPSDVersion = 2;
iMinorGPSDVersion = 21;
strCommands = wxT("lptvsyxa");
}
}
if (strError == wxT(""))
{
// minimum version = 2.21
if ((iMajorGPSDVersion * 1000 + iMinorGPSDVersion) < 2021)
strError = wxString::Format(wxT("Sorry, Roadnav requires at least gpsd 2.21 or higher (you're running %d.%d)."), iMajorGPSDVersion, iMinorGPSDVersion);
}
if (strError == wxT(""))
{
int iCmd;
char szRequiredCommands[32] = "lptvsyxa";
// ensure the commands we want to use are implemented
for (iCmd = 0; szRequiredCommands[iCmd] && strError == wxT(""); iCmd++)
{
if (strCommands.First(szRequiredCommands[iCmd]) < 0)
strError = wxString::Format(wxT("Sorry, your gpsd server does not support the \"%c\" command, which is required by Roadnav."), szRequiredCommands[iCmd]);
}
}
if (strError != wxT(""))
{
LibRoadnavDebug1(wxT("gpsd"), wxT("Error occurred: %s - disabling GPS"), strError.c_str());
m_strLastError = wxT("gpsd error: ") + strError + wxT(" - disabling GPS");
return GPSStatusErrorDisableGPS;
}
}
ptGPS.m_fLat = 0;
ptGPS.m_fLong = 0;
fSpeed = 0;
fHeading = 0;
vSatelliteInfo.clear();
nSatellites = 0;
strError = GPSDGetLastTimestamp(&m_sockGPSD, m_iGPSDProtocolVersion, &fGPSDTimestamp);
if (strError == wxT(""))
{
if (fGPSDTimestamp != m_fGPSDLastTimestamp)
{
// if this is the first time we read the timestamp, then don't assume
// we've received a sentence recently
if (m_fGPSDLastTimestamp > 1)
m_tLastGPSSentence = wxDateTime::Now();
m_fGPSDLastTimestamp = fGPSDTimestamp;
}
strError = GPSDGetLockType(&m_sockGPSD, m_iGPSDProtocolVersion, &iLockType);
}
if (strError == wxT(""))
{
int iTmp;
strError = GPSDGetSatelliteInfo(&m_sockGPSD, m_iGPSDProtocolVersion, &vSatelliteInfo, &iTmp);
nSatellites = iTmp;
}
if (strError == wxT(""))
{
switch (iLockType)
{
case 1:
case 2:
strFixType = wxT("SPS");
break;
case 3:
strFixType = wxT("DGPS");
break;
default:
strFixType = wxT("Invalid");
}
if (strFixType != wxT("Invalid"))
strError = GPSDGetPosition(&m_sockGPSD, m_iGPSDProtocolVersion, &ptGPS);
}
if (strError == wxT("") && strFixType != wxT("Invalid"))
{
strError = GPSDGetSpeed(&m_sockGPSD, m_iGPSDProtocolVersion, &fSpeed);
}
if (strError == wxT("") && strFixType != wxT("Invalid"))
{
strError = GPSDGetHeading(&m_sockGPSD, m_iGPSDProtocolVersion, &fHeading);
if (fSpeed < m_fMinimumSpeedForHeading)
{
// We're traveling too slow for an accurate heading
fHeading = m_fLastHeading;
}
else
{
// remember the heading, provided it appears valid
if (fHeading >= 0 && fHeading < 360)
m_fLastHeading = fHeading;
}
}
if (strError != wxT(""))
{
LibRoadnavDebug1(wxT("gpsd"), wxT("Error occurred: %s"), strError.c_str());
if (strError != wxT("No data"))
{
LibRoadnavDebug0(wxT("gpsd"), wxT("GPS disabled"));
m_strLastError = wxT("gpsd error: ") + strError + wxT(" - disabling GPS");
return GPSStatusErrorDisableGPS;
}
}
if (strFixType != wxT("Invalid") && strError != wxT("No data"))
{
// locked
*pGPSEvent = wxGPSEvent(1, 1, 1, ptGPS, fSpeed, fHeading, strFixType, nSatellites, vSatelliteInfo, wxT(""));
return GPSStatusOK;
}
else
{
if (wxDateTime::Now() - m_tLastGPSSentence > wxTimeSpan(0, 0, 10, 0))
{
// no GPS unit
vSatelliteInfo.clear();
*pGPSEvent = wxGPSEvent(1, 0, 0, Point(0, 0), 0, 0, wxT("None"), 0, vSatelliteInfo, wxT(""));
return GPSStatusOK;
}
// no lock
*pGPSEvent = wxGPSEvent(1, 1, 0, Point(0, 0), 0, 0, wxT("None"), 0, vSatelliteInfo, wxT(""));
return GPSStatusOK;
}
wxASSERT(0);
return GPSStatusOK;
}
IGPSInterface::EGPSStatus GPSInterface_GPSD::AutoDetect(wxThread * pThread)
{
wxGPSEvent ev;
GPSDDisconnect(&m_sockGPSD);
g_pConfig->Write(wxT("gpsdHost"), wxT("localhost"));
g_pConfig->Write(wxT("gpsdPort"), wxT("2947"));
if (GetData(&ev) == GPSStatusOK)
return GPSStatusOK;
if (ev.m_bActive)
return GPSStatusAutoDetectionGPSDetectedButNoLock;
return GPSStatusAutoDetectionNoGPSDetected;
}
wxString GPSInterface_GPSD::GetLastError()
{
return m_strLastError;
}
wxString GPSInterface_GPSD::GPSDConnect(wxSocketClient * psockGPSD, wxString strHost, int iPort)
{
wxIPV4address addrGPSD;
LibRoadnavDebug2(wxT("gpsd"), wxT("Connecting to %s:%d"), strHost.c_str(), iPort);
addrGPSD.Hostname(strHost);
addrGPSD.Service(iPort);
psockGPSD->Connect(addrGPSD, true);
if (!psockGPSD->IsConnected())
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Connection failed"));
return wxT("Error connecting to gpsd server!");
}
LibRoadnavDebug0(wxT("gpsd"), wxT("Connection established"));
return wxT("");
}
wxString GPSInterface_GPSD::GPSDDisconnect(wxSocketBase * psockGPSD)
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Closing gpsd connection"));
psockGPSD->Close();
LibRoadnavDebug0(wxT("gpsd"), wxT("Connection closed"));
return wxT("");
}
wxString GPSInterface_GPSD::GPSDGetProtocolVersion(wxSocketBase * psockGPSD, int * piProtocolVersion, int * piMajorGPSDVersion, int * piMinorGPSDVersion, wxString * pstrCommands)
{
char szBuf[64];
int iRead = 0;
*piProtocolVersion = 0;
*piMajorGPSDVersion = 0;
*piMinorGPSDVersion = 0;
*pstrCommands = wxT("");
LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting version string from server"));
psockGPSD->Write("l\n", 2);
if (psockGPSD->Error())
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Write error"));
return wxT("gpsd write error");
}
psockGPSD->Read(szBuf, 63);
if (psockGPSD->Error())
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Read error"));
return wxT("gpsd read error");
}
iRead = psockGPSD->LastCount();
LibRoadnavDebug1(wxT("gpsd"), wxT("Got %d bytes from server"), iRead);
if (iRead < 1)
return wxT("gpsd socket error");
wxASSERT(iRead <= 63);
if (iRead > 63)
iRead = 63;
szBuf[iRead] = 0;
LibRoadnavDebug1(wxT("gpsd"), wxT("szBuf = %hs"), szBuf);
// tokenize version string from gpsd server
wxString strInfo(szBuf, wxConvUTF8);
wxStringTokenizer tkz(strInfo, wxT(" .,=\r\n"));
wxString strPrefix = tkz.GetNextToken();
if (strPrefix != wxT("GPSD"))
return wxT("This does not appear to be a gpsd 2.x server (wrong prefix)");
wxString strResponseType = tkz.GetNextToken();
if (strResponseType.Lower() != wxT("l"))
return wxT("This does not appear to be a gpsd 2.x server (it does not understand the l command). Perhaps you're running the GPSdrive fork of gpsd instead of the official http://gpsd.berlios.de/ version?");
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no protocol version)");
wxString strProtocolVersion = tkz.GetNextToken();
long lProtocolVersion = 0;
strProtocolVersion.ToLong(&lProtocolVersion, 10);
*piProtocolVersion = lProtocolVersion;
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no major version)");
wxString strMajorVersion = tkz.GetNextToken();
long lMajorVersion = 0;
strMajorVersion.ToLong(&lMajorVersion, 10);
*piMajorGPSDVersion = lMajorVersion;
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no minor version)");
wxString strMinorVersion = tkz.GetNextToken();
long lMinorVersion = 0;
strMinorVersion.ToLong(&lMinorVersion, 10);
*piMinorGPSDVersion = lMinorVersion;
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no command set)");
*pstrCommands = tkz.GetNextToken();
return wxT("");
}
wxString GPSInterface_GPSD::GPSDGetPosition(wxSocketBase * psockGPSD, int iProtocolVersion, Point * pptGPS)
{
char szBuf[64];
int iRead = 0;
int iAttemptsLeft = 5;
pptGPS->m_fLat = 0;
pptGPS->m_fLong = 0;
while (iAttemptsLeft--)
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting position from server"));
psockGPSD->Write("p\n", 2);
if (psockGPSD->Error())
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Write error"));
return wxT("gpsd write error");
}
psockGPSD->Read(szBuf, 63);
if (psockGPSD->Error())
{
LibRoadnavDebug0(wxT("gpsd"), wxT("Read error"));
return wxT("gpsd read error");
}
iRead = psockGPSD->LastCount();
LibRoadnavDebug1(wxT("gpsd"), wxT("Got %d bytes from server"), iRead);
if (iRead < 1)
return wxT("gpsd socket error");
wxASSERT(iRead <= 63);
if (iRead > 63)
iRead = 63;
szBuf[iRead] = 0;
LibRoadnavDebug1(wxT("gpsd"), wxT("szBuf = %hs"), szBuf);
// tokenize version string from gpsd server
wxString strInfo(szBuf, wxConvUTF8);
wxStringTokenizer tkz(strInfo, wxT(" ,=\r\n"));
wxString strPrefix = tkz.GetNextToken();
if (strPrefix.Left(5) != wxT("GPSD"))
return wxT("This does not appear to be a gpsd 2.x server (wrong prefix)");
wxString strResponseType = tkz.GetNextToken();
if (strResponseType.Lower() != wxT("p"))
return wxT("This does not appear to be a gpsd 2.x server (does not understand p command)");
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no position report)");
wxString strLat = tkz.GetNextToken();
if (strLat == wxT("?"))
{
wxThread::Sleep(1000);
continue;
}
pptGPS->m_fLat = 0;
strLat.ToDouble(&pptGPS->m_fLat);
if (!tkz.HasMoreTokens())
return wxT("This does not appear to be a gpsd 2.x server (no longitude)");
wxString strLong = tkz.GetNextToken();
if (strLong == wxT("?"))
{
wxThread::Sleep(1000);
continue;
}
pptGPS->m_fLong = 0;
strLong.ToDouble(&pptGPS->m_fLong);
return wxT("");
}
LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting position"));
return wxT("No data");
}
wxString GPSInterface_GPSD::GPSDGetHeading(wxSocketBase * psockGPSD, int iProtocolVersion, double * pfHeading)
{
char szBuf[64];
int iRead = 0;
int iAttemptsLeft = 5;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -