gpsinterface_gpsd.cpp

Powerful and Portable GPS application -- support Linux, Windows, Windows CE GPS navigation and Map m

/*
 *  Roadnav
 *  GPSInterface_GPSD.cpp
 *
 *  Copyright (c) 2004 - 2007 Richard L. Lynch <[e-mail]>
 *
 *  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;

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Constructor - zero out everything.
///
//////////////////////////////////////////////////////////////////////////////
GPSInterface_GPSD::GPSInterface_GPSD() : m_tLastGPSSentence((time_t) 0)
{
	m_fGPSDLastTimestamp = 0;
	m_iGPSDProtocolVersion = -1;
	m_fMinimumSpeedForHeading = 5.0;
	m_fLastHeading = -1;
	m_fLastSpeed = 0;
	m_nLastSatUsedForLock = 0;
	m_nLastSatellitesVisible = 0;
	m_iLastLockType = 0;
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Destructor - nothing to do
///
//////////////////////////////////////////////////////////////////////////////
GPSInterface_GPSD::~GPSInterface_GPSD()
{
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Returns name of this GPS module.
///
//////////////////////////////////////////////////////////////////////////////
wxString GPSInterface_GPSD::Name()
{
	return wxT("gpsd");
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Tries to get updated data from gpsd server.
///
/// \param pGPSEvent		Receives updated gps data.
///
/// \return					Status - either GPSStatusErrorDisableGPS for 
///							serious error, or GPSStatusOK if data was
///							obtained.
///
//////////////////////////////////////////////////////////////////////////////
IGPSInterface::EGPSStatus GPSInterface_GPSD::GetData(wxGPSEvent * pGPSEvent)
{
	Point ptGPS;
	double fSpeed = 0;
	double fHeading = 0;
	wxString strFixType;
	vector<SSatelliteInfo> vSatelliteInfo;
	wxString strError;
	int iLockType = 0;
	double fGPSDTimestamp = 0;
	int nSatUsedForLock;

	// 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] = "lptvsyx";
			
			// 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;
	nSatUsedForLock = 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("No data"))
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("No lock type available - using old lock type instead"));
		iLockType = m_iLastLockType;
		strError = wxT("");
	}
	else
	{
		m_iLastLockType = iLockType;
	}

	if (strError == wxT(""))
	{
		int iTmp;
		strError = GPSDGetSatelliteInfo(&m_sockGPSD, m_iGPSDProtocolVersion, &vSatelliteInfo, &iTmp);
		nSatUsedForLock = iTmp;
	}
	
	if (strError == wxT("No data"))
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("No satellite info available - using old satellite info instead"));
		strError = wxT("");
	}
	else
	{
		unsigned int i;
		
		m_nLastSatellitesVisible = vSatelliteInfo.size();
		for (i = 0; i < vSatelliteInfo.size(); i++)
			m_arLastSatellitesVisible[i] = vSatelliteInfo[i];
		m_nLastSatUsedForLock = nSatUsedForLock;
	}

	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 (strFixType != wxT("Invalid"))
	{
		if (strError == wxT(""))
		{
			strError = GPSDGetSpeed(&m_sockGPSD, m_iGPSDProtocolVersion, &fSpeed);
		}

		if (strError == wxT("No data"))
		{
			LibRoadnavDebug0(wxT("gpsd"), wxT("No speed available - using old speed instead"));
			fSpeed = m_fLastSpeed;
			strError = wxT("");
		}
		else
		{
			m_fLastSpeed = fSpeed;
		}

		if (strError == wxT(""))
		{
			strError = GPSDGetHeading(&m_sockGPSD, m_iGPSDProtocolVersion, &fHeading);
		}
			
		if (strError == wxT("No data"))
		{
			LibRoadnavDebug0(wxT("gpsd"), wxT("No heading available - using old heading instead"));
			fHeading = m_fLastHeading;
			strError = wxT("");
		}

		if (strError == wxT(""))
		{
			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") && wxDateTime::Now() - m_tLastGPSSentence <= wxTimeSpan(0, 0, 10, 0))
	{
		// locked
		*pGPSEvent = wxGPSEvent(1, 1, 1, ptGPS, fSpeed, fHeading, strFixType, m_nLastSatUsedForLock, m_arLastSatellitesVisible, m_nLastSatellitesVisible, wxT(""));

		return GPSStatusOK;
	}
	else
	{
		if (wxDateTime::Now() - m_tLastGPSSentence > wxTimeSpan(0, 0, 10, 0))
		{
			// no GPS unit
			*pGPSEvent = wxGPSEvent(1, 0, 0, Point(0, 0), 0, 0, wxT("None"), 0, NULL, 0, wxT(""));
			return GPSStatusOK;
		}

		// no lock
		*pGPSEvent = wxGPSEvent(1, 1, 0, Point(0, 0), 0, 0, wxT("None"), 0, m_arLastSatellitesVisible, m_nLastSatellitesVisible, wxT(""));
		return GPSStatusOK;
	}
	
	wxASSERT(0);

	return GPSStatusOK;
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Attempts to autodetect if a gpsd server is present.
///
/// \param pThread			Pointer to wxThread obtain this code is running
///							in.
///
/// \return					Status - either GPSStatusOK if a gpsd server
///							was found and a lock was detected, or 
///							GPSStatusAutoDetectionGPSDetectedButNoLock
///							if a gpsd server was found, but no lock, or
///							GPSStatusAutoDetectionNoGPSDetected for
///							no gpsd server.
///
//////////////////////////////////////////////////////////////////////////////
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;
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Returns an explanation of the last error.
///
//////////////////////////////////////////////////////////////////////////////
wxString GPSInterface_GPSD::GetLastError()
{
	return m_strLastError;
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Attempt to connect to a gpsd server.
///
/// \param psockGPSD		Pointer to socket used to connect to gpsd server.
/// \param strHost			Hostname of gpsd server.
/// \param iPort			Port number of gpsd server.
/// \return					Empty string on success, or error message on error.
///
//////////////////////////////////////////////////////////////////////////////
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("");
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Disconnect from gpsd server.
///
/// \param psockGPSD		Pointer to socket connected to gpsd server.
/// \return					Always returns empty string, indicating sucess.
///
//////////////////////////////////////////////////////////////////////////////
wxString GPSInterface_GPSD::GPSDDisconnect(wxSocketBase * psockGPSD)
{
	LibRoadnavDebug0(wxT("gpsd"), wxT("Closing gpsd connection"));

	psockGPSD->Close();

	LibRoadnavDebug0(wxT("gpsd"), wxT("Connection closed"));
	
	return wxT("");
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Retrieves information about the gpsd server.
///
/// \param psockGPSD			Socket connected to gpsd server.
/// \param piProtocolVersion	Returns gpsd protocol version.
/// \param piMajorGPSDVersion	Returns major gpsd version.
/// \param piMinorGPSDVersion	Returns minor gpsd version.
/// \param pstrCommands			Returns a string containing the commands the
///								gpsd server supports.
/// \return						Empty string on success, or error message on error.
///
//////////////////////////////////////////////////////////////////////////////
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("");
}

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Retrieves the gps position from the gpsd server.
///
/// \param psockGPSD			Socket connected to gpsd server.
/// \param iProtocolVersion		Protocol version used by gpsd server.
/// \param pptGPS				Returns gps coordinates
/// \return						Empty string on success, or error message on error.
///
//////////////////////////////////////////////////////////////////////////////
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");