leicheng 发表于 2014-2-18 14:45:03

I2C通信之加速度计、陀螺仪、磁强计（345+3205+5883）

本帖最后由 leicheng 于 2020-12-31 17:58 编辑

       之前简单写了陀螺仪传感器ITG3205与加速度传感器ADXL345 的相关帖子。现在把这两种传感器和HMC5883L磁强计放在一起，使用I2C与Arduino通信，读出这三种传感器的原始数据。没有加入滤波算法，没有额外库文件。方便大家对I2C通信、几种常见惯性传感器初步配置进行学习、交流~
IDE环境：Arduino1.0.5      I2C速率 ：100Kbps
#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设置量程、分辨率的寄存器
#define A_BW_RATE   0x2C      //Acc设置输出数据速率和功率模式的寄存器
#define A_POWER_CTL   0x2D      //Acc设置测量模式的寄存器

#define G_SMPLRT_DIV0x15      // 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;                  //存放寄存器高低位值，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 = 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 << 8) | buff )+ a_offx;   
yAcc = ((buff << 8) | buff )+ a_offy;
zAcc = ((buff << 8) | buff) + a_offz;
}

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 << 8) | buff) + g_offx;
yGyro = ((buff << 8) | buff) + g_offy;
zGyro = ((buff << 8) | buff) + g_offz;
}

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 << 8) | buff ) +m_offx ;
zMag = (( buff << 8) | buff ) +m_offz;
yMag = (( buff << 8) | buff )+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);
}





   看到这里，大家或许会去把这些原始数据转换成对应的物理量，没关系，请参考Malc童鞋的：加速度计和陀螺仪指南。如果希望滤波，以便得到更准确的姿态角，请参考黑马前辈的：我的自平衡小车D3和笨笨童鞋的：Arduino+MPU6050+Kalman filter。 需要注意的是帖子中的kalman滤波算法只能对水平面姿态角之一：Pitch或者Roll正确滤波(欧拉角微分方程简化形式：yaw=0&&pitch=0||roll=0)。如果打算使用全姿态欧拉角，推荐用四元数法，再采用卡尔曼滤波估计最优的全姿态角。当然，还是推荐互补滤波算法解决这类问题。



arduinoSuper 发表于 2014-2-18 16:58:11

:)    学习

转角 发表于 2014-2-28 11:34:38

// 陀螺仪传感器误差修正的偏移量
int g_offx = 83;
int g_offy = 27;
int g_offz = 17;

这偏移量是怎么来的？不同地区的偏移量一样吗？

leicheng 发表于 2014-2-28 13:11:29

本帖最后由 leicheng 于 2014-3-8 09:18 编辑

转角 发表于 2014-2-28 11:34 static/image/common/back.gif
// 陀螺仪传感器误差修正的偏移量
int g_offx = 83;
int g_offy = 27;


这个坛子里有童鞋介绍的方法就是采集一段时间的数据后求均值，然后再处理。

转角 发表于 2014-2-28 13:19:29

leicheng 发表于 2014-2-28 13:11 static/image/common/back.gif
如何得到加计、陀螺仪的修正偏移量，论坛里有基本的方法，还是讲下，主要思路就是采集一段时间的数据后求 ...

哦，原来这样，谢谢啊

leicheng 发表于 2014-2-28 23:36:09

本帖最后由 leicheng 于 2014-3-1 19:49 编辑

转角 发表于 2014-2-28 13:19 static/image/common/back.gif
哦，原来这样，谢谢啊

不客气呀~这个方法只是偷懒的校正偏移量，把陀螺仪的输出看成是简单的常值零偏与白噪声之和，对付小应用足够了。

xuqin3 发表于 2014-3-10 23:27:11

不错，收着。

zhangzhe0617 发表于 2014-3-27 09:49:51

我当时写的确实不能得出偏航角，而且具体参数也没优化就飞了。
惭愧的是四元素还有点搞不定，能否教一下四元素的卡尔曼滤波？

转角 发表于 2014-3-31 11:01:14

亲，我最近做头追遇到问题了。对姿态传感器不熟悉，左右转动的姿态应该怎么去算？现在只有上下可动

转角 发表于 2014-3-31 13:36:16

没头绪啊，有公式吗？或者现成的代码更好

smilepig 发表于 2014-3-31 16:27:49

我是个菜鸟 请问这么多I2C怎么连接？用面包版并联然后引出4根线接到arduino上？

leicheng 发表于 2014-4-26 18:58:28

本帖最后由 leicheng 于 2014-11-16 19:58 编辑

1234567890

Roson 发表于 2014-12-29 18:39:39

学习中 mark

Joyjay 发表于 2015-12-7 21:01:33

确实挺好的

Joyjay 发表于 2015-12-7 21:28:41

楼主为啥限制下载呢，遮掩搞不太好啊！:o:o:o:o:o:o:o
页: [1]
查看完整版本: I2C通信之加速度计、陀螺仪、磁强计（345+3205+5883）