private static void measure()
{
_connectedMtwData.Clear();
foreach (var detectedDevices in _xda._DetectedDevices)
{
_measuringDevice = _xda.getDevice(detectedDevices.deviceId());
_measuringDevice.gotoMeasurement();
var deviceIds = _measuringDevice.children();
for (uint i = 0; i < deviceIds.size(); i++)
{
var trackingDevice = new XsDevice(deviceIds.at(i));
var callback = new MyMtCallback();
var mtwData = new ConnectedMtData();
_connectedMtwData.Add(trackingDevice.deviceId().toInt(), mtwData);
// connect signals
callback.DataAvailable += new EventHandler <DataAvailableArgs>(_callbackHandler_DataAvailable);
trackingDevice.addCallbackHandler(callback);
_measuringMts.Add(trackingDevice, callback);
logger.Log($"Completed setting up tracker {trackingDevice.deviceId().toInt().ToString()} for measuring.");
}
logger.Log($"Completed setting up device {detectedDevices.deviceId().toInt().ToString()} for measuring.");
}
}
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);
}
}
}
}
private void btnScan_Click(object sender, EventArgs e)
{
rtbSteps.Text = "Scanning for devices...";
btnScan.Enabled = false;
Update();
GT_Senxda.m_object.reset();
GT_Senxda.m_object.scanPorts();
if (GT_Senxda.m_object._DetectedDevices.Count > 0)
{
rtbSteps.Text = string.Format("Found {0} device(s)\n", GT_Senxda.m_object._DetectedDevices.Count);
XsPortInfo portInfo = GT_Senxda.m_object._DetectedDevices[0];
if (portInfo.deviceId().isMtMk4() || portInfo.deviceId().isFmt_X000() || portInfo.deviceId().isMt9c() || portInfo.deviceId().isLegacyMtig())
{
rtbSteps.Text += "Opening port...\n";
GT_Senxda.m_object.openPort(portInfo);
MasterInfo ai = new MasterInfo(portInfo.deviceId());
ai.ComPort = portInfo.portName();
ai.BaudRate = portInfo.baudrate();
_measuringDevice = GT_Senxda.m_object.getDevice(ai.DeviceId);
ai.ProductCode = new XsString(_measuringDevice.productCode());
// Print information about detected MTi / MTx / MTmk4 device
rtbSteps.Text += string.Format("Found a device with id: {0} @ port: {1}, baudrate: {2}\n", _measuringDevice.deviceId().toXsString().toString(), ai.ComPort.toString(), ai.BaudRate);
// Create and attach callback handler to device
_myMtCallback = new MyMtCallback();
_measuringDevice.addCallbackHandler(_myMtCallback);
ConnectedMtData mtwData = new ConnectedMtData();
// connect signals
_myMtCallback.DataAvailable += new EventHandler <DataAvailableArgs>(_callbackHandler_DataAvailable);
// Put the device in configuration mode
rtbSteps.Text += "Putting device into configuration mode...\n";
if (!_measuringDevice.gotoConfig()) // Put the device into configuration mode before configuring the device
{
rtbSteps.Text = "Could not put device into configuration mode. Aborting.";
return;
}
// Configure the device. Note the differences between MTix and MTmk4
rtbSteps.Text += "Configuring the device...\n";
if (_measuringDevice.deviceId().isMt9c() || _measuringDevice.deviceId().isLegacyMtig())
{
XsOutputMode outputMode = XsOutputMode.XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XsOutputSettings.XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode = new XsDeviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);
// set the device configuration
if (!_measuringDevice.setDeviceMode(deviceMode))
{
rtbSteps.Text = "Could not configure MTix device. Aborting.";
return;
}
}
else if (_measuringDevice.deviceId().isMtMk4() || _measuringDevice.deviceId().isFmt_X000())
{
XsOutputConfigurationArray configArray = new XsOutputConfigurationArray();
if (_measuringDevice.deviceId().isMtMk4_1() || _measuringDevice.deviceId().isMtMk4_10() || _measuringDevice.deviceId().isMtMk4_100() || _measuringDevice.deviceId().isFmt1010())
{
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_PacketCounter, 0));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_SampleTimeFine, 0));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_DeltaV, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_DeltaQ, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_MagneticField, 100));
}
else
{
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_PacketCounter, 0));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_SampleTimeFine, 0));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_Quaternion, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_DeltaV, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_DeltaQ, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_MagneticField, 100));
configArray.push_back(new XsOutputConfiguration(XsDataIdentifier.XDI_StatusWord, 0));
}
if (!_measuringDevice.setOutputConfiguration(configArray))
{
rtbSteps.Text = "Could not configure MTmk4 device. Aborting.";
return;
}
}
else
{
rtbSteps.Text = "Unknown device while configuring. Aborting.";
return;
}
// Put the device in measurement mode
rtbSteps.Text += "Putting device into measurement mode...\n";
if (!_measuringDevice.gotoMeasurement())
{
rtbSteps.Text = "Could not put device into measurement mode. Aborting.";
return;
}
btnRecord.Enabled = true;
}
timer1.Interval = 20;
timer1.Enabled = true;
}
else
{
rtbSteps.Text = "No devices detected. Press 'Scan' to retry.";
btnScan.Enabled = true;
}
}
public void Start()
{
if (state != XSensState.NotStarted)
{
Debug.Log("[XSensGyroscope] Cannot call Start twice! Ignoring...\n");
return;
}
this.UpdateState(XSensState.Starting);
_xda.scanPorts();
Debug.LogFormat("[XSensGyroscope] Found {0} device(s)\n", _xda._DetectedDevices.Count);
if (_xda._DetectedDevices.Count > 0)
{
XsPortInfo portInfo = _xda._DetectedDevices[0];
if (portInfo.deviceId().isMtMk4())
{
_xda.openPort(portInfo);
MasterInfo ai = new MasterInfo(portInfo.deviceId());
ai.ComPort = portInfo.portName();
ai.BaudRate = portInfo.baudrate();
_measuringDevice = _xda.getDevice(ai.DeviceId);
ai.ProductCode = new XsString(_measuringDevice.productCode());
// Print information about detected MTi / MTx / MTmk4 device
Debug.LogFormat("[XSensGyroscope] Found a device with id: {0} @ port: {1}, baudrate: {2}\n",
_measuringDevice.deviceId().toXsString().toString(), ai.ComPort.toString(), ai.BaudRate);
// Create and attach callback handler to device
_myMtCallback = new MyMtCallback();
_measuringDevice.addCallbackHandler(_myMtCallback);
ConnectedMtData mtwData = new ConnectedMtData();
// connect signals
_myMtCallback.DataAvailable += new EventHandler<DataAvailableArgs>(DataAvailable);
// Put the device in configuration mode
Debug.Log("[XSensGyroscope] Putting device into configuration mode...\n");
if (!_measuringDevice.gotoConfig()) // Put the device into configuration mode before configuring the device
{
Debug.Log("[XSensGyroscope] Could not put device into configuration mode. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
// Configure the device. Note the differences between MTix and MTmk4
Debug.Log("[XSensGyroscope] Configuring the device...\n");
if (_measuringDevice.deviceId().isMt9c())
{
XsOutputMode outputMode = XsOutputMode.XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XsOutputSettings.XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode = new XsDeviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);
// set the device configuration
if (!_measuringDevice.setDeviceMode(deviceMode))
{
Debug.Log("[XSensGyroscope] Could not configure MTix device. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
}
else if (_measuringDevice.deviceId().isMtMk4())
{
XsOutputConfiguration quat = new XsOutputConfiguration(XsDataIdentifier.XDI_Quaternion, 0);
XsOutputConfigurationArray configArray = new XsOutputConfigurationArray();
configArray.push_back(quat);
if (!_measuringDevice.setOutputConfiguration(configArray))
{
Debug.Log("[XSensGyroscope] Could not configure MTmk4 device. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
}
else
{
Debug.Log("[XSensGyroscope] Unknown device while configuring. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
// Put the device in measurement mode
Debug.Log("[XSensGyroscope] Putting device into measurement mode...\n");
if (!_measuringDevice.gotoMeasurement())
{
Debug.Log("[XSensGyroscope] Could not put device into measurement mode. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
this.UpdateState(XSensState.Started);
}
}
}
public void Start()
{
if (state != XSensState.NotStarted)
{
Debug.Log("[XSensGyroscope] Cannot call Start twice! Ignoring...\n");
return;
}
this.UpdateState(XSensState.Starting);
_xda.scanPorts();
Debug.LogFormat("[XSensGyroscope] Found {0} device(s)\n", _xda._DetectedDevices.Count);
if (_xda._DetectedDevices.Count > 0)
{
XsPortInfo portInfo = _xda._DetectedDevices[0];
if (portInfo.deviceId().isMtMk4())
{
_xda.openPort(portInfo);
MasterInfo ai = new MasterInfo(portInfo.deviceId());
ai.ComPort = portInfo.portName();
ai.BaudRate = portInfo.baudrate();
_measuringDevice = _xda.getDevice(ai.DeviceId);
ai.ProductCode = new XsString(_measuringDevice.productCode());
// Print information about detected MTi / MTx / MTmk4 device
Debug.LogFormat("[XSensGyroscope] Found a device with id: {0} @ port: {1}, baudrate: {2}\n",
_measuringDevice.deviceId().toXsString().toString(), ai.ComPort.toString(), ai.BaudRate);
// Create and attach callback handler to device
_myMtCallback = new MyMtCallback();
_measuringDevice.addCallbackHandler(_myMtCallback);
ConnectedMtData mtwData = new ConnectedMtData();
// connect signals
_myMtCallback.DataAvailable += new EventHandler <DataAvailableArgs>(DataAvailable);
// Put the device in configuration mode
Debug.Log("[XSensGyroscope] Putting device into configuration mode...\n");
if (!_measuringDevice.gotoConfig()) // Put the device into configuration mode before configuring the device
{
Debug.Log("[XSensGyroscope] Could not put device into configuration mode. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
// Configure the device. Note the differences between MTix and MTmk4
Debug.Log("[XSensGyroscope] Configuring the device...\n");
if (_measuringDevice.deviceId().isMt9c())
{
XsOutputMode outputMode = XsOutputMode.XOM_Orientation; // output orientation data
XsOutputSettings outputSettings = XsOutputSettings.XOS_OrientationMode_Quaternion; // output orientation data as quaternion
XsDeviceMode deviceMode = new XsDeviceMode(100); // make a device mode with update rate: 100 Hz
deviceMode.setModeFlag(outputMode);
deviceMode.setSettingsFlag(outputSettings);
// set the device configuration
if (!_measuringDevice.setDeviceMode(deviceMode))
{
Debug.Log("[XSensGyroscope] Could not configure MTix device. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
}
else if (_measuringDevice.deviceId().isMtMk4())
{
XsOutputConfiguration quat = new XsOutputConfiguration(XsDataIdentifier.XDI_Quaternion, 0);
XsOutputConfigurationArray configArray = new XsOutputConfigurationArray();
configArray.push_back(quat);
if (!_measuringDevice.setOutputConfiguration(configArray))
{
Debug.Log("[XSensGyroscope] Could not configure MTmk4 device. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
}
else
{
Debug.Log("[XSensGyroscope] Unknown device while configuring. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
// Put the device in measurement mode
Debug.Log("[XSensGyroscope] Putting device into measurement mode...\n");
if (!_measuringDevice.gotoMeasurement())
{
Debug.Log("[XSensGyroscope] Could not put device into measurement mode. Aborting.");
this.UpdateState(XSensState.Failed);
return;
}
this.UpdateState(XSensState.Started);
}
}
}
