//==========================================================================================
//
// Display Tracker data
//
//==========================================================================================
static public void showStationData(int handle, ISenseLib.ISD_TRACKER_INFO_TYPE Tracker,
ISenseLib.ISD_STATION_INFO_TYPE Station, ISenseLib.ISD_STATION_DATA_TYPE data)
{
Console.ForegroundColor = ConsoleColor.Yellow;
// display position only if system supports it
if (Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS600 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS900 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500)
{
Console.Write("[{0:d2}%] ({1:F2},{2:F2},{3:F2})m ",
(int)(data.TrackingStatus / 2.55), data.Position[0], data.Position[1], data.Position[2]);
}
// all products can return orientation
if (Station.AngleFormat == ISenseLib.ISD_QUATERNION)
{
Console.Write("{0:F2} {1:F2} {2:F2} {3:F2} ",
data.Quaternion[0], data.Quaternion[1],
data.Quaternion[2], data.Quaternion[3]);
Console.ForegroundColor = ConsoleColor.Red;
Console.Write("Time: {0:F1} ", data.TimeStamp);
}
else // Euler angles
{
Console.Write("({0:F2},{1:F2},{2:F2})deg ",
data.Euler[0], data.Euler[1], data.Euler[2]);
Console.ForegroundColor = ConsoleColor.Red;
Console.Write("Time: {0:F1} ", data.TimeStamp);
}
// Reset the cursor
Console.CursorLeft = 0;
}
//==========================================================================================
//
// Display Tracker data in Unity console
//
//==========================================================================================
static public void ShowStationData(int handle, ISenseLib.ISD_TRACKER_INFO_TYPE Tracker,
ISenseLib.ISD_STATION_INFO_TYPE Station, ISenseLib.ISD_STATION_DATA_TYPE data)
{
// Display position only if system supports it
if (Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS600 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS900 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 ||
Tracker.TrackerModel == (uint)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500)
{
print("[" + (int)(data.TrackingStatus / 2.55) + "%] (" + data.Position[0] + "," + data.Position[1] + "," + data.Position[2] + ")m ");
}
// All products can return orientation
if (Station.AngleFormat == ISenseLib.ISD_QUATERNION)
{
print(data.Quaternion[0] + " " + data.Quaternion[1] + " " + data.Quaternion[2] + " " + data.Quaternion[3] + " Time: " + data.TimeStamp);
}
else // Euler angles
{
print("(" + data.Euler[0] + "," + data.Euler[1] + "," + data.Euler[2] + ")deg Time: " + data.TimeStamp);
}
}
//==========================================================================================
//
// Prints pretty much everything not in ShowStationData
//
//==========================================================================================
static public void DebugPrintEverything(int handle, ISenseLib.ISD_TRACKER_INFO_TYPE Tracker,
ISenseLib.ISD_STATION_INFO_TYPE Station, ISenseLib.ISD_STATION_DATA_TYPE data)
{
String s;
s = "isense Lib Version: " + Tracker.LibVersion;
s = s + " Tracker Type: " + Tracker.TrackerType;
s = s + " Tracker Model: " + Tracker.TrackerModel;
s = s + " Port: " + Tracker.Port;
s = s + " Records Per Second: " + Tracker.RecordsPerSec;
s = s + " Kbits Per Second: " + Tracker.KBitsPerSec;
s = s + " Sync State: " + Tracker.SyncState;
s = s + " Sync Rate: " + Tracker.SyncRate;
s = s + " Sync Phase: " + Tracker.SyncPhase;
s = s + " Interface: " + Tracker.Interface;
s = s + " Ult Timeout: " + Tracker.UltTimeout;
s = s + " Ult Volume: " + Tracker.UltVolume;
s = s + " Firmware Revision: " + Tracker.FirmwareRev;
s = s + " LED Enable: " + Tracker.LedEnable;
print("ISD_TRACKER_INFO_TYPE");
print(s);
s = "ID: " + Station.ID;
s = s + " State: " + Station.State;
s = s + " Compass: "******" InertiaCube: " + Station.InertiaCube;
s = s + " Enhancement: " + Station.Enhancement;
s = s + " Sensitivity: " + Station.Sensitivity;
s = s + " Prediction: " + Station.Prediction;
s = s + " AngleFormat: " + Station.AngleFormat;
s = s + " TimeStamped: " + Station.TimeStamped;
s = s + " GetInputs: " + Station.GetInputs;
s = s + " GetEncoderData: " + Station.GetEncoderData;
s = s + " Compass Compensation: " + Station.CompassCompensation;
s = s + " ImuShockSuppression: " + Station.ImuShockSuppression;
s = s + " UrmRejectionFactor: " + Station.UrmRejectionFactor;
s = s + " GetAHRSData: " + Station.GetAHRSData;
s = s + " CoordFrame: " + Station.CoordFrame;
s = s + " AccelSensitivity: " + Station.AccelSensitivity;
for (int i = 0; i < 3; i = i + 1)
{
s = s + " TipOffset[" + i + "]: " + Station.TipOffset[i];
}
s = s + " GetCameraData: " + Station.GetCameraData;
s = s + " GetAuxInputs: " + Station.GetAuxInputs;
s = s + " GetCovarianceData: " + Station.GetCovarianceData;
s = s + " GetExtendedData: " + Station.GetExtendedData;
print("ISD_STATION_INFO_TYPE");
print(s);
ISenseLib.ISD_HARDWARE_INFO_TYPE hwInfo = new ISenseLib.ISD_HARDWARE_INFO_TYPE();
ISenseLib.ISD_GetSystemHardwareInfo(handle, ref hwInfo);
s = "Valid: " + hwInfo.Valid;
s = s + " TrackerType: " + hwInfo.TrackerType;
s = s + " TrackerModel: " + hwInfo.TrackerModel;
s = s + " Port: " + hwInfo.Port;
s = s + " Interface: " + hwInfo.Interface;
s = s + " OnHost: " + hwInfo.OnHost;
s = s + " AuxSystem: " + hwInfo.AuxSystem;
s = s + " FirmwareRev: " + hwInfo.FirmwareRev;
s = s + " Model Name: " + hwInfo.ModelName;
s = s + " Cap_Position: " + hwInfo.Cap_Position;
s = s + " Cap_Orientation: " + hwInfo.Cap_Orientation;
s = s + " Cap_Encoders: " + hwInfo.Cap_Encoders;
s = s + " Cap_Prediction: " + hwInfo.Cap_Prediction;
s = s + " Cap_Compass: "******" Cap_SelfTest: " + hwInfo.Cap_SelfTest;
s = s + " Cap_ErrorLog: " + hwInfo.Cap_ErrorLog;
s = s + " Cap_UltVolume: " + hwInfo.Cap_UltVolume;
s = s + " Cap_UltGain: " + hwInfo.Cap_UltGain;
s = s + " Cap_UltTimeout: " + hwInfo.Cap_UltTimeout;
s = s + " Cap_PhotoDiode: " + hwInfo.Cap_PhotoDiode;
s = s + " Cap_MaxStations: " + hwInfo.Cap_MaxStations;
s = s + " Cap_MaxImus: " + hwInfo.Cap_MaxImus;
s = s + " Cap_MaxFPses: " + hwInfo.Cap_MaxFPses;
s = s + " Cap_MaxChannels: " + hwInfo.Cap_MaxChannels;
s = s + " Cap_MaxButtons: " + hwInfo.Cap_MaxButtons;
s = s + " Cap_MeasData: " + hwInfo.Cap_MeasData;
s = s + " Cap_DiagData: " + hwInfo.Cap_DiagData;
s = s + " Cap_PseConfig: " + hwInfo.Cap_PseConfig;
s = s + " Cap_ConfigLock: " + hwInfo.Cap_ConfigLock;
s = s + " Cap_UltMaxRange: " + hwInfo.Cap_UltMaxRange;
s = s + " Cap_CompassCal: " + hwInfo.Cap_CompassCal;
s = s + " BaudRate: " + hwInfo.BaudRate;
s = s + " NumTestLevels: " + hwInfo.NumTestLevels;
print("ISD_HARDWARE_INFO_TYPE");
print(s);
ISenseLib.ISD_STATION_HARDWARE_INFO_TYPE sh = new ISenseLib.ISD_STATION_HARDWARE_INFO_TYPE();
ISenseLib.ISD_GetStationHardwareInfo(handle, ref sh, 0);
s = "Valid: " + sh.Valid;
s = s + " ID: " + sh.ID;
s = s + " DescVersion: " + sh.DescVersion;
s = s + " FirmwareRev: " + sh.FirmwareRev;
s = s + " Serial Number: " + sh.SerialNum;
s = s + " CalDate: " + sh.CalDate;
s = s + " Port: " + sh.Port;
s = s + " Type: " + sh.Type;
print("ISD_STATION_HARDWARE_INFO_TYPE");
print(s);
s = "TrackingStatus: " + data.TrackingStatus;
s = s + " NewData: " + data.NewData;
s = s + " CommIntegrity: " + data.CommIntegrity;
s = s + " BatteryState: " + data.BatteryState;
s = s + " TimeStamp: " + data.TimeStamp;
s = s + " StillTime: " + data.StillTime;
s = s + " BatteryLevel: " + data.BatteryLevel;
s = s + " CompassYaw: " + data.CompassYaw;
s = s + " MeasQuality: " + data.MeasQuality;
s = s + " HardIronCal: " + data.HardIronCal;
s = s + " SoftIronCal: " + data.SoftIronCal;
s = s + " EnvironmentalCal: " + data.EnvironmentalCal;
s = s + " TimeSTampSeconds: " + data.TimeStampSeconds;
s = s + " TimeStampMicroSec: " + data.TimeStampMicroSec;
s = s + " OSTimeStampSeconds: " + data.OSTimeStampSeconds;
s = s + " OSTimeSTampMicroSec: " + data.OSTimeStampMicroSec;
s = s + " CompassQuality: " + data.CompassQuality;
s = s + " Temperature: " + data.Temperature;
for (int i = 0; i < 3; i = i + 1)
{
s = s + " MagBodyFrame[" + i + "]: " + data.MagBodyFrame[i];
}
s = s + " TrackingState: " + data.TrackingState;
print("ISD_STATION_DATA_TYPE");
print(s);
}
//==========================================================================================
//
// Get and display tracker information
//
//==========================================================================================
static public void showTrackerStats(int handle, ref ISenseLib.ISD_HARDWARE_INFO_TYPE hwInfo)
{
ISenseLib.ISD_TRACKER_INFO_TYPE Tracker;
ISenseLib.ISD_STATION_INFO_TYPE Station;
int i, numStations = 4;
Tracker = new ISenseLib.ISD_TRACKER_INFO_TYPE();
Station = new ISenseLib.ISD_STATION_INFO_TYPE();
Console.ForegroundColor = ConsoleColor.Green;
if (ISenseLib.ISD_GetTrackerConfig(handle, ref Tracker, true))
{
Console.Out.WriteLine("\n\n********** InterSense Tracker Information ***********\n\n");
Console.Out.WriteLine("DLL Version: " + Tracker.LibVersion);
Console.Out.WriteLine("Type: " + systemType((int)Tracker.TrackerType) + " device on port " +
Tracker.Port);
Console.Out.WriteLine("Model: " + (hwInfo.Valid ?
new String(hwInfo.ModelName) : "Unknown Tracker"));
switch (Tracker.TrackerModel)
{
case (int)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS300:
case (int)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200:
numStations = 4;
break;
case (int)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS600:
case (int)ISenseLib.ISD_SYSTEM_MODEL.ISD_IS900:
numStations = ISenseLib.ISD_MAX_STATIONS;
break;
default:
numStations = 1;
break;
}
Console.Out.WriteLine("\nStation\tTime\tState\tCube Enhancement Sensitivity Compass Prediction");
for (i = 1; i <= numStations; i++)
{
Console.Out.Write("{0:d}\t", i);
if (ISenseLib.ISD_GetStationConfig(handle, ref Station, i, false))
{
Console.Out.WriteLine("{0:s}\t{1:s}\t{2:s}\t {3:d}\t\t{4:d}\t {5:d}\t {6:d}",
Station.TimeStamped ? "ON" : "OFF",
Station.State ? "ON" : "OFF",
Station.InertiaCube == -1 ? "None" : Station.InertiaCube.ToString(),
Station.Enhancement,
Station.Sensitivity,
Station.Compass,
Station.Prediction);
}
else
{
Console.Out.WriteLine("ISD_GetStationConfig failed");
break;
}
}
Console.Out.WriteLine();
}
else
{
Console.Out.WriteLine("ISD_GetTrackerConfig failed");
}
}
static void Main(string[] args)
{
int handle;
ISenseLib.ISD_TRACKING_DATA_TYPE data;
ISenseLib.ISD_STATION_INFO_TYPE[] Station;
ISenseLib.ISD_TRACKER_INFO_TYPE Tracker;
ISenseLib.ISD_HARDWARE_INFO_TYPE hwInfo;
IntPtr handy;
bool done = false;
int station = 1;
uint maxStations = 8;
float lastTime;
// Detect first tracker. If you have more than one InterSense device and
// would like to have a specific tracker, connected to a known port,
// initialized first, then enter the port number instead of 0. Otherwise,
// tracker connected to the rs232 port with lower number is found first
handy = new IntPtr();
Console.ForegroundColor = ConsoleColor.Cyan;
Console.Out.WriteLine("Connecting to InterSense tracking device...");
// handle = ISenseLib.ISD_OpenTracker(IntPtr.Zero, 0, false, true);
handle = ISenseLib.ISD_OpenAllTrackers(IntPtr.Zero, ref handy, false, true);
// Check value of handle to see if tracker was located
if (handle < 1)
{
Console.Out.WriteLine("Failed to detect InterSense tracking device");
}
else
{
Console.Out.WriteLine("Connected; press 'q' to quit, 'e' for enhancement, 'i' to get info\n");
if (handle > 0)
{
Tracker = new ISenseLib.ISD_TRACKER_INFO_TYPE();
hwInfo = new ISenseLib.ISD_HARDWARE_INFO_TYPE();
Station = new ISenseLib.ISD_STATION_INFO_TYPE[8];
// Get tracker configuration info
ISenseLib.ISD_GetTrackerConfig(handle, ref Tracker, true);
if (ISenseLib.ISD_GetSystemHardwareInfo(handle, ref hwInfo))
{
if (hwInfo.Valid)
{
maxStations = hwInfo.Cap_MaxStations;
}
}
lastTime = ISenseLib.ISD_GetTime();
ISenseLib.ISD_GetStationConfig(handle, ref Station[station - 1], station, true);
data = new ISenseLib.ISD_TRACKING_DATA_TYPE();
while (!done)
{
ISenseLib.ISD_GetTrackingData(handle, ref data);
if (ISenseLib.ISD_GetTime() - lastTime > 0.02f)
{
lastTime = ISenseLib.ISD_GetTime();
showStationData(handle, Tracker,
Station[station - 1], data.Station[station - 1]);
}
if (Console.KeyAvailable)
{
switch (Console.ReadKey(true).KeyChar)
{
case '1':
station = 1;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
break;
case '2':
if (maxStations > 1)
{
station = 2;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '3':
if (maxStations > 2)
{
station = 3;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '4':
if (maxStations > 3)
{
station = 4;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '5':
if (maxStations > 4)
{
station = 5;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '6':
if (maxStations > 5)
{
station = 6;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '7':
if (maxStations > 6)
{
station = 7;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case '8':
if (maxStations > 7)
{
station = 8;
Console.Write("\n>> Current Station is set to {0:d} <<\n", station);
}
break;
case 'Q':
case 'q':
done = true;
break;
case 'I':
case 'i':
showTrackerStats(handle, ref hwInfo);
break;
case 'e': // Set enhancement; IS-x products only, not for InterTrax
case 'E':
// First get current station configuration
if (ISenseLib.ISD_GetStationConfig(handle,
ref Station[station - 1], station, true))
{
// Cycle enhancement
Station[station - 1].Enhancement =
(Station[station - 1].Enhancement + 1) % 3;
// Send the new configuration to the tracker
if (ISenseLib.ISD_SetStationConfig(handle,
ref Station[station - 1], station, true))
{
// display the results
showTrackerStats(handle, ref hwInfo);
}
}
break;
case 't':
case 'T':
// First get current station configuration
if (ISenseLib.ISD_GetStationConfig(handle,
ref Station[station - 1], station, true))
{
Station[station - 1].TimeStamped = !(Station[station - 1].TimeStamped);
// Send the new configuration to the tracker
if (ISenseLib.ISD_SetStationConfig(handle,
ref Station[station - 1], station, true))
{
// display the results
showTrackerStats(handle, ref hwInfo);
}
}
break;
case 'd':
case 'D':
showTrackerStats(handle, ref hwInfo);
break;
case 'p':
case 'P':
// First get current station configuration
if (ISenseLib.ISD_GetStationConfig(handle,
ref Station[station - 1], station, true))
{
// Cycle enhancement
Station[station - 1].Prediction =
(Station[station - 1].Prediction + 10) % 60;
// Send the new configuration to the tracker
if (ISenseLib.ISD_SetStationConfig(handle,
ref Station[station - 1], station, true))
{
// display the results
showTrackerStats(handle, ref hwInfo);
}
}
break;
case 's':
case 'S':
// First get current station configuration
if (ISenseLib.ISD_GetStationConfig(handle,
ref Station[station - 1], station, true))
{
// Cycle enhancement
Station[station - 1].Sensitivity =
(Station[station - 1].Sensitivity + 1) % 5;
if (Station[station - 1].Sensitivity == 0)
{
Station[station - 1].Sensitivity = 1;
}
// Send the new configuration to the tracker
if (ISenseLib.ISD_SetStationConfig(handle,
ref Station[station - 1], station, true))
{
// display the results
showTrackerStats(handle, ref hwInfo);
}
}
break;
case 'c':
case 'C':
// First get current station configuration
if (ISenseLib.ISD_GetStationConfig(handle,
ref Station[station - 1], station, true))
{
// Cycle enhancement
Station[station - 1].Compass =
(Station[station - 1].Compass + 1) % 3;
// Send the new configuration to the tracker
if (ISenseLib.ISD_SetStationConfig(handle,
ref Station[station - 1], station, true))
{
// display the results
showTrackerStats(handle, ref hwInfo);
}
}
break;
}
}
}
ISenseLib.ISD_CloseTracker(handle);
}
}
}
