ximeaAirborneSystem/Source_Files/sbgrecorder.cpp

#include "Header_Files/sbgrecorder.h"
#include <iostream>
#include <string.h>
#include <QDebug>

typedef unsigned char BYTE;


using namespace sbgtc;


sbgtc::SbgRecorder::SbgRecorder()
{
    connect(this, SIGNAL(sbgReady(double,QString)),this, SLOT(onSbgReady(double,QString)));

    m_bIsSbgReady = false;
    m_bIsRecordHyperspecatralImage = false;
    m_bIsNAV_POSITION_MODE = false;
    m_bIsAccuracyLessThan7 = false;
    m_bIsSyncSystemTimeBaseGpstime = false;

    m_iSolutionMode=SBG_ECOM_SOL_MODE_UNINITIALIZED;
    m_iSolutionModeCounter=0;

    m_iSbgState=0;

    std::cout<<"SbgRecorder::SbgRecorder(construction) run!"<<std::endl;
}

void sbgtc::SbgRecorder::openSerialPort()
{
    QList<QSerialPortInfo> infos = QSerialPortInfo::availablePorts();

    //std::cout<<"number of availablePorts:"<<infos.size()<<std::endl;

    //只能是大疆psdk程序发送命令给本程序, 而大疆子自己需要一个串口, 所以当只有一个串口时, sbg的就没有串口可用
//    if(infos.size()==0||infos.size()==1)
//    {
//        m_iSbgState=0;
//        emit serialPortStatus(m_iSbgState);
//        return;
//    }

    //如果在构造函数中创建m_serial就会出现错误:
    //QObject: Cannot create children for a parent that is in a different thread.
    //(Parent is QSerialPort(0x2e31b20), parent's thread is QThread(0x2e2f130), current thread is QThread(0x2e31110)
    m_serial=new QSerialPort();

    if(m_serial->isOpen())//如果串口已经打开了 先给他关闭了
    {
        m_serial->clear();
        m_serial->close();
    }

//    QString portname = "sbg_serial_port";
    QString portname = "ttyUSB0";

    m_serial->setPortName(portname);
    m_serial->open(QIODevice::ReadWrite);


    bool x=m_serial->setBaudRate(921600);//
    if(x)
    {
        std::cout<<"波特率被成功设置为:"<<m_serial->baudRate()<<std::endl;
    }
    else
    {
        std::cout<<"波特率设置失败!"<<std::endl;

        m_iSbgState=1;
        emit serialPortStatus(m_iSbgState);
        return;
    }

    m_iSbgState=2;
    emit serialPortStatus(m_iSbgState);
}

void sbgtc::SbgRecorder::closeSerialPort()
{
    if(m_iSbgState>0)
    {
        if(m_iSbgState==3)
            stopRecordSbg();

        m_serial->close();
        m_iSbgState=0;
        emit serialPortStatus(m_iSbgState);
    }
}

void sbgtc::SbgRecorder::onSbgReady(double second,QString baseFileName)
{
    m_bIsSbgReady=true;
    m_bIsRecordHyperspecatralImage=false;
}

QByteArray sbgtc::SbgRecorder::merge(QByteArray sbgMessage)
{
//    BYTE mC = 0x0C;
//    BYTE mD = 0x0D;
//    BYTE mOut = 0x0C<<4|0x0D;
//    BYTE mOut1 = mC<<4|mD;

    QByteArray x;
    QByteArray y;
    x.resize(sbgMessage.size()/2);
    y.resize(sbgMessage.size()/2);
    for(int i=0;i<sbgMessage.size()/2;i++)
    {
        char tmp=sbgMessage.at(i*2);
        char tmp2=tmp<<4;

        x[i]=sbgMessage[i*2]<<4|sbgMessage[i*2+1];
        y[i]=sbgMessage[i*2]<<4;
    }
    return x;
}

bool sbgtc::SbgRecorder::verify(int index, QByteArray sbgMessage)
{
    if(index<0)
    {
        return false;
    }
    else
    {
        //std::cout<<"index:"<<index<<std::endl;
        //std::cout<<"count:"<<count<<std::endl;
        //return;
    }

    int scale=2;

    QByteArray msgId=sbgMessage.mid(index+2*scale,2);
    QByteArray classId=sbgMessage.mid(index+3*scale,2);

    QByteArray lengthOfData=sbgMessage.mid(index+4*scale,4);
    std::reverse(lengthOfData.begin(), lengthOfData.end());
    int tmp=lengthOfData.toInt();//?????????????

    QByteArray crc=sbgMessage.mid(index+(6+tmp)*scale,4);
    QByteArray etx=sbgMessage.mid(index+(6+tmp+2)*scale,2);//end of frame

    if(etx=="33")
    {
        std::cout<<"结尾符正确!"<<std::endl;
        return true;
    }
    else
    {
        std::cout<<"结尾符不正确!--------"<<std::endl;
        return false;
    }
}

