823 lines
28 KiB
C++
823 lines
28 KiB
C++
#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 & 0xf;
|
||
|
||
//一秒钟发射一次mode
|
||
if(m_iSolutionModeCounter%200 == 0)
|
||
{
|
||
emit sbgSolutionModeSignal(mode);
|
||
// std::cout << "logData.ekfEulerData.status: " << status << std::endl;
|
||
|
||
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;
|
||
//
|
||
// std::cout<<"latitude："<<logData.gpsPosData.latitude<<std::endl;
|
||
// std::cout<<"longitude："<<logData.gpsPosData.longitude<<std::endl;
|
||
// std::cout<<"altitude："<<logData.gpsPosData.altitude<<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 && m_bIsNAV_POSITION_MODE)
|
||
{
|
||
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;
|
||
}
|