From 2ca069fdb925241bc3d72e69384fad68d76e656f Mon Sep 17 00:00:00 2001 From: Billie Ponder Date: Thu, 15 Oct 2026 07:40:34 -0400 Subject: [PATCH] fix(sensors): clear stale calibration state and preserve remount recovery (#46) Co-authored-by: Developer --- lib/services/calibration_service.dart | 24 +- lib/services/sensor_fusion_service.dart | 328 ++++++++++++------ .../sensor_calibration_lifecycle_test.dart | 279 +++++++++++++++ 3 files changed, 529 insertions(+), 102 deletions(-) create mode 100644 test/services/sensor_calibration_lifecycle_test.dart diff --git a/lib/services/calibration_service.dart b/lib/services/calibration_service.dart index 5608eaa..48119bf 100644 --- a/lib/services/calibration_service.dart +++ b/lib/services/calibration_service.dart @@ -39,7 +39,10 @@ class CalibrationService { await prefs.setDouble('calibration_gyro_bias_x_$motorcycleId', gyroBiasX); await prefs.setDouble('calibration_gyro_bias_y_$motorcycleId', gyroBiasY); await prefs.setDouble('calibration_gyro_bias_z_$motorcycleId', gyroBiasZ); - await prefs.setBool('calibration_is_calibrated_$motorcycleId', isCalibrated); + await prefs.setBool( + 'calibration_is_calibrated_$motorcycleId', + isCalibrated, + ); await prefs.setDouble('calibration_accel_x_$motorcycleId', accelCalX); await prefs.setDouble('calibration_accel_y_$motorcycleId', accelCalY); await prefs.setDouble('calibration_accel_z_$motorcycleId', accelCalZ); @@ -66,6 +69,9 @@ class CalibrationService { await prefs.setBool('calibration_is_calibrated_$motorcycleId', false); await prefs.setDouble('calibration_pitch_$motorcycleId', 0.0); await prefs.setDouble('calibration_roll_$motorcycleId', 0.0); + await prefs.setDouble('calibration_gyro_bias_x_$motorcycleId', 0.0); + await prefs.setDouble('calibration_gyro_bias_y_$motorcycleId', 0.0); + await prefs.setDouble('calibration_gyro_bias_z_$motorcycleId', 0.0); await prefs.setDouble('calibration_accel_x_$motorcycleId', 0.0); await prefs.setDouble('calibration_accel_y_$motorcycleId', 0.0); await prefs.setDouble('calibration_accel_z_$motorcycleId', 0.0); @@ -73,19 +79,27 @@ class CalibrationService { await prefs.setBool(_isCalibratedKey, false); await prefs.setDouble(_pitchKey, 0.0); await prefs.setDouble(_rollKey, 0.0); + await prefs.setDouble(_gyroBiasXKey, 0.0); + await prefs.setDouble(_gyroBiasYKey, 0.0); + await prefs.setDouble(_gyroBiasZKey, 0.0); await prefs.setDouble(_accelCalXKey, 0.0); await prefs.setDouble(_accelCalYKey, 0.0); await prefs.setDouble(_accelCalZKey, 0.0); } - Future> loadCalibrationOffsets({int? motorcycleId}) async { + Future> loadCalibrationOffsets({ + int? motorcycleId, + }) async { final prefs = await SharedPreferences.getInstance(); if (motorcycleId != null) { final bikePitch = prefs.getDouble('calibration_pitch_$motorcycleId'); final bikeRoll = prefs.getDouble('calibration_roll_$motorcycleId'); - final bikeGbx = prefs.getDouble('calibration_gyro_bias_x_$motorcycleId') ?? 0.0; - final bikeGby = prefs.getDouble('calibration_gyro_bias_y_$motorcycleId') ?? 0.0; - final bikeGbz = prefs.getDouble('calibration_gyro_bias_z_$motorcycleId') ?? 0.0; + final bikeGbx = + prefs.getDouble('calibration_gyro_bias_x_$motorcycleId') ?? 0.0; + final bikeGby = + prefs.getDouble('calibration_gyro_bias_y_$motorcycleId') ?? 0.0; + final bikeGbz = + prefs.getDouble('calibration_gyro_bias_z_$motorcycleId') ?? 0.0; final isCal = prefs.getBool('calibration_is_calibrated_$motorcycleId'); final ax = prefs.getDouble('calibration_accel_x_$motorcycleId') ?? 0.0; final ay = prefs.getDouble('calibration_accel_y_$motorcycleId') ?? 0.0; diff --git a/lib/services/sensor_fusion_service.dart b/lib/services/sensor_fusion_service.dart index c8246d2..365e9c7 100644 --- a/lib/services/sensor_fusion_service.dart +++ b/lib/services/sensor_fusion_service.dart @@ -20,9 +20,11 @@ final gForceStreamProvider = StreamProvider((ref) { return ref.watch(sensorFusionServiceProvider).gForceStream; }); -final sensorDiagnosticsStreamProvider = StreamProvider((ref) { - return ref.watch(sensorFusionServiceProvider).diagnosticsStream; -}); +final sensorDiagnosticsStreamProvider = StreamProvider( + (ref) { + return ref.watch(sensorFusionServiceProvider).diagnosticsStream; + }, +); enum MountOrientation { portraitUpright, @@ -92,19 +94,19 @@ class SensorFusionDiagnostics { class GForceData { final double longitudinal; // Accel/braking (G's) - final double lateral; // Left/right (G's) - final double accelX; // User accel m/s^2 - final double accelY; // User accel m/s^2 - final double accelZ; // User accel m/s^2 - final double? rawAccelX; // Raw total accel m/s^2 - final double? rawAccelY; // Raw total accel m/s^2 - final double? rawAccelZ; // Raw total accel m/s^2 - final double? rawGyroX; // Raw gyro rad/s - final double? rawGyroY; // Raw gyro rad/s - final double? rawGyroZ; // Raw gyro rad/s - + final double lateral; // Left/right (G's) + final double accelX; // User accel m/s^2 + final double accelY; // User accel m/s^2 + final double accelZ; // User accel m/s^2 + final double? rawAccelX; // Raw total accel m/s^2 + final double? rawAccelY; // Raw total accel m/s^2 + final double? rawAccelZ; // Raw total accel m/s^2 + final double? rawGyroX; // Raw gyro rad/s + final double? rawGyroY; // Raw gyro rad/s + final double? rawGyroZ; // Raw gyro rad/s + GForceData({ - required this.longitudinal, + required this.longitudinal, required this.lateral, required this.accelX, required this.accelY, @@ -124,12 +126,14 @@ class SensorFusionService with WidgetsBindingObserver { final _leanAngleController = StreamController.broadcast(); final _pitchAngleController = StreamController.broadcast(); final _gForceController = StreamController.broadcast(); - final _diagnosticsController = StreamController.broadcast(); + final _diagnosticsController = + StreamController.broadcast(); Stream get leanAngleStream => _leanAngleController.stream; Stream get pitchAngleStream => _pitchAngleController.stream; Stream get gForceStream => _gForceController.stream; - Stream get diagnosticsStream => _diagnosticsController.stream; + Stream get diagnosticsStream => + _diagnosticsController.stream; StreamSubscription? _accelSub; StreamSubscription? _gyroSub; @@ -217,7 +221,8 @@ class SensorFusionService with WidgetsBindingObserver { int _calibrationVersion = 0; Future? _initializationFuture; - Future get initializationDone => _initializationFuture ?? Future.value(); + Future get initializationDone => + _initializationFuture ?? Future.value(); SensorFusionService( this._calibrationService, { @@ -231,7 +236,9 @@ class SensorFusionService with WidgetsBindingObserver { _recomputeBasisVectors(); WidgetsBinding.instance.addObserver(this); _initializationFuture = _loadInitialCalibration(); - if (autoStartSensors || accelerometerStream != null || gyroscopeStream != null) { + if (autoStartSensors || + accelerometerStream != null || + gyroscopeStream != null) { _startSensors( accelStream: accelerometerStream, gyroStream: gyroscopeStream, @@ -243,9 +250,11 @@ class SensorFusionService with WidgetsBindingObserver { void _recomputeBasisVectors() { if (_isCalibrated && (_calGravityX != 0.0 || _calGravityY != 0.0 || _calGravityZ != 0.0)) { - final calNorm = math.sqrt(_calGravityX * _calGravityX + - _calGravityY * _calGravityY + - _calGravityZ * _calGravityZ); + final calNorm = math.sqrt( + _calGravityX * _calGravityX + + _calGravityY * _calGravityY + + _calGravityZ * _calGravityZ, + ); if (calNorm > 3.0) { _upVector = [ _calGravityX / calNorm, @@ -271,7 +280,8 @@ class SensorFusionService with WidgetsBindingObserver { } // Project nominalRight to be strictly orthogonal to _upVector - final dot = nominalRight[0] * _upVector[0] + + final dot = + nominalRight[0] * _upVector[0] + nominalRight[1] * _upVector[1] + nominalRight[2] * _upVector[2]; final rx = nominalRight[0] - dot * _upVector[0]; @@ -381,7 +391,11 @@ class SensorFusionService with WidgetsBindingObserver { } void _checkRemountValidity(double ax, double ay, double az) { - final calNorm = math.sqrt(_calGravityX * _calGravityX + _calGravityY * _calGravityY + _calGravityZ * _calGravityZ); + final calNorm = math.sqrt( + _calGravityX * _calGravityX + + _calGravityY * _calGravityY + + _calGravityZ * _calGravityZ, + ); final curNorm = math.sqrt(ax * ax + ay * ay + az * az); if (calNorm < 3.0 || curNorm < 3.0) return; @@ -391,7 +405,11 @@ class SensorFusionService with WidgetsBindingObserver { final nz = az / curNorm; // Project current vector onto calibrated upright gravity axis - final vUp = (nx * (_calGravityX / calNorm) + ny * (_calGravityY / calNorm) + nz * (_calGravityZ / calNorm)).clamp(-1.0, 1.0); + final vUp = + (nx * (_calGravityX / calNorm) + + ny * (_calGravityY / calNorm) + + nz * (_calGravityZ / calNorm)) + .clamp(-1.0, 1.0); final totalAngleDiffDeg = math.acos(vUp) * (180.0 / math.pi); if (!_isCalibrated) { @@ -420,7 +438,11 @@ class SensorFusionService with WidgetsBindingObserver { } // Project current vector onto 3D Vehicle Coordinate Frame: - final vForward = (nx * _forwardVector[0] + ny * _forwardVector[1] + nz * _forwardVector[2]).clamp(-1.0, 1.0); + final vForward = + (nx * _forwardVector[0] + + ny * _forwardVector[1] + + nz * _forwardVector[2]) + .clamp(-1.0, 1.0); final pitchTiltDeg = (math.asin(vForward)).abs() * (180.0 / math.pi); // Motorcycle Lean & Remount Physics: @@ -431,7 +453,8 @@ class SensorFusionService with WidgetsBindingObserver { // - Placed flat on desk (screen up): totalAngleDiffDeg ~ 90°, pitchTiltDeg ~ 90° // - Placed in pocket/bag: totalAngleDiffDeg >= 80° // - Mount rotated or twisted when stationary: totalAngleDiffDeg >= 80° or pitchTiltDeg >= 65° - final isImmediateRemount = totalAngleDiffDeg >= 80.0 || pitchTiltDeg >= 65.0; + final isImmediateRemount = + totalAngleDiffDeg >= 80.0 || pitchTiltDeg >= 65.0; final isSuspectedRemount = totalAngleDiffDeg > 70.0 || pitchTiltDeg > 55.0; if (isImmediateRemount) { @@ -448,7 +471,9 @@ class SensorFusionService with WidgetsBindingObserver { } void _invalidateRemount(double angleDiffDeg) { - debugPrint('[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating runtime calibration.'); + debugPrint( + '[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating runtime calibration.', + ); _calibrationVersion++; _isCalibrated = false; _remountAnomalyCount = 0; @@ -465,7 +490,11 @@ class SensorFusionService with WidgetsBindingObserver { Stream? userAccelStream, }) { try { - final gStream = gyroStream ?? gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)); + final gStream = + gyroStream ?? + gyroscopeEventStream( + samplingPeriod: const Duration(milliseconds: 20), + ); _gyroSub = gStream.listen( handleGyroscopeEvent, onError: (err) { @@ -474,11 +503,17 @@ class SensorFusionService with WidgetsBindingObserver { cancelOnError: false, ); } catch (err) { - debugPrint('[SensorFusion] Gyroscope initialization skipped (headless test runner): $err'); + debugPrint( + '[SensorFusion] Gyroscope initialization skipped (headless test runner): $err', + ); } try { - final aStream = accelStream ?? accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)); + final aStream = + accelStream ?? + accelerometerEventStream( + samplingPeriod: const Duration(milliseconds: 20), + ); _accelSub = aStream.listen( handleAccelerometerEvent, onError: (err) { @@ -487,11 +522,17 @@ class SensorFusionService with WidgetsBindingObserver { cancelOnError: false, ); } catch (err) { - debugPrint('[SensorFusion] Accelerometer initialization skipped (headless test runner): $err'); + debugPrint( + '[SensorFusion] Accelerometer initialization skipped (headless test runner): $err', + ); } try { - final uStream = userAccelStream ?? userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)); + final uStream = + userAccelStream ?? + userAccelerometerEventStream( + samplingPeriod: const Duration(milliseconds: 20), + ); _userAccelSub = uStream.listen( handleUserAccelerometerEvent, onError: (err) { @@ -500,7 +541,9 @@ class SensorFusionService with WidgetsBindingObserver { cancelOnError: false, ); } catch (err) { - debugPrint('[SensorFusion] UserAccelerometer initialization skipped (headless test runner): $err'); + debugPrint( + '[SensorFusion] UserAccelerometer initialization skipped (headless test runner): $err', + ); } } @@ -529,12 +572,14 @@ class SensorFusionService with WidgetsBindingObserver { // Project device angular rates into 3D Vehicle Coordinate Frame: // Roll rate is rotation around the motorcycle forward axis (_forwardVector) // Pitch rate is rotation around the motorcycle transverse right axis (_rightVector) - final vGyroRollRate = (unbiasedGx * _forwardVector[0] + + final vGyroRollRate = + (unbiasedGx * _forwardVector[0] + unbiasedGy * _forwardVector[1] + unbiasedGz * _forwardVector[2]) * (180.0 / math.pi); - final vGyroPitchRate = (unbiasedGx * _rightVector[0] + + final vGyroPitchRate = + (unbiasedGx * _rightVector[0] + unbiasedGy * _rightVector[1] + unbiasedGz * _rightVector[2]) * (180.0 / math.pi); @@ -565,7 +610,9 @@ class SensorFusionService with WidgetsBindingObserver { _latestRawAccelZ = event.z; // Check remount validity when device is relatively stationary - final rawNorm = math.sqrt(event.x * event.x + event.y * event.y + event.z * event.z); + final rawNorm = math.sqrt( + event.x * event.x + event.y * event.y + event.z * event.z, + ); if ((rawNorm - _gravityConst).abs() < 1.0) { _checkRemountValidity(event.x, event.y, event.z); } else { @@ -584,9 +631,18 @@ class SensorFusionService with WidgetsBindingObserver { // Map raw accelerometer to Vehicle Frame using the 3D basis: // vx = Right, vy = Forward, vz = Up - final vx = event.x * _rightVector[0] + event.y * _rightVector[1] + event.z * _rightVector[2]; - final vy = event.x * _forwardVector[0] + event.y * _forwardVector[1] + event.z * _forwardVector[2]; - final vz = event.x * _upVector[0] + event.y * _upVector[1] + event.z * _upVector[2]; + final vx = + event.x * _rightVector[0] + + event.y * _rightVector[1] + + event.z * _rightVector[2]; + final vy = + event.x * _forwardVector[0] + + event.y * _forwardVector[1] + + event.z * _forwardVector[2]; + final vz = + event.x * _upVector[0] + + event.y * _upVector[1] + + event.z * _upVector[2]; // Low-pass filter to reject engine vibration (e.g. Yamaha R6 high-RPM inline-4 vibration) if (!_hasInitialAccel) { @@ -596,19 +652,31 @@ class SensorFusionService with WidgetsBindingObserver { _hasInitialAccel = true; } else { const accelFilterAlpha = 0.25; - _filteredVx = accelFilterAlpha * vx + (1.0 - accelFilterAlpha) * _filteredVx; - _filteredVy = accelFilterAlpha * vy + (1.0 - accelFilterAlpha) * _filteredVy; - _filteredVz = accelFilterAlpha * vz + (1.0 - accelFilterAlpha) * _filteredVz; + _filteredVx = + accelFilterAlpha * vx + (1.0 - accelFilterAlpha) * _filteredVx; + _filteredVy = + accelFilterAlpha * vy + (1.0 - accelFilterAlpha) * _filteredVy; + _filteredVz = + accelFilterAlpha * vz + (1.0 - accelFilterAlpha) * _filteredVz; } - final norm = math.sqrt(_filteredVx * _filteredVx + _filteredVy * _filteredVy + _filteredVz * _filteredVz); + final norm = math.sqrt( + _filteredVx * _filteredVx + + _filteredVy * _filteredVy + + _filteredVz * _filteredVz, + ); if (norm < 0.1) return; // Compute static gravity-based angles in vehicle frame // In vehicle frame: vx = Right, vy = Forward, vz = Up // When leaning right: gravity reaction tilts to vehicle left (vx < 0, vz > 0) // -atan2(vx, sqrt(vy^2 + vz^2)) gives positive angle for right lean, negative for left lean - final accelRoll = -math.atan2(_filteredVx, math.sqrt(_filteredVy * _filteredVy + _filteredVz * _filteredVz)) * (180.0 / math.pi); + final accelRoll = + -math.atan2( + _filteredVx, + math.sqrt(_filteredVy * _filteredVy + _filteredVz * _filteredVz), + ) * + (180.0 / math.pi); final accelPitch = math.atan2(_filteredVy, _filteredVz) * (180.0 / math.pi); final gMag = norm / _gravityConst; @@ -622,12 +690,20 @@ class SensorFusionService with WidgetsBindingObserver { double dynamicAlpha = 0.88; final rollRateAbs = _latestRollRateDegPerSec.abs(); if (rollRateAbs > 2.0 || dynamicDelta > 0.04) { - final dynamicFactor = math.max(rollRateAbs / 15.0, dynamicDelta / 0.35).clamp(0.0, 1.0); + final dynamicFactor = math + .max(rollRateAbs / 15.0, dynamicDelta / 0.35) + .clamp(0.0, 1.0); dynamicAlpha = (0.95 + 0.045 * dynamicFactor).clamp(0.95, 0.995); } - _currentRoll = (dynamicAlpha * _currentRoll + (1.0 - dynamicAlpha) * accelRoll).clamp(-90.0, 90.0); - _currentPitch = (dynamicAlpha * _currentPitch + (1.0 - dynamicAlpha) * accelPitch).clamp(-90.0, 90.0); + _currentRoll = + (dynamicAlpha * _currentRoll + (1.0 - dynamicAlpha) * accelRoll).clamp( + -90.0, + 90.0, + ); + _currentPitch = + (dynamicAlpha * _currentPitch + (1.0 - dynamicAlpha) * accelPitch) + .clamp(-90.0, 90.0); // Resting convergence: when stationary and upright, gently pull residual drift to 0.0 if (rollRateAbs < 0.5 && dynamicDelta < 0.03 && accelRoll.abs() < 1.0) { @@ -649,10 +725,23 @@ class SensorFusionService with WidgetsBindingObserver { } } - void _deriveFallbackLinearG(double vx, double vy, double vz, double norm, double accelRoll, double accelPitch) { + void _deriveFallbackLinearG( + double vx, + double vy, + double vz, + double norm, + double accelRoll, + double accelPitch, + ) { final gx = -norm * math.sin(accelRoll * math.pi / 180.0); - final gy = -norm * math.sin(accelPitch * math.pi / 180.0) * math.cos(accelRoll * math.pi / 180.0); - final gz = norm * math.cos(accelPitch * math.pi / 180.0) * math.cos(accelRoll * math.pi / 180.0); + final gy = + -norm * + math.sin(accelPitch * math.pi / 180.0) * + math.cos(accelRoll * math.pi / 180.0); + final gz = + norm * + math.cos(accelPitch * math.pi / 180.0) * + math.cos(accelRoll * math.pi / 180.0); final userVx = vx - gx; final userVy = vy - gy; @@ -661,8 +750,10 @@ class SensorFusionService with WidgetsBindingObserver { final pitchRad = _currentPitch * (math.pi / 180.0); final rollRad = _currentRoll * (math.pi / 180.0); - final rawLongitudinal = (userVy * math.cos(pitchRad) - userVz * math.sin(pitchRad)); - final rawLateral = (userVx * math.cos(rollRad) + userVz * math.sin(rollRad)); + final rawLongitudinal = + (userVy * math.cos(pitchRad) - userVz * math.sin(pitchRad)); + final rawLateral = + (userVx * math.cos(rollRad) + userVz * math.sin(rollRad)); double rawLongitudinalG = rawLongitudinal / _gravityConst; double rawLateralG = rawLateral / _gravityConst; @@ -670,38 +761,53 @@ class SensorFusionService with WidgetsBindingObserver { if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0; if (rawLateralG.abs() < 0.02) rawLateralG = 0.0; - _filteredLongitudinalG = _gAlpha * rawLongitudinalG + (1.0 - _gAlpha) * _filteredLongitudinalG; - _filteredLateralG = _gAlpha * rawLateralG + (1.0 - _gAlpha) * _filteredLateralG; + _filteredLongitudinalG = + _gAlpha * rawLongitudinalG + (1.0 - _gAlpha) * _filteredLongitudinalG; + _filteredLateralG = + _gAlpha * rawLateralG + (1.0 - _gAlpha) * _filteredLateralG; if (_filteredLongitudinalG.abs() < 0.008) _filteredLongitudinalG = 0.0; if (_filteredLateralG.abs() < 0.008) _filteredLateralG = 0.0; - _gForceController.add(GForceData( - longitudinal: _filteredLongitudinalG, - lateral: _filteredLateralG, - accelX: userVx, - accelY: userVy, - accelZ: userVz, - rawAccelX: _latestRawAccelX, - rawAccelY: _latestRawAccelY, - rawAccelZ: _latestRawAccelZ, - rawGyroX: _latestRawGyroX, - rawGyroY: _latestRawGyroY, - rawGyroZ: _latestRawGyroZ, - )); + _gForceController.add( + GForceData( + longitudinal: _filteredLongitudinalG, + lateral: _filteredLateralG, + accelX: userVx, + accelY: userVy, + accelZ: userVz, + rawAccelX: _latestRawAccelX, + rawAccelY: _latestRawAccelY, + rawAccelZ: _latestRawAccelZ, + rawGyroX: _latestRawGyroX, + rawGyroY: _latestRawGyroY, + rawGyroZ: _latestRawGyroZ, + ), + ); } void handleUserAccelerometerEvent(UserAccelerometerEvent event) { _hasReceivedUserAccel = true; - final vxUser = event.x * _rightVector[0] + event.y * _rightVector[1] + event.z * _rightVector[2]; - final vyUser = event.x * _forwardVector[0] + event.y * _forwardVector[1] + event.z * _forwardVector[2]; - final vzUser = event.x * _upVector[0] + event.y * _upVector[1] + event.z * _upVector[2]; + final vxUser = + event.x * _rightVector[0] + + event.y * _rightVector[1] + + event.z * _rightVector[2]; + final vyUser = + event.x * _forwardVector[0] + + event.y * _forwardVector[1] + + event.z * _forwardVector[2]; + final vzUser = + event.x * _upVector[0] + + event.y * _upVector[1] + + event.z * _upVector[2]; final pitchRad = _currentPitch * (math.pi / 180.0); final rollRad = _currentRoll * (math.pi / 180.0); - final rawLongitudinal = (vyUser * math.cos(pitchRad) - vzUser * math.sin(pitchRad)); - final rawLateral = (vxUser * math.cos(rollRad) + vzUser * math.sin(rollRad)); + final rawLongitudinal = + (vyUser * math.cos(pitchRad) - vzUser * math.sin(pitchRad)); + final rawLateral = + (vxUser * math.cos(rollRad) + vzUser * math.sin(rollRad)); double rawLongitudinalG = rawLongitudinal / _gravityConst; double rawLateralG = rawLateral / _gravityConst; @@ -709,30 +815,36 @@ class SensorFusionService with WidgetsBindingObserver { if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0; if (rawLateralG.abs() < 0.02) rawLateralG = 0.0; - _filteredLongitudinalG = _gAlpha * rawLongitudinalG + (1.0 - _gAlpha) * _filteredLongitudinalG; - _filteredLateralG = _gAlpha * rawLateralG + (1.0 - _gAlpha) * _filteredLateralG; + _filteredLongitudinalG = + _gAlpha * rawLongitudinalG + (1.0 - _gAlpha) * _filteredLongitudinalG; + _filteredLateralG = + _gAlpha * rawLateralG + (1.0 - _gAlpha) * _filteredLateralG; if (_filteredLongitudinalG.abs() < 0.008) _filteredLongitudinalG = 0.0; if (_filteredLateralG.abs() < 0.008) _filteredLateralG = 0.0; - _gForceController.add(GForceData( - longitudinal: _filteredLongitudinalG, - lateral: _filteredLateralG, - accelX: event.x, - accelY: event.y, - accelZ: event.z, - rawAccelX: _latestRawAccelX, - rawAccelY: _latestRawAccelY, - rawAccelZ: _latestRawAccelZ, - rawGyroX: _latestRawGyroX, - rawGyroY: _latestRawGyroY, - rawGyroZ: _latestRawGyroZ, - )); + _gForceController.add( + GForceData( + longitudinal: _filteredLongitudinalG, + lateral: _filteredLateralG, + accelX: event.x, + accelY: event.y, + accelZ: event.z, + rawAccelX: _latestRawAccelX, + rawAccelY: _latestRawAccelY, + rawAccelZ: _latestRawAccelZ, + rawGyroX: _latestRawGyroX, + rawGyroY: _latestRawGyroY, + rawGyroZ: _latestRawGyroZ, + ), + ); } void _emitDiagnostics(double finalLean) { final now = DateTime.now(); - final direction = finalLean < -0.5 ? 'LEFT' : (finalLean > 0.5 ? 'RIGHT' : 'CENTER'); + final direction = finalLean < -0.5 + ? 'LEFT' + : (finalLean > 0.5 ? 'RIGHT' : 'CENTER'); final displayedAngle = finalLean.abs(); final diag = SensorFusionDiagnostics( @@ -760,20 +872,27 @@ class SensorFusionService with WidgetsBindingObserver { _diagnosticsController.add(diag); // Throttled debugPrint logging (every 1.5 seconds) - if (_lastDiagnosticLogTime == null || now.difference(_lastDiagnosticLogTime!).inMilliseconds >= 1500) { + if (_lastDiagnosticLogTime == null || + now.difference(_lastDiagnosticLogTime!).inMilliseconds >= 1500) { _lastDiagnosticLogTime = now; - debugPrint('[SensorFusion Diag] Accel: (${_latestRawAccelX.toStringAsFixed(2)}, ${_latestRawAccelY.toStringAsFixed(2)}, ${_latestRawAccelZ.toStringAsFixed(2)}) ' - 'Gyro: (${_latestRawGyroX.toStringAsFixed(3)}, ${_latestRawGyroY.toStringAsFixed(3)}, ${_latestRawGyroZ.toStringAsFixed(3)}) ' - 'Roll: ${_currentRoll.toStringAsFixed(1)}° Offset: ${_rollOffset.toStringAsFixed(1)}° Lean: ${finalLean.toStringAsFixed(1)}° ' - 'Display: ${displayedAngle.toStringAsFixed(0)}° $direction Calibrated: $_isCalibrated'); + debugPrint( + '[SensorFusion Diag] Accel: (${_latestRawAccelX.toStringAsFixed(2)}, ${_latestRawAccelY.toStringAsFixed(2)}, ${_latestRawAccelZ.toStringAsFixed(2)}) ' + 'Gyro: (${_latestRawGyroX.toStringAsFixed(3)}, ${_latestRawGyroY.toStringAsFixed(3)}, ${_latestRawGyroZ.toStringAsFixed(3)}) ' + 'Roll: ${_currentRoll.toStringAsFixed(1)}° Offset: ${_rollOffset.toStringAsFixed(1)}° Lean: ${finalLean.toStringAsFixed(1)}° ' + 'Display: ${displayedAngle.toStringAsFixed(0)}° $direction Calibrated: $_isCalibrated', + ); } } Future loadCalibrationForMotorcycle(int? motorcycleId) async { final version = ++_calibrationVersion; try { - final offsets = await _calibrationService.loadCalibrationOffsets(motorcycleId: motorcycleId); - final mountMode = await _calibrationService.loadMountMode(motorcycleId: motorcycleId); + final offsets = await _calibrationService.loadCalibrationOffsets( + motorcycleId: motorcycleId, + ); + final mountMode = await _calibrationService.loadMountMode( + motorcycleId: motorcycleId, + ); if (version != _calibrationVersion) return; _pitchOffset = offsets['pitch'] ?? 0.0; @@ -839,11 +958,16 @@ class SensorFusionService with WidgetsBindingObserver { _recomputeBasisVectors(); - // In newly calibrated frame, current lean and pitch start at zero + // In newly calibrated frame, current lean, pitch, and vehicle-frame filter state start fresh _currentRoll = 0.0; _currentPitch = 0.0; _rollOffset = 0.0; _pitchOffset = 0.0; + _filteredVx = 0.0; + _filteredVy = 0.0; + _filteredVz = 0.0; + _hasInitialAccel = false; + _latestRollRateDegPerSec = 0.0; // Immediately re-emit calibrated 0.0° lean _leanAngleController.add(0.0); @@ -872,9 +996,19 @@ class SensorFusionService with WidgetsBindingObserver { _remountReturnCount = 0; _rollOffset = 0.0; _pitchOffset = 0.0; + _gyroBiasX = 0.0; + _gyroBiasY = 0.0; + _gyroBiasZ = 0.0; _calGravityX = 0.0; _calGravityY = 0.0; _calGravityZ = 0.0; + _currentRoll = 0.0; + _currentPitch = 0.0; + _filteredVx = 0.0; + _filteredVy = 0.0; + _filteredVz = 0.0; + _hasInitialAccel = false; + _latestRollRateDegPerSec = 0.0; _recomputeBasisVectors(); await _calibrationService.clearCalibration(motorcycleId: motorcycleId); _emitDiagnostics(0.0); diff --git a/test/services/sensor_calibration_lifecycle_test.dart b/test/services/sensor_calibration_lifecycle_test.dart new file mode 100644 index 0000000..76758ec --- /dev/null +++ b/test/services/sensor_calibration_lifecycle_test.dart @@ -0,0 +1,279 @@ +import 'package:flutter_test/flutter_test.dart'; +import 'package:motorcycle_dash/services/calibration_service.dart'; +import 'package:motorcycle_dash/services/sensor_fusion_service.dart'; +import 'package:sensors_plus/sensors_plus.dart'; +import 'package:shared_preferences/shared_preferences.dart'; + +void main() { + TestWidgetsFlutterBinding.ensureInitialized(); + + const double g = 9.80665; + + AccelerometerEvent accel(double x, double y, double z) => + AccelerometerEvent(x, y, z, DateTime(2026, 10, 15, 12, 0, 0)); + + GyroscopeEvent gyro(double x, double y, double z) => + GyroscopeEvent(x, y, z, DateTime(2026, 10, 15, 12, 0, 0)); + + setUp(() { + SharedPreferences.setMockInitialValues({}); + }); + + group('Sensor Calibration Lifecycle Regression Tests', () { + test('1. Calibration records gyroscope bias correctly and neutralizes steady drift', () async { + final calibrationService = CalibrationService(); + final fusionService = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService.dispose); + await fusionService.initializationDone; + + // Feed upright portrait gravity and a steady gyro zero-rate offset on Z (-0.12 rad/s) + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.01, -0.02, -0.12)); + + await fusionService.calibrateUpright(); + + // Verify persisted offsets in CalibrationService + final savedOffsets = await calibrationService.loadCalibrationOffsets(); + expect(savedOffsets['isCalibrated'], equals(1.0)); + expect(savedOffsets['gyroBiasX'], closeTo(0.01, 1e-6)); + expect(savedOffsets['gyroBiasY'], closeTo(-0.02, 1e-6)); + expect(savedOffsets['gyroBiasZ'], closeTo(-0.12, 1e-6)); + + // Subsequent identical gyro event should produce 0 deg/s roll rate once dt > 0 + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.01, -0.02, -0.12)); + expect(fusionService.latestRollRateDegPerSec, closeTo(0.0, 1e-4)); + }); + + test('2. Resetting calibration removes stale gyroscope bias from active calculations', () async { + final calibrationService = CalibrationService(); + final fusionService = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService.dispose); + await fusionService.initializationDone; + + // Calibrate while a spurious gyro spike (-0.25 rad/s on Z) is present + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.25)); + await fusionService.calibrateUpright(); + + // Reset calibration + await fusionService.resetCalibration(); + expect(fusionService.isCalibrated, isFalse); + + // Feed a true zero-rate gyro event (0, 0, 0) after a short tick so dt > 0. + // Active calculations must not subtract the stale -0.25 rad/s bias. + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, 0.0)); + + expect( + fusionService.latestRollRateDegPerSec, + closeTo(0.0, 0.01), + reason: 'Stale gyro bias remained active in SensorFusionService after resetCalibration()', + ); + }); + + test('3. Clearing calibration removes persisted bias so it is not restored after service reinitialization', () async { + final calibrationService = CalibrationService(); + final fusionService1 = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + await fusionService1.initializationDone; + + // Save calibration with non-zero gyro biases (both global and motorcycleId: 7) + fusionService1.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService1.handleGyroscopeEvent(gyro(0.05, -0.05, -0.20)); + await fusionService1.calibrateUpright(motorcycleId: 7); + + // Clear calibration via resetCalibration + await fusionService1.resetCalibration(motorcycleId: 7); + fusionService1.dispose(); + + // Verify persisted gyro biases are cleared in SharedPreferences + final globalOffsets = await calibrationService.loadCalibrationOffsets(); + expect(globalOffsets['isCalibrated'], equals(0.0)); + expect( + globalOffsets['gyroBiasX'], + closeTo(0.0, 1e-6), + reason: 'Global gyroBiasX was not cleared by clearCalibration()', + ); + expect( + globalOffsets['gyroBiasY'], + closeTo(0.0, 1e-6), + reason: 'Global gyroBiasY was not cleared by clearCalibration()', + ); + expect( + globalOffsets['gyroBiasZ'], + closeTo(0.0, 1e-6), + reason: 'Global gyroBiasZ was not cleared by clearCalibration()', + ); + + final bikeOffsets = await calibrationService.loadCalibrationOffsets(motorcycleId: 7); + expect( + bikeOffsets['gyroBiasZ'], + closeTo(0.0, 1e-6), + reason: 'Per-motorcycle gyroBiasZ was not cleared by clearCalibration()', + ); + + // Reinitialize a fresh SensorFusionService and verify zero bias in active calculations + final fusionService2 = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService2.dispose); + await fusionService2.initializationDone; + + fusionService2.handleGyroscopeEvent(gyro(0.0, 0.0, 0.0)); + await Future.delayed(const Duration(milliseconds: 5)); + fusionService2.handleGyroscopeEvent(gyro(0.0, 0.0, 0.0)); + + expect( + fusionService2.latestRollRateDegPerSec, + closeTo(0.0, 0.01), + reason: 'Reinitialized SensorFusionService restored stale gyro bias after clearCalibration()', + ); + }); + + test('4. Repeated calibration does not accumulate old offsets', () async { + final calibrationService = CalibrationService(); + final fusionService = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService.dispose); + await fusionService.initializationDone; + + final emittedLeans = []; + final sub = fusionService.leanAngleStream.listen(emittedLeans.add); + addTearDown(sub.cancel); + + // Calibration 1 with bias -0.10 rad/s and slight X tilt + fusionService.handleAccelerometerEvent(accel(-0.8, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.10)); + await fusionService.calibrateUpright(); + + // Calibration 2 with bias -0.04 rad/s and upright gravity + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.04)); + await fusionService.calibrateUpright(); + + // Calibration 3 with bias +0.02 rad/s and upright gravity + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, 0.02)); + await fusionService.calibrateUpright(); + + final offsets = await calibrationService.loadCalibrationOffsets(); + // Should equal the latest bias (+0.02), not the sum (-0.12) + expect(offsets['gyroBiasZ'], closeTo(0.02, 1e-6)); + expect(offsets['roll'], closeTo(0.0, 1e-6)); + expect(offsets['pitch'], closeTo(0.0, 1e-6)); + + // Verify roll rate with latest bias (+0.02) is 0.0 deg/s and lean angle is 0.0 deg + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, 0.02)); + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + await Future.delayed(Duration.zero); + + expect(fusionService.latestRollRateDegPerSec, closeTo(0.0, 1e-4)); + expect(emittedLeans.last, closeTo(0.0, 0.1)); + }); + + test('5. Motorcycle-specific calibration isolation preserves per-bike offsets', () async { + final calibrationService = CalibrationService(); + final fusionService = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService.dispose); + await fusionService.initializationDone; + + // Calibrate Motorcycle 1 with -0.15 rad/s Z gyro bias + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.15)); + await fusionService.calibrateUpright(motorcycleId: 1); + + // Calibrate Motorcycle 2 with +0.08 rad/s Z gyro bias + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, 0.08)); + await fusionService.calibrateUpright(motorcycleId: 2); + + // Load Motorcycle 1 and verify its specific gyro bias is active + await fusionService.loadCalibrationForMotorcycle(1); + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.15)); + expect(fusionService.latestRollRateDegPerSec, closeTo(0.0, 1e-4)); + + // Reset Motorcycle 2 only; Motorcycle 1 persisted calibration should remain intact + await fusionService.resetCalibration(motorcycleId: 2); + + final bike2Offsets = await calibrationService.loadCalibrationOffsets(motorcycleId: 2); + expect(bike2Offsets['isCalibrated'], equals(0.0)); + expect(bike2Offsets['gyroBiasZ'], closeTo(0.0, 1e-6)); + + final bike1Offsets = await calibrationService.loadCalibrationOffsets(motorcycleId: 1); + expect(bike1Offsets['isCalibrated'], equals(1.0)); + expect(bike1Offsets['gyroBiasZ'], closeTo(-0.15, 1e-6)); + + // Reload Motorcycle 1 and verify it still neutralizes its -0.15 rad/s bias + await fusionService.loadCalibrationForMotorcycle(1); + expect(fusionService.isCalibrated, isTrue); + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.15)); + expect(fusionService.latestRollRateDegPerSec, closeTo(0.0, 1e-4)); + }); + + test('6. Automatic remount invalidation suspends calibration without wiping gyro bias on re-lock', () async { + final calibrationService = CalibrationService(); + final fusionService = SensorFusionService( + calibrationService, + autoStartSensors: false, + ); + addTearDown(fusionService.dispose); + await fusionService.initializationDone; + + // Calibrate upright portrait with -0.10 rad/s Z gyro bias + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.10)); + await fusionService.calibrateUpright(); + expect(fusionService.isCalibrated, isTrue); + + // Simulate placing phone flat on desk (90-degree pitch shift from portrait upright) + fusionService.handleAccelerometerEvent(accel(0.0, 0.0, g)); + expect( + fusionService.isCalibrated, + isFalse, + reason: 'Immediate remount (90 deg shift) should invalidate runtime calibration', + ); + + // Persisted calibration in SharedPreferences must still exist (not wiped by temporary remount) + final persisted = await calibrationService.loadCalibrationOffsets(); + expect(persisted['isCalibrated'], equals(1.0)); + expect(persisted['gyroBiasZ'], closeTo(-0.10, 1e-6)); + + // Return phone to upright mount for 15 samples to trigger automatic re-lock + for (int i = 0; i < 15; i++) { + fusionService.handleAccelerometerEvent(accel(0.0, g, 0.0)); + } + expect( + fusionService.isCalibrated, + isTrue, + reason: 'Returning device to calibrated mount orientation should auto-relock calibration', + ); + + // Verify gyro bias (-0.10 rad/s) is still compensated after auto-relock + await Future.delayed(const Duration(milliseconds: 5)); + fusionService.handleGyroscopeEvent(gyro(0.0, 0.0, -0.10)); + expect( + fusionService.latestRollRateDegPerSec, + closeTo(0.0, 1e-4), + reason: 'Auto-relock after remount should retain calibrated gyro bias compensation', + ); + }); + }); +}