gpsinterface_serial.cpp

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

/*
 *  Roadnav
 *  GPSInterface_Serial.cpp
 *
 *  Copyright (c) 2004 - 2006 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
///
/// NMEA serial gps 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 "libroadnav/Debug.h"

#include "App.h"
#include "GPSInterface_Serial.h"

#ifdef ROADNAV_SERIAL_SUPPORT

using namespace std;

GPSInterface_Serial::GPSInterface_Serial() : m_tLastGPSSentence((time_t) 0), m_tLastGPSLock((time_t) 0)
{
	LibRoadnavDebug0(wxT("SerialIO"), wxT("Initializing serial I/O"));
	m_bSerialInitialized = false;
	
	m_fLastHeading = -1;

	m_fSpeed = 0;
	m_fHeading = 0;

	m_fMinimumSpeedForHeading = 5.0;

	m_bSeenGPGGA = false;
	m_bSeenGPRMC = false;

	m_nSatellites = 0;
	
	*m_szBuffer = 0;
	
	m_pThread = NULL;
}

GPSInterface_Serial::~GPSInterface_Serial()
{
	if (m_bSerialInitialized)
		ShutdownSerialIO();
}

wxString GPSInterface_Serial::Name()
{
	return wxT("Serial");
}

bool GPSInterface_Serial::VerifyGPSChecksum(wxString strSentence)
{
	unsigned int iPos;
	wxUint32 uiChecksum;
	
	uiChecksum = 0;
	
	LibRoadnavDebug1(wxT("SerialIO"), wxT("Verifying checksum on: %s"), strSentence.c_str());
	
	while (strSentence.Length() >= 3 && 
				(strSentence.GetChar(strSentence.Length() - 1) == wxT('\r') ||
				strSentence.GetChar(strSentence.Length() - 1) == wxT('\n')))
		strSentence = strSentence.Left(strSentence.Length() - 1);
		
	if (strSentence.Length() < 3)
	{
		LibRoadnavDebug0(wxT("SerialIO"), wxT("Length too short after removing CR and LFs"));
		return true;
	}
	
	if (strSentence.GetChar(strSentence.Length() - 3) != wxT('*'))
	{
		LibRoadnavDebug0(wxT("SerialIO"), wxT("Asterisk missing"));
		return true;
	}
	
	if (strSentence.GetChar(0) != wxT('$'))
	{
		LibRoadnavDebug0(wxT("SerialIO"), wxT("Dollar sign missing"));
		return true;
	}
	
	for (iPos = 1; iPos < strSentence.Length() - 3; iPos++)
	{
		uiChecksum ^= strSentence.GetChar(iPos);
		uiChecksum &= 0xff;
	}

	LibRoadnavDebug1(wxT("SerialIO"), wxT("Computed checksum: %02x"), uiChecksum);
	
	if (strSentence.Right(2).Lower() != wxString::Format(wxT("%02x"), uiChecksum))
	{
		LibRoadnavDebug0(wxT("SerialIO"), wxT("Checksum mismatch"));
		return true;
	}

	LibRoadnavDebug0(wxT("SerialIO"), wxT("Checksum is good"));
		
	return false;
}

IGPSInterface::EGPSStatus GPSInterface_Serial::GetData(wxGPSEvent * pGPSEvent)
{
	vector<SSatelliteInfo> vSatelliteInfoTmp;
	wxDateTime tStart = wxDateTime::Now();

	if (!m_bSerialInitialized)
	{
		if (InitSerialIO())
		{
			m_strLastError = wxT("Disabling GPS due to error opening serial port: ") + GetLastSerialIOError();
			
			wxThread::Sleep(1000);
			
			ShutdownSerialIO();

			return GPSStatusErrorDisableGPS;
		}
		m_bSerialInitialized = true;
	}

	while (1)
	{
		if (sizeof(m_szBuffer) - strlen(m_szBuffer) < 192)
		{
			LibRoadnavDebug0(wxT("SerialIO"), wxT("Buffer overrun - dumping oldest 192 bytes"));

			memmove(m_szBuffer, m_szBuffer + 192, strlen(m_szBuffer + 192) + 1);
		}
		
		if (ReadSerial(m_szBuffer + strlen(m_szBuffer), 128))
		{
			LibRoadnavDebug0(wxT("SerialIO"), wxT("Serial data received"));
		}

		LibRoadnavDebug1(wxT("SerialIO"), wxT("Serial buffer: %hs"), m_szBuffer);
		
		while (1)
		{
			// locate first CR, LF, or NULL
			int iEnd;
			wxString strSentence;
			
			for (iEnd = 0; m_szBuffer[iEnd] && m_szBuffer[iEnd] != '\r' && m_szBuffer[iEnd] != '\n'; iEnd++);

			// stop processing once we run out of CR or LFs
			if (!m_szBuffer[iEnd])
				break;
				
			// isolate sentence
			m_szBuffer[iEnd] = 0;
			strSentence = wxString(m_szBuffer, *wxConvCurrent);

			// remove sentence from m_szBuffer
			memmove(m_szBuffer, m_szBuffer + iEnd + 1, strlen(m_szBuffer + iEnd + 1) + 1);
			
			// check validity
			if (!VerifyGPSChecksum(strSentence))
			{
				LibRoadnavDebug1(wxT("SerialIO"), wxT("Valid GPS sentence received: %s"), strSentence.c_str());
				m_tLastGPSSentence = wxDateTime::Now();
				
				//////////////////////////////////////////////////////////////////
				// find the start of a position sentence
				//////////////////////////////////////////////////////////////////
				if (strSentence.Left(6) == wxT("$GPGGA"))
				{
					LibRoadnavDebug1(wxT("SerialIO"), wxT("Got $GPGGA sentence: %s"), strSentence.c_str());
						
					//////////////////////////////////////////////////////////
					// actual longitude and latitude
					//////////////////////////////////////////////////////////
					double fLong, fLat;

					//////////////////////////////////////////////////////////
					// true = northern hemisphere
					// false = southern hemisphere
					//////////////////////////////////////////////////////////
					bool bNorth = false;

					//////////////////////////////////////////////////////////
					// true = eastern hemisphere
					// false = western hemisphere
					//////////////////////////////////////////////////////////
					bool bEast = false;

					
					// Fix type
					// 0 = invalid
					// 1 = valid SPS
					// 2 = valid DGPS
					// 3 = valid PPS
					long lFixType;

					// skip over the $GPGGA
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the UTC
					strSentence = strSentence.AfterFirst(wxT(','));

					// now we have latitude in ddmm.mmmm format
					strSentence.BeforeFirst(',').ToDouble(&fLat);
					strSentence = strSentence.AfterFirst(wxT(','));

					// North or South
					if (strSentence.Left(1) == wxT("N"))
						bNorth = true;

					strSentence = strSentence.AfterFirst(wxT(','));

					// longitutde
					strSentence.BeforeFirst(',').ToDouble(&fLong);
					strSentence = strSentence.AfterFirst(wxT(','));

					// East or west
					if (strSentence.Left(1) == wxT("E"))
						bEast = true;
					
					strSentence = strSentence.AfterFirst(wxT(','));

					// Position fix
					strSentence.BeforeFirst(',').ToLong(&lFixType);
					strSentence = strSentence.AfterFirst(wxT(','));

					// Number of satellites
					strSentence.BeforeFirst(',').ToLong(&m_nSatellites);
					strSentence = strSentence.AfterFirst(wxT(','));

					// HDOP
					strSentence = strSentence.AfterFirst(wxT(','));

					// Altitude in meters
					//strSentence.BeforeFirst(',').ToDouble(&fAltitude);
					strSentence = strSentence.AfterFirst(wxT(','));

					// process the data

					switch (lFixType)
					{
						case 1:
							m_strFixType = wxT("SPS");
							break;
						case 2:
							m_strFixType = wxT("DGPS");
							break;
						case 3:
							m_strFixType = wxT("PPS");
							break;
						default:
							m_strFixType = wxT("Invalid");
							break;
					}

					if (!bNorth)
						fLat *= -1;
					if (!bEast)
						fLong *= -1;

					// now convert ddmm.mmmm to dd.dddddd
					fLat /= 100;
					fLong /= 100;

					double fFraction;
					int iWhole;

					iWhole = (int) fLong;
					fFraction = fLong - iWhole;
					fLong = iWhole + fFraction / 0.6;

					iWhole = (int) fLat;
					fFraction = fLat - iWhole;