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即输出频率1KHzSMPLRT_DIV19设置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); dat1; } } /****************************************************************** * 函 数 名 称Read_Byte * 函 数 说 明IIC读时序 * 函 数 形 参无 * 函 数 返 回读到的数据 * 作 者LC * 备 注无 ******************************************************************/ unsigned char Read_Byte(void) { unsigned char i,receive0; SDA_IN();//SDA设置为输入 for(i0;i8;i ) { SCL(0); delay_us(5); SCL(1); delay_us(5); receive1; 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((addr1)|0); if( I2C_WaitAck() 1 ) {IIC_Stop();return 1;} Send_Byte(regaddr); if( I2C_WaitAck() 1 ) {IIC_Stop();return 2;} for(i0;inum;i) { Send_Byte(regdata[i]); if( I2C_WaitAck() 1 ) {IIC_Stop();return (3i);} } 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((addr1)|0); if( I2C_WaitAck() 1 ) {IIC_Stop();return 1;} Send_Byte(regaddr); if( I2C_WaitAck() 1 ) {IIC_Stop();return 2;} IIC_Start(); Send_Byte((addr1)|1); if( I2C_WaitAck() 1 ) {IIC_Stop();return 3;} for(i0;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*)(fsr3)); //设置陀螺仪满量程范围 } /****************************************************************** * 函 数 名 称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*)(fsr3)); //设置加速度传感器满量程范围 } /****************************************************************** * 函 数 名 称MPU_Set_LPF * 函 数 说 明设置MPU6050的数字低通滤波器 * 函 数 形 参lpf:数字低通滤波频率(Hz) * 函 数 返 回0,设置成功 其他,设置失败 * 作 者LC * 备 注无 ******************************************************************/ uint8_t MPU_Set_LPF(uint16_t lpf) { uint8_t data0; if(lpf188)data1; else if(lpf98)data2; else if(lpf42)data3; else if(lpf20)data4; else if(lpf10)data5; else data6; return dataMPU6050_WriteReg(0x68,MPU_CFG_REG,1,data);//设置数字低通滤波器 } /****************************************************************** * 函 数 名 称MPU_Set_Rate * 函 数 说 明设置MPU6050的采样率(假定Fs1KHz) * 函 数 形 参rate:4~1000(Hz) 初始化中rate取50 * 函 数 返 回0,设置成功 其他,设置失败 * 作 者LC * 备 注无 ******************************************************************/ uint8_t MPU_Set_Rate(uint16_t rate) { uint8_t data; if(rate1000)rate1000; if(rate4)rate4; data1000/rate-1; dataMPU6050_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%d\r\n,MPU6050_ReadData(0x68,0X75,1,Re)); //读器件地址 if (Re[0] ! 0x68) { printf(检测不到 MPU6050 模块); return 1; } else { printf(MPU6050 ID %x\r\n,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的时钟源设置CLKSEL001 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?pwdLCKF 提取码LCKF 下载完成之后复制到bsp文件夹下 图片 然后导入工程即可。 随后在main函数中编写如下代码 float pitch0,roll0,yaw0; //欧拉角 int main(void) { Uart_Init();//UART初始化 MPU6050_Init();//mpu6050初始化 mpu_dmp_init();//dmp自检 printf(HELLO\r\n); while(1) { //获取欧拉角 if( mpu_dmp_get_data(pitch,roll,yaw) 0 ) { printf(\r\npitch %.2f\r\n, pitch);//获取翻滚角 printf(\r\nroll %.2f\r\n, roll);//获取俯仰角 printf(\r\nyaw %.2f\r\n, yaw);//获取偏航角 } Delay_ms(200);//根据设置的采样率不可设置延时过大 } }四、工作现象先将烧录器或者USB转TTL的TX接到PA3RX接到PA2然后打开串口助手波特率为9600随后将本次实验代码烧录进开发板即可观察到串口打印出来的角度信息如下