Arduino GY85 I2C测试

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接： https://blog.csdn.net/qq_42944558/article/details/102753225

Arduino GY85 I2C测试

#include <Wire.h>

#define ADXL345 0x53
#define HMC5883 0x1E
#define ITG3205 0x68

/*-----------ADXL345矫正-----------*/
const float alpha = 0.5;
double fXg = 0;
double fYg = 0;
double fZg = 0;
/*--------------------------------*/
struct
{
  int ACC_X;
  int ACC_Y;
  int ACC_Z;
} Accelerometor;

struct
{
  int COM_X;
  int COM_Y;
  int COM_Z;
  int ANGLE;
} Compass;

struct
{
  int GYRO_X;
  int GYRO_Y;
  int GYRO_Z;
} AngleSpeed;

byte ACC_BUF[5];
byte COM_BUF[5];
byte ANG_BUF[5];

void setup()
{
  Wire.begin();
  Init_ADXL345();
  Init_HMC5883();
  Init_ITG3205();
  Serial.begin(115200);
}

void loop()
{
  ReadAccelerometor();
  ReadCompass();
  ReadAngleSpeed();
  Serial.println("ACC=");
  Serial.println(Accelerometor.ACC_X);
  Serial.println(Accelerometor.ACC_Y);
  Serial.println(Accelerometor.ACC_Z);
  Serial.println("compass=");
  Serial.println(Compass.COM_X);
  Serial.println(Compass.COM_Y);
  Serial.println(Compass.COM_Z);
  Serial.println(Compass.ANGLE);
  Serial.println("angle=");
  Serial.println(AngleSpeed.GYRO_X);
  Serial.println(AngleSpeed.GYRO_Y);
  Serial.println(AngleSpeed.GYRO_Z);
}

void Single_Write(int Sensor,char REG_Address,char REG_data)
{               
  Wire.beginTransmission(Sensor);   
  Wire.write(REG_Address);    
  Wire.write(REG_data);        
  Wire.endTransmission();                 
}

void Init_ADXL345()
{
  Single_Write(ADXL345,0x31,0x0B);   //测量范围,正负16g，13位模式
  Single_Write(ADXL345,0x2C,0x08);   //速率设定为12.5 
  Single_Write(ADXL345,0x2D,0x08);   //选择电源模式
  Single_Write(ADXL345,0x2E,0x80);   //使能 DATA_READY 中断
  Single_Write(ADXL345,0x1E,0x00);   //X 偏移量
  Single_Write(ADXL345,0x1F,0x00);   //Y 偏移量 
  Single_Write(ADXL345,0x20,0x05);   //Z 偏移量 
}

void Init_HMC5883()
{
  Single_Write(HMC5883,0x02,0x00);  
  }

void Init_ITG3205()
{
  Single_Write(ITG3205, 0x3E, 0x80);   
  Single_Write(ITG3205, 0x15, 0x07);    
  Single_Write(ITG3205, 0x16, 0x1E);    //±2000°
  Single_Write(ITG3205, 0X17, 0x00);  
  Single_Write(ITG3205, 0x3E, 0x00);   
  }
  
void ReadAccelerometor()
{
  for(int i=0; i<6; i++)
  {
    Wire.beginTransmission(ADXL345);
    Wire.write(0x32+i);
    Wire.endTransmission();
    Wire.requestFrom(ADXL345,6);
    ACC_BUF[i] = Wire.read();
    }
  double Xg = ((ACC_BUF[0]<<8)+ ACC_BUF[1])*0.0039;
  double Yg = ((ACC_BUF[2]<<8)+ ACC_BUF[3])*0.0039;
  double Zg = ((ACC_BUF[4]<<8)+ ACC_BUF[5])*0.0039;
  Accelerometor.ACC_X = Xg * alpha + (fXg * (1.0 - alpha));
  Accelerometor.ACC_Y = Yg * alpha + (fYg * (1.0 - alpha));
  Accelerometor.ACC_Z = Zg * alpha + (fZg * (1.0 - alpha));
  }

void ReadCompass()
{
  for(int i=0; i<6; i++)
  {
    Wire.beginTransmission(HMC5883);
    Wire.write(0x03+i);
    Wire.endTransmission();
    Wire.requestFrom(HMC5883,6);
    COM_BUF[i] = Wire.read();
    }
  Compass.COM_X = ((COM_BUF[0]<<8)+ COM_BUF[1])*0.92;
  Compass.COM_Z = ((COM_BUF[2]<<8)+ COM_BUF[3])*0.92;
  Compass.COM_Y = ((COM_BUF[4]<<8)+ COM_BUF[5])*0.92;
  Compass.ANGLE = tan(Compass.COM_Y / Compass.COM_X)*(180/3.1415926)+180;
  }

void ReadAngleSpeed()
{
  for(int i=0; i<6; i++)
  {
    Wire.beginTransmission(ITG3205);
    Wire.write(0x1D+i);
    Wire.endTransmission();
    Wire.requestFrom(ITG3205,6);
    ANG_BUF[i] = Wire.read();
    }
  AngleSpeed.GYRO_X = (((ANG_BUF[0]<<8)+ ANG_BUF[1])/14.375);
  AngleSpeed.GYRO_Z = (((ANG_BUF[2]<<8)+ ANG_BUF[3])/14.375);
  AngleSpeed.GYRO_Y = (((ANG_BUF[4]<<8)+ ANG_BUF[5])/14.375);
  }