132 lines
4.0 KiB
C++
132 lines
4.0 KiB
C++
#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);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|