目录
前言
在嵌入式开发中，实时姿态检测是平衡车、无人机、机器人等项目的常见需求。MPU6050作为一款集成三轴陀螺仪与三轴加速度计的运动传感器，凭借其高性价比和易用性，成为众多初学者的首选方案。
然而，直接使用MPU6050的原始数据存在明显缺陷：陀螺仪存在积分漂移，加速度计噪声较大。为获得平滑、准确的姿态信息，通常需要采用滤波算法进行数据融合。
本文将详细介绍如何在CCS上基于MSPM0G3507单片机，通过软件模拟I2C协议读取MPU6050数据，并结合卡尔曼滤波器实现数据融合，最终稳定检测并显示俯仰角（Pitch）、横滚角（Roll）与偏航角（Yaw）。
先看看最终效果：
一、卡尔曼滤波相关知识
卡尔曼滤波是一种基于递归估计的最优状态估计算法。在MPU6050的姿态融合中，它的目标是从带有噪声的陀螺仪和加速度计数据中，实时估计出真实的姿态角度（如俯仰角Pitch和横滚角Roll）。理解其原理需要掌握两个核心概念：先验估计与后验估计，以及它们如何通过“预测-更新”的循环不断逼近真实值。
卡尔曼滤波是一个递归过程：
-
根据上一时刻的后验估计，通过预测得到当前时刻的先验估计。
-
获得当前时刻的加速度计观测值后，结合先验估计计算出卡尔曼增益。
-
用增益修正先验估计，得到当前时刻的后验估计。
-
更新协方差，为下一时刻的预测做准备。
-
重复1-4。
正是通过这种“预测-修正”的递归，卡尔曼滤波器能够实时输出丝滑且准确的姿态角度，既消除了陀螺仪的积分漂移，又滤除了加速度计的高频噪声。
这里推荐一位B站up讲的，通俗易懂：
二、软件I2C读写MPU6050
在理解了卡尔曼滤波的基本原理后，接下来开始实操，首先从MPU6050获取原始数据。
1. 引脚连接
| SCL | B9 |
| SDA | B8 |
这里mpu6050的SCL和SDA都用GPIO，B8、B9都是相同配置：
2. 代码实现
在正式编写MPU6050的驱动之前，需要先将底层的I2C通信封装成模块化的文件，方便后续调用。
myi2c.c
#include "myi2c.h"
void SDA_OUT(void)
{
DL_GPIO_enableOutput(mpu6050_PORT, mpu6050_B8_PIN);
}
void SDA_IN(void)
{
DL_GPIO_disableOutput(mpu6050_PORT, mpu6050_B8_PIN);
}
void i2c_scl_write(uint8_t bit)
{
if(bit)
{
DL_GPIO_setPins(mpu6050_PORT, mpu6050_B9_PIN);
}
else
{
DL_GPIO_clearPins(mpu6050_PORT, mpu6050_B9_PIN);
}
delay_us(10);
}
void i2c_sda_write(uint8_t bit)
{
SDA_OUT();
if(bit)
{
DL_GPIO_setPins(mpu6050_PORT, mpu6050_B8_PIN);
}
else
{
DL_GPIO_clearPins(mpu6050_PORT, mpu6050_B8_PIN);
}
delay_us(10);
}
uint8_t i2c_sda_read(void)
{
uint8_t value;
SDA_IN();
if(DL_GPIO_readPins(mpu6050_PORT, mpu6050_B8_PIN))
{
value = 1;
}
else
{
value = 0;
}
delay_us(10);
return value;
}
void i2c_init(void)
{
// B8推挽输出，高电平
DL_GPIO_initDigitalOutput(mpu6050_B8_IOMUX);
DL_GPIO_setPins(mpu6050_PORT, mpu6050_B8_PIN); // 初始高
DL_GPIO_enableOutput(mpu6050_PORT, mpu6050_B8_PIN);
DL_GPIO_initDigitalInputFeatures(mpu6050_B8_IOMUX,
DL_GPIO_INVERSION_DISABLE,
DL_GPIO_RESISTOR_PULL_UP, // 内部上拉
DL_GPIO_HYSTERESIS_DISABLE,
DL_GPIO_WAKEUP_DISABLE);
}
void i2c_start(void)
{
SDA_OUT();
i2c_sda_write(1);
i2c_scl_write(1);
i2c_sda_write(0);
i2c_scl_write(0);
}
void i2c_stop(void)
{
SDA_OUT();
i2c_sda_write(0);
i2c_scl_write(1);
i2c_sda_write(1);
SDA_IN();
}
void i2c_sendbyte(uint8_t byte)
{
SDA_OUT();
uint8_t i;
for(i = 0;i < 8;i++)
{
i2c_sda_write(byte & (0x80 >> i));
i2c_scl_write(1);
i2c_scl_write(0);
}
}
uint8_t i2c_receivebyte(void)
{
uint8_t byte = 0x00;
uint8_t i;
SDA_IN();
for(i = 0; i < 8; i++)
{
i2c_scl_write(1);
delay_us(2);
if(i2c_sda_read() == 1)
{
byte |= (0x80 >> i);
}
i2c_scl_write(0);
if(i < 7)
{
delay_us(2);
}
}
// SDA_OUT();
return byte;
}
void i2c_sendack(uint8_t ack)
{
SDA_OUT();
i2c_sda_write(ack);
i2c_scl_write(1);
i2c_scl_write(0);
}
uint8_t i2c_receiveack(void)
{
uint8_t ack;
SDA_IN();
i2c_scl_write(1);
delay_us(5);
ack = i2c_sda_read();
i2c_scl_write(0);
// SDA_OUT();
return ack;
}
myi2c.h
#ifndef _MYI2C_H_
#define _MYI2C_H_
#include "ti_msp_dl_config.h"
#include "delay.h"
void i2c_init(void);
void i2c_start(void);
void i2c_stop(void);
void i2c_sendbyte(uint8_t byte);
uint8_t i2c_receivebyte(void);
void i2c_sendack(uint8_t ack);
uint8_t i2c_receiveack(void);
#endif /* #ifndef _MSPM0_I2C_H_ */
有了稳定的I2C底层驱动，接下来就可以封装MPU6050的专用读写函数，包括初始化 、数据读取等。
mpu.c
#include "mpu.h"
#define mpu_address 0xD0
void mpu_writeid(uint8_t address,uint8_t data)
{
i2c_start();
i2c_sendbyte(mpu_address); //从机地址+写入
i2c_receiveack();
i2c_sendbyte(address); //寄存器地址
i2c_receiveack();
i2c_sendbyte(data);
i2c_receiveack();
i2c_stop();
}
uint8_t mpu_readid(uint8_t address)
{
uint8_t data;
i2c_start();
i2c_sendbyte(mpu_address); //从机地址
i2c_receiveack();
i2c_sendbyte(address); //寄存器地址
i2c_receiveack();
i2c_start();
i2c_sendbyte(mpu_address | 0x01); //从机地址+读取
i2c_receiveack();
data = i2c_receivebyte();
i2c_sendack(1); //给从机应答
i2c_stop();
return data;
}
void mpu_init(void)
{
i2c_init();
mpu_writeid(MPU6050_PWR_MGMT_1,0x01); //解除睡眠，陀螺仪时钟
mpu_writeid(MPU6050_PWR_MGMT_2,0x00); //6个轴均不待机
mpu_writeid(MPU6050_SMPLRT_DIV,0x09); //数据输出速度，采样分频10
mpu_writeid(MPU6050_CONFIG,0x06); //滤波参数
mpu_writeid(MPU6050_GYRO_CONFIG,0x18); //陀螺仪量程
mpu_writeid(MPU6050_ACCEL_CONFIG,0x18); //加速度量程
}
void mpu_getdata(int16_t *ax,int16_t *ay,int16_t *az,int16_t *gx,int16_t *gy,int16_t *gz)
{
uint16_t data_h,data_l;
data_h = mpu_readid(MPU6050_ACCEL_XOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_XOUT_L);
*ax = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_ACCEL_YOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_YOUT_L);
*ay = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_ACCEL_ZOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_ZOUT_L);
*az = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_XOUT_H);
data_l = mpu_readid(MPU6050_GYRO_XOUT_L);
*gx = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_YOUT_H);
data_l = mpu_readid(MPU6050_GYRO_YOUT_L);
*gy = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_ZOUT_H);
data_l = mpu_readid(MPU6050_GYRO_ZOUT_L);
*gz = (data_h << 8) | data_l;
}
uint8_t mpu_getid(void)
{
return mpu_readid(MPU6050_WHO_AM_I);
}
mpu.h
#ifndef __MPU_H
#define __MPU_H
#include "ti_msp_dl_config.h"
#include "myi2c.h"
#include "mpu_id.h"
void mpu_writeid(uint8_t address,uint8_t data);
uint8_t mpu_readid(uint8_t address);
void mpu_init(void);
void mpu_getdata(int16_t *ax,int16_t *ay,int16_t *az,int16_t *gx,int16_t *gy,int16_t *gz);
uint8_t mpu_getid(void);
#endif
mpu_id.h
#ifndef __MPU_ID_H
#define __MPU_ID_H
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
#endif
empty.c
/*
* Copyright (c) 2021, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ti_msp_dl_config.h"
#include "delay.h"
#include "OLED.h"
#include "myuart.h"
#include "mpu.h"
uint8_t id;
int16_t ax,ay,az, gx,gy,gz;
int main(void)
{
SYSCFG_DL_init();
NVIC_ClearPendingIRQ(UART1_INST_INT_IRQN);
NVIC_EnableIRQ(UART1_INST_INT_IRQN);
OLED_Init();
OLED_Clear();
mpu_init();
id = mpu_getid();
// OLED_ShowNum(96, 0, id, 3, 16);
while (1)
{
mpu_getdata(&ax,&ay,&az,&gx,&gy,&gz);
OLED_ShowNum(0, 0, ax, 5, 16);
OLED_ShowNum(0, 2, ay, 5, 16);
OLED_ShowNum(0, 4, az, 5, 16);
OLED_ShowNum(48, 0, gx, 5, 16);
OLED_ShowNum(48, 2, gy, 5, 16);
OLED_ShowNum(48, 4, gz, 5, 16);
printf("%d,%d,%d,%d,%d,%d\r\n",ax,ay,az,gx,gy,gz);
}
}
现在运行测试一下，看看效果：
（OLED频闪是手机录制的问题）
此时ax, ay, az, gx, gy, gz是mpu6050的原始数据，存在较强噪声：
接下来就需要通过卡尔曼滤波算法进行数据融合，消除噪声与漂移，输出丝滑的姿态角度。
三、卡尔曼滤波
有了从MPU6050读取的原始加速度和角速度数据，然后通过卡尔曼滤波对它们进行融合，以解决陀螺仪积分漂移和加速度计噪声大的问题。
卡尔曼滤波相关代码：
float Offset_ax = 0.0;
float Offset_ay = 0.0;
float Offset_az = 0.0;
float Offset_gx = 0.0;
float Offset_gy = 0.0;
float Offset_gz = 0.0;
void mpuout_get_realdata(float *AccX, float *AccY, float *AccZ, float *GyroX, float *GyroY, float *GyroZ)
{
int16_t AccX_Temp;
int16_t AccY_Temp;
int16_t AccZ_Temp;
static int16_t GyroX_Temp;
static int16_t GyroY_Temp;
static int16_t GyroZ_Temp;
/*获取MPU6050原始数据*/
mpu_getdata(&AccX_Temp, &AccY_Temp, &AccZ_Temp, &GyroX_Temp, &GyroY_Temp, &GyroZ_Temp);
// 加速度值 = 原始数据 / 灵敏度
*AccX = AccX_Temp / AccSPL - Offset_ax;
*AccY = AccY_Temp / AccSPL - Offset_ay;
*AccZ = AccZ_Temp / AccSPL - Offset_az;
// *AccX = AccX_Temp / AccSPL;
// *AccY = AccY_Temp / AccSPL;
// *AccZ = AccZ_Temp / AccSPL;
// 角速度值 = 原始数据 / 灵敏度
*GyroX = GyroX_Temp / GyroSPL - Offset_gx;
*GyroY = GyroY_Temp / GyroSPL - Offset_gy;
// *GyroX = GyroX_Temp / GyroSPL;
// *GyroY = GyroY_Temp / GyroSPL;
*GyroZ = GyroZ_Temp / GyroSPL - Offset_gz;
}
void mpuout_getoffset(void)
{
uint16_t i;
float Count = 100.0;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
static float Offset_a[3];
static float Offset_g[3];
delay_ms(10);
for (i = 0; i < Count; i++)
{
delay_us(20);
mpuout_get_realdata(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
Offset_a[0] += AccX;
Offset_a[1] += AccY;
Offset_a[2] += AccZ;
Offset_g[0] += GyroX;
Offset_g[1] += GyroY;
Offset_g[2] += GyroZ;
}
/*求出平均值*/
Offset_a[0] /= Count;
Offset_a[1] /= Count;
Offset_a[2] /= Count;
Offset_g[0] /= Count;
Offset_g[1] /= Count;
Offset_g[2] /= Count;
/*求出偏移量*/
Offset_ax = Offset_a[0] - 0.0;
Offset_ay = Offset_a[1] - 0.0;
Offset_az = Offset_a[2] - 1.0; // Z轴加速度标准是等于1g
Offset_gx = Offset_g[0] - 0.0;
Offset_gy = Offset_g[1] - 0.0;
Offset_gz = Offset_g[2] - 0.0;
}
void mpuout_getangle(float *Roll_A, float *Pitch_A, float *Roll_G, float *Pitch_G, float *Yaw_G)
{
/*定义积分时间变量*/
static float dt = 0.01;
float AccX, AccY, AccZ;
static float GyroX, GyroY, GyroZ;
mpuout_get_realdata(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
/*加速度求夹角*/
/* 横滚角 = atan2(AccY, Accz) * 180 / Π */
*Roll_A = atan2(AccY, AccZ) * 180 / 3.141593;
/* 俯仰角 = -atan2(AccX, sqrt(AccY * AccY + AccZ * Accz)) * 180 / Π */
*Pitch_A = -atan2(AccX, sqrt( AccY * AccY + AccZ * AccZ ) ) * 180 / 3.141593;
/*角速度求夹角*/
*Roll_G += GyroX *dt; //公式： 角度 += 角速度 * dt
*Pitch_G += GyroY *dt;
*Yaw_G += GyroZ *dt;
}
float kalman_roll(float Angle_a, float Angle_g)
{
static float Angle; //最终角度
float dt = 0.01; //采样周期即计算任务周期10ms
float Q_Angle = 0.001; //角度数据置信度，角度噪声的协方差
float Q_Gyro = 0.003; //角速度数据置信度，角速度噪声协方差
float R_Angle = 0.5; //加速度计测量噪声的协方差
float Q_Bias = 0; //上次最优估计值偏差
float Angle_err; //角度偏差
float K_0; //用于计算最优估计值
float K_1; //用于计算最优估计值的偏差
static float P[2][2] = { {1, 0}, {0, 1} }; //过程协方差矩阵P，初始值为单位阵
/*Step1: 先验估计*/
/* 状态方程,角度值等于上次最优角度加角速度减零漂后积分 */
Angle += (Angle_g - Q_Bias) * dt;
/*Step2: 预测协方差矩阵 */
P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
P[0][1] += -P[1][1] * dt;
P[1][0] += -P[1][1] * dt;
P[1][1] += Q_Gyro * dt;
/*Step3: 计算卡尔曼增益 */
K_0 = P[0][0] / ( R_Angle + P[0][0] );
K_1 = P[0][1] / ( R_Angle + P[0][0] );
/*Step4: 更新协方差矩阵 */
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
/*Step5: 计算最优角度值 */
Angle_err = Angle_a - Angle; //先验估计，计算角度偏差
Angle += K_0 * Angle_err; //后验估计，得到最优估计值
Q_Bias += K_1 * Angle_err; //后验估计，得到最优估计偏差
return Angle;
}
float kalman_pitch(float Angle_a, float Angle_g)
{
static float Angle;
static float dt = 0.01; //采样周期即计算任务周期10ms
static float Q_Angle = 0.001; //角度数据置信度，角度噪声的协方差
static float Q_Gyro = 0.003; //角速度数据置信度，角速度噪声协方差
static float R_Angle = 0.5; //加速度计测量噪声。抖动时数值越大曲线越平滑,但是响应也会变慢。
static float Q_Bias; //上次最优估计值偏差
static float Angle_err; //角度偏差
static float K_0; //用于计算最优估计值
static float K_1; //用于计算最优估计值的偏差
static float P[2][2] = { {1, 0}, {0, 1} }; //过程协方差矩阵
/*Step1: 先验估计 */
Angle += (Angle_g - Q_Bias) * dt;
/*Step2: 计算过程协方差矩阵的微分 */
P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
P[0][1] += -P[1][1] * dt;
P[1][0] += -P[1][1] * dt;
P[1][1] += Q_Gyro * dt;
/*Step3: 计算卡尔曼增益 */
K_0 = P[0][0] / ( R_Angle + P[0][0] );
K_1 = P[0][1] / ( R_Angle + P[0][0] );
/*Step4: 后验估计误差协方差 */
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
/*Step: 计算最优角度 */
Angle_err = Angle_a - Angle; //先验估计，计算角度偏差
Angle +=K_0 * Angle_err; //后验估计，得到最优估计值
Q_Bias +=K_1 * Angle_err; //后验估计，得到最优估计偏差
return Angle;
}
void kalman_getangle(float *Roll, float *Pitch, float *Yaw)
{
/*定义暂存加速度欧拉角变量*/
static float Roll_a;
static float Pitch_a;
// static float Yaw_a;
/*定义暂存角速度欧拉角变量*/
static float Roll_g;
static float Pitch_g;
static float Yaw_g;
/*读取加速度和角速度的原始欧拉角*/
mpuout_getangle(&Roll_a, &Pitch_a, &Roll_g, &Pitch_g, &Yaw_g);
/*对加速度和角速度欧拉角进行卡尔曼滤波*/
*Roll = kalman_roll(Roll_a, Roll_g);
*Pitch = kalman_pitch(Pitch_a, Pitch_g);
*Yaw = Yaw_g * 5.62;
}
这里我们将卡尔曼滤波代码部分接在mpu.c后面，完整代码如下：
mpu.c
#include "mpu.h"
#define mpu_address 0xD0
float Offset_ax = 0.0;
float Offset_ay = 0.0;
float Offset_az = 0.0;
float Offset_gx = 0.0;
float Offset_gy = 0.0;
float Offset_gz = 0.0;
void mpu_writeid(uint8_t address,uint8_t data)
{
i2c_start();
i2c_sendbyte(mpu_address); //从机地址+写入
i2c_receiveack();
i2c_sendbyte(address); //寄存器地址
i2c_receiveack();
i2c_sendbyte(data);
i2c_receiveack();
i2c_stop();
}
uint8_t mpu_readid(uint8_t address)
{
uint8_t data;
i2c_start();
i2c_sendbyte(mpu_address); //从机地址
i2c_receiveack();
i2c_sendbyte(address); //寄存器地址
i2c_receiveack();
i2c_start();
i2c_sendbyte(mpu_address | 0x01); //从机地址+读取
i2c_receiveack();
data = i2c_receivebyte();
i2c_sendack(1); //给从机应答
i2c_stop();
return data;
}
void mpu_init(void)
{
i2c_init();
mpu_writeid(MPU6050_PWR_MGMT_1,0x01); //解除睡眠，陀螺仪时钟
mpu_writeid(MPU6050_PWR_MGMT_2,0x00); //6个轴均不待机
mpu_writeid(MPU6050_SMPLRT_DIV,0x09); //数据输出速度，采样分频10
mpu_writeid(MPU6050_CONFIG,0x06); //滤波参数
mpu_writeid(MPU6050_GYRO_CONFIG,0x18); //陀螺仪量程
mpu_writeid(MPU6050_ACCEL_CONFIG,0x18); //加速度量程
}
void mpu_getdata(int16_t *ax,int16_t *ay,int16_t *az,int16_t *gx,int16_t *gy,int16_t *gz)
{
uint16_t data_h,data_l;
data_h = mpu_readid(MPU6050_ACCEL_XOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_XOUT_L);
*ax = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_ACCEL_YOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_YOUT_L);
*ay = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_ACCEL_ZOUT_H);
data_l = mpu_readid(MPU6050_ACCEL_ZOUT_L);
*az = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_XOUT_H);
data_l = mpu_readid(MPU6050_GYRO_XOUT_L);
*gx = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_YOUT_H);
data_l = mpu_readid(MPU6050_GYRO_YOUT_L);
*gy = (data_h << 8) | data_l;
data_h = mpu_readid(MPU6050_GYRO_ZOUT_H);
data_l = mpu_readid(MPU6050_GYRO_ZOUT_L);
*gz = (data_h << 8) | data_l;
}
uint8_t mpu_getid(void)
{
return mpu_readid(MPU6050_WHO_AM_I);
}
// 卡尔曼滤波
void mpuout_get_realdata(float *AccX, float *AccY, float *AccZ, float *GyroX, float *GyroY, float *GyroZ)
{
int16_t AccX_Temp;
int16_t AccY_Temp;
int16_t AccZ_Temp;
static int16_t GyroX_Temp;
static int16_t GyroY_Temp;
static int16_t GyroZ_Temp;
/*获取MPU6050原始数据*/
mpu_getdata(&AccX_Temp, &AccY_Temp, &AccZ_Temp, &GyroX_Temp, &GyroY_Temp, &GyroZ_Temp);
// 加速度值 = 原始数据 / 灵敏度
*AccX = AccX_Temp / AccSPL - Offset_ax;
*AccY = AccY_Temp / AccSPL - Offset_ay;
*AccZ = AccZ_Temp / AccSPL - Offset_az;
// *AccX = AccX_Temp / AccSPL;
// *AccY = AccY_Temp / AccSPL;
// *AccZ = AccZ_Temp / AccSPL;
// 角速度值 = 原始数据 / 灵敏度
*GyroX = GyroX_Temp / GyroSPL - Offset_gx;
*GyroY = GyroY_Temp / GyroSPL - Offset_gy;
// *GyroX = GyroX_Temp / GyroSPL;
// *GyroY = GyroY_Temp / GyroSPL;
*GyroZ = GyroZ_Temp / GyroSPL - Offset_gz;
}
void mpuout_getoffset(void)
{
uint16_t i;
float Count = 100.0;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
static float Offset_a[3];
static float Offset_g[3];
delay_ms(10);
for (i = 0; i < Count; i++)
{
delay_us(20);
mpuout_get_realdata(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
Offset_a[0] += AccX;
Offset_a[1] += AccY;
Offset_a[2] += AccZ;
Offset_g[0] += GyroX;
Offset_g[1] += GyroY;
Offset_g[2] += GyroZ;
}
/*求出平均值*/
Offset_a[0] /= Count;
Offset_a[1] /= Count;
Offset_a[2] /= Count;
Offset_g[0] /= Count;
Offset_g[1] /= Count;
Offset_g[2] /= Count;
/*求出偏移量*/
Offset_ax = Offset_a[0] - 0.0;
Offset_ay = Offset_a[1] - 0.0;
Offset_az = Offset_a[2] - 1.0; // Z轴加速度标准是等于1g
Offset_gx = Offset_g[0] - 0.0;
Offset_gy = Offset_g[1] - 0.0;
Offset_gz = Offset_g[2] - 0.0;
}
void mpuout_getangle(float *Roll_A, float *Pitch_A, float *Roll_G, float *Pitch_G, float *Yaw_G)
{
/*定义积分时间变量*/
static float dt = 0.01;
float AccX, AccY, AccZ;
static float GyroX, GyroY, GyroZ;
mpuout_get_realdata(&AccX, &AccY, &AccZ, &GyroX, &GyroY, &GyroZ);
/*加速度求夹角*/
/* 横滚角 = atan2(AccY, Accz) * 180 / Π */
*Roll_A = atan2(AccY, AccZ) * 180 / 3.141593;
/* 俯仰角 = -atan2(AccX, sqrt(AccY * AccY + AccZ * Accz)) * 180 / Π */
*Pitch_A = -atan2(AccX, sqrt( AccY * AccY + AccZ * AccZ ) ) * 180 / 3.141593;
/*角速度求夹角*/
*Roll_G += GyroX *dt; //公式： 角度 += 角速度 * dt
*Pitch_G += GyroY *dt;
*Yaw_G += GyroZ *dt;
}
float kalman_roll(float Angle_a, float Angle_g)
{
static float Angle; //最终角度
float dt = 0.01; //采样周期即计算任务周期10ms
float Q_Angle = 0.001; //角度数据置信度，角度噪声的协方差
float Q_Gyro = 0.003; //角速度数据置信度，角速度噪声协方差
float R_Angle = 0.5; //加速度计测量噪声的协方差
float Q_Bias = 0; //上次最优估计值偏差
float Angle_err; //角度偏差
float K_0; //用于计算最优估计值
float K_1; //用于计算最优估计值的偏差
static float P[2][2] = { {1, 0}, {0, 1} }; //过程协方差矩阵P，初始值为单位阵
/*Step1: 先验估计*/
/* 状态方程,角度值等于上次最优角度加角速度减零漂后积分 */
Angle += (Angle_g - Q_Bias) * dt;
/*Step2: 预测协方差矩阵 */
P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
P[0][1] += -P[1][1] * dt;
P[1][0] += -P[1][1] * dt;
P[1][1] += Q_Gyro * dt;
/*Step3: 计算卡尔曼增益 */
K_0 = P[0][0] / ( R_Angle + P[0][0] );
K_1 = P[0][1] / ( R_Angle + P[0][0] );
/*Step4: 更新协方差矩阵 */
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
/*Step5: 计算最优角度值 */
Angle_err = Angle_a - Angle; //先验估计，计算角度偏差
Angle += K_0 * Angle_err; //后验估计，得到最优估计值
Q_Bias += K_1 * Angle_err; //后验估计，得到最优估计偏差
return Angle;
}
float kalman_pitch(float Angle_a, float Angle_g)
{
static float Angle;
static float dt = 0.01; //采样周期即计算任务周期10ms
static float Q_Angle = 0.001; //角度数据置信度，角度噪声的协方差
static float Q_Gyro = 0.003; //角速度数据置信度，角速度噪声协方差
static float R_Angle = 0.5; //加速度计测量噪声。抖动时数值越大曲线越平滑,但是响应也会变慢。
static float Q_Bias; //上次最优估计值偏差
static float Angle_err; //角度偏差
static float K_0; //用于计算最优估计值
static float K_1; //用于计算最优估计值的偏差
static float P[2][2] = { {1, 0}, {0, 1} }; //过程协方差矩阵
/*Step1: 先验估计 */
Angle += (Angle_g - Q_Bias) * dt;
/*Step2: 计算过程协方差矩阵的微分 */
P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
P[0][1] += -P[1][1] * dt;
P[1][0] += -P[1][1] * dt;
P[1][1] += Q_Gyro * dt;
/*Step3: 计算卡尔曼增益 */
K_0 = P[0][0] / ( R_Angle + P[0][0] );
K_1 = P[0][1] / ( R_Angle + P[0][0] );
/*Step4: 后验估计误差协方差 */
P[0][0] -= K_0 * P[0][0];
P[0][1] -= K_0 * P[0][1];
P[1][0] -= K_1 * P[0][0];
P[1][1] -= K_1 * P[0][1];
/*Step: 计算最优角度 */
Angle_err = Angle_a - Angle; //先验估计，计算角度偏差
Angle +=K_0 * Angle_err; //后验估计，得到最优估计值
Q_Bias +=K_1 * Angle_err; //后验估计，得到最优估计偏差
return Angle;
}
void kalman_getangle(float *Roll, float *Pitch, float *Yaw)
{
/*定义暂存加速度欧拉角变量*/
static float Roll_a;
static float Pitch_a;
// static float Yaw_a;
/*定义暂存角速度欧拉角变量*/
static float Roll_g;
static float Pitch_g;
static float Yaw_g;
/*读取加速度和角速度的原始欧拉角*/
mpuout_getangle(&Roll_a, &Pitch_a, &Roll_g, &Pitch_g, &Yaw_g);
/*对加速度和角速度欧拉角进行卡尔曼滤波*/
*Roll = kalman_roll(Roll_a, Roll_g);
*Pitch = kalman_pitch(Pitch_a, Pitch_g);
*Yaw = Yaw_g * 5.62;
}
mpu.h
#ifndef __MPU_H
#define __MPU_H
#include "ti_msp_dl_config.h"
#include "myi2c.h"
#include <stdint.h>
#include <math.h>
#include "mpu_id.h"
#include "delay.h"
void mpu_writeid(uint8_t address,uint8_t data);
uint8_t mpu_readid(uint8_t address);
void mpu_init(void);
void mpu_getdata(int16_t *ax,int16_t *ay,int16_t *az,int16_t *gx,int16_t *gy,int16_t *gz);
uint8_t mpu_getid(void);
void mpuout_get_realdata(float *AccX, float *AccY, float *AccZ, float *GyroX, float *GyroY, float *GyroZ);
void mpuout_getoffset(void);
void mpuout_getangle(float *Roll_A, float *Pitch_A, float *Roll_G, float *Pitch_G, float *Yaw_G);
float kalman_roll(float Angle_a, float Angle_g);
float kalman_pitch(float Angle_a, float Angle_g);
void kalman_getangle(float *Roll, float *Pitch, float *Yaw);
#endif
mpu_id.h
#ifndef __MPU_ID_H
#define __MPU_ID_H
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_ACCEL_XOUT_L 0x3C
#define MPU6050_ACCEL_YOUT_H 0x3D
#define MPU6050_ACCEL_YOUT_L 0x3E
#define MPU6050_ACCEL_ZOUT_H 0x3F
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_TEMP_OUT_H 0x41
#define MPU6050_TEMP_OUT_L 0x42
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_WHO_AM_I 0x75
// 卡尔曼滤波
#define AccSPL 16384.0 //加速度计灵敏度±2g
#define GyroSPL 16.4 //角速度灵敏度±2000°/s
#endif
empty.c
/*
* Copyright (c) 2021, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ti_msp_dl_config.h"
#include "delay.h"
#include "OLED.h"
#include "myuart.h"
#include "mpu.h"
#include "key.h"
uint8_t id;
float roll, pitch, yaw;
int main(void)
{
SYSCFG_DL_init();
NVIC_ClearPendingIRQ(UART1_INST_INT_IRQN);
NVIC_EnableIRQ(UART1_INST_INT_IRQN);
OLED_Init();
OLED_Clear();
mpu_init();
id = mpu_getid();
// OLED_ShowNum(96, 0, id, 3, 16);
while (1)
{
kalman_getangle(&roll, &pitch, &yaw);
OLED_ShowNum(0, 0, roll, 5, 16);
OLED_ShowNum(0, 2, pitch, 5, 16);
OLED_ShowNum(0, 4, yaw, 5, 16);
printf("%f,%f,%f\r\n",roll,pitch,yaw);
}
}
运行测试一下，看看效果：
可以发现，此时偏航角零飘比较大：
我们采用零偏校准，通过采集静止状态下的多组数据，每次读取原始数据后，减去对应的偏移量，用于后续数据修正。在main中加入mpuout_getoffset()。
empty.c
/*
* Copyright (c) 2021, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ti_msp_dl_config.h"
#include "delay.h"
#include "OLED.h"
#include "myuart.h"
#include "mpu.h"
#include "key.h"
uint8_t id;
float roll, pitch, yaw;
int main(void)
{
SYSCFG_DL_init();
NVIC_ClearPendingIRQ(UART1_INST_INT_IRQN);
NVIC_EnableIRQ(UART1_INST_INT_IRQN);
OLED_Init();
OLED_Clear();
mpu_init();
id = mpu_getid();
// OLED_ShowNum(96, 0, id, 3, 16);
mpuout_getoffset();
while (1)
{
kalman_getangle(&roll, &pitch, &yaw);
OLED_ShowNum(0, 0, roll, 5, 16);
OLED_ShowNum(0, 2, pitch, 5, 16);
OLED_ShowNum(0, 4, yaw, 5, 16);
printf("%f,%f,%f\r\n",roll,pitch,yaw);
}
}
现在好多了：
看看完整效果：
结语
以上内容是我个人在学习MSPM0与MPU6050过程中的一次实践记录，从底层I2C驱动到上层卡尔曼滤波的实现思路均基于个人理解与调试经验。该方案在实际测试中能够较好地抑制传感器噪声与漂移，输出相对丝滑的姿态角度，希望对有类似需求的读者有所启发。
由于水平有限，文中难免存在疏漏或不当之处，欢迎大家批评指正。如果你在尝试的过程中遇到问题，欢迎留言交流、共同探讨。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
