本帖最后由 lycals 于 2012-11-20 16:57 编辑
我在淘宝上买了一个AHRS的板子，上面有STM32+mpu6050+HMC5883L+BMP180 组合，现在希望使用这个开发板去测算在运动时，其运动的方向及位移，想个人导航仪一样可以获得，在移动了一段时间可以测得，移动的距离是多少
[pre lang="arduino" line="1" file="MPU6050"]
#include "MPU6050.h"
#include "IOI2C.h"
#include "LED.h"
uint8_t buffer[14];
int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;
/**************************实现函数********************************************
*函数原型: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功 能: 将新的ADC数据更新到 FIFO数组，进行滤波处理
*******************************************************************************/
void MPU6050_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[0][i-1]=MPU6050_FIFO[0];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2];
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3];
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5];
}
MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面
MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx;
MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz;
sum=0;
for(i=0;i<10;i++){ //求当前数组的合，再取平均值
sum+=MPU6050_FIFO[0];
}
MPU6050_FIFO[0][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[1];
}
MPU6050_FIFO[1][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[2];
}
MPU6050_FIFO[2][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[3];
}
MPU6050_FIFO[3][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[4];
}
MPU6050_FIFO[4][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=MPU6050_FIFO[5];
}
MPU6050_FIFO[5][10]=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[0];
}
/**************************实现函数********************************************
*函数原型: 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[6];
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[0],&temp[1],&temp[2],&temp[3],&temp[4],&temp[5]);
}
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[0]) << 8) | buffer[1];
MPU6050_Lastay=(((int16_t)buffer[2]) << 8) | buffer[3];
MPU6050_Lastaz=(((int16_t)buffer[4]) << 8) | buffer[5];
//跳过温度ADC
MPU6050_Lastgx=(((int16_t)buffer[8]) << 8) | buffer[9];
MPU6050_Lastgy=(((int16_t)buffer[10]) << 8) | buffer[11];
MPU6050_Lastgz=(((int16_t)buffer[12]) << 8) | buffer[13];
MPU6050_newValues(MPU6050_Lastax,MPU6050_Lastay,MPU6050_Lastaz
,MPU6050_Lastgx,MPU6050_Lastgy,MPU6050_Lastgz);
*ax =MPU6050_FIFO[0][10];
*ay =MPU6050_FIFO[1][10];
*az = MPU6050_FIFO[2][10];
*gx =MPU6050_FIFO[3][10]-Gx_offset;
*gy = MPU6050_FIFO[4][10]-Gy_offset;
*gz = MPU6050_FIFO[5][10]-Gz_offset;
} else {
*ax = MPU6050_FIFO[0][10];//=MPU6050_FIFO[0][10];
*ay = MPU6050_FIFO[1][10];//=MPU6050_FIFO[1][10];
*az = MPU6050_FIFO[2][10];//=MPU6050_FIFO[2][10];
*gx = MPU6050_FIFO[3][10]-Gx_offset;//=MPU6050_FIFO[3][10];
*gy = MPU6050_FIFO[4][10]-Gy_offset;//=MPU6050_FIFO[4][10];
*gz = MPU6050_FIFO[5][10]-Gz_offset;//=MPU6050_FIFO[5][10];
}
}
void MPU6050_getlastMotion6(int16_t* ax, int16_t* ay,
int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
*ax =MPU6050_FIFO[0][10];
*ay =MPU6050_FIFO[1][10];
*az = MPU6050_FIFO[2][10];
*gx =MPU6050_FIFO[3][10]-Gx_offset;
*gy = MPU6050_FIFO[4][10]-Gy_offset;
*gz = MPU6050_FIFO[5][10]-Gz_offset;
}
/**************************实现函数********************************************
*函数原型: void MPU6050_InitGyro_Offset(void)
*功 能: 读取 MPU6050的陀螺仪偏置
此时模块应该被静止放置。以测试静止时的陀螺仪输出
*******************************************************************************/
void MPU6050_InitGyro_Offset(void)
{
unsigned char i;
int16_t temp[6];
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[0],&temp[1],&temp[2],&temp[3],&temp[4],&temp[5]);
LED_Change();
}
for(i=0;i<100;i++){
delay_us(200);
MPU6050_getMotion6(&temp[0],&temp[1],&temp[2],&temp[3],&temp[4],&temp[5]);
tempax+= temp[0];
tempay+= temp[1];
tempaz+= temp[2];
tempgx+= temp[3];
tempgy+= temp[4];
tempgz+= temp[5];
LED_Change();
}
Gx_offset=tempgx/100;//MPU6050_FIFO[3][10];
Gy_offset=tempgy/100;//MPU6050_FIFO[4][10];
Gz_offset=tempgz/100;//MPU6050_FIFO[5][10];
tempax/=100;
tempay/=100;
tempaz/=100;
}[/code]
[pre lang="arduino" line="1" file="HMC5883L"]
#include "HMC5883L.h"
float HMC5883_lastx,HMC5883_lasty,HMC5883_lastz;
int16_t HMC5883_FIFO[3][11]; //磁力计滤波
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[3];
unsigned char i;
for(i=0;i<50;i++){
HMC58X3_getRaw(&temp[0],&temp[1],&temp[2]);
delay_us(200); //延时再读取数据
LED_Change(); //LED闪烁
}
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_newValues(int16_t x,int16_t y,int16_t z)
*功 能: 更新一组数据到FIFO数组
输入参数： 磁力计三个轴对应的ADC值
输出参数： 无
*******************************************************************************/
void HMC58X3_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[0][i-1]=HMC5883_FIFO[0];
HMC5883_FIFO[1][i-1]=HMC5883_FIFO[1];
HMC5883_FIFO[2][i-1]=HMC5883_FIFO[2];
}
HMC5883_FIFO[0][9]=x;
HMC5883_FIFO[1][9]=y;
HMC5883_FIFO[2][9]=z;
sum=0;
for(i=0;i<10;i++){ //取数组内的值进行求和再取平均
sum+=HMC5883_FIFO[0];
}
HMC5883_FIFO[0][10]=sum/10; //将平均值更新
sum=0;
for(i=0;i<10;i++){
sum+=HMC5883_FIFO[1];
}
HMC5883_FIFO[1][10]=sum/10;
sum=0;
for(i=0;i<10;i++){
sum+=HMC5883_FIFO[2];
}
HMC5883_FIFO[2][10]=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[6];
vbuff[0]=vbuff[1]=vbuff[2]=vbuff[3]=vbuff[4]=vbuff[5]=0;
IICreadBytes(HMC58X3_ADDR,HMC58X3_R_XM,6,vbuff);
HMC58X3_newValues(((int16_t)vbuff[0] << 8) | vbuff[1],((int16_t)vbuff[4] << 8) | vbuff[5],((int16_t)vbuff[2] << 8) | vbuff[3]);
*x = HMC5883_FIFO[0][10];
*y = HMC5883_FIFO[1][10];
*z = HMC5883_FIFO[2][10];
}
/**************************实现函数********************************************
*函数原型: 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[0][10];
*y = HMC5883_FIFO[1][10];
*z = HMC5883_FIFO[2][10];
}
/**************************实现函数********************************************
*函数原型: void HMC58X3_mgetValues(float *arry)
*功 能: 读取 校正后的 磁力计ADC值
输入参数： 输出数组指针
输出参数： 无
*******************************************************************************/
void HMC58X3_mgetValues(float *arry) {
int16_t xr,yr,zr;
HMC58X3_getRaw(&xr, &yr, &zr);
arry[0]= HMC5883_lastx=(float)(xr);
arry[1]= HMC5883_lasty=(float)(yr);
arry[2]= 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[3])
*功 能: 读取芯片的ID
输入参数： ID存放的数组
输出参数： 无
*******************************************************************************/
void HMC58X3_getID(char id[3])
{
id[0]=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDA);
id[1]=I2C_ReadOneByte(HMC58X3_ADDR,HMC58X3_R_IDB);
id[2]=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();
}[/code]
|
发表于 2012-11-20 16:35:47
楼主
