GPS Programming & .NET
by Johan Franson


Listing One

// Returns true if checksum of NMEA sentence is valid
public bool ValidateChecksum(string Sentence)
{
    int  sum = 0, inx;
    char[] sentence_chars = Sentence.ToCharArray();
    char tmp;
    // All charachter xor:ed results in the trailing hex checksum
    // The checksum calc starts after '$' and ends before '*'
    for(inx = 1; ; inx++)
    {
        if(inx >= sentence_chars.Length) // No checksum found
            return false;
        tmp = sentence_chars[inx];
        // Indicates end of data and start of checksum
        if(tmp == '*')   
            break;
        sum = sum ^ tmp;    // Build checksum
    }
    // Calculated checksum converted to a 2 digit hex string
    string sum_str = String.Format("{0:X2}", sum);
    // Compare to checksum in sentence
    return sum_str.Equals(Sentence.Substring(inx + 1, 2));
}


Listing Two

private string m_raw_buffer;
// Returns an array of all fields in the next valid NMEA sentence RawData 
// adds new raw data to be parsed. If RawData is null parsing of old data 
// continues. Returns null if no more valid sentences are found
public string[] Parse(string RawData)
{

    string sentence;
    int start, end;
    if(RawData != null)
    {
        m_raw_buffer += RawData; // Add new data
    }
    do
    {
        // Find start of next sentence
        start = m_raw_buffer.IndexOf("$");
        if(start == -1)
        {
            // No start found
            m_raw_buffer = null;
            return null;
        }
        m_raw_buffer = m_raw_buffer.Substring(start);
        // Find end of sentence
        end = m_raw_buffer.IndexOf("\r\n");
        if(end == -1)
        {
            // No end found, wait for more data
            return null;
        }
        sentence = m_raw_buffer.Substring(0, end + 2);
        m_raw_buffer = m_raw_buffer.Substring(end + 2);
    }
    while(!ValidateChecksum(sentence));
    // Valid sentence found!
    // Remove trailing checksum and \r\n
    sentence = sentence.Substring(0, sentence.IndexOf("*"));
    // Split into fields and return array
    return sentence.Split(",".ToCharArray());
}


Listing Three

// Converts NMEA formated (DDMM.MMMMM) position (latitude or longitude)
// to decimal degrees
public double Nmea2DecDeg(string NmeaLonLat, string Hemisphere)
{
    int inx = NmeaLonLat.IndexOf(".");
    if(inx == -1)
    {
        return 0;    // Invalid syntax
    }
    string minutes_str = NmeaLonLat.Substring(inx - 2);
    double minutes = Double.Parse(minutes_str, new 
              System.Globalization.CultureInfo("en-US"));
    string degrees_str = NmeaLonLat.Substring(0, inx-2);
    double degrees = Convert.ToDouble(degrees_str) + minutes / 60.0;
    if(Hemisphere.Equals("W") || Hemisphere.Equals("S"))
    {
        degrees = -degrees;

    }
    return degrees;
}


Listing Four

// Paramters for the GWS84 ellipsoid
const double WGS84_E2 = 0.006694379990197;
const double WGS84_E4 = WGS84_E2 * WGS84_E2;
const double WGS84_E6 = WGS84_E4 * WGS84_E2;
const double WGS84_SEMI_MAJOR_AXIS = 6378137.0;
const double WGS84_SEMI_MINOR_AXIS = 6356752.314245;
// Paramters for UTM projetion
const double UTM_LONGITUDE_OF_ORIGIN = 3.0 / 180 * Math.PI;
const double UTM_LATITUDE_OF_ORIGIN = 0;
const double UTM_FALSE_EASTING = 500000;
const double UTM_FALSE_NORTHING_N = 0;    // Northern hemisphere
const double UTM_FALSE_NORTHING_S = 10000000; // Southern hemisphere
const double UTM_SCALE_FACTOR = 0.9996;

