#include <Wire.h>
                            

                            
#define Acc_address   0x53      //ADXL345的I2C地址（ADDR接地）
                            

                            
#define Gyr_address   0x68      //ITG3205的I2C地址（AD0接地）
                            

                            
#define HMCAddress    0x1E      //HMC5883L的I2C地址
                            


                            

                            
#define A_DATA_FORMAT 0x31      //Acc設(shè)置量程、分辨率的寄存器
                            

                            
#define A_BW_RATE     0x2C      //Acc設(shè)置輸出數(shù)據(jù)速率和功率模式的寄存器
                            

                            
#define A_POWER_CTL   0x2D      //Acc設(shè)置測(cè)量模式的寄存器
                            


                            

                            
#define G_SMPLRT_DIV  0x15      // Gyr設(shè)置采樣率的寄存器
                            

                            
#define G_DLPF_FS     0x16      // Gyr設(shè)置量程、低通濾波帶寬、時(shí)鐘頻率的寄存器
                            

                            
#define G_INT_CFG     0x17      // Gyr設(shè)置中斷的寄存器
                            

                            
#define G_PWR_MGM     0x3E      // Gyr設(shè)置電源管理的寄存器
                            


                            

                            
#define ConfigurationRegisterA 0x00  //Mag配置寄存器A
                            

                            
#define ConfigurationRegisterB 0x01  //Mag配置寄存器B
                            

                            
#define ModeRegister 0x02            //Mag模式寄存器
                            


                            


                            

                            
int xAcc,  yAcc,  zAcc;         //存放加速度值
                            

                            
int xGyro, yGyro, zGyro;        //存放角速度值
                            

                            
int xMag,  yMag,  zMag;         // 存放地磁場(chǎng)值
                            


                            

                            
int buff[6];                    //存放寄存器高低位值，X、Y、Z軸共6個(gè)
                            


                            

                            
// 加速度傳感器誤差修正的偏移量
                            

                            
int a_offx = -2;
                            

                            
int a_offy = -3;
                            

                            
int a_offz =10;
                            


                            

                            
// 陀螺儀傳感器誤差修正的偏移量
                            

                            
int g_offx = 83;
                            

                            
int g_offy = 27;
                            

                            
int g_offz = 17;
                            


                            

                            
// 磁強(qiáng)計(jì)橢圓校正的偏移量
                            

                            
int m_offx=-45;
                            

                            
int m_offy=-98;
                            

                            
int m_offz= 75;
                            


                            

                            
void writeRegister(int deviceAddress, byte address, byte val)
                            

                            
 {
                            

                            
   Wire.beginTransmission(deviceAddress); 
                            

                            
   Wire.write(address);       
                            

                            
   Wire.write(val);        
                            

                            
   Wire.endTransmission();
                            

                            
}
                            


                            

                            
void readRegister(int deviceAddress, byte address) 
                            

                            
{
                            

                            
  Wire.beginTransmission(deviceAddress);  
                            

                            
  Wire.write(address);        
                            

                            
  Wire.endTransmission(); 
                            

                            
  Wire.beginTransmission(deviceAddress); 
                            

                            
  Wire.requestFrom(deviceAddress, 6);   
                            


                            

                            
  int i = 0;
                            

                            
  while(Wire.available())    
                            

                            
  {  buff[i++] = Wire.read();  }
                            

                            
  Wire.endTransmission(); 
                            

                            
}
                            


                            

                            
void initAcc() 
                            

                            
{
                            

                            
  /*****************************************
                            

                            
   * ADXL345
                            

                            
   * A_DATA_FORMAT：量程=+-2g,10位分辨率 3.9 LSB/mg
                            

                            
   * A_BW_RATE： 輸出數(shù)據(jù)速率50Hz,帶寬25Hz
                            

                            
   * A_POWER_CTL：測(cè)量模式
                            

                            
   ******************************************/
                            

                            
  writeRegister (Acc_address, A_DATA_FORMAT, 0x00); 
                            

                            
  writeRegister (Acc_address, A_BW_RATE, 0x09);
                            

                            
  writeRegister (Acc_address, A_POWER_CTL, 0x08);  
                            

                            
 }
                            


                            

                            
