/*! \brief Read all messages from the buffered read data after adding new data supplied in \a rawIn
\details This function will read all present messages in the read buffer. In order for this function
to work, you need to call readDataToBuffer() first.
\param rawIn The buffered data in which to search for messages
\param messages The messages found in the data
\return The messages that were read.
*/
XsResultValue processBufferedData(XsByteArray& rawIn, XsMessageArray& messages)
{
ProtocolHandler protocol;
if (rawIn.size())
m_dataBuffer.append(rawIn);
int popped = 0;
messages.clear();
for(;;)
{
XsByteArray raw(m_dataBuffer.data()+popped, m_dataBuffer.size()-popped);
XsMessage message;
MessageLocation location = protocol.findMessage(message, raw);
if (location.isValid())
{
// message is valid, remove data from cache
popped += location.m_size + location.m_startPos;
messages.push_back(message);
}
else
{
if (popped)
m_dataBuffer.pop_front(popped);
if (messages.empty())
return XRV_TIMEOUTNODATA;
return XRV_OK;
}
}
}
void IMU::UpdateData(IMU_DATA &data)
{
XsByteArray imuData;
XsMessageArray msgs;
while (msgs.empty())
{
Aris::Core::Sleep(1);
pDevice->readDataToBuffer(imuData);
pDevice->processBufferedData(imuData, msgs);
}
//std::cout << "msg num:" << msgs.size()<<std::endl;
for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it)
{
// Retrieve a packet
XsDataPacket packet;
if ((*it).getMessageId() == XMID_MtData2)
{
packet.setMessage((*it));
packet.setDeviceId(pDevice->mtPort.deviceId());
}
// Get the all data
auto eul = packet.orientationEuler();
auto sdi = packet.sdiData();
auto acc = packet.calibratedAcceleration();
data.yaw = eul.yaw()*PI / 180;
data.pitch = eul.pitch()*PI / 180;
data.roll = eul.roll()*PI / 180;
data.va = sdi.orientationIncrement().x() * 2 * 100;
data.vb = sdi.orientationIncrement().y() * 2 * 100;
data.vc = sdi.orientationIncrement().z() * 2 * 100;
std::copy_n(acc.data(), acc.size(), data.acc);
data.time = packet.timeOfArrival().nowMs();
data.pmLhs = *pDevice->pmImuGround2BodyGround;
data.pmRhs = *pDevice->pmBody2Imu;
}
msgs.clear();
}
