gpsinterface_serial.cpp

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

					fLat = iWhole + fFraction / 0.6;
					
					// are they non zero (i.e. good fix)?
					if (fabs(fLong) > 1e-6 && fabs(fLat) > 1e-6)
					{
						m_ptGPS.Set(fLong, fLat);

						LibRoadnavDebug1(wxT("SerialIO"), wxT("Lat and long appear valid - %s"), m_ptGPS.FormatPoint().c_str());

						m_bSeenGPGGA = true;
						m_tLastGPSLock = wxDateTime::Now();
					} // if (fabs(fLong) > 1e-6 && fabs(fLat) > 1e-6)
				} // if (strSentence.Left(6) == wxT("$GPGGA"))


				//////////////////////////////////////////////////////////////////
				// find the start of a course over ground sentence
				//////////////////////////////////////////////////////////////////
				if (strSentence.Left(6) == wxT("$GPRMC"))
				{
					LibRoadnavDebug1(wxT("SerialIO"), wxT("Got $GPRMC sentence: %s"), strSentence.c_str());

					//////////////////////////////////////////////////////////
					// found what appears to be the right sentence
					//////////////////////////////////////////////////////////

					// skip over the $GPRMC
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the UTC
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the status
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the latitude
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the N/S
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the longitude
					strSentence = strSentence.AfterFirst(wxT(','));
					// skip over the E/W
					strSentence = strSentence.AfterFirst(wxT(','));
					
					// speed in knots
					strSentence.BeforeFirst(wxT(',')).ToDouble(&m_fSpeed);
					strSentence = strSentence.AfterFirst(wxT(','));
					// convert to MPH
					m_fSpeed *= 1.151;
					
					// heading
					strSentence.BeforeFirst(wxT(',')).ToDouble(&m_fHeading);
					strSentence = strSentence.AfterFirst(wxT(','));
					
					if ((fabs(m_fSpeed) < 1e-6 && fabs(m_fHeading) < 1e-6) || m_fSpeed < m_fMinimumSpeedForHeading)
					{
						// GPS unit isn't reporting heading, or we're traveling too slow for an accurate heading
						m_fHeading = m_fLastHeading;

						LibRoadnavDebug1(wxT("SerialIO"), wxT("New heading: %.1f (using last heading)"), m_fHeading);
					}
					else
					{
						m_fLastHeading = m_fHeading;

						LibRoadnavDebug1(wxT("SerialIO"), wxT("New heading: %.1f (from sentence)"), m_fHeading);
					}
					
					m_bSeenGPRMC = true;
				} // if (strSentence.Left(6) == wxT("$GPRMC"))
				
				//////////////////////////////////////////////////////////////////
				// find the start of a satellite sentence
				//////////////////////////////////////////////////////////////////
				if (strSentence.Left(6) == wxT("$GPGSV"))
				{
					LibRoadnavDebug1(wxT("SerialIO"), wxT("Got $GPGSV sentence: %s"), strSentence.c_str());

					//////////////////////////////////////////////////////////
					// found what appears to be the right sentence
					//////////////////////////////////////////////////////////
					
					long nMessagesPerSet = 0;
					long lSequenceNumber = 0;
					long lSatellitesVisible = 0;
					
					// skip over the $GPGSV
					strSentence = strSentence.AfterFirst(wxT(','));

					// number of messages in set
					strSentence.BeforeFirst(wxT(',')).ToLong(&nMessagesPerSet);
					strSentence = strSentence.AfterFirst(wxT(','));

					// sequence number
					strSentence.BeforeFirst(wxT(',')).ToLong(&lSequenceNumber);
					strSentence = strSentence.AfterFirst(wxT(','));

					// satellites visible
					strSentence.BeforeFirst(wxT(',')).ToLong(&lSatellitesVisible);
					strSentence = strSentence.AfterFirst(wxT(','));
					
					if (lSequenceNumber == 1)
						vSatelliteInfoTmp.clear();
					
					while (strSentence != wxT(""))
					{
						long lID = -1;
						SSatelliteInfo sInfo;
						wxString strTmp;
						
						sInfo.m_fAzimuth = -1;
						sInfo.m_fElevation = -1;
						sInfo.m_fSNR = 0;

						// ID
						strTmp = strSentence.BeforeFirst(wxT(','));
						if (strTmp != wxT(""))
							strTmp.ToLong(&lID);
						strSentence = strSentence.AfterFirst(wxT(','));
						sInfo.m_iID = lID;
					
						// Elevation
						strTmp = strSentence.BeforeFirst(wxT(','));
						if (strTmp != wxT(""))
							strTmp.ToDouble(&sInfo.m_fElevation);
						strSentence = strSentence.AfterFirst(wxT(','));
					
						// Azimuth
						strTmp = strSentence.BeforeFirst(wxT(','));
						if (strTmp != wxT(""))
							strTmp.ToDouble(&sInfo.m_fAzimuth);
						strSentence = strSentence.AfterFirst(wxT(','));
					
						// SNR
						strTmp = strSentence.BeforeFirst(wxT(','));
						if (strTmp != wxT(""))
							strTmp.ToDouble(&sInfo.m_fSNR);
						strSentence = strSentence.AfterFirst(wxT(','));
					
						// validate info
						if (strSentence != wxT("") &&
							sInfo.m_fAzimuth >= 0 &&
							sInfo.m_fElevation >= 0 &&
							sInfo.m_fSNR >= 0 &&
							sInfo.m_iID > 0)
						{
							vSatelliteInfoTmp.push_back(sInfo);
						}
					}
						
					// check if this is the last message in the set
					if (lSequenceNumber == nMessagesPerSet && nMessagesPerSet > 0)
					{
						m_vSatelliteInfo = vSatelliteInfoTmp;
						vSatelliteInfoTmp.clear();
					}
				} // if (strSentence.Left(6) == wxT("$GPGSV"))
			} // if (!VerifyGPSChecksum(strSentence))
		} // while (1)

		LibRoadnavDebug2(wxT("SerialIO"), wxT("bSeenGPGGA = %d, bSeenGPRMC = %d"), m_bSeenGPGGA, m_bSeenGPRMC);
		
		if (m_bSeenGPGGA || m_bSeenGPRMC)
		{
			m_bSeenGPGGA = false;
			m_bSeenGPRMC = false;

			*pGPSEvent = wxGPSEvent(1, 1, 1, m_ptGPS, m_fSpeed, m_fHeading, m_strFixType, m_nSatellites, m_vSatelliteInfo, wxT(""));

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

			return GPSStatusOK;
		}
		else if (wxDateTime::Now() - m_tLastGPSLock > wxTimeSpan(0, 0, 10, 0) &&
					wxDateTime::Now() - tStart > wxTimeSpan(0, 0, 10, 0))
		{
			// no lock
			*pGPSEvent = wxGPSEvent(1, 1, 0, Point(0, 0), 0, 0, wxT("None"), 0, m_vSatelliteInfo, wxT(""));

			return GPSStatusOK;
		}

		// check if thread wants to terminate
		if (m_pThread)
		{
			if (m_pThread->TestDestroy())
			{
				m_vSatelliteInfo.clear();
				*pGPSEvent = wxGPSEvent(1, 0, 0, Point(0, 0), 0, 0, wxT("None"), 0, m_vSatelliteInfo, wxT(""));

				return GPSStatusOK;
			}
		}
			
		// check for serial inactivity
		if (wxDateTime::Now() - m_tLastGPSSentence > wxTimeSpan(0, 0, 5, 0))
		{
			LibRoadnavDebug0(wxT("SerialIO"), wxT("GPS timeout. Restarting serial I/O"));

			// no serial activity for 5 seconds .. try reconnecting to serial port
			ShutdownSerialIO();
			InitSerialIO();
		}
		
		wxThread::Sleep(500);
	}
	
	wxASSERT(0);

	return GPSStatusOK;
}

