Getting Data from a GPS Moduleand calculating distance and bearing to a waypoint

Getting Data from a GPS Module

and calculating distance and bearing to a waypoint

Copyright:	Copyright 2007 Dean Hall. All rights reserved.
Author:	Dean Hall
Revision:	2
Date:	2007/11/25

Purpose

This page describes an early stage of experimentation with GPS navigation. For more, see the parent GPS page.

Test readings from the GPS modules

When using a GPS module on a Unix/Posix/Linux/Mac OS X machine, first find the path to the GPS device:

$ ls /dev/cu*
/dev/cu.Bender-DialupNetworking-1       /dev/cu.modem
/dev/cu.Bluetooth-Modem                 /dev/cu.usbserial
/dev/cu.Bluetooth-PDA-Sync

In my case, the path is /dev/cu.usbserial. Your path will be different based on what type of USB module you have. Use process of elmination to figure it out. Also, most OSs require the device to be plugged in for it to show up in /dev.

Most USB devices initalize their serial output to 4800 baud, 8N1. I use Python's serial module to open the device for reading and print the NMEA data stream:

$ python
>>> import serial
>>> s = serial.Serial("/dev/cu.usbserial", 4800)
>>> while True: s.readline() #ignore any junk on the first line of output
...
'x\x0f\xcf\xf8\xe6\xe0\x86\x00VTG,,T,,M,,N,,K,N*2C\r\n'
'$GPGGA,175914.000,2940.1044,N,09531.4268,W,1,04,1.8,5.4,M,-23.6,M,,0000*6F\r\n'
'$GPRMC,175914.000,A,2940.1044,N,09531.4268,W,0.00,,161107,,,A*65\r\n'
'$GPVTG,,T,,M,0.00,N,0.0,K,A*13\r\n'
'$GPGGA,175915.000,2940.1044,N,09531.4268,W,1,04,1.8,5.4,M,-23.6,M,,0000*6E\r\n'
'$GPGSA,A,3,28,11,26,04,,,,,,,,,4.4,1.8,4.0*35\r\n'
'$GPGSV,3,1,10,08,67,162,23,17,63,277,,28,58,003,37,27,41,159,*7B\r\n'
'$GPGSV,3,2,10,11,39,058,36,25,21,154,,29,19,278,,26,18,288,32*7F\r\n'
'$GPGSV,3,3,10,04,16,192,27,20,03,113,*76\r\n'
'$GPVTG,,T,,M,0.00,N,0.0,K,A*13\r\n'
^C

NMEA Data

The snippet of NMEA sentences from above is slightly contrived. The GGA, RMC and VTG are the most common. The GSA and GSV sentences occur less often. I looked up the meanings of the fields in the NMEA sentences in the SiRF NMEA Manual. Note that the RMC sentence is NMEA version 2.3 (or later), so it has one more field than prior versions. Also note when speed is zero, course is blank.

The following fields contain data that I plan to use in the robot software:

