Personal tools
Document Actions

GPS statistics cpp

by aullmj — last modified 2009-02-13 17:28

source code for GPS program

Click here to get the file

Size 6.3 kB - File type text/x-c++src

File contents

// ReadGPS.cpp : Demo for reading NMEA format GPS data
//see NMEA reference: http://www.kh-gps.de/nmea-faq.htm
// or http://home.mira.net/~gnb/gps/nmea.html
#include <windows.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <time.h>
using namespace std;

HANDLE OpenComPort(int nPortNo=1,int BaudRate = CBR_4800,int ByteSize = 8,int Parity = NOPARITY,int StopBits = ONESTOPBIT);
bool GetGPSFix(HANDLE COMGPS,int &TIME,double& LAT,double& LON);
const int NUMPTS=5*60;//buffer for 5 minutes at 1 reading per second

void main(void)//int argc, char* argv[]
{
	time_t start,stop;
	bool error;
	int ii,TIME;
	double latdat[NUMPTS],londat[NUMPTS];
	double avgLAT=0,avgLON=0,tmp1,tmp2;
	double covar[4]={0,0,0,0};//lat^2,lon^2,lat*lon,dist^2
	fstream out("gpsdata.txt",ios::out);
	cout << "GPS statistics recording tool by Mark Aull"<<endl;
	cout << "This program is intended to run while the GPS unit is stationary to calculate the actual position and noise present."<<endl;

	start=time(NULL);
	HANDLE m_hFile=OpenComPort(1,CBR_4800);
	if (m_hFile==NULL)
	{
		cout << "Com port failed: don't ask me what to do."<<endl;
		return;
	}

	for (ii=0;ii<NUMPTS;ii++)
	{
		start=time(NULL);
		error=GetGPSFix(m_hFile,TIME,latdat[ii],londat[ii]);
		avgLAT=(ii*avgLAT+latdat[ii])/(ii+1);//running average lat
		avgLON=(ii*avgLON+londat[ii])/(ii+1);//running average lon

		if (!error)
		{
			for (int jj=0;jj<=ii;jj++)
			{
				tmp1=100000*(latdat[jj]-avgLAT);
				tmp2=100000*(londat[jj]-avgLON);
				covar[0]=tmp1*tmp1/(ii+1);//covar lat^2
				covar[1]=tmp2*tmp2/(ii+1);//covar lon^2
				covar[2]=tmp1*tmp2/(ii+1);//covar lat*lon
				tmp1=tmp1*tmp1+tmp2*tmp2;//distance^2 from mean
				covar[3]=tmp1/(ii+1);//no square here, already done
			}

			out <<"TIME:\t"<< TIME<<setiosflags(ios::fixed)<<setprecision(6);
			out <<"\tLAT:\t"<<latdat[ii]<<"\tLON:\t"<<londat[ii];
			out<<"\tcovariance:\t"<<setprecision(3)<<covar[0]<<'\t'<<covar[1]<<'\t'<<covar[2]<<'\t'<<covar[3];

			cout /*<<"TIME: "<< TIME*/<<setiosflags(ios::fixed)<<setprecision(6);
			cout <<"LAT: "<<avgLAT<<' '<<"LON: "<< avgLON;
			cout<<"covariance: "<<setprecision(3)<<covar[0]<<' '<<covar[1]<<' '<<covar[2]<<' '<<covar[3]<<'\n';

		}
		stop=time(NULL);
		out << stop-start<<" seconds elapsed"<<'\n';
	}
	CloseHandle( m_hFile ) ;
	out.close();
	return;
}

