四旋翼上位机模拟显示四轴状态

      这里只是简单介绍一下思路，因为我的上位机是用C#开发的，所以绘图方面用的是opengl 的csgl框架，虽然版本不高，但已绰绰有余！这是简单写的一个小程序，C#下环境配置可以参考我上一篇博客。程序思想虽然简单，但是不太好想到。程序界面如图：

程序我只是简单的 使用按键  改变欧拉角，可以简单的看到转动的效果。核心代码如下 ：

这里是按键改变输入欧拉角的大小。

 private void OpenGLBase_KeyDown(object sender, KeyEventArgs e)
        {
            switch (e.KeyCode)
            {
                case Keys.S:
                x+=2;
                break;
                case Keys.W:
                x -=2 ;
                break;
                case Keys.D:
                y+=2;
                break;
                case Keys.A:
                y -= 2;
                break;
                case Keys.J:
                z += 2;
                break;
                case Keys.K:
                z -= 2;
                break;

                default:
                break;
            }
        }

下面是转动的基本方法，分别绕X,Y,Z轴次序转动

public override void glDraw()
        {

            // Here's Where We Do All The Drawing
            GL.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);                            // Clear Screen And Depth Buffer
            GL.glLoadIdentity();                                                            // Reset The Current Modelview Matrix,单位化
            //gluLookAt本身有一个全局固定坐标系,不随模型的变换而变换,实图变换必需在任何模型变换之前被调用.
            // GL.gluLookAt(6, 0, -6, 0, 0, -6, 0, 1, 0);
            //模型变换 是局部坐标系统
            // GL.glTranslatef(0.0f, 0.0f, -6.0f);                                            // Move Right 1.5 Units And Into The Screen 7.0
            //局部轴是还物体固连的
            GL.glTranslatef(0.0f, 0.0f, -6.0f);        
            GL.glRotatef(x, 1.0f, 0.0f, 0.0f);                                            // Rotate The Quad On The X, Y, And Z Axes    
            GL.glRotatef(y, 0.0f, 1.0f, 0.0f);
            GL.glRotatef(z, 0.0f, 0.0f, 1.0f);

.............................//略
        
}

四轴项目最近进展很慢，不过思路已经基本清晰，有时间就研究学习一下机算机视觉相关的知识。

还有另外一种思想是：

FinalQ =CameraQ * NewQ

theta, ax, ay, az = From (FinalQ)

glRotatef(theta, ax, ay, az);

说明 ：下位机传回姿态四元数，求得转动角度和 旋转轴。目前还未分析哪种方面更好一些。这种方法在控制方面会避免Gimbal Lock问题。