sbgtc::SbgErrorCode sbgtc::SbgRecorder::extractOneValidFrame(rawBuffer *pHandle, uint8_t *pMsgClass, uint8_t *pMsg, void *pData, size_t *pSize, size_t maxSize)
{
    SbgErrorCode		errorCode = SBG_NOT_READY;
    SbgStreamBuffer		inputStream;
    bool				syncFound;
    size_t				payloadSize = 0;
    uint16_t			frameCrc;//
    uint16_t			computedCrc;//
    //size_t				i;

    uint8_t				receivedMsgClass;
    uint8_t				receivedMsg;
    size_t				payloadOffset;

    while (pHandle->rxBufferSize > 0)
    {
        //
        // For now, we haven't found any start of frame
        //
        syncFound = false;

        //
        // To find a valid start of frame we need at least 2 bytes in the reception buffer
        //
        if (pHandle->rxBufferSize >= 2)
        {
            //
            // Try to find a valid start of frame by looking for SYNC_1 and SYNC_2 chars
            //
            for (int i = 0; i < pHandle->rxBufferSize-1; i++)
            {
                //
                // A valid start of frame should begin with SYNC and when STX chars
                //
                if ( (pHandle->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pHandle->rxBuffer[i+1] == SBG_ECOM_SYNC_2) )
                {
                    //
                    // We have found the sync char, test if we have dummy bytes at the begining of the frame
                    //
                    if (i > 0)
                    {
                        //
                        // Remove all dumy received bytes before the begining of the frame
                        //
                        memmove(pHandle->rxBuffer, pHandle->rxBuffer+i, pHandle->rxBufferSize-i);
                        pHandle->rxBufferSize = pHandle->rxBufferSize-i;
                    }

                    //
                    // The sync has been found
                    //
                    syncFound = true;
                    break;
                }
            }
        }

        //
        // Check if a valid start of frame has been found
        //
        if (syncFound)
        {
            //
            // A valid start of frame has been found, try to extract the frame if we have at least a whole frame.
            //
            if (pHandle->rxBufferSize < 8)
            {
                //
                // Don't have enough data for a valid frame
                //
                return SBG_NOT_READY;
            }

            //
            // Initialize an input stream buffer to parse the received frame
            //
            sbgStreamBufferInitForRead(&inputStream, pHandle->rxBuffer, pHandle->rxBufferSize);

            //
            // Skip both the Sync 1 and Sync 2 chars
            //
            sbgStreamBufferSeek(&inputStream, sizeof(uint8_t)*2, SB_SEEK_CUR_INC);

            //
            // Read the command
            //
            receivedMsg = sbgStreamBufferReadUint8LE(&inputStream);
            receivedMsgClass = sbgStreamBufferReadUint8LE(&inputStream);

            //
            // Read the payload size
            //
            payloadSize = (uint16_t)sbgStreamBufferReadUint16LE(&inputStream);

            //
            // Check that the payload size is valid
            //
            if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE)
            {
                //
                // Check if we have received the whole frame
                //
                if (pHandle->rxBufferSize < payloadSize+9)
                {
                    //
                    // Don't have received the whole frame
                    //
                    return SBG_NOT_READY;
                }

                //
                // We should have the whole frame so for now, skip the payload part but before store the current cursor
                //
                payloadOffset = sbgStreamBufferTell(&inputStream);
                sbgStreamBufferSeek(&inputStream, payloadSize, SB_SEEK_CUR_INC);

                //
                // Read the frame CRC
                //
                frameCrc = sbgStreamBufferReadUint16LE(&inputStream);

                //
                // Read and test the frame ETX
                //
                if (sbgStreamBufferReadUint8LE(&inputStream) == SBG_ECOM_ETX)//
                {
                    //
                    // Go back at the beginning of the payload part
                    //
                    sbgStreamBufferSeek(&inputStream, payloadOffset, SB_SEEK_SET);

                    //
                    // We have a frame so return the received command if needed even if the CRC is still not validated
                    //
                    if (pMsg)
                    {
                        *pMsg = receivedMsg;
                    }
                    if (pMsgClass)
                    {
                        *pMsgClass = receivedMsgClass;
                    }

                    //
                    // Compute the CRC of the received frame (Skip SYNC 1 and SYNC 2 chars)
                    //
                    computedCrc = sbgCrc16Compute(((uint8_t*)sbgStreamBufferGetLinkedBuffer(&inputStream)) + 2, payloadSize + 4);//??????????????????????????????????????????????????????????????????

                    //
                    // Check if the received frame has a valid CRC
                    //
                    if (frameCrc == computedCrc)//frameCrc == computedCrc??????????????????????????????????????????????????????????????????????????????????????????????????????
                    {
                        //
                        // Extract the payload if needed
                        //
                        if (payloadSize > 0)
                        {
                            //
                            // Check if input parameters are valid
                            //
                            if ( (pData) && (pSize) )
                            {
                                //
                                // Check if we have enough space to store the payload
                                //
                                if (payloadSize <= maxSize)
                                {
                                    //
                                    // Copy the payload and return the payload size
                                    //
                                    *pSize = payloadSize;
                                    memcpy(pData, sbgStreamBufferGetCursor(&inputStream), payloadSize);
                                    errorCode = SBG_NO_ERROR;

                                    //std::cout<<"receive valid frame!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<std::endl;
                                }
                                else
                                {
                                    //
                                    // Not enough space to store the payload, we will just drop the received data
                                    //
                                    errorCode = SBG_BUFFER_OVERFLOW;
                                }
                            }
                            else
                            {
                                errorCode = SBG_NULL_POINTER;
                            }
                        }
                        else
                        {
                            //
                            // No payload but the frame has been read successfully
                            //
                            errorCode = SBG_NO_ERROR;
                        }
                    }
                    else
                    {
                        //
                        // We have an invalid frame CRC and we will directly return this error
                        //
                        errorCode = SBG_INVALID_CRC;
                    }

                    //
                    // We have read a whole valid frame so remove it from the buffer
                    // First, test if the reception buffer contains more than just the current frame
                    //
                    if (pHandle->rxBufferSize > payloadSize+9)
                    {
                        //
                        // We remove the read frame but we keep the remaining data
                        //
                        pHandle->rxBufferSize = pHandle->rxBufferSize-(payloadSize+9);
                        memmove(pHandle->rxBuffer, pHandle->rxBuffer+payloadSize+9, pHandle->rxBufferSize);
                    }
                    else
                    {
                        //
                        // We have parsed the whole received buffer so just empty it
                        //
                        pHandle->rxBufferSize = 0;
                    }

                    //
                    // We have at least found a complete frame
                    //
                    return errorCode;
                }
                else
                {
                    //
                    // The ETX char hasn't been found where it should be
                    //
                    errorCode = SBG_INVALID_FRAME;
                }
            }
            else
            {
                //
                // The payload size isn't valid
                //
                errorCode = SBG_INVALID_FRAME;
            }

            //
            // Frame size invalid or the found frame is invalid so we should have incorrectly detected a start of frame.
            // Remove the SYNC 1 and SYNC 2 chars to retry to find a new frame
            //
            pHandle->rxBufferSize -= 2;
            memmove(pHandle->rxBuffer, pHandle->rxBuffer+2, pHandle->rxBufferSize);
        }
        else
        {
            //
            // Unable to find a valid start of frame so check if the last byte is a SYNC char in order to keep it for next time
            //
            if (pHandle->rxBuffer[pHandle->rxBufferSize-1] == SBG_ECOM_SYNC_1)
            {
                //
                // Report the SYNC char and discard all other bytes in the buffer
                //
                pHandle->rxBuffer[0] = SBG_ECOM_SYNC_1;
                pHandle->rxBufferSize = 1;
            }
            else
            {
                //
                // Discard the whole buffer
                //
                pHandle->rxBufferSize = 0;
            }

            //
            // Unable to find a frame
            //
            return SBG_NOT_READY;//？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？？
        }
    }

    //
    // The whole buffer has been paresed and no valid frame has been found
    //
    return SBG_NOT_READY;
}


