Main Site Documentation

Critique my GPS parser


#1

I finally bit the bullet and decided to make my own little GPS parser.

I have not tested the code yet, as I am still working on it, but I figured in the mean time, I’d post it here to see if anyone had any suggestions on it.

So, if you have a thought, post it. I don’t care how negative it is. All criticism is good.

EDIT: For those that don’t want to read through 350 lines of code here, I also uploaded it to pastbin: http://pastebin.com/jkUJy5v2

/*
 * TinyGPS -- A very small GPS parser for NETMF
 *	Coded by Chris Seto 2010
 *	
 * This sofware is released under the Apache 2.0 license, copyright Chris Seto 2010
 * 
 * Some code derived from other sources, such as MFGps
 * */

using System;
using Microsoft.SPOT;
using ChrisSeto.GeoInfoSystems.System;
using ChrisSeto.GeoInfoSystems.GeoInfoAPI;
using System.IO.Ports;
using System.Text;
using System.Threading;

namespace ChrisSeto.GeoInfoSystems.MicroGPS
{
	/// <summary>
	/// GPS fix levels
	/// </summary>
	public enum FixType
	{
		Void,
		Active,
		Unknown,
	}

	/// <summary>
	/// $GPRMC Output
	/// </summary>
	public struct RMCOutput
	{
		/*
		    $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
		    0      1      2 3        4 5         6 7     8     9      10    11

			Where:
				0 RMC          Recommended Minimum sentence C
				1 123519       Fix taken at 12:35:19 UTC
				2 A            Status A=active or V=Void.
				3/4 4807.038,N   Latitude 48 deg 07.038' N
				5/6 01131.000,E  Longitude 11 deg 31.000' E
				6 022.4        Speed over the ground in knots
				7 084.4        Track angle in degrees True
				8 230394       Date - 23rd of March 1994
				9/10 003.1,W      Magnetic Variation
				11 *6A          The checksum data, always begins with *
		 */

		/// <summary>
		/// Where we are going, degrees
		/// </summary>
		double Heading;

		/// <summary>
		/// How fast we are going, knots
		/// </summary>
		double Speed;

		/// <summary>
		/// Our current location
		/// </summary>
		Location Coords;

		/// <summary>
		/// Our GPS fix status, Void or Active
		/// </summary>
		FixType Fix;

		public RMCOutput(string[] rmcParams)
		{
			// Heading
			Heading = double.Parse(rmcParams[7]);

			// Speed
			Speed = double.Parse(rmcParams[6]);

			// Coords
			// Parse into degree time format
			double[] lat = GPSHelper.ParseGPSDegreeTime(rmcParams[3]);
			double[] lon = GPSHelper.ParseGPSDegreeTime(rmcParams[5]);

			// Create the new location
			Coords = new Location(GPSHelper.DegreeTmeToDegrees(lat), GPSHelper.DegreeTmeToDegrees(lon) * -1, 0);

			// Fix status
			switch (rmcParams[2])
			{
				case "A":
					Fix = FixType.Active;
					break;

				case "V":
					Fix = FixType.Void;
					break;

				default:
					Fix = FixType.Unknown;
					break;
			}
		}
	}

	/// <summary>
	/// 
	/// </summary>
	/// <param name="rmc"></param>
	public delegate void GPSUpdateDelegate(RMCOutput rmc);

	public static class GPSHelper
	{
		/// <summary>
		/// Parse out the concatenated degree time format the GPS returns
		/// </summary>
		/// <param name="from"></param>
		/// <returns></returns>
		public static double[] ParseGPSDegreeTime(string from)
		{
			double[] output = new double[3];

			if (from.Length == 10)
			{
				output[0] = int.Parse(from.Substring(0, 2));
				output[1] = int.Parse(from.Substring(2, 2));
				output[2] = ToDouble(from.Substring(4, 4)) * 60;
			}
			else if (from.Length == 9)
			{
				output[0] = int.Parse(from.Substring(0, 3));
				output[1] = int.Parse(from.Substring(3, 2));
				output[2] = ToDouble(from.Substring(5, 4)) * 60;
			}

			return output;
		}

		/// <summary>
		/// Convert degree time to degrees
		/// </summary>
		/// <param name="degreeTimeInput"></param>
		/// <returns></returns>
		public static double DegreeTmeToDegrees(double[] degreeTimeInput)
		{
			return (degreeTimeInput[0] + (degreeTimeInput[1] * (1 / 60)) + (degreeTimeInput[2] * (1 / 60) * (1 / 60)));
		}

