6DOF IMU ADXL345/ITG3200

Hello, I just registered to this forum, because I can’t find solution anywhere. So I got this https://www.sparkfun.com/products/10121 sensor. I am thinking to use it for my quadcopter project, but first I must have correct results of it. I was searching for code for about 4 hours, I got many of them, but many of them gave me bad results. So finally best code is this:

#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>

#include <Wire.h>
float conv = 0.00390625;
float angles[3]; // yaw pitch roll
float allValues[6];

// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();

void setup() { 
  Serial.begin(115200);
  Wire.begin();
  
  delay(5);
  sixDOF.init(); //begin the IMU
  delay(5);
}

void loop() { 
  
  sixDOF.getValues(allValues);
  Serial.print("Velocities ");
  Serial.print(allValues[0] * conv);
  Serial.print(" | ");  
  Serial.print(allValues[1] * conv);
  Serial.print(" | ");
  Serial.println(allValues[2] * conv);
  
  
  delay(100); 
}

But it doesn’t show me z axes,only x and y (this is arduino code).
When I use example for processing program, it shows me how it moves to x,y,z axes.

import processing.serial.*;

Serial myPort;  // Create object from Serial class

final String serialPort = "/dev/tty.usbmodem621"; // replace this with your serial port. On windows you will need something like "COM1".

float [] q = new float [4];
float [] hq = null;
float [] Euler = new float [3]; // psi, theta, phi

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

PFont font;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;


void setup() 
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  myPort = new Serial(this, serialPort, 115200);  

  font = createFont("Courier", 32); 
  

  /*
  float [] axis = new float[3];
   axis[0] = 0.0;
   axis[1] = 0.0;
   axis[2] = 1.0;
   float angle = PI/2.0;
   
   hq = quatAxisAngle(axis, angle);
   
   hq = new float[4];
   hq[0] = 0.0;
   hq[1] = 0.0;
   hq[2] = 0.0;
   hq[3] = 1.0;
   */

  delay(100);
  myPort.clear();
  myPort.write("1");
}


float decodeFloat(String inString) {
  byte [] inData = new byte[4];

  if (inString.length() == 8) {
    inData[0] = (byte) unhex(inString.substring(0, 2));
    inData[1] = (byte) unhex(inString.substring(2, 4));
    inData[2] = (byte) unhex(inString.substring(4, 6));
    inData[3] = (byte) unhex(inString.substring(6, 8));
  }

  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  return Float.intBitsToFloat(intbits);
}







void readQ() {
  if (myPort.available() >= 18) {
    String inputString = myPort.readStringUntil('\n');
    //print(inputString);
    if (inputString != null && inputString.length() > 0) {
      String [] inputStringArr = split(inputString, ",");
      if (inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
        q[0] = decodeFloat(inputStringArr[0]);
        q[1] = decodeFloat(inputStringArr[1]);
        q[2] = decodeFloat(inputStringArr[2]);
        q[3] = decodeFloat(inputStringArr[3]);
      }
    }
  }
}


void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);

  //Z+ (to the drawing area)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);

  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);

  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);

  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);

  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  endShape();
}


void drawCube() {  
  pushMatrix();
  translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
  scale(5, 5, 5);

  // a demonstration of the following is at 
  // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
  rotateZ(-Euler[2]);
  rotateX(-Euler[1]);
  rotateY(-Euler[0]);

  buildBoxShape();

  popMatrix();
}


void draw() {
  background(#000000);
  fill(#ffffff);

  readQ();

  if (hq != null) { // use home quaternion
    quaternionToEuler(quatProd(hq, q), Euler);
    text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
  }
  else {
    quaternionToEuler(q, Euler);
    text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
  }

  textFont(font, 20);
  textAlign(LEFT, TOP);
  text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
  text("Euler Angles:\nYaw (psi)  : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi)  : " + degrees(Euler[2]), 200, 20);

  drawCube();
}


void keyPressed() {
  if (key == 'h') {
    println("pressed h");

    // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
    hq = quatConjugate(q);
  }
  else if (key == 'n') {
    println("pressed n");
    hq = null;
  }
}

// See Sebastian O.H. Madwick report 
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation

void quaternionToEuler(float [] q, float [] euler) {
  euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}

float [] quatProd(float [] a, float [] b) {
  float [] q = new float[4];

  q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
  q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
  q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];

  return q;
}

// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
  float [] q = new float[4];

  float halfAngle = angle / 2.0;
  float sinHalfAngle = sin(halfAngle);
  q[0] = cos(halfAngle);
  q[1] = -axis[0] * sinHalfAngle;
  q[2] = -axis[1] * sinHalfAngle;
  q[3] = -axis[2] * sinHalfAngle;

  return q;
}

// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
  float [] conj = new float[4];

  conj[0] = quat[0];
  conj[1] = -quat[1];
  conj[2] = -quat[2];
  conj[3] = -quat[3];

  return conj;
}

What I should do to get correct results with arduino code?
Sorry for bad english

That sensor board is a little outdated and slightly over-expensive, 40 dollars for a simple accelerometer and gyro.
A MPU-6050 is acceleromter + gyro in a single chip and costs less than 3 dollars on Ebay inclusive shipping.
The MPU-9150 costs 9 dollars and is a MPU-6050 + magnetometer.

The 3 dollars for the MPU-6050 doesn't mean that it is less quality than the ITG-3200. The ITG-3200 (perhaps year 2008) is actually the predecessor of the MPU-6050 (year 2011). So the MPU-6050 is better.

For software, you can have a look at the i2cdevlib: http://www.i2cdevlib.com/
There is support for ITG-3200, ADXL345, MPU-6050, MPU-9150.

so perhaps you could recommend me some similar IMU, which would be best for quadcopter? with the same price ? and not outdated

The MPU-6050 or MPU-9150 use a single chip so they weigh less.
They use I2C (just as the ITG-3200 and ADXL345) which is slow. For a small quadcopter you might try a sensor with SPI interface.

You can start by using the i2cdevlib for the ITG-3200 and ADXL345 to see if you can make that library work. That makes it easier when you exchange the sensors with the the MPU-6050 or MPU-9150.
The i2cdevlib is able to boost the speed of the i2c communication.

As you can see here, the MPU-6050 with i2cdevlib is not a bad choice : http://www.instructables.com/id/3D-Printed-Arduino-Quadricopter/?ALLSTEPS

This project uses your IMU board : Low Cost Arduino Based Auto-Stabilizing System |

I am thinking to buy this:
http://www.ebay.com/itm/200949159205?ssPageName=STRK:MEWNX:IT&_trksid=p3984.m1497.l2649

That is a standard Ebay circuit board with a good chip.
While it is being shipped to you, you might check the i2cdevlib, it is not an easy library.
The code at Github has been removed a while ago, so I don't know what is going on. But you can use it as a MPU-6050.