Navigation calcs inside a Pic16F876a

7 posts in this topic

Posted (edited) · Report post

If anyone is interested, I have some code working for waypoint navigation inside a PIC chip (rather than relying on the gps for waypoint route calculation). It calculates distance and direction to the waypoint and steers a servo to direct your plane to that location.

To make it work on a Pic chip with PicBasic, I had to simplify the math involved. I wrote an explanation of the math if anyone is interested, here is the pdf file. http://www.uavs.net/Waypoint-Math.pdf . Here is the waypoint navigation code. http://www.uavs.net/18beta.zip . It is written for the RCAP2 or RCAP hardware. A description of the hardware is located here: http://en.wikibooks.org/wiki/Rcap

The zip file contains the PicBasic code for the project. I am working on an updated version of the code. I currently have code that has added accuracy, and is capable of performing waypoint navigation south of the equator. It should be released after some more test flights.

If anyone is wondering how fast this can be calculated with a 20 mhz Pic16F876A, the answer is over 1,000 times per second.

Edited by radiohound

Share this post


Link to post
Share on other sites

Posted · Report post

Many thanks for sharing the code.

Share this post


Link to post
Share on other sites

Posted · Report post

radio hound, you may want to also look at the Haversine formula or any of the great circle bearing calculations. The problem, I think, with the simple Pythagoras formula is that as you get to latitudes away from the equator the formula doesn't work as well. Just something to consider.

Daniel

Share this post


Link to post
Share on other sites

Posted (edited) · Report post

The program that I posted compensates for the changes in the length of longitude as your Latitude changes, in an admittedly crude way.... See page 2 of the pdf for the paragraph starting with "But wait!"

A quick look at the Haversine equations make it look a bit difficult to perform (at least for me) in PicBasic code. That was the reason I approximate the answer in the above code. Also the reason the little pic chip can compute it at 1,000 times per second.

Edited by radiohound

Share this post


Link to post
Share on other sites

Posted (edited) · Report post

After some testing of the navigation code I had (a more accurate version of 18beta). I found a couple things. It was pretty accurate (within about two or three degrees 95% of the time). But there were some areas that confused my approximation routine. Some of these areas could be off by as much as 18 degrees. This bugged me enough to buy a C assembler program, which can do higher math, but it turns out I don't think I need to use it.

I have found an application note that describes how to perform trig on a PIC16 device. It uses a cordic formula to determine the angle to a waypoint! This is from microchip, and located at: http://ww1.microchip.com/downloads/en/AppN...otes/01061A.pdf

I have been able to port it to assembly that PicBasic can handle (at least for the ATAN function, I am still working on getting the Cos(x) part of it working). This is a very cool function! It takes a value of x and y, and spits out the angle to the waypoint. To solve for longitude being different lengths at different latitudes, you use Cos(lat_current) * longitude difference (just like my division table attempted, but this time I will do it with higher precision). This will give you a certain percentage of the longitude difference. x= the new fraction of longitude difference (about 80% of its original value for 37 degrees north lat) and y = latitude difference between waypoints. So, just plug these into the assembly routine, and you get the angle to the location. You do need to tweak the angle depending on which quadrant the destination point lies, just like my earlier code did.

Anyway, the cordic function simplifies the way the pic determines the angle, and is performed very, very fast. I counted over 2000 times a second for waypoints located about 80 miles apart. And the code space is minimal. So I am going to integrate this new code with the RCAP to get results within a degree ... ahem ... for 100 percent of the time.

Edited by radiohound

Share this post


Link to post
Share on other sites

Posted · Report post

Hi RadioHound,

What Pic basic are you using? I used to use meLabs Pic Basic Pro, but got frustrated with their lack of development. I have just bought Proton Basic which is continually developed and handles 32bit floating point maths with built in trig functions like atan and cos. I am planning to use the spherical law of cosines to find the bearing to the destination waypoint.

Andy.

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!


Register a new account

Sign in

Already have an account? Sign in here.


Sign In Now