STM32F1与STM32CubeIDE综合实例-MPU6050数据3D可视化（基于Python）

MPU6050数据3D可视化（基于Python）

MPU-6050™ 部件是一款针对智能手机、平板电脑和可穿戴传感器的低功耗、低成本和高性能要求而设计的运动跟踪设备。

MPU-6050 集成了 InvenSense 的 MotionFusion™ 和运行时校准固件，使制造商能够消除在运动支持产品中分立器件的昂贵和复杂的选择、鉴定和系统级集成，确保传感器融合算法和校准程序提供最佳的为消费者提供性能。

MPU-6050 器件在同一硅芯片上结合了 3 轴陀螺仪和 3 轴加速度计，以及处理复杂 6 轴 MotionFusion 算法的板载 Digital Motion Processor™ (DMP™)。该器件可以通过辅助主 I²C 总线访问外部磁力计或其他传感器，从而使器件无需系统处理器干预即可收集全套传感器数据。

本次实例将实现MPU6050采集的3轴加速计数据在Python中显示3D动画效果。

1、硬件准备

  • STM32F103VE开发板一块
  • MPU6050传感器一块
  • 面包板一块
  • 杜邦线若干
  • TTL转USB数据线一条
  • PC电脑一台

关于STM32CubeIDE工程创建、配置请参考前面文章：

本次MPU-6050的配置如下：

在这里插入图片描述

2、软件准备

Python开发环境安装：

Python依赖库：

pip install pygame

pip install pyserial

pip install PyOpenGL PyOpenGL_accelerate

3、MPU6050驱动实现及数据格式传输定义

关于MPU6050的驱动，在这里就再做介绍了，前的文章已经做了详细的说明，请参考：

MPU6050的驱动实现后，需要将数据通过上传给PC机，因此有必要简单定义一下数据的传输格式。定义如下：

位1 位2 位3 位4 位5 位6 位7 位8 位9
‘$’ X轴数据低位 X轴数据高位 Y轴数据低位 Y轴数据高位 Z轴数据低位 Z轴数据高位 ‘\r’ ‘\n’

在前面MPU6050驱动的STM32CubeIDE工程中，将main.c的代码更改如下：

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
int main(void) {
	/\* USER CODE BEGIN 1 \*/

	/\* USER CODE END 1 \*/

	/\* MCU Configuration--------------------------------------------------------\*/

	/\* Reset of all peripherals, Initializes the Flash interface and the Systick. \*/
	HAL\_Init();

	/\* USER CODE BEGIN Init \*/

	/\* USER CODE END Init \*/

	/\* Configure the system clock \*/
	SystemClock\_Config();

	/\* USER CODE BEGIN SysInit \*/

	/\* USER CODE END SysInit \*/

	/\* Initialize all configured peripherals \*/
	MX\_GPIO\_Init();
	MX\_USART1\_UART\_Init();
	MX\_I2C1\_Init();
	/\* USER CODE BEGIN 2 \*/
	i2c\_scan\_device(&hi2c1);
	mpu6050.I2C_Handle = &hi2c1;
	mpu6050.device_address = MPU6050_ADDR;

//
	if (!mpu6050\_begin(&mpu6050)) {
		printf("init mpu6050 failed\r\n");
		while (1)
			;
	}
	printf("mpu6050 inited\r\n");
// mpu6050\_calibrate(&mpu6050);

	/\* USER CODE END 2 \*/

	/\* Infinite loop \*/
	/\* USER CODE BEGIN WHILE \*/
	while (1) {
		/\* USER CODE END WHILE \*/

		/\* USER CODE BEGIN 3 \*/
		mpu6050\_update(&mpu6050);

		uint8\_t msg[9] = {0};
		msg[0] = '$'; // '$'
		msg[1] = mpu6050.acc_raw_x_lo;
		msg[2] = mpu6050.acc_raw_x_hi;
		msg[3] = mpu6050.acc_raw_y_lo;
		msg[4] = mpu6050.acc_raw_y_hi;
		msg[5] = mpu6050.acc_raw_z_lo;
		msg[6] = mpu6050.acc_raw_z_hi;
		msg[7] = '\r'; //
		msg[8] = '\n'; //

// printf("%s",msg); // 使用printf格式化会莫名其妙多出3个字节
        
		while(HAL\_UART\_Transmit(&huart1, msg, 9, 100) != HAL_OK);

		HAL\_Delay(50);

	}
	/\* USER CODE END 3 \*/
}

