陀螺仪上位机

9轴姿态角测量模块和基于OpenGL的上位机

淘宝买的陀螺仪模块，不是打广告。
https://item.taobao.com/item.htm?spm=a1z09.2.0.0.Zgu9if&id=43511899945&_u=52agd5p482ac

输出精度还算可以，主要是在集成度高的情况下给出了比较高的配置性。可以选择输出数据包括加速度，角速度，磁力计，气压计，以及经过处理的欧拉角，高度，四元数。如果连接GPS，还可以输出时间和经纬度。

在提供的资料中有一个控制台工程，能通过串口读取这个模块输出的信息。但仅有数据不是很直观，考虑用opengl强大的3d显示功能实时显示姿态角。

模块自带的工程提供的代码中有对于这个模块返回数据的需要的结构体，类的封装，寄存器地址的宏定义等，开发非常方便。

为了能用opengl显示3d图像，我封装了OpenGLViewer类和CQuaternion类。用于显示3d图像以及姿态角、四元数、旋转矩阵之间的转换。

OpenGLViewer类的封装：

class OpenGLViewer
{
public:
    OpenGLViewer(const char* strSampleName);
    virtual ~OpenGLViewer();

    virtual OpenGLStatus init(int argc, char **argv);
    virtual OpenGLStatus run();  //Does not return

protected:
    virtual void display();
    virtual void displayPostDraw(){};   // Overload to draw over the screen image

    virtual void onKey(unsigned char key, int x, int y);
    virtual void onMouse(int button, int state, int x, int y);
    virtual void onMotion(int x, int y);
    virtual void onMenuEvents(int option);

    virtual OpenGLStatus initOpenGL(int argc, char **argv);
    void initOpenGLHooks();

    //变量定义

private:
    OpenGLViewer(const OpenGLViewer&);
    OpenGLViewer& operator=(OpenGLViewer&);

    static OpenGLViewer* ms_self;
    static void glutIdle();
    static void glutDisplay();
    static void glutKeyboard(unsigned char key, int x, int y);
    static void glutMouse(int button, int state, int x, int y);
    static void glutMotion(int x, int y);
    static void glutMenuEvents(int option);
    static bool IsDataReady(const unsigned long ulComNo);
    static void CreateMenus();

    //变量定义
    char    m_strSampleName[100];
};

OpenGLViewer类关键方法的实现：

初始化：

OpenGLStatus OpenGLViewer::initOpenGL(int argc, char **argv)
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
    glutInitWindowPosition(800, 300);
    glutCreateWindow (m_strSampleName);
    initOpenGLHooks();        //
    glClearColor(1.0, 1.0, 1.0, 0.0); // 设置白色背景
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glEnable(GL_DEPTH_TEST);//允许深度测试
    glDepthFunc(GL_LEQUAL);//设置深度测试类型
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);//进行透视校正
    return OpenGLSTATUS_OK;
}

注册对于动作相应函数：

void OpenGLViewer::initOpenGLHooks()
{
    glutKeyboardFunc(glutKeyboard);
    glutMouseFunc(glutMouse);
    glutMotionFunc(glutMotion);
    glutDisplayFunc(glutDisplay);
    glutIdleFunc(glutIdle);
}

具体执行什么不再一一展开，可以根据自身项目需求自己编写。

最关键的显示函数：

void OpenGLViewer::display()
{
    unsigned short usLength = 0;
    char chrBuffer[2000];

    usLength = CollectUARTData(ulComNo, chrBuffer);
    if (usLength > 0)
    {
        JY901.CopeSerialData(chrBuffer, usLength);
    }

    system("cls");

    printf("Acc:%.3f %.3f %.3f\r\n", (float)JY901.stcAcc.a[0] / 32768 * 16, (float)JY901.stcAcc.a[1] / 32768 * 16, (float)JY901.stcAcc.a[2] / 32768 * 16);

    printf("Gyro:%.3f %.3f %.3f\r\n", (float)JY901.stcGyro.w[0] / 32768 * 2000, (float)JY901.stcGyro.w[1] / 32768 * 2000, (float)JY901.stcGyro.w[2] / 32768 * 2000);

    printf("Angle:%.3f %.3f %.3f\r\n", (float)JY901.stcAngle.Angle[0] / 32768 * 180, (float)JY901.stcAngle.Angle[1] / 32768 * 180, (float)JY901.stcAngle.Angle[2] / 32768 * 180);

    Sleep(100);


    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);                                                                                                                         
    glLoadIdentity();              //重置当前的模型观察矩阵
    glTranslatef(0.0, 0.0, 0.0);//将绘制平面移动到屏幕的左半平面和里面

    glRotatef(tempAngle[2], 0.0, 1.0, 0.0);
    glRotatef(tempAngle[0], 0.0, 0.0, 1.0);
    glRotatef(tempAngle[1], 1.0, 0.0, 0.0);

    DrawPyramid();                  //画四棱锥

    //gluLookAt(JY901.stcAngle.Angle[0], JY901.stcAngle.Angle[0], JY901.stcAngle.Angle[0], 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);

    glFlush();                      //将所有的输出显示出来
    glutSwapBuffers();              //一定不能缺
}