sbgtc::SbgErrorCode sbgtc::SbgRecorder::sbgBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData)
{
    SbgErrorCode		errorCode = SBG_NO_ERROR;
    SbgStreamBuffer		inputStream;

//    assert(pPayload);
//    assert(payloadSize > 0);
//    assert(pOutputData);

    //
    // Create an input stream buffer that points to the frame payload so we can easily parse it's content
    //
    sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize);

    //
    // Handle the different classes of messages differently
    //
    if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0)
    {
        //
        // Parse the incoming log according to its type
        //
        switch (msg)
        {
        case SBG_ECOM_LOG_STATUS:
//            errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData);
            break;
        case SBG_ECOM_LOG_IMU_DATA:
//            errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData);
            break;
        case SBG_ECOM_LOG_IMU_SHORT:
//            errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort);
            break;
        case SBG_ECOM_LOG_EKF_EULER:
            errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData);
            break;
        case SBG_ECOM_LOG_EKF_QUAT:
//            errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData);
            break;
        case SBG_ECOM_LOG_EKF_NAV:
//            errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData);
            break;
        case SBG_ECOM_LOG_SHIP_MOTION:
        case SBG_ECOM_LOG_SHIP_MOTION_HP:
//            errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData);
            break;
        case SBG_ECOM_LOG_ODO_VEL:
//            errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData);
            break;
        case SBG_ECOM_LOG_UTC_TIME:
            errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData);
            break;
        case SBG_ECOM_LOG_GPS1_VEL:
        case SBG_ECOM_LOG_GPS2_VEL:
