STM32之MPU6050获取欧拉角_mpu6050欧拉角

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 晶振作为时钟源，但其精度不高， 因此一般选择 X、Y、Z 轴陀螺仪作为参考的 PLL 作为时钟源。 .

陀螺仪配置寄存器（0x1B）

配置 mpu6050 陀螺仪的量程范围。

• 该寄存器，仅需关心 FS_SEL[1:0] 这两个比特位，其他位不用管，设置为0。FS_SEL[1:0] 用于配置陀螺仪的满量程范围，具体的配置描述，如下表所示：

一般可以将 FS_SEL[1:0]配置为 11，即配置陀螺仪的满量程范围为±2000dps。
.

加速度计配置寄存器（0x1C）

配置 mpu6050 加速计的量程范围。

• 该寄存器，仅需关心 AFS_SEL[1:0] 这两个比特位，其他位不用管，设置为0。AFS_SEL[1:0] 用于配置加速度计的满量程范围，具体的配置描述，如下表所示：

一般可以将 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] 用来配置加速度计和陀螺仪的带宽，具体的配置描述，如下表所示：

一般情况下，配置陀螺仪带宽为陀螺仪采样频率的一半。例如如果陀螺仪采样频率为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

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    usart.c
  * @brief   This file provides code for the configuration
  *          of the USART instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "usart.h"

/* USER CODE BEGIN 0 */
 
#include <stdio.h>
#include <string.h>
 
#define USART_REC_LEN 200
 
// 串口接收缓存（1字节）
uint8_t buf = 0;
 
uint8_t UART1_RX_Buffer[USART_REC_LEN]; // 接收缓冲，串口接收的数据存放地点
 
// 串口接收状态，16位
uint16_t UART1_RX_STA = 0;
// bit15： 如果是1表示接收完成
// bit14： 如果是1表示接收到回车(0x0d)
// bit13~bit0： 接收到的有效字节数目
 

/* USER CODE END 0 */

UART_HandleTypeDef huart3;

/* USART3 init function */

void MX_USART3_UART_Init(void)
{

  /* USER CODE BEGIN USART3_Init 0 */

  /* USER CODE END USART3_Init 0 */

  /* USER CODE BEGIN USART3_Init 1 */

  /* USER CODE END USART3_Init 1 */
  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();
  }
  /* USER CODE BEGIN USART3_Init 2 */

	/* 开启串口1的接收中断 */
  HAL_UART_Receive_IT(&huart3, &buf, 1); /* 每接收一个串口数据调用一次串口接收完成回调函数 */
	printf("usart3 is ok\r\n");
	
  /* USER CODE END USART3_Init 2 */

}

void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(uartHandle->Instance==USART3)
  {
  /* USER CODE BEGIN USART3_MspInit 0 */

  /* USER CODE END USART3_MspInit 0 */
    /* USART3 clock enable */
    __HAL_RCC_USART3_CLK_ENABLE();

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**USART3 GPIO Configuration
    PB10     ------> USART3_TX
    PB11     ------> USART3_RX
    */
    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);

    /* USART3 interrupt Init */
    HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(USART3_IRQn);
  /* USER CODE BEGIN USART3_MspInit 1 */

  /* USER CODE END USART3_MspInit 1 */
  }
}

void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{

  if(uartHandle->Instance==USART3)
  {
  /* USER CODE BEGIN USART3_MspDeInit 0 */

  /* USER CODE END USART3_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_USART3_CLK_DISABLE();

    /**USART3 GPIO Configuration
    PB10     ------> USART3_TX
    PB11     ------> USART3_RX
    */
    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10|GPIO_PIN_11);

    /* USART3 interrupt Deinit */
    HAL_NVIC_DisableIRQ(USART3_IRQn);
  /* USER CODE BEGIN USART3_MspDeInit 1 */

  /* USER CODE END USART3_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */

/* 重写stdio.h文件中的prinft()里的fputc()函数 */
int fputc(int my_data, FILE *p)
{
    unsigned char temp = my_data;
    // 改写后，使用printf()函数会将数据通过串口一发送出去
    HAL_UART_Transmit(&huart3, &temp, 1, 0xffff); // 0xfffff为最大超时时间
    return my_data;
}
 
/* 串口接收完成回调函数 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    // 判断中断是哪个串口触发的
    if (huart->Instance == USART1)
    {
 
        // 判断接收是否完成，即判断UART1_RX_STA的bit15是否为1
        if (!(UART1_RX_STA & 0x8000))
        { // 如果没接收完成就进入接收流程
 
            // 判断是否接收到回车0x0d
            if (UART1_RX_STA & 0x4000)
            {
 
                // 判断是否接收到换行0x0a
                if (buf == 0x0a)
                {
 
                    // 如果回车和换行都接收到了，则表示接收完成,即把bit15拉高
                    UART1_RX_STA |= 0x8000;
                }
                else
                { // 如果接收到回车0x0d没有接收到换行0x0a
 
                    // 则认为接收错误，重新开始接收
                    UART1_RX_STA = 0;
                }
            }
            else
            { // 如果没有接收到回车0x0d
 
                // 则判断收到的这个字符是否是回车0x0d
                if (buf == 0x0d)
                {
 
                    // 如果这个字符是回车，则将将bit14拉高，表示接收到回车
                    UART1_RX_STA |= 0x4000;
                }
                else
                { // 如果不是回车
 
                    // 则将这个字符存放到缓存数组中
                    UART1_RX_Buffer[UART1_RX_STA & 0x3ffff] = buf;
                    UART1_RX_STA++;
 
                    // 如果接收数据大于UART1_REC_LEN（200字节），则重新开始接收
                    if (UART1_RX_STA > USART_REC_LEN - 1)
                    {
                        UART1_RX_STA = 0;
                    }
                }
            }
        }
        // 如果接收完成则重新开启串口1的接收中断
        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;
    }
}

/* USER CODE END 1 */

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238

.

修改文件usart.h

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    usart.h
  * @brief   This file contains all the function prototypes for
  *          the usart.c file
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USART_H__
#define __USART_H__

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

extern UART_HandleTypeDef huart3;

/* USER CODE BEGIN Private defines */

/* USER CODE END Private defines */

void MX_USART3_UART_Init(void);

/* USER CODE BEGIN Prototypes */

void usart1_receive_data_handle(void);

/* USER CODE END Prototypes */

#ifdef __cplusplus
}
#endif

