|
|
楼主 |
发表于 2013-1-9 23:05:09
|
显示全部楼层
本帖最后由 rovershie 于 2013-1-9 23:10 编辑
圆点博士微型四轴飞行器参考源代码之 ----- 读取MPU6050。
该代码由坛友js200300953提功，并在圆点博士微型四轴飞行器上测试通过
=================================================
// bsp/mpu6050.c
// 2013-1-7 18:47:54
// js200300953
//博客: blog.sina.com.cn/js200300953
#include "mpu6050.h"
#include "i2c.h"
#include "time.h"
#define MPU6050_ADDRESS 0xD0
#define MPU6050_REG_RAW_DATA 0x3B
#define MPU6050_REG_ACC 0x3B
#define MPU6050_REG_TEM 0x41
#define MPU6050_REG_GYR 0x43
const static float MPU6050_ACC_OFFSET[3] =
{
-24.161 ,
-30.1335,
281.5 ,
};
const static float MPU6050_ACC_GAIN[3] =
{
0.00240318,
0.00237744,
0.00238118,
};
const static float MPU6050_GYR_OFFSET[3] =
{
0,
0,
0,
};
const static float MPU6050_GYR_GAIN[3] =
{
0.00106f,
0.00106f,
0.00106f,
};
static struct
{
int16_t acc[3];
int16_t tem;
int16_t gyr[3];
}mpu6050_rawData;
int32_t mpu6050_init(void);
int32_t mpu6050_isDataReady(void);
int32_t mpu6050_read(void);
int32_t mpu6050_getRawAcc(int16_t acc[3]);
int32_t mpu6050_getRawGyr(int16_t gyr[3]);
int32_t mpu6050_getCalibratedAcc(float acc[3]);
int32_t mpu6050_getCalibratedGyr(float gyr[3]);
int32_t mpu6050_init(void)
{
for(int i=0;i<sizeof(mpu6050_rawData);i++)
((uint8_t *)&mpu6050_rawData) = 0;
//
// 检查ID。
uint8_t id = 0;
if(i2c_read(MPU6050_ADDRESS,0x75,&id,1) != 0)
return MPU6050_FAILED;
if(id != 0x68)
return MPU6050_FAILED;
//
// 设置MPU6050的参数。
const uint8_t param[][2] =
{
// {寄存器地址,寄存器值},
{0x6B,1 }, // 退出睡眠模式，设取样时钟为陀螺X轴。
{0x19,4 }, // 取样时钟4分频，1k/4，取样率为25Hz。
{0x1A,2 }, // 低通滤波，截止频率100Hz左右。
{0x1B,3<<3 }, // 陀螺量程，2000dps。
{0x1C,2<<3 }, // 加速度计量程，8g。
{0x37,0x32 }, // 中断信号为高电平，推挽输出，直到有读取操作才消失，直通辅助I2C。
{0x38,1 }, // 使用“数据准备好”中断。
{0x6A,0x00 }, // 不使用辅助I2C。
};
for(int i=0;i<sizeof(param)/2;i++)
{
if(i2c_write(MPU6050_ADDRESS,param[0],&(param[1]),1) != 0)
return MPU6050_FAILED;
//
uint8_t check = 0;
if(i2c_read(MPU6050_ADDRESS,param[0],&check,1) != 0)
return MPU6050_FAILED;
if(check != param[1])
return MPU6050_FAILED;
}
//
return MPU6050_SUCCEED;
}
int32_t mpu6050_dataIsReady(void)
{
static uint32_t target = 0;
uint32_t now = time_nowMs();
if(target <= now)
{
target = now + 3;//333Hz
//
return 1;
}
return 0;
}
int32_t mpu6050_read(void)
{
if(i2c_read(MPU6050_ADDRESS,MPU6050_REG_RAW_DATA,&mpu6050_rawData
,sizeof(mpu6050_rawData)) != 0)
return MPU6050_FAILED;
//
// 大端转小端。
uint8_t * p = (uint8_t *)&mpu6050_rawData;
for(int i=0;i<sizeof(mpu6050_rawData)/2;i++)
{
uint8_t tmp = p[2*i];
p[2*i] = p[2*i + 1];
p[2*i + 1] = tmp;
}
//
return MPU6050_SUCCEED;
}
// acc : LSB
int32_t mpu6050_getRawAcc(int16_t acc[3])
{
for(int i=0;i<3;i++)
acc = mpu6050_rawData.acc;
//
return MPU6050_SUCCEED;
}
//gyr : LSB
int32_t mpu6050_getRawGyr(int16_t gyr[3])
{
for(int i=0;i<3;i++)
gyr = mpu6050_rawData.gyr;
//
return MPU6050_SUCCEED;
}
// acc : m/s²
int32_t mpu6050_getCalibratedAcc(float acc[3])
{
for(int i=0;i<3;i++)
acc = (MPU6050_ACC_OFFSET + mpu6050_rawData.acc) * MPU6050_ACC_GAIN;
//
return MPU6050_SUCCEED;
}
// gyr : rad/s
int32_t mpu6050_getCalibratedGyr(float gyr[3])
{
for(int i=0;i<3;i++)
gyr = (MPU6050_GYR_OFFSET + mpu6050_rawData.gyr) * MPU6050_GYR_GAIN;
//
return MPU6050_SUCCEED;
}
|
|
楼主: rovershie
[复制链接]
发表于 2012-12-29 21:15:30
。
据说7天是人类忍耐饥饿的极限。
支持开源。学习了
