OSG仿真案例（9）——JY61陀螺仪控制飞机姿态

最新推荐文章于 2024-05-07 17:26:26 发布

rexinx

最新推荐文章于 2024-05-07 17:26:26 发布

阅读量816

点赞数 1

文章标签：百度 java 单片机 android stm32

本文链接：https://blog.csdn.net/weixin_40523119/article/details/120684410

版权

前言
在调试osg中模型运动姿态时，总觉得直观性不够强。所以有了想买个硬件陀螺仪(当时并不知道这个硬件应该叫什么名字，在淘宝搜索角度传感器的)。
几个驱动
1.CH340驱动
这个驱动在自带资源包里面，但是不可以用，只能自己在网上找，发现是型号不对。(自己芯片是CH340N，但是资源的驱动是CH341)。
2.串口驱动
切记不要自己炫技自己写，真的是得不偿失，最后在官网找到了。

include "Com.h"的内容：

#ifndef __UART_NET_H
#define __UART_NET_H
signed char SendUARTMessageLength(const unsigned long ulChannelNo, const char chrMessage[],const unsigned short usLen);
unsigned short CollectUARTData(const unsigned long ulChannelNo, char chrUARTBufferOutput[]);
signed char OpenCOMDevice(const unsigned long ulPortNo);
signed char SetBaundrate(const unsigned long ulPortNo,const unsigned long ulBaundrate);
signed char OpenCOMDevice(const unsigned long ulPortNo,const unsigned long ulBaundrate);
void CloseCOMDevice(void);
#endif

include "Com.cpp"的内容：

#include <stdio.h>
#include <tchar.h>
#include <windows.h>
#include "Com.h"

#define     TOTAL_PORT_NUM      65
#define     START_PORT_NUM      0

#define     iBufferSize 250
#define     UARTBufferLength 2000
#undef  SYNCHRONOUS_MODE

static HANDLE        hComDev[TOTAL_PORT_NUM]         ={NULL};
static unsigned long long ulComMask = 0;
static HANDLE        hCOMThread[TOTAL_PORT_NUM]      ={NULL};
static OVERLAPPED    stcWriteStatus[TOTAL_PORT_NUM]  = {0};
static OVERLAPPED    stcReadStatus[TOTAL_PORT_NUM]   = {0};

#ifdef SYNCHRONOUS_MODE
static HANDLE        hReceiveEvent[TOTAL_PORT_NUM]   ={NULL};
#endif

static volatile char chrUARTBuffers[TOTAL_PORT_NUM][UARTBufferLength]={0};
static volatile unsigned long ulUARTBufferStart[TOTAL_PORT_NUM]={0}, ulUARTBufferEnd[UARTBufferLength]={0};

unsigned short CollectUARTData(const unsigned long ulCOMNo,char chrUARTBufferOutput[])
{
    unsigned long ulLength=0;
    unsigned long ulEnd ;
    unsigned long ulStart ;

#ifdef SYNCHRONOUS_MODE
    WaitForSingleObject(hReceiveEvent[ulIndexCorrect],INFINITE);
    ResetEvent(hReceiveEvent[ulIndexCorrect]);
#endif
    ulEnd = ulUARTBufferEnd[ulCOMNo];
    ulStart = ulUARTBufferStart[ulCOMNo];
    if (ulEnd == ulStart)
        return(0);
    if (ulEnd > ulStart)
    {
        memcpy((void*)chrUARTBufferOutput,(void*)(chrUARTBuffers[ulCOMNo]+ulStart),ulEnd-ulStart);
        ulLength = ulEnd-ulStart;
    }
    else
    {
        memcpy((void*)chrUARTBufferOutput,(void*)(chrUARTBuffers[ulCOMNo]+ulStart),UARTBufferLength-ulStart);
        if ( ulEnd != 0 )
        {
            memcpy((void*)(chrUARTBufferOutput+(UARTBufferLength-ulStart)),(void*)chrUARTBuffers[ulCOMNo],ulEnd);
        }
        ulLength = UARTBufferLength+ulEnd-ulStart;
    }
    ulUARTBufferStart[ulCOMNo] = ulEnd;
    return (unsigned short) ulLength;
}

