GPS Parser and Distance Finding

This is a small, compact library used for parsing GPS GPMRC data written in C. The library is only one header file, it will parse the NEMA strings and put them into user readable data. Unit conversion is done within the library functions to get information such as:

Latitude and Longitude
current time (UTC)
date
heading (both degrees and letter direction)
ground speed (knots, miles per hour, and feet per second)

There is also a function that uses the Harversine formula to determine the distance between two points on the planet. This formula takes into account the earth being an ellipsoid. It will give you distance in Kilometers, miles, and feet.

Below is a small sample program that shows how to parse a GPS NEMA strings. It will also show how to set your current location and a target location and how to use the library to calculate the distance between two points.

struct Packet loc;
struct Position curPos, target;

The first two lines are used to set up the structures that will be used to hold the parsed data as well as the two positions that we will find the distance between.

clearPacket(&loc);
char buffer[256]= {'\0'};
	
fgets(buffer, sizeof(buffer), stdin);

The first function will do what it says, clear the packet structure so that all values are \0. the Buffer will hold the string that is taken from stdin.

if(parseGPS(buffer, &loc)>=0) //the parser will return -1 if the packet is not a full and valid GPS packet. 
{
	printData(&loc);
}
else
{
	printf("not a valid packet\n");
}

This is where all the processing is done. We feed the buffer and the address to the packet structure. The parser function will go through the string and place its contents into the correct location within the structure. The parser will return -1 if there is not a real or valid packet. If a packet is showing correct latitude and longitude but no other values then it is probably a GPGGA string which have the same validation byte, but not the same formating…hence why only part of the data is shown. The printData function will print the data into a neatly formated human readable form. The longitude and latitude are converted to decimal degrees which can be used for things like google maps or other applications that accept lat and lon in decimal degrees.

/////////////////////////////////////////////////////////
//using distance measuring
//set the current position
setPosition(&curPos, loc.lat, loc.lon);

//set the target position using a known coordinate 
setPosition(&target, 38.897701, -77.036637);

here we set two positions. The first set of coordinates are taken from the structure holding all of our current GPS data. The second position is the target, this can be hardcoded into your program, like it is here, or any other way that you want to store them.

//print distance in KM, miles, and feet
	int dist = getDistanceKM(&curPos, &target);
	printf("distance KM:%d\n", dist);
	
	dist = getDistanceMiles(&curPos, &target);
	printf("distance Miles:%d\n", dist);
	
	dist = getDistanceFeet(&curPos, &target);
	printf("distance Feet:%d\n", dist);

once we have the two positions set we can pass them to one of the getDistance functions. There are three variations which will return an int in KM, Feet, or Miles. That is all! if you want to get altitude you can feel free to modify the code in the ParseGPS function and add another IF statement that will read in GPGAA packets. When compiling your code make sure to link the math library which is used for the trig functions sin, cos, acos, etc. Using GCC you could compile with

GCC -Wall -o gpsDecode gpsDecode.c -lm

Below are the two files in their entirety.
GPS_DECODE.c

#include "gpsDecode.h"


int main(void)
{

	
	struct Packet loc;
	struct Position curPos, target;
	

		
	clearPacket(&loc);
	char buffer[256]= {'\0'};
	
	fgets(buffer, sizeof(buffer), stdin);

	if(parseGPS(buffer, &loc)>=0) //the parser will return -1 if the packet is not a full and valid GPS packet. 
	{
		printData(&loc);
	}
	else
	{
		printf("not a valid packet\n");
	}
	
	/////////////////////////////////////////////////////////
	//using distance measuring
	//set the current position
	setPosition(&curPos, loc.lat, loc.lon);
	
	//set the target position using a known coordinate 
	setPosition(&target, 38.897701, -77.036637);
	printf("\n");
	
	//print distance in KM, miles, and feet
	int dist = getDistanceKM(&curPos, &target);
	printf("distance KM:%d\n", dist);
	
	dist = getDistanceMiles(&curPos, &target);
	printf("distance Miles:%d\n", dist);
	
	dist = getDistanceFeet(&curPos, &target);
	printf("distance Feet:%d\n", dist);


	return 0;


}
	

GPS_DECODE.h

//gpsDecoder.h
//decode GPS NEMA data
//This decoder is very lightweight and only parses data from GPRMC packets
//this offers the minimum ammount of data to be parsed and gets you everything but altitude. 
//if you need altitude feel free to modify the parseGPS routine and add an alternate IF statment for GPGGA Packets

//written by: Bill Heaster
//1/20/16
//This shit is free, do what you want with it, just give me some credit. 
//theCreator a.t ApexLogic d0t Net
/////////////////////////////////////////////////////////////////


