Propeller Autopilot

**Note: I have not had time to fill out all of the steps yet, however, I will finish after I am done with work today, sorry for the premature post, I want to get this in before the project deadline**
Hi All!
I am VERY excited to bring this instructable to you! This project has been in the works for a while now, and it is finally time to share the work the I have done!
In this instructable I am going to explain how I built my first powered UAV.
The goal for the project was have the following:
    ** Ability to switch between autopilot control and radio control
    ** Accurate orientation information
    ** Ability to hold a set orientation 
    ** Functional Altitude Heading Reference System (AHRS) to navigate between GPS coordinates
    ** Live communication between the aircraft and laptop for in-flight trimming
    ** Live visual representation of the airplanes orientation and gps location
    ** Autonomous take-offs

We did not meet all of these goals, partly due to the fact that I had to go on an unanticipated business trip to the Netherlands. We were able to get everything working EXCEPT:
    **Waypoint Navigation (I know this is the most important feature... )
    **Autonomous take-offs
    **Live plot of GPS location 

This is a great project if your goal is to build a platform for you to experiment with different control methods. While there is a lot of formal theory available regarding unmanned aircraft, it is fun to test out your own ideas and see if any of them work. If you never try out your own ideas you will always end up using somebody else's.
I have also done everything I can to make sure THIS PROJECT IS 100% OPEN SOURCE. It seems like school is taking forever to get through, and the open-source community has done a lot to support my "extra-curricular" education and I am so thankful!
Projects like these are an interesting amalgamation of mathematics, physics, computers, and model building; and draws upon hardware, firmware, and software design. And because it spans across so many disciplines, it is important to get help when you need it.! 
When I started down this road about a year ago, I had just about zero experience in any of these fields. It has been a very gratifying experience putting my all my extra brain power into this project and then chucking it off the side of a cliff : P

A few more comments before we embark:

I have easily sunk enough money into this project to buy more then a few APM boards from 3DRobotics. If you're goal is to quickly and cheaply get a model-sized UAV into the air, buy a pre-made flight controller and plug it into one your existing aircraft. While 3DRobotics has a great community and lots of support, there are actually a few options, here are a couple more:

http://www.cloudcaptech.com/piccolo_nano.shtm
http://transition-robotics.com/products/lisa-m-v2-0
https://pixhawk.ethz.ch/px4/modules/px4fmu
http://www.buildyourowndrone.co.uk/AttoPilot-s/64.htm

That being said, quite a few people on DIYDrones have really helped me pull this project through, and it seems like a great place to start the hobby.

I am very excited that 3DRobotics is hosting a drones competition! Me and my buddy have been meaning to mount an arm-based computer onto a quadcopter for some time now, with the intent of trying some 3D-Modeling by using cloud computing from 123D Catch http://www.123dapp.com/catch#catchApps
Although creating the model require 40 + pictures, it would be an interesting, introduction to modeling large objects with minimal hardware/software.

Throughout this instructable I will try to create sequential instructions on how to build a UAS using my design. I would love to simply host a drawing of my board for you to etch/solder accompanied with a little git-hub zip for you to compile on your computer and microcontroller. But the truth of the matter is my code has been calibrated specifically for my airplane and my IMU. So I will do my level best to illuminate my full understanding of the model-theory, control-theory, and controller functionality so if you UAV crashes, you will have the tools necessary to troubleshoot.  

Everyone gets "hung up" at different points through a project. I will go into as much detail as possible to make sure my work is easy to build off of. I am sorry if it moves too slow, please skip ahead if that is the case. If you are stuck for a significant amount of time on any one aspect of this kind of project, please feel free to message me, or post to the relevant forums (Arduino, Parallax, DIYDrones etc). There is a lot of information available from a variety of sources.
If you intend to code your own UAV from scratch, patience is key. Nothing every quite works as I pre-cognate, so I always have to take my time sifting through.

