first commit
This commit is contained in:
167
MPU6050/APP/MPU6050.c
Normal file
167
MPU6050/APP/MPU6050.c
Normal file
@ -0,0 +1,167 @@
|
||||
#include "stm32f10x.h" // Device header
|
||||
#include "MPU6050_Reg.h"
|
||||
#include "MPU6050.h"
|
||||
#include "kalman.h"
|
||||
#include "alldata.h"
|
||||
|
||||
|
||||
#define MPU6050_ADDRESS 0xD0
|
||||
|
||||
|
||||
//<2F>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>0 <20><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
|
||||
uint8_t MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT)
|
||||
{
|
||||
uint32_t Timeout;
|
||||
Timeout = 10000;
|
||||
while (I2C_CheckEvent(I2Cx, I2C_EVENT) != SUCCESS)
|
||||
{
|
||||
Timeout --;
|
||||
if (Timeout == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
|
||||
{
|
||||
I2C_GenerateSTART(I2C1, ENABLE);
|
||||
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT);
|
||||
|
||||
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
|
||||
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
|
||||
|
||||
I2C_SendData(I2C1, RegAddress);
|
||||
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTING);
|
||||
|
||||
I2C_SendData(I2C1, Data);
|
||||
MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
|
||||
|
||||
I2C_GenerateSTOP(I2C1, ENABLE);
|
||||
}
|
||||
|
||||
uint8_t MPU6050_ReadReg(uint8_t RegAddress,uint8_t *Data)
|
||||
{
|
||||
I2C_GenerateSTART(I2C1, ENABLE);
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT) != 0) return 1;
|
||||
|
||||
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter);
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)!= 0) return 1;
|
||||
|
||||
I2C_SendData(I2C1, RegAddress);
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED)!= 0) return 1;
|
||||
|
||||
I2C_GenerateSTART(I2C1, ENABLE);
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)!= 0) return 1;
|
||||
|
||||
I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Receiver);
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)!= 0) return 1;
|
||||
|
||||
I2C_AcknowledgeConfig(I2C1, DISABLE);
|
||||
I2C_GenerateSTOP(I2C1, ENABLE);
|
||||
|
||||
if(MPU6050_WaitEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED)!= 0) return 1;
|
||||
*Data = I2C_ReceiveData(I2C1);
|
||||
|
||||
I2C_AcknowledgeConfig(I2C1,ENABLE);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void MPU6050_Init(void)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
I2C_InitTypeDef I2C_InitStructure;
|
||||
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//GPIOB
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
I2C_DeInit(I2C1);
|
||||
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
|
||||
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
|
||||
I2C_InitStructure.I2C_OwnAddress1 = 0; //I2C_OAR1
|
||||
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
|
||||
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||
I2C_InitStructure.I2C_ClockSpeed = 400000; //400KHz
|
||||
I2C_Init(I2C1, &I2C_InitStructure);
|
||||
I2C_Cmd(I2C1, ENABLE);
|
||||
|
||||
MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
|
||||
MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
|
||||
MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
|
||||
MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
|
||||
MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
|
||||
MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
|
||||
}
|
||||
|
||||
uint8_t MPU6050_GetID(void)
|
||||
{
|
||||
uint8_t data;
|
||||
MPU6050_ReadReg(MPU6050_WHO_AM_I,&data);
|
||||
return data;
|
||||
}
|
||||
|
||||
|
||||
uint8_t buffer[12];
|
||||
|
||||
uint8_t MPU6050_GetData()
|
||||
{
|
||||
for(u8 i=0;i<6;i++)
|
||||
{
|
||||
if(MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H + i,&buffer[i]) !=0) return 1;
|
||||
}
|
||||
for(u8 i=0;i<6;i++)
|
||||
{
|
||||
if(MPU6050_ReadReg(MPU6050_GYRO_XOUT_H + i,&buffer[6+i]) !=0) return 1;;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
_st_Mpu MPU6050; //MPU6050ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
|
||||
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
|
||||
int16_t MpuOffset[6] = {0}; //MPU6050<35><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
||||
|
||||
/* <20><>ȡMPU6050<35><30><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>˲<EFBFBD> */
|
||||
//<2F><><EFBFBD><EFBFBD>ֵ:0,<2C><><EFBFBD><EFBFBD>
|
||||
//<2F><><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
uint8_t MpuGetData(void)
|
||||
{
|
||||
uint8_t i;
|
||||
int res;
|
||||
|
||||
if( MPU6050_GetData() != 0) return 1;
|
||||
|
||||
for(i=0;i<6;i++)
|
||||
{
|
||||
pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i]; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16bit<69><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥˮƽУֵ */
|
||||
|
||||
|
||||
if(i < 3) /* <20>Ǽ<EFBFBD><C7BC>ٶȿ<D9B6><C8BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD> */
|
||||
{
|
||||
{
|
||||
static struct KalmanFilter EKF[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};
|
||||
kalmanfiter(&EKF[i],(float)pMpu[i]);
|
||||
pMpu[i] = (int16_t)EKF[i].Out;
|
||||
// printf("EKF:%f\r\n",EKF[i].Out);
|
||||
}
|
||||
}
|
||||
if(i > 2) /* <20><><EFBFBD>ٶ<EFBFBD>һ<EFBFBD><EFBFBD><D7BB><EFBFBD><EFBFBD>˲<EFBFBD> */
|
||||
{
|
||||
uint8_t k=i-3;
|
||||
const float factor = 0.15f;
|
||||
static float tBuff[3];
|
||||
|
||||
pMpu[i] = tBuff[k] = tBuff[k] * (1 - factor) + pMpu[i] * factor;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user