Arduino Tutorial: Data Acquisition, Analysis and Processing of MPU6050

The data writing and reading of MPU6050 are realized through the internal registers of the chip. The address of these registers is 1 byte, which is 8-bit addressing space. Please click to download the detailed list of registers: https:/ /www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf

1.1 Write data to MPU-6050

Before writing data to the device each time, turn on the transmission mode of Wire and specify the bus address of the device. The bus address of MPU6050 is 0x68 (the address is 0x69 when the AD0 pin is high). Then write a byte of register start address, and then write data of any length. These data will be continuously written to the specified starting address, and those that exceed the current register length will be written to the register at the following address. Turn off the transmission mode of Wire after writing. The following sample code is to write a byte 0 to the 0x6B register of MPU6050.

1. Wire.beginTransmission(0x68); //Enable the transmission of MPU6050
2. Wire.write(0x6B); //Specify the register address
3. Wire.write(0); //Write one byte of data
4. Wire.endTransmission(true); //End transmission, true means release the bus

1.2 Read data from MPU-6050

Reading is the same as writing. The transmission mode of Wire must be turned on first, and then the register start address of one byte must be written. Next, read the data at the specified address into the buffer of the Wire library and turn off the transmission mode. Finally read the data from the cache. The following sample code reads 2 bytes of data from the 0x3B register of MPU6050:

1. Wire.beginTransmission(0x68); //Enable the transmission of MPU6050
2. Wire.write(0x3B); //Specify the register address
3. Wire.requestFrom(0x68, 2, true); //Read the input data to the buffer
4. Wire.endTransmission(true); //Close transmission mode
5. int val = Wire.read() << 8 | Wire.read(); //Two bytes form a 16-bit integer

1.3 Specific implementation

Usually the Wire library should be initialized in the setup function:

Wire.begin();

Before performing various operations on MPU6050, you must start the device, and write a byte 0 to its 0x6B to start. It is usually completed in the setup function, and the code is shown in section 1.1.

2. Data format of MPU6050

The data we are interested in is located in the 14-byte register from 0x3B to 0x48. These data will be dynamically updated, and the update frequency can be up to 1000HZ. The address of the relevant register and the name of the data are listed below. Note that each data is 2 bytes.

• 0x3B, the X-axis component of the accelerometer ACC_X
• 0x3D, the Y-axis component of the accelerometer ACC_Y
• 0x3F, the Z-axis component of the accelerometer ACC_Z
• 0x41, current temperature TEMP
• 0x43, the angular velocity of rotation around the X axis GYR_X
• 0x45, the angular velocity of rotation around the Y axis GYR_Y
• 0x47, the angular velocity GYR_Z rotating around the Z axis

The coordinate system of the MPU6050 chip is defined as follows: make the surface of the chip face you, and turn the surface text to the correct angle. At this time, the center of the chip is the origin, the horizontal right is the X axis, and the vertical upward is Y The axis, pointing to itself is the Z axis. See the figure below:  We only care about the meaning of accelerometer and angular velocity meter data, which are introduced separately below.

2.1 Accelerometer

The three-axis components ACC_X, ACC_Y and ACC_Z of the accelerometer are all 16-bit signed integers, which represent the acceleration of the device in the three axes respectively. When the value is negative, the acceleration is along the negative direction of the coordinate axis, and when the value is positive, it is along the positive direction. .

The three acceleration components are in units of multiples of the gravitational acceleration g, and the acceleration range that can be expressed, that is, the magnification can be set uniformly, and there are 4 optional magnifications: 2g, 4g, 8g, 16g. Take ACC_X as an example, if the magnification is set to 2g (default), it means that when ACC_X takes the minimum value of -32768, the current acceleration is 2 times the gravitational acceleration along the positive direction of the X axis; if it is set to 4g, when it is -32768 Represents 4 times the acceleration of gravity along the positive direction of the X axis, and so on. Obviously, the lower the magnification, the better the accuracy, and the higher the magnification, the larger the range, which should be set according to the specific application.

We use f to represent the magnification, f=0 is 2g, f=3 is 16g, and the code for setting the acceleration magnification is as follows:

