代码收藏家 STM32-HAL库MPU6050姿态传感器（创社派-STM32F407ZGT6）
新建工程参考：STM32-HAL库 新建工程以及点亮LED灯(创社派-STM32F407ZGT6)_hal库新建工程-CSDN博客https://blog.csdn.net/H_andQ/article/details/143302091?spm=1001.2014.3001.5502
一、MPU6050基本介绍
1、MPU6050工作原理
加速度计
加速度计通过测量由重力引起的加速度来工作。在静态条件下，加速度计可以检测到设备相对于地心的重力分量，从而推断出设备的倾斜角度。在动态条件下，加速度计可以测量设备在各个方向上的加速度变化。
陀螺仪
陀螺仪通过检测由于旋转引起的科里奥利力来测量角速度。当设备围绕某个轴旋转时，陀螺仪内部的微型振动结构会感受到这种力，并通过电容式传感器检测到的信号转换为电信号，输出为旋转速度的测量值。
数据融合
由于单个传感器在某些情况下（如长时间积分或静态条件下）会积累误差，MPU6050通常会采用数据融合技术，如卡尔曼滤波，来结合加速度计和陀螺仪的数据，以获得更为准确和稳定的姿态估计。
2、MPU6050硬件接口和编程
MPU6050通过I2C接口与外部系统通信，它有两个地址配置选项，分别是0x68和0x69。在硬件连接方面，MPU6050的SCL和SDA引脚通常内置有上拉电阻，因此在连接到微控制器时不需要额外的上拉电阻。此外，MPU6050还提供了其他引脚，如INT用于中断信号输出，以及用于扩展其他传感器的接口。
二、cubeMX配置：
1、新建工程
STM32-HAL库 新建工程以及点亮LED灯(创社派-STM32F407ZGT6)_hal库新建工程-CSDN博客https://blog.csdn.net/H_andQ/article/details/143302091?spm=1001.2014.3001.55022、串口配置
STM32-HAL库 串口配置，printf函数重定向（创社派-STM32F407ZGT6）_stm32usart3端口配置hal库-CSDN博客https://blog.csdn.net/H_andQ/article/details/143374994?spm=1001.2014.3001.55023、开启IIC
三、keil5代码
1、新建mpu6050.c文件
新建一个文件，并按下键盘“ctrl+s”组合键保存该文件，保存路径为工程路径下的“Core/Src”。文件命名为“mpu6050.h”
然后粘贴该代码：
/*
* mpu6050.c
*
* Created on: Nov 13, 2019
* Author: Bulanov Konstantin
*
* Contact information
* -------------------
*
* e-mail : leech001@gmail.com
*/
/*
* |---------------------------------------------------------------------------------
* | Copyright (C) Bulanov Konstantin,2021
* |
* | This program is free software: you can redistribute it and/or modify
* | it under the terms of the GNU General Public License as published by
* | the Free Software Foundation, either version 3 of the License, or
* | any later version.
* |
* | This program is distributed in the hope that it will be useful,
* | but WITHOUT ANY WARRANTY; without even the implied warranty of
* | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* | GNU General Public License for more details.
* |
* | You should have received a copy of the GNU General Public License
* | along with this program. If not, see <http://www.gnu.org/licenses/>.
* |
* | Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
* |---------------------------------------------------------------------------------
*/
#include <math.h>
#include "mpu6050.h"
#define RAD_TO_DEG 57.295779513082320876798154814105
#define WHO_AM_I_REG 0x75
#define PWR_MGMT_1_REG 0x6B
#define SMPLRT_DIV_REG 0x19
#define ACCEL_CONFIG_REG 0x1C
#define ACCEL_XOUT_H_REG 0x3B
#define TEMP_OUT_H_REG 0x41
#define GYRO_CONFIG_REG 0x1B
#define GYRO_XOUT_H_REG 0x43
// Setup MPU6050
#define MPU6050_ADDR 0xD0
const uint16_t i2c_timeout = 100;
const double Accel_Z_corrector = 14418.0;
uint32_t timer;
Kalman_t KalmanX = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f};
Kalman_t KalmanY = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f,
};
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
{
uint8_t check;
uint8_t Data;
// check device ID WHO_AM_I
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout);
if (check == 104) // 0x68 will be returned by the sensor if everything goes well
{
// power management register 0X6B we should write all 0's to wake the sensor up
Data = 0;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
// Set DATA RATE of 1KHz by writing SMPLRT_DIV register
Data = 0x07;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);
// Set accelerometer configuration in ACCEL_CONFIG Register
// XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> � 2g
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout);
// Set Gyroscopic configuration in GYRO_CONFIG Register
// XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> � 250 �/s
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
return 0;
}
return 1;
}
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6];
// Read 6 BYTES of data starting from ACCEL_XOUT_H register
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
/*** convert the RAW values into acceleration in 'g'
we have to divide according to the Full scale value set in FS_SEL
I have configured FS_SEL = 0. So I am dividing by 16384.0
for more details check ACCEL_CONFIG Register ****/
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
}
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6];
// Read 6 BYTES of data starting from GYRO_XOUT_H register
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
/*** convert the RAW values into dps (�/s)
we have to divide according to the Full scale value set in FS_SEL
I have configured FS_SEL = 0. So I am dividing by 131.0
for more details check GYRO_CONFIG Register ****/
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
}
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[2];
int16_t temp;
// Read 2 BYTES of data starting from TEMP_OUT_H_REG register
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);
temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
}
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[14];
int16_t temp;
// Read 14 BYTES of data starting from ACCEL_XOUT_H register
HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);
DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);
DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
// Kalman angle solve
double dt = (double)(HAL_GetTick() - timer) / 1000;
timer = HAL_GetTick();
double roll;
double roll_sqrt = sqrt(
DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW + DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
if (roll_sqrt != 0.0)
{
roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG;
}
else
{
roll = 0.0;
}
double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90))
{
KalmanY.angle = pitch;
DataStruct->KalmanAngleY = pitch;
}
else
{
DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
}
if (fabs(DataStruct->KalmanAngleY) > 90)
DataStruct->Gx = -DataStruct->Gx;
DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
}
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
{
double rate = newRate - Kalman->bias;
Kalman->angle += dt * rate;
Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);
Kalman->P[0][1] -= dt * Kalman->P[1][1];
Kalman->P[1][0] -= dt * Kalman->P[1][1];
Kalman->P[1][1] += Kalman->Q_bias * dt;
double S = Kalman->P[0][0] + Kalman->R_measure;
double K[2];
K[0] = Kalman->P[0][0] / S;
K[1] = Kalman->P[1][0] / S;
double y = newAngle - Kalman->angle;
Kalman->angle += K[0] * y;
Kalman->bias += K[1] * y;
double P00_temp = Kalman->P[0][0];
double P01_temp = Kalman->P[0][1];
Kalman->P[0][0] -= K[0] * P00_temp;
Kalman->P[0][1] -= K[0] * P01_temp;
Kalman->P[1][0] -= K[1] * P00_temp;
Kalman->P[1][1] -= K[1] * P01_temp;
return Kalman->angle;
};2、新建mpu6050.h文件
同样方法新建mpu6050.h文件，h文件保存路径在“Core/Inc”路径下，然后粘贴代码。
/*
* mpu6050.h
*
* Created on: Nov 13, 2019
* Author: Bulanov Konstantin
*/
#ifndef INC_GY521_H_
#define INC_GY521_H_
#endif /* INC_GY521_H_ */
#include <stdint.h>
#include "i2c.h"
// MPU6050 structure
typedef struct
{
int16_t Accel_X_RAW;
int16_t Accel_Y_RAW;
int16_t Accel_Z_RAW;
double Ax;
double Ay;
double Az;
int16_t Gyro_X_RAW;
int16_t Gyro_Y_RAW;
int16_t Gyro_Z_RAW;
double Gx;
double Gy;
double Gz;
float Temperature;
double KalmanAngleX;
double KalmanAngleY;
} MPU6050_t;
// Kalman structure
typedef struct
{
double Q_angle;
double Q_bias;
double R_measure;
double angle;
double bias;
double P[2][2];
} Kalman_t;
uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx);
void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);3、导入mpu6050.c文件
刚才只是创建了c文件，但是工程中没有该文件，所以需要导入该文件。
鼠标右键工程路径，点击“Manage Project ltems…”
新建mpu6050文件夹
点击“Add Files”，点击返回上级目录，进入Core/Src路径，选择mpu6050.c文件后点击“Add”
同样方法添加mpu6050.h
4、调用代码
1、串口配置参考：
STM32-HAL库 串口配置，printf函数重定向（创社派-STM32F407ZGT6）_stm32usart3端口配置hal库-CSDN博客https://blog.csdn.net/H_andQ/article/details/143374994?spm=1001.2014.3001.55022、在“main.c”文件中进行编辑
①添加6050头文件
/* USER CODE BEGIN Includes */
#include "mpu6050.h"
/* USER CODE END Includes */
②定义6050结构体
/* USER CODE BEGIN PV */
MPU6050_t MPU6050;
/* USER CODE END PV */③在main函数中初始化6050
/ * USER CODE BEGIN 2 * /
while (MPU6050_Init(&hi2c1) == 1);
/ * USER CODE END 2 * /④读取参数，在主循环中写入以下代码：
MPU6050_Read_All(&hi2c1, &MPU6050);
printf("加速度 x:%.2f \t y:%.2f \t z:%.2f\n",MPU6050.Ax,MPU6050.Ay,MPU6050.Az);
printf("陀螺仪 x:%.2f \t y:%.2f \t z:%.2f\n",MPU6050.Gx,MPU6050.Gy,MPU6050.Gz);
printf("温度 %.2f\n",MPU6050.Temperature);
⑤读取角度
MPU6050_Read_All(&hi2c1, &MPU6050);
printf("X: %f, Y: %f\n", MPU6050.KalmanAngleX, MPU6050.KalmanAngleY);5、效果
自主设计的开发板，直插MPU6050，简单方便
作者：H_andQ
