novatel_gps_msgs::InscovPtr
novatel_gps_driver::InscovParser::ParseBinary(const BinaryMessage& bin_msg) throw(ParseException)
{
if (bin_msg.data_.size() != BINARY_LENGTH)
{
std::stringstream error;
error << "Unexpected inscov message size: " << bin_msg.data_.size();
throw ParseException(error.str());
}
novatel_gps_msgs::InscovPtr ros_msg = boost::make_shared<novatel_gps_msgs::Inscov>();
HeaderParser h_parser;
ros_msg->novatel_msg_header = h_parser.ParseBinary(bin_msg);
ros_msg->novatel_msg_header.message_name = GetMessageName();
ros_msg->week = ParseUInt32(&bin_msg.data_[0]);
ros_msg->seconds = ParseDouble(&bin_msg.data_[4]);
int offset = 12;
for (int i = 0; i < 9; i++, offset += 8)
{
ros_msg->position_covariance[i] = ParseDouble(&bin_msg.data_[offset]);
}
for (int i = 0; i < 9; i++, offset += 8)
{
ros_msg->attitude_covariance[i] = ParseDouble(&bin_msg.data_[offset]);
}
for (int i = 0; i < 9; i++, offset += 8)
{
ros_msg->velocity_covariance[i] = ParseDouble(&bin_msg.data_[offset]);
}
return ros_msg;
}
void LoadDatabaseMessage()
{
char filename[DATABASE_MESSAGE_NAME_LENGTH] = {"\000"};
FILE* message_fp;
struct _stat filestatus;
int filesize;
GetMessageName(filename);
message_fp = fopen(filename, "rb");
GLOBALASSERT(message_fp);
_stat(filename, &filestatus);
filesize = filestatus.st_size;//filestatus.off_t;
MessageString = AllocateMem(filesize + 1);
if (!MessageString)
{
memoryInitialisationFailure = 1;
return;
}
fread(MessageString, 1, filesize, message_fp);
// add the terminator
*((char*)MessageString + filesize) = 0;
}
/******************************************************************************
Function Name : hUpdateFormattedMsgStruct
Input(s) :
Output :
Functionality : Format the requested Msg and save it in Format data
structure which is accessible from the User module
Member of : CMsgContainerCAN
Friend of : -
Author(s) : Anish kumar
Date Created : 01.04.2010
******************************************************************************/
HRESULT CMsgContainerCAN::hUpdateFormattedMsgStruct(int nListIndex,
int& nMsgCode,
BYTE bExprnFlag_Disp,
__int64 nTimeOffset)
{
HRESULT hResult = S_FALSE;
nMsgCode = 0;
static STCANDATA sCANCurrData;
static STCANDATASPL sCANCurrDataSpl;
memset( &m_sOutFormattedData, 0, sizeof(m_sOutFormattedData) );
if( IS_MODE_APPEND(bExprnFlag_Disp) )
{
//INT nType = 0;
//INT nSize =sizeof(sCANCurrDataSpl);
//In append mode providing interpret state is not required
//hResult = m_ouAppendCanBuf.ReadEntry(nType, (BYTE*)&sCANCurrDataSpl, nSize, nListIndex, 0);
hResult = m_ouAppendCanBuf.ReadFromBuffer(&sCANCurrDataSpl, nListIndex);
sCANCurrData = *((STCANDATA*)&sCANCurrDataSpl);
if (IS_TM_REL_SET(bExprnFlag_Disp))
{
//If relative time then the time will be delta time, for formatting
sCANCurrData.m_lTickCount.QuadPart = sCANCurrDataSpl.m_nDeltime;
}
}
else
{
hResult = m_ouOWCanBuf.ReadFromBuffer(&sCANCurrData, nListIndex);
if (hResult == S_OK)
{
if (IS_TM_REL_SET(bExprnFlag_Disp))
{
//If relative time then the time will be delta time, for formatting
sCANCurrData.m_lTickCount.QuadPart -= nTimeOffset;
}
}
}
if (hResult == S_OK)
{
m_ouFormatCAN.vFormatCANDataMsg(&sCANCurrData,
&m_sOutFormattedData,
bExprnFlag_Disp);
//Now add the name of message if present in database else show the code
nMsgCode = sCANCurrData.m_uDataInfo.m_sCANMsg.m_unMsgID;
CString name;
GetMessageName(sCANCurrData, name, IS_NUM_HEX_SET(bExprnFlag_Disp));
strcpy(m_sOutFormattedData.m_acMsgDesc, name.GetBuffer(0));
}
else
{
memset (&m_sOutFormattedData, 0, sizeof(m_sOutFormattedData));
nMsgCode = -1;
}
return hResult;
}
HRESULT CMsgContainerCAN::GetMessageDetails(__int64 nMapIndex, unsigned int& msgId, CString& msgName, bool formatHexForId)
{
if (nullptr == mBmNetwork)
{
return S_FALSE;
}
STCANDATA sCANMsg;
HRESULT hResult = hReadFromOWBuffer(&sCANMsg, nMapIndex);
//Message Id
msgId = sCANMsg.m_uDataInfo.m_sCANMsg.m_unMsgID;
//Message Name
GetMessageName(sCANMsg, msgName, formatHexForId);
}
const char *GENERIC_MESSAGE<Pack>::Description()
{
return GetMessageName( Info.msg );
}
google::protobuf::Message *UserMessages::StartProtobufMessage(int msg_id, const cell_t players[], unsigned int playersNum, int flags)
{
#ifndef USE_PROTOBUF_USERMESSAGES
return NULL;
#else
protobuf::Message *buffer;
if (m_InExec || m_InHook)
{
return NULL;
}
if (msg_id < 0 || msg_id >= 255)
{
return NULL;
}
m_CurId = msg_id;
m_CellRecFilter.Initialize(players, playersNum);
m_CurFlags = flags;
if (m_CurFlags & USERMSG_INITMSG)
{
m_CellRecFilter.SetToInit(true);
}
if (m_CurFlags & USERMSG_RELIABLE)
{
m_CellRecFilter.SetToReliable(true);
}
m_InExec = true;
if (m_CurFlags & USERMSG_BLOCKHOOKS)
{
// direct message creation, return buffer "from engine". keep track
m_FakeEngineBuffer = GetMessagePrototype(msg_id)->New();
buffer = m_FakeEngineBuffer;
} else {
char messageName[32];
if (!GetMessageName(msg_id, messageName, sizeof(messageName)))
{
m_InExec = false;
return NULL;
}
protobuf::Message *msg = OnStartMessage_Pre(static_cast<IRecipientFilter *>(&m_CellRecFilter), msg_id, messageName);
switch (m_FakeMetaRes)
{
case MRES_IGNORED:
case MRES_HANDLED:
m_FakeEngineBuffer = GetMessagePrototype(msg_id)->New();
buffer = m_FakeEngineBuffer;
break;
case MRES_OVERRIDE:
m_FakeEngineBuffer = GetMessagePrototype(msg_id)->New();
// fallthrough
case MRES_SUPERCEDE:
buffer = msg;
break;
}
OnStartMessage_Post(static_cast<IRecipientFilter *>(&m_CellRecFilter), msg_id, messageName);
}
return buffer;
#endif // USE_PROTOBUF_USERMESSAGES
}
/******************************************************************************
Function Name : hUpdateFormattedMsgStruct
Input(s) :
Output :
Functionality : Format the requested Msg and save it in Format data
structure which is accessible from the User module
Member of : CMsgContainerJ1939
Friend of : -
Author(s) : Arun kumar K
Date Created : 20.01.2011
******************************************************************************/
HRESULT CMsgContainerJ1939::hUpdateFormattedMsgStruct(int nListIndex,
int& nMsgCode,
BYTE bExprnFlag_Disp,
__int64 nTimeOffset)
{
HRESULT hResult = S_FALSE;
INT nSize = 0;
nMsgCode = 0;
//memset( &m_sOutFormattedData, 0, sizeof(m_sOutFormattedData) );
if( IS_MODE_APPEND(bExprnFlag_Disp) )
{
INT nType = 0;
nSize = MAX_MSG_LEN_J1939 + sizeof(__int64);
//In append mode providing interpret state is not required
hResult = m_ouAppendJ1939Buf.ReadEntry(nType, m_pbyJ1939Data, nSize,
nListIndex, FALSE);
if (hResult == S_OK)
{
// Save it into J1939 message structure
m_sJ1939ReadMsgSpl.vSetDataStream(m_pbyJ1939Data);
// Copy delta time
BYTE* pBytTemp = m_pbyJ1939Data;
pBytTemp += /*m_sJ1939Data.unGetSize()*/nSize - sizeof(__int64);
memcpy(&m_sJ1939ReadMsgSpl.m_nDeltime, pBytTemp, sizeof(__int64));
}
}
else
{
hResult = m_ouOWJ1939Buf.ReadFromBuffer(&m_sJ1939Data,
nListIndex);
if (hResult == S_OK)
{
UINT64 u64DelTS = m_sJ1939Data.m_sMsgProperties.m_un64TimeStamp - nTimeOffset;
m_sJ1939Data.vGetDataStream(m_pbyJ1939Data);
m_sJ1939ReadMsgSpl.vSetDataStream(m_pbyJ1939Data);
m_sJ1939ReadMsgSpl.m_nDeltime = u64DelTS;
}
}
if (hResult == S_OK)
{
m_ouFormatJ1939.vFormatJ1939DataMsg(
(PSTJ1939_MSG) &(m_sJ1939ReadMsgSpl), m_sJ1939ReadMsgSpl.m_nDeltime,
&m_sOutFormattedData, bExprnFlag_Disp);
CString name;
GetMessageName(m_sJ1939ReadMsgSpl, name, IS_NUM_HEX_SET(bExprnFlag_Disp));
strcpy(m_sOutFormattedData.m_acMsgName, name.GetBuffer(0));
//Now add the name of message if present in database else show the code
if (nullptr != m_sJ1939ReadMsgSpl.m_pbyData)
{
//nMsgCode = m_sJ1939ReadMsgSpl.m_pbyData[0];
nMsgCode = m_sJ1939ReadMsgSpl.m_sMsgProperties.m_uExtendedID.m_s29BitId.unGetPGN();
}
}
return hResult;
}