//            errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData);
            break;
        case SBG_ECOM_LOG_GPS1_POS:
        case SBG_ECOM_LOG_GPS2_POS:
            errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData);
            break;
        case SBG_ECOM_LOG_GPS1_HDT:
        case SBG_ECOM_LOG_GPS2_HDT:
//            errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData);
            break;
        case SBG_ECOM_LOG_GPS1_RAW:
        case SBG_ECOM_LOG_GPS2_RAW:
//            errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData);
            break;
        case SBG_ECOM_LOG_MAG:
//            errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData);
            break;
        case SBG_ECOM_LOG_MAG_CALIB:
//            errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData);
            break;
        case SBG_ECOM_LOG_DVL_BOTTOM_TRACK:
//            errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
            break;
        case SBG_ECOM_LOG_DVL_WATER_TRACK:
//            errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData);
            break;
        case SBG_ECOM_LOG_AIR_DATA:
//            errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData);
            break;
        case SBG_ECOM_LOG_USBL:
//            errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData);
            break;
        case SBG_ECOM_LOG_DEPTH:
//            errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData);
            break;
        case SBG_ECOM_LOG_EVENT_A:
        case SBG_ECOM_LOG_EVENT_B:
        case SBG_ECOM_LOG_EVENT_C:
        case SBG_ECOM_LOG_EVENT_D:
        case SBG_ECOM_LOG_EVENT_E:
        case SBG_ECOM_LOG_EVENT_OUT_A:
        case SBG_ECOM_LOG_EVENT_OUT_B:
//            errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker);
            break;
        case SBG_ECOM_LOG_DIAG:
//            errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData);
            break;

        default:
            errorCode = SBG_ERROR;
        }
    }
//    else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1)
//    {
//        //
//        // Parse the message depending on the message ID
//        //
//        switch ((SbgEComLog1)msg)
//        {
//        case SBG_ECOM_LOG_FAST_IMU_DATA:
//            errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData);
//            break;
//        default:
//            errorCode = SBG_ERROR;
//        }
//    }
    else
    {
        //
        // Un-handled message class
        //
        errorCode = SBG_ERROR;
    }

    return errorCode;
}


