CW32L012/F030灵眸X1智能小车——用MPU6050做运动检测和姿态控制

一、MPU6050模块

MPU6050又叫六轴运动传感器，集成了三轴加速度计和三轴陀螺仪。它通常用于运动检测和姿态估计等应用，广泛应用于机器人、无人机、游戏控制器、智能手环等领域。以下是关于MPU6050的详细介绍。

主要特点

六轴传感器: MPU6050包含三轴加速度计和三轴陀螺仪，能够检测物体的加速度和角速度。

I2C通信: MPU6050使用I2C接口进行数据传输，与微控制器的连接非常简单。

内置温度传感器: 此模块内置温度传感器，可以在一些应用中提供环境温度数据。

小巧轻便: MPU6050体积小、重量轻，非常适合需要较小空间和低重量的应用。

可编程量程: 允许用户根据需求配置加速度计和陀螺仪的灵敏度，以适应不同的应用场景。

驱动方式介绍

复位MPU6050，让MPU6050内部的所有寄存器恢复默认值（向0X6B写入0x80）

设置电源管理寄存器位0x00，以唤醒MPU6050，进入正常工作状态（向0x6B写入0x00）

陀螺仪配置寄存器（0x1B）设置MPU6050陀螺仪传感器满量程范围，这里选择正负2000dps

加速度传感器配置寄存器（0x1C）这里选择正负2g

陀螺仪采样率，由采样率分频寄存器（0x19）控制；这里设置为50hz即输出频率=1KHz，SMPLRT_DIV=19

设置MPU6050的数字低通滤波器，因为配置为50hz，找一个接近值，所以配置为0x03,42hz

设置PLL，一般选择x轴陀螺PLL作为时钟源，以获得更高精度的时钟。（向0X6B写入0x01）

设置加速度与陀螺仪都工作（向0X6C写入0x00）

这里还有一个寄存器可以用来检测是否有mpu6050（当AD0接地时，向0x75读取数据则返回0x68;当AD0接VCC时,向0x75读取数据则返回0x69）

以上是初始化的部分，初始化完成之后开始读取数据。

读取温度的地址：

读取陀螺仪测量值（原始值）分别有X/Y/Z轴的数据

读取加速度计测量值（原始值）分别有X/Y/Z轴的数据

二、连接原理图

主控板上MPU6050接口如下

通过原理图可知MPU6050接到了PB6和PB7接口。

三、工程代码

将bsp_mpu6050.c与bsp_mpu6050.h拉入工程

在文件bsp_mpu6050.c中，编写如下代码。

