Here are close-ups of the before and after desoldering:

As you can see desoldering something is really easy, and you get a really-really clean result with this tool.

There are few things to take care of though:

- do not touch the PCB traces for more than one second, this tool is hot and if you heat up the pads for too long it will suck them up
- buy several tip sizes for different jobs ,  i got  1mm (comes in the kit) and additionally I got  1.8mm and 2.3mm tips
- clean your tool regularly. The most replaced items are the ceramic filters so stock up on those, when they harden from the flux it's time to replace them.

Source: Hakko 808 Manual  (Download PDF)

Hakko 808 Kit comes with a carry-on case and few spare parts. What is missing is a holder, that you can buy separately (Hakko 633), however the tool will lay on it's side on the table without touching the surface so that will suffice if you don't have a holder. Another feature that is missing is a  an on/off switch, unplugging this from power outlet is not convenient especially if you have to crawl under the table with a hot tool on the table

Overall Hakko 808 is great tool , worth every penny, and I would buy it again if it breaks, speaking of which it is nicely build and has spare parts available so self-repair is possible and even encouraged and explained in the manual.

//starlino//

Hakko A1229: Hakko Replacement Pre-Filters for 808-5, 10/pkg.

Demo

Testing one axis:

Schematic

How it Works

The RC transmitter uses a potentiometer for each axis, it acts as a voltage divider sending a voltage of 0..5V (the middle position corresponds to 2.5V) to the analog input that is converted into a pule of  1..2ms that is sent over RF.

This module converts (amplifies and shifts) the accelerometer analog output , usually  1.65 +/- 0.4V to the same range of the potentiometer and sends it to the transmitter instead.

An op-amp in an inverting amplifier configuration is used. Vref is set manually by tuning the output to be 2.5V (or the PWM pulse to be 1.5 ms). However it is possible to calculate the theoretical value as follows:

Note that according to the rules of  a feedback op-amp the voltage on it's inverting/non-inverting terminals tends to equalize so   V(+) = V(-) and in our case  = Vref.

Since no significant current enters the op-amp , the currents going through R1 and R2 are equal:

( V(-) – Vin ) / R1 = ( Vout – V(-) ) / R2  

(Vref – Vin) / R1 = (Vout – Vref) / R2

solving for Vout gives us

Vout = Vref -  R2/R1 (Vin – Vref)    =   Vref( 1 + R2/R1)  – R2/R1 * Vin

now let's do some notations

G = – R2/R1

Vout = Vref( 1 – G)  -  G * Vin

According to our schematic  G =  R2 / R1 =  – 5.12 , this will convert the accelerometer swing of 0.4 V to a swing of   0.4 V * 5.12  ~  2V .

We want  to make   Vin = 1.65  correspond to a  Vout = 2.5 so we  have the equation

2.5 =  Vref (1 + 5.12)  – 5.12 * 1.65

from here we find

Vref =  (2.5 + 5.12 * 1.65 ) /  (1 + 5.12) = 1.78888 V

Well, this is the theoretical value , in practice we adjust the trimmer R3 until the output is 2.5 while the accelerometer is in laying in horizontal position (has an output of 1.65V).

How to Build

To build use a small proto-board following schematic. Part numbers are mentioned on schematic. Hook-up with the transmitter is described in images below and on the schematic. For accelerometer use Acc_Gyro or similar module, or build your own accelerometer break-out board.

The module is mounted in a free space under antenna using double-sided foam tape – best way to mount an accelerometer to avoid vibration. Note that we get +5V power for the module from the potentiometer contacts. You can test  with a led that the power contacts can deliver at least 20mV, the module uses far less <5mA.

Here is a close-up of the module, as you can see I did  my own accelerometer break-out board, but you can buy a pre-assembled one , there are many choices. You will need an analog accelerometer for this project.

Enjoy your new RC Tilt Transmitter. For any comments/questions use the comment form below.

//starlino//

The source code will be Open-Source and will be hosted here:

http://code.google.com/p/picquadcontroller/source/browse/#svn/trunk

As usual I like to start with a video demo, it's basically me controlling the tilt of the quad using a RC controller:

This is a PID feedback algorithm and a "Simplified Kalman Filter" as  described in my other articles.

First of all there's a a RCGroups thread where I first introduced the project

http://www.rcgroups.com/forums/showthread.php?t=1235360

You'll find some specs there and I will put some material here  as well.

Here is Revision 0.1 of schematic (what I started with):

PicQuadControllerRev01.pdf

The major parts list is as follows:

