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);
}