wxString GPSInterface_Serial::GetLastError()
{
	return m_strLastError;
}

IGPSInterface::EGPSStatus GPSInterface_Serial::AutoDetect(wxThread * pThread)
{
	int iPort;
	int arRates[] = {4800, 9600, 19200, 38400, 0};
	int iRate;
	bool bGPSDetected = false;
	
	m_pThread = pThread;

	{	
		// try current settings first
		wxGPSEvent ev;
		if (m_bSerialInitialized)
		{
			ShutdownSerialIO();
			m_bSerialInitialized = false;
		}
		
		if (GetData(&ev) == GPSStatusOK)
		{
			if (ev.m_bLocked)
			{
				m_pThread = NULL;
				return GPSStatusOK;
			}
		}
		
		if (ev.m_bActive)
			bGPSDetected = true;
	}
	
	for (iPort = 0; EnumSerialPort(iPort) != wxT(""); iPort++)
	{
		wxString strPort = EnumSerialPort(iPort);
		
		for (iRate = 0; arRates[iRate]; iRate++)
		{
			wxGPSEvent ev;
			
			g_pConfig->Write(wxT("GPSSerialPort"), strPort);
			g_pConfig->Write(wxT("GPSBaudRate"), arRates[iRate]);
			
			if (m_bSerialInitialized)
			{
				ShutdownSerialIO();
				m_bSerialInitialized = false;
			}
			
			if (GetData(&ev) == GPSStatusOK)
			{
				if (ev.m_bLocked)
				{
					m_pThread = NULL;
					return GPSStatusOK;
				}
			}

			if (ev.m_bActive)
				bGPSDetected = true;
			
			if (pThread && pThread->TestDestroy())
			{
				m_pThread = NULL;
				return GPSStatusAutoDetectionNoGPSDetected;
			}
		}
	}

	m_pThread = NULL;
	
	if (bGPSDetected)
		return GPSStatusAutoDetectionGPSDetectedButNoLock;
		
	return GPSStatusAutoDetectionNoGPSDetected;
}

#endif