4、Python数据可视化

Python将MPU6050采集到的数据可视化的主要步骤如下：

  • 1）通过OpenGL构造一个长方体
  • 2）打开串口，读取并解析MPU6050数据
  • 3）将MPU6050的数据，转换为X、Y、Z三轴方向的旋转角度
  • 4）将MPU6050的旋转角度应用到长方体中

第一步：导入Python依赖库

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
import pygame
from pygame.locals import \*
from OpenGL.GL import \*
from OpenGL.GLU import \*
from tkinter import \*
from tkinter import ttk
import time
import serial
import threading

USE_YARM = False

# 长方体顶点
vertices= (
    (3, -.2, -1),
    (3, .2, -1),
    (-3, .2, -1),
    (-3, -.2, -1),
    (3, -.2, 1),
    (3, .2, 1),
    (-3, .2, 1),
    (-3, -.2, 1)
    )
# 长方体边
edges = (
    (0,1),
    (0,3),
    (0,4),
    (2,1),
    (2,3),
    (2,6),
    (5,1),
    (5,4),
    (5,6),
    (7,3),
    (7,4),
    (7,6)
)
# 长方体表面
surfaces = (
    (0,1,2,3),
    (4,5,6,7),
    (1,5,4,0),
    (3,2,6,7),
    (1,2,6,5),
    (0,3,7,4)
)
# 长方体表面颜色
colors = (
    ((1.0/255\*0),(1.0/255\*0),(1.0/255\*255)), # BLUE
    ((1.0/255\*148),(1.0/255\*0),(1.0/255\*211)), # Brown1
    ((1.0/255\*0),(1.0/255\*191),(1.0/255\*255)), # DeepSkyBlue1
    ((1.0/255\*124),(1.0/255\*252),(1.0/255\*0)), # LawnGreen
    ((1.0/255\*255),(1.0/255\*255),(1.0/255\*0)), # Yellow1
    ((1.0/255\*255),(1.0/255\*0),(1.0/255\*0)), # RED
)

ApplicationGL = False

第二步，定义MPU和串口

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
class PortSettings:
    Name = "COM5"
    Speed = 115200
    Timeout = 2


class IMU:
    Roll = 0
    Pitch = 0
    Yaw = 0


myport = PortSettings()
myimu = IMU()


第三步，创建串口选择窗口

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
def RunAppliction():
    global ApplicationGL
    # 选择串口名称和波特率
    myport.Name = Port_entry.get()
    myport.Speed = Baud_entry.get()
    ApplicationGL = True
    ConfWindw.destroy()

ConfWindw = Tk()
ConfWindw.title("Configure Serial Port")
ConfWindw.configure(bg="#2E2D40")
ConfWindw.geometry('300x150')
ConfWindw.resizable(width=False, height=False)
positionRight = int(ConfWindw.winfo_screenwidth() / 2 - 300 / 2)
positionDown = int(ConfWindw.winfo_screenheight() / 2 - 150 / 2)
ConfWindw.geometry("+{}+{}".format(positionRight, positionDown))

Port_label = Label(text="Port:", font=("", 12), justify="right", bg="#2E2D40", fg="#FFFFFF")
Port_label.place(x=50, y=30, anchor="center")
Port_entry = Entry(width=20, bg="#37364D", fg="#FFFFFF", justify="center")
Port_entry.insert(INSERT, myport.Name)
Port_entry.place(x=180, y=30, anchor="center")

Baud_label = Label(text="Speed:", font=("", 12), justify="right", bg="#2E2D40", fg="#FFFFFF")
Baud_label.place(x=50, y=80, anchor="center")
Baud_entry = Entry(width=20, bg="#37364D", fg="#FFFFFF", justify="center")
Baud_entry.insert(INSERT, str(myport.Speed))
Baud_entry.place(x=180, y=80, anchor="center")

