Navigating with GPS
The race course at the Sparkfun AVC requires our car to navigate around the building. To do this, we'll define a set of GPS waypoints that will guide the car on a closed path from start to finish. Using a GPS receiver, the car will determine it's current location and compare this to the next waypoint it needs to reach. From this comparison it can calculate the distance to the next waypoint and, with a big of trigonometry, the heading to follow to get there. Of course, as the car moves, it will need to constantly recheck it current position and update it's distance and heaading values otherwise it may veer off course, or overshoot the waypoint. But, before we get into these details, we need to understand a bit more about how GPS coordinates works and how to interface to a GPS module.
LS20031 GPS Test Shield
In a previous project of mine, GPS vs Barometric Altitude, I showed how interfaced an Arduino Mini to a LS20031 GPS Receiver and used it to read altitude using the TinyGPS library that's made available courtesy of Arduiniana. Using this same library, it's very easy to read out the module's current Latitude and Longitude values. However, since the LS20031 runs on 3.3 Volts, and I'm using a 5 Volt Arduino for this project, I needed to add another level shifter circuit. However, as serial communication does not require a bidirectional level shift circuit, I elected to use Sparkfun's cheaper ($1.95) Logic Level Converter board for this task. Here's how I connected it to the Arduino:
By default, the LS20031 powers up in state that transmits data at 9600 bps, 8 data bits, no parity, 1 stop bit. I always like to start with the basics, so here's a simple test program that uses the Arduino's SoftwareSerial library to receive and print the raw data coming from LS20031:
#include <SoftwareSerial.h>
#define rxPin 10
#define txPin 11
SoftwareSerial mySerial(rxPin, txPin);
void setup() {
Serial.begin(57600);
mySerial.begin(9600);
}
void loop() {
if (mySerial.available()) {
Serial.write(mySerial.read());
}
}
If you run this code, open the Console, (set to 57600 baud), you'll see a bunch of data like this printed out in the console display:
$GPGSA,A,1,,,,,,,,,,,,,,,*1E
$GPGSV,1,1,03,13,,,21,07,,,24,23,,,26*7F
$GPRMC,024254.000,V,3255.3311,N,11706.5772,W,0.08,0.00,100312,,,N*6F
$GPVTG,0.00,T,,M,0.08,N,0.15,K,N*3E
$GPGGA,024255.000,3255.3559,N,11706.5162,W,0,3,,185.4,M,-35.4,M,,*7B
$GPGLL,3255.3559,N,11706.5162,W,024255.000,V,N*5D
Each line should begin with the sequence "$GP" that indicates that these are NMEA 0183 messages. However, if you watch for awhile, you may notice that, sometimes, the $GP sequence appears elsewhere in the text stream. After many hours of head scratching, I figured out that I needed to increase the buffer size in the SoftwareSerial library when using the new Arduino 1.0 release. Doing this is a bit tricky (at least on a Mac) because this code for SoftwareSerial is buried inside the Arduino 1.0 App itself. To increase the buffer size, select the Arduino 1.0 App and right click to select "Show Package Contents", then open the folders to this path:
Contents->Resources->Java->libraries->SoftwareSerial
Then, open the file named "SoftwareSerial.h" in a text editor and scroll down to line 42, where you should see:
#define _SS_MAX_RX_BUFF 64 // RX buffer size
Increase the value 64, to a more reasonable 128 and then save the file. Then, recompile the Arduino sketch above and verify that the output messages now only display the "$GP" sequence at the beginning of each line. It's important to get this right because the garbling produced by this problem will confuse the TinyGPS library and cause it to miss GPS position information which, in turn, will lead to erratic operation of the car. Note: even though this change seems to "fix" the garbling problem, it's not clear to me why a 64 byte buffer causes a problem in the first place. Since the test sketch above does nothing but grab the incoming data from the SoftwareSerial input and then pass it directly on to much faster hardware serial ports, there really should't be any load placed on the buffer. I suspect that this may be due to a problem with the SoftwareSerial library, perhaps unique to the 9600 baud rate at which I'm using it here, that may be fixed in a future Arduino version.
Once you're satisfied the NMEA 0183 messages are OK, take a look at the messages being printed out in the console and notice that there are several different types of messages coming from the LS20031. If you look at the 3 characters that follow the $GP sequence, you'll see 6 distinct combinations; GSA, GSV, RMC, VTG, GGA and GLL. Each of these message lines (actually, they're called "sentences" in NMEA 0183 parlance) each different type convey different GPS information. You can learn more about this by reading more about NMEA 0183 at this reference. However, for our purposes, the only messages we care about the ones prefixed by $GPGGA, which we'll call "GGA" messages. These provide Latitude, Longitude and Altitude and a few other useful bits of information we can use to determine the quality of the GPS position fix. And, since we really only want the GPS unit to output just these GGA messages, the following modifications to the setup() function in the above sketch will accomplish this:
void setup() {
Serial.begin(57600);
Serial.println("Start");
mySerial.begin(9600);
mySerial.print("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");
}
The last line adds code that sends a configuration command to the LS20031 that tells it to only send GGA messages. Of course, we're still only getting one message output every second, so our position fix isn't being updated often enough to navigate a moving car. But, now that we're suppressed the other, unneeded messages, we can send the LS20031 another command that tells it to send out GGA messages 5 times a second. To do this, add the following line just after the one we added to setup() above:
mySerial.print("$PMTK220,200*2C\r\n");
Since each GGA message is about 92 characters long (including the CR and LF characters), the LS20031 is now sending at the rate of 92 times 5, or 460 characters/second. This is still well below the maximum rate of 960 characters/second that can be safely sent at 9600 baud, so this should work reliably for you and you should not see garbled messages.
Parsing GGA messages into Usable Coordinates
If you examine the GGA message in more detail, you'll see that the Latitude and Longitude values in the 3rd and 5th comma-separated slots are odd decimal values like 4124.8963 and 08151.6838. To be more usable with the kinds of distance and heading calculations that are needed to navigate a car, these values must be converted into angular degree values, such as 32.9199617 and -117.1141033. The math to do this isn't hard, but we can use the TinyGPS library to do these calculations are done for us. To try this out, download and install the latest TinyGPS library and modify the above sketch to look like this:
#include <TinyGPS.h>
#include <SoftwareSerial.h>
#define rxPin 10
#define txPin 11
TinyGPS gps;
SoftwareSerial mySerial(rxPin, txPin);
void setup() {
Serial.begin(57600);
Serial.println("Start");
mySerial.begin(9600);
mySerial.print("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");
mySerial.print("$PMTK220,200*2C\r\n");
}
void loop() {
while (mySerial.available()) {
unsigned char cc = mySerial.read();
if (gps.encode(cc)) {
float flat, flon;
unsigned long age;
gps.f_get_position(&flat, &flon, &age);
Serial.print("Lat: ");
Serial.print(flat);
Serial.print(", Long: ");
Serial.println(flon);
}
}
}
You won't see the full precision of the Latitude and Longitude values in the information printed to the console because the printf() functions for float values on the Arduino seem to only display 2 fractional digits, but this won't be a problem because the full precision is still available in the flat and flon variables.