gpsinterface_serial.cpp

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

					bool bNorth = false;
					bool bEast = false;
					
					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(','));
					
					// status
					if (strSentence.Left(1) == wxT("A"))
					{
						if (m_strFixType == wxT("Invalid"))
							m_strFixType = wxT("SPS");
					}
					else if (strSentence.Left(1) == wxT("V"))
					{
						m_strFixType = wxT("Invalid");
					}
					
					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(','));

					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;
					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_tLastGPSLock = wxDateTime::Now();
					} // if (fabs(fLong) > 1e-6 && fabs(fLat) > 1e-6)

					// 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)
					{
						unsigned int i;
						
						m_nSatellitesVisible = vSatelliteInfoTmp.size();
						for (i = 0; i < vSatelliteInfoTmp.size(); i++)
							m_arSatellitesVisible[i] = vSatelliteInfoTmp[i];
							
						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) && wxDateTime::Now() - m_tLastGPSLock <= wxTimeSpan(0, 0, 10, 0))
		{
			m_bSeenGPGGA = false;
			m_bSeenGPRMC = false;

			*pGPSEvent = wxGPSEvent(1, 1, 1, m_ptGPS, m_fSpeed, m_fHeading, m_strFixType, m_nSatellites, m_arSatellitesVisible, m_nSatellitesVisible, 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
			*pGPSEvent = wxGPSEvent(1, 0, 0, Point(0, 0), 0, 0, wxT("None"), 0, NULL, 0, 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_arSatellitesVisible, m_nSatellitesVisible, wxT(""));

			return GPSStatusOK;
		}

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

				return GPSStatusOK;
			}
		}
			
		// check for serial inactivity
		if (wxDateTime::Now() - m_tLastSerialActivity > 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();

			m_tLastSerialActivity = wxDateTime::Now();
		}
		
		wxThread::Sleep(500);
	}
	
	wxASSERT(0);

	return GPSStatusOK;
}

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

//////////////////////////////////////////////////////////////////////////////
///
/// \brief Attempt to autodetect if a GPS unit is attached to a serial port.
///
/// \param pThread			Pointer to wxThread obtain this code is running
///							in.
///
/// \return					Status - either GPSStatusOK if a gps unit
///							was found and a lock was detected, or 
///							GPSStatusAutoDetectionGPSDetectedButNoLock
///							if a gps unit was found, but wasn't locked, or
///							GPSStatusAutoDetectionNoGPSDetected for
///							no gps units.
///
//////////////////////////////////////////////////////////////////////////////
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