 | Boat Guided by GPS Example Last Modified: 2006-11-07 | | |
| Acroname Robotics | PDF webpage version  | ||
| | Boat Guided by GPS Example Last Modified: 2006-11-07 | | |
| Acroname Robotics | PDF webpage version | ||
|  Introduction A common task for a robot is following a pre-determined route. When a robot is outdoors, it can use a GPS system to determine its position and find heading and distance to waypoints. Guiding a small boat is an ideal application for GPS navigation for several reasons. A boat on open water has a clear view of the sky and gets good GPS reception. There are few obstacles on a lake or pond so the boat can move in any direction without much risk of a collision. On a nice day, it's fun to go to a lake or pond and play with boats! This example code makes a robot boat follow a straight course that will take it to a target point. We used the Pond Explorer robot boat with a BrainStem Moto 1.0 module and a Total Robots GPM for this project. Once turned on, the boat continually checks its position every few seconds and tries to maintain a course that will take it to the target point.
Theory In an ideal case, navigating a vehicle to reach a known point can be fairly simple: compute the heading to a target point, compare that with the heading of the vehicle, and steer based on the difference. It can get much more complicated (obstacles, dynamics, noisy data, etc.) but this basic scheme works well for a boat. The first task is to determine where the target point is relative to the boat. A vector to the target can be calculated by subtracting the boat's position from the target's position. One of the most common representations for a GPS position is latitude and longitude given in degrees, minutes, and thousandths of minutes. The latitude and longitude coordinates can each be represented as a single floating-point value, but a BrainStem only does 16-bit integer math. Representing the value with separate degrees, minutes, and thousandths of minutes terms makes it possible to do the subtraction with integer math. This requires three subtractions starting with the thousandths of minutes term and borrowing from the next term when necessary. The result is a difference in thousandths of minutes. With 16-bit integers, the maximum difference that can be calculated is 32.767 minutes. At Acroname's place in the world, this is over 30 miles. With a vector to the target, a four-quadrant arctangent operation will yield the heading. In a C program, this requires floating point operations and the atan2 trigonometric function. The "aMath.tea" library from the math example has an integer approximation of the atan2 function called aMath_Atan2 . It accepts x-y values in the range from -1000 to 1000 and returns an angle in the range from 0 to 359. The boat's heading can be calculated by taking the difference between its positions at two times in order to get a direction vector and then calculating the heading with the arctangent operation. This sounds easy, but in practice there are some problems. The boat is very slow, cruising at less than a foot per second. If the time between position updates is small, the boat's motion may be neglible compared to the precision of the GPS position measurement. Then it is nearly impossible to get a good heading measurement. GPS position measurements also drift slowly over time. Since the boat is also moving slowly, it may move at nearly the same rate as the drift. Then the measurements will be nearly useless. These problems were demonstrated in a test run where the boat meandered all over a lake. The time between samples was only 1 second. Changing the time between samples to 10 seconds greatly improved performance. This allowed enough travel between points to provide a reasonably good heading estimation. Some GPS units provide velocity and/or heading information, but the GPS must be moving in order to get this data. If the GPS is moving too slowly, these features may not be usable. This was the case with the boat and its GPS unit. Increasing the boat's speed would probably make things work better. The program would be smaller since the boat heading calculations could be replaced with code that simply reads the data from the GPS. The data from the GPS would probably be of much better quality also. Source Code - GPS TEA Library All the computation required for GPS navigation produces a compiled program that is bigger than the normal 1K TEA file. It could be spread out over several TEA files by using some multi-tasking techniques, but using the big 16K TEA file slot in a Moto board makes things a lot easier. The aMath.tea library from the math example should be placed in the aSystem folder of the brainstem directory. The rest of the code shown here can be copied into the the aUser folder of the brainstem directory. The GPS routines are located in the "gpslib.tea" file listed below. It has routines for retrieving data from the GPS and storing it in the Scratchpad. This is handy if separate TEA processes must share GPS data. The Scratchpad acts like an array. Defined indexes tell where to find data for two different positions, A and B. Typically, B is a target position and A is a vehicle position. Additional functions do operations to compare positions A and B. /* filename: gpslib.tea */
#include <aCore.tea>
#include <aPrint.tea>
#include <aPad.tea>
#include <aGPM.tea>
#include <aMath.tea>
#define aGPS_ADDR (unsigned char)0xD0
#define aGPS_ERR_NONE 0
#define aGPS_ERR_OVERFLOW 1
#define aGPS_POS_A 0
#define aGPS_LAT_A 0
#define aGPS_LAT_DEG_A 0
#define aGPS_LAT_MIN_A 2
#define aGPS_LAT_FRAC_A 4
#define aGPS_LONG_A 6
#define aGPS_LONG_DEG_A 6
#define aGPS_LONG_MIN_A 8
#define aGPS_LONG_FRAC_A 10
#define aGPS_POS_B 12
#define aGPS_LAT_B 12
#define aGPS_LAT_DEG_B 12
#define aGPS_LAT_MIN_B 14
#define aGPS_LAT_FRAC_B 16
#define aGPS_LONG_B 18
#define aGPS_LONG_DEG_B 18
#define aGPS_LONG_MIN_B 20
#define aGPS_LONG_FRAC_B 22
#define aGPS_HDOP 26
#define aGPS_HEADING 28
#define aGPS_SPEED 30
#define aGPS_ERROR 55
#define aGPS_ErrLat aGPS_SubPosAfromB(90, aGPS_LAT_A, aGPS_LAT_B)
#define aGPS_ErrLong aGPS_SubPosAfromB(360, aGPS_LONG_A, aGPS_LONG_B)
void aGPS_DumpLatLong(char index)
{
int dlat;
int dlong;
int s;
dlat = aPad_ReadInt(index);
dlong = aPad_ReadInt(index + 6);
s = 1;
if (dlat < 0) s = -1;
aPrint_IntDec(dlat);
aPrint_Char(':');
aPrint_IntDec(s * aPad_ReadInt(index + 2));
aPrint_Char(':');
aPrint_IntDec(s * aPad_ReadInt(index + 4));
aPrint_Char('\n');
s = 1;
if (dlong < 0) s = -1;
aPrint_IntDec(dlong);
aPrint_Char(':');
aPrint_IntDec(s * aPad_ReadInt(index + 8));
aPrint_Char(':');
aPrint_IntDec(s * aPad_ReadInt(index + 10));
aPrint_Char('\n');
aPrint_IntDec(aPad_ReadInt(aGPS_HDOP));
aPrint_Char('\n');
}
void aGPS_AssignCoord(char index, char dir, int deg, int min, int fracmin)
{
if ((dir == 'S') || (dir == 'W')) {
deg = -deg;
min = -min;
fracmin = -fracmin;
}
aPad_WriteInt(index, deg);
aPad_WriteInt(index + 2, min);
aPad_WriteInt(index + 4, fracmin);
}
void aGPS_ReadLatLongGPM(char index)
{
char dir;
int d, m, f;
d = aGPM_GetLatitudeDegrees(aGPS_ADDR);
m = aGPM_GetLatitudeMinutes(aGPS_ADDR);
f = aGPM_GetLatitudeFrac(aGPS_ADDR);
dir = aGPM_GetLatitudeDirChar(aGPS_ADDR);
aGPS_AssignCoord(index, dir, d, m, f);
d = aGPM_GetLongitudeDegrees(aGPS_ADDR);
m = aGPM_GetLongitudeMinutes(aGPS_ADDR);
f = aGPM_GetLongitudeFrac(aGPS_ADDR);
dir = aGPM_GetLongitudeDirChar(aGPS_ADDR);
aGPS_AssignCoord(index + 6, dir, d, m, f);
aPad_WriteInt(aGPS_HDOP, aGPM_GetHDOP(aGPS_ADDR));
}
int aGPS_SubPosAfromB(int xdeg, char ca, char cb)
{
int da, ma, fa;
int db, mb, fb;
int d_f;
int d_m;
int d_d;
int val;
int sign = 1;
char error = 0;
// extract coordinates from pad
da = aPad_ReadInt(ca);
ma = aPad_ReadInt(ca + 2);
fa = aPad_ReadInt(ca + 4);
db = aPad_ReadInt(cb);
mb = aPad_ReadInt(cb + 2);
fb = aPad_ReadInt(cb + 4);
// subtract fractional minutes
// possibly borrow 1000 fractional minutes from minute term
d_f = fb - fa;
if (d_f < 0) {
d_f = 1000 + d_f;
mb = mb - 1;
}
// subtract minutes
// possibly borrow 60 minutes from degree term
d_m = mb - ma;
if (d_m < 0) {
d_m = 60 + d_m;
db = db - 1;
}
// subtract degrees
// possibly borrow X degrees and set sign flag
d_d = db - da;
if (da > db) {
d_d = xdeg + d_d;
sign = -1;
}
// invert negative result
// to make degrees, minutes, fraction all positive
if (sign == -1) {
d_d = xdeg - d_d - 1;
d_m = 59 - d_m;
d_f = 1000 - d_f;
}
// get magnitude of difference in fractional minutes
val = (d_f + d_m * 1000) * sign;
// more than 32 minutes is overflow
// non-zero degrees is overflow
if (d_m >= 32) error = aGPS_ERR_OVERFLOW;
if (d_d != 0) error = aGPS_ERR_OVERFLOW;
aPad_WriteChar(aGPS_ERROR, error);
return val;
}
int aGPS_GetHeadingFromPosErr(int dlat, int dlong)
{
int heading = -1;
// divide down values into range for atan function
// then convert errors to heading
while ((aMath_Absval(dlat) > 1000) || (aMath_Absval(dlong) > 1000)) {
dlat = dlat >> 1;
dlong = dlong >> 1;
}
heading = aMath_Atan2(dlong, dlat);
return heading;
}
int aGPS_GetHeadingFromAtoB()
{
int dlong = 0;
int dlat = 0;
int heading = -1;
char error = 0;
// get difference
dlat = aGPS_ErrLat;
error = aPad_ReadChar(aGPS_ERROR);
if (!error) {
dlong = aGPS_ErrLong;
error = aPad_ReadChar(aGPS_ERROR);
}
if (!error)
heading = aGPS_GetHeadingFromPosErr(dlat, dlong);
// heading will be -1 if error occurred
return heading;
}
The aGPS_DumpLatLong routine is handy for debugging. Since latitude and longitudes are usually specified with a direction instead of a sign, the aGPS_AssignCoord routine negates the coordinate terms for South ('S') Latitudes and West ('W') Longitudes so the math works out right. The aGPS_ReadLatLongGPM routine reads the current coordinates from the GPM and writes them to the Scratchpad after doing the direction-to-sign conversion. The grunt work for navigation is done in the aGPS_SubPosAfromB routine. Two macros call this function: aGPS_ErrLat and aGPS_ErrLong. The subtraction logic is described below:
Two more routines provide some convenience. The aGPS_GetHeadingFromPosErr routine automatically scales down latitude or longitude differences with absolute values beyond 1000 then calls the aMath_Atan2 function to get a heading. This extra scaling is necessary if the vehicle is more than a mile or so from the target point. The aGPS_GetHeadingFromAtoB routine does all the work to calculate the heading from stored position A to stored position B. Source Code - Reflex for Main Timing Loop A reflex controls the timing of the loop in the main program. The reflex routine acts as a 1-second clock. This provides a more regular timing interval than sleep statements in the TEA program since subroutines may not always take the same time to execute. The reflex code is shown below. Once started, a timer loop continually resets itself. When the timer restarts, it signals TEA process 0 by sending a semaphore message with the cmdDEV_VAL command. /* filename: gps.leaf */
#include <aIOPorts.tea>
#include <aCmd.tea>
#include <aMotoReflexes.tea>
#define STEMADDR 4
#define msgSETTIMER 122
#define msgSIGNALPROC0 121
/* 1-second timer reflex */
module[STEMADDR] {
message[msgSETTIMER] {
STEMADDR, cmdTMR_SET, 1, 39, 16
}
message[msgSIGNALPROC0] {
STEMADDR, cmdDEV_VAL, 53, 0, 0
}
vector[aMoto_RFX_TIMER_1] {
msgSETTIMER,
msgSIGNALPROC0
}
}
Source Code - TEA Main Calling Routine The main program "gps.tea" has an initialization routine and a control loop to implement the navigation algorithm. The init routine waits 5 seconds while flashing an LED. This gives some set-up time for the boat. Then another loop flashes the LED rapidly while waiting for GPS acquisition. In the main loop, timing is regulated by the reflex. The aMulti_Wait statement stops program execution until it gets a semaphore message from the reflex loop. After counting 10 seconds, the main loop takes a GPS reading and calls navCalc to calculate heading to target and boat heading as described in the navigation discussion. Then it calls steerCalc to determine if any adjustments to the boat's course are necessary. If the boat is off course, one motor is turned off and the boat starts turning. The main loop will turn both motors back on after one second to resume a straight course. With one motor turned off for one second, the boat turns about 5 degrees. There is also a small "dead zone" in the steering calculation. If the boat heading is within 5 degrees of the target heading, then it just keeps going straight. These steering behaviors keep the slow-moving boat on a fairly steady course. /* filename: gps.tea */
#include <aCore.tea>
#include <aDig.tea>
#include <aPrint.tea>
#include <aMotion.tea>
#include <aMulti.tea>
#include <aMotoReflexes.tea>
#include "gpslib.tea"
#define DTEST 0
#define MLEFT 0
#define MRIGHT 1
int dlat = 0;
int dlong = 0;
int pre_dlat = 0;
int pre_dlong = 0;
int htarg = 0;
int hboat = 0;
void init()
{
char k;
char flag = 0;
// configure IO
aDig_Config(DTEST, ADIG_OUTPUT);
aMotion_SetMode(MLEFT, aMOTION_MODE_PWM, 0);
aMotion_SetMode(MRIGHT, aMOTION_MODE_PWM, 0);
// provide some set-up time
for (k = 0; k !=5; k++)
{
aDig_Write(DTEST, 1);
aCore_Sleep(2000);
aDig_Write(DTEST, 0);
aCore_Sleep(18000);
}
// wait for GPS acquisition
while (1)
{
flag = aGPM_GetPosFoundFlag(aGPS_ADDR);
if (flag) break;
aDig_Write(DTEST, 1);
aCore_Sleep(1000);
aDig_Write(DTEST, 0);
aCore_Sleep(1000);
}
}
void navCalc()
{
int qlat = 0;
int qlong = 0;
// get offsets to target (waypoint B)
dlat = aGPS_ErrLat;
dlong = aGPS_ErrLong;
// get difference between last two offsets to target
// (equivalent to offset between two latest boat positions)
// use this value to estimate current heading
qlat = pre_dlat - dlat;
qlong = pre_dlong - dlong;
// update previous offset to target
pre_dlat = dlat;
pre_dlong = dlong;
// if there are non-zero offsets then
// calculate heading to target
// and calculate change in heading of boat
htarg = -1;
hboat = -1;
if (dlat || dlong)
htarg = aGPS_GetHeadingFromPosErr(dlat, dlong);
if (qlat || qlong)
hboat = aGPS_GetHeadingFromPosErr(qlat, qlong);
}
void steerCalc()
{
int e = 0;
int eabs = 0;
int rail = 90;
// check for valid heading data
// invalid data will just propagate zero error
if ((hboat >= 0) && (htarg >= 0))
{
aDig_Write(DTEST, 1);
e = htarg - hboat;
}
// handle wrap-around
if (e < -180) e = e + 360;
if (e > 180) e = e - 360;
// and apply limit
if (e > rail) e = rail;
if (e < -rail) e = -rail;
// find absolute heading error
eabs = aMath_Absval(e);
// within 5 degrees (-2,-1,0,1,2) just go straight
// negative error needs CCW turn
// positive error needs CW turn
if (eabs < 3)
{
aMotion_SetSetpoint(MLEFT, 32767);
aMotion_SetSetpoint(MRIGHT, 32767);
}
else if (e < 0)
{
aMotion_SetSetpoint(MLEFT, 0);
aMotion_SetSetpoint(MRIGHT, 32767);
}
else
{
aMotion_SetSetpoint(MLEFT, 32767);
aMotion_SetSetpoint(MRIGHT, 0);
}
}
void main()
{
int k = 0;
char flag = 0;
init();
// assign target
aGPS_AssignCoord(aGPS_LAT_B, 'N', 40, 3, 738);
aGPS_AssignCoord(aGPS_LONG_B, 'W',105, 12, 20);
// initialize target vector
aGPS_ReadLatLongGPM(aGPS_POS_A);
navCalc();
// start one-second reflex loop
aCore_Outporti(aPortRawInput + aMoto_RFX_TIMER_1, 0);
// make a course correction every 10 seconds
while (1)
{
aMulti_Wait();
if (k == 10)
{
k = 0;
flag = aGPM_GetPosFoundFlag(aGPS_ADDR);
if (flag) {
aGPS_ReadLatLongGPM(aGPS_POS_A);
navCalc();
} else {
htarg = 0;
hboat = 0;
}
steerCalc();
aCore_Sleep(5000);
aDig_Write(DTEST, 0);
}
else
{
aMotion_SetSetpoint(MLEFT, 32767);
aMotion_SetSetpoint(MRIGHT, 32767);
}
k++;
}
}
Compile and Load Programs The following commands compile and load the reflex and main TEA program. File Slot 11 on the Moto is a 16K TEA file slot, which easily handles the compiled program that is only 2400 bytes. leaf "gps"
steep "gps"
batch "gps.bag"
load "gps" 4 11
Results The Pond Explorer works fairly well on calm water. If it's windy, the boat tends to get pushed sideways fairly easily since it has no keel or rudder. If there is too much wind, the boat may be unable to stay on course. A different vehicle might work much better with the same navigation logic. Another problem is heat. All the electronic hardware is housed in a transparent container. It acts like a greenhouse! During one test late on a partly cloudy afternoon, the temperature got up to 100 degrees F inside the container. In the middle of a sunny day, it could get much hotter. That could lead to malfunctions. For now, the boat will remain a fun project for calm cool afternoons. Revision History:
| ||||||
Related Examples: | |||||||
| voice: 720-564-0373, email: sales@acroname.com, address: 4822 Sterling Dr., Boulder CO, 80301-2350, privacy © Copyright 1994-2008 Acroname, Inc., Boulder, Colorado. All rights reserved. |