Please check out my instructable from last summer if you have not done so already!
Although I have made significant strides since last summer (much more sophisticated IMU processing, faster calculations, no more silly relays) it might be worth while to give this a once over. I have had no formal schooling on the subject matter, so if I can do it, you can too!
https://www.instructables.com/id/Intro-to-Model-Airplane-Autopilot/

Look at all the extra space inside that hatch!

This mostly depends on what you are comfortable developing with, and what you're developing with. As a bare minimum, you're micro-controller must have :
    ** PWM/PCM for controlling hobby servos
    ** A way of measuring pulse widths for reading the rc receiver (generally accomplished with interrupts)
    ** I2C acquiring IMU data
    ** SPI for SD Card read/write
    ** Serial for GPS and Xbee communication
    ** Float math & matrix math for directional cosine computation

There are really a lot of choices here. This might be a good time to shop around and see whats new. Here is a little article from hackaday (2011)  reviewing some popular development boards:
http://hackaday.com/2011/02/01/what-development-board-to-use/

Obviously, arduino has very well developed libraries for all of these packages. However, for this project I ended up using a parallax propeller chip. If you have never used a propeller chip before, here is the parallax website:
http://www.parallax.com/tabid/407/Default.aspx

The propeller chip runs 5x faster than the arduino (80 Mhz vs 16 Mhz) and essentially consists of 8 32 bit microcontrollers, while the arduino only has 1 8 bit microcontroller (albeit, with awesome C++ libraries and community support). The propeller chip lets you dedicate 8 individual "cogs" to specific functions, but in exchange you cannot use interrupts. The extra speed and processing power gives you a large "fudge factor" if you do not get your algorithm exactly right. 
Surprisingly, a propeller quickstart board costs about the same as an arduino uno.

I recently purchased an STM32F3 discovery board off of digikey for $10. The discovery board includes a 72 Mhz arm-core microcontroller, acceleromter, gyroscope, and magnetometer. This could make development a lot easier because the chip is developed in good old C/C++, which allows the use of 2D arrays (handy if you are going to use a rotation matrix), and is generally more universal. The catch is that the software development suite is commercial software (aka non-free). There are some workarounds available, especially if your are linux proficient. The board can be found here: http://www.digikey.com/product-detail/en/STM32F3DISCOVERY/497-13192-ND/3522185 

Once you've picked out your controller you need to pick up your inertial measurement unit (IMU). This consists of a 3-axis accelerometer, gyroscope, and magnetometer. The most important thing here is to pick something compatible with your controller. All mems (micro-electro-mechanical-systems) are analog devices, and require different digital interfaces to communicate with your controller (typically I2C or SPI). If you have never written an I2C driver before, now is a good time to learn. Otherwise try to find some example code online that uses your chipset.

I am personally fond of a pololu brand IMU :
http://www.pololu.com/catalog/product/1268
Lots of online support/examples. In addition, somebody has already written an I2C driver for this chipset in assembly. This makes it a lot easier to read IMU data VERY quickly. Here is the github with the propeller assembly driver (pasm)
https://github.com/tdeyle/PropellerIMU

However, i recently found some dirt-cheap boards on amazon :
http://www.amazon.com/Attitude-Indicator-L3G4200D-ADXL345-HMC5883L/dp/B00CD239UG/ref=sr_1_cc_1?s=aps&ie=UTF8&qid=1370604682&sr=1-1-catcorr&keywords=sunkee+dof

http://www.amazon.com/L3G4200D-ADXL345-HMC5883L-BMP085-Breakout-/dp/B008G7T3QQ/ref=sr_1_2?s=electronics&ie=UTF8&qid=1370605789&sr=1-2&keywords=dof
This chipset seems pretty common for this application, and the propeller object exchange (OBEX) already has example code for all of these chips. I have modified the example codes so that we can include them as a library for our final spin file. Its takes a little under .02 seconds to read and store values from the entire chipset. This is OK because we can dedicate the process on its own cog, but it is still a little slow. I am working on using one of the I2C assembly drivers from the OBEX to speed up the process, but I had to stop working on that in order to do this write up :)