void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage)
{
    rawBuffer pHandleTmp=initRawBuffer(sbgMessage);
    rawBuffer * pHandle=&pHandleTmp;

    uint8_t				receivedMsgClass;
    uint8_t				receivedMsg;
    size_t				payloadSize;
    uint8_t				pData[SBG_ECOM_MAX_PAYLOAD_SIZE]={0};//payloadData


    //重要的第1步：提取有效帧
    SbgErrorCode errorCode = extractOneValidFrame(pHandle,&receivedMsgClass,&receivedMsg,pData,&payloadSize,sizeof(pData));



    //SBG_INVALID_CRC,SBG_NO_ERROR,SBG_NOT_READY
    if(errorCode==SBG_NOT_READY)
        return;
    if(errorCode==SBG_INVALID_CRC)
        std::cout<<"SBG_INVALID_CRC: ------------------------------------------------------"<<std::endl;


    //重要的第2步：解析有效帧
    SbgBinaryLogData	logData;
    memset(&logData,0,sizeof(logData));
    sbgBinaryLogParse((SbgEComClass)receivedMsgClass,(SbgEComMsgId)receivedMsg,pData,payloadSize,&logData);


    //判断模式是否为: NAV_POSITION
    if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER)
    {
        m_iSolutionModeCounter++;//

        uint32_t status=logData.ekfEulerData.status;
        uint32_t mode=status>>24;//?????????????????????????????????????
//        uint32_t mode=status;//这是错的

        //一秒钟发射一次mode
        if(m_iSolutionModeCounter%200 == 0)
        {
            emit sbgSolutionModeSignal(mode);

            switch (mode)
            {
            case SBG_ECOM_SOL_MODE_UNINITIALIZED:
//                std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
                break;
            case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
//                std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
                break;
            case SBG_ECOM_SOL_MODE_AHRS:
//                std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
                break;
            case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
//                std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
                break;
            case SBG_ECOM_SOL_MODE_NAV_POSITION:
//                std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;

                break;
            default:
                break;
            }
        }

        switch (mode)
        {
        case SBG_ECOM_SOL_MODE_UNINITIALIZED:
//            std::cout<<"此刻模式为: "<<"UNINITIALIZED"<<std::endl;
            m_bIsNAV_POSITION_MODE=false;
            break;
        case SBG_ECOM_SOL_MODE_VERTICAL_GYRO:
//            std::cout<<"此刻模式为: "<<"VERTICAL_GYRO"<<std::endl;
            m_bIsNAV_POSITION_MODE=false;
            break;
        case SBG_ECOM_SOL_MODE_AHRS:
//            std::cout<<"此刻模式为: "<<"AHRS"<<std::endl;
            m_bIsNAV_POSITION_MODE=false;
            break;
        case SBG_ECOM_SOL_MODE_NAV_VELOCITY:
//            std::cout<<"此刻模式为: "<<"NAV_VELOCITY"<<std::endl;
            m_bIsNAV_POSITION_MODE=false;
            break;
        case SBG_ECOM_SOL_MODE_NAV_POSITION:
//            std::cout<<"此刻模式为: "<<"NAV_POSITION"<<std::endl;
            m_bIsNAV_POSITION_MODE=true;

            break;
        default:
            break;
        }
    }
    else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_GPS1_POS)
    {
        float maximal=0;
        float latitudeAccuracy=logData.gpsPosData.latitudeAccuracy;
        float longitudeAccuracy=logData.gpsPosData.longitudeAccuracy;
        float altitudeAccuracy=logData.gpsPosData.altitudeAccuracy;
        int satelliteCounter=(int)logData.gpsPosData.numSvUsed;

        if(latitudeAccuracy<longitudeAccuracy)
            maximal = longitudeAccuracy;
        else
            maximal = latitudeAccuracy;

        if(maximal<altitudeAccuracy)
            maximal = altitudeAccuracy;

//        std::cout<<"纬度精度为："<<maximal<<std::endl;
//        std::cout<<"numSvUsed："<<satelliteCounter<<std::endl;

        emit sbgAccuracySignal(static_cast<int>(maximal), satelliteCounter);

        if(maximal<7)
        {
            m_bIsAccuracyLessThan7=true;
        }
    }
    else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME && m_bIsAccuracyLessThan7 && !m_bIsSbgReady)
    {
        //std::cout<<"0pen time:"<<(long)logData.utcData.timeStamp/1000000<<std::endl;

        uint16_t year = logData.utcData.year;
        int month = logData.utcData.month;
        int day = logData.utcData.day;
        int hour = logData.utcData.hour;
        int minute = logData.utcData.minute;
        int second = logData.utcData.second;
//        int32_t nanoSecond = logData.utcData.nanoSecond;
//        std::cout<<"gps时间为："<<year<<","<<month<<","<<day<<","<<hour<<","<<minute<<","<<second<<","<<nanoSecond<<std::endl;

        char setGpsTimeCommand[256];
        sprintf(setGpsTimeCommand,"date --set=\"%d%02d%02d %02d:%02d:%02d\"",year,month,day,hour,minute,second);//02中的2代表2位数字，0代表以0补全

//        system(setGpsTimeCommand);

        m_bIsSyncSystemTimeBaseGpstime=true;
    }
    else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_QUAT)
    {
        //std::cout<<"1111111111111------"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
    }
    else if(receivedMsgClass!=SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_NAV)
    {
        //std::cout<<"0000000000000"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<<std::endl;
    }



    //控制开始采集高光谱影像,m_bIsNAV_POSITION_MODE &&
    if(m_bIsRecordHyperspecatralImage && m_bIsSyncSystemTimeBaseGpstime && receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_UTC_TIME)
    {
        m_baseFileName=getFileNameBaseOnTime();
        emit sbgReady(calculateTimeDifferenceBetweenSystemAndSbg(logData),m_baseFileName);
    }
}