		/// <summary>
		/// Calculate checksum and compare
		/// </summary>
		/// <param name="sentence"></param>
		/// <returns></returns>
		public static bool IsValidChecksum(string sentence)
		{
			if (sentence.IndexOf('*') > -1)
			{
				string valid_checksum = sentence.Substring(sentence.IndexOf('*') + 1, 2);
				sentence = sentence.Substring(1, sentence.IndexOf('*') - 1);

				int checksum = 0;

				for (int i = 0; i < sentence.Length; i++)
					checksum ^= (int)sentence[i];

				return (checksum == Convert.ToInt32(valid_checksum, 16));
			}
			else
				return true;
		}

		/// <summary>
		/// Stolen from Mark Harris
		/// </summary>
		/// <param name="number"></param>
		/// <returns></returns>
		public static double ToDouble(string number)
		{
			bool isNegative;
			int characterPosition;

			int value = 0;
			int dec = 0;
			int mult = 1;

			if (number[0] == '-')
			{
				isNegative = true;
				characterPosition = 1;
			}
			else
			{
				isNegative = false;
				characterPosition = 0;
			}

			char c = '0';
			int stringLength = number.Length;

			for (; characterPosition < stringLength; characterPosition = characterPosition + 1)
			{
				c = number[characterPosition];

				if (c >= '0' && c <= '9')
				{
					value = (value * 10) + (c - '0');
				}
				else
				{
					break;
				}
			}

			if (characterPosition < stringLength && c == '.')
			{
				for (characterPosition = characterPosition + 1; characterPosition < stringLength; characterPosition = characterPosition + 1)
				{
					c = number[characterPosition];

					if (c >= '0' && c <= '9')
					{
						dec = (dec * 10) + (c - '0');
						mult = mult * 10;
					}
					else
					{
						break;
					}
				}
			}

			float full = value + ((float)dec / mult);//(dec / (float)digits);

			return isNegative ? -full : full;
		}
	}

	/// <summary>
	/// 
	/// </summary>
	class TinyGPS : IDisposable
	{
		/// <summary>
		/// GPS 
		/// </summary>
		private SerialPort GPSSerialHandle;

		/// <summary>
		/// Event to call when we have new GPS info
		/// </summary>
		public event GPSUpdateDelegate GPSUpdate;

		/// <summary>
		/// Saved up messages
		/// </summary>
		private string Message;

		/// <summary>
		/// Construct, bind the serial port
		/// </summary>
		/// <param name="comPort"></param>
		/// <param name="baud"></param>
		public TinyGPS(string comPort, int baud)
		{
			// Bind the serial port
			GPSSerialHandle = new SerialPort(comPort, baud);

			// Options..
			GPSSerialHandle.ReadTimeout = 0;

			// Flush and open the port
			GPSSerialHandle.Flush();
			GPSSerialHandle.Open();

			// Subscribe to the data recv event
			GPSSerialHandle.DataReceived += new SerialDataReceivedEventHandler(GPSSerialHandle_DataReceived);
		}

		/// <summary>
		/// Handle the data
		/// </summary>
		/// <param name="sender"></param>
		/// <param name="e"></param>
		void GPSSerialHandle_DataReceived(object sender, SerialDataReceivedEventArgs e)
		{
			byte[] response = new byte[GPSSerialHandle.BytesToRead];
			string fracturedMessage = String.Empty;
			string[] messageQueue = new string[100];
			string messageCheck;
			int k = -1;
			int sentenceLength = 0;

			GPSSerialHandle.Read(response, 0, response.Length);
			fracturedMessage = new string(UTF8Encoding.UTF8.GetChars(response));
			Message += fracturedMessage;

			int startIndex = Message.IndexOf("$");
			int newLine = Message.IndexOf("\r\n");

			if (newLine < startIndex)
			{
				Message = Message.Substring(startIndex, Message.Length - Message.Substring(0, startIndex).Length);

				startIndex = Message.IndexOf("$");
				newLine = Message.IndexOf("\r\n");
			}

			while (startIndex > -1 && newLine > -1)
			{
				messageCheck = Message.Substring(startIndex, newLine).Trim();

				if (GPSHelper.IsValidChecksum(messageCheck))
					messageQueue[++k] = messageCheck;

				// Remove the message regardless...
				sentenceLength = Message.Substring(startIndex, newLine).Length;
				Message = Message.Substring(sentenceLength, Message.Length - sentenceLength);

				startIndex = Message.IndexOf("$");
				newLine = (Message.Length > 3) ? Message.IndexOf("\r\n", 3) : -1;
			}

			// Iterate through the queue and parse the message
            for (int i = 0; i < 100; i++)
            {
                if (messageQueue[i] != null && messageQueue[i] != String.Empty)
                {
                    string[] splitSentence = messageQueue[i].Split(',');

					if (splitSentence[0] == "$GPRMC")
						GPSUpdate(new RMCOutput(splitSentence));

                    messageQueue[i] = null;
                }
                else
                    break;
			}
		}
	}
}