signed char SendUARTMessageLength(const unsigned long ulChannelNo, const char chrSendBuffer[],const unsigned short usLen)
{
    DWORD iR;
    DWORD dwRes;
    DCB dcb;
    char chrDataToSend[1000] = {0};
    memcpy(chrDataToSend,chrSendBuffer,usLen);
    memcpy(&chrDataToSend[usLen],chrSendBuffer,usLen);

    GetCommState(hComDev[ulChannelNo] ,&dcb);
    dcb.fDtrControl = 0;//DTR = 1;发送
    SetCommState(hComDev[ulChannelNo] ,&dcb);

    if ( WriteFile(hComDev[ulChannelNo],chrSendBuffer,usLen,&iR,&(stcWriteStatus[ulChannelNo])) || GetLastError() != ERROR_IO_PENDING  ) 
        return -1;
    dwRes = WaitForSingleObject(stcWriteStatus[ulChannelNo].hEvent,1000);
    Sleep(10);
    dcb.fDtrControl = 1;//DTR = 0;接收
    SetCommState(hComDev[ulChannelNo] ,&dcb);
    Sleep(10);

    if(dwRes != WAIT_OBJECT_0 || ! GetOverlappedResult(hComDev[ulChannelNo], &stcWriteStatus[ulChannelNo], &iR, FALSE))
        return 0;
    return 0;
}

DWORD WINAPI ReceiveCOMData(PVOID pParam)
{
    unsigned long uLen;
    unsigned long ulLen1;
    unsigned long ulLen2;
    DWORD   dwRes;
    COMSTAT Comstat;
    DWORD dwErrorFlags;
    char chrBuffer[iBufferSize]={0};
    unsigned long ulUARTBufferEndTemp=ulUARTBufferEnd[0];

    unsigned long ulComNumber = 0;
    memcpy(&ulComNumber,pParam,4);


    while (1)
    {
        if ( ! ReadFile(hComDev[ulComNumber],chrBuffer,iBufferSize-1,&uLen,&(stcReadStatus[ulComNumber])) )
        {
            dwRes = GetLastError() ;
            if ( dwRes != ERROR_IO_PENDING)
            {
                ClearCommError(hComDev[ulComNumber],&dwErrorFlags,&Comstat);
                continue;
            }

            WaitForSingleObject(stcReadStatus[ulComNumber].hEvent,INFINITE);
            if ( !GetOverlappedResult(hComDev[ulComNumber], &(stcReadStatus[ulComNumber]), &uLen, FALSE))
                continue;
            if(uLen <= 0)
                continue;
            if ( (ulUARTBufferEndTemp + uLen) > UARTBufferLength )
            {
                ulLen1 = UARTBufferLength - ulUARTBufferEndTemp;
                ulLen2 = uLen - ulLen1;
                if (ulLen1 > 0)
                {
                    memcpy((void *)&chrUARTBuffers[ulComNumber][ulUARTBufferEnd[ulComNumber]],(void *)chrBuffer,ulLen1);
                }
                if (ulLen2 > 0)
                {
                    memcpy((void *)&chrUARTBuffers[ulComNumber][0],(void *)(chrBuffer+ulLen1),ulLen2);
                }
                ulUARTBufferEndTemp = ulLen2;
            }
            else
            {
                memcpy((void *)&chrUARTBuffers[ulComNumber][ulUARTBufferEnd[ulComNumber]],(void *)chrBuffer,uLen);
                ulUARTBufferEndTemp+=uLen;  
            }

            if (  ulUARTBufferEndTemp == ulUARTBufferStart[ulComNumber])
            {
                printf("Error!");
            }
            else
            {
                ulUARTBufferEnd[ulComNumber] = ulUARTBufferEndTemp;
            }

#ifdef SYNCHRONOUS_MODE
            SetEvent(hReceiveEvent[ucComNumber]);
#endif
            continue;
        }

        if(uLen <= 0)
            continue;
        if ( (ulUARTBufferEndTemp + uLen) > (UARTBufferLength) )
        {
            ulLen1 = UARTBufferLength - ulUARTBufferEndTemp;
            ulLen2 = uLen - ulLen1;
            if (ulLen1 > 0)
            {
                memcpy((void *)&chrUARTBuffers[ulComNumber][ulUARTBufferEnd[ulComNumber]],(void *)chrBuffer,ulLen1);
            }
            if (ulLen2 > 0)
            {
                memcpy((void *)&chrUARTBuffers[ulComNumber][0],(void *)(chrBuffer+ulLen1),ulLen2);
            }
            ulUARTBufferEndTemp = ulLen2;
        }
        else
        {
            memcpy((void *)&chrUARTBuffers[ulComNumber][ulUARTBufferEnd[ulComNumber]],(void *)chrBuffer,uLen);
            ulUARTBufferEndTemp+=uLen;  
        }

        if (  ulUARTBufferEndTemp== ulUARTBufferStart[ulComNumber])
        {
            printf("Error!");
        }
        else
        {
            ulUARTBufferEnd[ulComNumber] = ulUARTBufferEndTemp;
        }   

#ifdef SYNCHRONOUS_MODE
        SetEvent(hReceiveEvent[ucComNumber]);
#endif

    }
    return 0;
}

