EEPW论坛

本次小车使用的传感器是InvenSense公司出品的MPU6050。这个传感器提供的姿态数据包括3个轴向(x,y,z)的加速度和绕这3个轴的角加速度。轴向和角加速度方向的定义如下图所示。我们不妨将这六个变量用gx,gy,gz和ωx,ωy,ωz来表示。显然，传感器并没有直接提供角度信息。想要获得角度信息，需要采用间接的计算方式。

本次小车工具包中还提供了一个上位机软件，可以辅助观察小车运行时设计者感兴趣的数据。这个上位机软件设计的初衷就是观察六轴和或九轴传感器的数据，使用方便。

1.      传感器数据的读取

MPU6050有两种数据的获取方式，可以直接读取原始的6个数据，或者通内置的DMP获取被处理过的数据。因为硬件的特点，MPU6050(或者其他类似的姿态传感器)的原始数据漂移是比较大的，基本无法直接使用。为了使用这些数据，需要设计者在得到数据后进行适当的处理，即滤波。滤波方法多种多样，比较典型的或成熟的就是卡尔曼滤波和平衡滤波。这两种滤波方式小车代码中已经包含，原理请参考他文。

不过多的纠结传感器的驱动程序部分。这里来关注如何获取原始姿态数据。

如下，设计一个数据读取函数。这六个参数在MPU6050内是以16位二进制补码的显示存储的，因此我们使用int16_t的类型就直接获得了带有符号的加速度和角加速度的比例值。负值意味着和图1中定义的方向相反。下一步我们需要将比例值转化成真实的实型数据。比例转换与MPU6050的初始化有关。在原始的小车代码中，MPU6050被初始化成如下工作参数: 加速度的满刻度为±2g, 角加速度的满刻度为±1000°/s。也就是说，需要采用如下的公式将数据进行转换:

omega = gyro / (32768/1000)    单位：°/s

void ReadSensor(float *g, float *w)
{
    int16_t Accel_X, Accel_Y, Accel_Z;
    int16_t Gyro_X, Gyro_Y, Gyro_Z;
    float gx, gy, gz;
    float wx, wy, wz;
    
    Gyro_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_XOUT_L);   
    Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L);   
    Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L);   
    
    Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); 
    Accel_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_YOUT_L); 
    Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); 
    
    gx = (float) Accel_X / (32768.0 / 2.0);
    gy = (float) Accel_Y / (32768.0 / 2.0);
    gz = (float) Accel_Z / (32768.0 / 2.0);
    wx = (float) Gyro_X / 32.8;
    wy = (float) Gyro_Y / 32.8;
    wz = (float) Gyro_Z / 32.8;
    
    *g++ = gx;
    *g++ = gy;
    *g++ = gz;
    *w++ = wx;
    *w++ = wy;
    *w++ = wz;
}

1. 数据的上位机的显示

上位机应用程序DataScope通过串口接收数据，最多可以接收并显示10路数据。原则上上位机对数据没有任何要求，但如果数据是按照gx,gy,gz，ωx,ωy,ωz的顺序传送给DataScope程序，除了用类似示波器的方式显示波形外，DataScope还可以动态的显示模块的姿态。

上位机的数据包接口遵循如下的格式：

‘$’+ 浮点格式数据 + 数据长度

一个字节的数据包头: 字符$(0x24)

数据部分: 采用浮点格式，一个数据占4个字节。多个数据连续存放，最多10个数据。

数据长度: 一个字节，大小为所有数据所占字节数加1。

在上位机一侧，DataScope是按照接收到数据的顺序将他们安排到“通道1”, “通道2” ,...,“通道10”上显示的。DataScope并不关心数据的性质。如果你想只显示一组数据，而又想将数据显示在第3通道上，你也必须在发送数据时将前两组数据填0，计算长度时也需要将前两组数据一并算上。

#define DATA_QTY    6
static char instr[1+4*DATA_QTY+1] = "$";

DisplaySensor(float *g, float *w)
{
    float *pf = (float*) &instr[1];

    *pf = w[0]; pf++;
    *pf = w[1]; pf++;
    *pf = w[2]; pf++;
    *pf = g[0]; pf++;
    *pf = g[1]; pf++;
    *pf = g[2]; pf++;

    instr[4*DATA_QTY+1] = 4*DATA_QTY+1;
    xputm(instr, DATA_QTY*4+2);
}

这里有一个问题要特别注意，传输时串口的波特率设置。在姿态传感器的应用中，数据采集的速度是相当快的，通常为毫秒量级，例如5ms或者10ms。假设一组为6个数据，即总共4*6+2=28字节，要在10ms内传输完成，波特率就要求达到28*10/0.01=28,000。显然38,400以下的波特率是不合适的。

试试这个工程文件吧。通过小车的串口1发送数据给上位机

Ex5-MPU6050 with DataScope.rar