外设使用1:mpu6050
本文目的
简单的介绍一下是什么mpu6050,有什么用,如何使用,并且给出代码包,不考虑内部过于数学的解算,从代码封装和使用角度分析。
前置知识点
- iic协议
- C基本语法基础
- 欧拉角得先会把
mpu6050简介
三轴加速度+三轴角速度传感器
支持spi和iic读取
mpu6050有什么用
1. 可以直接读出三轴加速度和三轴角速度
- 以芯片自身如下图建立直角坐标系
- 可以读出采样时刻的芯片受到的加速度,当静止的时候会有1g的重力加速度
- 可以读出采样时刻的芯片绕xyz三个轴的瞬时角速度
2. 可以算出四元数从而得到芯片姿态
- dmp可以直接算出四元数
- 用三轴角速度和四元数的关系建立微分方程,求解得出四元数,而四元数可以表征姿态。
- 三轴角速度积分出来的四元数会出现漂移(角速度存在器件温度等物理特效相关的噪声)。
- 三轴加速度用于对四元数进行部分修正让他没那么飘,但是这个修正是不完全的,体现在欧拉角上是没办法修正yaw轴。常见算法有卡尔曼,互补滤波,以及mpu6050自带的dmp。
- 四元数可以结算得到欧拉角(yaw,pitch,row),以zyx方式旋转的欧拉角,这就是无人机常用且直观的姿态表示方式了。
3. 可以外接磁力计变成九轴传感器(没用过)
六轴姿态无法矫正yaw,会出现零飘现象,这个时候需要磁力计(理解为指南针)进行修正yaw轴。
mpu6050传感器原理
略
mpu6050的使用
相关内容参考“正点原子六轴传感器模块ATK-MPU6050模块资料”
按照代码模块进行了修改,接口封装
1. 所有的文件以及说明
需要包含如下文件以及目录结构,说明如下
myHDL
│
└─mpu6050
│ readme.md 使用文档
│
├─Inc
│ dmpKey.h 相关寄存器
│ dmpmap.h 相关寄存器
│ inv_mpu.h 基本使用函数
│ inv_mpu_dmp_motion_driver.h dmp相关函数
│
└─Src
inv_mpu.c
inv_mpu_dmp_motion_driver.c
2. 针对不同的开发板进行底层配置
- 时钟配置,要求向所有毫秒级以及us级延时函数提供具体实现
- io初始化,需要对iic部分的SDA和SCL,以及6050的地址管脚的初始化提供具体实现,包括声明具体引脚
- io读写实现,需要对iic部分的SDA和SCL的读写提供具体实现,包括声明具体引脚
- 需要对官方的代码库提供iic的具体实现(这里使用的就是正点的iic函数)
3. 初始化设置
3.1 mpu6050初始化,完成后可以读出原始的加速度和角速度数据
不使用dmp的一种设置方式,具体哪个参数设置成多少看手册和需求
uint8_t atk_ms6050_init(void)
{
i2c_init();
uint8_t ret;
ret = mpu_init(NULL); /* 硬件初始化,需要修改输出速率量程等在这里修改,量程目前已经够用的情况不必修改了,不知道会不会影响到dmp*/
ret += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); /* 开启指定传感器 */
return ((ret == 0) ? 0 : 1);
}
3.2 dmp初始化,完成后可以读出原始的加速度和角速度数据,同时可以读出四元数(可以转换成欧拉角)
使用dmp的一种设置方式,具体哪个参数设置成多少看手册和需求
uint8_t atk_ms6050_dmp_init(void)
{
atk_iic_init();
uint8_t ret;
ret = mpu_init(NULL); /* 硬件初始化 */
ret += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); /* 开启指定传感器 */
ret += mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); /* 设置FIFO */
ret += mpu_set_sample_rate(DEFAULT_MPU_HZ); /* 设置采样率,不超过两百*/
ret += dmp_load_motion_driver_firmware(); /* 加载DMP镜像 */
ret += dmp_set_orientation( /* 设置陀螺仪方向 */
inv_orientation_matrix_to_scalar(gyro_orientation));
ret += dmp_enable_feature( DMP_FEATURE_6X_LP_QUAT | /* 设置DMP功能 */
DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT |
DMP_FEATURE_SEND_RAW_ACCEL |
DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
ret += dmp_set_fifo_rate(DEFAULT_MPU_HZ); /* 设置DMP输出速率 */
ret += atk_ms6050_run_self_test(); /* 传感器自测试以及参考系设置,如果需要以初始状态作为参考系需要增加写入重力在初始状态下的bias*/
ret += mpu_set_dmp_state(1); /* 使能DMP */
return ((ret == 0) ? 0 : 1);
}
3.3 初始化需要关注的内容
- 输出速率和量程
输出速率取决于分频寄存器,具体的设置函数是mpu_set_sample_rate,读取的速率快于输出速率会采集到相同的内容,慢的话会产生浪费。不使能dmp的情况下最快能达到1000hz,使能的情况似乎只有200hz最高。速率设置修改#define DEFAULT_MPU_HZ 定义采样频率(也就是输出速率)。
量程包括加速度量程和角速度量程,使用一个int16变量表示。 - 是否使用dmp进行四元数的解算和输出
上面提到了使能dmp的初始化方式,这个时候速率设置不应当超过200hz - 参考系设置,从欧拉角的角度去理解
默认水平的时候是pitch=0,roll=0。如果需要以陀螺仪初始情况下为参考系的话需要修改atk_ms6050_run_self_test()函数,把加速度偏移写入dmp。
陀螺仪初始情况下yaw是认为0。
4. 在需要的地方调用读取函数
/**
* @brief ATK-MS6050获取陀螺仪(角速度)值,更新速率和DEFAULT_MPU_HZ一致
* @param gx,gy,gz: 陀螺仪x、y、z轴的原始度数(应该是过了器件自带的lpf的了,带符号)
* @retval ATK_MS6050_EOK : 函数执行成功
* ATK_MS6050_EACK: IIC通讯ACK错误,函数执行失败
*/
uint8_t atk_ms6050_get_gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
uint8_t dat[6];
uint8_t ret;
ret = i2c_read(st.hw->addr, st.reg->raw_gyro, 6, dat);
if (ret == 0)
{
*gx = ((uint16_t)dat[0] << 8) | dat[1];
*gy = ((uint16_t)dat[2] << 8) | dat[3];
*gz = ((uint16_t)dat[4] << 8) | dat[5];
}
return ret;
}
/**
* @brief ATK-MS6050获取加速度值,更新速率和DEFAULT_MPU_HZ一致
* @param ax,ay,az: 加速度x、y、z轴的原始度数(应该是过器件自带的lpf的了,带符号)
* @retval ATK_MS6050_EOK : 函数执行成功
* ATK_MS6050_EACK: IIC通讯ACK错误,函数执行失败
*/
uint8_t atk_ms6050_get_accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
uint8_t dat[6];
uint8_t ret;
ret = i2c_read(st.hw->addr, st.reg->raw_accel, 6, dat);
if (ret == 0)
{
*ax = ((uint16_t)dat[0] << 8) | dat[1];
*ay = ((uint16_t)dat[2] << 8) | dat[3];
*az = ((uint16_t)dat[4] << 8) | dat[5];
}
return ret;
}
uint8_t atk_ms6050_dmp_get_data(float *pitch, float *roll, float *yaw)
{
float q0 = 0.0f;
float q1 = 0.0f;
float q2 = 0.0f;
float q3 = 0.0f;
short gyro[3], accel[3], sensors;
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
/* 读取ATK-MS6050 FIFO中数据的频率需与宏DEFAULT_MPU_HZ定义的频率一直
* 读取得太快或太慢都可能导致读取失败
* 读取太快:ATK-MS6050还未采样,FIFO中无数据,读取失败
* 读取太慢:ATK-MS6050的FIFO溢出,读取失败
*/
if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more) != 0)
{
return 1;
}
if (sensors & INV_WXYZ_QUAT)
{
/* ATK-MS6050的DMP输出的是姿态解算后的四元数,
* 采用q30格式,即结果被放大了2的30次方倍,
* 因为四元数并不是角度信号,因此为了得到欧拉角,
* 就需要对ATK-MS6050的DMP输出结果进行转换
*/
q0 = quat[0] / q30;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
/* 计算俯仰角、横滚角、航向角
* 57.3为弧度转角度的转换系数,即180/PI
*/
*pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
*yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
}
else
{
return 1;
}
return 0;
}
其他
由于只有200hz的dmp有时候无法满足要求,可以考虑自行进行姿态解算和滤波,只要mcu够快理论上可以到1000hz的速度更新。代码可以直接从六轴传感器模块ATK-MPU6050处下载。