子函数画四棱锥函数：

void DrawPyramid(void)
{
    float scale = 0.5;
    glBegin(GL_TRIANGLES);
    //前正面的绘制
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(0.0, scale, 0.0);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(-scale, -scale, scale);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(scale, -scale, scale);
    //右侧面的绘制
    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(0.0, scale, 0.0);
    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(scale, -scale, scale);
    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(scale, -scale, -scale);
    //后侧面的绘制
    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(0.0, scale, 0.0);
    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(scale, -scale, -scale);
    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(-scale, -scale, -scale);
    //左侧面的绘制
    glColor3f(1.0, 1.0, 0.0);
    glVertex3f(0.0, scale, 0.0);
    glColor3f(1.0, 1.0, 0.0);
    glVertex3f(-scale, -scale, -scale);
    glColor3f(1.0, 1.0, 0.0);
    glVertex3f(-scale, -scale, scale);
    glEnd();

    glBegin(GL_QUADS);
    //底面绘制
    glColor3f(0.0, 1.0, 1.0);
    glVertex3f(-scale, -scale, scale);
    glColor3f(0.0, 1.0, 1.0);
    glVertex3f(scale, -scale, scale);
    glColor3f(0.0, 1.0, 1.0);
    glVertex3f(scale, -scale, -scale);
    glColor3f(0.0, 1.0, 1.0);
    glVertex3f(-scale, -scale, -scale);
    glEnd();
}

CQuaternion类的声明：

#ifndef CQuaternion_h
#define CQuaternion_h

class CMatrix
{
public:
    double m[4][4];
};

class CEular
{
public:
    double p;
    double h;
    double r;
};
class CVector
{
public:
    double x;
    double y;
    double z;
    double w;
};


class CQuaternion
{
public:
    double w,x,y,z;
    CVector vec;             //绕vec转angle
    double angle;

    //默认构造函数
    CQuaternion(void);
    ~CQuaternion(void);

    //拷贝构造函数
    CQuaternion(CQuaternion &q);

    //赋值函数
    void SetQuaternion(double w, double x, double y,double z);

    //重载
    CQuaternion operator+(CQuaternion &q);//+
    CQuaternion operator-(CQuaternion &q);//-
    CQuaternion operator*(double n);//*
    void operator=(CQuaternion &q);//=
    bool operator==(CQuaternion &q);//==

    //求模
    double ModQuaternion();

    //共轭
    CQuaternion ConjugateQuaternion();

    //求逆
    CQuaternion InversionQuaternion();

    //单位化
    void NormalizeQuaternion();

    //求旋转四元数的旋转轴和角度
    void getAxisAngle(double &angle,CVector &axis);

    //根据旋转轴和角度 求四元数
    void getQuaternion(double angle,CVector axis);

    //求幂
    CQuaternion ExpTQuaternion(double t);

    //求对数
    CQuaternion LnQuaternion();

    //求指数
    CQuaternion ExpQuaternion();

    //点乘
    double DotMultiQuaternion(CQuaternion &q);

    //叉乘 
    CQuaternion CrossMultiQuaternion(CQuaternion &q);

    //求差
    CQuaternion InterAngleQuaternion(CQuaternion q0, CQuaternion &q1);

    //与一个向量乘 实现对此向量的 旋转
    CVector RotateVector(CVector &v);

    //四元数插值函数
    CQuaternion Slerp(CQuaternion &q0,CQuaternion &q1,double t);

    //转换到矩阵
    CMatrix Change2Matrix();

    //转换到欧拉角
    CEular Change2Eular();
};
#endif

CQuaternion类的实现：

#include "stdafx.h"
#include "CQuaternion.h"
#include "math.h"



//默认构造函数
CQuaternion::CQuaternion(void)
{
    w = 0;
    x = 0;
    y = 0;
    z = 0;
}