MCU:  DSPIC33FJ128MC802  (http://www.microchip.com/wwwproducts/Devices.aspx?dDocName=en532302)

Sensors: 5DOF Acc_Gyro ( http://gadgetgangster.com/213 )  + 1Dof Pololu LIST300AL-BREAKOUT (http://www.pololu.com/catalog/product/765).

Motor Drivers:  Power N-Channel Mosfets (Many choices -Low Rds(on) and rated for the current , I used  IRLR8743 ). Schottky diodes to protect against back-EMF ( I used SS2H10-E3/52T )

Frame , motors propellers: I picked a ready platform that was on sale Dragandfly IV Frame these are actually brushed motors same ones as used in Esky helicopters.

I did some tests on the lift force potential of this frame+motors+props, and was surprise to find out it performed better than some entry-level brushless motors. The lift force of this platform is up to 2.25 lbs.  The results of lift-force experiments are compiled in this spreadheet: MotorLiftPower.pdf.

How I measured the lift force ?  I didn't have any fancy equipment so I simply fixed the quad in a vise and weighted  it it at different throttle levels – the difference in weight gave me the lift power.

For testing just one motor, I used a string attached to the weight:

I built a test rig out of wood for testing the tilt balancing algorithm as you see in the video.

Here are some close-ups of the motor drivers. You'll  see the Mosfets and the Schottky diodes, also note the extra solder added to support the high currents that will flow through those traces ( up to 5A per motor !).

Finally here is a view on the top of the board ( notice the PIC , the switching regulator  ,  Acc_Gyro 5DOF IMU and single axes Gyro breakout , from top-left to bottom right).

This is it for now hope to bring you more interesting stuff on this and other projects if time allows  !

//starlino//

Demo:

About the project:

For the hardware I used the Acc_Gyro sensor and a thumb-size PIC platform UsbThumb built around the inexpensive PIC18F14K50 chip that provides all the necessary USB connectivity and ADC inputs.

This setup is actually hardware compatible with my original Motion Gamepad design so all the software utilities and firmware will work on the Acc_Gyro + UsbThumb combination that is available as a kit under the name of UsbThumbImu . The Acc_Gyro fits on top of UsbThumb and this is not a coincidence since UsbThumb was designed as microcontroller helper for the Acc_Gyro with the idea of being able to provide the USB/Serial/SPI/I2C interface, as well as making use of the built-in 10bit ADC module of the PIC18F14K50.

In the pictures above headers are used to connect the device, so the end result is a little taller  than you could get if you'd simply solder the two boards together. Both devices have been designed for people that would like to extend  or experiment with the existing solution so there is a second row of connectors on the UsbThumb that can be used to connect buttons or other devices to the project. 

All that's it for now – hope you liked the project. It was really easy to built having the two important components UsbThumb and Acc_Gyro. You'll find more info on individual components on my website as well as  on the GadgetGangster site with whom I partnered to provide kits for anyone interested. If you don't find the necessary information, or you have any any suggestions, ideas or questions – as always feel free to drop me a line with in the comment area below.

//starlino//

It is basically a customization of Mouse emulation demo that Microchip bundles with their USB Framework.

The code responsible for enabling/disabling mouse emulation is triggered by a timer interrupt like so:

        OpenTimer0( TIMER_INT_ON &
                 T0_16BIT              &
                 T0_SOURCE_INT         &
                T0_PS_1_256);

                     ………….

        //These are your actual interrupt handling routines.

        #pragma interrupt YourHighPriorityISRCode
        void YourHighPriorityISRCode()
        {
                //Check which interrupt flag caused the interrupt.
                //Service the interrupt
                //Clear the interrupt flag
                //Etc.
        #if defined(USB_INTERRUPT)
                USBDeviceTasks();
        #endif
       
                if(INTCONbits.TMR0IF){
                        counter_s++;
                        //to interrupt every 1s we need to count FOSC / 4/ 256  = 46875 ticks (0xB71B)
                        //to count these ticks till overflow we need to set TMR0 to  0xFFFF – 0xB71B = 0x48E4
                       
                        TMR0H = 0×48; //will write to TMR0H buffer
                        TMR0L = 0xE4; //will write actual TMR0L / TMR0H 

                        INTCONbits.TMR0IF = 0;
                }

        }       //This return will be a "retfie fast", since this is in a #pragma interrupt section

       ……………..

       INTCONbits.TMR0IE = 0;  //disable Timer0 interrupt
        if(emulate_mode && counter_s > 3){      // seconds on
                counter_s = 0;
                emulate_mode = 0;
        }else if (!emulate_mode && counter_s > 60){     //seconds off
                counter_s = 0;
                emulate_mode = 1;
        }
        INTCONbits.TMR0IE = 1;

For full source code see:

http://code.google.com/p/usbthumb/source/browse/#svn/trunk/UsbThumbMousePrank

UsbThumb is an open-hardware platform, anyone can build their own:

http://code.google.com/p/usbthumb/source/browse/#svn/trunk/Hardware

or pick up a prebuilt SMT version of UsbThumb here: http://www.gadgetgangster.com/303

In order to upload a new firmware to USBThumb you need to enter the bootloader mode. This is done by connecting the  VPP pin to ground during the time when device is plugged into the USB port.

This can achieved by placing a jumper wire or a 1K resistor (recommended) between GND and VPP pin.

GND pin is number 1, and VPP is number 5. Make sure you do not confuse VPP with the nearby VDD(number 4) or you will get a short !!! To be safe use a 1K resistor instead of the wire. Wire/resistor can be held by the spring action, it only needs to make contact during a split second while the device is plugged. If you plan on making frequent firmware updates , soldering a header is recommended.

Before plugging the device to USB port,  start the "USB HID Bootloader" application (it is installed as part of the Microchip USB Framework, look in C:\Microchip Solutions\USB Device – Bootloaders\HID – Bootloader), a copy is placed here for download:

HIDBootLoader.zip

Next insert the USBThumb into the USB port (with the VPP and GND pins connected) , you should see a "Device attached." message in the USB HID Bootloader window.

Next steps are simple:

1. Click on “Open Hex File” button and select the new firmware .HEX file you want to load
2. Click on “Program/Verify” and wait for the process to complete
3. Unplug the device from USB port. Remove the jumber wire/resistor between GND and VPP pin.
4. Plug the device back and verify the new firmware is working (it depends on firmware and what you expect it to do).

The pictures below illustrate the above steps:

A custom PIC firmware  comes preloaded on the
"USBThumb Propeller Programmer" available on GadgetGangster.com, it allows you to use USBThumb as a Propeller Programmer but also as USB to Serial converter.

USBThumb (loaded with USBThumbSerial firmware)  is detected on your computer as a standard  CDC Modem device and works on all modern operating systems (Windows, Mac os , Linux).

On Windows you might be asked for driver path once you attach the device, simply use the driver part of the Microchip's USB Framework (it can be found in C:\Microchip Solutions\USB Device – CDC – Serial Emulator\inf\win2k_winxp ) , and I am including a copy for direct download here:

microchip_cdc_win2k_winxp.zip

On Windows you can identify USBThumb's COM port number from Device Manager:

On Mac OS, once you attach the device  you will get a prompt "A new network interface has been detected". Look in the /dev folder. You should see it as "/dev/tty.usbmodem411" or similar.

PropellerTool can scan all available ports and find the one that has a propeller chip attached. So the above step might not be necessary, however if you already know the COM port of USBThumb you can specify it explicitly.

In the PropellerTool go to  Edit > Preferences>Operation and update the "Serial Port Search" to the port number of USBThumb (COM15 in my example). In the "Propeller Reset Signal" choose DTR.

Next attach the USBThumb to your Propeller development board, USBThumb's serial  4-pin connector is pin compatible with PropPlug. The pins are marked as follows RX,TX,RST,GND.

To test the communication with the propeller board press F7.

If you experience problems make sure the port is not open by any other software (like for example the propeller terminal software)

In some rare cases if you computer power supply is too noisy, to ensure greater stability you can place a 18pF filtering capacitor between pins 24 (RB5)  and 25 (RB6). See picture below:

You can solder the capacitor permanently between these pins, or if you're planning to use USBThumb for other projects and you have soldered sockets to the board simply insert the capacitor in the socket between pins 24 and 25. You only need to take this step if you have intermittent problems with PropellerTool not detecting the Propeller chip.

The setup of BusPirate was simple. Simply plug it to your USB port, install the FDTI driver (if necessary). After that it will appear as a virtual COM port on you computer. You then can use your terminal of choice to interact with it. Here is sample session in Termite (my choice of terminal software):

You will have to figure out which port your BusPirate is on. If you can't figure it out, unplug it , observe list of ports, then plug it back to see which new port appeared – that's the one. List of COM ports can be found in Device Manager on Windows or in Termite Settings menu. Other COM settings are as follows:

Using the probe cable accessory, I did all connections on a breadboard guided by the schematic from MMA7456L datasheet . You will need just 4 Bus Pirate probes connected to the accelerometer. Below I am showing which ones and where you should connect them:

On the breadboard this looks something like this (I also have the oscilloscope probes connected to CLK and MOSI):

First thing to note – I used some shrink-wrap to isolate the Bus Pirate, but you can also put it in a case.

Second thing to note is that my accelerometer was mounted on a breakout board I made myself, I have a detailed tutorial how you can do SMT mounting and etch your own boards in my article "DIY Surface Mount on a Budget – Complete Walkthrough from PCB etching to Reflow" , check it out

Third thing – don't forget the the pull-up resistors (10K) on each bus line. It is required to pull-up the SDA and SCL lines (see resistors R1 and R2 on schematic). You can also try the BP pullup feature instead of external resistors.  The BusPirate on-board 10K pull-up resistors can be connected to the bus pins, by connecting  Vpu pin to a volate pin (3.3V), and then entering the p command to turn the pull-up feature on.

Finally, I used just one decoupling capacitor 0.1uF (the schematic shows 4 caps), also I did not bother to connect the N/C pins to ground.

However for stability you should ground the IADDR0 pin (3), on schematic they show a switch that allows a position where you can change the default address of the device. We won't need the switch for this experiment.

I later discovered that while using the BusPirate internal pull-up feature (Vpu connected to 3.3V),  after I disconnect the oscilloscope probes the setup was no longer working!!! It must be the noise on the bus – I said to myself. Oscilloscope probes have 18-22pf capacitors that actually eliminate some noise. I encountered this phenomenon many times, so I knew what is going on. I simply added two 18pF capacitors between SDA/SCL pins and ground and I was back in business. The noise is coming most of all from my computer power supply , and while working at 100Khz it becomes an issue.

Ok, so once done with the hardware part, moving on to the BusPirate interface. First send a reset command and check your firmware/hardware version. A command is sent in bus pirate by typing the command # followed by Enter. Now take note of the version I am using , if you're reading this article in a year or two there might be other versions available, so if something doesn't make sense , it might be because your have a different version of BusPirate.

I2C>#
#
RESET

Bus Pirate v3
Firmware v4.1 Bootloader v4.1
DEVID:0×0447 REVID:0×3043 (B5)
http://dangerousprototypes.com
HiZ>

For a complete list of commands you can type ?, I won't cover all commands in this article I'll just explain the ones we use. By default BP is in HiZ mode. HiZ means "High Impedance". Z is a common symbol for impedance – simply a fancy way to say that all probes are disconnected. To enter I2C mode simply type m and choose the I2C option ( 4 – in my version of BP). You will be asked other details about I2C setup, I chose 100Khz speed. Once you enter a mode other than HiZ the green MODE led should turn on on your BusPirate.

HiZ>m
m
1. HiZ
2. 1-WIRE
3. UART
4. I2C
5. SPI
6. JTAG
7. RAW2WIRE
8. RAW3WIRE
9. PC KEYBOARD
10. LCD
(1) >4
4
Mode selected
Set speed:
1. ~50KHz
2. ~100KHz
3. ~400KHz
(1) >2
2
READY

Next step – you need to turn power supplies on – you do this by typing W (must be capital W !!! lower w turns them off). Once this is done, the VREG led will turn on, indicating you have power on your 3.3 and/or 5V pins. We'll just use the 3.3V pin to power our project and leave the 5V pin alone, however you could selectively turn the later off.

I2C>W
W
POWER SUPPLIES ON

Now it's time to interact with our accelerometer, BP has a set of macros for each mode that you can check with the (0) command, the (1) macro happens to be a macro that scans all I2C address on the bus and displays the ones present. Each I2C slave device will have a 7bit address ($1D for MMA7456L) that comes in two flavors read(R) and write (W). To obtain the 8bit read/write address you shift the 7bit address to the left (same as multiply by 2) and set the 0th bit to 0 for write, and to 1 for read.

I2C>(1)
(1)
Searching 7bit I2C address space.
Found devices at:
0x3A(0x1D W) 0x3B(0x1D R)

You can verify that 0x1D * 2 + 0 = 0x3A , 0x1D * 2 +1 = 0x3B.

Now it's time to read something from the device. The MMA7456L configuration is divided into 1 byte registers that are described in the datasheet, for testing purposes let's read the register $0D that should contain the device I2C address (0x1D):

Let's send the following command and look at the output:

I2C>{0x3A 0x0D {0x3B r}
{0x3A 0x0D {0x3B r}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0x0D ACK
I2C START BIT
WRITE: 0x3B ACK
READ: 0x1D NACK
I2C STOP BIT

Now let me explain the BusPirate commands:

{ – sends a I2C start condition (ST)

} – send a I2C stop condition (SP)

0x3A 0x0D 0x3B are simply hex numbers representing bytes sent over the bus

r – reads one byte

There's no need to send the NAK signal, BusPirate will do it for you and also it will detect and display any AK(ACK) signals received from the slave (the accelerometer).

If this looks like magic to you right now , the following diagram will clarify things, it shows how I used the datasheet in order to construct a BusPirate command that would read one register. The register I am reading is $0D , which happens to be the device address, it should return $1D which is the I2C address for MMA7456L.

Now let's talk about writing a byte on the I2C bus.

Before getting acceleration data , we need to put MMA7456L into measurement mode, we also need to specify the sensibility for which there are 3 options 2g/4g/8g. This configuration is done through the Mode Control Register $16 (get the details from MMA7456L datasheet , page 9). For our settings we need to send the binary value of : 0b00000101 which sets mode to "measurement", and g-select to 2g. Let's do it:

I2C>{0x3A 0×16 0b00000101}
{0x3A 0×16 0b00000101}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0×16 ACK
WRITE: 0×05 ACK
I2C STOP BIT

Writing a register is even simpler than reading and I will show you how the BusPirate write command relates to the diagram from the MMA7456L datasheet:

Now that we've mastered reading/writing to an I2C bus , we're ready to read acceleration data. We have the option of reading 8-bit signed values from registers $06,$07,$08. These registers have a 64count/g resolution in 2g mode, meaning that an acceleration of 1 g will be represented as 64 (0×40) and  -1g will be represented as -64 (0xBF). We can read 3 consecutive registers from one command line as follows:

I2C>{0x3A 0×06 {0x3B r:3}
{0x3A 0×06 {0x3B r:3}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0×06 ACK
I2C START BIT
WRITE: 0x3B ACK
READ 0×03 BYTES:
0xFF ACK 0xF8 ACK 0x4A NACK
I2C STOP BIT

So we got following accelerometer output values for X,Y,Z axes: 0xFF (-1) , 0xF8 (-8) , 0x4A (74) which divided by sensitivity of 64counts per g, give us the values in Gs ~ -0.015g , -0.125g , 1.16g . It is close to what is expected from the output when the device is in horizontal position. The Z output seems to be off. We'll double check this and deal with it later.

Let's tilt the device on the X axis and see if it makes any difference in output, I tilted it roughly 45 degrees so that X and Z axis modulus should be roughly close to 64/2 = 32 and Y should remain close to 0.

I2C>{0x3A 0×06 {0x3B r:3}
{0x3A 0×06 {0x3B r:3}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0×06 ACK
I2C START BIT
WRITE: 0x3B ACK
READ 0×03 BYTES:
0xD7 ACK 0xF8 ACK 0x3F NACK
I2C STOP BIT

The results we got this time are X = 0xD7 (-41), Y = 0xF8 (-8), Z = 0x3F(63 !!!), now we can see clear that Z axis reading is offset. Luckily MMA7456L offers a way to correct these values and we'll use it. From first experiment we can see it needs to be offset by 74 – 64 ~ 10 counts. We can correct the accelerometer drift (for each axis). For Z axis we'll be using 2 registers:

"Offset drift Z value (LSB)" – register $14
"Offset drift Z value (MSB)" – register $15.

But what values should we write ? Let's start with the fact that our drift is about 10 counts, but this is in 2g scale. Scale in which drift must be specified is apparently 4g , so we need to offset Z by 10*4g/2g = 20 counts (in 4g scale) . Also our correction must be in different direction so the value to write is -20. Expressed as a signed DWORD this is 0xFFEC.

We'll write 2 registeres at once LSB (least significant byte) first.

I2C>{0x3A 0×14 0xEC 0xFF}
{0x3A 0×14 0xEC 0xFF}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0×14 ACK
WRITE: 0xEC ACK
WRITE: 0xFF ACK
I2C STOP BIT

Now let's check the readings in horizontal position again.

I2C>{0x3A 0×06 {0x3B r:3}
{0x3A 0×06 {0x3B r:3}
I2C START BIT
WRITE: 0x3A ACK
WRITE: 0×06 ACK
I2C START BIT
WRITE: 0x3B ACK
READ 0×03 BYTES:
0xFB ACK 0xF6 ACK 0×41 NACK
I2C STOP BIT

And here you go – the value for Z axis has been corrected to 0×41 (65) , close to what's expected ( 64).

In Conclusion:

This real life example of exploring and troubleshooting a chip with I2C interface shows what a valuable tool Bus Pirate is. Rather than bore you with a lot of theory and specs I tried to walk you through an example scenario I encountered myself. I hope that I interested you enough in Bus Pirate , accelerometers and digital interfaces in general to do your own further research of this topic.

//starlino//

The Acc_Gyro is mounted on a regular proto-shield on top of an Arduino Duemilanove board.

Parts needed to complete the project:

- Arduino Duemilanove (or similar Arduino platform)
- Acc_Gyro 5DOF IMU board
- Protoshield (optional)
- Breadboard
- Hook-up wire 22AWG

The hook-up diagram is as follows:

Acc_Gyro <—>  Arduino
5V          <—>  5V 
GND      <—>  GND
AX       <—>  AN0
AY       <—>  AN1
AZ       <—>  AN2
GX4      <—>  AN3 
GY4      <—>  AN4  

Once you have completed the hardware part, load the following sketch to your Arduino.

imu_arduino.zip

Run the project and make sure you are receiving an output on your serial terminal (you can start the terminal from your Arduino IDE).

To analyze the data I have developed a small utility called SerialChart. It is open-source so feel free to customize it for your own needs.

Here is the output from SerialChart software:

The test was performed as follows:

- first I was tilting the board slowly (marked "smooth tilting" on the screenshot)

- next I continued tilting the board, but I also started applying some vibration – by tapping the board quickly with my finger (marked "Titlting with vibration noise")

As you can see from the chart the filtered  signal (red line)  is indeed more immune to noise than the accelerometer readings alone (blue line). The filtered signal was obtained by combining the Accelerometer and Gyroscope data. Gyroscope data is important, because if you would simply average the Accelerometer data you would get a delayed signal. Given the simplicity of the code and of the algorithm I am satisfied with the results. One feature that I would like to add is compensation for the drift effect that you might encounter with some gyroscopes. However the Acc_Gyro board proved to be very stable in this respect, since it has built-in high pass filters.

If you'd like to experiment on your own, I recommended first reproducing this testing setup ,  then shift slowly towards your application needs. For example you may take the C code and port it to PIC's C18/C30 or AVR-GCC (it shouldn't be too dificult).

Below are some useful resources and their descriptions.

SerialChart executables can be downloaded from here:

SerialChart_01.zip

Once you start SerialChart application you will need to load the imu_arduino.scc configuration file for this project(included in the imu_arduino.zip) archive.

In this configuration file make sure to update the 'port' settings to Arduino's COM port. On my computer Arduino was detected on COM3, on yours it might be different.

For more information on configuration file syntax see:

http://code.google.com/p/serialchart/wiki/ConfigurationFileSyntax

You can also download and compile SerialChart from Google Code:

http://code.google.com/p/serialchart/source/browse/#svn/trunk

You will need a SVN client to checkout the code (I use RapidSVN for Windows).

SerialChart was developed using  Qt SDK from Nokia: http://qt.nokia.com/downloads

UPDATE 2010-03-18

Many people ask me what about the other 2 axis, here is the code that outputs 3 axis, including the SerialChart configuration script.

Imu_Arduino_3axis_output_2010-03-18.zip

I also removed some overhead code that Alex pointed out in the comments, this reduced the interval between samples.

In the example below I rotate the board around the X axis(blue) which is parallel to the ground.I do it by hand so X is not exactly 0, but close. The axes that change are Y(red) and Z(green).  Please note the relationship X^2+Y^2+Z^2 = 1. The dashed  cyan, magenta and lime lines are unfiltered signals coming from accelerometer alone (RwAcc).

//starlino//

This guide is intended to everyone interested in inertial MEMS (Micro-Electro-Mechanical Systems) sensors, in particular Accelerometers and Gyroscopes as well as combination IMU devices (Inertial Measurement Unit).

I'll try try to cover few basic but important topics in this article:

- what does an accelerometer measure
- what does a gyroscope (aka gyro) measure
- how to convert analog-to-digital (ADC) readings that you get from these sensor to physical units (those would be g for accelerometer, deg/s for gyroscope)
- how to combine accelerometer and gyroscope readings in order to obtain accurate information about the inclination of your device relative to the ground plane

Throughout the article I will try to keep the math to the minimum. If you know what Sine/Cosine/Tangent are then you should be able to understand and use these ideas in your project no matter what platform you're using Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC, etc. There are people out there who believe that you need complex math in order to make use of an IMU unit (complex FIR or IIR filters such as Kalman filters, Parks-McClellan filters, etc). You can research all those and achieve wonderful but complex results. My way of explaining things require just basic math. I am a great believer in simplicity. I think a system that is simple is easier to control and monitor, besides many embedded devices do not have the power and resources to implement complex algorithms requiring matrix calculations.

I'll use as an example a new IMU unit that GadgetGangster just launched – the Acc_Gyro Accelerometer + Gyro IMU. We'll use parameters of this device in our examples below. This unit is a good device to start with because it consists of 2 devices:

- LIS331AL (data sheet) – a triaxial 2G accelerometer
- LPR550AL (data sheet) – a dual-axis pitch and roll, 500 deg/sec gyroscope

Together they represent a 5-Degrees of Freedom Inertial Measurement Unit. Now that's a fancy name! Nevertheless, behind the fancy name is a very useful combination device that we'll cover and explain in detail below.

Part 1. Accelerometer

To understand this unit we'll start with the accelerometer. When thinking about accelerometers it is often useful to image a box in shape of a cube with a ball inside it. You may imagine something else like a cookie or a donut , but I'll imagine a ball:

If we take this box in a place with no gravitation fields or for that matter with no other fields that might affect the ball's position – the ball will simply float in the middle of the box. You can imagine the box is in outer-space far-far away from any cosmic bodies, or if such a place is hard to find imagine at least a space craft orbiting around the planet where everything is in weightless state . From the picture above you can see that we assign to each axis a pair of walls (we removed the wall Y+ so we can look inside the box). Imagine that each wall is pressure sensitive. If we move suddenly the box to the left (we accelerate it with acceleration 1g = 9.8m/s^2), the ball will hit the wall X-. We then measure the pressure force that the ball applies to the wall and output a value of -1g on the X axis.

Please note that the accelerometer will actually detect a force that is directed in the opposite direction from the acceleration vector. This force is often called Inertial Force or Fictitious Force . One thing you should learn from this is that an accelerometer measures acceleration indirectly through a force that is applied to one of it's walls (according to our model, it might be a spring or something else in real life accelerometers). This force can be caused by the acceleration , but as we'll see in the next example it is not always caused by acceleration.

If we take our model and put it on Earth the ball will fall on the Z- wall and will apply a force of 1g on the bottom wall, as shown in the picture below:

In this case the box isn't moving but we still get a reading of -1g on the Z axis. The pressure that the ball has applied on the wall was caused by a gravitation force. In theory it could be a different type of force – for example, if you imagine that our ball is metallic, placing a magnet next to the box could move the ball so it hits another wall. This was said just to prove that in essence accelerometer measures force not acceleration. It just happens that acceleration causes an inertial force that is captured by the force detection mechanism of the accelerometer.

While this model is not exactly how a MEMS sensor is constructed it is often useful in solving accelerometer related problems. There are actually similar sensors that have metallic balls inside, they are called tilt switches, however they are more primitive and usually they can only tell if the device is inclined within some range or not, not the extent of inclination.

So far we have analyzed the accelerometer output on a single axis and this is all you'll get with a single axis accelerometers. The real value of triaxial accelerometers comes from the fact that they can detect inertial forces on all three axes. Let's go back to our box model, and let's rotate the box 45 degrees to the right. The ball will touch 2 walls now: Z- and X- as shown in the picture below:

The values of 0.71 are not arbitrary, they are actually an approximation for SQRT(1/2). This will become more clear as we introduce our next model for the accelerometer.

In the previous model we have fixed the gravitation force and rotated our imaginary box. In last 2 examples we have analyzed the output in 2 different box positions, while the force vector remained constant. While this was useful in understanding how the accelerometer interacts with outside forces, it is more practical to perform calculations if we fix the coordinate system to the axes of the accelerometer and imagine that the force vector rotates around us.

Please have a look at the model above, I preserved the colors of the axes so you can make a mental transition from the previous model to the new one. Just imagine that each axis in the new model is perpendicular to the respective faces of the box in the previous model. The vector R is the force vector that the accelerometer is measuring (it could be either the gravitation force or the inertial force from the examples above or a combination of both). Rx, Ry, Rz are projection of the R vector on the X,Y,Z axes. Please notice the following relation:

R^2 = Rx^2 + Ry^2 + Rz^2     (Eq. 1)

which is basically the equivalent of the Pythagorean theorem in 3D.

Remember that a little bit earlier I told you that the values of SQRT(1/2) ~ 0.71 are not random. If you plug them in the formula above, after recalling that our gravitation force was 1 g we can verify that:

1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2

simply by substituting R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2) in Eq.1

After a long preamble of theory we're getting closer to real life accelerometers. The values Rx, Ry, Rz are actually linearly related to the values that your real-life accelerometer will output and that you can use for performing various calculations.

Before we get there let's talk a little about the way accelerometers will deliver this information to us. Most accelerometers will fall in two categories: digital and analog. Digital accelerometers will give you information using a serial protocol like I2C , SPI or USART, while analog accelerometers will output a voltage level within a predefined range that you have to convert to a digital value using an ADC (analog to digital converter) module. I will not go into much detail about how ADC works, partly because it is such an extensive topic and partly because it is different from one platform to another. Some microcontroller will have a built-in ADC modules some of them will need external components in order to perform the ADC conversions. No matter what type of ADC module you use you'll end up with a value in a certain range. For example a 10-bit ADC module will output a value in the range of 0..1023, note that 1023 = 2^10 -1. A 12-bit ADC module will output a value in the range of 0..4095, note that 4095 = 2^12-1.

Let's move on by considering a simple example, suppose our 10bit ADC module gave us the following values for the three accelerometer channels (axes):

AdcRx = 586
AdcRy = 630
AdcRz = 561

Each ADC module will have a reference voltage, let's assume in our example it is 3.3V. To convert a 10bit adc value to voltage we use the following formula:

VoltsRx = AdcRx * Vref / 1023

A quick note here: that for 8bit ADC the last divider would be 255 = 2 ^ 8 -1 , and for 12bit ADC last divider would be 4095 = 2^12 -1.

Applying this formula to all 3 channels we get:

VoltsRx = 586 * 3.3V / 1023 =~ 1.89V (we round all results to 2 decimal points)
VoltsRy = 630 * 3.3V / 1023 =~ 2.03V
VoltsRz = 561 * 3.3V / 1023 =~ 1.81V

Each accelerometer has a zero-g voltage level, you can find it in specs, this is the voltage that corresponds to 0g. To get a signed voltage value we need to calculate the shift from this level. Let's say our 0g voltage level is VzeroG = 1.65V. We calculate the voltage shifts from zero-g voltage as follows::

DeltaVoltsRx = 1.89V – 1.65V = 0.24V
DeltaVoltsRy = 2.03V – 1.65V = 0.38V
DeltaVoltsRz = 1.81V – 1.65V = 0.16V

We now have our accelerometer readings in Volts , it's still not in g (9.8 m/s^2), to do the final conversion we apply the accelerometer sensitivity, usually expressed in mV/g. Lets say our Sensitivity = 478.5mV/g = 0.4785V/g. Sensitivity values can be found in accelerometer specifications. To get the final force values expressed in g we use the following formula:

Rx = DeltaVoltsRx / Sensitivity

Rx = 0.24V / 0.4785V/g =~ 0.5g
Ry = 0.38V / 0.4785V/g =~ 0.79g
Rz = 0.16V / 0.4785V/g =~ 0.33g

We could of course combine all steps in one formula, but I went through all the steps to make it clear how you go from ADC readings to a force vector component expressed in g.

Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity (Eq.2)
Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

We now have all 3 components that define our inertial force vector, if the device is not subject to other forces other than gravitation, we can assume this is the direction of our gravitation force vector. If you want to calculate inclination of device relative to the ground you can calculate the angle between this vector and Z axis. If you are also interested in per-axis direction of inclination you can split this result into 2 components: inclination on the X and Y axis that can be calculated as the angle between gravitation vector and X / Y axes. Calculating these angles is more simple than you might think, now that we have calculated the values for Rx,Ry and Rz. Let's go back to our last accelerometer model and do some additional notations:

The angles that we are interested in are the angles between X,Y,Z axes and the force vector R. We'll define these angles as Axr, Ayr, Azr. You can notice from the right-angle triangle formed by R and Rx that:

cos(Axr) = Rx / R , and similarly :
cos(Ayr) = Ry / R
cos(Azr) = Rz / R

We can deduct from Eq.1 that R = SQRT( Rx^2 + Ry^2 + Rz^2).

We can find now our angles by using arccos() function (the inverse cos() function ):

Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)

We've gone a long way to explain the accelerometer model, just to come up to these formulas. Depending on your applications you might want to use any intermediate formulas that we have derived. We'll also introduce the gyroscope model soon, and we'll see how accelerometer and gyroscope data can be combined to provide even more accurate inclination estimations.

But before we do that let's do some more useful notations:

cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R

This triplet is often called Direction Cosine , and it basically represents the unit vector (vector with length 1) that has same direction as our R vector. You can easily verify that:

SQRT(cosX^2 + cosY^2 + cosZ^2) = 1

This is a nice property since it absolve us from monitoring the modulus(length) of R vector. Often times if we're just interested in direction of our inertial vector, it makes sense to normalize it's modulus in order to simplify other calculations.

Part 2. Gyroscope

We're not going to introduce any equivalent box model for the gyroscope like we did for accelerometer, instead we're going to jump straight to the second accelerometer model and we'll show what does the gyroscope measure according to this model.

Each gyroscope channel measures the rotation around one of the axes. For instance a 2-axes gyroscope will measure the rotation around (or some may say "about") the X and Y axes. To express this rotation in numbers let's do some notations. First let's define:

Rxz – is the projection of the inertial force vector R on the XZ plane
Ryz – is the projection of the inertial force vector R on the YZ plane

From the right-angle triangle formed by Rxz and Rz, using Pythagorean theorem we get:

Rxz^2 = Rx^2 + Rz^2 , and similarly:
Ryz^2 = Ry^2 + Rz^2

also note that:

R^2 = Rxz^2 + Ry^2 , this can be derived from Eq.1 and above equations, or it can be derived from right-angle triangle formed by R and Ryz
R^2 = Ryz^2 + Rx^2

We're not going to use these formulas in this article but it is useful to note the relation between all the values in our model.

Instead we're going to define the angle between the Z axis and Rxz, Ryz vectors as follows:

Axz – is the angle between the Rxz (projection of R on XZ plane) and Z axis
Ayz – is the angle between the Ryz (projection of R on YZ plane) and Z axis

Now we're getting closer to what the gyroscope measures. Gyroscope measures the rate of changes of the angles defined above. In other words it will output a value that is linearly related to the rate of change of these angles. To explain this let's assume that we have measured the rotation angle around axis Y (that would be Axz angle) at time t0, and we define it as Axz0, next we measured this angle at a later time t1 and it was Axz1. The rate of change will be calculated as follows:

RateAxz = (Axz1 – Axz0) / (t1 – t0).

If we express Axz in degrees, and time in seconds , then this value will be expressed in deg/s . This is what a gyroscope measures.

In practice a gyroscope(unless it is a special digital gyroscope) will rarely give you a value expressed in deg/s. Same as for accelerometer you'll get an ADC value that you'll need to convert to deg/s using a formula similar to Eq. 2 that we have defined for accelerometer. Let's introduce the ADC to deg/s conversion formula for gyroscope (we assume we're using a 10bit ADC module , for 8bit ADC replace 1023 with 255, for 12bit ADC replace 1023 with 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity Eq.3
RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

AdcGyroXZ, AdcGyroYZ – are obtained from our adc module and they represent the channels that measure the rotation of projection of R vector in XZ respectively in YZ planes, which is the equivalent to saying rotation was done around Y and X axes respectively.

Vref – is the ADC reference voltage we'll use 3.3V in the example below

VzeroRate – is the zero-rate voltage, in other words the voltage that the gyroscope outputs when it is not subject to any rotation, for the Acc_Gyro board it is for example 1.23V (you can find this values in the specs)

Sensitivity – is the sensitivity of your gyroscope it is expressed in mV / (deg / s) often written as mV/deg/s , it basically tells you how many mV will the gyroscope output increase , if you increase the rotation speed by one deg/s. The sensitivity of Acc_Gyro board is for example 2mV/deg/s or 0.002V/deg/s

Let's take an example, suppose our ADC module returned following values:

AdcGyroXZ = 571
AdcGyroXZ = 323

Using the above formula, and using the specs parameters of Acc_Gyro board we'll get:

RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s
RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s

In other words the device rotates around the Y axis (or we can say it rotates in XZ plane) with a speed of 306 deg/s and around the X axis (or we can say it rotates in YZ plane) with a speed of -94 deg/s. Please note that the negative sign means that the device rotates in the opposite direction from the conventional positive direction. By convention one direction of rotation is positive. A good gyroscope specification sheet will show you which direction is positive, otherwise you'll have to find it by experimenting with the device and noting which direction of rotation results in increasing voltage on the output pin. This is best done using an oscilloscope since as soon as you stop the rotation the voltage will drop back to the zero-rate level. If you're using a multimeter you'd have to maintain a constant rotation rate for at least few seconds and note the voltage during this rotation, then compare it with the zero-rate voltage. If it is greater than the zero-rate voltage it means that direction of rotation is positive.

Part 3. Putting it all together. Combining accelerometer and gyroscope data.

If you're reading this article you probably acquired or are planning to acquire a IMU device, or probably you're planning to build one from separate accelerometer and gyroscope devices.

The first step in using a combination IMU device that combines an accelerometer and a gyroscope is to align their coordinate systems. The easiest way to do it is to choose the coordinate system of accelerometer as your reference coordinate system. Most accelerometer data sheets will display the direction of X,Y,Z axes relative to the image of the physical chip or device. For example here are the directions of X,Y,Z axes as shown in specifications for the Acc_Gyro board:

Next steps are:

- identify the gyroscope outputs that correspond to RateAxz , RateAyz values discussed above.
- determine if these outputs need to be inverted due to physical position of gyroscope relative to the accelerometer

Do not assume that if a gyroscope has an output marked X or Y, it will correspond to any axis in the accelerometer coordinate system, even if this output is part of an IMU unit. The best way is to test it.

Here is a sample sequence to determine which output of gyroscope corresponds to RateAxz value discussed above.

- start from placing the device in horizontal position. Both X and Y outputs of accelerometer would output the zero-g voltage (for example for Acc_Gyro board this is 1.65V)
- next start rotating the device around the Y axis, another way to say it is that you rotate the device in XZ plane, so that X and Z accelerometer outputs change and Y output remains constant.
- while rotating the device at a constant speed note which gyroscope output changes, the other gyroscope outputs should remain constant
- the gyroscope output that changed during the rotation around Y axis (rotation in XZ plane) will provide the input value for AdcGyroXZ, from which we calculate RateAxz
- the final step is to ensure the rotation direction corresponds to our model, in some cases you may have to invert the RateAxz value due to physical position of gyroscope relative to the accelerometer
- perform again the above test, rotating the device around the Y axis, this time monitor the X output of accelerometer (AdcRx in our model). If AdcRx grows (the first 90 degrees of rotation from horizontal position), then AdcGyroXZ should decrease. This is due to the fact that we are monitoring the gravitation vector and when device rotates in one direction the vector will rotate in oposite direction (relative to the device coordonate system, which we are using). So, otherwise you need to invert RateAxz , you can achieve this by introducing a sign factor in Eq.3, as follows:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity , where InvertAxz is 1 or -1

same test can be done for RateAyz , by rotating the device around the X axis, and you can identify which gyroscope output corresponds to RateAyz, and if it needs to be inverted. Once you have the value for InvertAyz, you should use the following formula to calculate RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

If you would do these tests on Acc_Gyro board you would get following results:

- the output pin for RateAxz is GX4 and InvertAxz = 1
- the output pin for RateAyz is GY4 and InvertAyz = 1

From this point on we'll consider that you have setup your IMU in such a way that you can calculate correct values for Axr, Ayr, Azr (as defined Part 1. Accelerometer) and RateAxz, RateAyz (as defined in Part 2. Gyroscope). Next we'll analyze the relations between these values that turn out useful in obtaining more accurate estimation of the inclination of the device relative to the ground plane.

You might be asking yourself by this point, if accelerometer model already gave us inclination angles of Axr,Ayr,Azr why would we want to bother with the gyroscope data ? The answer is simple: accelerometer data can't always be trusted 100%. There are several reason, remember that accelerometer measures inertial force, such a force can be caused by gravitation (and ideally only by gravitation), but it might also be caused by acceleration (movement) of the device. As a result even if accelerometer is in a relatively stable state, it is still very sensitive to vibration and mechanical noise in general. This is the main reason why most IMU systems use a gyroscope to smooth out any accelerometer errors. But how is this done ? And is the gyroscope free from noise ?

The gyroscope is not free from noise however because it measures rotation it is less sensitive to linear mechanical movements, the type of noise that accelerometer suffers from, however gyroscopes have other types of problems like for example drift (not coming back to zero-rate value when rotation stops). Nevertheless by averaging data that comes from accelerometer and gyroscope we can obtain a relatively better estimate of current device inclination than we would obtain by using the accelerometer data alone.

In the next steps I will introduce an algorithm that was inspired by some ideas used in Kalman filter, however it is by far more simple and easier to implement on embedded devices. Before that let's see first what we want our algorithm to calculate. Well , it is the direction of gravitation force vector R = [Rx,Ry,Rz] from which we can derive other values like Axr,Ayr,Azr or cosX,cosY,cosZ that will give us an idea about the inclination of our device relative to the ground plane, we discuss the relation between these values in Part 1. One might say – don't we already have these values Rx, Ry , Rz from Eq.2 in Part 1 ? Well yes, but remember that these values are derived from accelerometer data only, so if you would be to use them directly in your application you might get more noise than your application can tolerate. To avoid further confusion let's re-define the accelerometer measurements as follows:

Racc – is the inertial force vector as measured by accelerometer, that consists of following components (projections on X,Y,Z axes):

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity
RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

So far we have a set of measured values that we can obtain purely from accelerometer ADC values. We'll call this set of data a "vector" and we'll use the following notation.

Racc = [RxAcc,RyAcc,RzAcc]

Because these components of Racc can be obtained from accelerometer data , we can consider it an input to our algorithm.

Please note that because Racc measures the gravitation force you'll be correct if you assume that the length of this vector defined as follows is equal or close to 1g.

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),

However to be sure it makes sense to update this vector as follows:

Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].

This will ensure the length of your normalized Racc vector is always 1.

Next we'll introduce a new vector and we'll call it

Rest = [RxEst,RyEst,RzEst]

This will be the output of our algorithm , these are corrected values based on gyroscope data and based on past estimated data.

Here is what our algorithm will do:
- accelerometer tells us: "You are now at position Racc"
- we say "Thank you, but let me check",
- then correct this information with gyroscope data as well as with past Rest data and we output a new estimated vector Rest.
- we consider Rest to be our "best bet" as to the current position of the device.

Let's see how we can make it work.

We'll start our sequence by trusting our accelerometer and assigning:

Rest(0) = Racc(0)

By the way remember Rest and Racc are vectors , so the above equation is just a simple way to write 3 sets of equations, and avoid repetition:

RxEst(0) = RxAcc(0)
RyEst(0) = RyAcc(0)
RzEst(0) = RzAcc(0)

Next we'll do regular measurements at equal time intervals of T seconds, and we'll obtain new measurements that we'll define as Racc(1), Racc(2) , Racc(3) and so on. We'll also issue new estimates at each time intervals Rest(1), Rest(2), Rest(3) and so on.

Suppose we're at step n. We have two known sets of values that we'd like to use:

Rest(n-1) – our previous estimate, with Rest(0) = Racc(0)
Racc(n) – our current accelerometer measurement

Before we can calculate Rest(n) , let's introduce a new measured value, that we can obtain from our gyroscope and a previous estimate.

We'll call it Rgyro , and it is also a vector consisting of 3 components:

Rgyro = [RxGyro,RyGyro,RzGyro]

We'll calculate this vector one component at a time. We'll start with RxGyro.

Let's start by observing the following relation in our gyroscope model, from the right-angle triangle formed by Rz and Rxz we can derive that:

tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)

Atan2 might be a function you never used before, it is similar to atan, except it returns values in range of (-PI,PI) as opposed to (-PI/2,PI/2) as returned by atan, and it takes 2 arguments instead of one. It allows us to convert the two values of Rx,Rz to angles in the full range of 360 degrees (-PI to PI). You can read more about atan2 here.

So knowing RxEst(n-1) , and RzEst(n-1) we can find:

Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).

Remember that gyroscope measures the rate of change of the Axz angle. So we can estimate the new angle Axz(n) as follows:

Axz(n) = Axz(n-1) + RateAxz(n) * T

Remember that RateAxz can be obtained from our gyroscope ADC readings. A more precise formula can use an average rotation rate calculated as follows:

RateAxzAvg = ( RateAxz(n) + RateAxz(n-1) ) / 2
Axz(n) = Axz(n-1) + RateAxzAvg * T

The same way we can find:

Ayz(n) = Ayz(n-1) + RateAyz(n) * T

Ok so now we have Axz(n) and Ayz(n). Where do we go from here to deduct RxGyro/RyGyro ? From Eq. 1 we can write the length of vector Rgyro as follows:

|Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

Also because we normalized our Racc vector, we may assume that it's length is 1 and it hasn't changed after the rotation, so it is relatively safe to write:

|Rgyro| = 1

Let's adopt a temporary shorter notation for the calculations below:

x =RxGyro , y=RyGyro, z=RzGyro

Using the relations above we can write:

x = x / 1 = x / SQRT(x^2+y^2+z^2)

Let's divide numerator and denominator of fraction by SQRT(x^2 + z^2)

x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )

Note that x / SQRT(x^2 + z^2) = sin(Axz), so:

x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )

Now multiply numerator and denominator of fraction inside SQRT by z^2

x = sin(Axz) / SQRT (1 + y^2  * z ^2 / (z^2 * (x^2 + z^2)) )

Note that z / SQRT(x^2 + z^2) = cos(Axz) and y / z = tan(Ayz), so finally:

x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )

Going back to our notation we get:

RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )

same way we find that

RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )

