private void _bluetera_ConnectionStatusChanged(IBlueteraDevice sender, ConnectionStatus args)
{
Dispatcher.Invoke(async() =>
{
try
{
// avoid some potential races
if (sender != _bluetera)
{
return;
}
// re-enable IMU
if (args == ConnectionStatus.Connected)
{
await StartImu();
ApplicationState = ApplicationStateType.Connected;
UpdateControls();
}
else
{
StopAll(); // This will prevent auto-reconnection
}
}
catch (BlueteraException)
{
ApplicationState = ApplicationStateType.Error;
}
});
}
private void Bluetera_ConnectionStatusChanged(IBlueteraDevice sender, ConnectionStatus args)
{
if (args == ConnectionStatus.Connected)
{
_connectedDevices.TryAdd(sender.Id, sender);
}
else
{
IBlueteraDevice dummy;
_connectedDevices.TryRemove(sender.Id, out dummy);
}
}
private static void Device_ConnectionStatusChanged(IBlueteraDevice sender, ConnectionStatus args)
{
Console.WriteLine($"Connection status changed. Device = {sender.AddressAsString}, Status = {args}");
if (args == ConnectionStatus.Connected)
{
device = sender;
}
else
{
//device.Dispose();
device = null;
}
}
private void StopAll()
{
try
{
// Stop scanning - will be ignored if not relevant
_btManager.StopScan();
// Dispose Bluetera device. Will disconnect if needed
_bluetera?.Disconnect();
_bluetera = null;
}
catch (Exception) { /* Ignore */ }
ApplicationState = ApplicationStateType.Idle;
UpdateControls();
}
private void _btManager_AdvertismentReceived(IBlueteraManager sender, BlueteraAdvertisement args)
{
Dispatcher.InvokeAsync(async() =>
{
try
{
// avoid race
if (ApplicationState != ApplicationStateType.Scanning)
{
return;
}
_btManager.StopScan();
_dataRateMeter.Reset();
ApplicationState = ApplicationStateType.Connecting;
UpdateControls();
// Try connecting to Bluetera
_bluetera = await _btManager.Connect(args.Address); // This method will either connect or throw
_bluetera.ConnectionStatusChanged += _bluetera_ConnectionStatusChanged;
_bluetera.DownlinkMessageReceived += _bluetera_DownlinkMessageReceived;
// Start IMU. Methods will throw on failure
await StartImu();
// update UI
ApplicationState = ApplicationStateType.Connected;
UpdateControls();
}
catch (BlueteraException ex)
{
_bluetera?.Disconnect();
_bluetera = null;
ApplicationState = ApplicationStateType.Error;
UpdateControls();
}
});
}
private void _bluetera_DownlinkMessageReceived(IBlueteraDevice sender, DownlinkMessage args)
{
Dispatcher.Invoke(() =>
{
// update data rate UI
//DataRate = _dataRateMeter.DataRate;
switch (args.PayloadCase)
{
case DownlinkMessage.PayloadOneofCase.Quaternion:
{
// log
_dataLogger.Info(args.Quaternion.ToString());
// update rate meter
_dataRateMeter.Update(args.Quaternion.Timestamp);
DataRate = _dataRateMeter.DataRate;
// raw Bluetera quaternion
var qRaw = new Quaternion(args.Quaternion.X, args.Quaternion.Y, args.Quaternion.Z, args.Quaternion.W);
// transform to WPF model coordinates
_qt = new Quaternion(-qRaw.X, -qRaw.Y, qRaw.Z, qRaw.W);
// capture the first quanternion as q0
if (_q0.IsIdentity)
{
_q0 = _qt.Inverse();
}
// apply IMU-to-Body transform
var qBody = _q0 * _qt * _qbm;
model.Transform = new RotateTransform3D(new QuaternionRotation3D(qBody));
// update Euler angles
var angles = qBody.GetEuelerAngles();
Roll = angles[0];
Pitch = angles[1];
Yaw = angles[2];
}
break;
case DownlinkMessage.PayloadOneofCase.Acceleration:
// log
_dataLogger.Info(args.Acceleration.ToString());
//// update rate meter
//_dataRateMeter.Update(args.Acceleration.Timestamp);
//DataRate = _dataRateMeter.DataRate;
// Update chart
AccX = args.Acceleration.X;
AccelerationValues_X.Add(AccX);
if (AccelerationValues_X.Count > 100)
{
AccelerationValues_X.RemoveAt(0);
}
AccY = args.Acceleration.Y;
AccelerationValues_Y.Add(AccY);
if (AccelerationValues_Y.Count > 100)
{
AccelerationValues_Y.RemoveAt(0);
}
AccZ = args.Acceleration.Z;
AccelerationValues_Z.Add(AccZ);
if (AccelerationValues_Z.Count > 100)
{
AccelerationValues_Z.RemoveAt(0);
}
break;
default:
/* Currently ignore all other message types */
break;
}
});
}
static void Main(string[] args)
{
// Note
// When running in non-UWP desktop applications like this one, you must either:
// - pair with the Bluetera from Windows Settings before running this example
// - uncomment the relevant lines in the sdkConnect() method
Console.WriteLine("Running");
BlueteraManager sdk = BlueteraManager.Instance;
sdk.AdvertismentReceived += BlueteraDevice_AdvertismentReceived;
bool running = true;
Console.WriteLine("\n\n'q' - quit\n's' - start scan\n't' - stop scan\n'c' - connect\n'd' - disconnect\n'e' - send Echo\n'i' - configure and start IMU\n'j' - stop IMU\n'l' - start IMU and logging\n\n");
while (running)
{
var key = Console.ReadKey(true);
switch (key.KeyChar)
{
case 'q':
Console.WriteLine("Quitting");
running = false;
break;
case 's':
Console.WriteLine("Starting Scan");
sdk.StartScan();
break;
case 't':
Console.WriteLine("Stopping Scan");
sdk.StopScan();
break;
case 'c':
Console.WriteLine("Connecting");
sdk.StopScan();
device = sdk.Connect(lastAddress, autopair: true).Result; // Note: non-UWP apps (like this one) should set autopair:true. See BlueteraSdk.Connect() for more info
device.ConnectionStatusChanged += Device_ConnectionStatusChanged;
device.DownlinkMessageReceived += Device_DownlinkMessageReceived;
Console.WriteLine($"Device connection status: {device.ConnectionStatus}");
break;
case 'd':
Console.WriteLine("Disconnecting");
if (device != null)
{
device.Disconnect();
device = null;
}
break;
case 'e':
{
Console.WriteLine("Sending echo");
var result = SendEcho().Result;
Console.WriteLine($"GattCommunicationStatus: {result}");
}
break;
case 'i':
{
Console.WriteLine("Configuring and starting IMU");
var result = StartImu().Result;
Console.WriteLine($"GattCommunicationStatus: {result}");
}
break;
case 'j':
{
Console.WriteLine("Stopping IMU");
var result = StopImu().Result;
Console.WriteLine($"GattCommunicationStatus: {result}");
}
break;
case 'l':
{
Console.WriteLine("Configuring and starting IMU with logging to SD card");
var result = StartLogging().Result;
Console.WriteLine($"GattCommunicationStatus: {result}");
}
break;
default:
Console.WriteLine("Invalid command");
break;
}
}
sdk.DisposeAll();
}
private static void Device_DownlinkMessageReceived(IBlueteraDevice sender, DownlinkMessage args)
{
Console.WriteLine($"Recevied message: {args.ToString()}");
}
