ESP32上手笔记 | 05 - 获取MPU6050数据进行姿态解算和展示（I2Cdev+MPU6050+Processing）_processing姿态可视化程序

一、MPU6050陀螺仪加速度计传感器

1. 介绍

MPU6050是一个带有3轴加速度计和3轴陀螺仪的传感器，也称之为惯性测量单元（IMU）传感器：

陀螺仪测量回转的速度（rad/s），是在X、Y、Z三个轴的角位置变化，分别称为roll、pitch、yaw，这可以使我们判断物体的朝向：

加速度计用来测量加速度，也就是物体速度的变化率。

2. 模块引脚说明

• VCC：3.3V
• GND
• SCL：用于I2C通信
• SDA：用于I2C通信
• XDA：用来连接其它的I2C传感器到MPU6050
• XCL：用来连接其它的I2C传感器到MPU6050
• AD0：用来设置I2C从机地址
• INT：中断引脚，用来表示有新的测量数据可用

3. I2C通信协议

MPU6050的I2C从机地址是110100X，7bit长度，最低位X由AD0引脚来控制。

MPU6050支持的最大I2C速度为400kHz。

二、i2cdevlib

I2C Device Library(i2cdevlib)是一组基本统一且文档良好的类的集合，为I2C设备提供简单直观的接口。

1. 安装库

Github仓库地址：https://github.com/jrowberg/i2cdevlib

拉取到之后，将其中Arduino下的I2Cdev文件夹和MPU6050文件夹复制到platformIO工程的lib路径中。

2. 使用库

包含头文件：

#include "I2Cdev.h"
#include "MPU6050.h"
1
2

2.1. 创建MPU6050对象

MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0);
1

构造函数中address参数是指MPU6050的从机地址，

默认是0x68（AD0引脚为低电平），如果AD0引脚接为高电平，可以指定地址为0x69。

2.2. 初始化

/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void MPU6050_Base::initialize();
1
2
3
4
5
6
7
8

2.3. 测试通信是否正常

/** Verify the I2C connection.
 * Make sure the device is connected and responds as expected.
 * @return True if connection is valid, false otherwise
 */
bool MPU6050_Base::testConnection() {
    return getDeviceID() == 0x34;
}
1
2
3
4
5
6
7

2.4. 获取六轴数据

/** Get raw 6-axis motion sensor readings (accel/gyro).
 * Retrieves all currently available motion sensor values.
 * @param ax 16-bit signed integer container for accelerometer X-axis value
 * @param ay 16-bit signed integer container for accelerometer Y-axis value
 * @param az 16-bit signed integer container for accelerometer Z-axis value
 * @param gx 16-bit signed integer container for gyroscope X-axis value
 * @param gy 16-bit signed integer container for gyroscope Y-axis value
 * @param gz 16-bit signed integer container for gyroscope Z-axis value
 * @see getAcceleration()
 * @see getRotation()
 * @see MPU6050_RA_ACCEL_XOUT_H
 */
void MPU6050_Base::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
1
2
3
4
5
6
7
8
9
10
11
12
13

三、获取MPU6050原始数据

1. 硬件连接

2. 代码编写

#include <Arduino.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

class IMU {
    private:
        MPU6050 imu;
        int16_t ax, ay, az;
        int16_t gx, gy, gz;
        int16_t temperature;
    public:
        int init();
        void update();

        int16_t getAccelX();
        int16_t getAccelY();
        int16_t getAccelZ();

        int16_t getGyroX();
        int16_t getGyroY();
        int16_t getGyroZ();

        int16_t getTemperature();
};

IMU imu;

void setup() {
    Serial.begin(115200);
    imu.init();
}

void loop() {
    imu.update();

    // display tab-separated accel/gyro x/y/z values
    Serial.print("a/g/t:\t");
    Serial.print(imu.getAccelX()); Serial.print("\t");
    Serial.print(imu.getAccelY()); Serial.print("\t");
    Serial.print(imu.getAccelZ()); Serial.print("\t");
    Serial.print(imu.getGyroX()); Serial.print("\t");
    Serial.print(imu.getGyroY()); Serial.print("\t");
    Serial.print(imu.getGyroZ()); Serial.print("\t");
    Serial.println(imu.getTemperature());

    delay(100);
}

int IMU::init()
{
    // initialize i2c
    Wire.begin();
    Wire.setClock(400000);

    // initialize device
    Serial.println("Initializing I2C devices...");
    imu.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    if (imu.testConnection()) {
        Serial.println("MPU6050 connection successful");
        return 0;
    } else {
        Serial.println("MPU6050 connection failed");
        return -1;
    }
}

void IMU::update()
{
    // read raw accel/gyro measurements from device
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // read temperature
    temperature = imu.getTemperature();
}

