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;
}
}
static void Main(string[] args)
{
using (var device = SensorDevices.GetFirstAvailable())
{
device.Tare();
Console.WriteLine("Port: {0}", device.PortName);
Console.WriteLine("Friendly Name: {0}", device.FriendlyPortName);
Console.WriteLine("Sensor Type: {0}", device.SensorType);
Console.WriteLine("Connected: {0}", device.IsConnected);
Console.WriteLine("Serial: {0}", device.SerialNumber);
var line = string.Empty;
while (line == string.Empty)
{
var quatSuccess = device.GetQuaternion();
Console.WriteLine("Quaternion: {0:0.000},{1:0.000},{2:0.000},{3:0.000}", device.Quaternion.W, device.Quaternion.X, device.Quaternion.Y, device.Quaternion.Z);
var eulerSuccess = device.GetEulerAngles();
Console.WriteLine("Euler: {0:0.000},{1:0.000},{2:0.000}", device.Euler.X, device.Euler.Y, device.Euler.Z);
var sensorSuccess = device.GetNormalizedSensorData();
Console.WriteLine("Sensor data... {0} / {1}", sensorSuccess, device.TimeStamp);
Console.WriteLine("Gyro: {0:0.000},{1:0.000},{2:0.000}", device.Gyro.X, device.Gyro.Y, device.Gyro.Z);
Console.WriteLine("Accel: {0:0.000},{1:0.000},{2:0.000}", device.Accelerometer.X, device.Accelerometer.Y, device.Accelerometer.Z);
Console.WriteLine("Compass: {0:0.000},{1:0.000},{2:0.000}", device.Compass.X, device.Compass.Y, device.Compass.Z);
var color = new Color
{
R = (device.Accelerometer.X + 1) / 2,
G = (device.Accelerometer.Y + 1) / 2,
B = (device.Accelerometer.Z + 1) / 2,
};
device.SetLedColour(color);
color = device.GetLedColour();
Console.WriteLine("LED: {0:0.000},{1:0.000},{2:0.000}", color.R, color.G, color.B);
line = Console.ReadLine();
}
}
Console.ReadLine();
}
protected override void InitializeDriver()
{
var sensors = SensorDevices.GetDevices();
var i = 0;
foreach (var sensor in sensors)
{
var sensorConfig = new SensorConfig {
Sensor = sensor, Index = i
};
Sensors.Add(sensorConfig);
SetSensorState(sensorConfig);
sensor.Tare();
i++;
}
}