double sbgtc::SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg(SbgBinaryLogData logData)//
{
    std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)logData.utcData.second<<","<<(int)logData.utcData.nanoSecond<<std::endl;

    //获取sbg(gps)时间（纳秒）
    double year=logData.utcData.year;
    double month=logData.utcData.month;
    double day=logData.utcData.day;

    double hour=logData.utcData.hour;//+8
    double minute=logData.utcData.minute;
    double second=logData.utcData.second;
    double nanoSecond=logData.utcData.nanoSecond;

    double timeTemp=day*24*60*60+hour*60*60+minute*60+second;//(day-1)
    double time=timeTemp+nanoSecond/1000000000;

    printf("SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg--secocnd: %f\n", time);
    //std::cout<<"SbgRecorder::calculateTimeDifferenceBetweenSystemAndSbg: "<< time <<std::endl;


    //获取系统时间（纳秒）
    struct timespec systemTime;
    clock_gettime(CLOCK_REALTIME,&systemTime);
    tm systemTime_rili;
    localtime_r(&systemTime.tv_sec, &systemTime_rili);

    double secondSystem=(systemTime_rili.tm_mday-1)*24*60*60+systemTime_rili.tm_hour*60*60+systemTime_rili.tm_min*60+systemTime_rili.tm_sec;
    double nanosecondSystem=secondSystem+static_cast<double>(systemTime.tv_nsec)/1000000000;


    //计算系统时间和gps时间之间的差距
    double TimeDifferenceBetweenSystemAndSbg = nanosecondSystem - time;

    return TimeDifferenceBetweenSystemAndSbg;
}

int sbgtc::SbgRecorder::getSbgState() const
{
    return m_iSbgState;
}

void sbgtc::SbgRecorder::startRecordSbg()
{
    openSerialPort();
    if(m_iSbgState!=2)
    {
        return;
    }

    m_iSbgState=3;
    emit serialPortStatus(m_iSbgState);

    m_baseFileName=getFileNameBaseOnTime();

    QString sbgFileName=m_baseFileName+".bin";

    FILE * fileHandle=fopen(sbgFileName.toStdString().c_str(),"w+b");

    QByteArray requestData;
    m_bRecordControl=true;
    while (m_bRecordControl)
    {
        //std::cout<<"SbgRecorder::startRecordSbg--------------:"<<std::endl;
        if(m_serial->waitForReadyRead(30000))
        {
            //requestData.resize(m_serial->size());
            requestData = m_serial->readAll();
            fwrite(requestData.data(),requestData.size(),1,fileHandle);//????????????

//            if(!m_bIsSbgReady)
//            {
//                parseSbgMessage(&requestData);
//            }
            parseSbgMessage(&requestData);//边采集惯导数据边解析，并不会导致惯导漏帧
        }
        else
        {
            std::cout<<"SbgRecorder::startRecordSbg----:Wait write response timeout"<<std::endl;
        }
    }

    m_bIsSbgReady=false;//确保每次执行函数SbgRecorder::startRecordSbg,都会执行parseSbgMessage(&requestData);

    fclose(fileHandle);

    m_iSbgState=2;
    emit serialPortStatus(m_iSbgState);
}

void sbgtc::SbgRecorder::stopRecordSbg()
{
    m_bRecordControl=false;
}

void SbgRecorder::startRecordHyperspectral()
{
    m_bIsRecordHyperspecatralImage=true;
    std::cout<<"SbgRecorder::startRecordHyperspectral----m_bIsRecordHyperspecatralImage设置为true!"<<std::endl;
}

void SbgRecorder::stopRecordHyperspectral()
{
    m_bIsRecordHyperspecatralImage=false;
}