The airframe you chose should be tailored to the functionality of your UAS. Because stabilit

Starting with a fresh IDE screen is always a little intimidating. Especially when the goal is to be able to write some code like : 
Set_Waypoint(longitude,latitude,altitude)
In reality, there are still lots of bytes that must be moved between registers, and lots of timers that need to be set precisely. This is my first time working with such "low-level" code, but understanding how these things work is really gratifying, and will give you a lot more confidence in the long run.
At its most basic level, our autopilot system is essentially a servo switch, that decides whether or not to write servo values from the radio receiver (sent by you via transmitter) or from the autopilot calculations. Whether or not the plane is in autopilot will be decided from the state of a toggle switch from the transmitter. The state of this switch will be sent over one of the radios normal channels (ie landing gear or flaps). 
Calculating autopilot outputs starts with sensor readings. My project accomplishes this using the I2C bus. Each sensor serves a very specific purpose. Your accelerometer will let you measure the net force on your airplane minus the acceleration due to gravity. Assuming that gravity is the greatest force on your airplane, you can easily write a vector <accel_x,accel_y,accel_z> to find the pitch and roll of your airplane. Your magnetometer will tell you the net magnetic force on your airplane, in Gauss, <mag_x,mag_y,mag_z> This information can be used to find your planes heading relative to the earths magnetic field. While in theory this should be enough information to fly, both of these sensors are very noise-prone. The accelerometer will become essentially meanlingless once your motors are running (and thus undergoing lots of linear-accelerations) and your magnetometer will drift as you throttle up and down (from changes made to the magnetic field induced by your motors). 
Luckily, your gyroscope is here to save the day. It is immune to linear accelerations (vibrations) and to changes in magnetic field. The gyroscope will tell you your angular velocity about the x,y, and z axis. If you integrate this value in the correct manner, you should be able to find how much the airplanes roll, pitch, and yaw have changed between calcuations.  You cannot, however, solely rely on the gyroscope because
1)The gyroscope will never read a zero even if you put it flat on the table
2) You accumulate errors as you integrate because you cannot solve for you integration constant (C)
If you do not know calculus, do not fear, we will walk you through the paces.
It also possible to use your GPS as source of drift-free yaw data if the refresh rate and accuracy of the GPS is good enough.

You combine all of this information to create an accurate oreintation of the aircraft (pitch,roll,yaw). With these values you can create a PID loop that SHOULD hold the airplane in a desired orientation.
Once you can control the attitude of the airplane, you need to calculate what attitude will get your aircraft to the next waypoint. This can be accomplished by comparing the GPS coordinates of the airplanes location to the airplanes desired location. The GPS will provide the bearing for the aircraft (the direction it should be facing) while the compass/gyroscope will provide the heading (the way the aircraft is actually facing). Althoo

I will post an actual drawing of the board I designed for this project when I return from the Netherlands on the 10th. For now I will just explain how it acheives functionality.
The board is powered from the airplanes Li-Po battery. The power from the battery is split between the esc and the board. A couple of voltage reducer step it down from 12.5v to 5v and then 3.3v. 
The board has to be able to read the radio receiver and write servo positions. The servos will share a 5v and Ground line with the receiver. The ground must also be attached to the parallax chip ground.
A wire must be attached from the parallax IO to each of the receiver channels and to each of the servos. 

I know that when it comes to microcontrollers, the majority of this forum is dedicated to the arduino. Even if the arduino is not your preferred microcontroller, it always deserves a nod-of-the-head for its accessibility, and comprehensive library development.
Regardless, this project has been developed with the propeller chip, which is most easily written in Spin.
This is a good time to note Parallax's  good-will towards the open source community. The propeller chip is  now completely programmable in C/C++ thanks to the creation SImpleIDE. The majority of this project was written in SimpleIDE due to its ease of install into Linux, and for accesiblity to comunication ports. 

https://sites.google.com/site/propellergcc/documentation/simpleide

