133 lines
3.8 KiB
C
133 lines
3.8 KiB
C
#include "data.h"
|
||
#include "hal.h"
|
||
|
||
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
|
||
#define CONFIGL 0x1A //低通滤波频率，典型值：0x06(5Hz)
|
||
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
|
||
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
|
||
#define ACCEL_ADDRESS 0x3B
|
||
#define ACCEL_XOUT_H 0x3B
|
||
#define ACCEL_XOUT_L 0x3C
|
||
#define ACCEL_YOUT_H 0x3D
|
||
#define ACCEL_YOUT_L 0x3E
|
||
#define ACCEL_ZOUT_H 0x3F
|
||
#define ACCEL_ZOUT_L 0x40
|
||
#define TEMP_OUT_H 0x41
|
||
#define TEMP_OUT_L 0x42
|
||
#define GYRO_XOUT_H 0x43
|
||
#define GYRO_ADDRESS 0x43
|
||
#define GYRO_XOUT_L 0x44
|
||
#define GYRO_YOUT_H 0x45
|
||
#define GYRO_YOUT_L 0x46
|
||
#define GYRO_ZOUT_H 0x47
|
||
#define GYRO_ZOUT_L 0x48
|
||
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
|
||
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
|
||
#define MPU6050_PRODUCT_ID 0x68
|
||
#define MPU6052C_PRODUCT_ID 0x72
|
||
#define MPU9250_PRODUCT_ID 0x71 //
|
||
//#define MPU6050_is_DRY() GPIO_ReadOutBit(HT_GPIOC, GPIO_PIN_0)//IRQ主机数据输入
|
||
#ifdef USE_I2C_HARDWARE
|
||
|
||
#define MPU6050_ADDRESS 0xD0//0x68
|
||
#else
|
||
#define MPU6050_ADDRESS 0xD0 //IIC写入时的地址字节数据，+1为读取
|
||
#endif
|
||
|
||
|
||
void delay_ms(int x){
|
||
|
||
for(x = 0; x < 1000; x++){
|
||
volatile int z = 0;
|
||
for (z = 0; z < 100;z++){
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
int16_t MpuOffset[6] = {0};
|
||
|
||
_st_Mpu MPU6050; //MPU6050原始数据
|
||
|
||
static volatile int16_t *pMpu = (int16_t *)&MPU6050;
|
||
|
||
|
||
|
||
/****************************************************************************************
|
||
*@brief
|
||
*@brief
|
||
*@param[in]
|
||
*****************************************************************************************/
|
||
int8_t mpu6050_rest(void)
|
||
{
|
||
if(MPUWriteReg( PWR_MGMT_1, 0x80) == FAILED)
|
||
return FAILED; //复位
|
||
//delay_ms(20);
|
||
return SUCCESS;
|
||
}
|
||
/****************************************************************************************
|
||
*@brief
|
||
*@brief
|
||
*@param[in]
|
||
*****************************************************************************************/
|
||
int8_t MpuInit(void) //初始化
|
||
{
|
||
uint8_t date = SUCCESS;
|
||
do
|
||
{
|
||
date = MPUWriteReg( PWR_MGMT_1, 0x80); //复位
|
||
delay_ms(30);
|
||
date += MPUWriteReg( SMPLRT_DIV, 0x02); //陀螺仪采样率，0x00(500Hz)
|
||
date += MPUWriteReg( PWR_MGMT_1, 0x03); //设置设备时钟源，陀螺仪Z轴
|
||
date += MPUWriteReg( CONFIGL, 0x03); //低通滤波频率，0x03(42Hz)
|
||
date += MPUWriteReg( GYRO_CONFIG, 0x18);//+-2000deg/s
|
||
date += MPUWriteReg( ACCEL_CONFIG, 0x09);//+-4G
|
||
}
|
||
while(date != SUCCESS);
|
||
date = MPUReadReg(0x75);
|
||
if(date!= MPU6050_PRODUCT_ID)
|
||
return FAILED;
|
||
else
|
||
//MpuGetOffset();
|
||
return SUCCESS;
|
||
}
|
||
/****************************************************************************************
|
||
*@brief
|
||
*@brief
|
||
*@param[in]
|
||
*****************************************************************************************/
|
||
|
||
#define Gyro_Read() MPUReadBuf( 0X3B,buffer,6)
|
||
#define Acc_Read() MPUReadBuf( 0x43,&buffer[6],6)
|
||
|
||
void MpuGetData(void) //读取陀螺仪数据加滤波
|
||
{
|
||
uint8_t i;
|
||
int8_t buffer[12];
|
||
|
||
Gyro_Read();
|
||
Acc_Read();
|
||
|
||
for(i=0;i<6;i++)
|
||
{
|
||
pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];
|
||
if(i < 3)
|
||
{
|
||
{
|
||
//static struct _1_ekf_filter 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}};
|
||
//kalman_1(&ekf[i],(float)pMpu[i]); //一维卡尔曼
|
||
//pMpu[i] = (int16_t)ekf[i].out;
|
||
}
|
||
}
|
||
if(i > 2)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**************************************END OF FILE*************************************/ |