1. Wire.beginTransmission(0x68); //Enable the transmission of MPU-6050
2. Wire.write(0x1C); //Address of acceleration rate register
3. Wire.requestFrom(0x68, 1, true); //Read the original configuration first
4. unsigned char acc_conf = Wire.read();
5. acc_conf = ((acc_conf & 0xE7) | (f << 3));
6. Wire.write(acc_conf);
7. Wire.endTransmission(true); //End transmission, true means release the bus

Taking ACC_X as an example again, if the current acceleration override is 4g, then the formula for converting ACC_X readings into acceleration is:, g can be taken as the local gravity acceleration.

 

2.2 Angular Velocity Meter

The angular velocity components GYR_X, GYR_Y and GYR_Z rotating around the three coordinate axes of X, Y and Z are all 16-bit signed integers. Viewed from the origin to the direction of the rotation axis, a positive value means clockwise rotation, and a negative value means counterclockwise rotation.

The three angular velocity components are all in "degree/second", the range of angular velocity that can be expressed, that is, the magnification can be set uniformly, and there are 4 optional magnifications: 250 degrees/second, 500 degrees/second, 1000 degrees/second, 2000 Degrees/second. Taking GYR_X as an example, if the magnification is set to 250 degrees/second, it means that when GYR takes the positive maximum value of 32768, the current angular velocity is 250 degrees/second clockwise; if it is set to 500 degrees/second, when it is set to 32768, it means the current The angular velocity is 500 degrees/sec clockwise. Obviously, the lower the magnification, the better the accuracy, and the higher the magnification, the larger the range.

We use f to represent the magnification, f=0 is 250 degrees/sec, f=3 is 2000 degrees/sec, except that the address of the angular velocity magnification register is 0x1B, the code for setting the acceleration magnification is consistent with the code in section 2.1.

Taking GYR_X as an example, if the currently set angular velocity magnification is 1000 degrees/second, then the formula for converting the GRY_X reading into angular velocity (clockwise) is:

。

Three, exercise data

After reading the accelerometer and angular velocity meter data and converting them into physical values, the data has different interpretation methods according to different applications. This chapter will take the aircraft motion model as an example to calculate the current flight attitude based on acceleration and angular velocity.

3.1 Accelerometer model

We can imagine the accelerometer as a ball placed in a cube box, the ball is fixed in the center of the cube by a spring. When the box moves, the current acceleration value can be calculated according to the position of the imaginary ball. Imagine if in space, when there is no force on the box, the imaginary ball will be in the center position, and the acceleration of the three axes is 0. See below:

If we apply a horizontal leftward force to the box, obviously the box will have an acceleration to the left, and the imaginary ball in the box will stick to the right side of the box due to inertia. As shown below:

In order to ensure the physical meaning of the data, the accelerometer of MPU6050 uses the opposite number of the coordinate value of the imaginary ball on the three axes as the acceleration value of the three axes. When the position of the imaginary ball deviates to the positive direction of an axis, the acceleration reading of that axis is negative. When the position of the imaginary ball deviates to the negative direction of an axis, the acceleration reading of the axis is positive.

According to the above analysis, when we place the MPU6050 chip horizontally, the surface of the chip faces the sky. At this time, due to the gravity of the earth, the position of the imaginary ball is biased to the negative direction of the Z axis, so the acceleration reading of the Z axis should be positive and Ideally, it should be g. Note that the physical meaning of this acceleration is not the acceleration of gravity, but the acceleration of its own motion, which can be understood as follows: It is precisely because the acceleration of its own motion is equal and opposite to the direction of gravity, the chip can remain stationary.

3.2 Roll-pitch-yaw model and pose calculation

A general model that represents the current flight attitude of the aircraft is to establish the coordinate system shown in the figure below, and use Roll to represent the rotation around the X axis, Pitch to represent the rotation around the Y axis, and Yaw to represent the rotation around the Z axis.

Since the MPU6050 can obtain acceleration in three axes, and the earth's gravity exists for a long time and is always vertical downward, we can calculate the current posture based on the direction of the gravity acceleration relative to the chip as a reference.

