gpsinterface_gpsd.cpp

roadnav 内含一个基于wxWindows库的车载导航系统。编译后

	*pfHeading = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting heading from server"));

		psockGPSD->Write("t\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("t"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand t command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no track)");
			
		wxString strHeading = tkz.GetNextToken();
		
		if (strHeading == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
			
		*pfHeading = 0;
		strHeading.ToDouble(pfHeading);

		return wxT("");
	}

	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting heading"));

	return wxT("No data");
}

wxString GPSInterface_GPSD::GPSDGetSpeed(wxSocketBase * psockGPSD, int iProtocolVersion, double * pfSpeed)
{
	char szBuf[64];
	int iRead = 0;
	int iAttemptsLeft = 5;
	
	*pfSpeed = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting speed from server"));

		psockGPSD->Write("v\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("v"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand v command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no speed)");
			
		wxString strSpeed = tkz.GetNextToken();
		
		if (strSpeed == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
			
		*pfSpeed = 0;
		strSpeed.ToDouble(pfSpeed);

		// convert to MPH
		*pfSpeed *= 1.151;

		return wxT("");
	}

	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting speed"));

	return wxT("No data");
}

wxString GPSInterface_GPSD::GPSDGetLockType(wxSocketBase * psockGPSD, int iProtocolVersion, int * piLockType)
{
	char szBuf[64];
	int iRead = 0;
	int iAttemptsLeft = 5;
	
	*piLockType = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting lock type from server"));
		psockGPSD->Write("s\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("s"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand s command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no lock type)");
			
		wxString strLockType = tkz.GetNextToken();
		
		if (strLockType == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
			
		long lLockType = -1;
		strLockType.ToLong(&lLockType);
		
		*piLockType = lLockType;

		return wxT("");
	}
	
	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting lock type"));
	
	return wxT("No data");
}

wxString GPSInterface_GPSD::GPSDGetSatelliteInfo(wxSocketBase * psockGPSD, int iProtocolVersion, vector<SSatelliteInfo> * pvSatInfo, int * pnSatellitesUsed)
{
	char szBuf[512];
	int iRead = 0;
	int iAttemptsLeft = 5;
	
	pvSatInfo->clear();
	*pnSatellitesUsed = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting satellite info from server"));

		psockGPSD->Write("y\n", 2);

		if (psockGPSD->Error())
		{
			LibRoadnavDebug0(wxT("gpsd"), wxT("Write error"));
			return wxT("gpsd write error");
		}

		psockGPSD->Read(szBuf, 511);

		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 <= 511);
		if (iRead > 511)
			iRead = 511;
		
		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("y"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand y command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no satellite info)");
		
		if (iProtocolVersion >= 2)
		{	
			wxString strTag = tkz.GetNextToken();
			
			if (strTag == wxT("?"))
			{
				wxThread::Sleep(1000);
				continue;
			}

			wxString strTimestamp = tkz.GetNextToken();
			// don't care about timestamp
		}
		
		wxString strCount = tkz.GetNextToken();
		// don't trust the count .. go by how many 5-tuples are really there

		if (strCount == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
		
		pvSatInfo->clear();
		*pnSatellitesUsed = 0;
		
		while (tkz.HasMoreTokens())
		{
			SSatelliteInfo sInfo;
			long lTmp;
			
			tkz.GetNextToken().ToLong(&lTmp);
			
			sInfo.m_iID = lTmp;
			
			tkz.GetNextToken().ToDouble(&sInfo.m_fElevation);
			tkz.GetNextToken().ToDouble(&sInfo.m_fAzimuth);
			tkz.GetNextToken().ToDouble(&sInfo.m_fSNR);
			
			if (sInfo.m_fSNR > 100)
				sInfo.m_fSNR = 100;
			
			if (tkz.HasMoreTokens())
			{
				long lUsed = 0;
				
				tkz.GetNextToken().ToLong(&lUsed);
				
				if (lUsed)
					(*pnSatellitesUsed)++;
				
				pvSatInfo->push_back(sInfo);
			}
		}

		return wxT("");
	}
	
	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting satellite info"));

	// it's okay if there's no satellite data
	return wxT("No data");
}

wxString GPSInterface_GPSD::GPSDGetLastTimestamp(wxSocketBase * psockGPSD, int iProtocolVersion, double * pfTimestamp)
{
	char szBuf[64];
	int iRead = 0;
	int iAttemptsLeft = 5;
	
	*pfTimestamp = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting timestamp from server"));

		psockGPSD->Write("x\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("x"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand x command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no timestamp)");
			
		wxString strTimestamp = tkz.GetNextToken();
		
		if (strTimestamp == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
			
		*pfTimestamp = 0;
		strTimestamp.ToDouble(pfTimestamp);

		return wxT("");
	}

	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting timestamp"));

	return wxT("No data");
}

wxString GPSInterface_GPSD::GPSDGetAltitude(wxSocketBase * psockGPSD, int iProtocolVersion, double * pfAltitude)
{
	char szBuf[64];
	int iRead = 0;
	int iAttemptsLeft = 1;
	
	*pfAltitude = 0;
	
	while (iAttemptsLeft--)
	{
		LibRoadnavDebug0(wxT("gpsd"), wxT("Requesting altitude from server"));
		psockGPSD->Write("a\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("a"))
			return wxT("This does not appear to be a gpsd 2.x server (does not understand a command)");

		if (!tkz.HasMoreTokens())
			return wxT("This does not appear to be a gpsd 2.x server (no altitude)");
			
		wxString strAltitude = tkz.GetNextToken();
		
		if (strAltitude == wxT("?"))
		{
			wxThread::Sleep(1000);
			continue;
		}
			
		*pfAltitude = 0;
		strAltitude.ToDouble(pfAltitude);

		return wxT("");
	}

	LibRoadnavDebug0(wxT("gpsd"), wxT("Timeout requesting altitude"));

	return wxT("");
}

#endif