signed char OpenCOMDevice(const unsigned long ulPortNo,const unsigned long ulBaundrate)
{
    DWORD dwThreadID,dwThreadParam;
    COMSTAT Comstat;
    DWORD dwErrorFlags;
    DWORD dwRes;
    DCB dcb;
    COMMTIMEOUTS comTimeOut;
    TCHAR PortName[10] = {'\\','\\','.','\\','C','O','M',0,0,0};//"\\\\.\\COM";
    TCHAR chrTemple[5]={0};

    if(ulPortNo >= TOTAL_PORT_NUM)
    {
        printf("\nerror: exceed the max com port num\n");
        return -1;
    }


    _itot(ulPortNo+START_PORT_NUM,chrTemple,10);
    _tcscat(PortName,chrTemple);

    if((hComDev[ulPortNo] = CreateFile(PortName,GENERIC_READ|GENERIC_WRITE,0,NULL,OPEN_EXISTING,FILE_FLAG_OVERLAPPED ,NULL))==INVALID_HANDLE_VALUE)
    {
        dwRes=GetLastError();
        return -1;
    }
    ulComMask |= 1<<ulPortNo;

    SetupComm(hComDev[ulPortNo] ,iBufferSize,iBufferSize);
    GetCommState(hComDev[ulPortNo] ,&dcb);
        dcb.BaudRate = ulBaundrate;
    dcb.fParity = NOPARITY;
    dcb.ByteSize=8;
    dcb.fDtrControl = 1;//DTR = 0;接收
    dcb.fRtsControl = 0;//RTS = 0;接收
    dcb.StopBits=ONESTOPBIT;

    SetCommState(hComDev[ulPortNo] ,&dcb);
    ClearCommError(hComDev[ulPortNo] ,&dwErrorFlags,&Comstat);
    dwRes = GetLastError();

    comTimeOut.ReadIntervalTimeout = 5;             
    comTimeOut.ReadTotalTimeoutMultiplier = 10;     
    comTimeOut.ReadTotalTimeoutConstant = 100;      
    comTimeOut.WriteTotalTimeoutMultiplier = 5;     
    comTimeOut.WriteTotalTimeoutConstant = 5;       
    SetCommTimeouts(hComDev[ulPortNo] ,&comTimeOut);    

    stcWriteStatus[ulPortNo] .hEvent = CreateEvent(NULL,TRUE,FALSE,NULL);
    stcReadStatus[ulPortNo] .hEvent = CreateEvent(NULL,TRUE,FALSE,NULL);
    stcReadStatus[ulPortNo].Internal = 0;
    stcReadStatus[ulPortNo].InternalHigh = 0;
    stcReadStatus[ulPortNo].Offset = 0;
    stcReadStatus[ulPortNo].OffsetHigh = 0;
    dwThreadParam = ulPortNo;
    hCOMThread[dwThreadParam] = CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)ReceiveCOMData,&dwThreadParam,0,&dwThreadID);
    SetThreadPriority(hCOMThread[ulPortNo],THREAD_PRIORITY_NORMAL);
    Sleep(200);

    return 0;

} 