HANDLE OpenComPort(int nPortNo,int BaudRate,int ByteSize,int Parity,int StopBits)
{
	char strPort[256];
//	char m_strReceivedData[256];
	HANDLE m_hFile;
	BOOL fSuccess;
	COMMTIMEOUTS timeout;
	DCB dcb;

	sprintf(strPort, "COM%d", nPortNo) ;

	//Create the handle for read the COM port
	m_hFile =  CreateFileA( strPort,                    
					GENERIC_READ | GENERIC_WRITE,
					(DWORD)NULL,   // exclusive access
					NULL,          // no security
					OPEN_EXISTING,
					FILE_ATTRIBUTE_NORMAL,
					NULL           // hTemplate
					);

	//Check weather the handle is valid or not
	if (INVALID_HANDLE_VALUE == m_hFile)
	{
		cout<<"Error: Could not open the RS232 port!"<<endl;
		return NULL;
	}

	// set up the comms parameters now that we have a handle
	SetupComm(m_hFile,(DWORD)2048, (DWORD)2048);

	// Set up the DCB with our settings
	// Get it first so we fill all members
	fSuccess = GetCommState(m_hFile,&dcb);

	if (!fSuccess) 
	{
	// Handle the error.
		cout<<"Error: Failed to Get the System Communication Settings."<<endl;
		return NULL;
	}

	dcb.BaudRate = BaudRate;
	dcb.ByteSize = ByteSize;			// data size
	dcb.Parity = Parity;		// No Parity Bit
	dcb.StopBits = StopBits;

	// assign it
	fSuccess = SetCommState(m_hFile, &dcb);

	//Now check the configuration of the communication device is valid or 
	//not after assign the comms parameters
	if ( fSuccess == 0 )
	{
		cout<<"Error: In Control Setting for a Serial Communications Device."<<endl;
		return NULL;
	}

	// Set up the timeouts to use, they are quite short, since we will loop anyway.
	// Do not make them zero, else we will have a CPU load problem. Too large a value, 
	// and we have to wait for comms to time out when shutting down

	// fill timeout structure
	GetCommTimeouts(m_hFile, &timeout);
	timeout.ReadIntervalTimeout = 100;           // 500ms between incomming chars.
	timeout.ReadTotalTimeoutConstant = 500;
	timeout.ReadTotalTimeoutMultiplier = 0;      
	timeout.WriteTotalTimeoutConstant = 2000;
	timeout.WriteTotalTimeoutMultiplier = 60;    // 60ms per char sent
	                                            
	SetCommTimeouts(m_hFile, &timeout);
	return m_hFile;
}

bool GetGPSFix(HANDLE COMGPS,int &TIME,double& LAT,double& LON)
{
	BOOL error;
	char *ch=new char[10];
	ch[0]=0;
	DWORD numberOfBytesRead = 0; 

	while (1)
	{
		while (ch[0]!='$')//get each character until a beginning of a message is found
			error = ReadFile(COMGPS,ch,1,&numberOfBytesRead,NULL);
		error = ReadFile(COMGPS,ch,5,&numberOfBytesRead,NULL);//read the message type
		ch[5]='\0';
		//cout <<"Prefix: "<<ch<<endl;
		if (0==strcmp("GPGGA",ch))//if it is a GPS fix message,
		{
			error = ReadFile(COMGPS,ch,1,&numberOfBytesRead,NULL);//,
			error = ReadFile(COMGPS,ch,6,&numberOfBytesRead,NULL);//UTC
			if(numberOfBytesRead == 0)
			{
				cout << "No messages on COM port."<<endl;
				return 1;
			}
			else if (ch[0]==',')
			{
				cout << "No satellite fix"<<endl;
				return 1;
			}
			ch[6]=0;
			TIME=atoi(ch);
			error = ReadFile(COMGPS,ch,1,&numberOfBytesRead,NULL);//,
			error = ReadFile(COMGPS,ch,11,&numberOfBytesRead,NULL);//LAT
			if(numberOfBytesRead == 0)
			{
				cout << "No messages on COM port."<<endl;
				return 1;
			}
			else if (ch[0]==',')
			{
				cout << "No satellite fix"<<endl;
				return 1;
			}
			ch[9]=0;
			LAT=(ch[10]=='S'?-1:1)*(atoi(ch)/100+atof(&ch[2])/60.);
			ch[9]=ch[10];
			ch[10]=0;

			error = ReadFile(COMGPS,ch,1,&numberOfBytesRead,NULL);//,
			error = ReadFile(COMGPS,ch,12,&numberOfBytesRead,NULL);//LON
			if(numberOfBytesRead == 0)
			{
				cout << "No messages on COM port."<<endl;
				return 1;
			}
			else if (ch[0]==',')
			{
				cout << "No satellite fix"<<endl;
				return 1;
			}
			ch[10]=0;
			LON=(ch[11]=='W'?-1:1)*(atoi(ch)/100+atof(&ch[3])/60.);
			ch[10]=ch[11];
			ch[11]=0;
			return 0;
		}
		else
			error = ReadFile(COMGPS,ch,10,&numberOfBytesRead,NULL);
	}
}