C#维特智能蓝牙5.0加速度陀螺仪角度姿态传感器协议解析
时间:2022-11-19 06:00:01
这是自带的windows以下是蓝牙连接库处理连接问题的代码:
using System; using System.Collections.Generic; using Windows.Devices.Bluetooth; using Windows.Devices.Bluetooth.Advertisement; using Windows.Devices.Bluetooth.GenericAttributeProfile; using Windows.Foundation; using Windows.Security.Cryptography; namespace WitBLE { class BleCore { private bool asyncLock = false; /// /// 当前连接服务 /// public GattDeviceService CurrentService { get; private set; } /// /// 目前连接的蓝牙设备 /// public BluetoothLEDevice CurrentDevice { get; private set; } /// /// 写特征对象 /// public GattCharacteristic CurrentWriteCharacteristic { get; private set; } /// /// 通知特征对象 /// public GattCharacteristic CurrentNotifyCharacteristic { get; private set; } /// /// 存储检测到的设备 /// public List DeviceList { get; private set; } /// /// 特征通知类型通知 /// private const GattClientCharacteristicConfigurationDescriptorValue CHARACTERISTIC_NOTIFICATION_TYPE = GattClientCharacteristicConfigurationDescriptorValue.Notify; /// /// 委托蓝牙设备定义搜索蓝牙设备 /// public delegate void DeviceWatcherChangedEvent(BluetoothLEDevice bluetoothLEDevice); /// /// 搜索蓝牙事件 /// public event DeviceWatcherChangedEvent DeviceWatcherChanged; /// /// 获得服务委托 /// public delegate void CharacteristicFinishEvent(int size); /// /// 获取服务事件 /// public event CharacteristicFinishEvent CharacteristicFinish; /// /// 特征委托 /// public delegate void CharacteristicAddedEvent(GattCharacteristic gattCharacteristic); /// /// 获取特征事件 /// public event CharacteristicAddedEvent CharacteristicAdded; /// /// 接受数据委托 /// /// /// public delegate void RecDataEvent(GattCharacteristic sender, byte[] data); /// /// 接受数据事件 /// public event RecDataEvent Recdate; /// /// 目前连接的蓝牙Mac /// private string CurrentDeviceMAC { get; set; } private BluetoothLEAdvertisementWatcher Watcher = null; public BleCore() { DeviceList = new List(); } /// /// 搜索蓝牙设备 /// public void StartBleDeviceWatcher() { Watcher = new BluetoothLEAdvertisementWatcher(); Watcher.ScanningMode = BluetoothLEScanningMode.Active; // only activate the watcher when we're recieving values >= -80 Watcher.SignalStrengthFilter.InRangeThresholdInDBm = -80; // stop watching if the value drops below -90 (user walked away) Watcher.SignalStrengthFilter.OutOfRangeThresholdInDBm = -90; // register callback for when we see an advertisements Watcher.Received = OnAdvertisementReceived; // wait 5 seconds to make sure the device is really out of range Watcher.SignalStrengthFilter.OutOfRangeTimeout = TimeSpan.FromMilliseconds(5000); Watcher.SignalStrengthFilter.SamplingInterval = TimeSpan.FromMilliseconds(2000); // starting watching for advertisements Watcher.Start(); Console.WriteLine("自动发现设备中.."); } /// /// 停止搜索蓝牙 /// public void StopBleDeviceWatcher() { if (Watcher != null) this.Watcher.Stop(); } /// /// 主动断开连接 /// /// public void Dispose() { CurrentDeviceMAC = null; CurrentService?.Dispose(); CurrentDevice?.Dispose(); CurrentDevice = null; CurrentService = null; CurrentWriteCharacteristic = null; CurrentNotifyCharacteristic = null; Console.WriteLine("主动断开连接"); } /// /// 匹配 ///
///
public void StartMatching(BluetoothLEDevice Device)
{
this.CurrentDevice = Device;
}
///
/// 发送数据接口
///
///
public void Write(byte[] data)
{
if (CurrentWriteCharacteristic != null)
{
CurrentWriteCharacteristic.WriteValueAsync(CryptographicBuffer.CreateFromByteArray(data), GattWriteOption.WriteWithResponse).Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
GattCommunicationStatus a = asyncInfo.GetResults();
Console.WriteLine("发送数据:" + BitConverter.ToString(data) + " State : " + a);
}
};
}
}
/// 获取蓝牙服务
///
public void FindService()
{
this.CurrentDevice.GetGattServicesAsync().Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
var services = asyncInfo.GetResults().Services;
Console.WriteLine("GattServices size=" + services.Count);
foreach (GattDeviceService ser in services)
{
FindCharacteristic(ser);
}
CharacteristicFinish?.Invoke(services.Count);
}
};
}
///
/// 按MAC地址直接组装设备ID查找设备
///
public void SelectDeviceFromIdAsync(string MAC)
{
CurrentDeviceMAC = MAC;
CurrentDevice = null;
BluetoothAdapter.GetDefaultAsync().Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
BluetoothAdapter mBluetoothAdapter = asyncInfo.GetResults();
byte[] _Bytes1 = BitConverter.GetBytes(mBluetoothAdapter.BluetoothAddress);//ulong转换为byte数组
Array.Reverse(_Bytes1);
string macAddress = BitConverter.ToString(_Bytes1, 2, 6).Replace('-', ':').ToLower();
string Id = "BluetoothLE#BluetoothLE" + macAddress + "-" + MAC;
Matching(Id);
}
};
}
///
/// 获取操作
///
///
public void SetOpteron(GattCharacteristic gattCharacteristic)
{
byte[] _Bytes1 = BitConverter.GetBytes(this.CurrentDevice.BluetoothAddress);
Array.Reverse(_Bytes1);
this.CurrentDeviceMAC = BitConverter.ToString(_Bytes1, 2, 6).Replace('-', ':').ToLower();
string msg = "正在连接设备<" + this.CurrentDeviceMAC + ">..";
Console.WriteLine(msg);
if (gattCharacteristic.CharacteristicProperties == GattCharacteristicProperties.Write)
{
this.CurrentWriteCharacteristic = gattCharacteristic;
}
if (gattCharacteristic.CharacteristicProperties == GattCharacteristicProperties.Notify)
{
this.CurrentNotifyCharacteristic = gattCharacteristic;
this.CurrentNotifyCharacteristic.ProtectionLevel = GattProtectionLevel.Plain;
this.CurrentNotifyCharacteristic.ValueChanged += Characteristic_ValueChanged;
this.EnableNotifications(CurrentNotifyCharacteristic);
}
if ((uint)gattCharacteristic.CharacteristicProperties == 26)
{
}
if (gattCharacteristic.CharacteristicProperties == (GattCharacteristicProperties.Write | GattCharacteristicProperties.Notify))
{
this.CurrentWriteCharacteristic = gattCharacteristic;
this.CurrentNotifyCharacteristic = gattCharacteristic;
this.CurrentNotifyCharacteristic.ProtectionLevel = GattProtectionLevel.Plain;
this.CurrentNotifyCharacteristic.ValueChanged += Characteristic_ValueChanged;
this.CurrentDevice.ConnectionStatusChanged += this.CurrentDevice_ConnectionStatusChanged;
this.EnableNotifications(CurrentNotifyCharacteristic);
}
}
private void OnAdvertisementReceived(BluetoothLEAdvertisementWatcher watcher, BluetoothLEAdvertisementReceivedEventArgs eventArgs)
{
BluetoothLEDevice.FromBluetoothAddressAsync(eventArgs.BluetoothAddress).Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
if (asyncInfo.GetResults() == null)
{
Console.WriteLine("没有得到结果集");
}
else
{
BluetoothLEDevice currentDevice = asyncInfo.GetResults();
if (DeviceList.FindIndex((x) => { return x.Name.Equals(currentDevice.Name); }) < 0)
{
this.DeviceList.Add(currentDevice);
DeviceWatcherChanged?.Invoke(currentDevice);
}
}
}
};
}
///
/// 获取特性
///
private void FindCharacteristic(GattDeviceService gattDeviceService)
{
this.CurrentService = gattDeviceService;
this.CurrentService.GetCharacteristicsAsync().Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
var services = asyncInfo.GetResults().Characteristics;
foreach (var c in services)
{
this.CharacteristicAdded?.Invoke(c);
}
}
};
}
///
/// 搜索到的蓝牙设备
///
///
private void Matching(string Id)
{
try
{
BluetoothLEDevice.FromIdAsync(Id).Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
BluetoothLEDevice bleDevice = asyncInfo.GetResults();
this.DeviceList.Add(bleDevice);
Console.WriteLine(bleDevice);
}
if (asyncStatus == AsyncStatus.Started)
{
Console.WriteLine(asyncStatus.ToString());
}
if (asyncStatus == AsyncStatus.Canceled)
{
Console.WriteLine(asyncStatus.ToString());
}
if (asyncStatus == AsyncStatus.Error)
{
Console.WriteLine(asyncStatus.ToString());
}
};
}
catch (Exception e)
{
string msg = "没有发现设备" + e.ToString();
Console.WriteLine(msg);
this.StartBleDeviceWatcher();
}
}
private void CurrentDevice_ConnectionStatusChanged(BluetoothLEDevice sender, object args)
{
if (sender.ConnectionStatus == BluetoothConnectionStatus.Disconnected && CurrentDeviceMAC != null)
{
if (!asyncLock)
{
asyncLock = true;
Console.WriteLine("设备已断开");
//this.CurrentDevice?.Dispose();
//this.CurrentDevice = null;
//CurrentService = null;
//CurrentWriteCharacteristic = null;
//CurrentNotifyCharacteristic = null;
//SelectDeviceFromIdAsync(CurrentDeviceMAC);
}
}
else
{
if (!asyncLock)
{
asyncLock = true;
Console.WriteLine("设备已连接");
}
}
}
///
/// 设置特征对象为接收通知对象
///
///
///
private void EnableNotifications(GattCharacteristic characteristic)
{
Console.WriteLine("收通知对象=" + CurrentDevice.Name + ":" + CurrentDevice.ConnectionStatus);
characteristic.WriteClientCharacteristicConfigurationDescriptorAsync(CHARACTERISTIC_NOTIFICATION_TYPE).Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
GattCommunicationStatus status = asyncInfo.GetResults();
if (status == GattCommunicationStatus.Unreachable)
{
Console.WriteLine("设备不可用");
if (CurrentNotifyCharacteristic != null && !asyncLock)
{
this.EnableNotifications(CurrentNotifyCharacteristic);
}
return;
}
asyncLock = false;
Console.WriteLine("设备连接状态" + status);
}
};
}
///
/// 接受到蓝牙数据
///
private void Characteristic_ValueChanged(GattCharacteristic sender, GattValueChangedEventArgs args)
{
byte[] data;
CryptographicBuffer.CopyToByteArray(args.CharacteristicValue, out data);
//Recdate?.Invoke(sender, data);
string byteResult = BitConverter.ToString(data);
DealCallDatas(byteResult);
}
///
/// 处理回调的数据
///
///
private void DealCallDatas(string data)
{
//返回的数据形式如:55-61-CD-FC-7D-FD-15-07-EA-FF-D4-FF-CA-FF-4B-F1-BD-0F-3D-12
//这里解析的数据依次为:
//数据包头[0]-标志位[1]
//-X轴加速度低8位[2]-X轴加速度高8位[3]
//-Y轴加速度低8位[4]-Y轴加速度高8位[5]
//-Z轴加速度低8位[6]-Z轴加速度高8位[7]
//-X轴角速度低8位[8]-X轴角速度高8位[9]
//-Y轴角速度低8位[10]-Y轴角速度高8位[11]
//-Z轴角速度低8位[12]-Z轴角速度高8位[13]
//-X轴角度低8位[14]-X轴角度高8位[15]
//-Y轴角度低8位[16]-Y轴角度高8位[17]
//-Z轴角度低8位[18]-Z轴角度高8位[19]
string[] list = data.Split('-');
if (list.Length > 1)
{
double AxH = Convert.ToInt32(list[3], 16); //加速度X高8位
int AxL = Convert.ToInt32(list[2], 16); //加速度X低8位
float Ax = (float)((short)((short)AxH << 8 | AxL) / 32768.0 * 16.0); //加速度X=((AxH<<8)|AxL)/32768*16g
//以下是Y和Z的,方法同上
double AyH = Convert.ToInt32(list[5], 16);
int AyL = Convert.ToInt32(list[4], 16);
float Ay = (float)((short)((short)AyH << 8 | AyL) / 32768.0 * 16.0);
double AzH = Convert.ToInt32(list[7], 16);
int AzL = Convert.ToInt32(list[6], 16);
float Az = (float)((short)((short)AzH << 8 | AzL) / 32768.0 * 16.0);
string text = "加速度: X:" + Ax.ToString("N2") + "g Y:" + Ay.ToString("N2") + "g Z:" + Az.ToString("N2") + "g";
//角速度和角度方法雷同,这里省略
//角速度X=((AxH<<8)|AxL)/32768*2000°/s
//角度X=((AxH<<8)|AxL)/32768*180°
Console.WriteLine(text);
}
}
}
}
搜索并连接设备:
private static void DeviceWatcherChanged(BluetoothLEDevice currentDevice)
{
byte[] _Bytes1 = BitConverter.GetBytes(currentDevice.BluetoothAddress);
Array.Reverse(_Bytes1);
string address = BitConverter.ToString(_Bytes1, 2, 6).Replace('-', ':').ToLower();
Console.WriteLine("发现设备:<" + currentDevice.Name + "> address:<" + address + ">");
//指定一个对象,使用下面方法去连接设备
//ConnectDevice(currentDevice);
//根据蓝牙设备名称或者蓝牙MAC地址读取设备
if (currentDevice.Name.Equals("WT901BLE67") || currentDevice.DeviceId.Contains("d0:3e:7d:a4:7a:cc"))
{
ConnectDevice(currentDevice);
//根据特征码获取数据
GattCharacteristic gattCharacteristic = characteristics.Find((x) => { return x.Uuid.Equals(new Guid("0000ffe4-0000-1000-8000-00805f9a34fb")); });
bleCore.SetOpteron(gattCharacteristic);
}
}
bleCore = new BleCore();
bleCore.DeviceWatcherChanged += DeviceWatcherChanged;
bleCore.CharacteristicAdded += CharacteristicAdded;
bleCore.CharacteristicFinish += CharacteristicFinish;
bleCore.Recdate += Recdata;
bleCore.StartBleDeviceWatcher();
如果是连接多设备,OnAdvertisementReceived改成如下:
private void OnAdvertisementReceived(BluetoothLEAdvertisementWatcher watcher, BluetoothLEAdvertisementReceivedEventArgs eventArgs)
{
BluetoothLEDevice.FromBluetoothAddressAsync(eventArgs.BluetoothAddress).Completed = (asyncInfo, asyncStatus) =>
{
if (asyncStatus == AsyncStatus.Completed)
{
if (asyncInfo.GetResults() == null)
{
Console.WriteLine("没有得到结果集");
}
else
{
BluetoothLEDevice currentDevice = asyncInfo.GetResults();
Boolean contain = false;
foreach (BluetoothLEDevice device in DeviceList) //过滤重复的设备
{
if (device.DeviceId == currentDevice.DeviceId) //设备已存在列表中
{
contain = true;
}
}
if (!contain)
{
this.DeviceList.Add(currentDevice);
DeviceWatcherChanged?.Invoke(currentDevice);
}
}
}
};
}
官网协议解释:https://wit-motion.yuque.com/docs/share/a1b38900-24ab-4853-9a00-847b3ec4d39b?#SQh02