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.");
            }
        }
        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);
                }
            }
        }
Exemple #3
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);
                }
            }
        }