完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
1)实验平台:alientek 阿波罗 STM32F767 开发板
2)摘自《STM32F7 开发指南(HAL 库版)》关注官方微信号公众号,获取更多资料:正点原子 第三十七章 MPU6050 六轴传感器实验 本章,我们介绍当下最流行的一款六轴(三轴加速度+三轴角速度(陀螺仪))传感器: MPU6050,该传感器广泛用于四轴、平衡车和空中鼠标等设计,具有非常广泛的应用范围。 ALIENTEK 探索者 STM32F4 开发板自带了 MPU6050 传感器。本章我们将使用 STM32F4 来驱 动 MPU6050,读取其原始数据,并利用其自带的 DMP 实现姿态解算,结合匿名四轴上位机软 件和 LCD 显示,教大家如何使用这款功能强大的六轴传感器。本章分为如下几个部分: 37.1 MPU6050 简介 37.2 硬件设计 37.3 软件设计 37.4 下载验证 37.1 MPU6050 简介 本节,我们将分 2 个部分介绍:1,MPU6050 基础介绍。2,DMP 使用简介。 37.1.1 MPU6050 基础介绍 MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件,相较于多组件方 案,免除了组合陀螺仪与加速器时之轴间差的问题,减少了安装空间。 MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器,并且含有一个第二 IIC 接口,可用 于连接外部磁力传感器,并利用自带的数字运动处理器(DMP: Digital Motion Processor)硬件 加速引擎,通过主 IIC 接口,向应用端输出完整的 9 轴融合演算数据。有了 DMP,我们可以使 用 InvenSense 公司提供的运动处理资料库,非常方便的实现姿态解算,降低了运动处理运算对 操作系统的负荷,同时大大降低了开发难度。 MPU6050 的特点包括: ① 以数字形式输出 6 轴或 9 轴(需外接磁传感器)的旋转矩阵、四元数(quaternion)、欧 拉角格式(Euler Angle forma)的融合演算数据(需 DMP 支持) ② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec 的 3 轴角速度感测器(陀螺仪) ③ 集成可程序控制,范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器 ④ 移除加速器与陀螺仪轴间敏感度,降低设定给予的影响与感测器的飘移 ⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算 数据、感测器同步化、姿势感应等的负荷 ⑥ 内建运作时间偏差与磁力感测器校正演算技术,免除了客户须另外进行校正的需求 ⑦ 自带一个数字温度传感器 ⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS ⑨ 可程序控制的中断(interrupt),支持姿势识别、摇摄、画面放大缩小、滚动、快速下降 中断、high-G 中断、零动作感应、触击感应、摇动感应功能 ⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%;VLOGIC 可低至 1.8V± 5% ⑪ 陀螺仪工作电流:5mA,陀螺仪待机电流:5uA;加速器工作电流:500uA,加速器省 电模式电流:40uA@10Hz ⑫ 自带 1024 字节 FIFO,有助于降低系统功耗 ⑬ 高达 400Khz 的 IIC 通信接口 ⑭ 超小封装尺寸:4x4x0.9mm(QFN) MPU6050 传感器的检测轴如图 37.1.1.1 所示: 图 37.1.1.1 MPU6050 检测轴及其方向 MPU6050 的内部框图如图 37.1.1.2 所示: 图 37.1.1.2 MPU6050 框图 其中,SCL 和 SDA 是连接 MCU 的 IIC 接口,MCU 通过这个 IIC 接口来控制 MPU6050, 另外还有一个 IIC 接口:AUX_CL 和 AUX_DA,这个接口可用来连接外部从设备,比如磁传感 器,这样就可以组成一个九轴传感器。VLOGIC 是 IO 口电压,该引脚最低可以到 1.8V,我们 一般直接接 VDD 即可。AD0 是从 IIC 接口(接 MCU)的地址控制引脚,该引脚控制 IIC 地址 的最低位。如果接 GND,则 MPU6050 的 IIC 地址是:0X68,如果接 VDD,则是 0X69,注意: 这里的地址是不包含数据传输的最低位的(最低位用来表示读写)!! 在探索者 STM32F4 开发板上,AD0 是接 GND 的,所以 MPU6050 的 IIC 地址是 0X68(不 含最低位),IIC 通信的时序我们在之前已经介绍过(第二十九章,IIC 实验),这里就不再细说了。 接下来,我们介绍一下利用 STM32F4 读取 MPU6050 的加速度和角度传感器数据(非中 断方式),需要哪些初始化步骤: 1)初始化 IIC 接口 MPU6050 采用 IIC 与 STM32F4 通信,所以我们需要先初始化与 MPU6050 连接的 SDA 和 SCL 数据线。这个在前面的 IIC 实验章节已经介绍过了,这里 MPU6050 与 24C02 共用一个 IIC, 所以初始化 IIC 完全一模一样。 2)复位 MPU6050 这一步让 MPU6050 内部所有寄存器恢复默认值,通过对电源管理寄存器 1(0X6B)的 bit7 写 1 实现。 复位后,电源管理寄存器 1 恢复默认值(0X40),然后必须设置该寄存器为 0X00, 以唤醒 MPU6050,进入正常工作状态。 3)设置角速度传感器(陀螺仪)和加速度传感器的满量程范围 这一步,我们设置两个传感器的满量程范围(FSR),分别通过陀螺仪配置寄存器(0X1B) 和加速度传感器配置寄存器(0X1C)设置。我们一般设置陀螺仪的满量程范围为±2000dps, 加速度传感器的满量程范围为±2g。 4)设置其他参数 这里,我们还需要配置的参数有:关闭中断、关闭 AUX IIC 接口、禁止 FIFO、设置陀螺 仪采样率和设置数字低通滤波器(DLPF)等。本章我们不用中断方式读取数据,所以关闭中断, 然后也没用到 AUX IIC 接口外接其他传感器,所以也关闭这个接口。分别通过中断使能寄存器 (0X38)和用户控制寄存器(0X6A)控制。MPU6050 可以使用 FIFO 存储传感器数据,不过 本章我们没有用到,所以关闭所有 FIFO 通道,这个通过 FIFO 使能寄存器(0X23)控制,默 认都是 0(即禁止 FIFO),所以用默认值就可以了。陀螺仪采样率通过采样率分频寄存器(0X19) 控制,这个采样率我们一般设置为 50 即可。数字低通滤波器(DLPF)则通过配置寄存器(0X1A) 设置,一般设置 DLPF 为带宽的 1/2 即可。 5)配置系统时钟源并使能角速度传感器和加速度传感器 系统时钟源同样是通过电源管理寄存器 1(0X6B)来设置,该寄存器的最低三位用于设置 系统时钟源选择,默认值是 0(内部 8M RC 震荡),不过我们一般设置为 1,选择 x 轴陀螺 PLL 作为时钟源,以获得更高精度的时钟。同时,使能角速度传感器和加速度传感器,这两个操作 通过电源管理寄存器 2(0X6C)来设置,设置对应位为 0 即可开启。 至此,MPU6050 的初始化就完成了,可以正常工作了(其他未设置的寄存器全部采用默认 值即可),接下来,我们就可以读取相关寄存器,得到加速度传感器、角速度传感器和温度传感 器的数据了。不过,我们先简单介绍几个重要的寄存器。 首先,我们介绍电源管理寄存器 1,该寄存器地址为 0X6B,各位描述如图 37.1.1.3 所示: 图 37.1.1.3 电源管理寄存器 1 各位描述 其中,DEVICE_RESET 位用来控制复位,设置为 1,复位 MPU6050,复位结束后,MPU 硬件自动清零该位。SLEEEP 位用于控制 MPU6050 的工作模式,复位后,该位为 1,即进入了 睡眠模式(低功耗),所以我们要清零该位,以进入正常工作模式。TEMP_DIS 用于设置是否 使能温度传感器,设置为 0,则使能。最后 CLKSEL[2:0]用于选择系统时钟源,选择关系如表 37.1.1.1 所示: 图 37.1.1.1 CLKSEL 选择列表 默认是使用内部 8M RC 晶振的,精度不高,所以我们一般选择 X/Y/Z 轴陀螺作为参考的PLL 作为时钟源,一般设置 CLKSEL=001 即可。 接着,我们看陀螺仪配置寄存器,该寄存器地址为:0X1B,各位描述如图 37.1.4 所示: 图 37.1.1.4 陀螺仪配置寄存器各位描述 该寄存器我们只关心 FS_SEL[1:0]这两个位,用于设置陀螺仪的满量程范围: 0,±250°/S; 1,±500°/S;2,±1000°/S;3,±2000°/S;我们一般设置为 3,即±2000°/S,因为陀螺 仪的 ADC 为 16 位分辨率,所以得到灵敏度为:65536/4000=16.4LSB/(°/S)。 接下来,我们看加速度传感器配置寄存器,寄存器地址为:0X1C,各位描述如图 37.1.1.5 所示: 图 37.1.1.5 加速度传感器配置寄存器各位描述 该寄存器我们只关心 AFS_SEL[1:0]这两个位,用于设置加速度传感器的满量程范围:0, ±2g;1,±4g;2,±8g;3,±16g;我们一般设置为 0,即±2g,因为加速度传感器的 ADC 也是 16 位,所以得到灵敏度为:65536/4=16384LSB/g。 接下来,我看看 FIFO 使能寄存器,寄存器地址为:0X23,各位描述如图 37.1.1.6 所示: 图 37.1.1.6 FIFO 使能寄存器各位描述 该寄存器用于控制 FIFO 使能,在简单读取传感器数据的时候,可以不用 FIFO,设置对应 位为 0 即可禁止 FIFO,设置为 1,则使能 FIFO。注意加速度传感器的 3 个轴,全由 1 个位 (ACCEL_FIFO_EN)控制,只要该位置 1,则加速度传感器的三个通道都开启 FIFO 了。 接下来,我们看陀螺仪采样率分频寄存器,寄存器地址为:0X19,各位描述如图 37.1.1.7 所示: 图 37.1.1.7 陀螺仪采样率分频寄存器各位描述 该寄存器用于设置 MPU6050 的陀螺仪采样频率,计算公式为: 采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV) 这里陀螺仪的输出频率,是 1Khz 或者 8Khz,与数字低通滤波器(DLPF)的设置有关, 当 DLPF_CFG=0/7 的时候,频率为 8Khz,其他情况是 1Khz。而且 DLPF 滤波频率一般设置为 采样率的一半。采样率,我们假定设置为 50Hz,那么 SMPLRT_DIV=1000/50-1=19。 接下来,我们看配置寄存器,寄存器地址为:0X1A,各位描述如图 37.1.1.8 所示: 图 37.1.1.8 配置寄存器各位描述 这里,我们主要关心数字低通滤波器(DLPF)的设置位,即:DLPF_CFG[2:0],加速度计 和陀螺仪,都是根据这三个位的配置进行过滤的。DLPF_CFG 不同配置对应的过滤情况如表 37.1. 1. 2 所示: 图 37.1.1.2 DLPF_CFG 配置表 这里的加速度传感器,输出速率(Fs)固定是 1Khz,而角速度传感器的输出速率(Fs), 则根据 DLPF_CFG 的配置有所不同。一般我们设置角速度传感器的带宽为其采样率的一半,如 前面所说的,如果设置采样率为 50Hz,那么带宽就应该设置为 25Hz,取近似值 20Hz,就应该 设置 DLPF_CFG=100。 接下来,我们看电源管理寄存器 2,寄存器地址为:0X6C,各位描述如图 37.1.1.9 所示: 图 37.1.1.9 电源管理寄存器 2 各位描述 该寄存器的 LP_WAKE_CTRL 用于控制低功耗时的唤醒频率,本章用不到。剩下的 6 位, 分别控制加速度和陀螺仪的 x/y/z 轴是否进入待机模式,这里我们全部都不进入待机模式,所以 全部设置为 0 即可。 接下来,我们看看陀螺仪数据输出寄存器,总共有 6 个寄存器组成,地址为:0X43~0X48, 通过读取这 6 个寄存器,就可以读到陀螺仪 x/y/z 轴的值,比如 x 轴的数据,可以通过读取 0X43 (高 8 位)和 0X44(低 8 位)寄存器得到,其他轴以此类推。 同样,加速度传感器数据输出寄存器,也有 6 个,地址为:0X3B~0X40,通过读取这 6 个 寄存器,就可以读到加速度传感器 x/y/z 轴的值,比如读 x 轴的数据,可以通过读取 0X3B(高 8 位)和 0X3C(低 8 位)寄存器得到,其他轴以此类推。 最后,温度传感器的值,可以通过读取 0X41(高 8 位)和 0X42(低 8 位)寄存器得到, 温度换算公式为: Temperature = 36.53 + regval/340 其中,Temperature 为计算得到的温度值,单位为℃,regval 为从 0X41 和 0X42 读到的温度 传感器值。 关于 MPU6050 的基础介绍,我们就介绍到这。MPU6050 的详细资料和相关寄存器介绍, 请参考光盘:7,硬件资料→MPU6050 资料→MPU-6000 and MPU-6050 Product Specification.pdf 和 MPU-6000 and MPU-6050 Register Map and Descriptions.pdf 这两个文档,另外该目录还提供 了部分 MPU6050 的中文资料,供大家参考学习。 37.1.2 DMP 使用简介 经过 37.1.1 节的介绍,我们可以读出 MPU6050 的加速度传感器和角速度传感器的原始数 据。不过这些原始数据,对想搞四轴之类的初学者来说,用处不大,我们期望得到的是姿态数 据,也就是欧拉角:航向角(yaw)、横滚角(roll)和俯仰角(pitch)。有了这三个角,我们就 可以得到当前四轴的姿态,这才是我们想要的结果。 要得到欧拉角数据,就得利用我们的原始数据,进行姿态融合解算,这个比较复杂,知识 点比较多,初学者 不易掌握。而 MPU6050 自带了数字运动处理器,即 DMP,并且,InvenSense 提供了一个 MPU6050 的嵌入式运动驱动库,结合 MPU6050 的 DMP,可以将我们的原始数据, 直接转换成四元数输出,而得到四元数之后,就可以很方便的计算出欧拉角,从而得到 yaw、 roll 和 pitch。 使用内置的 DMP,大大简化了四轴的代码设计,且 MCU 不用进行姿态解算过程,大大降 低了 MCU 的负担,从而有更多的时间去处理其他事件,提高系统实时性。 使用 MPU6050 的 DMP 输出的四元数是 q30 格式的,也就是浮点数放大了 2 的 30 次方倍。 在换算成欧拉角之前,必须先将其转换为浮点数,也就是除以 2 的 30 次方,然后再进行计算, 计算公式为: q0=quat[0] / q30; //q30 格式转换为浮点数 q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; //计算得到俯仰角/横滚角/航向角 pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角 roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //横滚角 yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角 其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数,q30 格式,所以要除以一个 2 的 30 次方,其中 q30 是一个常量:1073741824,即 2 的 30 次方,然后带入公式,计算出欧拉 角。上述计算公式的 57.3 是弧度转换为角度,即 180/π,这样得到的结果就是以度(°)为单 位的。关于四元数与欧拉角的公式推导,这里我们不进行讲解,感兴趣的朋友,可以自行查阅 相关资料学习。 InvenSense 提供的 MPU6050 运动驱动库是基于 MSP430 的,我们需要将其移植一下,才 可以用到 STM32F4 上面,官方原版驱动在光盘:7,硬件资料→MPU6050 资料→DMP 资料 → Embedded_MotionDriver_5.1.rar,这就是官方原版的驱动,代码比较多,不过官方提供了两个 资料供大家学习:Embedded Motion Driver V5.1.1 API 说明.pdf 和 Embedded Motion Driver V5.1.1 教程.pdf,这两个文件都在 DMP 资料文件夹里面,大家可以阅读这两个文件,来熟悉官 方驱动库的使用。 官方 DMP 驱动库移植起来,还是比较简单的,主要是实现这 4 个函数:i2c_write,i2c_read, delay_ms 和 get_ms,具体细节,我们就不详细介绍了,移植后的驱动代码,我们放在本例程 →HARDWARE→MPU6050→eMPL 文件夹内,总共 6 个文件,如图 37.1.2.1 所示: 图 37.1.2.1 移植后的驱动库代码 该驱动库,重点就是两个 c 文件:inv_mpu.c 和 inv_mpu_dmp_motion_driver.c。其中我们在 inv_mpu.c添加了几个函数,方便我们使用,重点是两个函数:mpu_dmp_init和mpu_dmp_get_data 这两个函数,这里我们简单介绍下这两个函数。 mpu_dmp_init,是 MPU6050 DMP 初始化函数,该函数代码如下: //mpu6050,dmp 初始化 //返回值:0,正常 // 其他,失败 u8 mpu_dmp_init(void) { u8 res=0; IIC_Init(); //初始化 IIC 总线 if(mpu_init()==0) //初始化 MPU6050 { res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置需要的传感器 if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置 FIFO if(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 if(res)return 3; res=dmp_load_motion_driver_firmware(); //加载 dmp 固件 if(res)return 4; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); //设置陀螺仪方向 if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL| DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL); //设置 dmp 功能 if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);//设置 DMP 输出速率(最大 200Hz) if(res)return 7; res=run_self_test(); //自检 if(res)return 8; res=mpu_set_dmp_state(1); //使能 DMP if(res)return 9; } return 0; } 此函数首先通过 IIC_Init(需外部提供)初始化与 MPU6050 连接的 IIC 接口,然后调用 mpu_init 函数,初始化 MPU6050,之后就是设置 DMP 所用传感器、FIFO、采样率和加载固件 等一些列操作,在所有操作都正常之后,最后通过 mpu_set_dmp_state(1)使能 DMP 功能,在使 能成功以后,我们便可以通过 mpu_dmp_get_data 来读取姿态解算后的数据了。 mpu_dmp_get_data 函数代码如下: //得到 dmp 处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多) //pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0° //roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0° //yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0° //返回值:0,正常 其他,失败 u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw) { float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; if(sensors&INV_WXYZ_QUAT) { q0 = quat[0] / q30; //q30 格式转换为浮点数 q1 = quat[1] / q30; q2 = quat[2] / q30; q3 = quat[3] / q30; //计算得到俯仰角/横滚角/航向角 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;// roll *yaw= atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw }else return 2; return 0; } 此函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。不过本函数局部变量有点 多,大家在使用的时候,如果死机,那么请设置堆栈大一点(在 startup_stm32f40_41xxx.s 里面设 置,默认是 400)。这里就用到了我们前面介绍的四元数转欧拉角公式,将 dmp_read_fifo 函数 读到的 q30 格式四元数转换成欧拉角。 利用这两个函数,我们就可以读取到姿态解算后的欧拉角,使用非常方便。DMP 部分,我 们就介绍到这。 37.2 硬件设计 本实验采用 STM32F4 的 3 个普通 IO 连接 MPU6050,本章实验功能简介:程序先初始化 MPU6050 等外设,然后利用 DMP 库,初始化 MPU6050 及使能 DMP,最后,在死循环里面 不停读取:温度传感器、加速度传感器、陀螺仪、DMP 姿态解算后的欧拉角等数据,通过串口 上报给上位机(温度不上报),利用上位机软件(ANO_Tech 匿名四轴上位机_V2.6.exe), 可以实时显示 MPU6050 的传感器状态曲线,并显示 3D 姿态,可以通过 KEY0 按键开启/关闭 数据上传功能。同时,在 LCD 模块上面显示温度和欧拉角等信息。DS0 来指示程序正在运行。 所要用到的硬件资源如下: 1) 指示灯 DS0 2) KEY0 按键 3) TFTLCD 模块 4) 串口 5) MPU6050 前 4 个,在之前的实例已经介绍过了,这里我们仅介绍 MPU6050 与探索者 STM32F4 开发 板的连接。该接口与 MCU 的连接原理图如 37.2.1 所示: 图 37.2.1 MPU6050 与 STM32F4 的连接电路图 从上图可以看出,MPU6050 通过三根线与 STM32F4 开发板连接,其中 IIC 总线时和 24C02 以及 WM8978 共用,接在 PB8 和 PB9 上面。MPU6050 的中断输出,连接在 STM32F4 的 PC0 脚,不过本例程我们并没有用到中断。另外,AD0 接的 GND,所以 MPU6050 的器件地址是: 0X68。 37.3 软件设计 打开本章工程可以看到,首先我们在工程中 HARDWARE 分组下首先添加了 IIC 支持的底 层驱动文件 myiic.c 和源文件 myiic.h,因为我们的 mpu6050 通信接口是 IIC。同时我们还增加 了 mpu6050.c 源文件和对应的头文件 mpu6050 用来编写 mpu6050 相关的底层驱动。最后我们 还 添 加 了 DMP 驱 动 库 代 码 到 我 们 实 验 工 程 , DMP 驱 动 库 代 码 包 含 inv_mpu.c 和 inv_mpu_dmp_motion_driver.c 两个源文件,以及几个头文件。 由于 mpu6050.c 里面代码比较多,这里我们就不全部列出来了,仅介绍几个重要的函数。 首先是:MPU_Init,该函数代码如下: //初始化 MPU6050 //返回值:0,成功 其他,错误代码 u8 MPU_Init(void) { u8 res; 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; }该函数就是按我们在 37.1.1 节介绍的方法,对 MPU6050 进行初始化,该函数执行成功后, 便可以读取传感器数据了。 然后再看 MPU_Get_Temperature、MPU_Get_Gyroscope 和 MPU_Get_Accelerometer 等三个 函数,源码如下: //得到温度值 //返回值:温度值(扩大了 100 倍) short MPU_Get_Temperature(void) { u8 buf[2]; short raw; float temp; MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); raw=((u16)buf[0]<<8)|buf[1]; temp=36.53+((double)raw)/340; return temp*100;; } //得到陀螺仪值(原始值) //gx,gy,gz:陀螺仪 x,y,z 轴的原始读数(带符号) //返回值:0,成功 // 其他,错误代码 u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz) { u8 buf[6],res; res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf); if(res==0) { *gx=((u16)buf[0]<<8)|buf[1]; *gy=((u16)buf[2]<<8)|buf[3]; *gz=((u16)buf[4]<<8)|buf[5]; } return res;; } //得到加速度值(原始值) //gx,gy,gz:陀螺仪 x,y,z 轴的原始读数(带符号) //返回值:0,成功 // 其他,错误代码 u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az) { u8 buf[6],res; res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf); if(res==0) { *ax=((u16)buf[0]<<8)|buf[1]; *ay=((u16)buf[2]<<8)|buf[3]; *az=((u16)buf[4]<<8)|buf[5]; } return res;; }其中 MPU_Get_Temperature 用于获取 MPU6050 自带温度传感器的温度值,然后MPU_Get_Gyroscope 和 MPU_Get_Accelerometer 分别用于读取陀螺仪和加速度传感器的原 始数据。 最后看 MPU_Write_Len 和 MPU_Read_Len 这两个函数,代码如下: //IIC 连续写 //addr:器件地址 reg:寄存器地址 //len:写入长度 buf:数据区 //返回值:0,正常 其他,错误代码 u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf) { u8 i; IIC_Start(); IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 if(IIC_Wait_Ack()){IIC_Stop();return 1;}//等待应答 IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 for(i=0;i { IIC_Send_Byte(buf); //发送数据 if(IIC_Wait_Ack()) {IIC_Stop();return 1;}//等待 ACK } IIC_Stop(); return 0; } //IIC 连续读 //addr:器件地址 reg:要读取的寄存器地址 //len:要读取的长度 buf:读取到的数据存储区 //返回值:0,正常 其他,错误代码 u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf) { IIC_Start(); IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令 if(IIC_Wait_Ack()){ IIC_Stop();return 1; } //等待应答 IIC_Send_Byte(reg); //写寄存器地址 IIC_Wait_Ack(); //等待应答 IIC_Start(); IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令 IIC_Wait_Ack(); //等待应答 while(len) { if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送 nACK else *buf=IIC_Read_Byte(1); //读数据,发送 ACK len--; buf++; } IIC_Stop(); //产生一个停止条件 return 0; }MPU_Write_Len 用于指定器件和地址,连续写数据,可用于实现 DMP 部分的:i2c_write 函数。而 MPU_Read_Len 用于指定器件和地址,连续读数据,可用于实现 DMP 部分的:i2c_read 函数。DMP 移植部分的 4 个函数,这里就实现了 2 个,剩下的 delay_ms 就直接采用我们 delay.c 里面的 delay_ms 实现,get_ms 则直接提供一个空函数即可。 关于 mpu6050.c 我们就介绍到这,另外 mpu6050.h 的代码,我们这里就不再贴出了,大家 看光盘源码即可。 最后看看 main.c 代码如下: //串口 1 发送 1 个字符 //c:要发送的字符 void usart1_send_char(u8 c) { while(__HAL_UART_GET_FLAG(&UART1_Handler,UART_FLAG_TC)==RESET){}; USART1->DR=c; } //传送数据给匿名四轴上位机软件(V2.6 版本) //fun:功能字. 0X01~0X1C //data:数据缓存区,最多 28 字节!! //len:data 区有效数据个数 void usart1_niming_report(u8 fun,u8*data,u8 len) { u8 send_buf[32]; u8 i; if(len>28)return; //最多 28 字节数据 send_buf[len+3]=0; //校验数置零 send_buf[0]=0XAA; //帧头 send_buf[1]=0XAA; //帧头 send_buf[2]=fun; //功能字 send_buf[3]=len; //数据长度 for(i=0;i //复制数据 for(i=0;i for(i=0;i } //发送加速度传感器数据+陀螺仪数据(传感器帧) //aacx,aacy,aacz:x,y,z 三个方向上面的加速度值 //gyrox,gyroy,gyroz:x,y,z 三个方向上面的陀螺仪值 void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz) { u8 tbuf[18]; tbuf[0]=(aacx>>8)&0XFF; tbuf[1]=aacx&0XFF; tbuf[2]=(aacy>>8)&0XFF; tbuf[3]=aacy&0XFF; tbuf[4]=(aacz>>8)&0XFF; tbuf[5]=aacz&0XFF; tbuf[6]=(gyrox>>8)&0XFF; tbuf[7]=gyrox&0XFF; tbuf[8]=(gyroy>>8)&0XFF; tbuf[9]=gyroy&0XFF; tbuf[10]=(gyroz>>8)&0XFF; tbuf[11]=gyroz&0XFF; tbuf[12]=0; //因为开启 MPL 后,无法直接读取磁力计数据,所以这里直接屏蔽掉.用 0 替代. tbuf[13]=0; tbuf[14]=0; tbuf[15]=0; tbuf[16]=0; tbuf[17]=0; usart1_niming_report(0X02,tbuf,18);//传感器帧,0X02 } //通过串口 1 上报结算后的姿态数据给电脑(状态帧) //roll:横滚角.单位 0.01 度。 -18000 -> 18000 对应 -180.00 -> 180.00 度 //pitch:俯仰角.单位 0.01 度。-9000 - 9000 对应 -90.00 -> 90.00 度 //yaw:航向角.单位为 0.1 度 0 -> 3600 对应 0 -> 360.0 度 //c***:超声波高度,单位:cm //prs:气压计高度,单位:mm void usart1_report_imu(short roll,short pitch,short yaw,short c***,int prs) { u8 tbuf[12]; tbuf[0]=(roll>>8)&0XFF; tbuf[1]=roll&0XFF; tbuf[2]=(pitch>>8)&0XFF; tbuf[3]=pitch&0XFF; tbuf[4]=(yaw>>8)&0XFF; tbuf[5]=yaw&0XFF; tbuf[6]=(c***>>8)&0XFF; tbuf[7]=c***&0XFF; tbuf[8]=(prs>>24)&0XFF; tbuf[9]=(prs>>16)&0XFF; tbuf[10]=(prs>>8)&0XFF; tbuf[11]=prs&0XFF; usart1_niming_report(0X01,tbuf,12);//状态帧,0X01 } int main(void) { u8 t=0,report=1; //默认开启上报 u8 key; float pitch,roll,yaw; //欧拉角 short aacx,aacy,aacz; //加速度传感器原始数据 short gyrox,gyroy,gyroz; //陀螺仪原始数据 short temp; //温度 HAL_Init(); //初始化 HAL 库 Stm32_Clock_Init(336,8,2,7); //设置时钟,168Mhz delay_init(168); //初始化延时函数 uart_init(500000); //初始化 USART usmart_dev.init(84); //初始化 USMART LED_Init(); //初始化 LED KEY_Init(); //初始化 KEY LCD_Init(); //初始化 LCD MPU_Init(); //初始化 MPU6050 POINT_COLOR=RED;//设置字体为红色 LCD_ShowString(30,50,200,16,16,"Explorer STM32F4"); LCD_ShowString(30,70,200,16,16,"MPU6050 TEST"); LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK"); LCD_ShowString(30,110,200,16,16,"2014/5/9"); while(mpu_dmp_init()) { LCD_ShowString(30,130,200,16,16,"MPU6050 Error"); delay_ms(200); LCD_Fill(30,130,239,130+16,WHITE); delay_ms(200); } LCD_ShowString(30,130,200,16,16,"MPU6050 OK"); LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF"); POINT_COLOR=BLUE;//设置字体为蓝色 LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); LCD_ShowString(30,200,200,16,16," Temp: . C"); LCD_ShowString(30,220,200,16,16,"Pitch: . C"); LCD_ShowString(30,240,200,16,16," Roll: . C"); LCD_ShowString(30,260,200,16,16," Yaw : . C"); while(1) { key=KEY_Scan(0); if(key==KEY0_PRES) { report=!report; if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON "); else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF"); } if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0) { temp=MPU_Get_Temperature(); //得到温度值 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据 if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz); //用自定义帧发送加速度和陀螺仪原始数据 if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100), (int)(pitch*100),(int)(yaw*10)); if((t%10)==0) { if(temp<0) { LCD_ShowChar(30+48,200,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分 LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分 temp=pitch*10; if(temp<0) { LCD_ShowChar(30+48,220,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分 temp=roll*10; if(temp<0) { LCD_ShowChar(30+48,240,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分 temp=yaw*10; if(temp<0) { LCD_ShowChar(30+48,260,'-',16,0); //显示负号 temp=-temp; //转为正数 }else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号 LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分 LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分 t=0; LED0=!LED0;//LED 闪烁 } } t++; } }此部分代码除了 main 函数,还有几个函数,用于上报数据给上位机软件,利用上位机软件 显示传感器波形,以及 3D 姿态显示,有助于更好的调试 MPU6050。上位机软件使用:ANO_Tech 匿名四轴上位机_V2.6.exe,该软件在:开发板光盘→ 6,软件资料→软件→匿名四轴上位机 文 件夹里面可以找到,该软件的使用方法,见该文件夹下的 README.txt,这里我们不做介绍。其 中,usart1_niming_report 函数用于将数据打包、计算校验和,然后上报给匿名四轴上位机软件。 mpu6050_send_data 函数用于上报加速度和陀螺仪的原始数据,可用于波形显示传感器数据, 通过 A1 自定义帧发送。而 usart1_report_imu 函数,则用于上报飞控显示帧,可以实时 3D 显示 MPU6050 的姿态,传感器数据等。 这里,main 函数是比较简单的,大家看代码即可,不过需要注意的是,为了高速上传数据, 这里我们将串口 1 的波特率设置为 500Kbps 了,测试的时候要注意下。 最后,我们将 MPU_Write_Byte、MPU_Read_Byte 和 MPU_Get_Temperature 等三个函数加 入 USMART 控制,这样,我们就可以通过串口调试助手,改写和读取 MPU6050 的寄存器数据 了,并可以读取温度传感器的值,方便大家调试(注意在 USMART 调试的时候,最好通过按 KEY0,先关闭数据上传功能,否则会受到很多乱码,妨碍调试)。 至此,我们的软件设计部分就结束了。 37.4 下载验证 在代码编译成功之后,我们通过下载代码到 ALIENTEK 探索者 STM32F4 开发板上,可以 看到 LCD 显示如图 37.4.1 所示的内容: 图 37.4.1 程序运行时 LCD 显示内容 屏幕显示了 MPU6050 的温度、俯仰角(pitch)、横滚角(roll)和航向角(yaw)的数值。 然后,我们可以晃动开发板,看看各角度的变化。 另外,通过按 KEY0 可以开启或关闭数据上报,开启状态下,我们可以打开:ANO_Tech 匿名四轴上位机_V2.6.exe(该软件双击后,会弹出一个蓝色的小界面,直接关闭即可。然后才 会进入主界面),这个软件,接收 STM32F4 上传的数据,从而图形化显示传感器数据以及飞行 姿态,如图 37.4.2 和图 37.4.3 所示: 图 37.4.2 传感器数据波形显示 图 37.4.3 飞控状态显示 图 37.4.2 就是波形化显示我们通过 mpu6050_send_data 函数发送的数据,采用 A1 功能帧发 送,总共 6 条线(Series1~6)显示波形,全部来自 A1 功能帧,int16 数据格式,Series1~6 分别 代表:加速度传感器 x/y/z 和角速度传感器(陀螺仪)x/y/z 方向的原始数据。 图图 37.4.3 则 3D 显示了我们开发板的姿态,通过 usart1_report_imu 函数发送的数据显示, 采用飞控显示帧格(AF)式上传,同时还显示了加速度陀螺仪等传感器的原始数据。 最后,我们还可以用 USMART 读写 MPU6050 的任何寄存器,来调试代码,这里我们就不 做演示了,大家自己测试即可。最后,建议大家用 USMART 调试的时候,先按 KEY0 关闭数 据上传功能,否则会收到很多乱码!!,注意波特率设置为:500Kbps(设置方法:XCOM 在关 闭串口状态下,选择自定义波特率,然后输入:500000,再打开串口就可以了)。 |
|
相关推荐
|
|
很棒棒哦,值得学习
|
|
|
|
|
|
965 浏览 2 评论
STM32CUBEMX(13)--SPI,W25Q128外部Flash移植
1993 浏览 0 评论
OV7670摄像头使用串口发送数据到另一个板子上的lcd,出现花屏
2003 浏览 1 评论
1349 浏览 0 评论
795 浏览 0 评论
小黑屋| 手机版| Archiver| 电子发烧友 ( 湘ICP备2023018690号 )
GMT+8, 2024-10-20 00:19 , Processed in 0.692004 second(s), Total 66, Slave 48 queries .
Powered by 电子发烧友网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号