|
|
本帖最后由 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_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] << 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、内部采样率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] << 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：连续测量模式
- * 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] << 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);
- }
看到这里，大家或许会去把这些原始数据转换成对应的物理量，没关系，请参考Malc童鞋的：加速度计和陀螺仪指南。如果希望滤波，以便得到更准确的姿态角，请参考黑马前辈的：我的自平衡小车D3和笨笨童鞋的：Arduino+MPU6050+Kalman filter。 需要注意的是帖子中的kalman滤波算法只能对水平面姿态角之一：Pitch或者Roll正确滤波(欧拉角微分方程简化形式：yaw=0&&pitch=0||roll=0)。如果打算使用全姿态欧拉角，推荐用四元数法，再采用卡尔曼滤波估计最优的全姿态角。当然，还是推荐互补滤波算法解决这类问题。
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
发表于 2014-2-18 14:45:03
学习
楼主
