Engine for calculating direction and distance between two GPS waypoints

S

srnet

Senior Member
This has been discussed on here before, I have made some progress ......

My Picaxe Lost Model Locator does not have GPS capability, the maths are a bit much. The FPU is a possibility but it aint cheap .....

So I set about producing an engine that I could use as a slave device to a PICAXE to allow the distance and direction between two waypoints to be calculated. The engine was written and tested in Mikrobasic on a 18F26K22, but it will (I mean should) fit in a 14 pin 16F1825. The purpose here is that the LML can report the distance and direction from the start location to its lost location as morse or telemetry.

To use the engine all you do is extract the 2 sets of Latttude and Longtitude text from a GPS string and send it to the engine via RS232 in this format (the engine sits there waiting for an AT to be received);

AT
5128.6970
N
00310.9570
W
5128.9620
N
00310.9140
W

The engine then replies with;

Distance 497M Direction 6D
Latitude1 = 5128.6970 N
Longtitude1 = 00310.9570 W
Latitude2 = 5128.9620 N
Longtitude2 = 00310.9140 W
La1 51.47828 Lo1 3.18262
La2 51.48270 Lo2 3.18190

Those are waypoints of the centre of the Millenium Stadium and the centre of Cardiff Castle keep. The (digital) OS map says its 493 Metres and 5 Degrees.

All I need now is a 16F1825 to try it out, about £1.20 from Farnell.

Is there general interest in the HEX code, all you need is a suitable PIC and a programmer ........
 
A

Armp

Senior Member
Maybe I'm missing something - but why not just send the 'lost' location, and locate the model using a map or handheld GPS?

Chris
 
S

srnet

Senior Member
Armp said:
Maybe I'm missing something - but why not just send the 'lost' location, and locate the model using a map or handheld GPS?

Chris
Click to expand...
Well in the UK, whilst most of the OS maps have lat and long on them, its not detailed enough to be of use. And the conversion from lat and long into grid referance is country specific and in the case of the UK, quite complex.

As for the handheld GPS, well there is no reason why the lat & long could not be transmitted for the navigationally challenged, but distance and direction is shorter and simpler, the only extra kit you need is a compass ......

I also recall several robot type projects where users wnated to calculate distance and direction between waypoints ......
 
A

Armp

Senior Member
Point taken - but I don't think I could figure out how to get 493 metres, 5N was if I was in the centre of a Stadium, surrounded by roads and buildings without a trusty GPS in hand, or access to Google Earth :) !

Anyway I have calculated distances and bearings with sufficient accuracy using 1' Lat ~ 1853 metres, and 1' Lon ~ 1853 Cos Lat (or 1153 metres in Cardiff), and a bit of trig.
 
S

srnet

Senior Member
Armp said:
I don't think I could figure out how to get 493 metres, 5N was if I was in the centre of a Stadium, surrounded by roads and buildings without a trusty GPS in hand, or access to Google Earth :) !
Click to expand...
It was an example.

In open country, where most Model flying takes place, its not difficult at all.
 
S

srnet

Senior Member
The code for the 'simple' lost model locator took 3313 bytes of teh 20X2s 4096, so its likely there is space in there for a background receive from a GPS and a transmit of the extracted location.

And what sort or resolution is 'sufficient accuracy' say over distances of 1-2Km ?
 
A

Armp

Senior Member
Approx solution for your example:

Using 1' Lat = 1853 metres, and 1' Lon = 1853 Cos (Lat) Thats about 1153 metres in Cardiff

The model is 51° 28.9620' - 51° 28.6970' North of the original location.
That's 0.265' or 1853 x 0.265 = 491 metres N.

And 003° 10.9140' - 003° 10.9570' West.
That's -0.0430' or 1153 X -0.0430 = - 49 metres West.

So the model is 491m north and 49m east.

BTW - Thats 493 metres ; bearing of 5.8 degrees.
 
Last edited:
hippy

hippy