#include <stdio.h>
#include <math.h> //for harversine sin, cos, acos, etc. 
#include <stdlib.h>
#include <string.h>



struct Packet
{
	
	//decimal degree format for use in mapping software. ie. google maps 
	float lat;
	float lon;
	
	//latitude and longitude
	int latDegree;
	double latMinSec;
	
	int lonDegree;
	double lonMinSec;
	
	//time
	int timeHours;
	int timeMinutes;
	int timeSeconds;
	
	//date
	int day;
	int month;
	int year;
	
	//hemisphere
	char latHem;
	char lonHem;
	
	//altitude
	int alt;
	
	//number of fixed satalites
	int numSats;
	
	//movement

	float heading;
	char headingDirection[3];
	
	float speedKnots;
	float speedFPS;
	float speedMPH;
	
	int isValid;
	int isLatNeg;
	int isLonNeg;
	
};

struct Position
{
	float lat;
	float lon;	
	float latRad;
	float lonRad;
};

int getDistanceKM(struct Position * point1, struct Position * point2)
{
	
	//sanity check
	if(point1->lat<0)
		point1->lat*=-1;
	if(point1->lon<0)
		point1->lon*=-1;
	if(point2->lat<0)
		point2->lat*=-1;
	if(point2->lon<0)
		point2->lon*=-1;

	
	
	//convert decimal degrees to radians
	point1->latRad = point1->lat * 0.017453293;
	point1->lonRad = point1->lon * 0.017453293;
	point2->latRad = point2->lat * 0.017453293;
	point2->lonRad = point2->lon * 0.017453293;
	
	//find the difference values between lat and lon radians
	float diffLat, diffLon = 0;
	
	if(point1->latRad > point2->latRad)
	{
		diffLat = point1->latRad - point2->latRad;
	}
	else
	{
		diffLat = point2->latRad - point1->latRad;
	}
	if(point1->lonRad > point2->lonRad)
	{
		diffLon = point1->lonRad - point2 ->lonRad;
	}
	else
	{
		diffLon = point2 ->lonRad - point1->lonRad;
	}
	
	

	
	float latSin = sin(diffLat/2);
	float lonSin = sin(diffLon/2);
	
	float distanceKM = 2* asin(sqrt((latSin*latSin)+cos(point1->latRad) *cos(point2->latRad) * (lonSin *lonSin)));
	distanceKM *= 6371;
	
	
		
	return (int)distanceKM;
	
}


int getDistanceMiles(struct Position * point1, struct Position * point2)
{
	
	//sanity check
	if(point1->lat<0)
		point1->lat*=-1;
	if(point1->lon<0)
		point1->lon*=-1;
	if(point2->lat<0)
		point2->lat*=-1;
	if(point2->lon<0)
		point2->lon*=-1;

	
	
	//convert decimal degrees to radians
	point1->latRad = point1->lat * 0.017453293;
	point1->lonRad = point1->lon * 0.017453293;
	point2->latRad = point2->lat * 0.017453293;
	point2->lonRad = point2->lon * 0.017453293;
	
	//find the difference values between lat and lon radians
	float diffLat, diffLon = 0;
	
	if(point1->latRad > point2->latRad)
	{
		diffLat = point1->latRad - point2->latRad;
	}
	else
	{
		diffLat = point2->latRad - point1->latRad;
	}
	if(point1->lonRad > point2->lonRad)
	{
		diffLon = point1->lonRad - point2 ->lonRad;
	}
	else
	{
		diffLon = point2 ->lonRad - point1->lonRad;
	}
	
	

	
	float latSin = sin(diffLat/2);
	float lonSin = sin(diffLon/2);
	
	float distanceKM = 2* asin(sqrt((latSin*latSin)+cos(point1->latRad) *cos(point2->latRad) * (lonSin *lonSin)));
	
	distanceKM *= 6371;
	
	distanceKM*= 0.621371192;
	
		
	return (int)distanceKM;
	
}

