Jump to content


Photo

Navigation calcs inside a Pic16F876a


  • Please log in to reply
6 replies to this topic

#1 radiohound

radiohound

    RC-Cam Visitor

  • Members+
  • Pip
  • 16 posts

Posted 06 November 2007 - 09:43 AM

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, 06 November 2007 - 09:58 AM.


#2 Mr.RC-Cam

Mr.RC-Cam

    RC-Cam Mentor

  • Admin
  • PipPipPipPipPip
  • 4,702 posts
  • Location:USA
  • Interests:R/C video and photography.

Posted 06 November 2007 - 10:21 AM

Many thanks for sharing the code.
- Thomas

#3 Daniel Wee

Daniel Wee

    RC-Cam Mentor

  • Members+
  • PipPipPipPipPip
  • 380 posts
  • Gender:Male
  • Location:Singapore

Posted 06 November 2007 - 10:36 AM

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

#4 radiohound

radiohound

    RC-Cam Visitor

  • Members+
  • Pip
  • 16 posts

Posted 06 November 2007 - 10:54 AM

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, 06 November 2007 - 11:05 AM.


#5 radiohound

radiohound

    RC-Cam Visitor

  • Members+
  • Pip
  • 16 posts

Posted 16 November 2007 - 12:27 PM

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...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, 17 November 2007 - 08:38 AM.


#6 Mr.RC-Cam

Mr.RC-Cam

    RC-Cam Mentor

  • Admin
  • PipPipPipPipPip
  • 4,702 posts
  • Location:USA
  • Interests:R/C video and photography.

Posted 16 November 2007 - 12:50 PM

Thanks for the update. Here is the corrected link: http://ww1.microchip...otes/01061A.pdf
- Thomas

#7 Peaps

Peaps

    RC-Cam Visitor

  • Members+
  • Pip
  • 11 posts
  • Gender:Male
  • Location:UK

Posted 03 March 2008 - 07:45 AM

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.