public override void Load()
{
try
{
IsEnabled = true;
PositionScaleFactor = 0.1;
_lastTimeStamp = 0;
_velocityVec = new Vector3D(0, 0, 0);
_positionVec = new Vector3D(0, 0, 0);
RawPosition = _positionVec;
_device = SensorDevices.GetFirstAvailable(FilterEnum.FindUSB);
_keepCallbackAlive = new ThreeSpaceInterop.DataCallbackDelegate(dataCallbackFunc);
// keepCallbackAlive is to prevent crash from garbage collection not being able to track into the unmanged code of ThreeSpace_API.dll
ThreeSpaceInterop.SetNewDataCallBack(_device.DeviceId, _keepCallbackAlive);
StreamCommandSlots slots = new StreamCommandSlots(StreamCommand.TSS_NULL);
slots.Slot0 = StreamCommand.TSS_GET_TARED_ORIENTATION_AS_QUATERNION;
slots.Slot1 = StreamCommand.TSS_GET_CORRECTED_ACCELEROMETER_VECTOR;
ThreeSpaceInterop.SetStreamingTiming(_device.DeviceId, 0, 0xffffffff, 0, ref _device.TimeStamp);
ThreeSpaceInterop.SetStreamingSlots(_device.DeviceId, ref slots, ref _device.TimeStamp);
ThreeSpaceInterop.StartStreaming(_device.DeviceId, ref _device.TimeStamp);
Calibrate();
}
catch (Exception exc)
{
IsEnabled = false;
}
}
public override void Unload()
{
if (_device.DeviceId != 0)
{
ThreeSpaceInterop.StopStreaming(_device.DeviceId, ref _device.TimeStamp);
ThreeSpaceInterop.CloseDevice(_device.DeviceId);
}
}
private void LoadSerialNumber()
{
var buffer = new byte[9];
uint timeStamp;
ThreeSpaceInterop.GetSerialNumber(_deviceId, buffer, out timeStamp);
SerialNumber = BitConverter.ToString(buffer);
}
/// <summary>
/// Returns the first available sensor device
/// </summary>
/// <returns></returns>
public static SensorDevice GetFirstAvailable()
{
var port = ThreeSpaceInterop.GetComPort(0);
if (port != null)
{
return(new SensorDevice((ComPort)port));
}
return(null);
}
/// <summary>
/// Returns the filtered tared quaternion direct from the sensor.
/// </summary>
/// <returns></returns>
public bool GetQuaternion()
{
if (!IsConnected || IsDongle)
{
return(false);
}
var result = ThreeSpaceInterop.GetTaredOrientationAsQuaternion(_deviceId, out Quaternion, out TimeStamp);
return(result == ResultEnum.NoError);
}
/// <summary>
/// Returns the filtered tared quaternion direct from the sensor.
/// </summary>
/// <returns></returns>
public bool GetNormalizedSensorData()
{
if (!IsConnected || IsDongle)
{
return(false);
}
var result = ThreeSpaceInterop.GetAllNormalizedComponentSensorData(_deviceId, out Gyro, out Accelerometer, out Compass, out TimeStamp);
return(result == ResultEnum.NoError);
}
/// <summary>
/// Create a sensor using the provided ComPort.
/// </summary>
/// <param name="port">The port to connect with.</param>
public SensorDevice(ComPort port)
{
_port = port;
_deviceId = ThreeSpaceInterop.CreateDevice(port.PortName, TimeStampModeEnum.Sensor);
IsConnected = _deviceId != Defines.NO_DEVICE_ID;
if (IsConnected)
{
LoadSerialNumber();
IsDongle = port.SensorType == SensorTypeEnum.WirelessDongle;
}
}
public void Dispose()
{
if (_isDisposed)
{
return;
}
if (IsConnected)
{
ThreeSpaceInterop.CloseDevice(_deviceId);
IsConnected = false;
}
_isDisposed = true;
}
public Color GetLedColour()
{
uint ignored;
Color result = new Color();
result.R = 0;
result.G = 0;
result.B = 0;
var resultCode = ThreeSpaceInterop.GetLedColor(_deviceId, out result, out ignored);
return(result);
}
/// <summary>
/// Return a list of all sensor devices.
/// Ensure that you dispose of them.
/// </summary>
/// <returns>List of all connected threespace devices</returns>
public static List <SensorDevice> GetDevices() //NOTE: I don't think this code works. I think I botched handling their vector thing.
{
var ports = new List <ComPort>();
var thisPort = ThreeSpaceInterop.GetComPort(0);
uint index = 0;
while (thisPort != null)
{
ports.Add((ComPort)thisPort);
index++;
thisPort = ThreeSpaceInterop.GetComPort(index);
}
var result = new List <SensorDevice>();
foreach (var port in ports)
{
result.Add(new SensorDevice(port));
}
return(result);
}
public void SetLedColour(Color color)
{
var resultCode = ThreeSpaceInterop.SetLedColor(_deviceId, new [] { color.R, color.G, color.B }, 0);
}
/// <summary>
/// Tare the device to the current orientation
/// </summary>
public void Tare()
{
uint timestamp;
ThreeSpaceInterop.TareWithCurrentOrientation(_deviceId, out timestamp);
}