持续创作,加速成长!这是我参与「掘金日新计划 · 6 月更文挑战」的第30天,点击查看活动详情
本文使用 mpu6050 控制 4 个 LED 流水灯,展示其原理。
外观介绍
mpu6050为6轴芯片(注:MPU9250为9轴芯片)。
外观、接口如图1所示。
基础知识
方向判定如下:
令芯片表面朝向自己,将其表面文字转至正确角度,此时,以芯片内部中心为原点,水平向右的为X轴,竖直向上的为Y轴,指向自己的为Z轴。传感器坐标方向定义如图2、图3所示,可作为测量数据的方向参考。
其使用右手坐标系,右手拇指指向 x 轴的正方向,食指指向 y 轴的正方向,中指指向 z 轴的正方向,如图4所示。
飞行器坐标也是一样的,注意,保持图4的手势再调整方向即可。
Roll表示绕X轴的旋转,Pitch表示绕Y轴的旋转,Yaw表示绕Z轴的旋转。读取 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw角。
重力流水灯
数据读取
从加速度寄存器0x3B开始读取14个字节(即7个16比特的有符号数值),放到定义的结构体,方便访问和处理。得到原始数据后,还需进行校准和单位转换。
经测试,即使保持芯片静止状态,读取的数据有较严重的噪音,经常跳变。除噪音外,数据还会出现偏移的现象,因此要先对数据进行校准,保持板子水平静止,上电初始化,读取若干次(如1000次),再求平均值,再与水平静止的理论值对比,得到校准数据。以后读取一次,减去校准的数(如读取ACC_X,则减去该值的校准数)。
经校准后,再进行单位换算,具体参考前一文章。
倾角计算
本文提到的重力流水灯,使用加速度数据即可,即将加速度数据转换为与重力(Z轴)的夹角,其中使用到了反余弦函数,通过夹角可推算出方向。有了方向和角度后,就可以控制流水灯了。具体三角函数应用参考文后资源。
代码实现
接口设计
- 初始化 int imu_init(void) 直接调用mpu6050_init函数
- 校准 void imu_calibration(void)
- 读取数据 int imu_read(imuinfo_t* info)
- 单位换算 void imu_convert(imuinfo_t* info, imuinfod_t* dinfo)
- 角度换算 void imu_convert_angle(imuinfod_t* info, imuangle_t* ainfo)
接口代码
校准代码:
int calibData[7] = {0};
imuinfo_t gcaliInfo;
// 静止水平时校准
void imu_calibration(void)
{
int nCalibTimes = 1000;
int16_t mpuVals[7] = {0};
int valSums[7] = {0};
int i = 0;
int j = 0;
// 用已知的预期值,求出偏差,并累计
for (i = 0; i < nCalibTimes; ++i) {
mpu6050_readrawbuff(mpuVals);
for (j = 0; j < 7; ++j)
{
if (j == 2)
{
mpuVals[j] = 16384 - mpuVals[j]; // Z轴加速度为1g
}
else
{
mpuVals[j] = 0 - mpuVals[j];
}
valSums[j] += mpuVals[j];
}
}
// 再求各参数平均值
float a = (float)valSums[0] / nCalibTimes;
gcaliInfo.acc_x = (int)a;
a = (float)valSums[1] / nCalibTimes;
gcaliInfo.acc_y = (int)a;
a = (float)valSums[2] / nCalibTimes;
gcaliInfo.acc_z = (int)a;
a = (float)valSums[4] / nCalibTimes;
gcaliInfo.gyro_x = (int)a;
a = (float)valSums[5] / nCalibTimes;
gcaliInfo.gyro_y = (int)a;
a = (float)valSums[6] / nCalibTimes;
gcaliInfo.gyro_z = (int)a;
NRF_LOG_INFO("---AGC: %d %d %d %d %d %d ",
gcaliInfo.acc_x, gcaliInfo.acc_y, gcaliInfo.acc_z,
gcaliInfo.gyro_x, gcaliInfo.gyro_y, gcaliInfo.gyro_z);
imuinfo_t info1;
imu_read(&info1);
NRF_LOG_INFO("after calibration: %d %d %d %d %d %d ",
info1.acc_x, info1.acc_y, info1.acc_z,
info1.gyro_x, info1.gyro_y, info1.gyro_z);
}
原理不复杂,连续读取1000次原始数据,使用预期的理论值减去读到的数据,求出平均值,得到纠偏数组,其后每次读取的值,均加上纠偏的数据。
读取数据
int imu_read(imuinfo_t* info)
{
int ret = 0;
ret = mpu6050_readraw(info);
if (ret < 0) return ret;
// 纠编
info->acc_x += gcaliInfo.acc_x;
info->acc_y += gcaliInfo.acc_y;
info->acc_z += gcaliInfo.acc_z;
info->gyro_x += gcaliInfo.gyro_x;
info->gyro_y += gcaliInfo.gyro_y;
info->gyro_z += gcaliInfo.gyro_z;
return 0;
}
单位换算
// 单位转换
void imu_convert(imuinfo_t* info, imuinfod_t* dinfo)
{
dinfo->acc_x = info->acc_x / 16384.0; // 量程为2g,精度为16384 LSB/g
dinfo->acc_y = info->acc_y / 16384.0;
dinfo->acc_z = info->acc_z / 16384.0;
dinfo->temp = info->temp / 340.0 + 36.53; // 温度转换公式,参见pdf手册说明
// 量程为2000度/S,精度为16.4 LSB/度/S x / 16.4 得到 度/秒 转换成弧度/秒
// gyro_x/(16.40 x 57.30)=gyro_x x 0.001064
dinfo->gyro_x = info->gyro_x * 0.001064;
dinfo->gyro_y = info->gyro_y * 0.001064;
dinfo->gyro_z = info->gyro_z * 0.001064;
}
角度换算
#include <math.h>
void imu_convert_angle(imuinfod_t* info, imuangle_t* ainfo)
{
// 反余弦 求出角度的弧度值
ainfo->angle_x = acos(info->acc_x);
ainfo->angle_y = acos(info->acc_y);
ainfo->angle_z = acos(info->acc_z);
// 再转换成角度
ainfo->angle_x = ainfo->angle_x * 57.29577;
ainfo->angle_y = ainfo->angle_y * 57.29577;
ainfo->angle_z = ainfo->angle_z * 57.29577;
}
使用示例
int main(void)
{
bool erase_bonds;
// ...
twi_master_init(); // 初始化twi
// 初始化 imu,并校准
while(imu_init() == -1)
{
NRF_LOG_INFO("mpu6050 init failed");
delay_ms(800);
}
imu_calibration();
NRF_LOG_INFO("BLE slave example started.");
NRF_LOG_INFO("app role peripheral built %s %s", __DATE__, __TIME__);
advertising_start(erase_bonds);
for (;;)
{
static uint8_t my_cnt = 0;
int next_led = 0;
imuinfo_t info1;
imuinfod_t info;
imuangle_t info_angle;
imu_read(&info1);
imu_convert(&info1, &info);
imu_convert_angle(&info, &info_angle);
if (info_angle.angle_z > 10) // 当Z 轴倾斜大于 10 时才点灯
{
dev_leds_off();
dev_led_set(next_led, ON);
my_cnt = (info_angle.angle_x < 90) ? my_cnt + 1 : my_cnt - 1; // 判断方向
next_led = my_cnt % 4;
my_delay(190 - info_angle.angle_z); // 延时,190为测试值
}
idle_state_handle();
}
}