I2C通信之加速度计.陀螺仪.磁强计(345+3205+5883)
I2C通信之加速度计、陀螺仪、磁强计（345+3205+5883）
[复制链接]
leicheng
7
主题
0
好友
387
积分
中级会员
发消息
电梯直达
楼主
发表于 2014-2-18 14:45:03 |只看该作者 |倒序浏览
本帖最后由 leicheng 于 2014-5-6 17:43 编辑
之前简单写了陀螺仪传感器ITG3205与加速度传感器ADXL345 的相关帖子。现在把这两种传感器和HMC5883L磁强计放在一起，使用I2C与Arduino通信，读出这三种传感器的原始数据。没有加入滤波算法，没有额外库文件。方便大家对I2C通信、几种常见惯性传感器初步配置进行学习、交流~
IDE环境：Arduino1.0.5 I2C速率 ：100Kbps
ARDUINO 代码复制打印
#include
#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设置量程、分辨率的寄存器
#define A_BW_RATE 0x2C //Acc设置输出数据速率和功率模式的寄存器
#define A_POWER_CTL 0x2D //Acc设置测量模式的寄存器
#define G_SMPLRT_DIV 0x15 // Gyr设置采样率的寄存器
#define G_DLPF_FS 0x16 // Gyr设置量程、低通滤波带宽、时钟频率的寄存器
#define G_INT_CFG 0x17 // Gyr设置中断的寄存器
#define G_PWR_MGM 0x3E // Gyr设置电源管理的寄存器
#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; // 存放地磁场值
int buff[6]; //存放寄存器高低位值，X、Y、Z轴共6个
// 加速度传感器误差修正的偏移量
int a_offx = -2;
int a_offy = -3;
int a_offz =10;
// 陀螺仪传感器误差修正的偏移量
int g_offx = 83;
int g_offy = 27;
int g_offz = 17;
// 磁强计椭圆校正的偏移量
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： 输出数据速率50Hz,带宽25Hz
* A_POWER_CTL：测量模式
******************************************/
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]
yAcc = ((buff[3]
zAcc = ((buff[5]
}
void initGyro()
{
/*****************************************
* ITG3205 分辨率 14.375 LSB 度/秒
* G_SMPLRT_DIV：采样率 = 125Hz
* G_DLPF_FS：+ - 2000度/秒、低通滤波器5HZ、内部采样率1kHz
* G_INT_CFG：没有中断
* G_PWR_MGM：电源管理设定：无复位、无睡眠模式、无待机模式、内部振荡器
******************************************/
writeRegister(Gyr_address, G_SMPLRT_DIV, 0x07); //设置采样率
writeRegister(Gyr_address, G_DLPF_FS, 0x1E); //设置量程、低通滤波、内部采样率
writeRegister(Gyr_address, G_INT_CFG, 0x00); //设置中断（默认值）
writeRegister(Gyr_address, G_PWR_MGM, 0x00); //设置电源管理（默认值）
}
void getGyroValues()
{
readRegister(Gyr_address, 0x1D); //读取陀螺仪ITG3205的数据
xGyro = ((buff[0]
yGyro = ((buff[2]
zGyro = ((buff[4]
}
void initMagn()
{ /*****************************************
* HMC5883L
* ModeRegister：连续测量模式
* ConfigurationRegisterA：输出数据速率15Hz、内部采样8次平均、正常测量配置
* ConfigurationRegisterB：磁场范围=+-1.3Ga、1090 LSB/Gauss
******************************************/
writeRegister(HMCAddress, ModeRegister, 0x00);
writeRegister(HMCAddress, ConfigurationRegisterA, 0x70);
writeRegister(HMCAddress, ConfigurationRegisterB, 0x20);
}
void getMagnValues()
{
readRegister(HMCAddress, 0x03); //读取磁强计HMC5883L的数据
xMag = ((buff[0]
zMag = (( buff[2]
yMag = (( buff[4]
}
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);
}
看到这里，大家或许会去把这些原始数据转换成对应的物理量，没关系，请参考Malc童鞋的：加速度计和陀螺仪指南。如果希望滤波，以便得到更准确的姿态角，请参考黑马前辈的：我的自平衡小车D3和笨笨童鞋的：Arduino+MPU6050+Kalman filter。 需要注意的是帖子中的kalman滤波算法只能对水平面姿态角之一：Pitch或者Roll正确滤波(欧拉角微分方程简化形式：yaw=0&&pitch=0||roll=0)。如果打算使用全姿态欧拉角，推荐用四元数法，再采用卡尔曼滤波估计最优的全姿态角。当然，还是推荐互补滤波算法解决这类问题。
kalman.rar
1.1 KB, 阅读权限: 70, 下载次数: 9
卡尔曼库文件
分享到:
QQ空间
腾讯微博
腾讯朋友
收藏5
支持1
反对0
Communication && Sharing
回复
举报