蓝牙GPS信息收取

最近在做一个有关GPS导航的项目，其中一个模块是蓝牙GPS连接。做完了发上来，尽管拍砖

#ifndef __BTHGPS__H
#include "BthGps.h"
#endif



/////////////////////////////////////////////////////////////////////
// *函数功能*
//  构造函数(初始化变量)
// *参数*  
//  无
// *返回值*
//  无
CBthGps::CBthGps(void)
{
    // 自动打开微软蓝牙
    HMODULE hBthUtil = LoadLibrary( L"BthUtil.dll" );
    if ( hBthUtil == NULL )
    {
        return;
    }
   
    FuncBthGetMode lpfnBthGetMode = (FuncBthGetMode)GetProcAddress( hBthUtil, L"BthGetMode" );
    FuncBthSetMode lpfnBthSetMode = (FuncBthSetMode)GetProcAddress( hBthUtil, L"BthSetMode" );
   
    if ( lpfnBthSetMode != NULL && lpfnBthGetMode != NULL )
    {
        DWORD dwMode = 0;
        if ( lpfnBthGetMode( &dwMode ) == ERROR_SUCCESS && dwMode != BTH_DISCOVERABLE )
        {
            lpfnBthSetMode( BTH_DISCOVERABLE );
        }
    }
    FreeLibrary( hBthUtil );
   
    ZeroMemory( &m_GpsInfo, sizeof(m_GpsInfo));
    ZeroMemory( m_szGPSLine, strlen(m_szGPSLine));
    m_GpsStatus = GPS_STATE_DISCONNECT;
    m_bThreadCtl = false;

    // init
    WSADATA wsaData;
    WORD wVersionRequested = MAKEWORD( 2, 2 );
    WSAStartup( wVersionRequested, &wsaData );
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  析构函数
// *参数*  
//  无
// *返回值*
//  无
CBthGps::~CBthGps(void)
{
    WSACleanup();
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  扫描能够连接的蓝牙GPS设备
// *参数*  
//  无
// *返回值*
//  无
bool CBthGps::ScanBth(void)
{
    m_GpsStatus = GPS_STATE_SEARCHING;
    // Create inquiry blob to limit time of search
    BTHNS_INQUIRYBLOB bthiqblob;
    memset( &bthiqblob, 0, sizeof(bthiqblob));
    bthiqblob.LAP = 0x9e8b33;
    bthiqblob.length = 16;
    bthiqblob.num_responses = 16;

    // Create BLOB to point to bthiqblob
    BLOB blob;
    memset( &blob, 0, sizeof(blob));
    blob.cbSize = sizeof( BTHNS_INQUIRYBLOB );
    blob.pBlobData = (PBYTE)&bthiqblob;

    // init query
    WSAQUERYSET wsqrset;
    memset( &wsqrset, 0, sizeof(WSAQUERYSET));
    wsqrset.dwSize = sizeof(WSAQUERYSET);
    wsqrset.dwNameSpace = NS_BTH;
    wsqrset.lpBlob = &blob;

    HANDLE hLookUp = NULL;
    int nLookUpResult = WSALookupServiceBegin( &wsqrset, LUP_CONTAINERS, &hLookUp );

    if ( 0 == nLookUpResult )
    {
        char buf[4096] = {0};
        WSAQUERYSET * pQueryResult = (WSAQUERYSET *)buf;
       
        for ( ; ; )
        {
            memset( pQueryResult, 0, sizeof(WSAQUERYSET));
            pQueryResult->dwSize = sizeof(WSAQUERYSET);
            pQueryResult->dwNameSpace = NS_BTH;
            memset( &m_bthinfo, 0, sizeof(m_bthinfo));
            DWORD dwSize  = sizeof(buf);

            nLookUpResult = WSALookupServiceNext( hLookUp,
                /*LUP_CONTAINERS | */LUP_RETURN_NAME | LUP_RETURN_ADDR | LUP_RETURN_BLOB,
                &dwSize,
                pQueryResult );

            if ( pQueryResult->lpszServiceInstanceName != NULL )
//                     && _tcsstr( pQueryResult->lpszServiceInstanceName, L"HOLUX" ) != NULL )
            {
                _tcscpy( m_bthinfo.szServiceInstanceName, pQueryResult->lpszServiceInstanceName );
                m_bthinfo.ulBTHAddr = ((SOCKADDR_BTH *)pQueryResult->lpcsaBuffer->RemoteAddr.lpSockaddr )->btAddr;
               
                if ( ConnBth() == true && RecvBth() == true )
                {
                    WSALookupServiceEnd( hLookUp );
                    return true;
                }
            }
            else
            {
                WSALookupServiceEnd( hLookUp );
                return false;
            }
        }
    }
    else
    {
        WSALookupServiceEnd( hLookUp );
        return false;
    }

}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  推出蓝牙连接
// *参数*  
//  无
// *返回值*
//  无
void CBthGps::ExitBth(void)
{
    ::InterlockedTestExchange( (long *)&m_bThreadCtl, true, false );
    CloseHandle( m_hThread );
    shutdown( m_hSocket, SB_BOTH );
    closesocket( m_hSocket );
    m_GpsStatus = GPS_STATE_DISCONNECT;
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  连接蓝牙设备
// *参数*  
//  无
// *返回值*
//  无
bool CBthGps::ConnBth(void)
{
    int nResult;

    m_hSocket = socket( AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM );
    if ( INVALID_SOCKET == m_hSocket )
    {
        m_GpsStatus = GPS_STATE_DISCONNECT;
        DWORD dwLastErr = WSAGetLastError();
        return false;
    }

//     unsigned long ul = 1;
//     nResult = ioctlsocket( m_hSocket, FIONBIO, &ul );
//     if ( SOCKET_ERROR == nResult )
//     {
//         DWORD dwLastErr = WSAGetLastError();
//         return 1;
//     }

    SOCKADDR_BTH sabth;
    sabth.addressFamily = AF_BTH;
    sabth.btAddr = m_bthinfo.ulBTHAddr;
    sabth.serviceClassId.Data1 = 0x00001101;
    sabth.serviceClassId.Data2 = 0x0000;
    sabth.serviceClassId.Data3 = 0x1000;
    sabth.serviceClassId.Data4[0] = 0x80;
    sabth.serviceClassId.Data4[1] = 0x00;
    sabth.serviceClassId.Data4[2] = 0x00;
    sabth.serviceClassId.Data4[3] = 0x80;
    sabth.serviceClassId.Data4[4] = 0x5F;
    sabth.serviceClassId.Data4[5] = 0x9B;
    sabth.serviceClassId.Data4[6] = 0x34;
    sabth.serviceClassId.Data4[7] = 0xFB;

    nResult = connect( m_hSocket, (sockaddr *)&sabth, sizeof(SOCKADDR_BTH));
    if ( nResult == WSAEINPROGRESS || nResult == WSAEALREADY || nResult == WSAETIMEDOUT || nResult == WSAEWOULDBLOCK )
    {
        m_GpsStatus = GPS_STATE_DISCONNECT;
        DWORD dwLastErr = WSAGetLastError();
        return false;
    }

    timeval timeout;
    fd_set fdWrite;

    timeout.tv_sec = 8;
    timeout.tv_usec = 0;
    FD_ZERO( &fdWrite );
    FD_SET( m_hSocket, &fdWrite );

    nResult = select( 0, 0, &fdWrite, 0, &timeout );
    if ( nResult <= 0 )
    {
        m_GpsStatus = GPS_STATE_DISCONNECT;
        DWORD dwLastErr = WSAGetLastError();
        return false;
    }
   
    m_GpsStatus = GPS_STATE_SEARCHING;
    return true;
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  接收蓝牙设备数据
// *参数*  
//  无
// *返回值*
//  无
bool CBthGps::RecvBth()
{
    fd_set fdRead;
    FD_ZERO( &fdRead );
    FD_SET( m_hSocket, &fdRead );
   
    timeval timeout;
    timeout.tv_sec = 8;
    timeout.tv_usec = 0;
   
    int nRcvTmout = 6000;
    setsockopt( m_hSocket, SOL_SOCKET, SO_RCVTIMEO, (char *)&nRcvTmout, sizeof(int));
   
    char szGPSBuf[256];
    char * pNextPos = szGPSBuf;

    int nRecv = 0;
   
   
    while ( 1 )
    {
        memset( szGPSBuf, 0, sizeof(szGPSBuf));
       
        if ( select( 0, &fdRead, 0, 0, &timeout ) <= 0 )
        {
            return false;
        }
       
        if ( !FD_ISSET( m_hSocket, &fdRead ))
        {
            return false;
        }
       
        if ( recv( m_hSocket, szGPSBuf, 1, 0 ) )
        {
            if( szGPSBuf[0] == '$' )
            {
                pNextPos = szGPSBuf + 1;
                if ( select( 0, &fdRead, 0, 0, &timeout ) <= 0 )
                {
                    return false;
                }
               
                if ( !FD_ISSET( m_hSocket, &fdRead ))
                {
                    return false;
                }
               
                if ( recv( m_hSocket, pNextPos, 5, 0 ))
                    /*&& ( strstr( szGPSBuf, "GPGGA") == szGPSBuf + 1 ) */
                {
                    if ( strstr( szGPSBuf, "GPRMC") == szGPSBuf + 1 )
                    {
                        pNextPos = szGPSBuf + 6;
                        if ( select( 0, &fdRead, 0, 0, &timeout ) <= 0 )
                        {
                            return false;
                        }
                       
                        if ( !FD_ISSET( m_hSocket, &fdRead ))
                        {
                            return false;
                        }
                        if ( nRecv = recv( m_hSocket, pNextPos, MIN_LEN_GPGGA_GPRMC, 0 ) )
                        {
                            pNextPos = szGPSBuf + 6 + nRecv;
                            if ( select( 0, &fdRead, 0, 0, &timeout ) <= 0 )
                            {
                                return false;
                            }
                           
                            if ( !FD_ISSET( m_hSocket, &fdRead ))
                            {
                                return false;
                            }
                            int i = 0;
                            while ( recv( m_hSocket, pNextPos, 1, 0 )
                                && *pNextPos != 0x0a )
                            {
                                i++;
                                pNextPos = szGPSBuf + 6 + nRecv + i;
                                if ( select( 0, &fdRead, 0, 0, &timeout ) <= 0 )
                                {
                                    return false;
                                }
                               
                                if ( !FD_ISSET( m_hSocket, &fdRead ))
                                {
                                    return false;
                                }
                            }
                            pNextPos = szGPSBuf + 6 + nRecv + i + 1;
                            *pNextPos = 0x0a;
                            memset( m_szGPSLine, 0, sizeof(m_szGPSLine));
                            memcpy( m_szGPSLine, strchr( szGPSBuf, '$' ), strlen( szGPSBuf ));
//                             MultiByteToWideChar( CP_ACP, 0, szGPSBuf, -1, m_szGPSInfo, strlen(szGPSBuf)*2 );
                            return true;
                        }
                        else
                        {
                            m_GpsStatus = GPS_STATE_DISCONNECT;
                            return false;
                        }
                    }
                }
                else
                {
                    m_GpsStatus = GPS_STATE_DISCONNECT;
                    return false;
                }
            }
        }
        else
        {
            m_GpsStatus = GPS_STATE_DISCONNECT;
            return false;
        }
    }
    return false;
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  使用校验码校验GPS数据是否完整
// *参数*  
//  pcGPSData 要校验的GPS数据
// *返回值*
//  非-1     读取成功,数值表示读取到的字符数量
//  -1       读取失败
bool CBthGps::IsValidGPSDataLine(const char* pcGPSData)
{   
    int nChecksum;

    int nLen = (int)strlen( pcGPSData );
    if ( nLen < 3 )
    {
        return false;
    }

    sscanf(&pcGPSData[ nLen - 3], "*%02X", &nChecksum);

    for (int i=1; i<nLen-3; i++)
    {
        nChecksum ^= pcGPSData[ i ];
    }
   
    return !nChecksum;

}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  分析从串口读到的数据,目前只分析GPRMC数据 $GPRMC,045000.399,V,0000.0000,N,00000.0000,E,,,130907,,,N*78
// *参数*  
//  pcGPSData 要分析的数据
//  GPRMCInfo 存放分析后的数据
// *返回值*
//  无
void CBthGps::ParseData(char* pcGPSData, NMEA_GPRMC_INFO &GPRMCInfo)
{
    char szTmpData[256];
    memcpy( szTmpData, pcGPSData, strlen(pcGPSData));
    if(!IsValidGPSDataLine(szTmpData))
    {
        return;
    }
   
    NMEA_GPRMC_INFO testGPRMCInfo;
    memset(&testGPRMCInfo, 0, sizeof(testGPRMCInfo));
    TIME_FORMAT tempTime;
    memset(&tempTime, 0, sizeof(TIME_FORMAT));

    int m = 0;
    int n = 7; // 跳过$GPRMC
    long lLength = (long)strlen(szTmpData);
    char cTemp[40];
    memset(cTemp, 0, sizeof(cTemp));

    // lHour lMinute lSecond
    for(int i=7; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    char cHour[3];
    memset(cHour, 0, sizeof(cHour));
    cHour[0] = cTemp[0];
    cHour[1] = cTemp[1];

    char cMinute[3];
    memset(cMinute, 0, sizeof(cMinute));
    cMinute[0] = cTemp[2];
    cMinute[1] = cTemp[3];

    char cSecond[3];
    memset(cSecond, 0, sizeof(cSecond));
    cSecond[0] = cTemp[4];
    cSecond[1] = cTemp[5];

    tempTime.lHour   = atoi(cHour);
    tempTime.lMinute = atoi(cMinute);
    tempTime.lSecond = atoi(cSecond);
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // cStatus
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }   
    testGPRMCInfo.cStatus = cTemp[0];
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // lLatitude
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    char buf[16];
    memset(buf, 0, sizeof(buf));
    int PositionValue = 0;
    strncpy(buf, cTemp, 2) ;
    PositionValue = atoi(buf);
    memset(buf, 0, 16);
    strncpy(buf, &cTemp[2], 7) ;
    testGPRMCInfo.lLatitude = (long)((PositionValue+atof(buf)/60)*100000);
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // cNS
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.cNS = cTemp[0];
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // lLongitude
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    memset(buf, 0, sizeof(buf));
    PositionValue = 0;
    strncpy(buf, cTemp, 3) ;
    PositionValue = atoi(buf);
    memset(buf, 0, 16);
    strncpy(buf, &cTemp[3], 7) ;
    testGPRMCInfo.lLongitude = (long)((PositionValue+atof(buf)/60)*100000);
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // cEW
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.cEW = cTemp[0];
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // fSpeed
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.fSpeed = (float)atof(cTemp)* 1852/(60*60); // 海里/小时 转成 米/秒
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // fAngle
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.fAngle = (float)atof(cTemp);
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // lDay lMonth lYear
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    char cDay[3];
    memset(cDay, 0, sizeof(cDay));
    cDay[0] = cTemp[0];
    cDay[1] = cTemp[1];

    char cMonth[3];
    memset(cMonth, 0, sizeof(cMonth));
    cMonth[0] = cTemp[2];
    cMonth[1] = cTemp[3];

    char cYear[3];
    memset(cYear, 0, sizeof(cYear));
    cYear[0] = cTemp[4];
    cYear[1] = cTemp[5];

    tempTime.lDay   = atoi(cDay);
    tempTime.lMonth = atoi(cMonth);
    tempTime.lYear  = atoi(cYear) + 2000;
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // fNeg
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.fNeg = (float)atoi(cTemp);
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    // cNegDirct
    for(i=n; i<lLength; i++)
    {
        if(szTmpData[i] == ',')
        {
            n++;
            break;
        }

        cTemp[m] = szTmpData[i];
        m++;
        n++;
    }
    testGPRMCInfo.cNegDirct = cTemp[0];
    memset(cTemp, 0, sizeof(cTemp));
    m = 0;

    FixUTCTimeToBeiJingTime(&testGPRMCInfo.Time, &tempTime);

    m_Lock.Lock();
    GPRMCInfo = testGPRMCInfo;   
    m_Lock.UnLock();
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  读取数据的线程
// *参数*  
//  this指针
// *返回值*
//  无
unsigned long CBthGps::ThreadRecv( LPVOID lpParameter )
{
    CBthGps * pThis = ( CBthGps *) lpParameter;
    pThis->m_bThreadCtl = true;
    while ( pThis->m_bThreadCtl == true )
    {
        if ( pThis->RecvBth() == true )
        {
            pThis->m_Lock.Lock();
            pThis->ParseData( pThis->m_szGPSLine, pThis->m_GpsInfo );
            pThis->m_Lock.UnLock();
            if ( pThis->m_GpsInfo.cStatus == 'A' )
            {
                pThis->m_GpsStatus = GPS_STATE_CONNECT_FIXED;
            }
            else
            {
                pThis->m_GpsStatus = GPS_STATE_CONNECT_FIXING;
            }
        }
        else
        {
            pThis->m_GpsStatus = GPS_STATE_DISCONNECT;
        }

    }
    return 1;
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  读取数据的线程
// *参数*  
//  this指针
// *返回值*
//  无
void CBthGps::SetGpsStatus( short nstate )
{
    m_Lock.Lock();
    m_GpsStatus = nstate;
    m_Lock.UnLock();
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  打开蓝牙GPS并创建新线程接收数据
// *参数*  
//  this指针
// *返回值*
//  无
bool CBthGps::StartBth()
{
    DWORD dwThreadID;
    if ( ScanBth() != true )
    {
        m_GpsStatus = GPS_STATE_DISCONNECT;
        return false;
    }
//     if ( ConnBth() != 0 )
//     {
//         return false;
//     }
    if ((m_hThread = CreateThread( NULL, 0, CBthGps::ThreadRecv, (void*)this, 0, &dwThreadID )) == NULL )
    {
        m_GpsStatus = GPS_STATE_DISCONNECT;
        return false;
    }
    return true;
}

/////////////////////////////////////////////////////////////////////
// *函数功能*
//  关闭蓝牙GPS
// *参数*  
//  无
// *返回值*
//  无
void CBthGps::StopBth(void)
{
    ExitBth();
}