外设使用1：mpu6050

本文目的

简单的介绍一下是什么mpu6050，有什么用，如何使用，并且给出代码包，不考虑内部过于数学的解算，从代码封装和使用角度分析。

前置知识点

1. iic协议
2. C基本语法基础
3. 欧拉角得先会把

mpu6050简介

三轴加速度+三轴角速度传感器
支持spi和iic读取

mpu6050有什么用

1. 可以直接读出三轴加速度和三轴角速度

1. 以芯片自身如下图建立直角坐标系
2. 可以读出采样时刻的芯片受到的加速度，当静止的时候会有1g的重力加速度
3. 可以读出采样时刻的芯片绕xyz三个轴的瞬时角速度
   

2. 可以算出四元数从而得到芯片姿态

1. dmp可以直接算出四元数
2. 用三轴角速度和四元数的关系建立微分方程，求解得出四元数，而四元数可以表征姿态。
3. 三轴角速度积分出来的四元数会出现漂移（角速度存在器件温度等物理特效相关的噪声）。
4. 三轴加速度用于对四元数进行部分修正让他没那么飘，但是这个修正是不完全的，体现在欧拉角上是没办法修正yaw轴。常见算法有卡尔曼，互补滤波，以及mpu6050自带的dmp。
5. 四元数可以结算得到欧拉角（yaw，pitch，row），以zyx方式旋转的欧拉角，这就是无人机常用且直观的姿态表示方式了。

3. 可以外接磁力计变成九轴传感器（没用过）

六轴姿态无法矫正yaw，会出现零飘现象，这个时候需要磁力计（理解为指南针）进行修正yaw轴。

mpu6050传感器原理

略

mpu6050的使用

相关内容参考“正点原子六轴传感器模块ATK-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. 针对不同的开发板进行底层配置

1. 时钟配置，要求向所有毫秒级以及us级延时函数提供具体实现
2. io初始化，需要对iic部分的SDA和SCL，以及6050的地址管脚的初始化提供具体实现，包括声明具体引脚
3. io读写实现，需要对iic部分的SDA和SCL的读写提供具体实现，包括声明具体引脚
4. 需要对官方的代码库提供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 初始化需要关注的内容

1. 输出速率和量程
   输出速率取决于分频寄存器，具体的设置函数是mpu_set_sample_rate，读取的速率快于输出速率会采集到相同的内容，慢的话会产生浪费。不使能dmp的情况下最快能达到1000hz，使能的情况似乎只有200hz最高。速率设置修改#define DEFAULT_MPU_HZ 定义采样频率（也就是输出速率）。
   量程包括加速度量程和角速度量程，使用一个int16变量表示。
2. 是否使用dmp进行四元数的解算和输出
   上面提到了使能dmp的初始化方式，这个时候速率设置不应当超过200hz
3. 参考系设置，从欧拉角的角度去理解
   默认水平的时候是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处下载。

参考资料

六轴传感器模块ATK-MPU6050