//拷贝构造函数
CQuaternion::CQuaternion(CQuaternion &qua)
{
    w = qua.w;
    x = qua.x;
    y = qua.y;
    z = qua.z;
}

CQuaternion::~CQuaternion(void)
{
}

//赋值函数
void CQuaternion::SetQuaternion(double w1, double x1, double y1,double z1)
{
    w = w1;
    x = x1;
    y = y1;
    z = z1;
}

//求旋转四元数的旋转轴和角度
void CQuaternion::getAxisAngle(double &angle,CVector &axis)
{
    (*this).NormalizeQuaternion();
    double mod = sqrt((*this).x*(*this).x +(*this).y*(*this).y +(*this).z*(*this).z);

       axis.x = (*this).x/mod;
       axis.y = (*this).y/mod;
       axis.z = (*this).z/mod;
       axis.w =0;
       angle = acos((*this).w) * 2.0;
}

//根据旋转轴和角度求 四元数
void CQuaternion::getQuaternion(double angle,CVector axis)
{

    double halfAngle = angle/2.0;
    double sn = sin(halfAngle);
    (*this).w = cos(halfAngle);
    (*this).x = axis.x *sn;
    (*this).y = axis.y *sn;
    (*this).z = axis.z *sn;
}

//重载+ - * = ==

//+
CQuaternion CQuaternion::operator+(CQuaternion &q)
{
    CQuaternion quater;
    quater.SetQuaternion(w + q.w, x + q.x, y + q.y, z + q.z);
    return quater;
}

//-
CQuaternion CQuaternion::operator-(CQuaternion &q)
{
    CQuaternion quater;
    quater.SetQuaternion(w - q.w, x - q.x, y - q.y, z - q.z);
    return quater;
}

//*
CQuaternion CQuaternion::operator*(double n)
{
    CQuaternion quater;
    quater.SetQuaternion(w * n, x * n, y * n, z * n);
    return quater;
}

//=
void CQuaternion::operator=(CQuaternion &q)
{
    SetQuaternion(q.w, q.x, q.y, q.z);  
}

//==
bool CQuaternion::operator==(CQuaternion &q)
{
    if((w == q.w)&&(x == q.x)&&(y == q.y)&&(z == q.z))
        return true;
    else return false;
}

//共轭
CQuaternion CQuaternion::ConjugateQuaternion()
{
    CQuaternion quater;
    quater.SetQuaternion(w, -x, -y, -z);
    return quater;
}

//求模
double CQuaternion::ModQuaternion()
{
    double n ;
    n = sqrt(w * w + x * x + y * y + z * z);
    return n;
}

//求逆
CQuaternion CQuaternion::InversionQuaternion()
{
    CQuaternion quater1,quater;
    double n;
    n = (*this).ModQuaternion();
    if(n == 0) exit(0);
    n = 1.0/n*n;
    quater1 = (*this).ConjugateQuaternion();    
    quater = quater1.operator*(n);
    return quater;
}

//单位化
void CQuaternion::NormalizeQuaternion()
{
    double n;
    n = (*this).ModQuaternion();
    if(n == 0) exit(0);
    n = 1.0/n;
    *this = (*this).operator*(n);
}

//求幂
CQuaternion CQuaternion::ExpTQuaternion(double t)
{
    CQuaternion quater;
    double a,angle;
    CVector vec;
    (*this).getAxisAngle(angle,vec);
    a = angle/2;
    quater.SetQuaternion(cos(t*a),sin(t*a)*vec.x,sin(t*a)*vec.y,sin(t*a)*vec.z);
    return quater;
}

//求对数
CQuaternion CQuaternion::LnQuaternion()
{
    CQuaternion quater;
    double a,angle;
    CVector vec;
    (*this).getAxisAngle(angle,vec);
    a = angle/2;
    quater.SetQuaternion(0,a*vec.x,a*vec.y,a*vec.z);
    return quater;
}

//求指数
CQuaternion CQuaternion::ExpQuaternion()
{
    CQuaternion quater;
    double a,angle;
    CVector vec;
     (*this).getAxisAngle(angle,vec);
    a = angle/2;
    quater.SetQuaternion(cos(a),sin(a)*vec.x,sin(a)*vec.y,sin(a)*vec.z);
    return quater;
}


//四元数叉乘
CQuaternion CQuaternion::CrossMultiQuaternion(CQuaternion &q)
{
    CQuaternion quater;
    quater.w = w*q.w - x*q.x - y*q.y - z*q.z;
    quater.x = w*q.x + x*q.w + z*q.y - y*q.z; 
    quater.y = w*q.y + y*q.w + x*q.z - z*q.x;
    quater.z = w*q.z + z*q.w + y*q.x - x*q.y;
    return quater;
}

