原理实现
采用MPU6050模块，实时获取飞行器的姿态，判断和中值姿态的差别，实现姿态纠正。本次使用一节互补滤波对原始值计算得到角度：Roll、Pitch。
MPU6050代码
采用的软件IIC，所以移植极为简单，直接把文件添加进工程，并且更改为自己使能的模拟IIC引脚即可。
#ifndef _mpu6050_i2c_H
#define _mpu6050_i2c_H
#include <main.h>
#include <inttypes.h>
#define IIC_WR 0 /* ???bit */
#define IIC_RD 1 /* ???bit */
void IIC_Start(void);
void IIC_Stop(void);
void IIC_Send_Byte(uint8_t _ucByte);
uint8_t IIC_Read_Byte(uint8_t ack);
uint8_t IIC_Wait_Ack(void);
void IIC_Ack(void);
void IIC_NAck(void);
uint8_t IIC_CheckDevice(uint8_t _Address);
void IIC_GPIO_Init(void);
#endif
#include "mpu6050_i2c.h"
#define GPIO_PORT_IIC GPIOA
#define RCC_IIC_ENABLE __HAL_RCC_GPIOA_CLK_ENABLE()
#define IIC_SCL_PIN GPIO_PIN_1
#define IIC_SDA_PIN GPIO_PIN_0
#if 1
#define IIC_SCL_1() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SCL_PIN, GPIO_PIN_SET) /* SCL = 1 */
#define IIC_SCL_0() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SCL_PIN, GPIO_PIN_RESET) /* SCL = 0 */
#define IIC_SDA_1() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SDA_PIN, GPIO_PIN_SET) /* SDA = 1 */
#define IIC_SDA_0() HAL_GPIO_WritePin(GPIO_PORT_IIC, IIC_SDA_PIN, GPIO_PIN_RESET) /* SDA = 0 */
#define IIC_SDA_READ() HAL_GPIO_ReadPin(GPIO_PORT_IIC, IIC_SDA_PIN)
#else
#define IIC_SCL_1() GPIO_PORT_IIC->BSRR = IIC_SCL_PIN /* SCL = 1 */
#define IIC_SCL_0() GPIO_PORT_IIC->BRR = IIC_SCL_PIN /* SCL = 0 */
#define IIC_SDA_1() GPIO_PORT_IIC->BSRR = IIC_SDA_PIN /* SDA = 1 */
#define IIC_SDA_0() GPIO_PORT_IIC->BRR = IIC_SDA_PIN /* SDA = 0 */
#define IIC_SDA_READ() ((GPIO_PORT_IIC->IDR & IIC_SDA_PIN) != 0)
#endif
void IIC_GPIO_Init(void);
/*
*********************************************************************************************************
* ? ? ?: IIC_Delay
* ????: IIC?????,??400KHz
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
static void IIC_Delay(void)
{
uint8_t i;
/*
???????????AX-Pro???????????
CPU??72MHz?,???Flash??, MDK?????
?????10?,SCL?? = 205KHz
?????7?,SCL?? = 347KHz, SCL?????1.5us,SCL?????2.87us
?????5?,SCL?? = 421KHz, SCL?????1.25us,SCL?????2.375us
IAR???????,?????7
*/
for (i = 0; i < 10; i++);
}
/*
*********************************************************************************************************
* ? ? ?: IIC_Start
* ????: CPU??IIC??????
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_Start(void)
{
/* ?SCL????,SDA?????????IIC?????? */
IIC_SDA_1();
IIC_SCL_1();
IIC_Delay();
IIC_SDA_0();
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
}
/*
*********************************************************************************************************
* ? ? ?: IIC_Start
* ????: CPU??IIC??????
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_Stop(void)
{
/* ?SCL????,SDA?????????IIC?????? */
IIC_SDA_0();
IIC_SCL_1();
IIC_Delay();
IIC_SDA_1();
}
/*
*********************************************************************************************************
* ? ? ?: IIC_SendByte
* ????: CPU?IIC??????8bit??
* ? ?:_ucByte : ???????
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_Send_Byte(uint8_t _ucByte)
{
uint8_t i;
/* ????????bit7 */
for (i = 0; i < 8; i++)
{
if (_ucByte & 0x80)
{
IIC_SDA_1();
}
else
{
IIC_SDA_0();
}
IIC_Delay();
IIC_SCL_1();
IIC_Delay();
IIC_SCL_0();
if (i == 7)
{
IIC_SDA_1(); // ????
}
_ucByte <<= 1; /* ????bit */
IIC_Delay();
}
}
/*
*********************************************************************************************************
* ? ? ?: IIC_ReadByte
* ????: CPU?IIC??????8bit??
* ? ?:?
* ? ? ?: ?????
*********************************************************************************************************
*/
uint8_t IIC_Read_Byte(uint8_t ack)
{
uint8_t i;
uint8_t value;
/* ???1?bit????bit7 */
value = 0;
for (i = 0; i < 8; i++)
{
value <<= 1;
IIC_SCL_1();
IIC_Delay();
if (IIC_SDA_READ())
{
value++;
}
IIC_SCL_0();
IIC_Delay();
}
if(ack==0)
IIC_NAck();
else
IIC_Ack();
return value;
}
/*
*********************************************************************************************************
* ? ? ?: IIC_WaitAck
* ????: CPU??????,??????ACK????
* ? ?:?
* ? ? ?: ??0??????,1???????
*********************************************************************************************************
*/
uint8_t IIC_Wait_Ack(void)
{
uint8_t re;
IIC_SDA_1(); /* CPU??SDA?? */
IIC_Delay();
IIC_SCL_1(); /* CPU??SCL = 1, ???????ACK?? */
IIC_Delay();
if (IIC_SDA_READ()) /* CPU??SDA???? */
{
re = 1;
}
else
{
re = 0;
}
IIC_SCL_0();
IIC_Delay();
return re;
}
/*
*********************************************************************************************************
* ? ? ?: IIC_Ack
* ????: CPU????ACK??
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_Ack(void)
{
IIC_SDA_0(); /* CPU??SDA = 0 */
IIC_Delay();
IIC_SCL_1(); /* CPU??1??? */
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
IIC_SDA_1(); /* CPU??SDA?? */
}
/*
*********************************************************************************************************
* ? ? ?: IIC_NAck
* ????: CPU??1?NACK??
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_NAck(void)
{
IIC_SDA_1(); /* CPU??SDA = 1 */
IIC_Delay();
IIC_SCL_1(); /* CPU??1??? */
IIC_Delay();
IIC_SCL_0();
IIC_Delay();
}
/*
*********************************************************************************************************
* ? ? ?: IIC_GPIO_Config
* ????: ??IIC???GPIO,????IO?????
* ? ?:?
* ? ? ?: ?
*********************************************************************************************************
*/
void IIC_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_IIC_ENABLE; /* ??GPIO?? */
GPIO_InitStructure.Pin = IIC_SCL_PIN | IIC_SDA_PIN;
GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_OD; /* ???? */
HAL_GPIO_Init(GPIO_PORT_IIC, &GPIO_InitStructure);
/* ???????, ??IIC????????????? */
IIC_Stop();
}
/*
*********************************************************************************************************
* ? ? ?: IIC_CheckDevice
* ????: ??IIC????,CPU???????,??????????????????
* ? ?:_Address:???IIC????
* ? ? ?: ??? 0 ????, ??1??????
*********************************************************************************************************
*/
uint8_t IIC_CheckDevice(uint8_t _Address)
{
uint8_t ucAck;
IIC_GPIO_Init(); /* ??GPIO */
IIC_Start(); /* ?????? */
/* ??????+????bit(0 = w, 1 = r) bit7 ?? */
IIC_Send_Byte(_Address|IIC_WR);
ucAck = IIC_Wait_Ack(); /* ?????ACK?? */
IIC_Stop(); /* ?????? */
return ucAck;
}
#ifndef __MPU6050_H
#define __MPU6050_H
#include "mpu6050_i2c.h"
#include "math.h"
#include "main.h"
#define delay_ms HAL_Delay
#define MPU_IIC_Init IIC_GPIO_Init
#define MPU_IIC_Start IIC_Start
#define MPU_IIC_Stop IIC_Stop
#define MPU_IIC_Send_Byte IIC_Send_Byte
#define MPU_IIC_Read_Byte IIC_Read_Byte
#define MPU_IIC_Wait_Ack IIC_Wait_Ack
//#define MPU_ACCEL_OFFS_REG 0X06 //accel_offs???,??????,????????
//#define MPU_PROD_ID_REG 0X0C //prod id???,?????????
#define MPU_SELF_TESTX_REG 0X0D //?????X
#define MPU_SELF_TESTY_REG 0X0E //?????Y
#define MPU_SELF_TESTZ_REG 0X0F //?????Z
#define MPU_SELF_TESTA_REG 0X10 //?????A
#define MPU_SAMPLE_RATE_REG 0X19 //???????
#define MPU_CFG_REG 0X1A //?????
#define MPU_GYRO_CFG_REG 0X1B //????????
#define MPU_ACCEL_CFG_REG 0X1C //?????????
#define MPU_MOTION_DET_REG 0X1F //???????????
#define MPU_FIFO_EN_REG 0X23 //FIFO?????
#define MPU_I2CMST_CTRL_REG 0X24 //IIC???????
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC??0???????
#define MPU_I2CSLV0_REG 0X26 //IIC??0???????
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC??0?????
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC??1???????
#define MPU_I2CSLV1_REG 0X29 //IIC??1???????
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC??1?????
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC??2???????
#define MPU_I2CSLV2_REG 0X2C //IIC??2???????
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC??2?????
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC??3???????
#define MPU_I2CSLV3_REG 0X2F //IIC??3???????
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC??3?????
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC??4???????
#define MPU_I2CSLV4_REG 0X32 //IIC??4???????
#define MPU_I2CSLV4_DO_REG 0X33 //IIC??4??????
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC??4?????
#define MPU_I2CSLV4_DI_REG 0X35 //IIC??4??????
#define MPU_I2CMST_STA_REG 0X36 //IIC???????
#define MPU_INTBP_CFG_REG 0X37 //??/???????
#define MPU_INT_EN_REG 0X38 //???????
#define MPU_INT_STA_REG 0X3A //???????
#define MPU_ACCEL_XOUTH_REG 0X3B //????,X??8????
#define MPU_ACCEL_XOUTL_REG 0X3C //????,X??8????
#define MPU_ACCEL_YOUTH_REG 0X3D //????,Y??8????
#define MPU_ACCEL_YOUTL_REG 0X3E //????,Y??8????
#define MPU_ACCEL_ZOUTH_REG 0X3F //????,Z??8????
#define MPU_ACCEL_ZOUTL_REG 0X40 //????,Z??8????
#define MPU_TEMP_OUTH_REG 0X41 //?????????
#define MPU_TEMP_OUTL_REG 0X42 //????8????
#define MPU_GYRO_XOUTH_REG 0X43 //????,X??8????
#define MPU_GYRO_XOUTL_REG 0X44 //????,X??8????
#define MPU_GYRO_YOUTH_REG 0X45 //????,Y??8????
#define MPU_GYRO_YOUTL_REG 0X46 //????,Y??8????
#define MPU_GYRO_ZOUTH_REG 0X47 //????,Z??8????
#define MPU_GYRO_ZOUTL_REG 0X48 //????,Z??8????
#define MPU_I2CSLV0_DO_REG 0X63 //IIC??0?????
#define MPU_I2CSLV1_DO_REG 0X64 //IIC??1?????
#define MPU_I2CSLV2_DO_REG 0X65 //IIC??2?????
#define MPU_I2CSLV3_DO_REG 0X66 //IIC??3?????
#define MPU_I2CMST_DELAY_REG 0X67 //IIC?????????
#define MPU_SIGPATH_RST_REG 0X68 //?????????
#define MPU_MDETECT_CTRL_REG 0X69 //?????????
#define MPU_USER_CTRL_REG 0X6A //???????
#define MPU_PWR_MGMT1_REG 0X6B //???????1
#define MPU_PWR_MGMT2_REG 0X6C //???????2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO????????
#define MPU_FIFO_CNTL_REG 0X73 //FIFO????????
#define MPU_FIFO_RW_REG 0X74 //FIFO?????
#define MPU_DEVICE_ID_REG 0X75 //??ID???
//??AD0?(9?)??,IIC???0X68(??????).
//???V3.3,?IIC???0X69(??????).
#define MPU_ADDR 0X68
//#define MPU_READ 0XD1
//#define MPU_WRITE 0XD0
uint8_t MPU_Init(void); //???MPU6050
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);//IIC???
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf); //IIC???
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data); //IIC?????
uint8_t MPU_Read_Byte(uint8_t reg); //IIC?????
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr);
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr);
uint8_t MPU_Set_LPF(uint16_t lpf);
uint8_t MPU_Set_Rate(uint16_t rate);
uint8_t MPU_Set_Fifo(uint8_t sens);
short MPU_Get_Temperature(void);
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az);
float yijiehubu_Roll(float angle__Roll1, float gyro__Roll);
float yijiehubu_Pitch(float angle_Pitch1, float gyro_Pitch);
#endif
#include "MPU6050.h"
// 一节互补滤波算法
float K1 =0.02;
float dt=0.01;
float angle_Roll,angle_Pitch;
float yijiehubu_Roll(float angle__Roll1, float gyro__Roll)
{
angle_Roll = K1 * angle__Roll1 + (1-K1) * (angle__Roll1 + gyro__Roll * dt);
return angle_Roll;
}
float yijiehubu_Pitch(float angle_Pitch1, float gyro_Pitch)
{
angle_Pitch = K1 * angle_Pitch1 + (1-K1) * (angle_Pitch1 + gyro_Pitch * dt);
return angle_Pitch;
}
uint8_t MPU_Init(void)
{
uint8_t res;
MPU_IIC_Init();//???IIC??
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //??MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //??MPU6050
MPU_Set_Gyro_Fsr(3); //??????,±2000dps
MPU_Set_Accel_Fsr(0); //??????,±2g
MPU_Set_Rate(50); //?????50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //??????
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C?????
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //??FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT???????
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//??ID??
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //??CLKSEL,PLL X????
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //??????????
MPU_Set_Rate(50); //??????50Hz
} else return 1;
return 0;
}
//??MPU6050???????????
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//???:0,????
// ??,????
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//??????????
}
//??MPU6050???????????
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//???:0,????
// ??,????
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//?????????????
}
//??MPU6050????????
//lpf:????????(Hz)
//???:0,????
// ??,????
uint8_t MPU_Set_LPF(uint16_t lpf)
{
uint8_t data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//?????????
}
//??MPU6050????(??Fs=1KHz)
//rate:4~1000(Hz)
//???:0,????
// ??,????
uint8_t MPU_Set_Rate(uint16_t rate)
{
uint8_t data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //?????????
return MPU_Set_LPF(rate/2); //????LPF???????
}
//?????
//???:???(???100?)
short MPU_Get_Temperature(void)
{
uint8_t buf[2];
short raw;
float temp;
MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
raw=((uint16_t)buf[0]<<8)|buf[1];
temp=36.53+((double)raw)/340;
return temp*100;;
}
//??????(???)
//gx,gy,gz:???x,y,z??????(???)
//???:0,??
// ??,????
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
uint8_t buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((uint16_t)buf[0]<<8)|buf[1];
*gy=((uint16_t)buf[2]<<8)|buf[3];
*gz=((uint16_t)buf[4]<<8)|buf[5];
}
return res;;
}
//??????(???)
//gx,gy,gz:???x,y,z??????(???)
//???:0,??
// ??,????
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
uint8_t buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((uint16_t)buf[0]<<8)|buf[1];
*ay=((uint16_t)buf[2]<<8)|buf[3];
*az=((uint16_t)buf[4]<<8)|buf[5];
}
return res;;
}
//IIC???
//addr:????
//reg:?????
//len:????
//buf:???
//???:0,??
// ??,????
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
uint8_t i;
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//??????+???
if(MPU_IIC_Wait_Ack()) //????
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //??????
MPU_IIC_Wait_Ack(); //????
for(i=0; i<len; i++)
{
MPU_IIC_Send_Byte(buf[i]); //????
if(MPU_IIC_Wait_Ack()) //??ACK
{
MPU_IIC_Stop();
return 1;
}
}
MPU_IIC_Stop();
return 0;
}
//IIC???
//addr:????
//reg:?????????
//len:??????
//buf:?????????
//???:0,??
// ??,????
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|0);//??????+???
if(MPU_IIC_Wait_Ack()) //????
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //??????
MPU_IIC_Wait_Ack(); //????
MPU_IIC_Start();
MPU_IIC_Send_Byte((addr<<1)|1);//??????+???
MPU_IIC_Wait_Ack(); //????
while(len)
{
if(len==1)*buf=MPU_IIC_Read_Byte(0);//???,??nACK
else *buf=MPU_IIC_Read_Byte(1); //???,??ACK
len--;
buf++;
}
MPU_IIC_Stop(); //????????
return 0;
}
//IIC?????
//reg:?????
//data:??
//???:0,??
// ??,????
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//??????+???
if(MPU_IIC_Wait_Ack()) //????
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Send_Byte(reg); //??????
MPU_IIC_Wait_Ack(); //????
MPU_IIC_Send_Byte(data);//????
if(MPU_IIC_Wait_Ack()) //??ACK
{
MPU_IIC_Stop();
return 1;
}
MPU_IIC_Stop();
return 0;
}
//IIC?????
//reg:?????
//???:?????
uint8_t MPU_Read_Byte(uint8_t reg)
{
uint8_t res;
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//??????+???
MPU_IIC_Wait_Ack(); //????
MPU_IIC_Send_Byte(reg); //??????
MPU_IIC_Wait_Ack(); //????
MPU_IIC_Start();
MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//??????+???
MPU_IIC_Wait_Ack(); //????
res=MPU_IIC_Read_Byte(0);//????,??nACK
MPU_IIC_Stop(); //????????
return res;
}
主函数代码
初始化：
short ax,ay,az,gx,gy,gz;
float Angle_Pitch,gyro_Pitch,Angle_Roll,gyro_Roll;
float Roll,Pitch;
MPU_Init();
获取角度代码：
MPU_Get_Accelerometer(&ax,&ay,&az);
MPU_Get_Gyroscope(&gx,&gy,&gz);
Angle_Roll=atan2(ay,az)*57.3;
gyro_Roll=gx/16.4;
Roll = yijiehubu_Roll( Angle_Roll, gyro_Roll);
Angle_Pitch=atan2(ax,az)*57.3;
gyro_Pitch=gy/16.4;
Pitch = yijiehubu_Pitch( Angle_Pitch, gyro_Pitch);
printf("Roll %.2f Pitch %.2f \r\n",Roll,Pitch);
HAL_Delay(10);
一开始是使用同一个一节互补滤波函数进行计算角度的，但是实际测试发现角度不准，于是采用了分别设置对应函数进行计算。
当一个角度翻转超过30°左右后会影响到另一个角度，目前还不清楚原因，但实际飞行器不可能偏转这么大角度，待后续测试再来更新结果。
5164
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
