HobbyKing.com New Products Flash Sale
Reply
Thread Tools
Old Jan 30, 2009, 03:30 PM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Discussion
Navigation routines on a PIC16F RCAP

I have been itermittently developing the RCAP code (mostly backburner and not so much frontburner) to perform the gps navigation math inside the PIC chip. Early on, I tried some table lookups for calculating angles and distance. These early attempts were pretty crude, and not very accurate.

I turned to the cordic routine to gain accuracy and speed, even for the slower 5 mips PIC16F series pics. The code is written in PicBasic Pro, and some assembly language for servo control, and the cordic math. The code does account for differences in the length of longitude with respect to latitude, and corrects for it. It does assume the earth is flat, but for shorter distances, even in the 50 mile range, the difference between a sphere and a flat earth is not very great. For more information about the cordic assembly code, and results, check out http://www.picbasic.co.uk/forum/showthread.php?t=7502

Rudder control still uses the RCAP's old code, and could use some improvement. However, the 5 hertz gps is a 10x improvement in time, and 100x in resolution. So at 35mph your course can be corrected every 50 feet, instead of 500.

Results have been pretty good while driving around in a car with the development board, a PICDEM 2 Plus. I am currently outputing distance in x.xx miles and in feet, and degrees in xxx.x degrees.

The project is connected to a EB-85A GPS at 38400 baud, giving 5 hertz output. Currently the EB-85A goes through a max232 before it goes through the max232 on the picdem 2 plus board.

The PIC16F877A stores up to 30 waypoints in its EEprom. These can be entered using waypoint.exe from rcgroups user MX. When entering waypoints, the first 8 bytes in eeprom are reserved for setup data, so only waypoints 2 through 31 will be used as waypoints, and waypoint 2 will be searched first. Waypoints will be considered reached when dist_away value is reached (distance = dist_away)

On the development board, Feet, waypoint# and bearing to, and distance in miles x.xx is shown on the LCD. Rudder input and output are controled by the board, and I have done some testing with this on a car based RCAP board.

One thing to keep in mind, is that on the EB-85A the gps nmea data valid character "A" or "V" is char #13 as far as this code is concerned. So if a different gps is used, you would need to see where this validity character falls.

The pic16f877a code is pretty easily tranfered to the pic16f876a to be used in RCAP hardware, however, no flight testing has been performed as of yet, just a lot of driving miles.

I am rewriting a short description of how this works, if anyone is interested, as my code sometimes confuses myself.

Code comes in at about 6,350 bytes, but it can be reduced a bit without the lcd code, and I am sure there are some ways to reduce the rest of the code some more. But where it currently stands, it leaves about 1/4 left for more stuff.

I hope that this code will be usefull to people, and that perhaps it will spark other's backburner projects.


Walter
radiohound is offline Find More Posts by radiohound
Last edited by radiohound; Jan 31, 2009 at 12:00 AM.
Reply With Quote
Sign up now
to remove ads between posts
Old Jan 31, 2009, 02:10 PM
7000mw of raw power!
rich smith's Avatar
New Hampshire (not the old one)
Joined Dec 2006
5,987 Posts
I found your project very interesting, particularly the PIC app note on atan. My own autopilot now uses Megs8 AVR but I went through the same kind of stuff as you. I started with BASIC (BASCOM) to but switched to C then ended up in Assembler due to space constraints. The atan stuff was also replaced with some 32 bit integer tricks for the same reason.

Sorry I cant; help with the PIC error. It's been a while since I used Microchip. Good luck.
rich smith is offline Find More Posts by rich smith
Reply With Quote
Old Jan 31, 2009, 03:27 PM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Quote:
Originally Posted by rich smith
I found your project very interesting, ....
Sorry I cant; help with the PIC error. It's been a while since I used Microchip. Good luck.
Thanks Rich. It does sound like a similar path. Yes, I have had many problems, and expect to see many more down the road. At one point of frustration, I did buy CCS's C programing language and started learning how to use it. However, at this point, I was trying to say that I have it working pretty well. It's almost ready for some flight testing.

Here is a pic of it routing to the same waypoint that the garmin is on a goto. The PIC is doing its own math on the NMEA data, and the Garmin is doing its own math. They are pretty close. The distance formula is a bit crude, and could be helped by a float, or a lot of other ways I am sure. It does get much more accurate with a bit more distance. The angle is very accurate, even on short distances. Shown are Feet, waypoint id, bearing to destination, and miles/100 (this is .03 miles).

