Copyright: | Copyright 2007 Dean Hall. All rights reserved. |
---|---|

Author: | Dean Hall |

Revision: | 2 |

Date: | 2007/11/25 |

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

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

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.

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.

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.

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)

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

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)

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.