signed char SetBaundrate(const unsigned long ulPortNo,const unsigned long ulBaundrate)
{

    DCB dcb;    
    GetCommState(hComDev[ulPortNo] ,&dcb);
    dcb.BaudRate = ulBaundrate;
    SetCommState(hComDev[ulPortNo] ,&dcb);
    return 0;

} 
void CloseCOMDevice()
{
    unsigned char i;
    for(i=0 ; i<sizeof(ulComMask)*8 ; i++)
    {
        if((ulComMask & (1<<i))==0)
            continue;
        ulUARTBufferEnd[i] = 0;ulUARTBufferStart[i]=0;
        TerminateThread(hCOMThread[i],0);
        WaitForSingleObject(hCOMThread[i],10000);
        PurgeComm(hComDev[i],PURGE_TXABORT|PURGE_RXABORT|PURGE_TXCLEAR|PURGE_RXCLEAR);
        CloseHandle(stcReadStatus[i].hEvent);
        CloseHandle(stcWriteStatus[i].hEvent);
        CloseHandle(hComDev[i]);
    }
    ulComMask = 0;
}

上面两个内容，一个代码也没有修改，尊重原始资料。
3.陀螺仪芯片驱动和数据读取

include "JY901.h"的内容(未做任何修改)：

#include "JY901.h"
#include "string.h"

CJY901 ::CJY901 ()
{
}

void CJY901 ::CopeSerialData(char ucData[],unsigned short usLength)
{
    static unsigned char chrTemp[2000];
    static unsigned char ucRxCnt = 0;   
    static unsigned short usRxLength = 0;


    memcpy(chrTemp,ucData,usLength);
    usRxLength += usLength;
    while (usRxLength >= 11)
    {
        if (chrTemp[0] != 0x55)
        {
            usRxLength--;
            memcpy(&chrTemp[0],&chrTemp[1],usRxLength);                        
            continue;
        }
        switch(chrTemp[1])
        {
            case 0x50:  memcpy(&stcTime,&chrTemp[2],8);break;
            case 0x51:  memcpy(&stcAcc,&chrTemp[2],8);break;
            case 0x52:  memcpy(&stcGyro,&chrTemp[2],8);break;
            case 0x53:  memcpy(&stcAngle,&chrTemp[2],8);break;
            case 0x54:  memcpy(&stcMag,&chrTemp[2],8);break;
            case 0x55:  memcpy(&stcDStatus,&chrTemp[2],8);break;
            case 0x56:  memcpy(&stcPress,&chrTemp[2],8);break;
            case 0x57:  memcpy(&stcLonLat,&chrTemp[2],8);break;
            case 0x58:  memcpy(&stcGPSV,&chrTemp[2],8);break;
        }
        usRxLength -= 11;
        memcpy(&chrTemp[0],&chrTemp[11],usRxLength);                     
    }
}
CJY901 JY901 = CJY901();

include "JY901.cpp"的内容(未修改)：

#include "JY901.h"
#include "string.h"

CJY901 ::CJY901 ()
{
}

void CJY901 ::CopeSerialData(char ucData[],unsigned short usLength)
{
    static unsigned char chrTemp[2000];
    static unsigned char ucRxCnt = 0;   
    static unsigned short usRxLength = 0;


    memcpy(chrTemp,ucData,usLength);
    usRxLength += usLength;
    while (usRxLength >= 11)
    {
        if (chrTemp[0] != 0x55)
        {
            usRxLength--;
            memcpy(&chrTemp[0],&chrTemp[1],usRxLength);                        
            continue;
        }
        switch(chrTemp[1])
        {
            case 0x50:  memcpy(&stcTime,&chrTemp[2],8);break;
            case 0x51:  memcpy(&stcAcc,&chrTemp[2],8);break;
            case 0x52:  memcpy(&stcGyro,&chrTemp[2],8);break;
            case 0x53:  memcpy(&stcAngle,&chrTemp[2],8);break;
            case 0x54:  memcpy(&stcMag,&chrTemp[2],8);break;
            case 0x55:  memcpy(&stcDStatus,&chrTemp[2],8);break;
            case 0x56:  memcpy(&stcPress,&chrTemp[2],8);break;
            case 0x57:  memcpy(&stcLonLat,&chrTemp[2],8);break;
            case 0x58:  memcpy(&stcGPSV,&chrTemp[2],8);break;
        }
        usRxLength -= 11;
        memcpy(&chrTemp[0],&chrTemp[11],usRxLength);                     
    }
}
CJY901 JY901 = CJY901();

