300TC 机载系统 完整功能,(1)采集影像(2)采集和解析惯导数据(3)惯导磁场校正
This commit is contained in:
131
Source_Files/sbglogparse.cpp
Normal file
131
Source_Files/sbglogparse.cpp
Normal 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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
Reference in New Issue
Block a user