/*#include "bsp_mpu6050.h"#include "stdio.h"/****************************************************************** * 函 数 名 称：MPU6050_GPIO_Init * 函 数 说 明：MPU6050的引脚初始化 * 函 数 形 参：无 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void MPU6050_GPIO_Init(void){    GPIO_InitTypeDef GPIO_InitStruct; // GPIO初始化结构体    RCC_MPU6050_ENABLE();        // 使能GPIO时钟    GPIO_InitStruct.Pins = GPIO_SCL|GPIO_SDA;             // GPIO引脚    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;           // 开漏输出    GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;              // 输出速度高    GPIO_Init(PORT_MPU6050, &GPIO_InitStruct);            // 初始化}/****************************************************************** * 函 数 名 称：IIC_Start * 函 数 说 明：IIC起始时序 * 函 数 形 参：无 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void IIC_Start(void){        SDA_OUT();        SCL(1);        SDA(0);        SDA(1);        delay_us(5);        SDA(0);        delay_us(5);        SCL(0);}/****************************************************************** * 函 数 名 称：IIC_Stop * 函 数 说 明：IIC停止信号 * 函 数 形 参：无 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void IIC_Stop(void){        SDA_OUT();        SCL(0);        SDA(0);        SCL(1);        delay_us(5);        SDA(1);        delay_us(5);}/****************************************************************** * 函 数 名 称：IIC_Send_Ack * 函 数 说 明：主机发送应答或者非应答信号 * 函 数 形 参：0发送应答  1发送非应答 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void IIC_Send_Ack(unsigned char ack){        SDA_OUT();        SCL(0);        SDA(0);        delay_us(5);        if(!ack) SDA(0);        else         SDA(1);        SCL(1);        delay_us(5);        SCL(0);        SDA(1);}/****************************************************************** * 函 数 名 称：I2C_WaitAck * 函 数 说 明：等待从机应答 * 函 数 形 参：无 * 函 数 返 回：0有应答  1超时无应答 * 作       者：LC * 备       注：无******************************************************************/unsigned char I2C_WaitAck(void){        char ack = 0;        unsigned char ack_flag = 10;        SCL(0);        SDA(1);        SDA_IN();        SCL(1);        while( (SDA_GET()==1) && ( ack_flag ) )        {                ack_flag--;                delay_us(5);        }        if( ack_flag <= 0 )        {                IIC_Stop();                return 1;        }        else        {                SCL(0);                SDA_OUT();        }        return ack;}/****************************************************************** * 函 数 名 称：Send_Byte * 函 数 说 明：写入一个字节 * 函 数 形 参：dat要写人的数据 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void Send_Byte(uint8_t dat){        int i = 0;        SDA_OUT();        SCL(0);//拉低时钟开始数据传输        for( i = 0; i < 8; i++ )        {                SDA( (dat & 0x80) >> 7 );                delay_us(1);                SCL(1);                delay_us(5);                SCL(0);                delay_us(5);                dat<<=1;        }}/****************************************************************** * 函 数 名 称：Read_Byte * 函 数 说 明：IIC读时序 * 函 数 形 参：无 * 函 数 返 回：读到的数据 * 作       者：LC * 备       注：无******************************************************************/unsigned char Read_Byte(void){        unsigned char i,receive=0;        SDA_IN();//SDA设置为输入    for(i=0;i<8;i++ )        {        SCL(0);        delay_us(5);        SCL(1);        delay_us(5);        receive<<=1;        if( SDA_GET() )        {            receive|=1;        }        delay_us(5);    }        SCL(0);  return receive;}/****************************************************************** * 函 数 名 称：MPU6050_WriteReg * 函 数 说 明：IIC连续写入数据 * 函 数 形 参：addr器件地址 regaddr寄存器地址 num要写入的长度 regdata写入的数据地址 * 函 数 返 回：0=读取成功   其他=读取失败 * 作       者：LC * 备       注：无******************************************************************/char MPU6050_WriteReg(uint8_t addr,uint8_t regaddr,uint8_t num,uint8_t *regdata){    uint16_t i = 0;        IIC_Start();        Send_Byte((addr<<1)|0);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return 1;}        Send_Byte(regaddr);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return 2;}        for(i=0;i<num;i++)    {        Send_Byte(regdata[i]);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return (3+i);}    }        IIC_Stop();    return 0;}/****************************************************************** * 函 数 名 称：MPU6050_ReadData * 函 数 说 明：IIC连续读取数据 * 函 数 形 参：addr器件地址 regaddr寄存器地址 num要读取的长度 Read读取到的数据要存储的地址 * 函 数 返 回：0=读取成功   其他=读取失败 * 作       者：LC * 备       注：无******************************************************************/char MPU6050_ReadData(uint8_t addr, uint8_t regaddr,uint8_t num,uint8_t* Read){        uint8_t i;        IIC_Start();        Send_Byte((addr<<1)|0);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return 1;}        Send_Byte(regaddr);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return 2;}        IIC_Start();        Send_Byte((addr<<1)|1);        if( I2C_WaitAck() == 1 ) {IIC_Stop();return 3;}        for(i=0;i<(num-1);i++){                Read[i]=Read_Byte();                IIC_Send_Ack(0);        }        Read[i]=Read_Byte();        IIC_Send_Ack(1);        IIC_Stop();        return 0;}/****************************************************************** * 函 数 名 称：MPU_Set_Gyro_Fsr * 函 数 说 明：设置MPU6050陀螺仪传感器满量程范围 * 函 数 形 参：fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps * 函 数 返 回：0,设置成功  其他,设置失败 * 作       者：LC * 备       注：无******************************************************************/uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr){        return MPU6050_WriteReg(0x68,MPU_GYRO_CFG_REG,1,(uint8_t*)(fsr<<3)); //设置陀螺仪满量程范围}/****************************************************************** * 函 数 名 称：MPU_Set_Accel_Fsr * 函 数 说 明：设置MPU6050加速度传感器满量程范围 * 函 数 形 参：fsr:0,±2g;1,±4g;2,±8g;3,±16g * 函 数 返 回：0,设置成功  其他,设置失败 * 作       者：LC * 备       注：无******************************************************************/uint8_t MPU_Set_Accel_Fsr(uint8_t fsr){        return MPU6050_WriteReg(0x68,MPU_ACCEL_CFG_REG,1,(uint8_t*)(fsr<<3)); //设置加速度传感器满量程范围}/****************************************************************** * 函 数 名 称：MPU_Set_LPF * 函 数 说 明：设置MPU6050的数字低通滤波器 * 函 数 形 参：lpf:数字低通滤波频率(Hz) * 函 数 返 回：0,设置成功  其他,设置失败 * 作       者：LC * 备       注：无******************************************************************/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 data=MPU6050_WriteReg(0x68,MPU_CFG_REG,1,&data);//设置数字低通滤波器}/****************************************************************** * 函 数 名 称：MPU_Set_Rate * 函 数 说 明：设置MPU6050的采样率(假定Fs=1KHz) * 函 数 形 参：rate:4~1000(Hz)  初始化中rate取50 * 函 数 返 回：0,设置成功  其他,设置失败 * 作       者：LC * 备       注：无******************************************************************/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=MPU6050_WriteReg(0x68,MPU_SAMPLE_RATE_REG,1,&data);        //设置数字低通滤波器         return MPU_Set_LPF(rate/2);            //自动设置LPF为采样率的一半}/****************************************************************** * 函 数 名 称：MPU6050ReadGyro * 函 数 说 明：读取陀螺仪数据 * 函 数 形 参：陀螺仪数据存储地址 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void MPU6050ReadGyro(short *gyroData){        uint8_t buf[6];        uint8_t reg = 0;        //MPU6050_GYRO_OUT = MPU6050陀螺仪数据寄存器地址        //陀螺仪数据输出寄存器总共由6个寄存器组成，        //输出X/Y/Z三个轴的陀螺仪传感器数据，高字节在前，低字节在后。        //每一个轴16位，按顺序为xyz        reg = MPU6050_ReadData(0x68,MPU6050_GYRO_OUT,6,buf);        if( reg == 0 )        {                gyroData[0] = (buf[0] << 8) | buf[1];                gyroData[1] = (buf[2] << 8) | buf[3];                gyroData[2] = (buf[4] << 8) | buf[5];        }}/****************************************************************** * 函 数 名 称：MPU6050ReadAcc * 函 数 说 明：读取加速度数据 * 函 数 形 参：加速度数据存储地址 * 函 数 返 回：无 * 作       者：LC * 备       注：无******************************************************************/void MPU6050ReadAcc(short *accData){        uint8_t buf[6];        uint8_t reg = 0;        //MPU6050_ACC_OUT = MPU6050加速度数据寄存器地址        //加速度传感器数据输出寄存器总共由6个寄存器组成，        //输出X/Y/Z三个轴的加速度传感器值，高字节在前，低字节在后。        reg = MPU6050_ReadData(0x68, MPU6050_ACC_OUT, 6, buf);        if( reg == 0)        {                accData[0] = (buf[0] << 8) | buf[1];                accData[1] = (buf[2] << 8) | buf[3];                accData[2] = (buf[4] << 8) | buf[5];        }}/****************************************************************** * 函 数 名 称：MPU6050_GetTemp * 函 数 说 明：读取MPU6050上的温度 * 函 数 形 参：无 * 函 数 返 回：温度值单位为℃ * 作       者：LC * 备       注：温度换算公式为：Temperature = 36.53 + regval/340******************************************************************/float MPU6050_GetTemp(void){        short temp3;        uint8_t buf[2];        float Temperature = 0;        MPU6050_ReadData(0x68,MPU6050_RA_TEMP_OUT_H,2,buf);    temp3= (buf[0] << 8) | buf[1];        Temperature=((double) temp3/340.0)+36.53;    return Temperature;}/****************************************************************** * 函 数 名 称：MPU6050ReadID * 函 数 说 明：读取MPU6050的器件地址 * 函 数 形 参：无 * 函 数 返 回：0=检测不到MPU6050   1=能检测到MPU6050 * 作       者：LC * 备       注：无******************************************************************/uint8_t MPU6050ReadID(void){        unsigned char Re[2] = {0};        //器件ID寄存器 = 0x75        printf("mpu=%drn",MPU6050_ReadData(0x68,0X75,1,Re)); //读器件地址        if (Re[0] != 0x68)        {                        printf("检测不到 MPU6050 模块");                        return 1;         }        else        {                        printf("MPU6050 ID = %xrn",Re[0]);                        return 0;        }}/****************************************************************** * 函 数 名 称：MPU6050_Init * 函 数 说 明：MPU6050初始化 * 函 数 形 参：无 * 函 数 返 回：0成功  1没有检测到MPU6050 * 作       者：LC * 备       注：无******************************************************************/char MPU6050_Init(void){    MPU6050_GPIO_Init();    delay_ms(10);    //复位6050    MPU6050_WriteReg(0x68,MPU6050_RA_PWR_MGMT_1, 1,(uint8_t*)(0x80));    delay_ms(100);    //电源管理寄存器    //选择X轴陀螺作为参考PLL的时钟源，设置CLKSEL=001    MPU6050_WriteReg(0x68,MPU6050_RA_PWR_MGMT_1,1, (uint8_t*)(0x00));    MPU_Set_Gyro_Fsr(3);    //陀螺仪传感器,±2000dps    MPU_Set_Accel_Fsr(0);   //加速度传感器,±2g    MPU_Set_Rate(50);    MPU6050_WriteReg(0x68,MPU_INT_EN_REG , 1,(uint8_t*)0x00);        //关闭所有中断    MPU6050_WriteReg(0x68,MPU_USER_CTRL_REG,1,(uint8_t*)0x00);        //I2C主模式关闭    MPU6050_WriteReg(0x68,MPU_FIFO_EN_REG,1,(uint8_t*)0x00);                //关闭FIFO    MPU6050_WriteReg(0x68,MPU_INTBP_CFG_REG,1,(uint8_t*)0X80);        //INT引脚低电平有效    if( MPU6050ReadID() == 0 )//检查是否有6050    {            MPU6050_WriteReg(0x68,MPU6050_RA_PWR_MGMT_1, 1,(uint8_t*)0x01);//设置CLKSEL,PLL X轴为参考            MPU6050_WriteReg(0x68,MPU_PWR_MGMT2_REG, 1,(uint8_t*)0x00);//加速度与陀螺仪都工作            MPU_Set_Rate(50);            return 1;    }    return 0;}

在文件bsp_mpu6050.h中，编写如下代码。

/*#ifndef _BSP_MPU6050_H_#define _BSP_MPU6050_H_#include "board.h"//端口移植#define RCC_MPU6050_ENABLE() __RCC_GPIOB_CLK_ENABLE()#define PORT_MPU6050         CW_GPIOB#define GPIO_SDA             GPIO_PIN_7#define GPIO_SCL             GPIO_PIN_6//设置SDA输出模式#define SDA_OUT()   {                                GPIO_InitTypeDef GPIO_InitStruct;                                        GPIO_InitStruct.Pins = GPIO_SDA;                                         GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;                              GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;                                 GPIO_Init(PORT_MPU6050, &GPIO_InitStruct);                            }//设置SDA输入模式#define SDA_IN()    {                                GPIO_InitTypeDef GPIO_InitStruct;                                        GPIO_InitStruct.Pins = GPIO_SDA;                                         GPIO_InitStruct.Mode = GPIO_MODE_INPUT_PULLUP;                           GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;                                 GPIO_Init(PORT_MPU6050, &GPIO_InitStruct);                           }//获取SDA引脚的电平变化#define SDA_GET()       GPIO_ReadPin(PORT_MPU6050, GPIO_SDA)//SDA与SCL输出#define SDA(x)          GPIO_WritePin(PORT_MPU6050, GPIO_SDA, (x?GPIO_Pin_SET:GPIO_Pin_RESET) )#define SCL(x)          GPIO_WritePin(PORT_MPU6050, GPIO_SCL, (x?GPIO_Pin_SET:GPIO_Pin_RESET) )//MPU6050的AD0是IIC地址引脚，接地则IIC地址为0x68,接VCC则IIC地址为0x69#define MPU6050_RA_SMPLRT_DIV       0x19        //陀螺仪采样率 地址#define MPU6050_RA_CONFIG           0x1A        //设置数字低通滤波器 地址#define MPU6050_RA_GYRO_CONFIG      0x1B        //陀螺仪配置寄存器#define MPU6050_RA_ACCEL_CONFIG     0x1C        //加速度传感器配置寄存器#define MPU_INT_EN_REG              0X38        //中断使能寄存器#define MPU_USER_CTRL_REG           0X6A        //用户控制寄存器#define MPU_FIFO_EN_REG             0X23        //FIFO使能寄存器#define MPU_PWR_MGMT2_REG           0X6C        //电源管理寄存器2#define MPU_GYRO_CFG_REG            0X1B        //陀螺仪配置寄存器#define MPU_ACCEL_CFG_REG           0X1C        //加速度计配置寄存器#define MPU_CFG_REG                 0X1A        //配置寄存器#define MPU_SAMPLE_RATE_REG         0X19        //采样频率分频器#define MPU_INTBP_CFG_REG           0X37        //中断/旁路设置寄存器#define MPU6050_RA_PWR_MGMT_1       0x6B#define MPU6050_RA_PWR_MGMT_2       0x6C#define MPU6050_WHO_AM_I            0x75#define MPU6050_SMPLRT_DIV          0            //8000Hz  #define MPU6050_DLPF_CFG            0        #define MPU6050_GYRO_OUT            0x43         //MPU6050陀螺仪数据寄存器地址#define MPU6050_ACC_OUT             0x3B         //MPU6050加速度数据寄存器地址
#define MPU6050_RA_TEMP_OUT_H       0x41        //温度高位#define MPU6050_RA_TEMP_OUT_L       0x42        //温度低位#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位寄存器char MPU6050_WriteReg(uint8_t addr,uint8_t regaddr,uint8_t num,uint8_t *regdata);char MPU6050_ReadData(uint8_t addr, uint8_t regaddr,uint8_t num,uint8_t* Read);char MPU6050_Init(void);void MPU6050ReadGyro(short *gyroData);void MPU6050ReadAcc(short *accData);float MPU6050_GetTemp(void);uint8_t MPU6050ReadID(void);#endif

移植完成以上文件后，只是完成了获取陀螺仪和加速度的原始数据，我们是希望获取到角度数据。

因为MPU6050内部带有DMP处理单元，加上官方提供了比较完整的运动处理驱动库，大大降低了我们的编程和对数据的处理难度。我们可以将各个运动的参数计算，如旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据，通过调用运动处理驱动库函数，直接读取出数据来。

这里提供官方的运动处理驱动库，分别需要用到“inv_mpu.h”、“inv_mpu.c”、“dmpKey.h”、“dmpmap.h“、“inv_mpu_dmp_motion_driver.h”、“inv_mpu_dmp_motion_driver.c”等六个文件。已经移植完成并适配开发板的官方库文件，见下方的文件下载。

下载链接

链接：https://pan.baidu.com/s/1zNmYa1-i6YtL5Wi0xuTtxA?pwd=LCKF 提取码：LCKF

下载完成之后，复制到bsp文件夹下

然后导入工程即可。

随后在main函数中编写如下代码

float pitch=0,roll=0,yaw=0;   //欧拉角int main(void){        Uart_Init();//UART初始化        MPU6050_Init();//mpu6050初始化        mpu_dmp_init();//dmp自检        printf("HELLOrn");        while(1)        {                //获取欧拉角        if( mpu_dmp_get_data(&pitch,&roll,&yaw) == 0 )        {             printf("rnpitch =%.2frn", pitch);//获取翻滚角            printf("rnroll =%.2frn", roll);//获取俯仰角            printf("rnyaw =%.2frn", yaw);//获取偏航角        }              Delay_ms(200);//根据设置的采样率，不可设置延时过大        }}

四、工作现象

先将烧录器或者USB转TTL的TX接到PA3，RX接到PA2，然后打开串口助手，波特率为9600，随后将本次实验代码烧录进开发板，即可观察到串口打印出来的角度信息，

扫码加入QQ群，3群| 610403240