While I'd prefer for to code this project in C++ (for use of 2D arrays, and for a shallower learning curve) all example code I have written for this project will be in Spin. This has been done in order to maximize functionality before the contest deadline. 
Spin: Pass by Value
Passing variables by value in Spin is very similar to c++. The only major difference is that when you declare a local variable in Spin, you do not name a type, (ie byte, word,long). This is misleading because it makes it seem like Spin has dynamic memory allocation; for all I was able to tell, this is no the case.

Spin : Pass by Reference
Passing variables in Spin is relatively easy. It is different from C++ however, in that you do not pass a pointer, you simply pass the memory address by value, and dereference the address within the function. I used this method of passing parameters extensively while developing various vector math function. In the following example, we fill every variable in both arrays with the number 1. Memory locations are retrieved with the “@” symbol, and memory locations are dereferenced by redeclaring the variable type and putting the variable in “[]”:

Spin: Float Math
Float math in Spin is a giant pain. I found four different libraries for using float math in spin, they use different numbers of cogs and run at difference speeds, and are pretty self-explanatory. One thing worth noting: If you are declare a constant or variable to a specific value directly in your code, and you intend to use floating point operations, you must make sure you put a decimal point at the end of every number.

Spin: Special Registers
There are quite a few special registers in Spin. I will run through a few of them here, but if you are really serious about using the propeller chip, you should DEFINITELY read the application release notes, as well as download a copy of the manual to your computer. Most of these registers will be used in the code examples on the next step.

DIRA: This register determines which IOs on the propeller chip are outputs and which are inputs. You can write to these registers using binary, decimal, or hex. The most intuitive way, however, is binary. Each bit in the register represents one of the propeller 32 IOs. Assigning a 0 or a 1 to that bit will assign that IO as an input or output. 

CTRA/CTRB: These two registers set the counter modes used by each timer. We use two different counter depending on whether we are measuring a pulse length, or writing one.

PHSA/PHSB: These registers are where the accumulated values generated by our counters are actually stored. On its most basic mode, the number of elapsed clock cycles are stored here.

FRQA/FRQB: These registers contain a factor that scales the counter every time it stores to PHSA/B.

If all of this talk about timers and counter modes is unfamiliar to you, you should definitely read the Although it might not be entirely be necessary, I would highly recommend reading through all of the propeller counter modes. Using the counter properly will give you incredible control over low-level IO functionality, and will allow you fudge your way through almost all digital communication protocols:
http://www.parallaxsemiconductor.com/an001

This function is used to write values to our servos. If you are not familiar with how to set a servo’s position, it is really quite simple. The data line of the servo is written high for a millisecond or two every 20 milliseconds. A pulse length of 1 millisecond sets the servo to its minimum throw. A pulse length of 2 milliseconds will set the servo to its maximum position.
My code does not calculate servo position from angle, but rather in terms of clock cycles. The propeller chip functions at 80Mhz; this means that in 1 second, the propeller chip goes through 80 million clock cycles. This values is stored in the variable clkfreq. You can easily calculate the pulse length/time ellapsed in terms of seconds by taking the number of clock cycles, and dividing by the clock frequency.

One of the bigger issues I had during this project was trying to make sense of the magnetometer data. It took me a long time to calibrate correctly; I was so exasperated with the process that I ended up writing some python/arduino code to do all future calibrations automatically.
Include hard/soft iron calibration pdf
You can download my python code here, and arduino code here

       This step was written by my partner Jack Reedy. Thanks for the help Jack!! ::


Unfortunately we had to hold off on getting the drone to fly to a designated longitude and latitude or waypoint. Although we are only about a month or less away from having it completed. On my first attempt to read the GPS unit on the parallax I was able to store everything successfully into multiple arrays of bytes. However it proved more difficult to get the arrays into a float variable, so I hit a roadblock. Even though I could access all of the information I needed I could not manipulate properly to use float math functions. The GPS unit gave us the information byte by byte so we could not find a way to store our separate array’s into individual float variables.

