示例#1
0
        private bool InitDriver()
        {
            // Step 1:	Connect to InterSense using the serial port driver

            Debug.Assert(isenseHandle == IntPtr.Zero);

            try
            {
                isenseHandle = ISDllBridge.ISD_OpenTracker(IntPtr.Zero, 0, true, false);
            }
            catch (DllNotFoundException ex)
            {
                Log.Write("Failed to connect to InterSense via serial port: " + ex.Message,
                          Log.LogLevel.Warning);
            }

            if (isenseHandle == IntPtr.Zero)
            {
                return(false);
            }

            // Step 2:	Tell InterSense to give us euler

            for (short i = 1; i <= ISDllBridge.ISD_MAX_STATIONS; ++i)
            {
                ISDllBridge.ISD_STATION_INFO_TYPE stationInfoType;

                bool bRet = ISDllBridge.ISD_GetStationConfig(isenseHandle,
                                                             out stationInfoType,
                                                             i,
                                                             true);
                if (!bRet)
                {
                    Log.Write("ISD_GetStationConfig() failed.", Log.LogLevel.Warning);
                    return(false);
                }

                stationInfoType.AngleFormat = ISDllBridge.ISD_EULER;

                bRet = ISDllBridge.ISD_SetStationConfig(isenseHandle,
                                                        ref stationInfoType,
                                                        i,
                                                        true);

                if (!bRet)
                {
                    Log.Write("ISD_SetStationConfig() failed.", Log.LogLevel.Warning);
                    return(false);
                }
            }

            return(true);
        }
示例#2
0
        public void Dispose()
        {
            // Close the socket (if open)
            if (isenseSocket != null)
            {
                isenseSocket.Dispose();
                isenseSocket = null;
            }

            // Close the serial port (if open)
            if (isenseHandle != IntPtr.Zero)
            {
                ISDllBridge.ISD_CloseTracker(isenseHandle);
                isenseHandle = IntPtr.Zero;
            }
        }
示例#3
0
        public void Update(TimeSpan elapsedTime, bool deviceActive)
        {
            if (!IsAvailable)
            {
                return;
            }

            ISDllBridge.ISD_TRACKER_DATA_TYPE dataISense;
            bool bSuccess = false;

            // Try the socket

            if (isenseSocket != null)
            {
                if (!isenseSocket.IsConnected())
                {
                    return;
                }

                // Pass the station array so that we know that stations to
                // request data for in the network message
                bSuccess = isenseSocket.GetData(stationArray, out dataISense);
                if (bSuccess)
                {
                    stationArray.SetData(dataISense);
                    return;
                }
                else
                {
                    // If that fails, try connecting directly using the serial port driver
                    InitDriver();
                    isenseSocket = null;
                }
            }

            // Try getting the data via serial port

            if (!bSuccess && isenseHandle != IntPtr.Zero)
            {
                bSuccess = ISDllBridge.ISD_GetData(isenseHandle, out dataISense);
                if (bSuccess)
                {
                    stationArray.SetData(dataISense);
                }
            }
        }
示例#4
0
        public void SetData(ISDllBridge.ISD_TRACKER_DATA_TYPE dataISense)
		{
			Debug.Assert(nStationIndex != -1);
			state = dataISense.Station[nStationIndex];
            positionVector.X = state.Position[0];
            positionVector.Y = state.Position[1];
            positionVector.Z = state.Position[2];
            orientationVector.X = state.Orientation[0];
            orientationVector.Y = state.Orientation[1];
            orientationVector.Z = state.Orientation[2];
            CreateWorldMatrix();
        }
示例#5
0
		public bool GetData(InterSense.StationArray stationArray, out ISDllBridge.ISD_TRACKER_DATA_TYPE dataISense)
		{
			dataISense = new ISDllBridge.ISD_TRACKER_DATA_TYPE();

			for(int i=0;i<ISDllBridge.ISD_MAX_STATIONS;i++)
			{
				ISDllBridge.ISD_STATION_STATE_TYPE isdStation = new ISDllBridge.ISD_STATION_STATE_TYPE();
				isdStation.Position = new float[3];
				isdStation.Orientation = new float[4];
				dataISense.Station[i] = isdStation;
			}

			//[Concern("EC")]
			if(udp==null)
				return false;

			string str = "ISD_GetData -station ";
			//first, send our request
			int activeCount = 0;	//how many stations are active
			for(int i=0;i<ISDllBridge.ISD_MAX_STATIONS;i++)
			{
				if(!stationArray.isActive(i))
					continue;
				activeCount++;
				if (activeCount > 1)
					str += ",";
				str += (i+1).ToString();
			}
			if(activeCount<1)
				return true;

			//should have our proper request string now
			//need to turn it into bytes (there must be a better way to do this in C#)
			byte[] bytes = new byte[str.Length];
			for(int i=0;i<str.Length;i++)
				bytes[i] = Convert.ToByte(str[i]);

			//send the command to get 6DOF information from the server (the server actually communicates with the device)
			//[Concern("EH")]
			try
			{
				udp.Send(bytes,bytes.Length);
			}
			catch(Exception ex)
			{
				HandleException(ex);
				return false;
			}

			//now read it back until we fill up everything
			//get the bytes
			//[Concern("EH")]
			try
			{
				const int timeout = 1; // secs
				//[Concern("EC")]
				if (!udp.PollForData(timeout))
				{
					//[Concern("Logging")]
					/*MyTrace.Log(TraceLevel.Warning,
						"Timed out waiting for station data. (Timeout: {0} secs).",
						timeout);*/
					return false;
				}

				bytes = udp.Receive(ref sourcePoint);
			}
			catch(Exception ex)
			{
				HandleException(ex);
				return false;
			}

			//parse it
			str = Encoding.ASCII.GetString(bytes);

			string[] stationMsgs = str.Split(splitSemicolon);

			//[Concern("Logging")]
			if (stationMsgs.Length != activeCount)
			{
				/*MyTrace.Log(TraceLevel.Warning,
					"Did not receive data for all stations. Requested {0} Received {1}",
					activeCount, stationMsgs.Length);*/
			}

			foreach(string stationMsg in stationMsgs)
			{
				string[] toks = stationMsg.Split(splitSpace);
			
				int station = Int32.Parse(toks[0].Split(splitEquals)[1])-1;

				dataISense.Station[station].Position[0] = (float)Double.Parse(toks[1].Split(splitEquals)[1]);
				dataISense.Station[station].Position[1] = (float)Double.Parse(toks[2].Split(splitEquals)[1]);
				dataISense.Station[station].Position[2] = (float)Double.Parse(toks[3].Split(splitEquals)[1]);
				
				dataISense.Station[station].Orientation[0] = (float)Double.Parse(toks[4].Split(splitEquals)[1]);
				dataISense.Station[station].Orientation[1] = (float)Double.Parse(toks[5].Split(splitEquals)[1]);
				dataISense.Station[station].Orientation[2] = (float)Double.Parse(toks[6].Split(splitEquals)[1]);
			}

			hasGottenData = true;

			return true;
		}