ok_button = Button(text="Ok", width=8, command=RunAppliction, bg="#135EF2", fg="#FFFFFF")
ok_button.place(x=150, y=120, anchor="center")

第四步，初始化PyGame和OpenGL

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
def InitPygame():
    global display
    pygame.init()
    display = (640, 480)
    pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
    pygame.display.set_caption('IMU visualizer (Press Esc to exit)')


def InitGL():
    glClearColor((1.0 / 255 \* 46), (1.0 / 255 \* 45), (1.0 / 255 \* 64), 1)
    glEnable(GL_DEPTH_TEST)
    glDepthFunc(GL_LEQUAL)
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)

    gluPerspective(100, (display[0] / display[1]), 0.1, 50.0)
    glTranslatef(0.0, 0.0, -5)


def DrawText(textString):
    font = pygame.font.SysFont("Courier New", 25, True)
    textSurface = font.render(textString, True, (255, 255, 0), (46, 45, 64, 255))
    textData = pygame.image.tostring(textSurface, "RGBA", True)
    glDrawPixels(textSurface.get_width(), textSurface.get_height(), GL_RGBA, GL_UNSIGNED_BYTE, textData)


def DrawBoard():
    glBegin(GL_QUADS)
    x = 0

    for surface in surfaces:

        for vertex in surface:
            glColor3fv((colors[x]))
            glVertex3fv(vertices[vertex])
        x += 1
    glEnd()

# 绘制长方体
def DrawGL():
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glLoadIdentity()
    gluPerspective(90, (display[0] / display[1]), 0.1, 50.0)
    glTranslatef(0.0, 0.0, -5)
	# 根据MPU数据旋转长方体
    glRotatef(round(myimu.Pitch, 1), 0, 0, 1)
    glRotatef(round(myimu.Roll, 1), -1, 0, 0)
    if USE_YARM:
        glRotatef(round(myimu.Yaw, 1), 0, 1, 0)

        DrawText("Roll: {}° Pitch: {}° Yaw:{}°".format(round(myimu.Roll, 1),
                                                                round(myimu.Pitch, 1),round(myimu.Yaw, 1)))
    else:
        DrawText("Roll: {}° Pitch: {}°".format(round(myimu.Roll, 1),round(myimu.Pitch, 1)))
    DrawBoard()
    pygame.display.flip()


第五步，打开串口，解析数据

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
def SerialConnection():
    global serial_object
    serial_object = serial.Serial(myport.Name, baudrate=myport.Speed, timeout=myport.Timeout)


def ReadData():
    while True:

        serial_input = serial_object.readline()
        print('len = ',len(serial_input),'start = ',serial_input[0])
        print(serial_input)
        if len(serial_input) == 9 and serial_input[0] == 0x24: # '$'
            X = [serial_input[2], serial_input[1]]
            Ax = int.from_bytes(X, byteorder='big', signed=True)

            Y = [serial_input[4], serial_input[3]]
            Ay = int.from_bytes(Y, byteorder='big', signed=True)

            Z = [serial_input[6], serial_input[5]]
            Az = int.from_bytes(Z, byteorder='big', signed=True)

            myimu.Roll = Ax / 16384.0 \* 90
            myimu.Pitch = Ay / 16384.0 \* 90
            myimu.Yaw = Az / 16384.0 \* 90
        else:
            print('invalid data')



第六步，主程序入口

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
def main():
    ConfWindw.mainloop()
    if ApplicationGL == True:
        InitPygame()
        InitGL()

        try:
            SerialConnection()
            myThread1 = threading.Thread(target=ReadData)
            myThread1.daemon = True
            myThread1.start()
            while True:
                event = pygame.event.poll()
                if event.type == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE):
                    pygame.quit()
                    break

                DrawGL()
                pygame.time.wait(10)

        except:
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
            DrawText("Sorry, something is wrong :c")
            pygame.display.flip()
            time.sleep(5)


if __name__ == '\_\_main\_\_':
    main()

运行结果如下：

在这里插入图片描述

文章来源: https://iotsmart.blog.csdn.net/article/details/125888199