Ex-Staff (retired)
srnet said:
5128.9620
N
00310.9140
W
Click to expand...
I keep thinking about trying to do this maths on a PICAXE but I think I need a crash course in GPS notation. As Cardiff is at 51°29'N 03°10'W the 5128 and 00310 are obvious ( degrees plus two digits of minutes ), but what of the number after the decimal points, 9620 and 9140 - Are they they ten thousandths of a minute, are they seconds in some form or other, or something else ?

Being someone who loathes imperial unit sub-divisions I don't know why they don't simply specify degrees and have done with it but that's an aside rant.
 
E

eclectic

Moderator
AFAIK, it's degrees, minutes then four decimal places of minute.

I set the mapping software for Degrees /Minutes.

Aside. What a complicated method. :)

e
 

Attachments

S

srnet

Senior Member
hippy said:
I keep thinking about trying to do this maths on a PICAXE but I think I need a crash course in GPS notation. As Cardiff is at 51°29'N 03°10'W the 5128 and 00310 are obvious ( degrees plus two digits of minutes ), but what of the number after the decimal points, 9620 and 9140 - Are they they ten thousandths of a minute, are they seconds in some form or other, or something else ?

Being someone who loathes imperial unit sub-divisions I don't know why they don't simply specify degrees and have done with it but that's an aside rant.
Click to expand...
In the example above the GPS reports for the latitude;

,5128.9620,N

Which is is 51deg 28.9620min.

For the calcs you have to convert to decimal degrees, 51.48270in this case, and then radians.
 
S

srnet

Senior Member
Armp said:
Approx solution for your example:

Using 1' Lat = 1853 metres, and 1' Lon = 1853 Cos (Lat) Thats about 1153 metres in Cardiff

The model is 51° 28.9620' - 51° 28.6970' North of the original location.
That's 0.265' or 1853 x 0.265 = 491 metres N.

And 003° 10.9140' - 003° 10.9570' West.
That's -0.0430' or 1153 X -0.0430 = - 49 metres West.

So the model is 491m north and 49m east.

BTW - Thats 493 metres ; bearing of 5.8 degrees.
Click to expand...
That is a bit simpler than the calculation I use;


sub procedure calcdirectdist()
'here comes the trigonometry! Calculate direction and distance from current location to home
'First, convert to radians
La1=La1*pi/180
Lo1=Lo1*pi/180
La2=La2*pi/180
Lo2=Lo2*pi/180
'calculate distance using great circle formula
Dist=2*Asin(sqrt(sq(sin((La1-La2)/2))+cos(La1)*cos(La2)*sq(Sin((Lo1-Lo2)/2))))
'calculate direction
Direct=acos((sin(La2)-sin(La1)*cos(Dist))/(sin(Dist)*cos(La1)))
if sin(Lo2-Lo1)>0 then
Direct=2*pi-Direct
end if
'convert results
Direct=(180/pi)*Direct 'convert to degrees
Dist=4010*Dist ' convert to miles
Dist=dist*1.6 ' convert to KM
dist = dist * 1000 'convert to Metres
end sub


But ...... how accurate would the approximation you did be if done in PICAXE basic, or maybe it was ?
 
Last edited:
hippy

hippy

Ex-Staff (retired)
srnet said:
In the example above the GPS reports for the latitude;

,5128.9620,N

Which is is 51deg 28.9620min.
Click to expand...
Thanks.

srnet said:
That is a bit simpler than the calculation I use;

But ...... how accurate would the approximation you did be if done in PICAXE basic, or maybe it was ?
Click to expand...
That's probably the calculation I was going to use ( and I think I said earlier that I thought the calculation being used may be unnecessarily complicated ). First step; slide the points around so one of them is at 0,0 - after that everything gets simpler.

How accurate on the PICAXE; that remains to be seen. Distance is simply Pythagoras, the trig for the angle a little more complicated.
 
A

Armp

