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);
}
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;
}
}
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);
}
}
}
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();
}
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;
}