For convenience, we fixed the chip face down on the plane above, and the coordinate system completely coincides with the plane's coordinate system. The acceleration vector in the three axes can be used as components. Assuming that the current chip is in a uniform linear motion state, it should be perpendicular to the ground up, that is, pointing to the negative direction of the Z axis, and the modulus length is (the same as the acceleration of gravity, but the direction is opposite, see section 3.1). If the chip (coordinate system) rotates, since the acceleration vector is still upright, the negative direction of the Z-axis will no longer coincide with it. See below.

For ease of presentation, the positive direction of the Z-axis (belly and front of the chip) of the coordinate system in the above figure is downward, and the positive direction of the X-axis (the direction of aircraft advancement) is to the right. The Roll angle (yellow) of the chip is the angle between the acceleration vector and its projection on the XZ plane, and the angle between the pitch angle (green) and its projection on the YZ plane. To find the angle between two vectors, you can use the dot product formula:, a simple derivation can be obtained:

,as well as

Note that because the arccos function can only return positive angles, you also need to take the positive and negative values ​​of the angle according to different situations. When the y value is positive, the roll angle must be negative, and when the x-axis is negative, the pitch angle must be negative.

3.4 The problem with Yaw angle

Because there is no reference value, it is impossible to find the absolute angle of the current Yaw angle, only the change of Yaw , which is the angular velocity GYR_Z . Of course, we can calculate the current Yaw angle by integrating GYR_Z (based on the initial value), but due to measurement accuracy problems, the estimated value will drift, and it will be completely meaningless after a period of time. However, in most applications, such as drones, you only need to obtain GRY_Z .

If you must obtain the absolute Yaw angle, you should choose the MPU9250 nine-axis motion tracking chip, which can provide additional three-axis compass data, so that we can calculate the Yaw angle according to the direction of the earth’s magnetic field. The specific method is not here. Repeat it again.

Fourth, data processing and realization

The data provided by the MPU6050 chip is mixed with serious noise, and the data swing may exceed 2% when the chip is processing the static state. In addition to noise, the data will also have an offset phenomenon, that is to say, the data does not swing around the static operating point, so the data offset must be calibrated first, and then the noise must be eliminated through the filtering algorithm.

4.1 Calibration

Calibration is a relatively simple task, we only need to find the center point around the swing data. Let's take GRY_X as an example. When the chip is in a static state, the reading should theoretically be 0, but it often has an offset. For example, we read 10 values ​​at 10ms intervals as follows:

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

The average of these 10 values, that is, the offset of this reading is -158.25. After obtaining the offset, subtract the offset from each reading to get the calibrated reading. Of course, this offset is only an estimated value. A more accurate offset can only be obtained by performing statistics on a large amount of data. The larger the amount of data, the more accurate, but the slower the statistical time. Generally, calibration can be performed every time the system is started, so you should make a trade-off between accuracy and start-up time.

The three angular velocity readings GYR_X, GYR_Y and GYR_Z can all be obtained through statistical averaging, but the three acceleration components cannot be completed in this simple way, because the acceleration when the chip is stationary is not zero.

The offset of the acceleration value comes from two aspects. One is that the acceleration vector measured by the chip is not perpendicular to the ground due to the measurement accuracy of the chip; the other is that the accuracy of the chip in the entire system (such as a drone) is limited. Yes, the coordinate system of the system and the chip is difficult to achieve perfect coincidence. The former is called reading offset, and the latter is called angular offset. Because there is a non-linear relationship between reading and angle, if you want to calibrate with high accuracy, you must first calibrate the reading offset separately, and then fix the chip in the system and calibrate the angle offset. However, since the calibration of the angle offset requires professional equipment, and for general applications, the accuracy improvement brought by the two-step calibration is not large, so usually only the reading calibration is required. The method of reading calibration is introduced below. We also take the 3.2 section of the aircraft as an example, divided into the following steps:

