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.");
            }
        }
예제 #2
0
        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()));
            }
        }
예제 #3
0
        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));
            }
        }
예제 #4
0
        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;
            }
        }
예제 #5
0
        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));
                }
            }
        }
예제 #6
0
    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()));
                }
            }
        }
    }
예제 #7
0
        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;
            }
        }
예제 #8
0
    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);
                }
            }
        }
    }
예제 #9
0
        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;
            }
        }
예제 #10
0
        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);
                }
            }
        }
예제 #11
0
        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);
                }
            }
        }