// Takes a position in latitude / longitude (WGS84) as input
// Returns position in UTM easting/northing/zone (in meters)
public void DecDeg2UTM(double latitude, double longitude,
    out double easting, out double northing, out int zone)
{
    // Normalize longitude into Zone, 6 degrees
    int int_zone = (int) (longitude / 6.0);
    if(longitude < 0)
        int_zone--;
    longitude -= (double) int_zone * 6.0;
    zone = int_zone + 31;    // UTM zone
    // Convert from decimal degrees to radians
    longitude *= Math.PI / 180.0;
    latitude *= Math.PI / 180.0;
    // Projection
    double M = WGS84_SEMI_MAJOR_AXIS * m_calc(latitude);
    double M_origin = WGS84_SEMI_MAJOR_AXIS * m_calc(UTM_LATITUDE_OF_ORIGIN);
    double A = (longitude - UTM_LONGITUDE_OF_ORIGIN) * Math.Cos(latitude);
    double A2 = A * A;
    double e2_prim = WGS84_E2 / (1 - WGS84_E2);
    double C = e2_prim * Math.Pow(Math.Cos(latitude), 2);
    double T = Math.Tan(latitude);
    T *= T;
    double v = WGS84_SEMI_MAJOR_AXIS / Math.Sqrt(1 - WGS84_E2 *
        Math.Pow(Math.Sin(latitude), 2));
    northing = UTM_SCALE_FACTOR * (M - M_origin + v * Math.Tan(latitude) * (
        A2 / 2 + (5 - T + 9 * C + 4 * C * C) * A2 * A2 / 24 +
        (61 - 58 * T + T * T + 600 * C - 330 * e2_prim) *
        A2 * A2 * A2 / 720));
    if(latitude < 0)
        northing += UTM_FALSE_NORTHING_S;
    easting = UTM_FALSE_EASTING + UTM_SCALE_FACTOR * v * (
        A + (1 - T + C) * A2 * A / 6 +
        (5 - 18 * T + T * T + 72 * C - 58 * e2_prim) * A2 * A2 * A / 120);

}
private double m_calc(double lat)
{
   return (1 - WGS84_E2 / 4 - 3 * WGS84_E4 / 64 - 5 * WGS84_E6 / 256) * lat -
        (3 * WGS84_E2 / 8 + 3 * WGS84_E4 / 32 + 45 * WGS84_E6 / 1024) * 
        Math.Sin(2 * lat) + (15 * WGS84_E4 / 256 + 45 * WGS84_E6 / 1024) * 
        Math.Sin(4 * lat) - (35 * WGS84_E6 / 3072) * Math.Sin(6 * lat);
}


Listing Five

// Calculates the distance between two UTM coordinates using Pythagoras 
// theorem. The coordinates must be in the same UTM zone and in 
// the same hemisphere.
public double Distance(double easting1, double northing1, double 
easting2, double northing2)
{
    return Math.Sqrt(
        Math.Pow((easting1 - easting2), 2) +
        Math.Pow((northing1 - northing2), 2));
}

Listing Six

// Calculates the bearing between two UTM coordinates. Works best on local 
// calculations and the coordinates must be in the same UTM zone.
public double Bearing(double easting1, double northing1, 
                                      double easting2, double northing2)
{
    double a = 0;
    double dEast = easting2 - easting1;
    double dNorth = northing2 - northing1;
    if(dEast == 0)   
    {
        if(dNorth < 0)
        {
            a = Math.PI;
        }
    }
    else
    {
        a = -Math.Atan(dNorth / dEast) + Math.PI / 2;
    }
    if(dEast < 0)
        a = a + Math.PI;
    return a * 180.0 / Math.PI;    // Convert from radians to degrees
}


Listing Seven

// An application the shows direction and distance from the current
// position to a saved position using the previous samples

private double m_latitude;
private double m_longitude;
// Should be called by the serial port handler.
private void OnSerialData(string data)
{
    string[] fields = Parse(data);
    // Parse all sentences in data
    while(fields != null)
    {
        if(fields[0] == "$GPRMC")
        {
            if(fields[2] == "A")
            {
                // Fix valid
                m_latitude = Nmea2DecDeg(fields[3], fields[4]);
                m_longitude = Nmea2DecDeg(fields[5], fields[6]);
            }
        }
        // Support for GLL and GGA should also be implemented
    }
}
private double m_saved_easting;
private double m_saved_northing;
// Should be called by a button handler or similar
private void OnSavePosition()
{
    double zone;
    DecDeg2UTM(m_latitude, m_longitude,
        out m_saved_easting, out m_saved_northing, out zone);
}
// Should be called by a timer or button handler
private void OnFindWay()
{
    double zone, easting, northing;
    DecDeg2UTM(m_latitude, m_longitude, out easting, out northing, out zone);
    double distance;
    distance = Distance(easting, northing, m_saved_easting, m_saved_northing);
    double bearing;
    bearing = Bearing(easting, northing, m_saved_easting, m_saved_northing);
    // Prints distance and bearing to those text boxes
    txtDistance.Text = distance.ToString();
    txtBearing.Text = bearing.ToString();
}

Listing Eight

// An application the shows direction and distance from the current
// position to a saved position using GpsTools from franson.biz

private GpsToolsNET.NmeaParser m_parser;
private GpsToolsNET.Position m_saved_position;

// Should be called at startup
private void Init()
{

    m_parser = new GpsToolsNET.NmeaParser();
    m_parser.NoEvents = true;
    m_parser.PortEnabled = true; // Auto detect GPS
}
// Should be called by a button handler or similar
private void OnSavePosition()
{
    GpsToolsNET.GpsFix fix = m_parser.GetGpsFix(2, 0);
    m_saved_position = fix.Position;
}
// Should be called by a timer or button handler
private void OnFindWay()
{
    GpsToolsNET.GpsFix fix = m_parser.GetGpsFix(2, 0);
    GpsToolsNET.Position position = fix.Position;
    double distance = position.Distance(m_saved_position);
    double bearing = position.Bearing(m_saved_position);
    // Prints distance and bearing to those text boxes
    txtDistance.Text = distance.ToString();
    txtBearing.Text = bearing.ToString();
}







6