Senior Member
srnet said:
But ...... how accurate would the approximation you did be if done in PICAXE basic, or maybe it was ?
Click to expand...
For a 5 km search radius the numerical difference between the simple expression and the Great Circle Calculation is about 0.003%.

If the calculation is done with 16 bit integers and appropriate scaling the total error would be less than 0.01% - about the accuracy of GPS itself....
 
S

Svejk

Senior Member
Indeed, declination is the name. Deviation has something to do with ship orientation. Never mind.

For short distances you may consider the surface a plane as the errors won't be big enough to make a difference. I think you can extract the local magnetic declination from NMEA sequence. +/- 1 degree when using a compass to locate the lost object won't matter too much.
 
hippy

hippy

Ex-Staff (retired)
Svejk said:
For short distances you may consider the surface a plane as the errors won't be big enough to make a difference.
Click to expand...
That was my thinking. For longer distance the error may be excessive to start with but as one heads towards the destination a recalculation should bring that error down the closer it gets.

For both distance and angle, it should be possible to scale the base data up and reduce it at the end, thus from (0,0) to (3,4)

d = SQRT ( 32 + 42 )
a = ASIN( 3 / 4 )

could equally be, or something like it ..

d = SQRT ( 302 + 402 ) / 10
a = ASIN( 30 / 40 )

By suitable scaling one can make everything integer then it's only an issue of dealing with big numbers rather than floating point.
 
hippy

hippy

Ex-Staff (retired)
Svejk said:
I think is ATAN(3/4).
Click to expand...
You are absolutely right; SOHCAHTOA, not a misremembered SOACAHTOA. I can never remember if (3,4) is (x,y) or (y,x) nor whether x or y is up or across so being 3/4 or 4/3 was a 50-50 chance, and I think I was taking the angle from the horizontal ! Probably a case there of more marks for effort than in getting it right :)

As we've got all three distances it doesn't really which ARC<thing> we use, whatever is easiest to implement.

Svejk said:
Dot product with an unity vector may yeld faster results for angle.
Click to expand...
Now you've lost me :)
 
S

Svejk

Senior Member
Sorry, not intended.

The dot product is often used to get the angle between two vectors (directions). Might not be suitable for this application. Anyway using dot product would give you the value of the cosine of the angle between the two vectors using simple multiplications and additions.
 
hippy

hippy

Ex-Staff (retired)
Thanks Svejk; I hadn't heard of that but looks like something to investigate. Anything which simplifies the maths will definitely help.
 
S

srnet

Senior Member
Can Picaxe basic do an INV SIN ?

If not you could set up a table of the SINs from 0-89 degrees in memory (possible ?) as words so to 4 'decimal' places. Search the table for the closest match, from the example above;

East Component/North Component = 49\490 = 0.1

Sin 5 = 0.0871, Sin 6 = 0.1045 so closest match = 6 degrees.

1 degree is the practical limit for navigation accuracy in most cases.
 
hippy

hippy

Ex-Staff (retired)
Good solution and even better with a little tweak -

As the ratio of the two distances can be kept between 0.000 and 1.000 by switching quadrants and adjusting back afterwards, scale that by 1000 and it's a simple lookup of scratchpad on a 28X2 or data split between EEPROM/TABLE to determine any desired ARC<thing> to 0.5 degree accuracy.

That makes it very simple -

1) Grab the two coordinates
2) Adjust to be (0,0) and (a,b)
3) Determine distance with Pythagoras
4) Calculate a ratio 0 to 1000 from two of three distances
5) Lookup angle from ratio
6) Adjust numbers to untwiddle any twiddling of values
 
A

Armp

Senior Member
ATAN(X) Calculation

From the days of 4 function calculators I use the polynomial approximation for ATAN(x) over the range 0 < x <= 1

Angle(Degs) = -0.3 + 62 X - 16.7 X2
PICAXE can do this with a bit of scaling

Gives:
X=0.0 Angle = -0.3°
X=1.0 Angle = 45.0°
X=0.5 Angle = 26.5°

