/* IMU Fusion Board - ADXL345 & IMU3000
Example Arduino Sketch to read the Gyro and Accelerometer Data
Written by www.hobbytronics.co.uk 
See the latest version at www.hobbytronics.co.uk/arduino-adxl345-imu3000
08-Apr-2011
*/
#define GYRO 0x68 // gyro I2C address
#define REG_GYRO_X 0x1D // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL 0x53 // Accel I2c Address
#define ADXL345_POWER_CTL 0x2D
byte buffer[12]; // Array to store ADC values 
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int i;
#include <Wire.h>
float m_inT = 0.0f;
float m_gx = 0.0f;
void setup()
{
Serial.begin(115200);
Wire.begin();
// Set Gyro settings
// Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s 
writeTo(GYRO, 0x16, 0x0B); 
//set accel register data address
writeTo(GYRO, 0x18, 0x32); 
// set accel i2c slave address
writeTo(GYRO, 0x14, ACCEL);
// Set passthrough mode to Accel so we can turn it on
writeTo(GYRO, 0x3D, 0x08); 
// set accel power control to 'measure'
writeTo(ACCEL, ADXL345_POWER_CTL, 8); 
//cancel pass through to accel, gyro will now read accel for us 
writeTo(GYRO, 0x3D, 0x28);
}
// Write a value to address register on device
void writeTo(int device, byte address, byte val) {
Wire.beginTransmission(device); // start transmission to device 
Wire.send(address); // send register address
Wire.send(val); // send value to write
Wire.endTransmission(); // end transmission
}
void loop()
{
// Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
// First set the register start address for X on Gyro 
Wire.beginTransmission(GYRO);
Wire.send(REG_GYRO_X); //Register Address GYRO_XOUT_H
Wire.endTransmission();
// New read the 12 data bytes
Wire.beginTransmission(GYRO);
Wire.requestFrom(GYRO,12); // Read 12 bytes
i = 0;
while(Wire.available())
{
buffer[i] = Wire.receive();
i++;
}
Wire.endTransmission();
//Combine bytes into integers
// Gyro format is MSB first
gyro_x = buffer[0] << 8 | buffer[1];
gyro_y = buffer[2] << 8 | buffer[3];
gyro_z = buffer[4] << 8 | buffer[5];
// Accel is LSB first. Also because of orientation of chips
// accel y output is in same orientation as gyro x
// and accel x is gyro -y
accel_y = buffer[7] << 8 | buffer[6];
accel_x = buffer[9] << 8 | buffer[8];
accel_z = buffer[11] << 8 | buffer[10];
double norm = sqrt(sq((float)accel_x) + sq((float)accel_y) + sq((float)accel_z));
float accelRoll = -(((degrees( acos( (float)accel_x / norm)) - 90.0f)));
m_gx = m_gx * 0.9f + 0.1f * (float)gyro_x;
// Print out what we have
Serial.print(m_inT ); // echo the number received to screen 
Serial.print(","); 
Serial.print(accelRoll); // echo the number received to screen 
Serial.print(gyro_x); // echo the number received to screen
Serial.print(",");
Serial.print(m_gx); // echo the number received to screen
Serial.print(",");
// I manually set m_gx after one run hardcoded (51 this time) 
m_inT += (float)(gyro_x / 65.0f - 51.5f / 65.0f) * 0.040;
// Force accelerometer and gyro sync
if (accelRoll > -1 && accelRoll < 1)
{
m_inT = 0; 
}
Serial.print(","); 
Serial.print(m_inT / accelRoll); // echo the number received to screen
Serial.println(""); // prints carriage return
delay(40); // wait for a second 
}