#endif /* __USART_H__ */


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

.

添加文件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软件复位  0x6B */
		
		mpu6050_set_rate(100);  /* 设置陀螺仪采样频率为100Hz  0x19 */
		
		mpu6050_set_accelerated_speed(0);  /* 设置加速度量程范围：±2g  0x1C */
		
		mpu6050_set_gyroscope(3);  /* 设置陀螺仪量程范围：±2000dps  0x1B */
		
		/* 关闭所有中断 */
		data = 0;
		mpu6050_write_len(MPU6050_ADDR,MPU_INT_EN_REG,1,&data);
		
		/* 关闭IIC主模式 */
		data = 0;
		mpu6050_write_len(MPU6050_ADDR,MPU_USER_CTRL_REG,1,&data);
		
	  /* 关闭FIFO */
		data = 0;
		mpu6050_write_len(MPU6050_ADDR,MPU_FIFO_EN_REG,1,&data);
		
	  /* 设置CLKSEL，PLL X轴为参考 */
		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;
}

/* mpu6050软件复位 */
void mpu6050_reset()
{
	uint8_t data;
	data = 0x80; 
	mpu6050_write_len(MPU6050_ADDR,MPU_PWR_MGMT1_REG,1,&data);  // 复位mpu6050
	
	HAL_Delay(100);
	
	data = 0x00;
	  // 唤醒mpu6050
}

/* 设置数字低通滤波器频率(陀螺仪输出频率) */
void mpu6050_set_lpf(uint16_t lpf)
{
	// 该函数设置陀螺仪输出频率为1000Hz
	uint8_t data;
	mpu6050_write_len(MPU6050_ADDR,MPU_CFG_REG,1,&data);
}	


/* 设置陀螺仪采样频率：4~1000Hz */
void mpu6050_set_rate(uint16_t rata)
{
	// 陀螺仪采样率rate = 陀螺仪输出频率（1000Hz或8000Hz,mpu6050_set_lpf函数设定为1000Hz） / (SMPLPT_DIV + 1)(采样周期)
	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); // 自动设置低通滤波频率为采样率的一半
	
}

/* 
设置加速度量程范围：
	data:
		0 --> ±2g
		1 --> ±4g
		2 --> ±8g
		3 --> ±16g
*/
void mpu6050_set_accelerated_speed(uint8_t data)
{
	data <<= 3; 
	mpu6050_write_len(MPU6050_ADDR,MPU_ACCEL_CFG_REG,1,&data);
}

/* 
设置陀螺仪量程范围：
	fsr:
		0 --> ±250dps
		1 --> ±500dps
		2 --> ±1000dps
		3 --> ±2000dps
*/
void mpu6050_set_gyroscope(uint8_t data)
{
	data <<= 3;
	mpu6050_write_len(MPU6050_ADDR,MPU_GYRO_CFG_REG,1,&data);
}

/* 获取mpu6050的原始数据 */
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);  //读取x，y，z三轴的加速度值
	
	*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);  //读取x，y，z三轴的陀螺仪值
	
	*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;
}


/* 下面两个函数用来代替dmp库中内容 */
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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213

.

添加文件mpu6050.h

#include "main.h"

#define  MPU6050_ADDR  0xD0  //mou6050APD0引脚接地，因此它的I2C地址为0xD0,即0x68 << 2

