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();
}