int getDistanceFeet(struct Position * point1, struct Position * point2)
{
	
	//sanity check
	if(point1->lat<0)
		point1->lat*=-1;
	if(point1->lon<0)
		point1->lon*=-1;
	if(point2->lat<0)
		point2->lat*=-1;
	if(point2->lon<0)
		point2->lon*=-1;

	
	
	//convert decimal degrees to radians
	point1->latRad = point1->lat * 0.017453293;
	point1->lonRad = point1->lon * 0.017453293;
	point2->latRad = point2->lat * 0.017453293;
	point2->lonRad = point2->lon * 0.017453293;
	
	//find the difference values between lat and lon radians
	float diffLat, diffLon = 0;
	
	if(point1->latRad > point2->latRad)
	{
		diffLat = point1->latRad - point2->latRad;
	}
	else
	{
		diffLat = point2->latRad - point1->latRad;
	}
	if(point1->lonRad > point2->lonRad)
	{
		diffLon = point1->lonRad - point2 ->lonRad;
	}
	else
	{
		diffLon = point2 ->lonRad - point1->lonRad;
	}
	
	

	
	float latSin = sin(diffLat/2);
	float lonSin = sin(diffLon/2);
	
	float distanceKM = 2* asin(sqrt((latSin*latSin)+cos(point1->latRad) *cos(point2->latRad) * (lonSin *lonSin)));
	distanceKM *= 6371;
	
	distanceKM*= 0.621371192;
	
	distanceKM *= 5282;
		
	return (int)distanceKM;
	
}

void clearPacket(struct Packet * loc)
{
	loc->latDegree = 0;
	loc->lonDegree =0;
	loc->latMinSec =0;
	loc->lonMinSec =0;
	loc->timeHours =0;
	loc->timeMinutes =0;
	loc->timeSeconds =0;
	loc->day =0;
	loc->month=0;
	loc->year=0;
	loc->latHem='\0';
	loc->lonHem='\0';
	loc->alt=0;
	loc->numSats=0;
	loc->heading=0;
	loc->speedFPS =0;
	loc->speedKnots =0;
	loc->speedMPH =0;
	loc->isValid=0;
	loc->isLatNeg = 0;
	loc->isLonNeg =0;
	bzero(loc->headingDirection, sizeof(loc->headingDirection));

	return;
}

//used to set the positions within the position structure. this must be done when using the distance measuring routines
void setPosition(struct Position *p, float lat, float lon)
{
	p->lat = lat;
	p->lon = lon;
	
	return;
}

float convertToDD(int degree, float minSec)
{
	float a = 0.00000;
	
	float temp = 0.00000;
	
	temp = minSec/60;
	
	a = degree + temp;
	
	
	
	return a;
	
}

//gps sends us data in knots, convert it
float knotsToMPH(float knots)
{
	
	return knots * 1.15077945;
	
	
	
}

//more percision if your module provides it
float mphToFPS(float mph)
{
	
	return mph * 1.46667;
	
}


//used for the heading when the compass goes from 0 to 360
int checkbounds(int num)
{
	if(num>360)
	{
		return num -=360;
	}
	
	if(num < 0)
	{
		return num +=360;
	}
	
	
	return num;
}

//adds threshold checking and turns the heading degrees into N, E, S , W, NE, NW, SE, SW headings
void setHeading(struct Packet * loc)
{
	int th= 15; //threshold in degrees
	int in = (int)loc->heading;	

	
	int north = 0;
	int east = 90;
	int south = 180;
	int west = 270;
	
	int nl = checkbounds(north-th);
	int nh = checkbounds(north+th);
	
	int el = checkbounds(east -th);
	int eh = checkbounds(east +th);
	
	int sl = checkbounds(south-th);
	int sh = checkbounds(south +th);
	
	int wl = checkbounds(west-th);
	int wh = checkbounds(west+th);
	
	if((in >=nl && in <=360)||(in>=0 && in<=nh))
	{
		loc->headingDirection[0] ='N';
		return;
	}
	if(in >=el && in <=eh)
	{
		loc->headingDirection[0] ='E';
		return;
	}
	if(in >=wl && in <=wh)
	{
		loc->headingDirection[0] ='W';
		return;
	}
	if(in >=sl && in <=sh)
	{
		loc->headingDirection[0] ='S';
		return;
	}
	
	if(in<el && in>nh)
	{
		loc->headingDirection[0] ='N';
		loc->headingDirection[1] ='E';
	}
	if(in>eh && in<sl)
	{
		loc->headingDirection[0] ='S';
		loc->headingDirection[1] ='E';
	}
	if(in>sh && in<wl)
	{
		loc->headingDirection[0] ='S';
		loc->headingDirection[1] ='W';
		return;
	}
	if(in>wh && in<nl)
	{
		loc->headingDirection[0] ='N';
		loc->headingDirection[1] ='W';
		return;
	}
	else
	loc->headingDirection[0] = 'u';
	
	return;
		
}

