R/C GPS Truck
This was a test bed for an Airplane UAV system. My intention was for taking aerial photos. The truck uses GPS to Navigate between preset way-points (using latitude and longitude)
| GPS Robo-Truck |
|
|
The truck is controlled via std RC radio, until the user hits the "Landing Gear Up" switch, which turns on the Auto-pilot navigation in the truck. The system reads GPS, calc's distance and bearing to the next way point, and uses a compass to steer itself towards the next way point. When it gets close enough (within 10 feet) it switches to the next way point in the list, and navigates towards it. Once it reaches the last point, it loops back to the first (and drives a continuous loop) Works great. Steering is a little sloppy, but it made for more robust steering code.
The truck uses standard R/C radio, Speed Controller and steering servo. I found the truck at Goodwill, tore out the electronics but kept the drive motor. Nav system uses a compass and GPS receiver. CPU is a PIC16F877A and an external uM Floating Point processor.
The truck runs on a PIC16F877A mpu, with a uM FPU chip for floating point.
I used a Deluo GPS unit (std Rs-232 4800 Baud text output) and Hitachi Compass module.
Here's a general schematic of the truck's electronics.
PicBasic Pro Source Code: GPS_Truck.pbp
The central control loop is fairly simple. This loop runs over and over:
Main_Control_Loop:
Read GPS - (get current Lat/Lon)
Calc Distance to next Way-Point - (how far to where we're going)
If Dist < 2 Meters Then Load_Next_WayPoint - (we're close enough - load next way point)
Read Compass (get our current heading)
Calc Direction to next Way-Point - (what's the heading to steer between here and there)
Set_Steering (adjust difference between our heading and Bearing to Way Point)Goto Main_Control_Loop (loop over and over)
That's really about it.
Most GPS units work the same: you plug them in, and they start spitting out ascii text data, usually once per second. The data comes out at 4800 baud, and can be read by any terminal program. The data format has been standardized in what is called the NMEA standard. It has a number of different fixed format messages that the GPS units spit out. Most GPS units transmit 2 or 3 strings over and over. You don't have to process all the data that comes in. Most can be ignored. You can read the Lat/Lon data from any of the various messages that comes in.
Here's an example of an actual data stream received from my GPS unit. Each block of three lines comes out every second:
$GPGSA,A,3,23,16,13,11,01,24,20,04,,,,,1.7,1.0,1.4*35
$GPRMC,203404.000,A,3657.4724,N,12201.7082,W,0.02,164.15,120706,,,A*75
$GPGGA,203405.000,3657.4724,N,12201.7082,W,1,08,1.0,12.7,M,-26.7,M,,0000*52$GPGSA,A,3,23,16,13,11,01,24,20,04,,,,,1.7,1.0,1.4*35
$GPRMC,203405.000,A,3657.4724,N,12201.7082,W,0.02,186.65,120706,,,A*7F
$GPGGA,203406.000,3657.4723,N,12201.7082,W,1,08,1.0,12.7,M,-26.7,M,,0000*56$GPGSA,A,3,23,16,13,11,01,24,20,04,,,,,1.7,1.0,1.4*35
$GPRMC,203406.000,A,3657.4723,N,12201.7082,W,0.03,193.41,120706,,,A*78
$GPGGA,203407.000,3657.4723,N,12201.7082,W,1,08,1.0,12.7,M,-26.7,M,,0000*57
Each NMEA string contains a different mix of data.
Here is one of the most common messages, the "GPRMC" message:
$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>
1) UTC time of position fix, hhmmss.sss format.
2) Status, A = data valid, V = data not valid.
3) Latitude, ddmm.mmmm format.
4) Latitude hemisphere, N or S.
5) Longitude, dddmmm.mmmm format.
6) Longitude hemisphere, E or W.
7) Speed over ground, 0.0 to 1851.8 knots.
8) Course over ground, 000.0 to 359.9 degrees, true.
9) Date, ddmmyy format.
10) Magnetic variation, 000.0 to 180.O.
11) Degrees
12) Checksum.
Here's another common message. Similar, but a different mix of data, the "GPGGA" message:
$GPGGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<11>,<12>,<13><CR><LF>
1) UTC time of position fix, hhmmss.sss format
2) Latitude, ddmm.mmmm format.
3) Latitude hemisphere, N or S.
4) Longitude, dddmm.mmmm format.
5) Longitude hemisphere, E or W.
6) Position Fix Indicator,
0 = fix not available, or invalid.
1 = GPS SPS Mode, fix valid.
2 = Differential GPS, SPS Mode, fix valid.
3 = GPS PPS Mode, fix valid.
7) Number of satellites in use, 00 to 12.
8) Horizontal Dilution of Precision, 0.5 to 99.9.
9) MSL Altitude, -9999.9 to 99999.9 meters.
10) Geoidal height, -999.9 to 9999.9 meters.
11) Differential GPS (RTCM SC-104) data age, number of seconds since last valid RTCM transmission (nu1l if non-DGPS).
12) Differential Reference Station ID, 0000 to 1023. (null if non-DGPS)
13) Checksum.
Here's the GGA message - essential fix data which provide 3D location and accuracy data.
$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
GGA Global Positioning System Fix Data
1) 123519 Fix taken at 12:35:19 UTC
2) 4807.038,N Latitude 48 deg 07.038' N
3) 01131.000,E Longitude 11 deg 31.000' E
4) 1 Fix quality: 0 = invalid; 1 = GPS fix (SPS); 2 = DGPS fix; 3 = PPS fix; 4 = Real Time Kinematic; 5 = Float RTK;
.... 6 = estimated; 7 = Manual input mode; 8 = Simulation mode
5) 08 Number of satellites being tracked
6) 0.9 Horizontal dilution of position
7) 545.4,M Altitude, Meters, above mean sea level
8) 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid
9) (empty field) time in seconds since last DGPS update
10) (empty field) DGPS station ID number
11) *47 the checksum data, always begins with *
To get your current GPS location, just read the serial strings as they come in, look for the one that starts with the Msg ID you're looking for (like $GPRMC, that includes "Course over ground" - our current heading) then scan the string, extracting the data you're looking for. (ie Lat, Lon) and convert the ascii data into your internal representation. (atoi)
I represent Lat/Lon within the 8Bit PIC using 3 16Bit Words: Degrees, Seconds, Fract_Seconds.
So the GPS format of 36-degrees, 25.4678 seconds is Deg=36, Sec=25, Frac=4678. Once loading into the FPU, they are converted to floating point degrees (36.424463 degrees) which is the value used in the calculations.
Calc Distance between two Lat/Lon locations:
There are at least two ways to do this - the simple way is to ignore the curve of the earth, and for local distances this works just fine. Just use trig and consider the map a flat x-y co-ordinate system. d = sqrt((X2 - X1)^2 + (Y2 - Y1)^2)
I decided to get fancy, since I had the FPU, and I use the "Spherical law of cosines" formula that does reflect that the world is a "sphere". This formulas will work for any two points on the globe.
d = acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(long2−long1)) * (EarthsRadius = 6,371 km)
If Earth's radius is expressed in Km, the answer d is also in Km. If r is in Miles, the answer is in Miles.
Calc Heading between here and there:
Again, I decided to get fancy, since I had the FPU, and I use the "Great Circle" formula that does reflect that the world is a "sphere" and that your initial heading may not be your final heading. (think about crossing over the north pole on the way to Russia)
Bearing = atan2( sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2)) - (sin(lat1)*cos(lat2)*cos(lon2-lon1)) )
