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