Distance between Long/Lat on BS2

M H

Hello All,

I’m using a BS2 microcontroller with the Parallax GPS Receiver module to get longitude/latitude data into my stamp. I have two sets of coordinates that I want to calculate the distance between. Is this possible, and how is it best to go about it?

I’ve programmed forever, but never on a system that couldn’t do floating-point arithmetic! My approach was to use the haversine formula, but I can’t figure out how with the BS2’s weird trig functions and lack of fractional number support. I’m including some (non-working) code I was fiddling with just so you can see the direction I was thinking. The commented out code is a working Javascript function I was trying to emulate.

But really, I have no idea what I’m doing. If anyone could give me some guidance on how to calculate the distance between two sets of longitude/latitude coordinates, or tell me that it absolutely cannot be done on a BS2, I’d really appreciate it!

Thanks in advance,
Matthew

' {$STAMP BS2}
' {$PBASIC 2.5}

Lat1Deg VAR Byte
Lat1Min VAR Word

Lon1Deg VAR Byte
Lon1Min VAR Word

Lat2Deg VAR Byte
Lat2Min VAR Word

Lon2Deg VAR Byte
Lon2Min VAR Word


R CON 6371

dLat VAR Word
dLon VAR Word

a VAR Word
c VAR Word

d VAR Word

' BEGIN

Lat1Deg = 38
Lat1Min = 0353

Lon1Deg = -78
Lon1Min = 5165

Lat2Deg = 38
Lat2Min = 0346

Lon2Deg = -78
Lon2Min = 5159


dLat = (Lat2Min - Lat1Min) *  (128 / 180)
dLon = (Lon2Min - Lon1Min) *  (128 / 180)

Lat1Min = Lat1Min *  (128 / 180)
Lat2Min = Lat2Min *  (128 / 180)

Lon1Min = Lon1Min *  (128 / 180)
Lon2Min = Lon2Min *  (128 / 180)


'var R = 6371; // km
'var dLat = (lat2-lat1).toRad();
'var dLon = (lon2-lon1).toRad();
'var a = Math.SIN(dLat/2) * Math.SIN(dLat/2) +
'        Math.COS(lat1.toRad()) * Math.COS(lat2.toRad()) *
'        Math.SIN(dLon/2) * Math.SIN(dLon/2);
'var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
'var d = R * c;

a = SIN(dLat/2) * SIN(dLat/2)
a = a + (COS(Lat1Min) * COS(Lat2Min) * SIN(dLon/2) * SIN(dLon/2))

c = 2 * (SQR(a) ATN SQR(1-a))

d = R * c

DEBUG SDEC d

MSDTech

There probably is some way to do it with the BS2 math, but I like to cheat. You might want to look at adding one of these:

http://www.parallax.com/tabid/617/List/0/CategoryID/82/Level/a/SortField/0/Default.aspx

phil kenny

If you use the FPU V3 coprocessor, here are links that show how to do just what you
are asking about:

www.micromegacorp.com/downloads/documentation/AN039-GC%20Distance.pdf

and

www.micromegacorp.com/downloads/software/AN39-GC%20Distance.zip

phil

M H

Wow, that sounds perfect. Thanks guys! Unfortunately, I don't think I can get it shipped in time for my project. Any way of doing it without additional components?

Franklin

What is the formula to do what you want?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
- Stephen

M H

The "Haversine" formula.

A javascript implementation is as follows:

var R = 6371; // km
var dLat = (lat2-lat1).toRad();
var dLon = (lon2-lon1).toRad(); 
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
        Math.cos(lat1.toRad()) * Math.cos(lat2.toRad()) * 
        Math.sin(dLon/2) * Math.sin(dLon/2); 
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); 
var d = R * c;

Where lat/lon are in decimal degrees (obviously not possible on the BS2).

More info on this formula can be found http://www.movable-type.co.uk/scripts/latlong.html

Thanks so much for the assistance!

Beau Schwabe

M H,
·
Have you looked at the ATN (Arctangent) or HYP (Hypotenuse) functions built into the BS2?
·
There is also SIN (Sine of an Angle), COS (Cosine of Angle), and SQR (Square Root)
·
The HYP could be used in determining distance... To get·more resolution you might need to apply the function in a long hand sort of way by looking at your Delta X and Delta Y in decade increments.
·
The ATN combined with SIN, COS, and SQR could be applied to your JavaScript example, but I think the long-hand HYP method might provide better resolution.
·




▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Larry

You can , in fact , do some fractional math with the BS2. Have a look at the ** and */ commands.

They allow you to get the results of 32 bit multiplications and allow multiplication by fractional amounts.


I'd use only the minutes and fractional minutes, ignoring the degrees in your computations to make the math a bit easier. You'd have to know what the distance was locally for a fraction of longitude and latitude (use google maps)

