STM32之MPU6050获取欧拉角
- MPU6050
- MPU6050特点
- MPU6050电路图以及框图
-
- MPU6050相关寄存器
- 电源管理寄存器1(0x6B)
- 陀螺仪配置寄存器(0x1B)
- 加速度计配置寄存器(0x1C)
- 陀螺仪采样率分频寄存器(0x19)
- 配置寄存器
- 电源管理寄存器2(0x6C)
- 加速度计数据输出寄存器(0x3B~0x40)
- 温度传感器数据输出寄存器(0x41~0x42)
- 陀螺仪数据输出寄存器(0x43~0x48)
- FIFO使能寄存器
- 我是谁寄存器
- MPU6050通信协议
- MPU6050写入寄存器时序
- MPU6050寄存器读取时序
- MPU6050获取三轴加速度、三轴陀螺仪以及温度
- MPU6050与STM32板子接线
- STM32CubeMX相关配置
- 配置SYS
- 配置RCC
- 配置USART3
- 配置NVIC
- 使用Micro库
- 文件编写
- 修改文件usart.c
- 修改文件usart.h
- 添加文件mpu6050.c
- 添加文件mpu6050.h
- 添加文件delay.c
- 添加文件delay.h
- 添加文件my_i2c.c
- 添加文件my_i2c.h
- main.c文件编写
- DMP介绍
- 使用DMP的目的
- 使用DMP将MPU6050的原始数据转换成欧拉角
- 移植DMP库
-
- 四元数
- MPU6050获取欧拉角:俯仰角(Pitch)、横滚角(Roll)、偏航角(Yaw)
-
MPU6050
mpu6050 集成了三轴 MEMS 陀螺仪,三轴 MEMS 加速度计,以及一个可扩展的数字运动处理器 DMP(Digital Motion Processor)。辅助I2C接口可以连接一个第三方的数字传感器,比如磁力计。扩展了磁力计之后就可以通过其主要I2C接口输出一个 9 轴的信号(否则只能输出一个6轴的信号)。mpu6050 也可以通过其辅助I2C接口连接非惯性的数字传感器,如压力传感器。总之,mpu6050 就是通过陀螺仪和加速度计来分别测量三轴的角速度和角加速度的数据,继而获得俯仰角(Pitch)、横滚角(Roll)、偏航角(Yaw)。
.
MPU6050特点
- 以数字形式输出六轴或九轴(需外接磁力传感器)的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据(需 DMP 支持)。
- 具有 131 LSB/°/s 敏感度与测量范围为±250、±500、±1000 与±2000 的三轴陀螺仪。
- 具有可程序控制,范围为±2g、±4g、±8g 和±16g 的三轴加速度计。
- 自带一个数字温度传感器。
- 自带数字运动处理(DMP,Digital Motion Processing)引擎可减少 MCU 复杂的融
合演算数据、感测器同步化、姿势感应等的负荷。 - 可程序控制的中断(interrupt)。
- 自带 1024 字节 FIFO,有助于降低系统功耗。
- 高达 400kHz 的 IIC 通信接口。
- 数字运动处理(DMP: Digital Motion Processing)引擎可减少复杂的融合演算数据、感测
器同步化、姿势感应等的负荷。 - 移除加速器与陀螺仪轴间敏感度,降低设定给予的影响与感测器的飘移。
- 内建运作时间偏差与磁力感测器校正演算技术。
.
MPU6050电路图以及框图
MPU6050框图
.
MPU6050电路图
.
MPU6050相关寄存器
电源管理寄存器1(0x6B)
mpu6050 复位,选择时钟源。
- DEVICE_RESET 位 用于控制复位,设置为 1,复位 mpu6050,复位结束后,MPU 硬件自动清零该位。
- SLEEEP 位 用于控制 mpu6050 的工作模式,复位后,该位为1,即进入了睡眠模式(低功耗),所以要清零该位,才能进入正常工作模式。
- CYCLE 位 当该位设置为1并且休眠模式被禁用时,mpu6050 将在休眠模式和唤醒模式之间循环,以电源管理寄存器2的 LP_WAKE_CTRL[1:0] 位确定的速率从加速计中获取单个采样。
- TEMP_DIS 位 用于设置是否使能温度传感器,设置为 0,则使能。
- CLKSEL[2:0] 用于选择系统时钟源,选择关系,如下表所示:
CLKSEL[2:0] | 时钟源 |
---|
000 | 内部 8M RC 晶振 |
001 | PLL,使用 X 轴陀螺仪作为参考 |
010 | PLL,使用 Y 轴陀螺仪作为参考 |
011 | PLL,使用 Z 轴陀螺仪作为参考 |
100 | PLL,使用外部 32.768kHz 时钟作为参考 |
101 | PLL,使用外部 19.2MHz 作为参考 |
110 | 保留 |
111 | 关闭时钟,保持时序产生电路复位状态 |
CLKSEL[2:0] 的默认值为 000,即使用内部的 8M RC 晶振作为时钟源,但其精度不高, 因此一般选择 X、Y、Z 轴陀螺仪作为参考的 PLL 作为时钟源。 .
陀螺仪配置寄存器(0x1B)
配置 mpu6050 陀螺仪的量程范围。
- 该寄存器,仅需关心 FS_SEL[1:0] 这两个比特位,其他位不用管,设置为0。FS_SEL[1:0] 用于配置陀螺仪的满量程范围,具体的配置描述,如下表所示:
FS_SEL[1:0] | 陀螺仪满量程范围 |
---|
00 | ± 250dps |
01 | ± 500dps |
10 | ± 1000dps |
11 | ± 2000dps |
一般可以将 FS_SEL[1:0]配置为 11,即配置陀螺仪的满量程范围为±2000dps。
.
加速度计配置寄存器(0x1C)
配置 mpu6050 加速计的量程范围。
- 该寄存器,仅需关心 AFS_SEL[1:0] 这两个比特位,其他位不用管,设置为0。AFS_SEL[1:0] 用于配置加速度计的满量程范围,具体的配置描述,如下表所示:
AFS_SEL[1:0] | 加速度传感器满量程范围 |
---|
00 | ± 2g |
01 | ± 4g |
10 | ± 8g |
11 | ± 16g |
一般可以将 FS_SEL[1:0]配置为 00,即配置加速度传感器的满量程范围为±2g。
.
陀螺仪采样率分频寄存器(0x19)
配置 mpu6050 陀螺仪采样频率,传感器数据输出和FIFO输出以及DMP采样都是基于这个采样频率。
- 采样频率 = 陀螺仪输出频率 / (1 + SMPLRT_DIV[7:0] ),陀螺仪输出频率与数字低通滤波器(DLPF)的配置有关,一般为8kH和1kHz。
.
配置寄存器
配置 mpu6050 加速度计和陀螺仪的带宽。
- 该寄存器,仅需关心 DLPF_CFG[2:0] 这三个比特位,其他位不用管,设置为0。DLPF_CFG[2:0] 用来配置加速度计和陀螺仪的带宽,具体的配置描述,如下表所示:
DLPF_CFG[2:0] | 加速度计(Fs = 1kHz) | 陀螺仪 |
---|
带宽(Hz) | 延迟(ms) | 带宽(Hz) | 延迟(ms) | Fs(kHz) |
000 | 260 | 0 | 256 | 0.98 | 8 |
001 | 184 | 2.0 | 188 | 1.9 | 1 |
010 | 94 | 3.0 | 98 | 2.8 | 1 |
011 | 44 | 4.9 | 42 | 4.8 | 1 |
100 | 21 | 8.5 | 20 | 8.3 | 1 |
101 | 10 | 13.8 | 10 | 13.4 | 1 |
110 | 5 | 19.0 | 5 | 18.6 | 1 |
111 | 保留 | 保留 | 8 |
一般情况下,配置陀螺仪带宽为陀螺仪采样频率的一半。例如如果陀螺仪采样频率为50Hz,那么带宽就应该设置为25Hz,取近似值20Hz,那么 DLPF_CFG[2:0] 就应该设置为100。
.
电源管理寄存器2(0x6C)
控制陀螺仪和加速度计是否进入待机模式。
- LP_WAKE_CTRL[1:0] 位用于控制低功耗时的唤醒频率。
- STBY_XA 位用于控制加速计X轴是否进入待机模式,设置为1,进入待机模式。
- STBY_YA 位用于控制加速计Y轴是否进入待机模式,设置为1,进入待机模式。
- STBY_ZA 位用于控制加速计Z轴是否进入待机模式,设置为1,进入待机模式。
- STBY_XG 位用于控制陀螺仪X轴是否进入待机模式,设置为1,进入待机模式。
- STBY_YG 位用于控制陀螺仪Y轴是否进入待机模式,设置为1,进入待机模式。
- STBY_ZG 位用于控制陀螺仪Z轴是否进入待机模式,设置为1,进入待机模式。
.
加速度计数据输出寄存器(0x3B~0x40)
存储加速度计的原始数据,这些原始数据会以陀螺仪的采样频率进行更新。
- 某个加速度轴的原始数据并不是加速度数据,如果想获得加速度数据需要以下转换:加速度 = (有符号的16位原始数据) / 灵敏度,单位:g(9.8m/s²)
- 灵敏度根据加速度计的量程变化而变化,如下表所示:
.
温度传感器数据输出寄存器(0x41~0x42)
存储温度传感器的原始数据,这些原始数据会以陀螺仪的采样频率进行更新。
- 该原始数据并不是以摄氏度为单位的温度值,如果需要转换成以摄氏度为单位的温度值需要经过以下转换:温度 = (有符号的16位原始数据)/ 340 + 36.53,单位:℃
.
陀螺仪数据输出寄存器(0x43~0x48)
存储陀螺仪的原始数据,这些原始数据会以陀螺仪的采样频率进行更新。
- 某个陀螺仪轴的原始数据并不是陀螺仪数据,如果想获得陀螺仪数据需要以下转换:陀螺仪 = (有符号的16位原始数据) / 灵敏度,单位:°/s
- 灵敏度根据陀螺仪的量程变化而变化,如下表所示:
.
FIFO使能寄存器
控制mpu6050的加速度计、温度传感器、陀螺仪的原始数据是否写入FIFO缓冲区。
- TEMP_FIFO_EN 位用于控制是否将温度传感器的原始数据写入FIFO缓冲区,设置为 1,写入FIFO缓冲区。
- XG_ FIFO_EN 位用于控制是否将陀螺仪X轴的原始数据写入FIFO缓冲区,设置为 1,写入FIFO缓冲区。
- YG_ FIFO_EN 位用于控制是否将陀螺仪Y轴的原始数据写入FIFO缓冲区,设置为 1,写入FIFO缓冲区。
- ZG_ FIFO_EN 位用于控制是否将陀螺仪Z轴的原始数据写入FIFO缓冲区,设置为 1,写入FIFO缓冲区。
- ACCEL_ FIFO_EN 位用于控制是否将加速度计的原始数据写入FIFO缓冲区,设置为 1,写入FIFO缓冲区。
- 其他位不用管,设置为0。
.
我是谁寄存器
存储 mpu6050 的7位I2C地址的高6位,用来验证设备的身份。
- 该寄存器的默认值为0x68(即 b0 110100 0)。
- mpu6050 的 Slave 地址为 b110100X,7 位字长,最低有效位 X 由 AD0 引脚上的逻辑电平决定,高电平为1,低电平为0。
.
MPU6050通信协议
当连接到系统芯片时,mpu6050 总是作为从设备,因此,系统芯片可以通过 400kHz 的 I2C 接口来操作 mpu6050 的内部寄存器。mpu6050 的 Slave 地址为 b110100X,7 位字长,最低有效位 X 由 AD0 引脚上的逻辑电平决定,高电平为1,低电平为0(即高电平时 mpu6050 从机地址为0x69,低电平时 mpu6050 从机地址为0x68)。
.
MPU6050写入寄存器时序
单字节写入时序:
.
多字节写入时序:
.
- ①:主设备发送起始信号。
- ②:主设备发送7位mpu6050的I2C设备地址以及一个W 位,即0 。
- ③:主设备等待从设备(mpu6050)发送应答信号。
- ④:主设备发送想要写入寄存器的地址。
- ⑤:主设备等待从设备(mpu6050)发送应答信号。
- ⑥:主设备发送想要写入寄存器的数据。
- ⑦:主设备等待从设备(mpu6050)发送应答信号。
- ⑧:主设备发送终止信号。
- 如果想连续写入寄存器数据则在主设备发送终止信号前重复⑥、⑦步骤即可。
.
MPU6050寄存器读取时序
单字节读取时序:
.
多字节读取时序:
.
- ①:主设备发送起始信号。
- ②:主设备发送7位mpu6050的I2C设备地址以及一个W 位,即0 。
- ③:主设备等待从设备(mpu6050)发送应答信号。
- ④:主设备发送想要写入寄存器的地址。
- ⑤:主设备等待从设备(mpu6050)发送应答信号。
- ⑥:主设备发送起始信号。
- ⑦:主设备发送7位mpu6050的I2C设备地址以及一个R 位,即1 。
- ⑧:主设备等待从设备(mpu6050)发送应答信号。
- ⑨:主设备读取想要读取寄存器的数据。
- ⑩:主设备发送应答信号(可选,多字节读取时序时使用)。
- ⑪:主设备读取想要读取寄存器的数据(可选,多字节读取时序时使用)。
- ⑫:主设备发送非应答信号。
- ⑬:主设备发送终止信号。
- 如果想连续读取寄存器数据则在主设备发送非应答信号和终止信号前重复⑩、⑪步骤即可。
.
MPU6050获取三轴加速度、三轴陀螺仪以及温度
MPU6050与STM32板子接线
- PB6 <-> SCL
- PB7 <-> SDA
- 单片机5V <-> VCC
- GND <-> GND
- AD0 <-> GND
.
STM32CubeMX相关配置
配置SYS
.
配置RCC
.
配置USART3
.
配置NVIC
.
使用Micro库
只要映射了printf用来发送数据去串口都要使用这个库。
.
文件编写
修改文件usart.c
#include "usart.h"
#include <stdio.h>
#include <string.h>
#define USART_REC_LEN 200
uint8_t buf = 0;
uint8_t UART1_RX_Buffer[USART_REC_LEN];
uint16_t UART1_RX_STA = 0;
UART_HandleTypeDef huart3;
void MX_USART3_UART_Init(void)
{
huart3.Instance = USART3;
huart3.Init.BaudRate = 115200;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
huart3.Init.Mode = UART_MODE_TX_RX;
huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart3.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart3) != HAL_OK)
{
Error_Handler();
}
HAL_UART_Receive_IT(&huart3, &buf, 1);
printf("usart3 is ok\r\n");
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(uartHandle->Instance==USART3)
{
__HAL_RCC_USART3_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_10;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(USART3_IRQn);
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART3)
{
__HAL_RCC_USART3_CLK_DISABLE();
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10|GPIO_PIN_11);
HAL_NVIC_DisableIRQ(USART3_IRQn);
}
}
int fputc(int my_data, FILE *p)
{
unsigned char temp = my_data;
HAL_UART_Transmit(&huart3, &temp, 1, 0xffff);
return my_data;
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if (huart->Instance == USART1)
{
if (!(UART1_RX_STA & 0x8000))
{
if (UART1_RX_STA & 0x4000)
{
if (buf == 0x0a)
{
UART1_RX_STA |= 0x8000;
}
else
{
UART1_RX_STA = 0;
}
}
else
{
if (buf == 0x0d)
{
UART1_RX_STA |= 0x4000;
}
else
{
UART1_RX_Buffer[UART1_RX_STA & 0x3ffff] = buf;
UART1_RX_STA++;
if (UART1_RX_STA > USART_REC_LEN - 1)
{
UART1_RX_STA = 0;
}
}
}
}
HAL_UART_Receive_IT(&huart3, &buf, 1);
}
}
void usart1_receive_data_handle()
{
if (UART1_RX_STA & 0x8000)
{
printf("接收完成\r\n");
if (!strcmp((const char *)UART1_RX_Buffer, "haozige"))
{
printf("浩子哥\r\n");
}
else
{
printf("%s\r\n", "输入错误,请重新输入");
}
memset(UART1_RX_Buffer, 0, USART_REC_LEN);
UART1_RX_STA = 0;
}
}
.
修改文件usart.h
#ifndef __USART_H__
#define __USART_H__
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h"
extern UART_HandleTypeDef huart3;
void MX_USART3_UART_Init(void);
void usart1_receive_data_handle(void);
#ifdef __cplusplus
}
#endif
#endif
.
添加文件mpu6050.c
#include "my_i2c.h"
#include "mpu6050.h"
#include "usart.h"
#include <stdio.h>
uint8_t mpu6050_init()
{
uint8_t id,data;
IIC_gpio_init();
mpu6050_read_len(MPU6050_ADDR,MPU_DEVICE_ID_REG,1,&id);
if(id == 0x68){
mpu6050_reset();
mpu6050_set_rate(100);
mpu6050_set_accelerated_speed(0);
mpu6050_set_gyroscope(3);
data = 0;
mpu6050_write_len(MPU6050_ADDR,MPU_INT_EN_REG,1,&data);
data = 0;
mpu6050_write_len(MPU6050_ADDR,MPU_USER_CTRL_REG,1,&data);
data = 0;
mpu6050_write_len(MPU6050_ADDR,MPU_FIFO_EN_REG,1,&data);
data = 0x01;
mpu6050_write_len(MPU6050_ADDR,MPU_PWR_MGMT1_REG,1,&data);
data = 0;
mpu6050_write_len(MPU6050_ADDR,MPU_PWR_MGMT2_REG,1,&data);
return 0;
}
return 1;
}
void mpu6050_reset()
{
uint8_t data;
data = 0x80;
mpu6050_write_len(MPU6050_ADDR,MPU_PWR_MGMT1_REG,1,&data);
HAL_Delay(100);
data = 0x00;
}
void mpu6050_set_lpf(uint16_t lpf)
{
uint8_t data;
mpu6050_write_len(MPU6050_ADDR,MPU_CFG_REG,1,&data);
}
void mpu6050_set_rate(uint16_t rata)
{
uint8_t data;
if(rata >= 1000) rata = 1000;
if(rata <= 4) rata = 4;
data = 1000/rata - 1;
mpu6050_write_len(MPU6050_ADDR,MPU_SAMPLE_RATE_REG,1,&data);
mpu6050_set_lpf(rata / 2);
}
void mpu6050_set_accelerated_speed(uint8_t data)
{
data <<= 3;
mpu6050_write_len(MPU6050_ADDR,MPU_ACCEL_CFG_REG,1,&data);
}
void mpu6050_set_gyroscope(uint8_t data)
{
data <<= 3;
mpu6050_write_len(MPU6050_ADDR,MPU_GYRO_CFG_REG,1,&data);
}
void mpu6050_read_accelerated_speed(int16_t *accelerated_speed_x,int16_t *accelerated_speed_y,int16_t *accelerated_speed_z)
{
uint8_t rev_buf[6];
mpu6050_read_len(MPU6050_ADDR,MPU_ACCEL_XOUTH_REG,6,rev_buf);
*accelerated_speed_x = (int16_t)(rev_buf[0] << 8 | rev_buf[1]);
*accelerated_speed_y = (int16_t)(rev_buf[2] << 8 | rev_buf[3]);
*accelerated_speed_z = (int16_t)(rev_buf[4] << 8 | rev_buf[5]);
}
void mpu6050_read_gyroscope(int16_t *gyroscope_x,int16_t *gyroscope_y,int16_t *gyroscope_z)
{
uint8_t rev_buf[6];
mpu6050_read_len(MPU6050_ADDR,MPU_GYRO_XOUTH_REG,6,rev_buf);
*gyroscope_x = (int16_t)(rev_buf[0] << 8 | rev_buf[1]);
*gyroscope_y = (int16_t)(rev_buf[2] << 8 | rev_buf[3]);
*gyroscope_z = (int16_t)(rev_buf[4] << 8 | rev_buf[5]);
}
float mpu6050_read_temperature()
{
uint8_t rev_buf[2];
int16_t *temperature;
mpu6050_read_len(MPU6050_ADDR,MPU_TEMP_OUTH_REG,2,rev_buf);
*temperature = (int16_t)(rev_buf[0] << 8 | rev_buf[1]);
return 36.53 + *temperature / 340;
}
uint8_t mpu6050_write_len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
uint8_t i;
IIC_start();
IIC_send_byte((addr | 0));
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
IIC_send_byte(reg);
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
for (i=0; i<len; i++)
{
IIC_send_byte(buf[i]);
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
}
IIC_stop();
return 0;
}
uint8_t mpu6050_read_len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
IIC_start();
IIC_send_byte(addr | 0);
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
IIC_send_byte(reg);
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
IIC_start();
IIC_send_byte(addr | 1);
if (IIC_wait_ack() == 1)
{
IIC_stop();
return 1;
}
while (len)
{
*buf = IIC_read_byte((len > 1) ? 1 : 0);
len--;
buf++;
}
IIC_stop();
return 0;
}
.
添加文件mpu6050.h
#include "main.h"
#define MPU6050_ADDR 0xD0
#define MPU_DEVICE_ID_REG 0X75
#define MPU_CFG_REG 0X1A
#define MPU_SAMPLE_RATE_REG 0X19
#define MPU_GYRO_CFG_REG 0X1B
#define MPU_ACCEL_CFG_REG 0X1C
#define MPU_ACCEL_XOUTH_REG 0X3B
#define MPU_ACCEL_XOUTL_REG 0X3C
#define MPU_ACCEL_YOUTH_REG 0X3D
#define MPU_ACCEL_YOUTL_REG 0X3E
#define MPU_ACCEL_ZOUTH_REG 0X3F
#define MPU_ACCEL_ZOUTL_REG 0X40
#define MPU_TEMP_OUTH_REG 0X41
#define MPU_TEMP_OUTL_REG 0X42
#define MPU_GYRO_XOUTH_REG 0X43
#define MPU_GYRO_XOUTL_REG 0X44
#define MPU_GYRO_YOUTH_REG 0X45
#define MPU_GYRO_YOUTL_REG 0X46
#define MPU_GYRO_ZOUTH_REG 0X47
#define MPU_GYRO_ZOUTL_REG 0X48
#define MPU_USER_CTRL_REG 0X6A
#define MPU_PWR_MGMT1_REG 0X6B
#define MPU_PWR_MGMT2_REG 0X6C
#define MPU_INTBP_CFG_REG 0X37
#define MPU_INT_EN_REG 0X38
#define MPU_INT_STA_REG 0X3A
#define MPU_FIFO_EN_REG 0X23
#define MPU_FIFO_CNTH_REG 0X72
#define MPU_FIFO_CNTL_REG 0X73
#define MPU_FIFO_RW_REG 0X74
void mpu6050_reset(void);
uint8_t mpu6050_init(void);
void mpu6050_set_lpf(uint16_t lpf);
void mpu6050_set_rate(uint16_t rata);
void mpu6050_set_accelerated_speed(uint8_t data);
void mpu6050_set_gyroscope(uint8_t data);
void mpu6050_read_accelerated_speed(int16_t *accelerated_speed_x,int16_t *accelerated_speed_y,int16_t *accelerated_speed_z);
void mpu6050_read_gyroscope(int16_t *gyroscope_x,int16_t *gyroscope_y,int16_t *gyroscope_z);
float mpu6050_read_temperature(void);
uint8_t mpu6050_write_len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);
uint8_t mpu6050_read_len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf);
.
添加文件delay.c
#include "main.h"
#include "delay.h"
void delay_us(uint32_t n)
{
uint8_t j;
while(n--)
for(j=0;j<10;j++);
}
void delay_ms(uint32_t n)
{
while(n--)
delay_us(1000);
}
void get_ms(unsigned long *time)
{
}
.
添加文件delay.h
#include "main.h"
void delay_us(uint32_t n);
void delay_ms(uint32_t n);
void get_ms(unsigned long *time);
.
添加文件my_i2c.c
#include "gpio.h"
#include "delay.h"
#include "my_i2c.h"
#define scl_pin GPIO_PIN_6
#define sda_pin GPIO_PIN_7
#define scl_gpio GPIOB
#define sda_gpio GPIOB
#define SCL_HIGH HAL_GPIO_WritePin(scl_gpio, scl_pin, GPIO_PIN_SET)
#define SCL_LOW HAL_GPIO_WritePin(scl_gpio, scl_pin, GPIO_PIN_RESET)
#define SDA_HIGH HAL_GPIO_WritePin(scl_gpio, sda_pin, GPIO_PIN_SET)
#define SDA_LOW HAL_GPIO_WritePin(sda_gpio, sda_pin, GPIO_PIN_RESET)
#define SDA_READ HAL_GPIO_ReadPin(sda_gpio, sda_pin)
static void i2c_delay_us()
{
delay_us(2);
}
void IIC_gpio_init()
{
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStruct = {0};
GPIO_InitStruct.Pin = scl_pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = sda_pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
IIC_stop();
}
void IIC_start()
{
SCL_HIGH;
SDA_HIGH;
i2c_delay_us();
SDA_LOW;
i2c_delay_us();
SCL_LOW;
i2c_delay_us();
}
void IIC_stop()
{
SDA_LOW;
i2c_delay_us();
SCL_HIGH;
i2c_delay_us();
SDA_HIGH;
i2c_delay_us();
}
uint8_t IIC_wait_ack()
{
SDA_HIGH;
i2c_delay_us();
SCL_HIGH;
i2c_delay_us();
if (SDA_READ == GPIO_PIN_SET)
{
IIC_stop();
return 1;
}
SCL_LOW;
i2c_delay_us();
return 0;
}
void IIC_ack()
{
SDA_LOW;
i2c_delay_us();
SCL_HIGH;
i2c_delay_us();
SCL_LOW;
i2c_delay_us();
SDA_HIGH;
i2c_delay_us();
}
void IIC_nack()
{
SDA_HIGH;
i2c_delay_us();
SCL_HIGH;
i2c_delay_us();
SCL_LOW;
i2c_delay_us();
}
void IIC_send_byte(uint8_t data)
{
for (uint8_t i = 0; i < 8; i++)
{
if ((data & 0x80) >> 7)
{
SDA_HIGH;
}
else
{
SDA_LOW;
}
i2c_delay_us();
SCL_HIGH;
i2c_delay_us();
SCL_LOW;
data <<= 1;
}
SCL_HIGH;
}
uint8_t IIC_read_byte(uint8_t ack)
{
uint8_t receive = 0;
for (uint8_t i = 0; i < 8; i++)
{
receive = receive << 1;
SCL_HIGH;
i2c_delay_us();
if (SDA_READ)
{
receive++;
}
SCL_LOW;
i2c_delay_us();
}
if (!ack)
{
IIC_nack();
}
else
{
IIC_ack();
}
return receive;
}
.
添加文件my_i2c.h
#include "main.h"
static void i2c_delay_us(void);
void IIC_gpio_init(void);
void IIC_start(void);
void IIC_stop(void);
uint8_t IIC_wait_ack(void);
void IIC_ack(void);
void IIC_nack(void);
void IIC_send_byte(uint8_t data);
uint8_t IIC_read_byte(uint8_t ack);
.
main.c文件编写
#include "main.h"
#include "usart.h"
#include "gpio.h"
#include <stdio.h>
#include "mpu6050.h"
#include "delay.h"
void SystemClock_Config(void);
int16_t accelerated_speed_x = 0;
int16_t accelerated_speed_y = 0;
int16_t accelerated_speed_z = 0;
int16_t gyroscope_x = 0;
int16_t gyroscope_y = 0;
int16_t gyroscope_z = 0;
float temperature = 0;
float ax = 0;
float ay = 0;
float az = 0;
float gx = 0;
float gy = 0;
float gz = 0;
int main(void)
{
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_USART3_UART_Init();
mpu6050_init();
while (1)
{
mpu6050_read_accelerated_speed(&accelerated_speed_x,&accelerated_speed_y,&accelerated_speed_z);
mpu6050_read_gyroscope(&gyroscope_x,&gyroscope_y,&gyroscope_z);
temperature = mpu6050_read_temperature();
ax = accelerated_speed_x/16384.0;
ay = accelerated_speed_y/16384.0;
az = accelerated_speed_z/16384.0;
gx = gyroscope_x/16.4;
gy = gyroscope_y/16.4;
gz = gyroscope_z/16.4;
printf("ax:%f\r\nay:%f\r\naz:%f\r\n",ax,ay,az);
printf("gx:%f\r\ngy:%f\r\ngz:%f\r\n",gx,gy,gz);
printf("temperature:%f\r\n",temperature);
delay_ms(100);
usart1_receive_data_handle();
}
}
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
void Error_Handler(void)
{
__disable_irq();
while (1)
{
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif
.
DMP介绍
嵌入式数字运动处理器(Digital Motion Processor)位于 mpu6050 内部,可从主机处理器中卸载运动处理算法的运算。 DMP从加速度计,陀螺仪以及其他第三方传感器(如磁力计)获取数据,并处理数据。结果数据可以从DMP的寄存器中读取,或者可以在FIFO中缓冲。 DMP可以访问其中的一个MPU的外部引脚,可用于产生中断。
.
使用DMP的目的
DMP的目的是卸载主机处理器的时序要求和处理能力。通常,运动处理算法应该以高速运行,通常在200Hz左右,以提供低延迟的精确结果。即使应用程序以更低的速率更新,这也是必需的。例如,一个低功率的用户界面可能会以5Hz的速度更新,但运动处理仍然应该以200Hz运行。 DMP可以作为一种工具使用,以最大限度地降低功耗,简化定时,简化软件架构,并在主机处理器上节省宝贵的MIPS,以便在应用中使用。
.
使用DMP将MPU6050的原始数据转换成欧拉角
在前面代码中,已经介绍了如何获取 mpu6050 的加速度计和陀螺仪的原始数据,但是这些原始数据并不是姿态数据。姿态数据也就是欧拉角:俯仰角(pitch)、横滚角(roll)、航向角(yaw),通过欧拉角就能够非常直观地了解当前三轴的姿态。想要得到欧拉角数据,就需要对原始数据进行姿态融合解算,姿态结算涉及较多的数学计算,如果我们直接利用原始数据进行姿态解算,不仅要求开发者有较丰富的知识储备和一定的数学能力,同时对 MCU 的运算性能也有较高的要求。而 mpu6050 自带的 DMP(数字运动处理器)就能够很好的解决这一些列的问题,配合 InvenSense 提供的 DMP 驱动库,就能够很方便地将 MPU-6050 输出的原始数据直接转换为四元数输出,在得到四元数之后,就能够通过少量的运算,计算出欧拉角,从而得到姿态数据。
.
移植DMP库
- nvenSense公司 提供的 DMP 驱动库是基于 MSP430 的,因此要在 STM32 上使用该 DMP 驱动库,还需要进行一定的移植。
- 我们可以参考这篇文章进行移植DMP库。STM32平台下官方DMP库6.12超详细移植教程
- 正点原子同样提供了移植好了的DMP库,轮子能用就行,我们可以基于该正点原子的DMP库进行修改。
STM32F103系列移植正点原子DMP库
1. 将DMP库搬运到自己的工程目录底下。
.
.
.
2. 将DMP库添加到工程中
这一步是添加DMP库相关的.c文件
.
.
这一步是添加DMP库相关的.h文件
.
.
.
3. 修改inv_npu.c文件
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "delay.h"
#include "usart.h"
.
修改后
.
.
#define MPU6050
#define MOTION_DRIVER_TARGET_MSP430
#define i2c_write mpu6050_write_len
#define i2c_read mpu6050_read_len
#define delay_ms delay_ms
#define get_ms get_ms
#define log_e printf
#define log_i printf
.
修改后
.
.
- 修改hw结构体的addr值,如果在(mpu6050_write_len、mpu6050_read_len)mpu6050连续写和mpu6050连续读函数里没有左移mpu6050的7位I2C地址就填0xD0,如果左移了就填0x68。(可选)
.
修改后
.
. - 修改几个重要函数
mpu_init()
.
.
atk_ms6050_dmp_init()
uint8_t mpu6050_dmp_init(void)
{
uint8_t ret;
if(mpu6050_init() == 0){
ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(ret) return 1;
ret = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(ret) return 2;
ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);
if(ret) return 3;
ret = dmp_load_motion_driver_firmware();
if(ret) return 4;
ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
if(ret) return 5;
ret = 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);
if(ret) return 6;
ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
if(ret) return 7;
ret = mpu_set_dmp_state(1);
if(ret) return 8;
ret = mpu6050_run_self_test();
if(ret) return 9;
return 0;
}
return 10;
}
.
.
atk_ms6050_get_clock_ms()
这个没什么用,删掉就行
.
.
atk_ms6050_run_self_test()
改名成mpu6050_run_self_test()就行
.
.
atk_ms6050_dmp_get_data()
改名成mpu6050_dmp_get_data()就行
.
.
.
4. 修改inv_mpu.h文件
uint8_t mpu6050_run_self_test(void);
uint8_t mpu6050_dmp_init(void);
uint8_t mpu6050_dmp_get_data(float *pitch, float *roll, float *yaw);
.
.
.
5. 修改inv_mpu_dmp_motion_driver.c文件
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "dmpKey.h"
#include "dmpmap.h"
#include "delay.h"
#include "usart.h"
.
.
- 修改部分宏定义。
这样我们的DMP库就移植完成了。
.
.
.
四元数
mpu6050 的 DMP 输出的四元数是 q30 格式的,也就是将正常浮点格式的四元数放大了 230 倍,因此在换算之前,需要将 DMP 输出的四元数转换为正常的浮点格式,也就是将其除以 230,然后才能将其转换为欧拉角。四元数的介绍可以看这篇文章四元数与欧拉角(Yaw、Pitch、Roll)的转换。
.
MPU6050获取欧拉角:俯仰角(Pitch)、横滚角(Roll)、偏航角(Yaw)
前面我们已经使用过mpu6050获取过加速度计和陀螺仪以及温度传感器的原始数据,同时我们也已经成功移植了DMP库,因此只需要修改main.c文件就行。
修改main.c文件
#include "main.h"
#include "usart.h"
#include "gpio.h"
#include <stdio.h>
#include "mpu6050.h"
#include "delay.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
void SystemClock_Config(void);
float temperature = 0;
float pitch,roll,yaw;
int main(void)
{
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_USART3_UART_Init();
while(mpu6050_dmp_init())
{
HAL_Delay(200);
printf("%s\r\n","Mpu6050_DMP Init Wrong!");
printf(" %d",mpu6050_dmp_init());
}
printf("%s\r\n","DMP_Mpu6050 Init OK!");
while (1)
{
temperature= mpu6050_read_temperature();
if(mpu6050_dmp_get_data(&pitch,&roll,&yaw)==0){
printf("三轴角度:%f-%f-%f\r\n",pitch,roll,yaw);
printf("temp:%f\r\n",temperature);
printf("pitch:%f\r\n",pitch);
printf("roll: %f\r\n",roll);
printf("yaw: %f\r\n",yaw);
}
delay_ms(100);
usart1_receive_data_handle();
}
}
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
void Error_Handler(void)
{
__disable_irq();
while (1)
{
}
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif
.
实验效果
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)