void printData(struct Packet * loc)
{

	
	printf("\n-------------------------------------------------------\n");
	printf("time:%d:%d:%d\t", loc->timeHours, loc->timeMinutes, loc->timeSeconds);
	printf("date:%d/%d/%d\n", loc->month, loc->day, loc->year);
	printf("latitude: %f %c\t",loc->lat, loc->latHem);
	printf("longitude: %f %c\n", loc->lon, loc->lonHem);
	
	
	
	loc->speedMPH= knotsToMPH(loc->speedKnots);
	loc->speedFPS = mphToFPS(loc->speedMPH);
	
	setHeading(loc);
	
	printf("knots:%f\tMPH:%f\tFPS:%f\n", loc->speedKnots, loc->speedMPH, loc->speedFPS);
	printf("heading:%f\t\tDirection:%s",loc->heading,loc->headingDirection);
	printf("\n-------------------------------------------------------\n");
	
	return;
}


int parseGPS(char * input, struct Packet * packet)
{
	
	char* currentLocation = input;
	
	char temp[128];
	bzero(temp, sizeof(temp));
	
	
	//check the string to see what kind of GPS packet it is. 
	//This module can only receive GPGSA, GPGGA, GPMRC 
	//All the data needed to fill the struct can be captured from the GPMRC packets
	//so we will process these, and drop all others. 
	//$GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
	
	
	char * needle = "$GPRMC";
	int len = strlen(needle);
	
	
	//if we have a valid GPMRC start code
	if(strstr(input, needle)!=NULL)
	{
		//put us on the first hour char
		currentLocation += (len+1);
		
		//next two are hours, place it in the struct
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->timeHours = atoi(temp);
		
		//get the minutes
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->timeMinutes = atoi(temp);
		
		//get seconds
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
				
		
		packet->timeSeconds = atoi(temp);
		bzero(temp, sizeof(temp));
		
		//current location on decimal point increment +5
		currentLocation+=5;
		
		//current location on packet status
		//if this isnt a valid packet clear the structure and return non-valid
		if(*currentLocation != 'A')
		{
			clearPacket(packet);
			return -1;
		}
		
		//increment +2 
		currentLocation +=2;
		
		//current location, first char of degree latitude
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->latDegree = atoi(temp);
		
		//current location is lat min first char
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		temp[2] = *currentLocation++;
		temp[3] = *currentLocation++;
		temp[4] = *currentLocation++;
		temp[5] = *currentLocation++;
		temp[6] = *currentLocation++;
		
		packet->latMinSec = atof(temp);
		bzero(temp, sizeof(temp));
		currentLocation++;
		
		//current location is N/S indicator
		packet->latHem = *currentLocation++;
		currentLocation++;
		
		//current location is first char degree lon
		if(*currentLocation == '0')
		{
			packet->isLonNeg = 1;
			currentLocation++;
			temp[0]=*currentLocation++;
			temp[1]=*currentLocation++;
			packet->lonDegree= atoi(temp);
			
		}
		else
		{
			temp[0] = *currentLocation++;	
			temp[1] = *currentLocation++;
			temp[2] = *currentLocation++;
			packet->lonDegree= atoi(temp);
		}
		

		
		//current location is first char of lon minutes
		
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		temp[2] = *currentLocation++;
		temp[3] = *currentLocation++;
		temp[4] = *currentLocation++;
		temp[5] = *currentLocation++;
		temp[6] = *currentLocation++;
		
		
		packet->lonMinSec= atof(temp);
		currentLocation++;
		
		//need to do this here i guess
		//convert coordinates into cartesian
		packet->lat = convertToDD(packet->latDegree, packet->latMinSec);
	
		packet->lon = convertToDD(packet->lonDegree, packet->lonMinSec);
		
		if(packet->isLonNeg)
		{
			packet->lon *= -1;
		}
		
		//current location is EW indicator
		packet->lonHem = *currentLocation++;
		currentLocation++;
		bzero(temp, sizeof(temp));
		
		//current location is speed in knots
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		temp[2] = *currentLocation++;
		temp[3] = *currentLocation++;
		
		packet->speedKnots= atof(temp);
		currentLocation++;
		bzero(temp, sizeof(temp));
		
		//current location is heading
		
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		temp[2] = *currentLocation++;
		temp[3] = *currentLocation++;
		temp[4] = *currentLocation++;
		temp[5] = *currentLocation++;
		
		packet->heading = atof(temp);
		currentLocation++;
		bzero(temp, sizeof(temp));
		
		//current location is day of date
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->day = atoi(temp);
		
		//current location is month
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->month = atoi(temp);
		
		//current location is year		
		temp[0] = *currentLocation++;
		temp[1] = *currentLocation++;
		
		packet->year = atoi(temp);		
		
		
	}	
	
	return 0;
	
}