nRF52实践:mpu6050重力流水灯

196 阅读3分钟

持续创作,加速成长!这是我参与「掘金日新计划 · 6 月更文挑战」的第30天,点击查看活动详情

本文使用 mpu6050 控制 4 个 LED 流水灯,展示其原理。

外观介绍

mpu6050为6轴芯片(注:MPU9250为9轴芯片)。

外观、接口如图1所示。

image-20220512004308299.jpg

基础知识

方向判定如下:

令芯片表面朝向自己,将其表面文字转至正确角度,此时,以芯片内部中心为原点,水平向右的为X轴,竖直向上的为Y轴,指向自己的为Z轴。传感器坐标方向定义如图2、图3所示,可作为测量数据的方向参考。

image-20220512011549451.png

image-20220512003535286.png 其使用右手坐标系,右手拇指指向 x 轴的正方向,食指指向 y 轴的正方向,中指指向 z 轴的正方向,如图4所示。

image-20220512003624173.png

飞行器坐标也是一样的,注意,保持图4的手势再调整方向即可。

bb0b0678c9b8ebea10e41331e84d9533_720w.jpg

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();
    }
}

参考资源

zhuanlan.zhihu.com/p/20082486

zhuanlan.zhihu.com/p/30621372