diff --git a/lib/src/constants.dart b/lib/src/constants.dart index 30e4ee7..e985ee8 100644 --- a/lib/src/constants.dart +++ b/lib/src/constants.dart @@ -46,3 +46,7 @@ const String powerSavingModeCharacteristicUuid = "d63fd1f1-5f68-4ebb-a7c7-5e0fb9ae7557"; const String powerSavingSupportedModesCharacteristicUuid = "d63fd1f2-5f68-4ebb-a7c7-5e0fb9ae7557"; + +/// Optional notification channel supporting compact IMU data (read bit 0). +const String sensorCompactDataCharacteristicUuid = + "34c2e3c1-34aa-11eb-adc1-0242ac120002"; diff --git a/lib/src/managers/v2_sensor_handler.dart b/lib/src/managers/v2_sensor_handler.dart index a8fd942..6c4f8e0 100644 --- a/lib/src/managers/v2_sensor_handler.dart +++ b/lib/src/managers/v2_sensor_handler.dart @@ -14,6 +14,34 @@ class V2SensorHandler extends SensorHandler { final SensorSchemeReader _sensorSchemeParser; final SensorValueParser _sensorValueParser; List? _sensorSchemes; + Future? _dataCharacteristic; + + Future _selectDataCharacteristic() async { + if (await _bleManager.hasCharacteristic( + deviceId: _discoveredDevice.id, + serviceId: sensorServiceUuid, + characteristicId: sensorCompactDataCharacteristicUuid, + )) { + final capabilities = await _bleManager.read( + deviceId: _discoveredDevice.id, + serviceId: sensorServiceUuid, + characteristicId: sensorCompactDataCharacteristicUuid, + ); + if (capabilities.length == 1 && (capabilities[0] & 1) != 0) { + // Bonded peers may restore the legacy CCC even when no old app is + // listening. Acquire/release it through the manager to clear that + // stale subscription before enabling the compact channel. + final legacy = await _bleManager.subscribe( + deviceId: _discoveredDevice.id, + serviceId: sensorServiceUuid, + characteristicId: sensorDataCharacteristicUuid, + ); + await legacy.listen((_) {}).cancel(); + return sensorCompactDataCharacteristicUuid; + } + } + return sensorDataCharacteristicUuid; + } V2SensorHandler({ required DiscoveredDevice discoveredDevice, @@ -34,7 +62,7 @@ class V2SensorHandler extends SensorHandler { } if (_sensorSchemes == null) { - _readSensorScheme(); + await _readSensorScheme(); } StreamController> streamController = @@ -42,12 +70,13 @@ class V2SensorHandler extends SensorHandler { final dataStream = await _bleManager.subscribe( deviceId: _discoveredDevice.id, serviceId: sensorServiceUuid, - characteristicId: sensorDataCharacteristicUuid, + characteristicId: await (_dataCharacteristic ??= _selectDataCharacteristic()), ); final subscription = dataStream.listen( (data) async { - if (data.isNotEmpty && data[0] == sensorId) { + if (data.isNotEmpty && + (data[0] == sensorId || (sensorId == 0 && data[0] == 0x80))) { List> parsedData = await _parseData(data); for (var d in parsedData) { if (!streamController.isClosed) { diff --git a/lib/src/utils/sensor_value_parser/v2_sensor_value_parser.dart b/lib/src/utils/sensor_value_parser/v2_sensor_value_parser.dart index 4a9b2d0..2929e5c 100644 --- a/lib/src/utils/sensor_value_parser/v2_sensor_value_parser.dart +++ b/lib/src/utils/sensor_value_parser/v2_sensor_value_parser.dart @@ -7,12 +7,15 @@ const int _boneAccelSensorId = 0x07; class V2SensorValueParser extends SensorValueParser { @override - List> parse(ByteData data, List sensorSchemes) { + List> parse( + ByteData data, List sensorSchemes,) { int i = 0; // Header _requireBytes(data, i, 1, 'sensorId'); - final sensorId = data.getUint8(i); + final wireId = data.getUint8(i); + final compactImu = wireId == 0x80; + final sensorId = compactImu ? 0 : wireId; i += 1; // treat one extra byte as reserved/flags for V2 (safe no-op if unused). @@ -30,17 +33,28 @@ class V2SensorValueParser extends SensorValueParser { // Precompute size of one component payload for efficiency. final compSizes = scheme.components.map((c) => c.type.size()).toList(); - final payloadSizePerSample = compSizes.fold(0, (a, b) => a + b); + if (compactImu && + (scheme.components.length != 9 || + scheme.components.any((c) => c.type != ParseType.float))) { + throw const FormatException( + 'Compact IMU requires the nine-axis float scheme',); + } + final payloadSizePerSample = + compactImu ? 24 : compSizes.fold(0, (a, b) => a + b); const timestampSize = 8; // size of absolute timestamp const offsetSize = 2; // size of relative timestamp offset const headerSize = 2; if (data.lengthInBytes - headerSize - payloadSizePerSample < 0) { - throw FormatException('Truncated frame: need at least ${timestampSize + offsetSize} bytes ' + throw FormatException( + 'Truncated frame: need at least ${timestampSize + offsetSize} bytes ' 'for first sample, have ${data.lengthInBytes - headerSize}.'); } - if ((data.lengthInBytes - headerSize - timestampSize) != payloadSizePerSample && - (data.lengthInBytes - headerSize - timestampSize - offsetSize) % payloadSizePerSample != 0) { + if ((data.lengthInBytes - headerSize - timestampSize) != + payloadSizePerSample && + (data.lengthInBytes - headerSize - timestampSize - offsetSize) % + payloadSizePerSample != + 0) { if (sensorId == _boneAccelSensorId) { final fixedBytes = Uint8List(data.lengthInBytes + 2); // Bulk-copy existing bytes @@ -55,18 +69,25 @@ class V2SensorValueParser extends SensorValueParser { data = fixedData; } - if ((data.lengthInBytes - headerSize - timestampSize) != payloadSizePerSample && - (data.lengthInBytes - headerSize - timestampSize - offsetSize) % payloadSizePerSample != 0) { - throw FormatException('Truncated frame: have ${data.lengthInBytes - headerSize} bytes, ' + if ((data.lengthInBytes - headerSize - timestampSize) != + payloadSizePerSample && + (data.lengthInBytes - headerSize - timestampSize - offsetSize) % + payloadSizePerSample != + 0) { + throw FormatException( + 'Truncated frame: have ${data.lengthInBytes - headerSize} bytes, ' 'which is not consistent with sample size $payloadSizePerSample, timestamp and offset sizes.'); } } int dataCount; - if (data.lengthInBytes - headerSize - timestampSize == payloadSizePerSample) { + if (data.lengthInBytes - headerSize - timestampSize == + payloadSizePerSample) { dataCount = 1; } else { - dataCount = (data.lengthInBytes - headerSize - timestampSize - offsetSize) ~/ payloadSizePerSample; + dataCount = + (data.lengthInBytes - headerSize - timestampSize - offsetSize) ~/ + payloadSizePerSample; } if (dataCount < 1) { @@ -86,6 +107,7 @@ class V2SensorValueParser extends SensorValueParser { scheme: scheme, timestamp: baseTimestamp + timeOffset, compSizes: compSizes, + compactImu: compactImu, ); results.add(sample.map); i = sample.nextIndex; @@ -149,6 +171,7 @@ _ParsedSample _parseSample({ required SensorScheme scheme, required int timestamp, required List compSizes, + bool compactImu = false, }) { int i = startIndex; @@ -160,24 +183,32 @@ _ParsedSample _parseSample({ }; // Ensure group maps + units exist - Map> groupMapCache = {}; Map ensureUnitsMap(String group) { - final grp = groupMapCache[group] ??= {}; + final grp = out.putIfAbsent(group, () => {}) + as Map; if (grp['units'] == null) grp['units'] = {}; out[group] ??= grp; return (grp['units'] as Map); } // Read components in scheme order - for (final comp in scheme.components) { + for (var componentIndex = 0; + componentIndex < scheme.components.length; + componentIndex++) { + final comp = scheme.components[componentIndex]; final parseType = comp.type; - final sz = parseType.size(); + final packedAxis = compactImu && componentIndex < 6; + final sz = packedAxis ? 2 : parseType.size(); _requireBytes(data, i, sz, 'component ${comp.componentName}'); - final val = _readValue(data, i, parseType); + final val = packedAxis + ? data.getInt16(i, Endian.little) * + (componentIndex < 3 ? 2 * 9.80665 / 32768 : 2000 / 32768) + : _readValue(data, i, parseType); i += sz; // install group and component - out.putIfAbsent(comp.groupName, () => {'units': {}}); + out.putIfAbsent( + comp.groupName, () => {'units': {}},); (out[comp.groupName] as Map)[comp.componentName] = val; // units diff --git a/test/compact_imu_test.dart b/test/compact_imu_test.dart new file mode 100644 index 0000000..4c23358 --- /dev/null +++ b/test/compact_imu_test.dart @@ -0,0 +1,83 @@ +import 'dart:typed_data'; +import 'package:flutter_test/flutter_test.dart'; +import 'package:open_earable_flutter/src/utils/sensor_scheme_parser/sensor_scheme_reader.dart'; +import 'package:open_earable_flutter/src/utils/sensor_value_parser/v2_sensor_value_parser.dart'; + +void main() { + final scheme = SensorScheme(0, 'IMU', 9, null); + for (final group in ['ACCELEROMETER', 'GYROSCOPE', 'MAGNETOMETER']) { + for (final axis in ['X', 'Y', 'Z']) { + scheme.components.add(Component( + ParseType.float, + group, + axis, + group == 'ACCELEROMETER' + ? 'm/s^2' + : group == 'GYROSCOPE' + ? 'dps' + : 'uT',),); + } + } + + ByteData packet(int count, {bool compact = true}) { + final width = compact ? 24 : 36; + final b = ByteData(10 + count * width + (count > 1 ? 2 : 0)); + b.setUint8(0, compact ? 0x80 : 0); + b.setUint8(1, b.lengthInBytes - 10); + b.setUint64(2, 0x100000010, Endian.little); + for (var n = 0; n < count; n++) { + for (var i = 0; i < 6; i++) { + if (compact) { + b.setInt16( + 10 + n * width + i * 2, i.isEven ? -32768 : 32767, Endian.little,); + } else { + b.setFloat32(10 + n * width + i * 4, 1.25, Endian.little); + } + } + for (var i = 0; i < 3; i++) { + b.setFloat32( + 10 + n * width + (compact ? 12 : 24) + i * 4, 42.5, Endian.little,); + } + } + if (count > 1) b.setUint16(b.lengthInBytes - 2, 10000, Endian.little); + return b; + } + + test( + 'compact batches preserve physical units, sign and microsecond timestamps', + () { + for (final count in [1, 2, 9]) { + final result = V2SensorValueParser().parse(packet(count), [scheme]); + expect(result.length, count); + for (var n = 0; n < count; n++) { + expect(result[n]['timestamp'], 0x100000010 + n * 10000); + expect(result[n]['sensorId'], 0); + expect(result[n]['ACCELEROMETER']['X'], closeTo(-19.6133, 1e-6)); + expect( + result[n]['GYROSCOPE']['X'], closeTo(32767 * 2000 / 32768, 1e-6),); + expect(result[n]['MAGNETOMETER']['Z'], 42.5); + expect(result[n]['ACCELEROMETER']['units']['X'], 'm/s^2'); + } + } + }); + test('legacy single and batched float samples still decode', () { + for (final count in [1, 6]) { + final result = + V2SensorValueParser().parse(packet(count, compact: false), [scheme]); + expect(result.length, count); + expect(result.last['ACCELEROMETER']['X'], 1.25); + expect(result.last['timestamp'], 0x100000010 + (count - 1) * 10000); + } + }); + test('truncated compact data and incompatible schemas are rejected', () { + final bytes = packet(2).buffer.asUint8List(); + expect( + () => V2SensorValueParser() + .parse(ByteData.sublistView(bytes, 0, bytes.length - 1), [scheme]), + throwsFormatException,); + final invalid = SensorScheme(0, 'bad', 1, null) + ..components.add(Component(ParseType.int16, 'A', 'X', 'raw')); + expect(() => V2SensorValueParser().parse(packet(1), [invalid]), + throwsFormatException,); + }); +} diff --git a/test/v2_compact_subscription_test.dart b/test/v2_compact_subscription_test.dart new file mode 100644 index 0000000..b31c483 --- /dev/null +++ b/test/v2_compact_subscription_test.dart @@ -0,0 +1,72 @@ +import 'dart:async'; +import 'dart:typed_data'; +import 'package:flutter_test/flutter_test.dart'; +import 'package:open_earable_flutter/src/constants.dart'; +import 'package:open_earable_flutter/src/managers/ble_gatt_manager.dart'; +import 'package:open_earable_flutter/src/managers/v2_sensor_handler.dart'; +import 'package:open_earable_flutter/src/models/devices/discovered_device.dart'; +import 'package:open_earable_flutter/src/utils/sensor_scheme_parser/sensor_scheme_reader.dart'; +import 'package:open_earable_flutter/src/utils/sensor_value_parser/v2_sensor_value_parser.dart'; + +class _Ble implements BleGattManager { + final bool compact; + _Ble(this.compact) { data.onCancel = () { cancellations++; }; } + int cancellations = 0; + final data = StreamController>.broadcast(); + String? subscribed; + @override + bool isConnected(String deviceId) => true; + @override + Future hasCharacteristic({required String deviceId, required String serviceId, required String characteristicId}) async => compact; + @override + Future> read({required String deviceId, required String serviceId, required String characteristicId}) async => [1]; + @override + Future>> subscribe({required String deviceId, required String serviceId, required String characteristicId}) async { + subscribed = characteristicId; + return data.stream; + } + @override + dynamic noSuchMethod(Invocation invocation) => super.noSuchMethod(invocation); +} + +class _Schemes implements SensorSchemeReader { + final scheme = SensorScheme(0, 'IMU', 9, null) + ..components.addAll(List.generate(9, (i) => Component(ParseType.float, 'group', 'axis$i', 'unit'))); + @override + Future> readSensorSchemes({bool forceRead = false}) async => [scheme]; + @override + Future getSchemeForSensor(int sensorId) async => scheme; +} + +void main() { + for (final compact in [false, true]) { + test('subscription routes ${compact ? "compact" : "legacy"} IMU notifications to sensor 0', () async { + final ble = _Ble(compact); + final handler = V2SensorHandler( + discoveredDevice: DiscoveredDevice(id: 'earable', name: 'Earable', manufacturerData: Uint8List(0), rssi: -40, serviceUuids: []), + bleManager: ble, + sensorSchemeParser: _Schemes(), + sensorValueParser: V2SensorValueParser(), + ); + final stream = await handler.subscribeToSensorData(0); + expect(ble.cancellations, compact ? 1 : 0); + final result = stream.first.timeout(const Duration(seconds: 1)); + expect(ble.subscribed, compact ? sensorCompactDataCharacteristicUuid : sensorDataCharacteristicUuid); + final bytes = ByteData(compact ? 34 : 46); + bytes.setUint8(0, compact ? 0x80 : 0); + bytes.setUint8(1, bytes.lengthInBytes - 10); + bytes.setUint64(2, 123456789, Endian.little); + if (compact) { + bytes.setInt16(10, -32768, Endian.little); + } else { + bytes.setFloat32(10, -19.6133, Endian.little); + } + ble.data.add(bytes.buffer.asUint8List()); + final sample = await result; + expect(sample['sensorId'], 0); + expect(sample['timestamp'], 123456789); + expect(sample['group']['axis0'], closeTo(-19.6133, 0.00001)); + await ble.data.close(); + }); + } +}