1. First, determine the coordinate system of the aircraft, which is very important for multi-axis aircraft. If there is a large deviation in the position of the origin of the coordinate system or the direction of the coordinate axis, it will cause adverse effects on the subsequent flight control.
2. After determining the plane's coordinate system, in order to avoid the influence of the reading offset as much as possible, first fix the MPU6050 firmly on the plane and make the two coordinate systems coincide as much as possible. Of course, it is also possible to install the Z axis in reverse, that is, a set of angle conversion formulas need to be recalculated.
3. Place the airplane on a level, firm surface and fully warm it up. For multi-axis drones, the XY plane during hovering in the air should be parallel to the XY plane during calibration. At this point, we believe that the acceleration direction of the chip should coincide with the negative direction of the Z axis, and the modulus length of the acceleration vector is g, so the theoretical value of ACC_X and ACC_Y should be 0, and the theoretical value of ACC_Z should be -16384 (assuming we set With a magnification of 2g, the reading of an acceleration of 1g should be half of the maximum value -32768).
4. Since the theoretical value of ACC_X and ACC_Y should be 0, similar to the calibration of angular velocity, the offset of these two readings can be calibrated by means of statistical mean. ACC_Z needs one more step, that is, in the process of statistical offset, 16384 must be added to each reading, and then statistical mean calibration is performed.

4.2 Kalman filter

For data with a lot of noise, the effect of Kalman filter is undoubtedly the best. If you don't want to consider the details of the algorithm, you can directly use the Klaman Filter library of Arduino. In our model, a Kalman filter accepts an axis angle value, angular velocity value and time increment, and estimates an angle value that eliminates noise. Based on the current angle value and the angle value estimated in the previous round, as well as the interval time between the two rounds of estimation, we can also infer the angular velocity for noise elimination.

See section 4.3 for the implementation code. The following describes the details of the Kalman filter algorithm, if you are not interested, you can skip it.

(If there are too many people who want to see, write again)

4.3 Implementation code

The following code is compiled, programmed and tested in Arduino software version 1.65.