PUB readGPS | u,i,v,w,q,j,e,f,g,h,k,d,dec, final, cntr, MN_AT[1], timestamp[5], datastatus[1], rmcE_W[1], rmcN_S[1], rmclong[10], rmclat[10], spd[5], mode[2], lat[10], N_S[1], E_W[1], alti[10], longi[10], quality[2], number[2], indicator[10], time[10], address, begin, finish, char
          ** this is where we have defined all of the variables we will need for the readGPS function, plan to store values in the arrays of information we will need
repeat
    repeat
      i := UARTS.rx(GPS)
      GPS_IN[v] := i
     if w == 0
        indicator[q] := GPS_IN[v]
        'UARTS.tx(PC, indicator[q])                      
        q ++
 
          **I created two repeat loops the first one is to set the counters back to zero also every time there is a comma another counter goes up to keep track of the information we wish to access in the NMEA string. The next repeat goes until there is a comma so the w counter goes up. Otherwise the next repeat loop uses the w counter to recognize its spot along the NMEA string and grab the information storing it to a specific variable. For example w == 0 so we are storing the indicator, this way I can use a simple if then statement to realize which NMEA string the computer is reading (look below).

     if indicator[2] == "G" and indicator[3] == "G"
        if w == 1
          time[j] := GPS_IN[v]                    
          'UARTS.tx(PC, time[j])
          j ++
        if w == 2                                               
          lat[u] := GPS_IN[v]
          'UARTS.tx(PC, lat[u])
          u ++
        if w == 3
           N_S[g] := GPS_IN[v]
           'UARTS.tx(PC, N_S[g])
           g ++
…….
        if w == 7
          number[h] := GPS_IN[v]
          'UARTS.tx(PC, number[h])
          h ++
        if w == 9
          alti[k] := GPS_IN[v]
          'UARTS.tx(PC, alti[k])
          k ++
     if indicator[3] == "S" and indicator[4] == "A"                                                
        if w == 1
          MN_AT[j] := GPS_IN[v]
          'UARTS.tx(PC, MN_AT[j])
          j ++
        if w == 2                                               
          mode[u] := GPS_IN[v]
          'UARTS.tx(PC, mode[u])
          u ++
     if GPS_IN[v] == ","
        w ++
     if GPS_IN[v] == "$"
         q := w := j := u := g := e := f := h := d := 0
     v ++
    until i == ","

Unfortunately I was not able to get the array to a float value so I had to hold off on this approach for now. We decided to use a GPS float library that will do exactly what I was trying to do except they found a way to do this with some assembly. So it appears I am not well enough versed in the spin language to get the array’s stored in floats. They used hex decimals and some very specific code that I have only seen for spin language. Regardless using the new library I am able to access all of the required information as float variables so I was then able to make a Bearing function where we will give our waypoint in longitude and latitude. Using a mathematical formula to find bearing I can use the GPS float math variables of longitude and latitude to calculate our bearing. The next step will be to get the drone to change its course based on what the bearing and the heading reads. Unfortunately we had to wrap up the project for the competition due to and unexpected leave to the Netherlands a week before the competition is over. Give us a few more weeks and we should have a complete ability to have the drone head towards specific waypoints.

PUB Bearing (lon, lat)| dlat, dlon, blon, blat, x, y, a,b,c, bear
blon := -121.8576
blat := 37.6967
dlon := fm.fsub(lon, blon)
dlat := fm.fsub(lat, blat)
a := fm.fmul(fm.cos(lat), fm.sin(blat))
b := fm.fmul(fm.fmul(fm.sin(lat), fm.cos(blat)), fm.cos(dlon))
y := fm.fmul(fm.sin(dlon), fm.cos(blat))
x := fm.fsub(a,b)
bear := fm.degrees(fm.atan2(y,x))

          **Blon is our ending longitude and our blat is our ending latitude to use for our bearing. Dlon and dlat are our difference in longitude and latitude from our heading and our waypoints longitude and latitude. Otherwise we just use a mathematical formula with our float math library and convert it all to degrees to get our bearing.