前言
我TM爆肝！！
MPU6050简介
MPU6050一般为六轴或九轴传感器
正常情况下为：
- 三轴陀螺仪（测角速度）
- 三轴加速度仪
- 三轴磁场轴（有些没有）
一般测量角度，陀螺仪和加速度仪就够了
若想测量角度，通过一些滤波算法，可以将所测速度和所测加速度转换成角度数据。
但是MPU6050自带DMP自结算，可将角度数据结算成为四元角，我们只需要移植并调用DMP库函数就行。
1.1、引脚说明
一般为六个引脚
- SDA和SCL为IIC通讯引脚
- VCC、GND不需要多讲
- 当获得数据INT引脚可触发EXIT中断，再中断中可以获得姿态角
- AD0为IIC地址引脚，接GND和悬空地址为0X68，接VCC地址为0X69
DMP库简介
MPU6050自带的解算角度数据的dmp库。
结算数据速度最大可以达到200HZ。
根据我们上次讲的增量式PID教程。
200hz频率的数据获取完全可以够用了。
解算得出的数据为四元角数据
MPU6050初始化函数讲解（若使用DMP库则可以忽略）
MPU6050使用的是IIC通讯，由于STM32硬件IIC有bug，所以我们使用软件模拟IIC。
模拟IIC的代码就略了。
这里粘贴原子得代码。
u8 MPU_Init(void)
{
u8 res;
MPU_IIC_Init();//初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
return 0;
}
这其中配置了
- 0x6B 电源管理寄存器1
- 0x6C 电源管理寄存器2
- 0x1B 陀螺仪设置寄存器
- 0x1C 加速度仪设置寄存器
- 0x19 陀螺仪分频采样寄存器
- 0x1A 配置寄存器
- 0X38 中断使能寄存器
- 0X6A 用户控制寄存器
- 0x23 FIFO使能寄存器
- 0X37 中断/旁路设置寄存器
- 0X75 器件地址寄存器
查找技术手册，对这些寄存器分析
1、电源管理寄存器1
- BIT7：复位：1复位，复位结束后MPU硬件自动清零。
- BIT6：1为睡眠模式（低功耗），0为正常工作模式，所以需要置零
- BIT3：是否使能温度传感器，设置0使能。
- BIT[2~0]：设置时钟设置001使用X轴为参考。
2、电源管理寄存器2
- BIT[7-6]：控制低功耗得唤醒频率，暂且用不到罢
- BIT[5-0]：控制陀螺仪和加速度仪xyz轴是进入待机模式，全部置0
3、陀螺仪配置寄存器
BIT[4-3]：设置陀螺仪量程。
- 0，±250°/S；
- 1，±500° /S；
- 2，±1000° /S；
- 3，±2000° /S
陀螺仪ADC分辨率为16位，我们设置为3
灵敏度为：65536/4000=16.4LSB/(° /S)
4、加速度仪配置寄存器
BIT[4-3]：
- 0，± 2g；
- 1，± 4g；
- 2，± 8g；
- 3，± 16g；
加速度仪的ADC分辨率为16位，我们设置为0
灵敏度为：65536/4=16384LSB/g
5、陀螺仪分频采样寄存器
BIT[7-0]：设置采样频率
公式为：采样频率 = 陀螺仪输出频率 /（1+BIT[7-0]）
这个采样频率的设置与配置寄存器的设置有关，我们先假定50HZ，陀螺仪频率为1Khz，那么
BIT[7-0] = 1000/50-1=19。
6、配置寄存器
BIT[2-0]：
带宽一般为采样频率得一半（你问我为啥，我也不知道，记住就完了）
所以取近似值20
BIT[2-0]就设置为100;
7、中断使能寄存器
BIT[7-0]全设置为0，关闭所有中断！！！;
8、用户控制寄存器
BIT5：置零，I2C主模式关闭
9、FIFO使能寄存器
全部置零，关闭FIFO寄存器！！
10、中断/旁路设置寄存器
- BIT7：0时逻辑电平为低，1时逻辑电平为高
- BIT6：0 INT引脚推挽,1 INT引脚OD
- BIT5：0 INT引脚产生50微秒脉冲
- BIT4：0 读取 INT_STATUS (寄存器 58)中断状态位清除。1，任何读取中断状态位清除
后四位略
所以设置0x80即低电平有效
11、WHO AM I
读取该寄存器，获取设备地址，检查IIC和元器件是否有问题。
DMP库移植
将这里面得六个文件复制粘贴至STM32得工程下
c文件中，需要修改得步骤已经有了
我们需要创建一个和注释中函数格式完全一致得函数来替代。
替代函数如下
/*********************************
功能：写数据到MPU6050寄存器
形参：reg_add为MPU6050寄存器地址，
dat为MPU6050写入寄存器数据
返回：无
**********************************/
uint8_t MPU6050_WriteReg(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char const *data)
{
uint8_t i;
IIC_Start();
IIC_Send_Byte((slave_addr<<1)|0);
if(IIC_Wait_Ack())
{
IIC_Stop();
return 3;
}
IIC_Send_Byte(reg_addr);
IIC_Wait_Ack();
for(i=0;i<length;i++)
{
IIC_Send_Byte(data[i]);
if(IIC_Wait_Ack())
{
IIC_Stop();
return 1;
}
}
IIC_Stop();
return 0;
}
/*********************************
功能：从MPU6050数据寄存器中读数据
形参：reg_add为MPU6050数据寄存器地址，
*dat 为MPU6050读取所得数据
num 为数据长度
返回：无
**********************************/
uint8_t MPU6050_ReadData(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char *data)
{
IIC_Start();
IIC_Send_Byte((slave_addr<<1)|0);
if(IIC_Wait_Ack())
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg_addr);
IIC_Wait_Ack();
IIC_Stop();
IIC_Start();
IIC_Send_Byte((slave_addr<<1)+1);
IIC_Wait_Ack();
while(length){
if(length == 1)
*data=IIC_Read_Byte(0);
else
*data=IIC_Read_Byte(1);
length--;
data++;
}
IIC_Stop();
return 0;
}
/*********************************
功能：获取时间戳
形参：*count传递数据
返回：无
**********************************/
void stm32_get_ms(unsigned long *count)
{
*count = HAL_GetTick();
}
/*********************************
功能：延迟时间
形参：num_ms表示ms时间
返回：无
**********************************/
void stm32_Delay_ms(unsigned long num_ms)
{
HAL_Delay(num_ms);
}
修改文件
inv_mpu.c
将
修改为
并将包含文件改变成自己得包含文件
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "spi.h"
#include "main.h"
inv_mpu_dmp_motion_drive.c中同理
同时，大部分教程中都没写的部分，在DMP驱动文件中，还有一个motion_driver_test.c的文件！！！！
里面也有重要的函数！！很多教程都是直接跳过了，导致我看了好长时间也不懂。
这个文件里有方向转换的函数，自检函数，还有可以学习参考的主函数！！！
我们需要将此文件中的
这两个函数复制粘贴进我们的工程中，并使inv_mpu_dmp_motion_drive.c和inv_mpu.c调用它！！！
如果此时移植完编译仍不通过，将错误行注释掉就行！！
DMP库初始化讲解
/**************************************************
功能：DMP初始化
形参：无
返回值：0：成功
其它：失败
***************************************************/
uint8_t DMP_Init(void)
{
uint8_t res = 0;
uint8_t RES = 0;
RES = mpu_init();
if(RES == 0)
{
res = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
if(res)return 88;
res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(100); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(100); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
run_self_test();
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
else
return RES;
for(uint16_t rd=0; rd<300; rd++)
{
//读取MPU内置DMP的姿态
read_dmp(&pitch,&roll,&yaw);
HAL_Delay(20);
}
return 0;
}
DMP库中存在mpu_init();函数，因此，先前得初始化可以注释掉了（前面的只是为了熟悉或者读取生数据所需）
首先是初始化MPU
让后另一个重点是
保证读取没有问题。
最后就是四元角的换算
/**************************************************
功能：DMP读取
形参：传递读取数据
返回值：0：成功
其它：失败
***************************************************/
uint8_t read_dmp(float *pitch, float *roll, float *yaw)
{
uint8_t more;
long quat[4]; //四元数
int16_t gyro[3], accel[3], sensors;
unsigned long sensor_timestamp;//传感器时间戳
float q0 = 1.0f,q1 = 0.0f,q2 = 0.0f,q3 = 0.0f;
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))
return 1;
if(sensors & 0x100)
{
q0 = quat[0]/q30;
q1 = quat[1]/q30;
q2 = quat[2]/q30;
q3 = quat[3]/q30;
*pitch = asin(-2*q1*q3 + 2*q0*q2)* 57.3f; //四元数解算, 俯仰角(Pitch)-->绕着X轴旋转
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f; //四元数解算, 翻滚角(Roll)-->绕着Y轴旋转
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3f; //四元数解算, 偏航角(Yaw)-->绕着Z轴旋转
return 0;
}
else
return 2;
}
读取数据排错检查
初始化函数中定义res
用串口发送res的数值来检查程序哪一步出错
常见错误
1、FIFO没开，再初始化前先用寄存器打开FIFO
2、mpu多次初始化
3、移植问题，有些警告没有注释
4、芯片损坏
5293
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
