一、MPU6050模块简介
MPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器
DMP(Digital Motion Processor),而且还可以连接一个第三方数字传感器(如磁力计)。
可以通过IIC接口输出一个九轴信号(链接第三方数字传感器才可以,否则只有六轴信号)。
更加方便的是,有了DMP,可以结合InvenSense公司提供的运动处理资料库,实现姿态解算。
通过自带的DMP,可以通过IIC接口输出9轴融合演算的数据,大大降低了运动处理运算对操作系统
的负荷,同时也降低了开发难度。
其实,简单一句话说,陀螺仪就是测角速度的,加速度传感器就是测角加速度的,二者数据通过算
法就可以得到PITCH、YAW、ROLL角了。
PITCH(俯仰角)低头抬头
ROLL(翻滚角)翻身
YAW(偏航角)转弯
单位均为°
由右手螺旋定则判断正方向
二、HAL--CubeMX配置
三、关于相关寄存器的宏定义
#define SMPLRT_DIV 0x19 // 采样率分频,典型值:0x09--1000/(9+1)==100Hz-->10ms
#define CONFIG 0x1A // 低通滤波频率,典型值:0x06--1kHz
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
#define ACCEL_CONFIG 0x1C // 加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz) */0x00
// 存储最近的X轴、Y轴、Z轴加速度感应器的测量值 */3B-40
#define ACCEL_XOUT_H 0x3B X轴加速度测量值高位
#define ACCEL_XOUT_L 0x3C X轴加速度测量值低位
#define ACCEL_YOUT_H 0x3D Y轴加速度测量值高位
#define ACCEL_YOUT_L 0x3E Y轴加速度测量值低位
#define ACCEL_ZOUT_H 0x3F Z轴加速度测量值高位
#define ACCEL_ZOUT_L 0x40 Z轴加速度测量值低位
// 存储的最近温度传感器的测量值 *///TEMP为当前温度,单位为℃
#define TEMP_OUT_H 0x41 温度测量值高位
#define TEMP_OUT_L 0x42 温度测量值低位
// 存储最近的X轴、Y轴、Z轴陀螺仪感应器的测量值 */43-48
#define GYRO_XOUT_H 0x43 X轴角度测量值高位
#define GYRO_XOUT_L 0x44 X轴角度测量值低位
#define GYRO_YOUT_H 0x45 Y轴角度测量值高位
#define GYRO_YOUT_L 0x46 Y轴角度测量值低位
#define GYRO_ZOUT_H 0x47 Z轴角度测量值高位
#define GYRO_ZOUT_L 0x48 Z轴角度测量值低位
#define PWR_MGMT_1 0x6B // 电源管理,典型值:0x00(正常启用) */
#define WHO_AM_I 0x75 // IIC地址寄存器(默认数值0x68,只读) */
#define MPU6050_ADDR 0xD0 // MPU6050手册上的地址,这里也可以使用serch函数搜索
四、MPU6050可移植代码
1、mmpu6050.h
#ifndef __MMPU6050_H
#define __MMPU6050_H
#include "main.h"
#define SMPLRT_DIV 0x19 // 陀螺仪采样率分频,典型值:0x07==64-->8000/64=125Hz */
#define CONFIG 0x1A // 数字低通滤波频率,典型值:0x06==(8KHz)*/
#define GYRO_CONFIG 0x1B // 陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s) */
#define ACCEL_CONFIG 0x1C // 加速计自检、测量范围及高通滤波频率,典型值:0x00(不自检,2G,5Hz) */
#define PWR_MGMT_1 0x6B // 电源管理,典型值:0x00(正常启用) */
#define WHO_AM_I 0x75 // IIC地址寄存器(默认数值0x68,只读) */
#define MPU6050_ADDR 0xD0 // MPU6050手册上的地址,这里也可以使用serch函数去搜索
// 存储最近的X轴、Y轴、Z轴加速度感应器的测量值 */
#define ACCEL_XOUT_H 0x3B //X轴加速度测量值高位
#define ACCEL_XOUT_L 0x3C //X轴加速度测量值低位
#define ACCEL_YOUT_H 0x3D //Y轴加速度测量值高位
#define ACCEL_YOUT_L 0x3E //Y轴加速度测量值低位
#define ACCEL_ZOUT_H 0x3F //Z轴加速度测量值高位
#define ACCEL_ZOUT_L 0x40 //Z轴加速度测量值低位
// 存储的最近温度传感器的测量值 */
#define TEMP_OUT_H 0x41 //温度测量值高位
#define TEMP_OUT_L 0x42 //温度测量值低位
// 存储最近的X轴、Y轴、Z轴陀螺仪感应器的测量值 */
#define GYRO_XOUT_H 0x43 //X轴角度测量值高位
#define GYRO_XOUT_L 0x44 //X轴角度测量值低位
#define GYRO_YOUT_H 0x45 //Y轴角度测量值高位
#define GYRO_YOUT_L 0x46 //Y轴角度测量值低位
#define GYRO_ZOUT_H 0x47 //Z轴角度测量值高位
#define GYRO_ZOUT_L 0x48 //Z轴角度测量值低位
typedef struct{
// 角加速度
float Accel_X;
float Accel_Y;
float Accel_Z;
// 角速度
float Gyro_X;
float Gyro_Y;
float Gyro_Z;
// 温度
float Temp;
// PITCH(俯仰角Y)、ROLL(翻滚角X)、YAW(偏航角Z)
float angleX;
float angleY;
float angleZ;
}MPU6050DATATYPE;
extern MPU6050DATATYPE Mpu6050_Data;
extern I2C_HandleTypeDef hi2c1;
int8_t MPU6050_Init(int16_t Addr);
int16_t Sensor_I2C_Serch(void);
void MPU6050_Read_Accel(void);
void MPU6050_Read_Gyro(void);
void MPU6050_Read_Temp(void);
//int8_t Sensor_I2C_ReadOneByte(uint16_t DevAddr, uint16_t MemAddr, uint8_t *oData);
//int8_t Sensor_I2C_WriteOneByte(uint16_t DevAddr, uint16_t MemAddr, uint8_t *iData);
#endif
2、mmpu6050.c
/*
利用MPU6050实现对三轴角速度,三轴角加速度,温度读取
计算得出三轴偏移角度
使用IIC1
陀螺仪与PID计算同步
*/
#include "mmpu6050.h"
#include "math.h"
static int16_t Mpu6050Addr = 0x68;
MPU6050DATATYPE Mpu6050_Data;
//封装I2C读函数
int8_t Sensor_I2C_Read(uint16_t DevAddr, uint16_t MemAddr, uint8_t *oData, uint8_t DataLen)
{
return HAL_I2C_Mem_Read(&hi2c1,DevAddr,MemAddr,1,oData,DataLen,1000);
}
//封装I2C写函数
int8_t Sensor_I2C_Write(uint16_t DevAddr, uint16_t MemAddr, uint8_t *iData, uint8_t DataLen)
{
return HAL_I2C_Mem_Write(&hi2c1,DevAddr,MemAddr,1,iData,DataLen,1000);
}
int16_t Sensor_I2C_Serch(void)
{
int16_t addr = 0xd0;
for(uint8_t i = 1; i < 255; i++)
{
if(HAL_I2C_IsDeviceReady(&hi2c1, i, 1, 1000) == HAL_OK)
{
Mpu6050Addr = i;
addr = i;
return addr;
}
}
addr = 0xd0;
return addr;
}
int8_t MPU6050_Init(int16_t Addr)
{
uint8_t check;
HAL_I2C_Mem_Read(&hi2c1,Addr,WHO_AM_I,1,&check,1,1000);
if(check == 0x68) // 确认设备用地址寄存器
{
check = 0x00;
Sensor_I2C_Write(Addr,PWR_MGMT_1,&check, 1); // 唤醒MPU6050
check = 0x09;
Sensor_I2C_Write(Addr,SMPLRT_DIV,&check, 1); // 采样率分频,100Hz
check = 0x06;
Sensor_I2C_Write(Addr,CONFIG,&check, 1); // 1Khz的速率
check = 0x00;
Sensor_I2C_Write(Addr,ACCEL_CONFIG,&check, 1); // 加速度配置
check = 0x18;
Sensor_I2C_Write(Addr,GYRO_CONFIG,&check, 1); // 陀螺仪配置
return 0;
}
return -1;
}
void MPU6050_Read_Accel(void)
{
uint8_t Read_Buf_accel[6]; //设置数组存放加速度
//寄存器依次是加速度X高,加速度X低,加速度Y高位,加速度Y低位,加速度Z高位,加速度Z低位
Sensor_I2C_Read(Mpu6050Addr, ACCEL_XOUT_H, Read_Buf_accel, 6);
Mpu6050_Data.Accel_X = (int16_t)(Read_Buf_accel[0] << 8 | Read_Buf_accel[1]);
Mpu6050_Data.Accel_Y = (int16_t)(Read_Buf_accel[2] << 8 | Read_Buf_accel[3]);
Mpu6050_Data.Accel_Z = (int16_t)(Read_Buf_accel[4] << 8 | Read_Buf_accel[5]);
//倾斜角度依据加速度原始数据计算
//计算 ROLL(翻滚角X)
//Mpu6050_Data.angleX = atan2( Mpu6050_Data.Accel_X,Mpu6050_Data.Accel_Z ) * (180/3.14159);
Mpu6050_Data.angleX = atan2(Mpu6050_Data.Accel_Y, sqrt(Mpu6050_Data.Accel_Z * Mpu6050_Data.Accel_Z + Mpu6050_Data.Accel_X * Mpu6050_Data.Accel_X)) *180/3.14f;
//计算 PITCH(俯仰角Y)
//Mpu6050_Data.angleY = Mpu6050_Data.angleY -Mpu6050_Data.Gyro_X *0.01;
Mpu6050_Data.angleY = -atan2(Mpu6050_Data.Accel_X, sqrt(Mpu6050_Data.Accel_Z * Mpu6050_Data.Accel_Z + Mpu6050_Data.Accel_Y * Mpu6050_Data.Accel_Y)) *180/3.14f;
//计算 YAW(偏航角Z)
Mpu6050_Data.angleZ = Mpu6050_Data.Gyro_Z;
Mpu6050_Data.Accel_X = Mpu6050_Data.Accel_X / 16384.0f; //换算出加速度/g
Mpu6050_Data.Accel_Y = Mpu6050_Data.Accel_Y / 16384.0f;
Mpu6050_Data.Accel_Z = Mpu6050_Data.Accel_Z / 16384.0f;
}
void MPU6050_Read_Gyro(void)
{
uint8_t Read_Buf_gyro[6];
//寄存器依次是角度X高,角度X低,角度Y高位,角度Y低位,角度Z高位,角度Z低位
Sensor_I2C_Read(Mpu6050Addr, GYRO_XOUT_H, Read_Buf_gyro, 6);
Mpu6050_Data.Gyro_X = (int16_t)(Read_Buf_gyro[0] << 8 | Read_Buf_gyro[1]);
Mpu6050_Data.Gyro_Y = (int16_t)(Read_Buf_gyro[2] << 8 | Read_Buf_gyro[3]);
Mpu6050_Data.Gyro_Z = (int16_t)(Read_Buf_gyro[4] << 8 | Read_Buf_gyro[5]);
Mpu6050_Data.Gyro_X = Mpu6050_Data.Gyro_X / 16.4f;
Mpu6050_Data.Gyro_Y = Mpu6050_Data.Gyro_Y / 16.4f; //换算出角度°/S
Mpu6050_Data.Gyro_Z = Mpu6050_Data.Gyro_Z / 16.4f;
}
void MPU6050_Read_Temp(void)
{
uint8_t Read_Buf_temp[2];
Sensor_I2C_Read(Mpu6050Addr, TEMP_OUT_H, Read_Buf_temp, 2);
//将高八位和低八位温度测量值合并
Mpu6050_Data.Temp = (int16_t)(Read_Buf_temp[0] << 8 | Read_Buf_temp[1]);
Mpu6050_Data.Temp = 36.53f + (Mpu6050_Data.Temp / 340.0f); //温度换算公式
}