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

131
Source_Files/sbglogparse.cpp Normal file
View File

@ -0,0 +1,131 @@
#include "Header_Files/sbglogparse.h"
using namespace sbgtc;
SbgErrorCode sbgtc::sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData)
{
//assert(pInputStream);
//assert(pOutputData);
//
// Read the frame payload
//
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
//
// Return if any error has occurred while parsing the frame
//
return sbgStreamBufferGetLastError(pInputStream);
}
SbgErrorCode sbgtc::sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData)
{
// assert(pInputStream);
// assert(pOutputData);
//
// Read the frame payload
//
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream);
pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream);
pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream);
pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream);
pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream);
pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream);
pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream);
pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream);
pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
//
// Return if any error has occurred while parsing the frame
//
return sbgStreamBufferGetLastError(pInputStream);
}
/*!
* \param[in] pInputStream Input stream buffer to read the payload from.
* \param[out] pOutputData Pointer on the output structure that stores parsed data.
* \return SBG_NO_ERROR if the payload has been parsed.
*/
SbgErrorCode sbgtc::sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData)
{
//
// Read the frame payload
//
pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream);
pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream);
pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream);
pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream);
pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream);
pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream);
pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream);
//
// Test if we have a additional information such as base station id (since version 1.4)
//
if (sbgStreamBufferGetSpace(pInputStream) >= 5)
{
//
// Read the additional information
//
pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream);
pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream);
pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream);
}
else
{
//
// Default the additional information
//
pOutputData->numSvUsed = 0;
pOutputData->baseStationId = 0xFFFF;
pOutputData->differentialAge = 0xFFFF;
}
//
// Return if any error has occurred while parsing the frame
//
return sbgStreamBufferGetLastError(pInputStream);
}