/*更新时间2024年9/17中秋节于重庆理工大学*/
/*
* ===== Yaw角自加问题根本原因 =====
* 陀螺仪Z轴静止时不是完全的0，而是存在一个小偏置（零漂）
* 这个偏置值虽然很小，但持续累加会导致Yaw角不断增加
*
* ===== 解决方案 =====
* 1. 开机时对陀螺仪进行标定，获取Z轴的零漂偏置值
* 2. 每次读取陀螺仪数据时都减去这个偏置值
* 3. 这样就能彻底解决Yaw角自加的问题
*/
#include "mpu6050.h"
#include "math.h"
//定义6个int16_t的数据，一会用于对于接收的数据拼接
//int16_t是有负数的，uint16_t是没负数的
int16_t Accel_X_RAW = 0;
int16_t Accel_Y_RAW = 0;
int16_t Accel_Z_RAW = 0;
int16_t Gyro_X_RAW = 0;
int16_t Gyro_Y_RAW = 0;
int16_t Gyro_Z_RAW = 0;
//这6个是把拼接好的数据经过计算得到结果
float Ax, Ay, Az, Gx, Gy, Gz;
//使用加速度计算《加速度欧拉角》，先定义几个变量。_a的意思就是用的加速度
float roll_a, pitch_a;
//使用角速度计算《角速度欧拉角》，先定义几个变量。_g的意思就是用的角速度
float roll_g, pitch_g, yaw_g;
//欧拉角
float roll, pitch, yaw;
//========== Yaw角零漂补偿 ==========
//陀螺仪Z轴零漂标定值（单位：度/秒） - 需要通过标定函数获取
float Gz_offset = 0.0f;
//标定陀螺仪Z轴零漂的函数
//方法：将陀螺仪放在平稳不动的地方，调用此函数收集100个样本计算平均值
void MPU6050_Calibrate_Yaw(void) {
float Gz_sum = 0.0f;
uint16_t samples = 100;
for(uint16_t i = 0; i < samples; i++) {
uint8_t Rec_Data_G[6];
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data_G, 6, 1000);
Gyro_Z_RAW = (int16_t)(Rec_Data_G[4] << 8 | Rec_Data_G[5]);
Gz_sum += Gyro_Z_RAW / 16.384; // 转换为度/秒
HAL_Delay(10); // 延迟10ms再读下一个
}
Gz_offset = Gz_sum / samples; // 计算平均零漂值
}
void MPU6050_Init(void)//初始化
{
uint8_t check;
uint8_t Data;
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, 1000);
//注意。我的板子I2C1给显示屏了，所以给陀螺仪的是I2C2，故用&hi2c3
if (check == 0x68)//寄存器文档最后一页写了WHO_AM_I_REG就是0x68
{
//电源管理1
Data = 0x01;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, 1000);
//电源管理2
Data = 0x00;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, PWR_MGMT_2_REG, 1, &Data, 1, 1000);
//滤波 - 设置为0x06(5Hz带宽)以提高稳定性
Data = 0x06;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, CONFIG, 1, &Data, 1, 1000);
//采样频率分频器寄存器 - 设置为0x04(200Hz采样率)以获得更多数据点
Data = 0x04;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, 1000);
//加速度计配置
// Data = 0x18;
Data = 0x00;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, 1000);
//陀螺仪配置
Data = 0x18;
HAL_I2C_Mem_Write(&hi2c3, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, 1000);
//初始化后需要调用标定函数
//MPU6050_Calibrate_Yaw(); // 在main中调用，或其他初始化位置
}
}
void MPU6050_Read_Accel(void)//读取加速度
{
uint8_t Rec_Data[6];
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, 1000);
Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
Ax = Accel_X_RAW / 2048.0;
Ay = Accel_Y_RAW / 2048.0;
Az = Accel_Z_RAW / 2048.0 - 0.5;//此处多减0.5是我的硬件问题，如果你硬件是好的，就不用减
}
//此处请特别注意！！！！！！我硬件有问题减了0.5！！！！你们用的时候可以先不减
void MPU6050_Read_Gyro(void)//读取角速度
{
uint8_t Rec_Data[6];
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, 1000);
Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
Gx = Gyro_X_RAW / 16.384;
Gy = Gyro_Y_RAW / 16.384;
Gz = Gyro_Z_RAW / 16.384;
}
//此函数是读取结果，当然这个结果是互补滤波法计算的
//如果你想读取yaw roll pitch 仅需调用此函数
void MPU6050_Read_Result(void){
//上面函数MPU6050_Read_Accel()中的，改了下变量名，让大家更易懂，你也可以直接调用函数
uint8_t Rec_Data_A[6];
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data_A, 6, 1000);
Accel_X_RAW = (int16_t)(Rec_Data_A[0] << 8 | Rec_Data_A[1]);
Accel_Y_RAW = (int16_t)(Rec_Data_A[2] << 8 | Rec_Data_A[3]);
Accel_Z_RAW = (int16_t)(Rec_Data_A[4] << 8 | Rec_Data_A[5]);
Ax = Accel_X_RAW / 2048.0;
Ay = Accel_Y_RAW / 2048.0;
// Az = Accel_Z_RAW / 2048.0 - 0.5; ← 先注释掉这行
Az = Accel_Z_RAW / 16384.0; // ← 改成这样（不减0.5，用16384）
//上面函数MPU6050_Read_Gyro()中的，改了下变量名，让大家更易懂，你也可以直接调用函数
uint8_t Rec_Data_G[6];
HAL_I2C_Mem_Read(&hi2c3, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data_G, 6, 1000);
Gyro_X_RAW = (int16_t)(Rec_Data_G[0] << 8 | Rec_Data_G[1]);
Gyro_Y_RAW = (int16_t)(Rec_Data_G[2] << 8 | Rec_Data_G[3]);
Gyro_Z_RAW = (int16_t)(Rec_Data_G[4] << 8 | Rec_Data_G[5]);
Gx = Gyro_X_RAW / 16.384;
Gy = Gyro_Y_RAW / 16.384;
Gz = Gyro_Z_RAW / 16.384;
//使用加速度计算欧拉角，如果用3.1415926，计算结果会慢，C8T6算小数太慢
roll_a = atan2(Ay, Az) /3.14f * 180;
pitch_a = - atan2(Ax, Az) /3.14f * 180;
//使用角速度计算欧拉角
//★★★ 关键修改：从陀螺仪读数中减去零漂偏置值，彻底解决Yaw角自加 ★★★
yaw_g = yaw + (Gz - Gz_offset) * 0.005; // 减去零漂，消除自加
roll_g = roll + Gx * 0.005;
pitch_g = pitch + Gy * 0.005;
//进行互补融合 - 修正融合公式以提高稳定性
const float alpha = 0.95;//这个0.98可以改，0-1之间的数，一般在0.95往上，一次增加0.01调试一下
//正确的互补滤波: 加速度权重较小用于长期纠正，陀螺仪权重较大用于实时响应
roll = alpha * roll_g + (1.0f - alpha) * roll_a;
pitch = alpha * pitch_g + (1.0f - alpha) * pitch_a;
//Yaw角: 由于加速度计无法检测Yaw旋转，Yaw仅依赖陀螺仪
//但已经通过零漂补偿消除了静止时的自加问题
yaw = yaw_g;
}
#ifndef __MPU6050_H__
#define __MPU6050_H__
//这是引用的头文件
#include "main.h"
#include "i2c.h"
#include"math.h"
//这一堆都是地址，可以看寄存器的文件
#define MPU6050_ADDR 0xD0//从机地址
#define SMPLRT_DIV_REG 0x19//采样率
#define CONFIG 0x1A
#define GYRO_CONFIG_REG 0x1B//陀螺仪
#define ACCEL_CONFIG_REG 0x1C//加速度
#define ACCEL_XOUT_H_REG 0x3B
#define TEMP_OUT_H_REG 0x41
#define GYRO_XOUT_H_REG 0x43
#define PWR_MGMT_1_REG 0x6B
#define PWR_MGMT_2_REG 0x6C
#define WHO_AM_I_REG 0x75
//这是咱们的函数，初始化，读A，读G
void MPU6050_Init(void);
void MPU6050_Read_Accel(void);
void MPU6050_Read_Gyro(void);
void MPU6050_Read_Result(void);//读roll, pitch, yaw
//补偿函数
void MPU6050_Calibrate_Yaw(void);
//导出，导出后数据才能被其他程序应用
extern float Ax, Ay, Az, Gx, Gy, Gz,roll, pitch, yaw;//A代表加速度，G代表角速度
#endif
请基于我的陀螺仪库帮我实现一个7步法的计步函数，简单实现即可
作者毕业后成为通信工程师，因感到迷茫和缺乏激情而决定转行。27岁那年，他选择从头开始学习Python，希望在IT行业找到自己的位置。
扫一扫
1897
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