Once I run out of space, I will try the migration to PIC18F devices using PICBasic, if I get too frustrated with that, I plan on 24HJ devices with C. So many options!
radiohound is offline Find More Posts by radiohound
Last edited by radiohound; Jan 31, 2009 at 04:04 PM.
Reply With Quote
Old Jan 31, 2009, 08:12 PM
Expert plane crasher
AXI Motor Models's Avatar
Central Coast, Australia
Joined May 2005
490 Posts
That is a great project and it will be good to see how it goes.
Are you going to implement return to home in the PIC?
AXI Motor Models is offline Find More Posts by AXI Motor Models
Reply With Quote
Old Jan 31, 2009, 11:27 PM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Quote:
Originally Posted by AXI Motor Models
Are you going to implement return to home in the PIC?
The more I think about your question, the more I like the sound of it.

As it stands, any time the autopilot is set to enable, it will find the first waypoint programmed, then it will find the next one. When it gets to the last waypoint, it will circle (currently a haphazard random figure 8 or 0 pattern).

The (old and not available) FMA FS-8 has a failsafe setting where you can set some of the servos if there is a loss of signal. Looks like a few other manufacturer's are making failsafes now. One of these channels could be used to tell the hardware to abort mission and return to last waypoint. I do not know exactly if you were thinking of a return to home that would be caused by one of these types of sensors, or on command. Either way, it would probably make the most sense to have it return to the last waypoint programmed.

As for an abort mission from the transmitter, the enable disable for the autopilot could be split into 3 position instead of the current 2 position. So you would have Manual (pass through), Autopilot, and return to home. Being controlled by a three position switch on your transmitter.
radiohound is offline Find More Posts by radiohound
Reply With Quote
Old Feb 01, 2009, 03:08 AM
Expert plane crasher
AXI Motor Models's Avatar
Central Coast, Australia
Joined May 2005
490 Posts
Quote:
Originally Posted by radiohound
As for an abort mission from the transmitter, the enable disable for the autopilot could be split into 3 position instead of the current 2 position. So you would have Manual (pass through), Autopilot, and return to home. Being controlled by a three position switch on your transmitter.
That sounds perfect.
On PCM recievers, the failsafe setting could be programmed to enable return to home mode so that when the reciever loses the transmitters signal, the plane travells towards home until the signal is regained.
AXI Motor Models is offline Find More Posts by AXI Motor Models
Reply With Quote
Old Feb 10, 2009, 05:09 PM
7000mw of raw power!
rich smith's Avatar
New Hampshire (not the old one)
Joined Dec 2006
5,987 Posts
I consider RTL an essential stepping stone on the road to true waypoint sequencing. So much easier than running all over the countryside wondering what the plane is up to. Failsafe an even more basic stage. You know, baby steps.

Walter, I've been following your project for last couple years but was discouraged by need for separate boards (MX?) to do autopilot for non-hiking type GPS. Do I understand that you are now integrating waypoint functions into a single RCAP unit? Has it been tested with 1hz receivers like em406 yet? (500' ok for me now)

I am again considering building an RCAP3. I learned so much from Tom Harper, Attopilot, Paparazzi, etc, this can only help me even more with my own. It is also perfect excuse to get back into the Microchip world. Gotta throw together another PIC programmer.






Quote:
Originally Posted by radiohound
The more I think about your question, the more I like the sound of it.

As it stands, any time the autopilot is set to enable, it will find the first waypoint programmed, then it will find the next one. When it gets to the last waypoint, it will circle (currently a haphazard random figure 8 or 0 pattern).

The (old and not available) FMA FS-8 has a failsafe setting where you can set some of the servos if there is a loss of signal. Looks like a few other manufacturer's are making failsafes now. One of these channels could be used to tell the hardware to abort mission and return to last waypoint. I do not know exactly if you were thinking of a return to home that would be caused by one of these types of sensors, or on command. Either way, it would probably make the most sense to have it return to the last waypoint programmed.

As for an abort mission from the transmitter, the enable disable for the autopilot could be split into 3 position instead of the current 2 position. So you would have Manual (pass through), Autopilot, and return to home. Being controlled by a three position switch on your transmitter.
rich smith is offline Find More Posts by rich smith
Reply With Quote
Old Feb 10, 2009, 05:47 PM
Registered User
Joined Dec 2006
369 Posts
My best memory was that during WPS testing, the RCAP2 would not work with the Sirf type gps modules. The guy that does the RCAP3 found the problem in the RCAP code and changed it for his unit. I emailed Radiohound about fixing the issue in the RCAP2 also, but he thought the WPS code was at fault. I gave up at that point as I didn't want to buy another Garmin module.....

Here's a quote

"I have problems at the begining with EM406 because this GPS don't follow exactly the NMEA standar. I had to made some fix in the RCAP code to accept it.

I think that 401 would work OK too, with RCAP3, but I have not test.

I you can capture and send me some NMEA code of your GPS I will tell you if it's compatible.

Good about EM406 is that it only cost now 56$ at sparkfun."