void getAccData()
                            

                            
{
                            

                            
  readRegister(Acc_address, 0x32);  
                            

                            
  xAcc = ((buff[1] << 8) | buff[0] )+ a_offx;   
                            

                            
  yAcc = ((buff[3] << 8) | buff[2] )+ a_offy;
                            

                            
  zAcc = ((buff[5] << 8) | buff[4]) + a_offz;
                            

                            
}
                            


                            

                            
void initGyro()
                            

                            
{
                            

                            
  /*****************************************
                            

                            
   * ITG3205     分辨率  14.375 LSB 度/秒
                            

                            
   * G_SMPLRT_DIV：采樣率 = 125Hz
                            

                            
   * G_DLPF_FS：+ - 2000度/秒、低通濾波器5HZ、內(nèi)部采樣率1kHz
                            

                            
   * G_INT_CFG：沒有中斷
                            

                            
   * G_PWR_MGM：電源管理設(shè)定：無復(fù)位、無睡眠模式、無待機(jī)模式、內(nèi)部振蕩器
                            

                            
   ******************************************/
                            

                            
  writeRegister(Gyr_address, G_SMPLRT_DIV, 0x07); //設(shè)置采樣率
                            

                            
  writeRegister(Gyr_address, G_DLPF_FS, 0x1E); //設(shè)置量程、低通濾波、內(nèi)部采樣率
                            

                            
  writeRegister(Gyr_address, G_INT_CFG, 0x00); //設(shè)置中斷（默認(rèn)值）
                            

                            
  writeRegister(Gyr_address, G_PWR_MGM, 0x00);    //設(shè)置電源管理（默認(rèn)值）
                            

                            
}
                            


                            

                            
void getGyroValues()
                            

                            
{
                            

                            
  readRegister(Gyr_address, 0x1D); //讀取陀螺儀ITG3205的數(shù)據(jù)
                            

                            
  xGyro = ((buff[0] << 8) | buff[1]) + g_offx;
                            

                            
  yGyro = ((buff[2] << 8) | buff[3]) + g_offy;
                            

                            
  zGyro = ((buff[4] << 8) | buff[5]) + g_offz;
                            

                            
}
                            


                            

                            
void initMagn()
                            

                            
{  /*****************************************
                            

                            
   * HMC5883L
                            

                            
   * ModeRegister：連續(xù)測(cè)量模式
                            

                            
   * ConfigurationRegisterA：輸出數(shù)據(jù)速率15Hz、內(nèi)部采樣8次平均、正常測(cè)量配置
                            

                            
   * ConfigurationRegisterB：磁場(chǎng)范圍=+-1.3Ga、1090 LSB/Gauss
                            

                            
   ******************************************/
                            

                            
  writeRegister(HMCAddress, ModeRegister, 0x00); 
                            

                            
  writeRegister(HMCAddress, ConfigurationRegisterA, 0x70);
                            

                            
  writeRegister(HMCAddress, ConfigurationRegisterB, 0x20);
                            

                            
}
                            


                            

                            
void getMagnValues()
                            

                            
{
                            

                            
  readRegister(HMCAddress, 0x03); //讀取磁強(qiáng)計(jì)HMC5883L的數(shù)據(jù)
                            

                            
  xMag = ((buff[0] << 8) | buff[1] ) +m_offx ;
                            

                            
  zMag = (( buff[2] << 8) | buff[3] ) +m_offz;
                            

                            
  yMag = (( buff[4] << 8) | buff[5] )+m_offy ;
                            

                            
}
                            


                            

                            
void setup()
                            

                            
{
                            

                            
  Serial.begin(9600);
                            

                            
  Wire.begin();
                            

                            
  initAcc();
                            

                            
  initGyro();
                            

                            
  initMagn();
                            

                            
  delay(50);
                            

                            
}
                            


                            

                            
void loop()
                            

                            
{
                            

                            
    getAccData();
                            

                            
    Serial.print("xAcc=");
                            

                            
    Serial.print(xAcc);
                            

                            
    Serial.print("  yAcc=");
                            

                            
    Serial.print(yAcc);
                            

                            
    Serial.print("  zAcc=");
                            

                            
    Serial.println(zAcc);
                            


                            

                            
    getGyroValues();
                            

                            
    Serial.print("xGyro=");
                            

                            
    Serial.print(xGyro);
                            

                            
    Serial.print("  yGyro=");
                            

                            
    Serial.print(yGyro);
                            

                            
    Serial.print("  zGyro=");
                            

                            
    Serial.println(zGyro);
                            


                            

                            
    getMagnValues();
                            

                            
    Serial.print("xMag=");
                            

                            
    Serial.print(xMag);
                            

                            
    Serial.print("  yMag=");
                            

                            
    Serial.print(yMag);
                            

                            
    Serial.print("  zMag=");
                            

                            
    Serial.println(zMag);
                            

                            
    delay(200);
                            

                            
}