int16_t IMU::getAccelX()
{
    return ax;
}

int16_t IMU::getAccelY()
{
    return ay;
}

int16_t IMU::getAccelZ()
{
    return az;
}

int16_t IMU::getGyroX()
{
    return gx;
}

int16_t IMU::getGyroY()
{
    return gy;
}

int16_t IMU::getGyroZ()
{
    return gz;
}

int16_t IMU::getTemperature()
{
    return temperature;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

3. 测试结果

四、获取MPU6050 DMP姿态解算数据

1. 姿态解算

2. 硬件连接

添加 引脚GPIO16用来连接MPU6050中断引脚：

3. 代码编写

#include <Arduino.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

#define INTERRUPT_PIN   16

class IMU {
    private:
        MPU6050 imu;
        float euler[3];         // [psi, theta, phi]    Euler angle container
        float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
        int16_t temperature;

        // MPU control/status vars
        bool dmpReady = false;  // set true if DMP init was successful
        
    public:
        int init(uint8_t pin);
        void update();

        float getYaw();
        float getPitch();
        float getRoll();

        int16_t getTemperature();
};

IMU imu;

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
    Serial.begin(115200);
    imu.init(INTERRUPT_PIN);
}

void loop() {
    imu.update();

    Serial.print("ypr\t");
    Serial.print(imu.getYaw());
    Serial.print("\t");
    Serial.print(imu.getPitch());
    Serial.print("\t");
    Serial.println(imu.getRoll());

    delay(100);
}

int IMU::init(uint8_t pin)
{
    uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
    uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
    uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

    // initialize i2c
    Wire.begin();
    Wire.setClock(400000);

    // initialize device
    Serial.println("Initializing I2C devices...");
    imu.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    if (imu.testConnection()) {
        Serial.println("MPU6050 connection successful");
    } else {
        Serial.println("MPU6050 connection failed");
        return -1;
    }

    pinMode(pin, INPUT);

    // load and configure the DMP
    devStatus = imu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    imu.setXGyroOffset(220);
    imu.setYGyroOffset(76);
    imu.setZGyroOffset(-85);
    imu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // Calibration Time: generate offsets and calibrate our MPU6050
        imu.CalibrateAccel(6);
        imu.CalibrateGyro(6);
        imu.PrintActiveOffsets();
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        imu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = imu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = imu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
}

void IMU::update()
{
    // orientation/motion vars
    Quaternion q;           // [w, x, y, z]         quaternion container
    VectorInt16 aa;         // [x, y, z]            accel sensor measurements
    VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
    VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
    VectorFloat gravity;    // [x, y, z]            gravity vector

    // MPU control/status vars
    uint8_t fifoBuffer[64]; // FIFO storage buffer

    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // read a packet from FIFO
    if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 
         // display Euler angles in degrees
            imu.dmpGetQuaternion(&q, fifoBuffer);
            imu.dmpGetGravity(&gravity, &q);
            imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }

    // read temperature
    temperature = imu.getTemperature();
}

float IMU::getYaw()
{
    return ypr[0] * 180/M_PI;
}

float IMU::getPitch()
{
    return ypr[1] * 180/M_PI;
}

float IMU::getRoll()
{
    return ypr[2] * 180/M_PI;
}

int16_t IMU::getTemperature()
{
    return temperature;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167

4. 测试结果

五、使用Processing进行姿态可视化

参考：如何用Processing对MPU 6050的值进行3D建模。

1. 安装Processing

下载地址：https://processing.org/download。

2. 安装toxiclibs库

3. 修改数据打印格式

添加格式定义：

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
1
2

删除之前的打印格式：

Serial.print("ypr\t");
Serial.print(imu.getYaw());
Serial.print("\t");
Serial.print(imu.getPitch());
Serial.print("\t");
Serial.println(imu.getRoll());
1
2
3
4
5
6

新增一个IMU类的发送数据函数：

void IMU::sendDataToProcessing()
{
    // display quaternion values in InvenSense Teapot demo format:
    teapotPacket[2] = fifoBuffer[0];
    teapotPacket[3] = fifoBuffer[1];
    teapotPacket[4] = fifoBuffer[4];
    teapotPacket[5] = fifoBuffer[5];
    teapotPacket[6] = fifoBuffer[8];
    teapotPacket[7] = fifoBuffer[9];
    teapotPacket[8] = fifoBuffer[12];
    teapotPacket[9] = fifoBuffer[13];
    Serial.write(teapotPacket, 14);
    teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

在update函数调用之后，调用该函数发送数据到上位机。

修改完毕，烧录代码。

4. 运行processing上位机

上位机为lib\MPU6050\examples\MPU6050_DMP6\Processing\MPUTeapot\MPUTeapot.pde，使用processing打开。

修改连接ESP32的串口：