我有个MPU6050+STM32 的板子是否可以测算位移
本帖最后由 lycals 于 2012-11-20 16:57 编辑我在淘宝上买了一个AHRS的板子，上面有STM32+mpu6050+HMC5883L+BMP180 组合，现在希望使用这个开发板去测算在运动时，其运动的方向及位移，想个人导航仪一样可以获得，在移动了一段时间可以测得，移动的距离是多少
#include "MPU6050.h"
#include "IOI2C.h"
#include "LED.h"
uint8_t buffer;
int16_tMPU6050_FIFO;
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
/**************************实现函数********************************************
*函数原型: voidMPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功 能: 将新的ADC数据更新到 FIFO数组，进行滤波处理
*******************************************************************************/
voidMPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){ //FIFO 操作
MPU6050_FIFO=MPU6050_FIFO;
MPU6050_FIFO=MPU6050_FIFO;
MPU6050_FIFO=MPU6050_FIFO;
MPU6050_FIFO=MPU6050_FIFO;
MPU6050_FIFO=MPU6050_FIFO;
MPU6050_FIFO=MPU6050_FIFO;
}
MPU6050_FIFO=ax;//将新的数据放置到 数据的最后面
MPU6050_FIFO=ay;
MPU6050_FIFO=az;
MPU6050_FIFO=gx;
MPU6050_FIFO=gy;
MPU6050_FIFO=gz;
sum=0;
for(i=0;i<10;i++){ //求当前数组的合，再取平均值
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO;
}
MPU6050_FIFO=sum/10;
}
/**************************实现函数********************************************
*函数原型: void MPU6050_setClockSource(uint8_t source)
*功 能: 设置MPU6050 的时钟源
* CLK_SEL | Clock Source
* --------+--------------------------------------
* 0 | Internal oscillator
* 1 | PLL with X Gyro reference
* 2 | PLL with Y Gyro reference
* 3 | PLL with Z Gyro reference
* 4 | PLL with external 32.768kHz reference
* 5 | PLL with external 19.2MHz reference
* 6 | Reserved
* 7 | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){
IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
/** Set full-scale gyroscope range.
* @param range New full-scale gyroscope range value
* @see getFullScaleRange()
* @see MPU6050_GYRO_FS_250
* @see MPU6050_RA_GYRO_CONFIG
* @see MPU6050_GCONFIG_FS_SEL_BIT
* @see MPU6050_GCONFIG_FS_SEL_LENGTH
*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
/**************************实现函数********************************************
*函数原型: void MPU6050_setFullScaleAccelRange(uint8_t range)
*功 能: 设置MPU6050 加速度计的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {
IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
/**************************实现函数********************************************
*函数原型: void MPU6050_setSleepEnabled(uint8_t enabled)
*功 能: 设置MPU6050 是否进入睡眠模式
enabled =1 睡觉
enabled =0 工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}
/**************************实现函数********************************************
*函数原型: uint8_t MPU6050_getDeviceID(void)
*功 能: 读取MPU6050 WHO_AM_I 标识 将返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {
IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);
return buffer;
}
/**************************实现函数********************************************
*函数原型: uint8_t MPU6050_testConnection(void)
*功 能: 检测MPU6050 是否已经连接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {
if(MPU6050_getDeviceID() == 0x68)//0b01101000;
return 1;
else return 0;
}
/**************************实现函数********************************************
*函数原型: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功 能: 设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}
/**************************实现函数********************************************
*函数原型: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功 能: 设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}
/**************************实现函数********************************************
*函数原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
int16_t temp;
unsigned char i;
MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO); //设置时钟
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_1000);//陀螺仪最大量程 +-1000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //进入工作状态
MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(1); //主控制器的I2C与 MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
//配置MPU6050 的中断模式 和中断电平模式
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, 0);
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, 0);
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, 1);
IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, 1);
//开数据转换完成中断
IICwriteBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, 1);
for(i=0;i<10;i++){//更新FIFO数组
delay_us(50);
MPU6050_getMotion6(&temp,&temp,&temp,&temp,&temp,&temp);
}
MPU6050_InitGyro_Offset();
}
/**************************实现函数********************************************
*函数原型: unsigned char MPU6050_is_DRY(void)
*功 能: 检查 MPU6050的中断引脚，测试是否完成转换
返回 1转换完成
0 数据寄存器还没有更新
*******************************************************************************/
unsigned char MPU6050_is_DRY(void)
{
if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6)==Bit_SET){
return 1;
}
else return 0;
}
int16_t MPU6050_Lastax,MPU6050_Lastay,MPU6050_Lastaz
,MPU6050_Lastgx,MPU6050_Lastgy,MPU6050_Lastgz;
/**************************实现函数********************************************
*函数原型: void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
*功 能: 读取 MPU6050的当前测量值
*******************************************************************************/
void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
if(MPU6050_is_DRY()){
IICreadBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
MPU6050_Lastax=(((int16_t)buffer) << 8) | buffer;
MPU6050_Lastay=(((int16_t)buffer) << 8) | buffer;
MPU6050_Lastaz=(((int16_t)buffer) << 8) | buffer;
//跳过温度ADC
MPU6050_Lastgx=(((int16_t)buffer) << 8) | buffer;
MPU6050_Lastgy=(((int16_t)buffer) << 8) | buffer;
MPU6050_Lastgz=(((int16_t)buffer) << 8) | buffer;
MPU6050_newValues(MPU6050_Lastax,MPU6050_Lastay,MPU6050_Lastaz
,MPU6050_Lastgx,MPU6050_Lastgy,MPU6050_Lastgz);
*ax=MPU6050_FIFO;
*ay=MPU6050_FIFO;
*az = MPU6050_FIFO;
*gx=MPU6050_FIFO-Gx_offset;
*gy = MPU6050_FIFO-Gy_offset;
*gz = MPU6050_FIFO-Gz_offset;
} else {
*ax = MPU6050_FIFO;//=MPU6050_FIFO;
*ay = MPU6050_FIFO;//=MPU6050_FIFO;
*az = MPU6050_FIFO;//=MPU6050_FIFO;
*gx = MPU6050_FIFO-Gx_offset;//=MPU6050_FIFO;
*gy = MPU6050_FIFO-Gy_offset;//=MPU6050_FIFO;
*gz = MPU6050_FIFO-Gz_offset;//=MPU6050_FIFO;
}
}
void MPU6050_getlastMotion6(int16_t* ax, int16_t* ay,
int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
*ax=MPU6050_FIFO;
*ay=MPU6050_FIFO;
*az = MPU6050_FIFO;
*gx=MPU6050_FIFO-Gx_offset;
*gy = MPU6050_FIFO-Gy_offset;
*gz = MPU6050_FIFO-Gz_offset;
}
/**************************实现函数********************************************
*函数原型: void MPU6050_InitGyro_Offset(void)
*功 能: 读取 MPU6050的陀螺仪偏置
此时模块应该被静止放置。以测试静止时的陀螺仪输出
*******************************************************************************/
void MPU6050_InitGyro_Offset(void)
{
unsigned char i;
int16_t temp;
int32_t tempgx=0,tempgy=0,tempgz=0;
int32_t tempax=0,tempay=0,tempaz=0;
Gx_offset=0;
Gy_offset=0;
Gz_offset=0;
for(i=0;i<50;i++){
delay_us(100);
MPU6050_getMotion6(&temp,&temp,&temp,&temp,&temp,&temp);
LED_Change();
}
for(i=0;i<100;i++){
delay_us(200);
MPU6050_getMotion6(&temp,&temp,&temp,&temp,&temp,&temp);
tempax+= temp;
tempay+= temp;
tempaz+= temp;
tempgx+= temp;
tempgy+= temp;
tempgz+= temp;
LED_Change();
}
Gx_offset=tempgx/100;//MPU6050_FIFO;
Gy_offset=tempgy/100;//MPU6050_FIFO;
Gz_offset=tempgz/100;//MPU6050_FIFO;
tempax/=100;
tempay/=100;
tempaz/=100;
}
#include "HMC5883L.h"
float HMC5883_lastx,HMC5883_lasty,HMC5883_lastz;
int16_tHMC5883_FIFO; //磁力计滤波
void HMC58X3_getRaw(int16_t *x,int16_t *y,int16_t *z);
/**************************实现函数********************************************
*函数原型: unsigned char HMC5883_IS_newdata(void)
*功 能: 读取DRDY 引脚，判断是否完成了一次转换
Low for 250 μsec when data is placed in the data output registers.
输入参数：无
输出参数：如果完成转换，则输出1否则输出 0
*******************************************************************************/
unsigned char HMC5883_IS_newdata(void)
{
if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==Bit_SET){
return 1;
}
else return 0;
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_FIFO_init(void)
*功 能: 连续读取100次数据，以初始化FIFO数组
输入参数：无
输出参数：无
*******************************************************************************/
void HMC58X3_FIFO_init(void)
{
int16_t temp;
unsigned char i;
for(i=0;i<50;i++){
HMC58X3_getRaw(&temp,&temp,&temp);
delay_us(200);//延时再读取数据
LED_Change(); //LED闪烁
}
}
/**************************实现函数********************************************
*函数原型: voidHMC58X3_newValues(int16_t x,int16_t y,int16_t z)
*功 能: 更新一组数据到FIFO数组
输入参数：磁力计三个轴对应的ADC值
输出参数：无
*******************************************************************************/
voidHMC58X3_newValues(int16_t x,int16_t y,int16_t z)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){
HMC5883_FIFO=HMC5883_FIFO;
HMC5883_FIFO=HMC5883_FIFO;
HMC5883_FIFO=HMC5883_FIFO;
}
HMC5883_FIFO=x;
HMC5883_FIFO=y;
HMC5883_FIFO=z;
sum=0;
for(i=0;i<10;i++){ //取数组内的值进行求和再取平均
sum+=HMC5883_FIFO;
}
HMC5883_FIFO=sum/10; //将平均值更新
sum=0;
for(i=0;i<10;i++){
sum+=HMC5883_FIFO;
}
HMC5883_FIFO=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=HMC5883_FIFO;
}
HMC5883_FIFO=sum/10;
} //HMC58X3_newValues
/**************************实现函数********************************************
*函数原型: void HMC58X3_writeReg(unsigned char reg, unsigned char val)
*功 能: 写HMC5883L的寄存器
输入参数： reg寄存器地址
val 要写入的值
输出参数：无
*******************************************************************************/
void HMC58X3_writeReg(unsigned char reg, unsigned char val) {
IICwriteByte(HMC58X3_ADDR,reg,val);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_getRaw(int16_t *x,int16_t *y,int16_t *z)
*功 能: 写HMC5883L的寄存器
输入参数： reg寄存器地址
val 要写入的值
输出参数：无
*******************************************************************************/
void HMC58X3_getRaw(int16_t *x,int16_t *y,int16_t *z) {
unsigned char vbuff;
vbuff=vbuff=vbuff=vbuff=vbuff=vbuff=0;
IICreadBytes(HMC58X3_ADDR,HMC58X3_R_XM,6,vbuff);
HMC58X3_newValues(((int16_t)vbuff << 8) | vbuff,((int16_t)vbuff << 8) | vbuff,((int16_t)vbuff << 8) | vbuff);
*x = HMC5883_FIFO;
*y = HMC5883_FIFO;
*z = HMC5883_FIFO;
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_getValues(int16_t *x,int16_t *y,int16_t *z)
*功 能: 读取 磁力计的当前ADC值
输入参数： 三个轴对应的输出指针
输出参数：无
*******************************************************************************/
void HMC58X3_getlastValues(int16_t *x,int16_t *y,int16_t *z) {
*x = HMC5883_FIFO;
*y = HMC5883_FIFO;
*z = HMC5883_FIFO;
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_mgetValues(float *arry)
*功 能: 读取 校正后的 磁力计ADC值
输入参数： 输出数组指针
输出参数：无
*******************************************************************************/
void HMC58X3_mgetValues(float *arry) {
int16_t xr,yr,zr;
HMC58X3_getRaw(&xr, &yr, &zr);
arry= HMC5883_lastx=(float)(xr);
arry= HMC5883_lasty=(float)(yr);
arry= HMC5883_lastz=(float)(zr);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_setGain(unsigned char gain)
*功 能: 设置 5883L的增益
输入参数： 目标增益 0-7
输出参数：无
*******************************************************************************/
void HMC58X3_setGain(unsigned char gain) {
// 0-7, 1 default
if (gain > 7) return;
HMC58X3_writeReg(HMC58X3_R_CONFB, gain << 5);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_setMode(unsigned char mode)
*功 能: 设置 5883L的工作模式
输入参数： 模式
输出参数：无
*******************************************************************************/
void HMC58X3_setMode(unsigned char mode) {
if (mode > 2) {
return;
}
HMC58X3_writeReg(HMC58X3_R_MODE, mode);
delay_us(100);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_init(u8 setmode)
*功 能: 设置 5883L的工作模式
输入参数： 模式
输出参数：无
*******************************************************************************/
void HMC58X3_init(u8 setmode) {
if (setmode) {
HMC58X3_setMode(0);
}
HMC58X3_writeReg(HMC58X3_R_CONFA, 0x70); // 8 samples averaged, 75Hz frequency, no artificial bias.
HMC58X3_writeReg(HMC58X3_R_CONFB, 0xA0);
HMC58X3_writeReg(HMC58X3_R_MODE, 0x00);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_setDOR(unsigned char DOR)
*功 能: 设置 5883L的 数据输出速率
输入参数： 速率值
0 -> 0.75Hz| 1 -> 1.5Hz
2 -> 3Hz | 3 -> 7.5Hz
4 -> 15Hz | 5 -> 30Hz
6 -> 75Hz
输出参数：无
*******************************************************************************/
void HMC58X3_setDOR(unsigned char DOR) {
if (DOR>6) return;
HMC58X3_writeReg(HMC58X3_R_CONFA,DOR<<2);
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_getID(char id)
*功 能: 读取芯片的ID
输入参数： ID存放的数组
输出参数：无
*******************************************************************************/
void HMC58X3_getID(char id)
{
id=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDA);
id=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDB);
id=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDC);
} // getID().
/**************************实现函数********************************************
*函数原型: void HMC5883L_SetUp(void)
*功 能: 初始化 HMC5883L 使之进入可用状态
输入参数：
输出参数：无
*******************************************************************************/
void HMC5883L_SetUp(void)
{
HMC58X3_init(0); // Don't set mode yet, we'll do that later on.
HMC58X3_setMode(0);
HMC58X3_setDOR(6);//75hz 更新率
HMC58X3_FIFO_init();
}
原理上可以
精度够呛。时间不长漂移的误差好大了 test01 发表于 2012-11-20 23:37 static/image/common/back.gif
原理上可以
精度够呛。时间不长漂移的误差好大了
那可以有什么消减误差好办法吗？ 这个我还没啥研究
常见短间隔定时GPS校正数据 test01 发表于 2012-11-21 15:50 static/image/common/back.gif
这个我还没啥研究
常见短间隔定时GPS校正数据
那要是在一段时间没有GPS信号怎么办啊 test01 发表于 2012-11-21 15:50 static/image/common/back.gif
这个我还没啥研究
常见短间隔定时GPS校正数据
还是谢谢啊 本帖最后由 test01 于 2012-11-21 19:21 编辑
lycals 发表于 2012-11-21 16:02 static/image/common/back.gif
那要是在一段时间没有GPS信号怎么办啊
搞其他定位方式
比如大范围用无线电信标，小范围激光信标，室内可用超声波信标
砸钱买高端惯性元件能提高一个数量级，投入的钱是6050的两个数量级咯 test01 发表于 2012-11-21 19:18 static/image/common/back.gif
搞其他定位方式
比如大范围用无线电信标，小范围激光信标，室内可用超声波信标
主要是看到有本书说mpu6050可以使用惯性导航，所以想利用一下 lycals 发表于 2012-11-22 09:02 static/image/common/back.gif
主要是看到有本书说mpu6050可以使用惯性导航，所以想利用一下
我说的其他信标是替代GPS校准惯性导航系统的意思 test01 发表于 2012-11-22 14:57 static/image/common/back.gif
我说的其他信标是替代GPS校准惯性导航系统的意思
这样的啊 ，知道了 那你不就是传说中的惯性导航系统么？ 一直有同样的想法,用MPU6050做套惯性导航,在室内用.目前再找资料,不过一直没理出头绪.等有结果在一起分享.
楼主，我也正在弄呢，我的毕业设计就是弄这个，用MPU6050来导航定位，摸索中啊，貌似要各种滤波，还有数学空间算法，我的程序还没怎么弄呢，一直在查资料、论文什么的，楼主到什么程度了呀？ 快乐小猪头爸爸 发表于 2012-12-28 15:30 static/image/common/back.gif
一直有同样的想法,用MPU6050做套惯性导航,在室内用.目前再找资料,不过一直没理出头绪.等有结果在一起分享.
...
同样遇到问题讨论MPU6050加速度的位移希望我们研究位移的可以能一起探讨！ Einstein2012 发表于 2013-2-5 18:23 static/image/common/back.gif
楼主，我也正在弄呢，我的毕业设计就是弄这个，用MPU6050来导航定位，摸索中啊，貌似要各种滤波，还有数学空 ...
我和你一样也是做毕业创作！讨论MPU6050加速度的位移希望我们研究位移的可以能一起探讨尽快得到答案！
页:
[1]
2