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.");
}
}
protected override void onProgressUpdated(SWIGTYPE_p_XsDevice dev, int current, int total, XsString identifier)
{
XsDevice device = new XsDevice(dev);
if (ProgressUpdate != null)
{
ProgressUpdate(this, new ProgressUpdateArgs(device.deviceId(), current, total, identifier.toString()));
}
}
protected override void onError(SWIGTYPE_p_XsDevice dev, XsResultValue error)
{
XsDevice device = new XsDevice(dev);
if (DeviceError != null)
{
DeviceError(this, new DeviceErrorArgs(device.deviceId(), error));
}
}
protected override void onLiveDataAvailable(SWIGTYPE_p_XsDevice dev, XsDataPacket packet)
{
XsDevice device = new XsDevice(dev);
XsDataPacket pack = new XsDataPacket(packet);
if (DataAvailable != null)
{
DataAvailable(this, new DataAvailableArgs(device, pack));
}
}
protected override void onInfoResponse(SWIGTYPE_p_XsDevice dev, XsInfoRequest request)
{
XsDevice device = new XsDevice(dev);
if (request == XsInfoRequest.XIR_BatteryLevel)
{
int batteryLevel = device.batteryLevel();
if (BatteryLevelChanged != null)
{
BatteryLevelChanged(this, new BatteryLevelChangedArgs(device.deviceId(), batteryLevel));
}
}
}
private void CheckForMTWConnections()
{
if (_acceptNewMTWs)
{
int nextCount = _masterDevice.childCount();
if (nextCount != _totalConnectedMTWs)
{
UnityEngine.Debug.Log("Number of connected MTWs: " + nextCount);
_totalConnectedMTWs = nextCount;
XsDevicePtrArray deviceIds = _masterDevice.children();
for (uint i = 0; i < deviceIds.size(); i++)
{
XsDevice dev = new XsDevice(deviceIds.at(i));
UnityEngine.Debug.Log(string.Format("Device {0} ({1})", i, dev.deviceId().toInt()));
}
}
}
}
protected override void onDeviceStateChanged(SWIGTYPE_p_XsDevice dev, XsDeviceState newState, XsDeviceState oldState)
{
XsDevice device = new XsDevice(dev);
switch (newState)
{
case XsDeviceState.XDS_Config:
if (oldState != XsDeviceState.XDS_Initial)
{
if (MeasurementStopped != null)
{
MeasurementStopped(this, new DeviceIdArg(device.deviceId()));
}
}
break;
case XsDeviceState.XDS_Measurement:
if (MeasurementStarted != null)
{
MeasurementStarted(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsDeviceState.XDS_WaitingForRecordingStart:
if (WaitingForRecordingStart != null)
{
WaitingForRecordingStart(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsDeviceState.XDS_Recording:
if (RecordingStarted != null)
{
RecordingStarted(this, new DeviceIdArg(device.deviceId()));
}
break;
default:
break;
}
}
private void ScanForStations()
{
List <MasterInfo> _stations = new List <MasterInfo>();
_xda.scanPorts();
if (_xda._DetectedDevices.Count > 0)
{
foreach (XsPortInfo portInfo in _xda._DetectedDevices)
{
if (portInfo.deviceId().isWirelessMaster() || portInfo.deviceId().isAwindaStation())
{
UnityEngine.Debug.Log("found wireless connector");
_xda.openPort(portInfo);
MasterInfo ai = new MasterInfo(portInfo.deviceId());
ai.ComPort = portInfo.portName();
ai.BaudRate = portInfo.baudrate();
_stations.Add(ai);
break;
}
}
if (_stations.Count > 0)
{
UnityEngine.Debug.Log("Found station: " + _stations[0].ToString() + " ... creating master device.");
_masterDevice = _xda.getDevice(_stations[0].DeviceId);
if (!_masterDevice.gotoConfig())
{
throw new UnityException("could not enter configuration mode of created master device");
}
UnityEngine.Debug.Log("master device created successfully, ready to enable radio");
}
else
{
throw new UnityException("no station could be found, make sure drivers are installed correctly");
}
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(XsDevice obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
protected override void onConnectivityChanged(SWIGTYPE_p_XsDevice dev, XsConnectivityState state)
{
XsDevice device = new XsDevice(dev);
if (state == XsConnectivityState.XCS_Wireless)
{
lock (_ConnectedMtws)
{
_ConnectedMtws.Add(device.deviceId());
}
}
else
{
lock (_ConnectedMtws)
{
_ConnectedMtws.Remove(device.deviceId());
}
}
switch (state)
{
case XsConnectivityState.XCS_Disconnected: /*!< Device has disconnected, only limited informational functionality is available. */
if (MtwDisconnected != null)
{
MtwDisconnected(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsConnectivityState.XCS_Rejected: /*!< Device has been rejected and is disconnected, only limited informational functionality is available. */
if (MtwRejected != null)
{
MtwRejected(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsConnectivityState.XCS_PluggedIn: /*!< Device is connected through a cable. */
if (MtwPluggedIn != null)
{
MtwPluggedIn(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsConnectivityState.XCS_Wireless: /*!< Device is connected wirelessly. */
if (MtwWireless != null)
{
MtwWireless(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsConnectivityState.XCS_File: /*!< Device is reading from a file. */
if (MtwFile != null)
{
MtwFile(this, new DeviceIdArg(device.deviceId()));
}
break;
case XsConnectivityState.XCS_Unknown: /*!< Device is in an unknown state. */
if (MtwUnknown != null)
{
MtwUnknown(this, new DeviceIdArg(device.deviceId()));
}
break;
default:
if (MtwError != null)
{
MtwError(this, new DeviceIdArg(device.deviceId()));
}
break;
}
}
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);
}
}
}
}
public DataAvailableArgs(XsDevice device, XsDataPacket packet)
{
Device = device;
Packet = packet;
}
public MtwEventArgs(XsDevice mtw, bool connected)
{
Mtw = mtw;
Connected = connected;
}
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);
}
}
}
