openGL实时显示IMU姿态

openGL实时显示IMU姿态，用于验证IMU标定效果

1. 获取IMU数据： 首先，你需要获取IMU传感器的数据，包括姿态（如欧拉角、四元数）以及可能的陀螺仪、加速度计和磁力计数据。
2. 解析姿态数据：
   解析IMU传感器提供的姿态数据，通常为欧拉角或四元数。如果姿态是欧拉角，则通常以角度的形式表示，例如roll、pitch和yaw。
3. 将姿态转换为OpenGL坐标系：
   根据IMU传感器提供的姿态数据，将其转换为OpenGL的世界坐标系。通常，IMU的姿态数据可能使用不同的坐标系，例如欧拉角的旋转顺序可能不同，或者四元数的坐标系表示方式可能不同。
4. 绘制模型：
   使用OpenGL绘制模型或几何图形来表示IMU的姿态。你可以绘制一个简单的模型（如立方体）或者更复杂的模型（如飞机、汽车等）来表示物体的方向和旋转。
5. 更新姿态数据： 定期更新IMU传感器提供的姿态数据，并重新绘制OpenGL中的模型，以反映物体的最新姿态。

C++ socket将IMU姿态信息发送到主机

        void LoggerWrite(const char *data, int len, const char *remote, int port)
        {
            static int logger_fd = -1;
            if (logger_fd < 0)
            {
                logger_fd = socket(AF_INET, SOCK_DGRAM, 0);
                if (logger_fd < 0)
                {
                    printf("socket create error\n");
                    return;
                }
                struct sockaddr_in server_addr;
                memset(&server_addr, 0, sizeof(server_addr));
                server_addr.sin_family = AF_INET;
                server_addr.sin_port = htons(port);
                server_addr.sin_addr.s_addr = htonl(INADDR_ANY);

                if (bind(logger_fd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)
                {
                    printf("bind error\n");
                    return;
                }
            }

            struct sockaddr_in client_addr;
            memset(&client_addr, 0, sizeof(client_addr));
            client_addr.sin_family = AF_INET;
            client_addr.sin_port = htons(port);
            client_addr.sin_addr.s_addr = inet_addr(remote);
            sendto(logger_fd, data, len, 0, (struct sockaddr *)&client_addr, sizeof(client_addr));
        }

python 接收数据并实时显示IMU姿态

import socket
import threading
import numpy as np
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from PyQt5.QtWidgets import QApplication, QOpenGLWidget, QMainWindow


class OpenGLWidget(QOpenGLWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.rotate = np.array([0.0, 0.0, 0.0, 0.0])

    def initializeGL(self):
        glClearColor(0.0, 0.0, 0.0, 1.0)
        glEnable(GL_DEPTH_TEST)
        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        gluPerspective(45, 4 / 3, 0.1, 50.0)
        glMatrixMode(GL_MODELVIEW)

    def resizeGL(self, w, h):
        glViewport(0, 0, w, h)

    def paintGL(self):
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glLoadIdentity()
        glTranslatef(0.0, 0.0, -5.0)

        glRotatef(self.rotate[0], self.rotate[1], self.rotate[2], self.rotate[3])

        self.drawCube()
        self.drawAxis()

    @staticmethod
    def drawCube():
        glBegin(GL_QUADS)

        # red face
        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(0.5, 0.5, -0.5)
        glVertex3f(-0.5, 0.5, -0.5)
        glVertex3f(-0.5, 0.5, 0.5)
        glVertex3f(0.5, 0.5, 0.5)

        # red face
        glColor3f(1.0, 0.0, 0.0)
        glVertex3f(0.5, -0.5, 0.5)
        glVertex3f(-0.5, -0.5, 0.5)
        glVertex3f(-0.5, -0.5, -0.5)
        glVertex3f(0.5, -0.5, -0.5)

        # blue face
        glColor3f(0.0, 0.0, 1.0)
        glVertex3f(0.5, 0.5, 0.5)
        glVertex3f(-0.5, 0.5, 0.5)
        glVertex3f(-0.5, -0.5, 0.5)
        glVertex3f(0.5, -0.5, 0.5)

        glColor3f(0.0, 0.0, 1.0)

        glVertex3f(0.5, -0.5, -0.5)
        glVertex3f(-0.5, -0.5, -0.5)
        glVertex3f(-0.5, 0.5, -0.5)
        glVertex3f(0.5, 0.5, -0.5)

        # green face
        glColor3f(0.0, 1.0, 0.0)

        glVertex3f(-0.5, 0.5, 0.5)
        glVertex3f(-0.5, 0.5, -0.5)
        glVertex3f(-0.5, -0.5, -0.5)
        glVertex3f(-0.5, -0.5, 0.5)

        # green face
        glColor3f(0.0, 1.0, 0.0)

        glVertex3f(0.5, 0.5, -0.5)
        glVertex3f(0.5, 0.5, 0.5)
        glVertex3f(0.5, -0.5, 0.5)
        glVertex3f(0.5, -0.5, -0.5)
        glEnd()

    @staticmethod
    def drawAxis():
        glLineWidth(5.0)
        glBegin(GL_LINES)

        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(-1.0, 0.0, 0.0)
        glVertex3f(1.0, 0.0, 0.0)

        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(0.0, -1.0, 0.0)
        glVertex3f(0.0, 1.0, 0.0)

        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(0.0, 0.0, -1.0)
        glVertex3f(0.0, 0.0, 1.0)
        glEnd()

        # Add arrows
        glBegin(GL_TRIANGLES)

        # x-axis arrow
        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(1.0, 0.0, 0.0)
        glVertex3f(0.9, 0.1, 0.0)
        glVertex3f(0.9, -0.1, 0.0)
        # y-axis arrow
        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(0.0, 1.0, 0.0)
        glVertex3f(0.1, 0.9, 0.0)
        glVertex3f(-0.1, 0.9, 0.0)
        # z-axis arrow
        glColor3f(1.0, 1.0, 1.0)
        glVertex3f(0.0, 0.0, 1.0)
        glVertex3f(0.0, 0.1, 0.9)
        glVertex3f(0.0, -0.1, 0.9)
        glEnd()

    def updateRotation(self, angle, rx, ry, rz):
        self.rotate = np.array([angle, rx, ry, rz])
        self.update()


class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.wig = OpenGLWidget()
        self.setWindowTitle("OpenGL Cube Rotation")
        self.setGeometry(100, 100, 800, 600)
        self.setCentralWidget(self.wig)

    def update_rotation(self, angle, rx, ry, rz):
        self.wig.updateRotation(angle, rx, ry, rz)
        self.wig.update()

    def reset_rotation(self):
        self.wig.rotate = np.array([0.0, 0.0, 0.0, 0.0])
        self.wig.update()


def socket_recv(window):
    # 创建一个UDP socket
    reader = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    # 绑定端口
    reader.bind(('192.168.143.32', 8888))

    # 键盘输入
    while True:
        # 接收imu数据 rx, ry, rz
        response, _ = reader.recvfrom(4096)
        print('Server response:', response.decode())
        angle, rx, ry, rz = response.decode().split(' ')
        window.update_rotation(float(angle), float(rx), float(ry), float(rz))


if __name__ == '__main__':
    app = QApplication(sys.argv)
    mainWindow = MainWindow()
    mainWindow.reset_rotation()
    thread = threading.Thread(target=socket_recv, args=(mainWindow,))
    thread.start()
    mainWindow.show()
    sys.exit(app.exec_())

显示效果