【STM32L476RG】第六节——MPU6050的简单测试

Zhao_kar · 发表于2023-10-24 20:29

【STM32L476RG】第六节——MPU6050的简单测试 [复制链接]

本帖最后由 Zhao_kar 于 2023-10-24 20:28 编辑

【STM32L476RG】第六节——MPU6050的简单测试

之前第五节写成了4，不过无所谓了

闲聊：这一篇是简单驱动MPU6050的，之前按照预计更新计划，更新了串口和oled的SPI，不过都是简单操作，这一篇同样也是简单测试，只是一个数据的读取和显示，实际配置起来不用几分钟就能配完。但是实际上MPU6050有非常多可以拿来玩的东西，我这一篇实际上只是移植了别人的库，如果按照正常的读手册到配置到使用官方的库，要花费大把时间，要学的东西也很多，这边还是希望各位能自己读读手册或者看看某点原子或者某火的教程深入学习一下，这边只是一个简单的测试，后续会打算补一个完整的mpu6050的测试报告。

叠甲和声明：本篇只是学习用，只能说是对MPU6050的一个大概理解，希望各位读者不要觉得看了就会了，实际上官方手册里面可是有太多东西了，你照着我这篇去测试下来，除了知道怎么获得这个数据，对mpu6050的所有配置和驱动你基本上都不知道，要深入学习一下建议还是看官方。

补充：目前已完成作品，是一个简易的计步器，但是作品里面依赖的是官方的算法，使用的是DMP库自带的七步计数器，然后这一篇是我学习过程中找到的快速上手的方法，也就是获取两组数据的办法，只需要移植一个库就行，并不需要一个一个的去更改官方库，至于官方的如何移植在后续总报告进行一个小分析。

一、MPU6050的简单理论

1、MPU6050是啥:MPU6050是一款六轴传感器，包括一个三轴陀螺仪和一个三轴加速度计,所以说可以获得两组数据。

2、功能：MPU6050可以测量物体的加速度和角速度。这使其非常适合用于测量物体的运动、方向和倾斜角度。这些数据可以用于各种应用，如无人机、机器人、游戏控制器、虚拟现实设备等。

3、通信接口：MPU6050通过I2C接口与微控制器或单片机通信，一般MPU6050有以下引脚

VCC（供电）
GND
SCL（IIC通信）
SDA（IIC通信）
AD0（地址引脚，一般是接GND）
INT（中断口，官方库会用到）
XCL（这两个暂时不管，想了解请自己查查，我没搞过车，不是很了解那些佬用的什么电磁传感器啥的）
XDA

二、cubemx配置

1、SYS和RCC不说了,时钟树也是

2、IIC配置打开，且fastmode，快速模式，如下图

3、生成就行了，就是这么简单

三、添加mpu6050文件

1、首先把两个.c和.h文件存在一个文件夹里面，然后命名，放到工程目录

2、添加文件路径，不放图了蛤，跟oled那期一样的

3、两个文件我放到末尾

四、main函数编写

1、添加库

/* USER CODE BEGIN Includes */
#include "mpu6050.h"
/* USER CODE END Includes */

2、定义结构体

/* USER CODE BEGIN PV */
MPU6050_t MPU6050;
/* USER CODE END PV */

3、初始化

  /* USER CODE BEGIN 2 */
  OLED_Init();
	OLED_Clear();
  OLED_Display_On();
	while (MPU6050_Init(&hi2c2) == 1);
//oled别管，上一节测试留下的
  /* USER CODE END 2 */

4、读取的函数

这里在h文件里面有如下四个，看着想用哪个用哪个，注，前面cubemx配置的IIC2，所以这里是2

MPU6050_Read_All	//所有参数
MPU6050_Read_Accel	//加速度
MPU6050_Read_Gyro	//陀螺仪
MPU6050_Read_Temp	//温度

//比如我这里用的
MPU6050_Read_All(&hi2c2, &MPU6050);

5、串口调试

说实在，这一篇我10.7就写完了，然后没上实物拍照，拖到现在，我就不用oled展示了，这边直接使用串口打印参数。

	printf("加速度计参数 X1:%.3f \t\r Y1:%.3f \t\r Z1:%.3f\t\r\n",MPU6050.Ax,MPU6050.Ay,MPU6050.Az);
	printf("陀螺仪参数 X2:%.3f \t\r Y2:%.3f \t\r Z2:%.3f\t\r\n",MPU6050.Gx,MPU6050.Gy,MPU6050.Gz);
	printf("温度参数 %.3f\r\n",MPU6050.Temperature);
//补充一下，前面的结构体可以去看看，里面能解释为什么我这边是这样，结构体的内容去看文章后面的两个文件吧

6、可以看到串口打印如下参数，如图

最后给个简单的演示视频吧，因为没用oled，所以看着没那么顺滑。

video1

五、两个文件+总结

1、mpu6050.c

/*
 * 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"
#include "main.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;
// const double Accel_Z_corrector = 16384.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
 *
 *  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的其他知识，包括四元数等等，滤波算法的计算，都是不知道的，然后作品已经做完了，下一篇报告就是完整的项目展示，同时会重点介绍官方的DMP库是如何实现的。

补充两点

1、移植官方库的过程非常繁琐，我也无法短时间内，一个十分钟内的视频去介绍，所以我在最终的视频报告只会讲主要部分。

2、官方的DMP库确实有计步器的功能，这个算法依赖于七步计数，但是市面上的计步器产品，大多都是自己写的算法，作者也是首次学习mpu6050，所以使用的官方库。

3、声明：这个库是我看别人文章发现的，如有侵权，请联系我

Jacktang · 发表于2023-10-25 07:26

移植了别人的开源库进行使用，也是实际开发人常用的技法

只要关键地方是自己写的就行，，，

哈哈，支持楼主

Verifone · 发表于2023-10-25 08:47

有开源库可以用还是可以缩短很多开发时间的，自己写库太麻烦了

【STM32L476RG】第六节——MPU6050的简单测试 [复制链接]

最新回复