MPU6050 of STM32 obtains Euler angle
- MPU6050
-
- Features of MPU6050
- MPU6050 circuit diagram and block diagram
- MPU6050 related registers
-
- Power Management Register 1 (0x6B)
- Gyroscope configuration register (0x1B)
- Accelerometer Configuration Register (0x1C)
- Gyroscope Sample Rate Divider Register (0x19)
- configuration register
- Power Management Register 2 (0x6C)
- Accelerometer data output register (0x3B~0x40)
- Temperature sensor data output register (0x41~0x42)
- Gyroscope data output register (0x43~0x48)
- FIFO enable register
- who am i register
- MPU6050 communication protocol
- MPU6050 obtains three-axis acceleration, three-axis gyroscope and temperature
- DMP introduction
- MPU6050 obtains Euler angles: pitch angle (Pitch), roll angle (Roll), yaw angle (Yaw)
MPU6050
The mpu6050 integrates a three-axis MEMS gyroscope, a three-axis MEMS accelerometer, and an expandable digital motion processor DMP (Digital Motion Processor). The auxiliary I2C interface can connect a third-party digital sensor, such as a magnetometer. After expanding the magnetometer, it can output a 9-axis signal through its main I2C interface (otherwise it can only output a 6-axis signal). The mpu6050 can also connect non-inertial digital sensors, such as pressure sensors, through its auxiliary I2C interface. In short, the mpu6050 uses the gyroscope and accelerometer to measure the angular velocity and angular acceleration data of the three axes, respectively, and then obtain the pitch angle (Pitch), roll angle (Roll), and yaw angle (Yaw).
.
Features of MPU6050
- Digitally output six-axis or nine-axis (requires external magnetic sensor) rotation matrix, quaternion (quaternion), fusion calculation data in Euler Angle format (requires DMP support).
- Three-axis gyroscope with 131 LSB/°/s sensitivity and measurement ranges of ±250, ±500, ±1000, and ±2000.
- Triaxial accelerometer with programmable ranges of ±2g, ±4g, ±8g and ±16g.
- Comes with a digital temperature sensor.
- The built-in digital motion processing (DMP, Digital Motion Processing) engine can reduce the MCU's complex fusion
calculation data, sensor synchronization, posture sensing and other loads. - Programmable interrupt (interrupt).
- Comes with 1024 bytes FIFO, which helps to reduce system power consumption.
- Up to 400kHz IIC communication interface.
- Digital Motion Processing (DMP: Digital Motion Processing) engine can reduce the load of complex fusion calculation data, sensor
synchronization, gesture sensing, etc. - Removed the inter-axis sensitivity of the accelerometer and gyroscope to reduce the influence of the setting and the drift of the sensor.
- Built-in operating time deviation and magnetic sensor calibration algorithm technology.
.
MPU6050 circuit diagram and block diagram
MPU6050 block diagram
.
MPU6050 circuit diagram
.
MPU6050 related registers
Power Management Register 1 (0x6B)
mpu6050 reset, select the clock source.
- The DEVICE_RESET bit is used to control the reset. Set it to 1 to reset the mpu6050. After the reset, the MPU hardware will automatically clear this bit.
- The SLEEEP bit is used to control the working mode of the mpu6050. After reset, this bit is 1, which means it enters the sleep mode (low power consumption), so it must be cleared to zero to enter the normal working mode.
- CYCLE bit When this bit is set to 1 and sleep mode is disabled, the mpu6050 will cycle between sleep mode and wake mode, taking single samples from the accelerometer at a rate determined by the LP_WAKE_CTRL[1:0] bits of the power management register 2 .
- The TEMP_DIS bit is used to set whether to enable the temperature sensor, set it to 0, then enable it.
- CLKSEL[2:0] is used to select the system clock source, and the selection relationship is shown in the following table:
| CLKSEL[2:0] | clock source |
|---|---|
| 000 | Internal 8M RC crystal oscillator |
| 001 | PLL, using X-axis gyroscope as reference |
| 010 | PLL, using Y-axis gyroscope as reference |
| 011 | PLL, using Z-axis gyroscope as reference |
| 100 | PLL, using external 32.768kHz clock as reference |
| 101 | PLL, using external 19.2MHz as reference |
| 110 | reserve |
| 111 | Turn off the clock and keep the timing generation circuit in reset state |
Gyroscope configuration register (0x1B)
Configure the range of the mpu6050 gyroscope.
- For this register, you only need to care about the two bits FS_SEL[1:0], the other bits are ignored and set to 0. FS_SEL[1:0] is used to configure the full-scale range of the gyroscope. The specific configuration description is shown in the following table:
| FS_SEL[1:0] | Gyroscope Full Scale Range |
|---|---|
| 00 | ± 250dps |
| 01 | ± 500dps |
| 10 | ± 1000dps |
| 11 | ± 2000dps |
Generally, FS_SEL[1:0] can be configured as 11, that is, the full-scale range of the gyroscope is configured to be ±2000dps.
.
Accelerometer Configuration Register (0x1C)
Configure the range of the mpu6050 accelerometer.
- For this register, you only need to care about the two bits of AFS_SEL[1:0], and the other bits are ignored and set to 0. AFS_SEL[1:0] is used to configure the full-scale range of the accelerometer. The specific configuration description is shown in the following table:
| AFS_SEL[1:0] | Accelerometer full scale range |
|---|---|
| 00 | ± 2g |
| 01 | ± 4g |
| 10 | ± 8g |
| 11 | ± 16g |
.
Gyroscope Sample Rate Divider Register (0x19)
Configure mpu6050 gyroscope sampling frequency, sensor data output, FIFO output and DMP sampling are all based on this sampling frequency.
- Sampling frequency = gyroscope output frequency/ (1 + SMPLRT_DIV[7:0] ), the gyroscope output frequency is related to the configuration of the digital low-pass filter (DLPF), generally 8kH and 1kHz.
.
configuration register
Configure the bandwidth of the mpu6050 accelerometer and gyroscope.
- For this register, you only need to care about the three bits of DLPF_CFG[2:0], and set the other bits to 0. DLPF_CFG[2:0] is used to configure the bandwidth of the accelerometer and gyroscope. The specific configuration description is shown in the following table:
| DLPF_CFG[2:0] | Accelerometer (Fs = 1kHz) | Gyro | |||
|---|---|---|---|---|---|
| Bandwidth (Hz) | delay (ms) | Bandwidth (Hz) | delay (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 | reserve | reserve | 8 | ||
.
Power Management Register 2 (0x6C)
Controls whether the gyroscope and accelerometer enter standby mode.
- The LP_WAKE_CTRL[1:0] bits are used to control the wake-up frequency during low power consumption.
- The STBY_XA bit is used to control whether the X-axis of the accelerometer enters the standby mode, set it to 1, and enter the standby mode.
- The STBY_YA bit is used to control whether the Y axis of the accelerometer enters the standby mode, set it to 1, and enter the standby mode.
- The STBY_ZA bit is used to control whether the Z axis of the accelerometer enters the standby mode, set it to 1, and enter the standby mode.
- The STBY_XG bit is used to control whether the X-axis of the gyroscope enters the standby mode, set it to 1, and enter the standby mode.
- The STBY_YG bit is used to control whether the Y-axis of the gyroscope enters the standby mode, set it to 1, and enter the standby mode.
- The STBY_ZG bit is used to control whether the Z-axis of the gyroscope enters the standby mode, set it to 1, and enter the standby mode.
.
Accelerometer data output register (0x3B~0x40)
Stores raw data from the accelerometer, which is updated at the sampling rate of the gyroscope.
- The raw data of an acceleration axis is not acceleration data. If you want to obtain acceleration data, you need the following conversion: acceleration = (signed 16-bit raw data) / sensitivity, unit: g (9.8m/s²)
- The sensitivity varies according to the range of the accelerometer, as shown in the table below:
.
Temperature sensor data output register (0x41~0x42)
Stores raw data from the temperature sensor, which is updated at the sampling frequency of the gyroscope.
- The original data is not a temperature value in degrees Celsius. If it needs to be converted into a temperature value in degrees Celsius, the following conversion is required: temperature = (signed 16-bit original data) / 340 + 36.53, unit: °C
.
Gyroscope data output register (0x43~0x48)
Store the raw data of the gyroscope, which will be updated at the sampling frequency of the gyroscope.
- The raw data of a certain gyroscope axis is not gyroscope data. If you want to get gyroscope data, you need the following conversion: gyroscope = (signed 16-bit raw data) / sensitivity, unit: °/s
- The sensitivity varies according to the range of the gyroscope, as shown in the table below:
.
FIFO enable register
Control whether the raw data of the accelerometer, temperature sensor, and gyroscope of the mpu6050 are written into the FIFO buffer.
- The TEMP_FIFO_EN bit is used to control whether to write the raw data of the temperature sensor to the FIFO buffer, set it to 1, and write to the FIFO buffer.
- The XG_FIFO_EN bit is used to control whether to write the raw data of the X-axis of the gyroscope into the FIFO buffer, set it to 1, and write it into the FIFO buffer.
- The YG_FIFO_EN bit is used to control whether to write the raw data of the Y axis of the gyroscope into the FIFO buffer, set it to 1, and write it into the FIFO buffer.
- The ZG_FIFO_EN bit is used to control whether to write the raw data of the Z axis of the gyroscope into the FIFO buffer, set it to 1, and write it into the FIFO buffer.
- The ACCEL_FIFO_EN bit is used to control whether to write the raw data of the accelerometer to the FIFO buffer, set it to 1, and write to the FIFO buffer.
- The other bits are ignored and set to 0.
.
who am i register
Store the upper 6 bits of the 7-bit I2C address of the mpu6050 to verify the identity of the device.
- The default value of this register is 0x68 (ie b0 110100 0).
- The Slave address of mpu6050 is b110100X, 7-bit word length, the least significant bit X is determined by the logic level on the AD0 pin, high level is 1, low level is 0.
.
MPU6050 communication protocol
When connected to the SoC, the mpu6050 always acts as a slave device, so the SoC can operate the internal registers of the mpu6050 through the 400kHz I2C interface. The Slave address of mpu6050 is b110100X, 7-bit word length, the least significant bit X is determined by the logic level on the AD0 pin, high level is 1, low level is 0 (that is, the mpu6050 slave address is 0x69 when high level, When low level, the mpu6050 slave address is 0x68).
.
MPU6050 write register timing
Single-byte write timing:
.Multi
-byte write timing:
.
- ①: The master device sends a start signal.
- ②: The master device sends the I2C device address of the 7-bit mpu6050 and a W bit, which is 0.
- ③: The master device waits for the slave device (mpu6050) to send a response signal.
- ④: The master device sends the address to be written to the register.
- ⑤: The master device waits for the slave device (mpu6050) to send a response signal.
- ⑥: The master device sends the data to be written into the register.
- ⑦: The master device waits for the slave device (mpu6050) to send a response signal.
- ⑧: The master device sends a termination signal.
- If you want to continuously write register data , repeat steps ⑥ and ⑦ before the master device sends a termination signal.
.
MPU6050 register read timing
Single-byte read timing:
.Multi
-byte read timing:
.
- ①: The master device sends a start signal.
- ②: The master device sends the I2C device address of the 7-bit mpu6050 and a W bit, which is 0.
- ③: The master device waits for the slave device (mpu6050) to send a response signal.
- ④: The master device sends the address to be written to the register.
- ⑤: The master device waits for the slave device (mpu6050) to send a response signal.
- ⑥: The master device sends a start signal.
- ⑦: The master device sends the 7-bit I2C device address of mpu6050 and an R bit, which is 1.
- ⑧: The master device waits for the slave device (mpu6050) to send a response signal.
- ⑨: The master device reads the data that it wants to read from the register.
- ⑩: The master device sends a response signal (optional, used when multi-byte read timing).
- ⑪: The master device reads the data that it wants to read from the register (optional, used when multi-byte read timing).
- ⑫: The master device sends a non-response signal.
- ⑬: The master device sends a termination signal.
- If you want to read the register data continuously , repeat the steps ⑩ and ⑪ before the master device sends the non-response signal and termination signal.
.
MPU6050 obtains three-axis acceleration, three-axis gyroscope and temperature
Wiring between MPU6050 and STM32 board
- PB6 <-> SCL
- PB7 <-> SDA
- MCU 5V <-> VCC
- GND <-> GND
- AD0 <-> GND
.
STM32CubeMX related configuration
Configure SYS
.
Configure RCC
.
Configure USART3
.
Configure NVIC
.
Using the Micro library
As long as printf is mapped to send data to the serial port, this library must be used.
.
document writing
Modify the file 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 */
.
Modify the file 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__ */
.
Add file 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;
}
.
Add file 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);
.
Add file 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)
{
}
.
Add file delay.h
#include "main.h"
void delay_us(uint32_t n);
void delay_ms(uint32_t n);
void get_ms(unsigned long *time);
.
Add the file 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;
}
.
Add the file 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);
.
main.c file writing
/* 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 */
.
DMP introduction
The embedded digital motion processor (Digital Motion Processor) is located inside the mpu6050, which can offload the operation of the motion processing algorithm from the host processor. The DMP takes data from accelerometers, gyroscopes, and other third-party sensors such as magnetometers, and processes the data. Result data can be read from the DMP's registers, or can be buffered in FIFO. The DMP can access one of the MPU's external pins, which can be used to generate interrupts.
.
Purpose of using DMP
The purpose of the DMP is to offload the timing requirements and processing power of the host processor. In general, motion processing algorithms should run at high speeds, usually around 200Hz, to provide accurate results with low latency. This is required even if the application updates at a slower rate. For example, a low-power UI might update at 5Hz, but motion processing should still run at 200Hz. DMP can be used as a tool to minimize power consumption, simplify timing, simplify software architecture, and save valuable MIPS on the host processor for use in applications.
.
Use DMP to convert the raw data of MPU6050 into Euler angles
In the previous code, it has been introduced how to obtain the raw data of the accelerometer and gyroscope of the mpu6050, but these raw data are not attitude data. The attitude data is the Euler angle: pitch angle (pitch), roll angle (roll), and heading angle (yaw). Through the Euler angle, you can understand the current three-axis attitude very intuitively. If you want to obtain Euler angle data, you need to perform attitude fusion calculation on the original data. The attitude calculation involves more mathematical calculations. If we directly use the original data to calculate the attitude, not only requires the developer to have a richer knowledge reserve and Certain mathematical ability, but also have higher requirements on the computing performance of the MCU. The DMP (Digital Motion Processor) that comes with the mpu6050 can solve these series of problems very well. With the DMP driver library provided by InvenSense, it is very convenient to directly convert the original data output by the MPU-6050 into quaternary Number output, after getting the quaternion, you can calculate the Euler angle through a small amount of calculation, so as to get the attitude data.
.
Porting the DMP library
- The DMP driver library provided by nvenSense is based on MSP430, so to use the DMP driver library on STM32, a certain amount of transplantation is required.
- We can refer to this article to transplant the DMP library. Official DMP library 6.12 ultra-detailed porting tutorial on STM32 platform
- Punctual Atom also provides a transplanted DMP library, as long as the wheel can be used, we can modify it based on the DMP library of Punctual Atom.
STM32F103 series porting punctual atomic DMP library
1. Move the DMP library to your own project directory.
.
.
.
2. Add the DMP library to the project.
This step is to add the .c files related to the DMP library
.
.
This step is to add the .h files related to the DMP library
.
.
.
3. Modify the inv_npu.c file
- Modify the header file.
#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"
.
after modification
.
.
- Modify some macro definitions.
#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
.
after modification
.
.
- Modify the addr value of the hw structure. If there is no left-shifting of the 7-bit I2C address of the mpu6050 in the (mpu6050_write_len, mpu6050_read_len) mpu6050 continuous writing and mpu6050 continuous reading functions, fill in 0xD0, and if it is left-shifted, fill in 0x68. (optional)
.
after modification
.
. - Modify several important functions
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;
}
.
.
atk_ms6050_get_clock_ms（）
This is useless, just delete it
.
.
atk_ms6050_run_self_test()
Just change the name to mpu6050_run_self_test()
.
.
atk_ms6050_dmp_get_data()
Just change the name to mpu6050_dmp_get_data()
.
.
.
4. Modify the inv_mpu.h file
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. Modify the inv_mpu_dmp_motion_driver.c file
- Modify the header file.
#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"
.
.
- Modify some macro definitions.
In this way, our DMP library is transplanted.
.
.
.
Quaternion
The quaternion output by the DMP of mpu6050 is in q30 format, that is, the quaternion in the normal floating-point format is enlarged by 230 times, so before conversion, the quaternion output by DMP needs to be converted into a normal floating-point format. That is, divide it by 230 before you can convert it to Euler angles. For an introduction to quaternions, see this article Conversion of quaternions and Euler angles (Yaw, Pitch, Roll) .
.
MPU6050 obtains Euler angles: pitch angle (Pitch), roll angle (Roll), yaw angle (Yaw)
We have used the mpu6050 to obtain the raw data of the accelerometer, gyroscope and temperature sensor, and we have also successfully transplanted the DMP library, so we only need to modify the main.c file.
Modify the main.c file
/* 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 */
.
Experimental effect
