//========================================================================================== // // 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; }
//========================================================================================== // // Called once per frame. If we've connected to a tracker, get its data and move attached object with device // //========================================================================================== void Update() { if (handle > 0) { if (sync) { // Try to predict ISenseLib.ISD_STATION_DATA_TYPE bufferData = new ISenseLib.ISD_STATION_DATA_TYPE(); ISenseLib.ISD_RingBufferQuery(handle, station, ref bufferData, new IntPtr(), new IntPtr()); float dT = (float)Time.deltaTime; float curTime = bufferData.TimeStamp; //TODO: is this right? float timeToPredict = curTime + (framePredict * dT); GetRingBufferDataAtTime(timeToPredict); } else { ISenseLib.ISD_GetTrackingData(handle, ref data); // Get the tracking data } ISenseLib.ISD_STATION_DATA_TYPE dataSet = data.Station[station - 1]; // Specify the dataset // Only IS-1200 and IS-1500 have trackState if (trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 || trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500) { trackState = dataSet.TrackingState; } Vector3 pos; // If device can control position, control position! if (trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS600 || trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS900 || trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 || trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500) { pos = new Vector3(dataSet.Position[1], -dataSet.Position[2], dataSet.Position[0]); // Their y is up/down while that's our z transform.localPosition = pos; } // Time to deal with rotation! float[] cbn = dataSet.Cbn; float[] eulers; cbn = AngleUtil.ConvertUnityCbn(cbn); // Converts our right-handed rotation frame to unity's left-handed one. eulers = AngleUtil.Cbn2Euler(cbn); transform.localRotation = Quaternion.Euler(eulers[1], eulers[2], eulers[0]); // Pitch, Yaw, Roll } }
//========================================================================================== // // 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); }