1. // The copyright of this code belongs to Devymex and is released as GNU GENERAL PUBLIC LICENSE V3.0
2. // http://www.gnu.org/licenses/gpl-3.0.en.html
3. //For related documents, see the original article published by the author Yu Zhihu column:
4. // http://zhuanlan.zhihu.com/devymex/20082486
5.  
6. // Connection method
7. // MPU-UNO
8. // VCC-VCC
9. //GND-GND
10. //SCL-A5
11. // SDA-A4
12. //INT-2 (Optional)
13.  
14. #include <Kalman.h>
15. #include <Wire.h>
16. #include <Math.h>
17.  
18. float fRad2Deg = 57.295779513f; // The multiplier that converts radians to angles
19. an MPU 0x68 = int const; // an MPU-6050 's I2C address
20. const int nValCnt = 7; // Read the number of registers at a time
21.  
22. const int nCalibTimes = 1000; // Number of readings during calibration
23. int calibData[nValCnt]; // Calibration data
24.  
25. unsigned long nLastTime = 0; //The time of the last reading
26. float fLastRoll = 0.0f; // Roll angle obtained from the last filtering
27. float fLastPitch = 0.0f; // Pitch angle obtained from the last filtering
28. Kalman kalmanRoll; //Roll corner filter
29. Kalman kalmanPitch; //Pitch angle filter
30.  
31. void setup() {
32.   Serial.begin(9600); // Initialize the serial port and specify the baud rate
33.   Wire.begin(); // Initialize the Wire library
34.   WriteMPUReg(0x6B, 0); // Start MPU6050 device
35.  
36.   Calibration(); // Perform calibration
37.   nLastTime = micros(); // Record the current time
38. }
39.  
40. void loop() {
41.   int readouts[nValCnt];
42.   ReadAccGyr(readouts); // Read the measured value
43.   
44.   float realVals[7];
45.   Rectify(readouts, realVals); // correct according to the offset of calibration
46.  
47.   // Calculation of the acceleration vector mold long, are g units
48.   float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
49.   float fRoll = GetRoll(realVals, fNorm); // Calculate Roll angle
50.   if (realVals[1] > 0) {
51.     fRoll = -fRoll;
52.   }
53.   float fPitch = GetPitch(realVals, fNorm); // Calculate the Pitch angle
54.   if (realVals[0] < 0) {
55.     fPitch = -fPitch;
56.   }
57.  
58.   // Calculate the time interval dt between two measurements , in seconds
59.   unsigned long nCurTime = micros();
60.   float dt = (double)(nCurTime - nLastTime) / 1000000.0;
61.   // Kalman filter on Roll angle and Pitch angle
62.   float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
63.   float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
64.   // Calculate the angle speed according to the filter value
65.   float fRollRate = (fNewRoll - fLastRoll) / dt;
66.   float fPitchRate = (fNewPitch - fLastPitch) / dt;
67.  
68.  // Update Roll angle and Pitch angle
69.   fLastRoll = fNewRoll;
70.   fLastPitch = fNewPitch;
71.   // Update the time of this test
72.   nLastTime = nCurTime;
73.  
74.   // Print out Roll and Pitch angles to the serial port, and view them in the Arduino serial monitor when running
75.   Serial.print("Roll:");
76.   Serial.print(fNewRoll); Serial.print('(');
77.   Serial.print(fRollRate); Serial.print("),\tPitch:");
78.   Serial.print(fNewPitch); Serial.print('(');
79.   Serial.print(fPitchRate); Serial.print(")\n");
80.   delay(10);
81. }
82.  
83. // Write one byte of data to MPU6050
84. // Specify the register address and the value of a byte
85. void WriteMPUReg (int nReg, unsigned char nVal) {
86.   Wire.beginTransmission (MPU);
87.   Wire.write(nReg);
88.   Wire.write(nVal);
89.   Wire.endTransmission(true);
90. }
91.  
92. // Read one byte of data from MPU6050
93. // Specify the register address and return the value read
94. unsigned char ReadMPUReg (int nReg) {
95.   Wire.beginTransmission (MPU);
96.   Wire.write(nReg);
97.   Wire.requestFrom(MPU, 1, true);
98.   Wire.endTransmission(true);
99.   return Wire.read();
100. }
101.  
102. // Read out the three components of accelerometer, temperature and three angular velocity meters from MPU6050
103. // Save in the specified array
104. void ReadAccGyr(int *pVals) {
105.   Wire.beginTransmission (MPU);
106.   Wire.write(0x3B);
107.   Wire.requestFrom(MPU, nValCnt * 2, true);
108.   Wire.endTransmission(true);
109.   for (long i = 0; i < nValCnt; ++i) {
110.     pVals[i] = Wire.read() << 8 | Wire.read();
111.   }
112. }
113.  
114. //Calculate a large number of readings and calibrate the average offset
115. void Calibration()
116. {
117.   float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
118.   // Sum first
119.   for (int i = 0; i < nCalibTimes; ++i) {
120.     int mpuVals[nValCnt];
121.     ReadAccGyr(mpuVals);
122.     for (int j = 0; j < nValCnt; ++j) {
123.       valSums[j] += mpuVals[j];
124.     }
125.   }
126.   // Find the average again
127.   for (int i = 0; i < nValCnt; ++i) {
128.     calibData[i] = int(valSums[i] / nCalibTimes);
129.   }
130.   calibData[2] += 16384; // Set the Z axis of the chip vertically downwards, and set the static operating point.
131. }
132.  
133. // Calculate the roll angle. See the documentation for the algorithm.
134. float GetRoll (float * pRealVals, float fNorm) {
135.   float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
136.   float fCos = fNormXZ / fNorm;
137.   return acos(fCos) * fRad2Deg;
138. }
139.  
140. // Calculate the pitch angle. See the documentation for the algorithm.
141. float GetPitch(float *pRealVals, float fNorm) {
142.   float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
143.   float fCos = fNormYZ / fNorm;
144.   return acos(fCos) * fRad2Deg;
145. }
146.  
147. // Correct the reading, eliminate the offset, and convert it to a physical quantity. See the documentation for the formula.
148. void Rectify(int *pReadout, float *pRealVals) {
149.   for (int i = 0; i < 3; ++i) {
150.     pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
151.   }
152.   pRealVals [3] = pReadout [3] / 340.0f + 36.53;
153.   for (int i = 4; i < 7; ++i) {
154.     pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
155.   }
156. }