If your robot path does cross a degree, you could just shift all the coordinates a half degree and proceed with the math, then shift back after the computations.

Or, you can change to brads and use the BS2's trig functions.

A good article was written by John Overstrom about the problem in the April 2008 issue of SERVO, including the following code--

www.servomagazine.com/media-files/866/GPS_Guided_Car.zip

That should get you started.

Pay attention to a couple of "gotchas" the author deals with, including the "zero crossing" steering issue.

Tracy Allen

What typical distances are you looking at? The formulas are really meant for dealing with long distances where it really matters that the triangle is drawn on spherical geometry. For short distances there is probably an approximation you can use (but I don't know specifically). The haversine formula is an approximation in any case due to topography and the fact that the earth is not a perfect sphere of radius 6371.1 kilometers. If you are near one spot on earth, the terms in lat1.toRad() and lat2.toRad() are going to be practically constants, maybe? The Stamp's builtin functions for trig and sqrt functions are accurate to about 7 bits, but that might be okay if you only have to deal with the differentials. On my web site I have some CORDIC math for trig and inverse trig functions to better than 12 bits, and formulae to extend the accuracy of sqrt. However, in fixed point math the problem is always one of scaling, and that takes concentrated use of the thinking cap, and more time than it might take to order the coprocessor. It is not as easy as plugging the formula into the coprocessor, into which you'd program the whole sequence of operations, then at run time the Stamp sends it the lat and lon of the two points, and out pops the answer.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

M H

We're talking small-scale distances here. As in, my units are meters. That CORDIC math library looks really neat, thanks for sharing! I think maybe you're right and I can just use Pythagoras for the distance calculations?

Tracy Allen

I think so. Guess a radius R of the earth at your location. Then

dy = R * dlat       ' arc length dy subtends a minisucle angle dlat on a great circle through poles
dx = R * cos(lat) * dlong   ' arc length dx that subtends an angle dlong on a circular section at latitude lat

And lat, dlat and dlong are the differences in radians, although you will have to play with the scaling in order to use the Stamp ** operator and to avoid integer overflow. R*cos(lat) will be a constant. I think that is right, but check me on it. Then apply Pythagoras. Here is a link to extend the accuracy of the SQR operator using the high school algorithm.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

M H

Nice! I did a bit of looking and the BS2 trig functions work in brads, and with a 127 unit circle, which are weird... anyone have some example code or pointers that would indicate how to implement the procedure Tracy is suggesting? Especially with regard to converting to the Stamp's odd unit system and then converting the answers back to something usable. Thanks!

Bruce Bates

M H -

All of the information you're seeking can be found in the PBASIC Reference Manual which can be downloaded for FREE from the Parallax web site. The Parallax web site is here:
http://www.parallax.com

Once you reach the web site, go to the Downloads Section. You will find the Reference Manual there along with other free downloads all of which relate to the PBASIC Stamp.

Regards,

Bruce Bates

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Involvement and committment can be best understood by looking at a plate of ham and eggs. The chicken was involved, but the pig was committed. ANON

Post Edited (Bruce Bates) : 4/28/2008 1:37:08 PM GMT

Tracy Allen

I have a bit of example material posted on brads, here.

However, note that the run-time math does not contain any trig.

-- The R*cos(lat) is a constant, at your average latitude. (we will assume you are not circling close to the north or south pole!) Forget about R, which will rolled into the scaling constant at the end. Find cos(lat) on your calculator, a number between zero and 1, and multiply that times 65536. Call that number B, B= 65536 * cos(lat).

-- The first calculation on the Stamp will be to subtract the two latitudes and the two longitudes to get the difference in fractional minutes. Will your distances be more than one minute (about 1800 meters, by quick calculation)? It might require a simple borrow from minutes and maybe even from degrees (where the borrow adds 60, not 100 minutes to the borrowee), but it is simple math rules. Both of those dlat and dlong will be Words in fractional minutes.

-- Now have the Stamp multiply the difference dlong by the factor B using the ** operator:
dlong = dlong ** B.
That accounts for the intrinsic scaling factor cos(lat).

-- At this point, The Stamp could do another scaling tto put dlat and dlong directly in units of meters. That would involve another ** operator to account for R and a local fudge factor and a unit conversion constant. You don't have to sort through all those constants in theory. Just take two points in your area, and you know the actual distance between them (from a map or from Google), and that lets you figure the scaling constant. How many meters do you expect to move?

-- Now comes the tricky part. It may be that the sum of the squares, dlong^2 + dlat^2 would overflow one word. That happens if the individual distances are combinations like (256,0) or (182,182). This is a situation where I might be inclined to go for double precision to have the Stamp figure the sum of the squares into 32 bits. Then take the square root of that, which can be done without too much trouble.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Post Edited (Tracy Allen) : 4/28/2008 4:40:27 PM GMT