Sentence	Field Num	Field Name	Format	Use
GPGGA	1	UTC	hhmmss.sss	Date/Time
GPGGA	2	Latitude	ddmm.mmmm	Position
GPGGA	3	N/S indicator	[N|S]	Position
GPGGA	4	Longitude	dddmm.mmmm	Position
GPGGA	5	E/W indicator	[E|W]	Position
GPGGA	6	Position Fix	[0..3]	QOS
GPGGA	7	Satellites	[0..12]	QOS
GPGGA	8	HDOP	#*.#*	QOS
GPRMC	2	Status	[A|V]	QOS
GPRMC	7	Speed [knots]	#*.##	Velocity (magnitude)
GPRMC	8	Course [deg]	[<blank>|#*.##]	Velocity (direction)
GPRMC	9	Date	ddmmyy	Date/Time
GPRMC	11	Mode	[A|D|E]	QOS

Position Fix := (0 == invalid, 1 == GPS SPS mode, 2 == differential, 3 == GPS PPS mode)
Status := (A == data valid, V == data not valid)
Mode := (A == autonomous, D == DGPS, E == DR)

QOS (Quality of Signal) is not a real measurement of the quality of signals from the satellites. Instead, I use QOS as a generalization of the level of trust I should have for the position and heading data.

HDOP is the Horizontal Dilution of Precision.

Units of Measure

When utilizing collected data, it is important that all units be compatible. I also prefer using a sane unit system. So, I decided to use the SI units.

While centimeters per second would have made a nice unit to measure velocity because the range of an 8-bit number [0..255] would have covered the estimated max velocity of the vehicle. Using cm/s as a unit is not typically done. Instead, I will be using meters per second [m/s].

This means I will have to convert GPVTG field 5 (Speed [kph]) to [m/s]. This is done using the following units conversion:

Speed Km    1000   m    1      h
--------  * --------  * -------- ;  Speed [kph] * 0.278 = Speed [m/s]
       h    1     Km    3600   s

Using the SI system also has one handy feature when dealing with GPS coordinates. Due to where I live on the planet, the least significant digit (out of eight) of a coordinate pair is roughly equal to one-tenth of a meter. This gives me a nice rule-of-thumb when watching coordinates scroll by. This also means that the GPS coordinate numbers are roughly ten times more accurate than the 1 meter accuracy and precision I am counting on when receiving a strong and clean GPS reading.

Prototype Calculations

Using the NMEA data above, I now need a way to calculate the direction and distance the robot needs to go in order to reach a desired waypoint. The waypoint is also given as a GPS coordinate pair. So, my inputs are two pairs of latitude and longitude and I want outputs of bearing [degrees] and distance [meters]. I'm going to prototype these calculations in Python so I can fiddle with things until I have a good method of getting my desired output.

Bearing

This site gives a great discussion of the different calculations that are available for yielding useful output from lattitude and longitude values. The site recommends the following function that I translated to Python:

def calcBearing(lat1, lon1, lat2, lon2):
    dLon = lon2 - lon1
    y = math.sin(dLon) * math.cos(lat2)
    x = math.cos(lat1) * math.sin(lat2) \
        - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
    return math.atan2(y, x)

Distance

For distance, the Haversine method and the Spherical Law of Cosines (SLC) method are two (of many) ways to calculate the distance between a pair of coordinates. The SLC method uses less computation, but may have more error when performed with 32-bit floats or, worse, fixed point values. For completeness, I'm going to show the Haversine, the SLC and my easy planar trig methods. First, Python code for the Haversine:

def havDistance(lat1, lon1, lat2, lon2):
    dLat = lat2 - lat1
    dLon = lon2 - lon1
    a = math.sin(dLat / 2) * math.sin(dLat / 2) \
        + math.cos(lat1) * math.cos(lat2) \
        * math.sin(dLon / 2) * math.sin(dLon / 2);
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    return R * c

And the SLC method:

def slcDistance(lat1, lon1, lat2, lon2):
    return math.acos(math.sin(lat1) * math.sin(lat2)
                     + math.cos(lat1) * math.cos(lat2)
                     * math.cos(lon2 - lon1)) * R

Turning

The vehicle's current bearing is measured. The desired bearing is calculated. From these two data points, the amount and direction to turn must be calculated. I used the following algorithm for this:

def calcTurn(Btarget, Bcurrent):
    """Returns tuple (turn angle [rads], turn dir [+1 == right, -1 == left])."""

    diff = Btarget - Bcurrent
    neg = diff < 0
    big = abs(diff) > PI

    if not neg and not big: theta = diff; lr = +1
    if not neg and big: theta = 2*PI - diff; lr = -1
    if neg and not big: theta = abs(diff); lr = -1
    if neg and big: theta = 2*PI - abs(diff); lr = +1

    return (theta, lr)

Outcome

I tried the above bearing and distance functions for three pairs of coordinates. The first pair of coordinates is a "short" distance, the length of my driveway. The second pair is a "medium" distance, the length of two opposing cul-du-sacs in my neighborhood. The third pair is from my house to a Home Depot a few kilometers away. Here are the results of running each function on all 3 pairs of coordinates:

Coordinates	Method	Result	Error
Driveway	Bearing	-0.180882271103	0
Driveway	havDistance	26.3229944381	0
Driveway	slcDistance	26.3230932984	-3.75566171046e-06
Street	Bearing	89.3235271726	0
Street	havDistance	385.415122077	0
Street	slcDistance	385.415120804	3.30248293708e-09
Homedepot	Bearing	106.472262623	0
Homedepot	havDistance	4636.54286474	0
Homedepot	slcDistance	4636.54286494	-4.36663269333e-11

I used the Haversine method of calculating distance as the benchmark for calculating the SLC method's error. Results show that the SLC's error is very small, approaching insignificance. Since the SLC method has fewer trig functions, I will use it in Argonaut's code.

It remains to be seen if Argonaut's AVR processor can calculate these trig-heavy functions using floating point values fast enough to run the control system in real time. Conversely, using fixed point numbers may not yeild enough precision to give meaningful results. I will let actual experiments determine the type of numbers used.