Now, finally we can find:

RzGyro  =  Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).

Where Sign(RzGyro) = 1 when RzGyro>=0 , and Sign(RzGyro) = -1 when RzGyro<0.

One simple way to estimate this is to take:

Sign(RzGyro) = Sign(RzEst(n-1))

In practice be careful when RzEst(n-1) is close to 0. You may skip the gyro phase altogether in this case and assign:  Rgyro = Rest(n-1). Rz is used as a reference for calculating Axz and Ayz angles and when it's close to 0, values may overflow and trigger bad results. You'll be in domain of large floating point numbers where tan() / atan() function implementations may lack precision.

So let's recap what we have so far, we are at step n of our algorithm and we have calculated the following values:

Racc – current readings from our accelerometer
Rgyro – obtained from Rest(n-1) and current gyroscope readings

Which values do we use to calculate the updated estimate Rest(n) ? You probably guessed that we'll use both. We'll use a weighted average, so that:

Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)

We can simplify this formula by dividing both numerator and denominator of the fraction by w1.

Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)

and after substituting w2/w1 = wGyro we get:

Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)

In the above formula wGyro tells us how much we trust our gyro compared to our accelerometer. This value can be chosen experimentally usually values between 5..20 will trigger good results.

The main difference of this algorithm from Kalman filter is that this weight is relatively fixed , whereas in Kalman filter the weights are permanently updated based on the measured noise of the accelerometer readings. Kalman filter is focused at giving you "the best" theoretical results, whereas this algorithm can give you results "good enough" for your practical application. You can implement an algorithm that adjusts wGyro depending on some noise factors that you measure, but fixed values will work well for most applications.

We are one step away from getting our updated estimated values:

RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)

Now let's  normalize this vector again:

R = SQRT(RxEst(n) ^2 + RyEst(n)^2 +  RzEst(n)^2 )

RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R
RzEst(n) = RzEst(n)/R

And we're ready to repeat our loop again.

Other Resources on Accelerometer and Gyroscope IMU Fusion:

http://www.mikroquad.com/pub/Research/ComplementaryFilter/filter.pdf

http://starlino.com/imu_kalman_arduino.html

http://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data

http://www.dimensionengineering.com/accelerometers.htm

//starlino//