Decode optional compact IMU sensor notifications

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";

lib/src/managers/v2_sensor_handler.dart

@@ -14,6 +14,34 @@ class V2SensorHandler extends SensorHandler<V2SensorConfig> {
   final SensorSchemeReader _sensorSchemeParser;
   final SensorValueParser _sensorValueParser;
   List<SensorScheme>? _sensorSchemes;
+  Future<String>? _dataCharacteristic;
+
+  Future<String> _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,20 +62,21 @@ class V2SensorHandler extends SensorHandler<V2SensorConfig> {
     }
 
     if (_sensorSchemes == null) {
-      _readSensorScheme();
+      await _readSensorScheme();
     }
 
     StreamController<Map<String, dynamic>> streamController =
         StreamController();
     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<Map<String, dynamic>> parsedData = await _parseData(data);
           for (var d in parsedData) {
             if (!streamController.isClosed) {

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<Map<String, dynamic>> parse(ByteData data, List<SensorScheme> sensorSchemes) {
+  List<Map<String, dynamic>> parse(
+      ByteData data, List<SensorScheme> 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<int>(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<int>(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<int> compSizes,
+  bool compactImu = false,
 }) {
   int i = startIndex;
 
@@ -160,24 +183,32 @@ _ParsedSample _parseSample({
   };
 
   // Ensure group maps + units exist
-  Map<String, Map<String, dynamic>> groupMapCache = {};
   Map<String, String> ensureUnitsMap(String group) {
-    final grp = groupMapCache[group] ??= <String, dynamic>{};
+    final grp = out.putIfAbsent(group, () => <String, dynamic>{})
+        as Map<String, dynamic>;
     if (grp['units'] == null) grp['units'] = <String, String>{};
     out[group] ??= grp;
     return (grp['units'] as Map<String, String>);
   }
 
   // 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, () => <String, dynamic>{'units': <String, String>{}});
+    out.putIfAbsent(
+        comp.groupName, () => <String, dynamic>{'units': <String, String>{}},);
     (out[comp.groupName] as Map<String, dynamic>)[comp.componentName] = val;
 
     // units

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,);
+  });
+}

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<List<int>>.broadcast();
+  String? subscribed;
+  @override
+  bool isConnected(String deviceId) => true;
+  @override
+  Future<bool> hasCharacteristic({required String deviceId, required String serviceId, required String characteristicId}) async => compact;
+  @override
+  Future<List<int>> read({required String deviceId, required String serviceId, required String characteristicId}) async => [1];
+  @override
+  Future<Stream<List<int>>> 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<List<SensorScheme>> readSensorSchemes({bool forceRead = false}) async => [scheme];
+  @override
+  Future<SensorScheme> 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();
+    });
+  }
+}