//四元数点乘
double CQuaternion::DotMultiQuaternion(CQuaternion &q)
{
     double n;
     n = w*q.w + x*q.x + y*q.y + z*q.z;
     return n;
}

//求差 返回的是角位移---新定义乘法
CQuaternion CQuaternion::InterAngleQuaternion(CQuaternion q0,CQuaternion &q1)
{//q0 到q1的角位移
     CQuaternion quater;    
     q0 = q0.InversionQuaternion();
     quater = quater.CrossMultiQuaternion(q1);
     return quater;
}

//与一个向量乘 实现对此向量的 旋转
CVector CQuaternion:: RotateVector(CVector &v)
{
    // q’=p-1qp

    CQuaternion quaterVec,quaterN,quaterN1,resVec,tempQ;
    CVector vec;

    //将向量变成四元数
    quaterVec.x = v.x;
    quaterVec.y = v.y;
    quaterVec.z = v.z;
    quaterVec.w = 0.0;
    quaterN.operator=((*this));
    quaterN1 = quaterN.InversionQuaternion();
    tempQ = quaterN1.CrossMultiQuaternion(quaterVec);
    resVec = tempQ.CrossMultiQuaternion(quaterN);
    vec.x = resVec.x;
    vec.y = resVec.y;
    vec.z = resVec.z;
    vec.w = 0;
    return vec;
}

//四元数插值函数
CQuaternion CQuaternion ::Slerp(CQuaternion &q0,CQuaternion &q1,double t){
    //q0(q0^-1q1)^t
    CQuaternion quater,temp1,temp2;
    temp1 = q0.InversionQuaternion();
    temp2 = temp1.CrossMultiQuaternion(q1);
    temp2 = temp2.ExpTQuaternion(t);
    quater = q0.CrossMultiQuaternion(temp2);
    return quater;
}
//转换到矩阵
CMatrix CQuaternion::Change2Matrix()
{
    CMatrix matrix;
    memset(matrix.m,0,sizeof(matrix.m));

    matrix.m[0][0] = 1 - 2*y*y - 2*z*z;  
    matrix.m[0][1] = 2*x*y - 2*w*z; 
    matrix.m[0][2] = 2*x*z + 2*w*y;      
    matrix.m[1][0] = 2*x*y + 2*w*z; 
    matrix.m[1][1] = 1 - 2*x*x - 2*z*z;  
    matrix.m[1][2] = 2*y*z - 2*w*x; 
    matrix.m[2][0] = 2*x*z - 2*w*y;      
    matrix.m[2][1] = 2*y*z + 2*w*x;
    matrix.m[2][2] = 1 - 2*x*x - 2*y*y;  
    matrix.m[3][3] = 1;
    return matrix;
}

//转换到欧拉角
CEular CQuaternion::Change2Eular()
{
    CEular e;
    e.p = asin(2*w*x - 2*y*z);
    if(cos(e.p) == 0){
        e.h = atan2(2*w*y - 2*z*x,1-2*y*y - 2*z*z);
        e.r = 0;
    }
    else
    {
        e.h = atan2(2*z*x + 2*w*y,1-2*x*x - 2*y*y);
        e.r = atan2(2*w*z + 2*y*x,1-2*x*x - 2*z*z);
    }
    return e;
}

main函数：

main函数中代码参考了部分jy901模块提供的例程中从串口读取代码过程:

int main(int argc, char** argv)
{
    OpenGLStatus rc = OpenGLSTATUS_OK;
    unsigned long ulBaund = 9600, ulComNo = 3;
    signed char cResult = 1;
    printf("Please enter the serial number:\r\nCom = ");
    scanf("%ld", &ulComNo);
    printf("Please enter the baud rate : (9600、115200 or else )\r\nBaud = ");
    scanf("%ld", &ulBaund);
    printf("Wait for the serial port to open...\r\n");

    while (cResult != 0)                 //等待直到串口就绪
    {
        cResult = OpenCOMDevice(ulComNo, ulBaund);
    }

    JY901.JY901ComNo = ulComNo;

    //==============================================================
    OpenGLViewer OpenGLViewer("三维显示");
    rc = OpenGLViewer.init(argc, argv);
    if (rc != OpenGLSTATUS_OK)
    {
        return 1;
    }
    OpenGLViewer.run();
    //==============================================================
    return 0;
}

程序执行效果