#define  MPU_DEVICE_ID_REG     0X75	 // 器件ID寄存器

#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  // 加速度值,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	 // 温度值高8位寄存器
#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_USER_CTRL_REG     0X6A	 // 用户控制寄存器
#define  MPU_PWR_MGMT1_REG     0X6B	 // 电源管理寄存器1
#define  MPU_PWR_MGMT2_REG     0X6C	 // 电源管理寄存器2


#define  MPU_INTBP_CFG_REG     0X37  // 中断/旁路设置寄存器
#define  MPU_INT_EN_REG        0X38  // 中断使能寄存器
#define  MPU_INT_STA_REG       0X3A  // 中断状态寄存器


#define  MPU_FIFO_EN_REG       0X23	 // FIFO使能寄存器
#define  MPU_FIFO_CNTH_REG     0X72	 // FIFO计数寄存器高八位
#define  MPU_FIFO_CNTL_REG     0X73	 // FIFO计数寄存器低八位
#define  MPU_FIFO_RW_REG       0X74	 // FIFO读写寄存器

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);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

.

添加文件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)
{

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

.

添加文件delay.h

#include "main.h"

void delay_us(uint32_t n);

void delay_ms(uint32_t n);

void get_ms(unsigned long *time);

1
2
3
4
5
6
7
8

.

添加文件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);  //延时2微秒
}

void IIC_gpio_init()
{
	// 打开时钟
  __HAL_RCC_GPIOB_CLK_ENABLE();

	GPIO_InitTypeDef GPIO_InitStruct = {0};
	
	/* 初始化SCL引脚 */
    GPIO_InitStruct.Pin    = scl_pin;  /* SCL引脚 */
    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);
    
    /* 初始化SDA引脚 */
    GPIO_InitStruct.Pin    = sda_pin;  /* SDA引脚 */
    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();
}

/* 检测应答信号：ACK返回0，NACK返回1 */
uint8_t IIC_wait_ack()
{
    SDA_HIGH;    /* 释放数据线 */
    i2c_delay_us();
    SCL_HIGH; /* 从机返回ACK */
    i2c_delay_us();

               
    if (SDA_READ == GPIO_PIN_SET)  /* 读取SDA的电平 */
    {
        /* 如果是高电平则为NACK */
        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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166

.

添加文件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);


/* 检测应答信号：ACK返回0，NACK返回1 */
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);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

.

main.c文件编写

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include <stdio.h>
#include "mpu6050.h"
#include "delay.h"


/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* 加速度原始数据 */
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;


/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */

	mpu6050_init();
	

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		
		/* 获取mpu6050原始数据 */
		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();
		
		/*
		printf("ax:%d\r\nay:%d\r\naz:%d\r\n",accelerated_speed_x,accelerated_speed_y,accelerated_speed_z);
		printf("gx:%d\r\ngy:%d\r\ngz:%d\r\n",gyroscope_x,gyroscope_y,gyroscope_z);
		printf("temperature:%f\r\n",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();
		
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  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();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  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();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240

.

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"
1
2
3
4
5
6
7
8
9
10

.
修改后
.
.

• 修改部分宏定义。

#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
1
2
3
4
5
6
7
8
9
10

.
修改后

.
.

• 修改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);    /* 设置FIFO */
			if(ret) return 2;
			
			ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);                 /* 设置采样率 */
			if(ret) return 3;
			
			ret = dmp_load_motion_driver_firmware();                   /* 加载DMP固件 */
			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功能 */
                                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);                   /* 设置DMP输出速率 */
			if(ret) return 7;
			
			ret = mpu_set_dmp_state(1);                                /* 使能DMP */
			if(ret) return 8;
			
			ret = mpu6050_run_self_test();                          /* 传感器自测试 */
			if(ret) return 9;
			
			return 0;
		}
		
		return 10;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

.
.
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);
1
2
3

.
.
.
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"
1
2
3
4
5
6
7
8
9
10
11

.
.

• 修改部分宏定义。
  

这样我们的DMP库就移植完成了。
.
.
.

四元数

mpu6050 的 DMP 输出的四元数是 q30 格式的，也就是将正常浮点格式的四元数放大了 230 倍，因此在换算之前，需要将 DMP 输出的四元数转换为正常的浮点格式，也就是将其除以 230，然后才能将其转换为欧拉角。四元数的介绍可以看这篇文章四元数与欧拉角（Yaw、Pitch、Roll）的转换。
.

MPU6050获取欧拉角：俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）

前面我们已经使用过mpu6050获取过加速度计和陀螺仪以及温度传感器的原始数据，同时我们也已经成功移植了DMP库，因此只需要修改main.c文件就行。

修改main.c文件

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include <stdio.h>
#include "mpu6050.h"
#include "delay.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

float temperature = 0;

float pitch,roll,yaw; 		//欧拉角




/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */

	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!");
	

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		
		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();
		
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  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();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  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();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219

.

实验效果