Eladio
Wmacky is online now Find More Posts by Wmacky
Last edited by Wmacky; Feb 10, 2009 at 05:54 PM.
Reply With Quote
Old Feb 10, 2009, 06:03 PM
Nemo in a blender
Connexxion's Avatar
The Netherlands-Amsterdam
Joined Mar 2007
940 Posts
RCAP3:

http://alai.h3m.com/~s0350672/catalo...918741b23731a7
Connexxion is offline Find More Posts by Connexxion
Reply With Quote
Old Feb 10, 2009, 06:20 PM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Hey Rich,

The RTL (Return To Launch Site) would be very usefull, and is being added as we speak, just a few more lines of code than return to last waypoint.

Yes, have just about completed the code for the internal navigating PIC chip. I have not used it with the OEM 1htz em406, but I have used it with the 5htz OEM EB-85A, which is somewhat similar. In the picture I was only showing the develpment board hooked up to a hiking gps (0.5 htz) so that you could see the difference in calculations on the two units. The PIC showed a 0.6 degree difference while the waypoint was less than 200 feet away. This is pretty darn close, and the hiking gps does not show decimals, only whole numbers, so the difference may have been even smaller. The cordic is an approximation tecnique, but it is very accurate, and very much faster than the sin and cos equivalent in any language. I am told the cordic was used in the Lunar Lander cpu, and that the moon landing might not have happened without it. But then, didn't they take it off autopilot because the cpu could not keep up with the data anyway ... hmmmm. Bad example...

Any chance you could email me some data logs of the em406 while it is getting a valid signal? I would be curious if it is straign compatible with the format of the EB-85A, as far as characters line up in the NMEA sentence. Specifically interested in RMC and GGA. That way I could make the code valid for anyone that has the em406. There seem to be a bunch out there.

As far as testing goes, I have tested it all over northern California, but only in the car. Will start some real flight testing soon, then I will put a new hex file out that others can test as well.

I am only a little over 80% of the code capacity on the PIC16F876a, so there is a bit of room left. Room for bells and whistles later.
radiohound is offline Find More Posts by radiohound
Reply With Quote
Old Feb 10, 2009, 06:55 PM
Registered User
Joined Dec 2006
369 Posts
Hers is another quote from the old WPS thread. The thread kinda died after the post so nothing much was said about this finding. Perhaps it's the answer?

"Hello

Finally my EM-406 + WPS + RCAP working OK.

Next weekeng I will do a Fly test and post results.

But I think that the RCAP code have a bug:

In the label DoB:

DoB:

.
.
IF gpsdata[1] != "V" THEN
.
.
.
I think that it must be gpsdata[2] != "V"

$GPRMB,A,0.00,L,000,000,3721.3995,N,00607.6164,W,1 1.557,259.8,000.0,V*10

I thing that:

"B" is gpsdata[0]
"," is gpsdata[1]
"A" or "V" is gpsdata[2]
"0" is gpsdata[4]
"0" is gpsdata[6]
"0" is gpsdata[7]

Now RCAP is always reading a "," no mater if A or V is in the GPS sentence

Correct me if i'm wrong

Best regards

Eladio


Oct 31, 2006, 01:54 PM
"
Wmacky is online now Find More Posts by Wmacky
Reply With Quote
Old Feb 10, 2009, 10:03 PM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Thanks Wmacky,

Yes, the author was right, it was looking at the wrong character to see if it was not a "V", so all data was considered good from the gps, instead of throwing it back to manual control. I though this was fixed in version 1.6b, but I may be wrong about that. I will have to look back. It is fixed in my current version, as it looks for an A instead of any other character besides V. I just need to get it to look at the right character for the em406. I will search for some data logs from that gps.

And just so everyone is on the same page:

The RCAP was made by Michael Pawlowsky. It needed a goto gps set to navigate to a waypoint or more than one waypoint.

I made something called an RCAP2 that was pretty much the above, but added some additional ports in the hardware, boosted the mhz up, and used a PIC16F876a which was capable to be programmed using a bootloader through the serial port. Did some very minor tweaks in code, but as you can see from above, I missed something.

MX made a WPS way point sequencer that allowed the RCAP to be used with OEM gps's (like the em406) that did not require a goto gps. He also created waypoint.exe which is used to input the waypoints to his WPS. (I am also using his waypoint.exe to program the waypoints in my new PIC16f876a based autopilot.)

Eladio (I think) makes an RCAP3 that adapts nicely to MX's WPS (way point sequencer) with a simple connector, and is smaller than the rcap1 and rcap2. It does not use a Max232 device, so it goes well with the TTL level Oem gps's.
radiohound is offline Find More Posts by radiohound
Reply With Quote
Old Feb 10, 2009, 10:11 PM
Expert plane crasher
AXI Motor Models's Avatar
Central Coast, Australia
Joined May 2005
490 Posts
I built an RCAP+WPS and couldn't get it working. Maybe due to that error in the code. It would guide me to a random direction and would intermittently stop updating.

