void Update()
{
if (_drawBoneOrientations)
{
_poseUpdater.DrawIMUBoneOrientations();
}
CheckForMTWConnections();
if (_acceptNewMTWs)
{
return;
}
if (!_MTWsInitialized)
{
_connectedMtwData.Clear();
if (!_masterDevice.gotoMeasurement())
{
throw new UnityException("could not enter measurement mode");
}
_masterDevice.clearCallbackHandlers();
XsDevicePtrArray deviceIds = _masterDevice.children();
for (uint i = 0; i < deviceIds.size(); i++)
{
XsDevice mtw = new XsDevice(deviceIds.at(i));
MyMtwCallback callback = new MyMtwCallback();
uint deviceId = mtw.deviceId().toInt();
if (_imuOrder.Contains(deviceId))
{
XsIMUMeasurement mtwData = new XsIMUMeasurement();
_connectedMtwData.Add(deviceId, mtwData);
callback.DataAvailable += new System.EventHandler <DataAvailableArgs>(DataAvailableCallback);
mtw.addCallbackHandler(callback);
_measuringMts.Add(mtw, callback);
}
}
_MTWsInitialized = true;
UnityEngine.Debug.Log(string.Format("Initialized {0} MTWs", _measuringMts.Keys.Count));
}
if (_MTWsInitialized)
{
// draw IMU measurements in Unity
// bake mesh so that we can get updated vertex positions
_meshRenderer.BakeMesh(_currentMesh);
foreach (KeyValuePair <uint, XsIMUMeasurement> data in _connectedMtwData)
{
if (_drawIMUOriAsBoneOri)
{
_poseUpdater.setBoneOrientation(_imuIdToBoneName[data.Key], _connectedMtwData[data.Key].quat);
}
data.Value.Draw(_meshRenderer.transform.position + _currentMesh.vertices[_imuIdToVertex[data.Key]],
_drawAcceleration);
}
if (_drawIMUOriAsBoneOri)
{
_poseUpdater.setBoneOrientation("Head", _connectedMtwData[_headId].quat);
}
// send IMU measurements to inference server and display the results
if (_getModelPrediction)
{
GetAndDisplayModelPrediction();
}
else
{
// make sure the head sensor is levelled
// only compute this when model inference not toggled
float pitch = getPitch(_connectedMtwData[_headId].quat);
float roll = getRoll(_connectedMtwData[_headId].quat);
if (Mathf.Abs(pitch) < 5.0f && Mathf.Abs(roll) < 5.0f)
{
_calibrationEnabled = true;
UnityEngine.Debug.Log("Calibration ENABLED");
}
else
{
_calibrationEnabled = false;
UnityEngine.Debug.Log("Head sensor not levelled, pitch: " + pitch + " roll: " + roll);
}
}
}
}