4.使用
用的例子还是上一节的内容，加载一个飞机的模型，在其中写上一个matrixTransform1->addUpdateCallback(new CAutoPlane( osg::Vec3(0.0f, 0.0f, 0.0f),osg::Vec3(0.000010f, 0.0f, 0.0f) ));
的回调函数，在这个 CAutoPlane类中，调用硬件数据，实时对模型的姿态进行刷新。

#include <Windows.h>
#include <iostream>
#include <osgDB/ReadFile>
#include <osgViewer/ViewerEventHandlers>
#include <osgAnimation/BasicAnimationManager>
#include <osgDB/ReadFile>
#include <osgEarthUtil/ExampleResources>
#include <osgEarth/Registry>
#include <osgEarthUtil/Controls>
#include <osgEarthSymbology/Color>
void model()
{
    osg::ref_ptr<osgViewer::Viewer> viewer1 = new osgViewer::Viewer;
    osg::ref_ptr<osg::Group> group1 = new osg::Group;
    osg::ref_ptr<osg::MatrixTransform> matrixTransform1 = new osg::MatrixTransform;

    osg::ref_ptr<osg::Node> node1 = osgDB::readNodeFile("E.......fbx");
    AnimationManagerFinder animationManagerFinder1;
    group1->accept(animationManagerFinder1);
    if (animationManagerFinder1._am.valid())
    {
        std::string playModeOpt;
        osgAnimation::Animation::PlayMode playMode = osgAnimation::Animation::LOOP;
        if (osgDB::equalCaseInsensitive(playModeOpt, "ONCE"))
        {
            playMode = osgAnimation::Animation::ONCE;
        }
        else if (osgDB::equalCaseInsensitive(playModeOpt, "STAY"))
        {
            playMode = osgAnimation::Animation::STAY;
        }
        else if (osgDB::equalCaseInsensitive(playModeOpt, "LOOP"))
        {
            playMode = osgAnimation::Animation::LOOP;
        }
        else if (osgDB::equalCaseInsensitive(playModeOpt, "PPONG"))
        {
            playMode = osgAnimation::Animation::PPONG;
        }

        for (osgAnimation::AnimationList::const_iterator animIter = animationManagerFinder1._am->getAnimationList().begin();
            animIter != animationManagerFinder1._am->getAnimationList().end();
            ++animIter)
        {
            (*animIter)->setPlayMode(playMode);
        }

    }
    matrixTransform1->setMatrix(osg::Matrix::translate(0.0, 0.0, 0.0));
    matrixTransform1->addChild(node1);
    matrixTransform1->addUpdateCallback(
        new CAutoPlane(
            osg::Vec3(0.0f, 0.0f, 0.0f),
            osg::Vec3(0.000010f, 0.0f, 0.0f)
        ));

    group1->addChild(matrixTransform1);
    viewer1->setSceneData(group1);

    viewer1->setUpViewInWindow(200, 200, 800, 600, 0);
    viewer1->run();
}

回调函数 CAutoPlane：