It will be good to have it all on one simple chip which is compatible with the EM-406a
AXI Motor Models is offline Find More Posts by AXI Motor Models
Reply With Quote
Old Feb 11, 2009, 09:32 AM
Scale Robotics Inc.
radiohound's Avatar
Gilroy, CA
Joined Feb 2004
180 Posts
Quote:
Originally Posted by Wmacky
$GPRMB,A,0.00,L,000,000,3721.3995,N,00607.6164,W,1 1.557,259.8,000.0,V*10
I don't really know whether this example is coming out of an em-406, or is what is being sent out of the WPS. I neither have a wps, nor an em-406. The following sentences are from a em-406 data log I got from the internet. The A character is the 13th character, just like my EB-85A, so it looks like my new code should read it just fine. This might have affected the way the WPS reads it. If the WPS doesn't reformat it into what the RCAP was expecting (its code isnt very forgiving), then you could get these errors. Interesting that it was intermittent for AXI Models.

$GPRMC,055550.982,A,3753.4224,N,12215.2577,W,0.63, 117.83,300808,,*18
$GPGGA,055551.982,3753.4192,N,12215.2493,W,1,03,5. 6,300.7,M,-24.9,M,,0000*6F
radiohound is offline Find More Posts by radiohound
Reply With Quote
Old Feb 11, 2009, 12:45 PM
7000mw of raw power!
rich smith's Avatar
New Hampshire (not the old one)
Joined Dec 2006
5,987 Posts
It is disappointing to see that 3 boards are still needed to make it work. Apparently there is some agreement between producers to keep separate products on the market. A shame because many of us would like to take advantage of the experience accumulated over the years on the RCAP project. This is why your mention of integrating WPS piqued my interest.

"this GPS don't follow exactly the NMEA standard" is a common lament but in fact it is the person who don't exactly follow the standard. Using references like 13th character is a big mistake. NMEA is a comma delineated format and out of 30 some different GPS models I have yet to see a single one which violated the standard. Here is em406 example:

$GPGGA,191910.000,4228.5451,N,07506.6224,W,1,08,0. 9,339.5,M,-33.9,M,,0000*69
$GPGSA,A,3,23,32,16,06,31,29,20,13,,,,,1.7,0.9,1.5 *31
$GPRMC,191910.000,A,4228.5451,N,07506.6224,W,0.30, 55.07,120708,,*27
$GPGGA,191911.000,4228.5451,N,07506.6225,W,1,08,0. 9,339.9,M,-33.9,M,,0000*65
$GPGSA,A,3,23,32,16,06,31,29,20,13,,,,,1.7,0.9,1.5 *31
$GPRMC,191911.000,A,4228.5451,N,07506.6225,W,0.06, 115.57,120708,,*12
$GPGGA,191912.000,4228.5452,N,07506.6225,W,1,08,0. 9,340.1,M,-33.9,M,,0000*63
$GPGSA,A,3,23,32,16,06,31,29,20,13,,,,,1.7,0.9,1.5 *31
$GPGSV,3,1,10,16,83,191,31,31,47,073,27,23,45,308, 34,20,29,245,25*7C
$GPGSV,3,2,10,32,27,219,23,06,23,159,22,29,17,044, 18,03,15,174,10*7C
$GPGSV,3,3,10,13,12,315,18,25,09,283,15*74
$GPRMC,191912.000,A,4228.5452,N,07506.6225,W,0.57, 75.56,120708,,*20






Quote:
Originally Posted by radiohound
I don't really know whether this example is coming out of an em-406, or is what is being sent out of the WPS. I neither have a wps, nor an em-406. The following sentences are from a em-406 data log I got from the internet. The A character is the 13th character, just like my EB-85A, so it looks like my new code should read it just fine. This might have affected the way the WPS reads it. If the WPS doesn't reformat it into what the RCAP was expecting (its code isnt very forgiving), then you could get these errors. Interesting that it was intermittent for AXI Models.

$GPRMC,055550.982,A,3753.4224,N,12215.2577,W,0.63, 117.83,300808,,*18
$GPGGA,055551.982,3753.4192,N,12215.2493,W,1,03,5. 6,300.7,M,-24.9,M,,0000*6F
rich smith is offline Find More Posts by rich smith
Reply With Quote
Reply


Thread Tools

Similar Threads
Category Thread Thread Starter Forum Replies Last Post
Navigation lights on Alfa ALFA 705 Submarines 16 Dec 11, 2005 05:42 AM
Location of Navigation Beacons on Bell 206??!! wannabe Scale Helicopters 1 Jun 16, 2005 08:15 AM
Navigation Lights on a Pitts Bi-Plane Screamn1 Scale Kit/Scratch Built 3 Jan 09, 2005 11:36 AM