Navigation with gps and compass on prop?

Botdocter

I am searching for a navigation script for the propeller.

I don't need a nmea parser because the variables from the gps are received through serial.
What i am looking for is the following:

The robot gets a location in lat/long coordinates. Then i want the bot to check it's bearing. So now he knows where he is and in what direction. Now it must calculate where to turn (heading (how much degrees)) to point towards the goal location. Then it should drive to the goal location.
This must be updated all the time because another script on the pc will take over controll every now and then. So after that the calculations must be redone. So a contantly update is a must!

I have searched all over the internet but couldn't find anything particulair.
I could really use your help!

max72

I created a small object to perform integer only navigation.
I posted it to the forum asking for opinions, but I haven't posted it to the obex yet.

http://forums.parallax.com/showthread.php?128884-integer-only-GPS-navigation-object

The object handles nmea parsing too, but you can discard it.
It internally calculates using integers (1/100000 of a prime) and 13 bit numbers for angles.
Navigation is performed using planar cartesian projection, but the errors are negligible in a robotic navigation context.
You get also angle to target (absolute and relative to your bearing). It is a spin only object, but it is rather fast, so you can run it constantly in the main COG. I'm working on a SpinLMM version of the object to gain speed, but at the moment only string parsing are working.
If you don't like the way it works you can probably adapt it to float32.

Massimo

Botdocter

Thank you!

I have looked at the code but i need a tiny bit of help. I need to know what variables to use for my gps and compass. the lat,long,bearing and whatever more is nescessairy, so that i can pass/skip the nmea parser?

max72

let's assume you have at disposal current position, and way point coordinates.
Suppose also to have the current heading.
If you have coordinates as 1/100000ths of primes (as an integer) and the heading transformed to 13 bit angle.
consider 1 prime difference in latitude is 1 nautical mile.
use:
angletotarget(currentlat,currentlong,targetlat,targetlong)
to get the absolute angle to the target (waypointangle).
disttotarget gives you the distance in 1/100000th of nautical mile, you can convert to meters with tometers.

AngleDiff(CurrentAngle,WayPointAngle) gives you correction angle to target.

you can get back to degrees with togrees function.

Test it with sample data or check the demo code to see how nmea conversion works. You should provide data already converted in this way.

Let me know if it works for you. I would like your opinion.
Massimo

Botdocter

max72 wrote: »

If you have coordinates as 1/100000ths of primes (as an integer) and the heading transformed to 13 bit angle.
consider 1 prime difference in latitude is 1 nautical mile.

This is still a bit unclear to me. my gps lat,long is coming in like this; Lat. 52.3652, Long. 4.9389
What exactly do i need to do with these values to make them right for the script?

Also my compass sensor outputs a value of 0-3600 for obviously 0 to 360 degrees. How do i make this a 13 bit value?

the rest is quite understandible!

Thank you for your help...

max72

You are correct.
I decided to go with primes because of the convenient scaling. On a sailing boat nautical miles and knots are the natural way to go, so even better.. I then added zeros as long as the range limit of the 32 bit signed integer is observed.
My GPS sends a string like that:
4510.12345
it means it is 45° 10.12345'
I convert it this way: (45*60+10.12345)*100000. The objects handles that.
If you have fractional degrees like your example you should have (52.2652*60*100000).
The sign is positive for N and E, negative for W and S.
The deltalongitude risks the overflow, so I shift the numbers right before the calculation, loosing a bit.
For the angle I use the sin/cos table in ROM, and it uses 13 bit angles. It means 360 degrees are converted to 8192. This way I can directly get sin/cos from he ROM table, convert it to 32 bit and use multiply high for VMG calculation, cross track error and so on.
The todegrees brings back angles in degrees.
You can do the same with float32 or the f32 object if you feel it is better for you.
The trick is you project the coordinates on a cartesian plane.
The two components of the distance between the two points along north end east are respectively the difference in latitude between the waypoint and the current position (along north) and the difference between the longitudes * cosine (average latitude).
Having the two projections along the two axis you can do Atan2 to get distance and angle between the points.
The solution is approximate, but it is good up to a few hundred miles, as long as you stay away from the poles. Near poles you need a dedicated projection, in case you need a tester let me know..

Massimo

Botdocter

It took me some time to figure out what to do exactly. I let go of the example and started reading the driver behind it. That made some things easier.. I think i have it down, except the variable that i have to use for the compass sensor data. I understand about the translation to 13bit but what is the variable i have to link it to, so it will be used in the code? Since there are a few options.

max72

in the demo I use gpsheading. The value is parsed from the nmea gps string in my case. In your case you'll use your compass angle.
angle is the absolute angle from the current position to the way point. It is calculated with trig.angletotarget. After that if you call trig.disttotarget you get also the distance to the waypoint (distance is a byproduct of angle...).
In anglediff I store the relative angle between my current course and the one I shold follow. So this last can be seen as an angle correction value.
when converting to degrees if anglediff is > 180 then anglediff is 360-anglediff. This way I have a correction angle between -180 and 180.
Hope it clarifies.
Massimo

Botdocter

In the code it says:

Heading:=trig.angletotarget(lat[11],lon[11],lat[i],lon[i])

What does " [11] and " mean?
I know the [11] has something to do with the reading of the demo nmea string, but what do i need to put there in place? Since i have ready made lat and lon variables that i link to it.

And the i saw allover the code saying: i:=0. What is that for?

max72

At the end of the code there is a DAT section with the strings that are parsed in two arrays.
The two arrays are declared in the var section. During the demo code the strings reproducing fake NMEA data are parsed the the arrays as integer longs.
In the code snippet a series of distance and heading are calculated from the coordinates stored in the 12th position of the arrays lat[] and lon[], against all the other values (0 to 11). The repeat loop starts setting i:=0 beforehand, and at every loop cycle i is incremented by 1. It could have been done all in the repeat sentence (repeat from to step), but I'm used to do it this way.
array index in spin starts from 0.
Massimo