tangchao0503/ximeaAirborneSystem
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

300TC 机载系统 完整功能，（1）采集影像（2）采集和解析惯导数据（3）惯导磁场校正

Browse Source
This commit is contained in:
tangchao0503
2022-06-13 12:01:30 +08:00
commit 1452bcc2b9
21 changed files with 4316 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

845
Source_Files/sbgrecorder.cpp Normal file
View File

@ -0,0 +1,845 @@
#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 = "ttyS4";
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)
{
// std::cout<<"纬度精度为："<<logData.gpsPosData.latitudeAccuracy<<std::endl;
float maximal=0;
float latitudeAccuracy=logData.gpsPosData.latitudeAccuracy;
float longitudeAccuracy=logData.gpsPosData.longitudeAccuracy;
float altitudeAccuracy=logData.gpsPosData.altitudeAccuracy;
if(latitudeAccuracy<longitudeAccuracy)
maximal = longitudeAccuracy;
else
maximal = latitudeAccuracy;
if(maximal<altitudeAccuracy)
maximal = altitudeAccuracy;
emit sbgAccuracySignal(static_cast<int>(maximal));
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_EULER)
{
// 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;
// std::cout<<"receivedMsg:"<<(int)receivedMsg<<std::endl;
}
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+".sbg";
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())
{
//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;
}