For the current example:
East Component/North Component = 49\490 = 0.1
Angle = -0.3 +6.2 - 0.167 or ~ 5.7°

If X>1 then replace with 1/X; and subtract result from 90

So ATAN(2.0) = 90° - ATAN(0.5)

Similarly for other quadrants.
 
Last edited:
A

Armp

Senior Member
A bit Off Topic...

The engine was written and tested in Mikrobasic on a 18F26K22, but it will (I mean should) fit in a 14 pin 16F1825.
Click to expand...
Can you give a little more info about Mikrobasic? I tried to send you a PM but I guess it didn't work?

I am currently working on a similar project to the LML. We fly a couple of RC planes, equipped for aerial photography, and sometimes they lose signal! My project is to enhance the current acc/gyro stabilization system by including GPS 'fly home' software that would bring us back in range. Should probably include LML in case it doesn't work :)

Presently prototyping an ARM + PICAXE system, but would like to move to PIC...
 
S

srnet

Senior Member
Armp said:
Can you give a little more info about Mikrobasic? I tried to send you a PM but I guess it didn't work?

I am currently working on a similar project to the LML. We fly a couple of RC planes, equipped for aerial photography, and sometimes they lose signal! My project is to enhance the current acc/gyro stabilization system by including GPS 'fly home' software that would bring us back in range. Should probably include LML in case it doesn't work :)

Presently prototyping an ARM + PICAXE system, but would like to move to PIC...
Click to expand...
I did not see the notification, thats all.

Mikcrobasic should be powerfull enough for the job, its a good deal faster than PICAXE basic, but you have to do lots of things for yourself, a thorough understandic of low level PIC stuff is essential. Has lots of libraries for UARTS, LCDs and lots more about the same as PICAXE really.

In the case of the GPS calculation the language takes a complex trig and floating point calculation with multiple bracket levels directly.

Downside is it is not easy to work with and the complier costs circa £150 (same price for the C and Pascal ones).

I fly AP too, my best AP ship is a Hawkye, and by the time you have a decent camera on it, there is several hundread pounds of investment, hence the LML. The PICAXE LML is good enough to locate a model, there is someone selling a basic commercial model using the same RF Module.

As for RTH stuff, I would not underestimate the complexity of that myself, there is published stuff for the A****, but if I want that functionality my Eagle Tree logger with the OSD can do that.

One thing I might well do, is to take the PICAXE LML and add a GPS read capability to it, whilst the Distance and Direction calculation looks like a fair bit of work, stripping the Lat & Long location from the GPS string and sending it out via morse ought to be easy enough.
 
A

Armp

Senior Member
Mikcrobasic should be powerfull enough for the job, its a good deal faster than PICAXE basic, but you have to do lots of things for yourself, a thorough understandic of low level PIC stuff is essential. Has lots of libraries for UARTS, LCDs and lots more about the same as PICAXE really.
...
Downside is it is not easy to work with and the complier costs circa £150 (same price for the C and Pascal ones).
Click to expand...
Thanks for the feedback - that's pretty much what I concluded. The ARM solution is a $30 card complete with a Basic compiler/simulator, or a $12 card with a number of free C compilers - so that's the way I'll continue. I do have a couple of years of ARM experience and am very familiar with coding the peripherals.

The PICAXE will do the in-flight data logging and LML tasks.
 
S

srnet

Senior Member
Armp;187021The PICAXE will do the in-flight data logging and LML tasks.[/QUOTE said:
No reason why the GPS cant be read by both units, a 28X2 with resonator (reducing the risk of baud matching issues) ought to be able to manage the GPS receive.

I have a PCB for a 28X2, RFM42 and GPS connector, see picture, uses 0603 SMT stuff, but easy enough to DIY, although by putting the 28X2 under the RFM42 the size could be reduced a fair bit.
Click to expand...
 

Attachments

You must log in or register to reply here.
Top