摘要：本例子是采用stm32f103rbt6和mpu6050的通訊實驗，mpu6050是直接淘寶上買的GY-521子產品。
本例子是庫函數版本。
結構：
調用ISys.c裡的MPU6050_Configuration();函數來初始化以下所有，包含iic和mpu6050。
mpu6050.c 初始化mpu6050的各個參數，并且給出輸出加速度和陀螺儀資料的函數
iic_analog.c iic的模拟，初始化iic
mpuiic.c iic所用引腳的gpio初始化，以及各種操作函數
Kalman_Filter.c 卡爾曼濾波器
ISys.c 調用以上各個函數完成初始化
代碼如下：
mpu6050.h
#ifndef _MPU6050_h_
#define _MPU6050_h_
#define SMPLRT_DIV 0x19 //陀螺儀采樣率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通濾波頻率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺儀自檢及測量範圍，典型值：0x18(不自檢，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速計自檢、測量範圍及高通濾波頻率，典型值：0x01(不自檢，2G，5Hz)
#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_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 SLAVEADRESS 0xD0 //IIC寫入時的位址位元組資料，+1為讀取
//初始化MPU6050
extern void MPU6050_Inital(void);
//擷取加速度計的值
extern short getAccX(void);
extern short getAccY(void);
extern short getAccZ(void);
//擷取陀螺儀的值
extern short getGyroX(void);
extern short getGyroY(void);
extern short getGyroZ(void);
//擷取溫度
extern short getTemperature(void);
#endif
mpu6050.c
#include "stm32f10x.h"
#include "MPU6050.h"
#include "iic_analog.h"
#include "delay.h"
void delay_IIC( int ms );
void MPU6050_Inital(void)
{
delay_IIC( 100 );
Single_Write_IIC( SLAVEADRESS , PWR_MGMT_1 , 0x00 ); //解除休眠
delay_ms(1000); //延時，保證初始化成功
Single_Write_IIC( SLAVEADRESS , PWR_MGMT_1 , 0x03 ); //選時鐘
Single_Write_IIC( SLAVEADRESS , SMPLRT_DIV , 0x00 ); //陀螺儀采樣率，1khz貌似不錯
Single_Write_IIC( SLAVEADRESS , CONFIG , 0x03 ); //加速度44hz濾波，陀螺儀42hz濾波
Single_Write_IIC( SLAVEADRESS , GYRO_CONFIG , 0x18 ); //陀螺儀最大量程-+2000度每秒 靈敏度：16.4 LSB/(o/s)
Single_Write_IIC( SLAVEADRESS , ACCEL_CONFIG , 0x08 ); //加速度最大量程-+4g 靈敏度: 8192 LSB/g
delay_IIC( 100 );
}
short getAccX(void)
{
short AccX = 0;
char AccXH = 0, AccXL = 0;
AccXH = Single_Read_IIC( SLAVEADRESS , ACCEL_XOUT_H );
AccXL = Single_Read_IIC( SLAVEADRESS , ACCEL_XOUT_L );
AccX = (AccXH<<8)|AccXL;
return AccX;
}
short getAccY(void)
{
short AccY = 0;
char AccYH = 0 , AccYL = 0;
AccYH = Single_Read_IIC( SLAVEADRESS , ACCEL_YOUT_H );
AccYL = Single_Read_IIC( SLAVEADRESS , ACCEL_YOUT_L );
AccY = (AccYH<<8)|AccYL;
return AccY;
}
short getAccZ(void)
{
short AccZ = 0;
char AccZH = 0 , AccZL = 0;
AccZH = Single_Read_IIC( SLAVEADRESS , ACCEL_ZOUT_H );
AccZL = Single_Read_IIC( SLAVEADRESS , ACCEL_ZOUT_L );
AccZ = (AccZH<<8)|AccZL;
return AccZ;
}
short getGyroX(void)
{
short GyroX = 0;
char GyroXH = 0 , GyroXL = 0;
GyroXH = Single_Read_IIC( SLAVEADRESS , GYRO_XOUT_H );
GyroXL = Single_Read_IIC( SLAVEADRESS , GYRO_XOUT_H );
GyroX = (GyroXH<<8)|GyroXL;
return GyroX;
}
short getGyroY(void)
{
short GyroY = 0;
char GyroYH = 0 , GyroYL = 0;
GyroYH = Single_Read_IIC( SLAVEADRESS , GYRO_YOUT_H );
GyroYL = Single_Read_IIC( SLAVEADRESS , GYRO_YOUT_H );
GyroY = (GyroYH<<8)|GyroYL;
return GyroY;
}
short getGyroZ(void)
{
short GyroZ = 0;
char GyroZH = 0 , GyroZL = 0;
GyroZH = Single_Read_IIC( SLAVEADRESS , GYRO_ZOUT_H );
GyroZL = Single_Read_IIC( SLAVEADRESS , GYRO_ZOUT_H );
GyroZ = (GyroZH<<8)|GyroZL;
return GyroZ;
}
short getTemperature(void)
{
short temperature = 0;
char temperatureH = 0 , temperatureL = 0;
temperatureH = Single_Read_IIC( SLAVEADRESS , TEMP_OUT_H );
temperatureL = Single_Read_IIC( SLAVEADRESS , TEMP_OUT_L );
temperature = (temperatureH<<8)|temperatureL;
return temperature;
}
void delay_IIC( int ms )
{
int i,j;
for( i = 0 ; i < ms ; i++ )
{
for( j = 0 ; j < 30000 ; j++ );
}
}
因使用IIC通訊，是以要初始化IIC。
mpuiic.h
#ifndef __MPUIIC_H
#define __MPUIIC_H
#include "sys.h"
//IO方向設定
#define MPU_SDA_IN() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}
//IO操作函數
#define MPU_IIC_SCL PBout(10) //SCL
#define MPU_IIC_SDA PBout(11) //SDA
#define MPU_READ_SDA PBin(11) //輸入SDA
//IIC所有操作函數
void MPU_IIC_Delay(void); //MPU IIC延時函數
void MPU_IIC_Init(void); //初始化IIC的IO口
void MPU_IIC_Start(void); //發送IIC開始信号
void MPU_IIC_Stop(void); //發送IIC停止信号
void MPU_IIC_Send_Byte(u8 txd); //IIC發送一個位元組
u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC讀取一個位元組
u8 MPU_IIC_Wait_Ack(void); //IIC等待ACK信号
void MPU_IIC_Ack(void); //IIC發送ACK信号
void MPU_IIC_NAck(void); //IIC不發送ACK信号
void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data);
u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr);
#endif
mpuiic.c
#include "mpuiic.h"
#include "delay.h"
//MPU IIC 延時函數
void MPU_IIC_Delay(void)
{
delay_us(2);
}
//初始化IIC
void MPU_IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//先使能外設IO PORTB時鐘
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11; // 端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽輸出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度為50MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); //根據設定參數初始化GPIO
GPIO_SetBits(GPIOB,GPIO_Pin_10|GPIO_Pin_11); //PB10,PB11 輸出高
}
//産生IIC起始信号
void MPU_IIC_Start(void)
{
MPU_SDA_OUT();//sda線輸出
MPU_IIC_SDA=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low
MPU_IIC_Delay();
MPU_IIC_SCL=0;//鉗住I2C總線，準備發送或接收資料
}
//産生IIC停止信号
void MPU_IIC_Stop(void)
{
MPU_SDA_OUT();//sda線輸出
MPU_IIC_SCL=0;
MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_SDA=1;//發送I2C總線結束信号
MPU_IIC_Delay();
}
//等待應答信号到來
//傳回值：1，接收應答失敗
// 0，接收應答成功
u8 MPU_IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
MPU_SDA_IN(); //SDA設定為輸入
MPU_IIC_SDA=1;MPU_IIC_Delay();
MPU_IIC_SCL=1;MPU_IIC_Delay();
while(MPU_READ_SDA)
{
ucErrTime++;
if(ucErrTime>250)
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_SCL=0;//時鐘輸出0
return 0;
}
//産生ACK應答
void MPU_IIC_Ack(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//不産生ACK應答
void MPU_IIC_NAck(void)
{
MPU_IIC_SCL=0;
MPU_SDA_OUT();
MPU_IIC_SDA=1;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
}
//IIC發送一個位元組
//傳回從機有無應答
//1，有應答
//0，無應答
void MPU_IIC_Send_Byte(u8 txd)
{
u8 t;
MPU_SDA_OUT();
MPU_IIC_SCL=0;//拉低時鐘開始資料傳輸
for(t=0;t<8;t++)
{
MPU_IIC_SDA=(txd&0x80)>>7;
txd<<=1;
MPU_IIC_SCL=1;
MPU_IIC_Delay();
MPU_IIC_SCL=0;
MPU_IIC_Delay();
}
}
//讀1個位元組，ack=1時，發送ACK，ack=0，發送nACK
u8 MPU_IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
MPU_SDA_IN();//SDA設定為輸入
for(i=0;i<8;i++ )
{
MPU_IIC_SCL=0;
MPU_IIC_Delay();
MPU_IIC_SCL=1;
receive<<=1;
if(MPU_READ_SDA)receive++;
MPU_IIC_Delay();
}
if (!ack)
MPU_IIC_NAck();//發送nACK
else
MPU_IIC_Ack(); //發送ACK
return receive;
}
iic_analog.h
#ifndef _iic_analog_h_
#define _iic_analog_h_
#include "stm32f10x.h"
#include "sys.h"
/*********************************************************************************/
/*修改模拟IIC的讀取引腳以及引腳的端口号 */
/*這些宏定義定義好了以後引腳初始化函數會自行初始化時鐘使能等必要的參數 */
/*********************************************************************************/
#define IIC_GPIO (GPIOB)
#define IIC_GOIO_SDA (GPIOB)
#define IIC_GPIO_SCL (GPIOB)
#define IIC_SDA (GPIO_Pin_7)
#define IIC_SCL (GPIO_Pin_6)
/*********************************************************************************/
/************************************************************************************/
/*使用此函數 初始化 模拟IIC 其中參數 在以上宏定義中 有定義 使用時隻需要修改宏定義即可 */
/* 此函數調用時 請複制以下代碼
IIC_GPIO_Configuration( IIC_GOIO_SDA , IIC_SDA , IIC_GPIO_SCL , IIC_SCL ); */
/************************************************************************************/
//IIC 引腳配置
extern void IIC_GPIO_Configuration( GPIO_TypeDef * GPIOx_SDA , uint16_t SDA_Pin , GPIO_TypeDef * GPIOx_SCL , uint16_t SCL_Pin );
/*********************************************************************************/
/*使用以下代碼時請勿修改 */
/* */
/*********************************************************************************/
//使用軟體模拟I2C
#define SET_SDA { GPIO_SetBits( IIC_GPIO , IIC_SDA ); }
#define RESET_SDA { GPIO_ResetBits( IIC_GPIO , IIC_SDA );}
#define SET_SCL { GPIO_SetBits( IIC_GPIO , IIC_SCL ); }
#define RESET_SCL { GPIO_ResetBits( IIC_GPIO , IIC_SCL); }
#define IIC_SDA_STATE (IIC_GPIO->IDR&IIC_SDA)
#define IIC_SCL_STATE (IIC_GPIO->IDR&IIC_SDA)
#define IIC_DELAY { IIC_Delay(); }
enum IIC_REPLAY_ENUM
{
IIC_NACK = 0,
IIC_ACK = 1
};
enum IIC_BUS_STATE_ENUM
{
IIC_BUS_READY = 0,
IIC_BUS_BUSY=1,
IIC_BUS_ERROR=2
};
//IIC 延時
extern void IIC_Delay(void);
//IIC 啟動函數
extern u8 IIC_Start(void);
//IIC 停止函數
extern void IIC_Stop(void);
//IIC 發送動作
extern void IIC_SendACK(void);
//IIC 停止動作
extern void IIC_SendNACK(void);
//IIC 發送單位元組
extern u8 IIC_SendByte(u8 Data);
//IIC 接收單位元組
extern u8 IIC_RecvByte(void);
//IIC 寫入單位元組
extern void Single_Write_IIC(u8 SlaveAddress,u8 REG_Address,u8 REG_data);
//IIC 讀取單位元組
extern u8 Single_Read_IIC(u8 SlaveAddress, u8 REG_Address);
//GPIO 過濾器
extern uint16_t GPIO_Filter( GPIO_TypeDef * GPIOx );
#endif
iic_analog.c
#include "stm32f10x.h"
#include "iic_analog.h"
/************************************************************/
/*模拟IIC引腳初始化函數*/
/************************************************************/
void IIC_GPIO_Configuration( GPIO_TypeDef * GPIOx_SDA , uint16_t SDA_Pin , GPIO_TypeDef * GPIOx_SCL , uint16_t SCL_Pin )
{
GPIO_InitTypeDef GPIO_InitStructure;
uint32_t RCC_GPIOx_SDA = 0;
uint32_t RCC_GPIOx_SCL = 0;
//得到濾波後的引腳端口
RCC_GPIOx_SDA = GPIO_Filter( GPIOx_SDA );
RCC_GPIOx_SCL = GPIO_Filter( GPIOx_SCL );
//使能時鐘
RCC_APB2PeriphClockCmd(RCC_GPIOx_SDA,ENABLE);
RCC_APB2PeriphClockCmd(RCC_GPIOx_SCL,ENABLE);
//配置引腳
GPIO_InitStructure.GPIO_Pin = SDA_Pin;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_OD;
GPIO_Init(GPIOx_SDA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = SCL_Pin;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_OD;
GPIO_Init(GPIOx_SCL, &GPIO_InitStructure);
//初始化ICC的模式
SET_SDA;
SET_SCL;
}
/************************************************************/
/************************************************************/
void IIC_Delay(void)
{
u32 i = 5;
while( i-- );
}
/*******************************************************************
TWI_START
發送啟動資料
*******************************************************************/
u8 IIC_Start(void)
{
SET_SDA;
IIC_DELAY;
SET_SCL;
IIC_DELAY;
if( IIC_SDA_STATE == RESET )
{
return IIC_BUS_BUSY;
}
RESET_SDA;
IIC_DELAY;
RESET_SCL;
IIC_DELAY;
if( IIC_SDA_STATE == SET )
{
return IIC_BUS_ERROR;
}
return IIC_BUS_READY;
}
/*******************************************************************
TWI_STOP
發送停止資料
*******************************************************************/
void IIC_Stop(void)
{
RESET_SDA;
IIC_DELAY;
SET_SCL;
IIC_DELAY;
SET_SDA;
IIC_DELAY;
}
/*******************************************************************************
* 函數名稱:TWI_SendNACK
* 描 述:收到資料,發送NACK
*******************************************************************************/
void IIC_SendNACK(void)
{
RESET_SDA;
IIC_DELAY;
SET_SCL;
IIC_DELAY;
RESET_SCL;
IIC_DELAY;
}
/*******************************************************************************
* 函數名稱:TWI_SendACK
* 描 述:收到資料,發送ACK
*******************************************************************************/
void IIC_SendACK(void)
{
SET_SDA;
IIC_DELAY;
SET_SCL;
IIC_DELAY;
RESET_SCL;
IIC_DELAY;
}
/*******************************************************************************
* 函數名稱:TWI_SendByte
* 描 述:發送一個位元組
*******************************************************************************/
u8 IIC_SendByte(u8 Data)
{
u8 i;
RESET_SCL;
for(i=0;i<8;i++)
{
//---------資料建立----------
if(Data&0x80)
{
SET_SDA;
}
else
{
RESET_SDA;
}
Data<<=1;
IIC_DELAY;
//---資料建立保持一定延時----
//----産生一個上升沿[正脈沖]
SET_SCL;
IIC_DELAY;
RESET_SCL;
IIC_DELAY;//延時,防止SCL還沒變成低時改變SDA,進而産生START/STOP信号
//---------------------------
}
//接收從機的應答
SET_SDA;
IIC_DELAY;
SET_SCL;
IIC_DELAY;
if(IIC_SDA_STATE)
{
RESET_SCL;
return IIC_NACK;
}
else
{
RESET_SCL;
return IIC_ACK;
}
}
/*******************************************************************************
* 函數名稱:TWI_ReceiveByte
* 描 述:接收一個位元組
*******************************************************************************/
u8 IIC_RecvByte(void)
{
u8 i,Dat = 0;
SET_SDA;
RESET_SCL;
Dat=0;
for(i=0;i<8;i++)
{
SET_SCL;//産生時鐘上升沿[正脈沖],讓從機準備好資料
IIC_DELAY;
Dat<<=1;
if(IIC_SDA_STATE) //讀引腳狀态
{
Dat|=0x01;
}
RESET_SCL;//準備好再次接收資料
IIC_DELAY;//等待資料準備好
}
return Dat;
}
/******單位元組寫入*******************************************/
void Single_Write_IIC(u8 SlaveAddress,u8 REG_Address,u8 REG_data)
{
IIC_Start(); //起始信号
IIC_SendByte(SlaveAddress); //發送裝置位址+寫信号
IIC_SendByte(REG_Address); //内部寄存器位址， //請參考中文pdf22頁
IIC_SendByte(REG_data); //内部寄存器資料， //請參考中文pdf22頁
IIC_Stop(); //發送停止信号
}
/********單位元組讀取*****************************************/
u8 Single_Read_IIC(u8 SlaveAddress, u8 REG_Address)
{
u8 REG_data;
IIC_Start(); //起始信号
IIC_SendByte(SlaveAddress); //發送裝置位址+寫信号
IIC_SendByte(REG_Address); //發送存儲單元位址，//從0開始
IIC_Start(); //起始信号
IIC_SendByte(SlaveAddress+1); //發送裝置位址+讀信号
REG_data = IIC_RecvByte(); //讀出寄存器資料
IIC_SendACK();
IIC_Stop(); //停止信号
return REG_data;
}
/*******************************************************************
引腳端口過濾器 傳回值為 引腳端口的時鐘編号
*******************************************************************/
uint16_t GPIO_Filter( GPIO_TypeDef * GPIOx )
{
uint32_t RCC_GPIOx = 0;
if( GPIOx == GPIOA )
{
RCC_GPIOx = RCC_APB2Periph_GPIOA;
}
else if( GPIOx == GPIOA )
{
RCC_GPIOx = RCC_APB2Periph_GPIOA;
}
else if( GPIOx == GPIOB )
{
RCC_GPIOx = RCC_APB2Periph_GPIOB;
}
else if( GPIOx == GPIOC )
{
RCC_GPIOx = RCC_APB2Periph_GPIOC;
}
else if( GPIOx == GPIOD )
{
RCC_GPIOx = RCC_APB2Periph_GPIOD;
}
else if( GPIOx == GPIOE )
{
RCC_GPIOx = RCC_APB2Periph_GPIOE;
}
else if( GPIOx == GPIOF )
{
RCC_GPIOx = RCC_APB2Periph_GPIOF;
}
else if( GPIOx == GPIOG )
{
RCC_GPIOx = RCC_APB2Periph_GPIOG;
}
return RCC_GPIOx;
}
Kalman_Filter.h
#ifndef _KALMAN_FILTER_H_
#define _KALMAN_FILTER_H_
float Kalman_Filter(float Accel,float Gyro);
#endif
Kalman_Filter.c
#include "Kalman_Filter.h"
float Angle = 0;
//******卡爾曼參數************
const float Q_angle=0.001;
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01; //dt為kalman濾波器采樣時間;
const char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
float Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先驗估計
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先驗估計誤差協方差微分的積分
PP[0][1] += Pdot[1] * dt; // =先驗估計誤差協方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先驗估計
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //後驗估計誤差協方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //後驗估計
Q_bias += K_1 * Angle_err; //後驗估計
Gyro = Gyro - Q_bias; //輸出值(後驗估計)的微分=角速度
return Angle;
}
ISys.h
#ifndef _ISys_h_
#define _ISys_h_
extern void MPU6050_Configuration(void);
#endif
ISys.c
#include "stm32f10x.h"
#include "ISys.h"
#include "iic_analog.h"
#include "MPU6050.h"
#include "delay.h"
void MPU6050_Configuration(void)
{
IIC_GPIO_Configuration( IIC_GOIO_SDA , IIC_SDA , IIC_GPIO_SCL , IIC_SCL );
delay_ms(300);
MPU6050_Inital();
}