#include "JY901.h"
#include <windows.h>
#include "Com.h"
class CAutoPlane : public osg::NodeCallback
{
public:
    CAutoPlane(osg::Vec3 start, osg::Vec3 speed)
    {
        m_speed = speed;
        heading = 0.0;
        m_pos = 0.0;
        unsigned short usLength = 0, usCnt = 0;
        unsigned long ulBaund = 9600;
        ulComNo = 4;
        signed char cResult = 1;
        while (cResult != 0)
        {
            cResult = OpenCOMDevice(ulComNo, ulBaund);
        }
        usLength = CollectUARTData(ulComNo, chrBuffer);
        if (usLength>0)
        {
            JY901.CopeSerialData(chrBuffer, usLength);
        }

    }
    ~CAutoPlane() {};
osg::Vec3 outRotate()
    {
        usLength = CollectUARTData(ulComNo, chrBuffer);
        if (usLength>0)
        {
            JY901.CopeSerialData(chrBuffer, usLength);
        }
        Sleep(100);

        if (usCnt++ >= 0)
        {
            usCnt = 0;
            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);
        }
        return osg::Vec3((float)JY901.stcAngle.Angle[0] / 32768 * 180, (float)JY901.stcAngle.Angle[1] / 32768 * 180, (float)JY901.stcAngle.Angle[2] / 32768 * 180);

    }
    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        outRotate();
        osg::MatrixTransform * tx = dynamic_cast <osg::MatrixTransform *> (node);
        if (tx != NULL)
        {
            heading += -osg::PI / 1800.0;
            osg::Matrixd orbitRotation;
            orbitRotation.makeRotate(
                osg::DegreesToRadians(outRotate()[0]), osg::Vec3(0, 1, 0),//roll angle (Y axis)
                osg::DegreesToRadians(outRotate()[1]), osg::Vec3(1, 0, 0),//pitch angle (X axis)
                osg::DegreesToRadians(outRotate()[2]), osg::Vec3(0, 0, 1));                //heading angle (Z axis)

            {
                m_start += m_speed;
                tx->setMatrix(orbitRotation * osg::Matrixd::translate(m_start));//直接赋值式改变位置
            }
        }
        traverse(node, nv);
    }
private:
    osg::Vec3 m_start;  //汽车的起始位置
    osg::Vec3 m_speed;  //汽车的行驶速度
    osg::Matrix tempmat;
    float heading;
    float m_pos;
    unsigned short usLength ;
    unsigned short  usCnt;
    char chrBuffer[2000];
    unsigned long ulComNo ;
};

调用

int main(int argc, char **argv)
{
   model();
}

结果

飞机姿态1

飞机姿态2

飞机姿态3

http://www.bilibili.com/video/BV1Zh411k7Ts?share_medium=android&share_source=copy_link&bbid=XY8DB868C46E6A7B9DCC719BE55B83C665BB6&ts=1611922641741
写在最后
(1)买硬件一定要买大厂家的产品，负责，到时候由于相关资料好和资源太少，你会欲哭无泪的。
本系统硬件资料：
链接：
(2)搜索资料，不要找相关，二值要直奔主题，找给厂家的资料，不要像我一样，从：找串口驱动(百度、谷歌、cscd......)——>从自带程序中将C#代码改造成C++——>找技术手册pdf自己写硬件驱动——>修改C51的代码做移植.......最后发现在管网直接由现成的代码考过了就可以用。
(3)关于osg已经oe的各种环境配置，且等我下回分解！
(4)代码链接：链接: https://pan.baidu.com/s/1yGy91iCshJyQokgT0qDqqQ 提取码: 1avh 复制这段内容后打开百度网盘手机App，操作更方便哦

rexinx

关注

1
点赞
踩
8

收藏

觉得还不错? 一键收藏
打赏
3
评论
OSG仿真案例（9）——JY61陀螺仪控制飞机姿态

前言在调试osg中模型运动姿态时，总觉得直观性不够强。所以有了想买个硬件陀螺仪(当时并不知道这个硬件应该叫什么名字，在淘宝搜索角度传感器的)。几个驱动1.CH340驱动这个驱动在自带资源包里面，但是不可以用，只能自己在网上找，发现是型号不对。(自己芯片是CH340N，但是资源的驱动是CH341)。2.串口驱动切记不要自己炫技自己写，真的是得不偿失，最后在官网找到了。include "...
复制链接

扫一扫