817 lines
29 KiB
Dart
817 lines
29 KiB
Dart
import 'dart:async';
|
|
import 'dart:math' as math;
|
|
import 'package:flutter/widgets.dart';
|
|
import 'package:flutter_riverpod/flutter_riverpod.dart';
|
|
import 'package:sensors_plus/sensors_plus.dart';
|
|
import 'calibration_service.dart';
|
|
|
|
final sensorFusionServiceProvider = Provider<SensorFusionService>((ref) {
|
|
final calibrationService = ref.watch(calibrationServiceProvider);
|
|
final service = SensorFusionService(calibrationService);
|
|
ref.onDispose(() => service.dispose());
|
|
return service;
|
|
});
|
|
|
|
final leanAngleStreamProvider = StreamProvider<double>((ref) {
|
|
return ref.watch(sensorFusionServiceProvider).leanAngleStream;
|
|
});
|
|
|
|
final gForceStreamProvider = StreamProvider<GForceData>((ref) {
|
|
return ref.watch(sensorFusionServiceProvider).gForceStream;
|
|
});
|
|
|
|
final sensorDiagnosticsStreamProvider = StreamProvider<SensorFusionDiagnostics>((ref) {
|
|
return ref.watch(sensorFusionServiceProvider).diagnosticsStream;
|
|
});
|
|
|
|
enum MountOrientation {
|
|
portraitUpright,
|
|
landscapeLeftUpright,
|
|
landscapeRightUpright,
|
|
flat,
|
|
}
|
|
|
|
class SensorFusionDiagnostics {
|
|
final double rawAccelX;
|
|
final double rawAccelY;
|
|
final double rawAccelZ;
|
|
final double rawGyroX;
|
|
final double rawGyroY;
|
|
final double rawGyroZ;
|
|
final double transformedRoll;
|
|
final double transformedPitch;
|
|
final double rollOffset;
|
|
final double pitchOffset;
|
|
final double finalLeanAngle;
|
|
final double dashboardDisplayedAngle;
|
|
final String dashboardDirection;
|
|
final bool isCalibrated;
|
|
final String mountOrientation;
|
|
|
|
const SensorFusionDiagnostics({
|
|
required this.rawAccelX,
|
|
required this.rawAccelY,
|
|
required this.rawAccelZ,
|
|
required this.rawGyroX,
|
|
required this.rawGyroY,
|
|
required this.rawGyroZ,
|
|
required this.transformedRoll,
|
|
required this.transformedPitch,
|
|
required this.rollOffset,
|
|
required this.pitchOffset,
|
|
required this.finalLeanAngle,
|
|
required this.dashboardDisplayedAngle,
|
|
required this.dashboardDirection,
|
|
required this.isCalibrated,
|
|
required this.mountOrientation,
|
|
});
|
|
|
|
Map<String, dynamic> toMap() => {
|
|
'rawAccel': [rawAccelX, rawAccelY, rawAccelZ],
|
|
'rawGyro': [rawGyroX, rawGyroY, rawGyroZ],
|
|
'transformedRoll': transformedRoll,
|
|
'transformedPitch': transformedPitch,
|
|
'rollOffset': rollOffset,
|
|
'pitchOffset': pitchOffset,
|
|
'finalLeanAngle': finalLeanAngle,
|
|
'dashboardDisplayedAngle': dashboardDisplayedAngle,
|
|
'dashboardDirection': dashboardDirection,
|
|
'isCalibrated': isCalibrated,
|
|
'mountOrientation': mountOrientation,
|
|
};
|
|
}
|
|
|
|
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
|
|
|
|
GForceData({
|
|
required this.longitudinal,
|
|
required this.lateral,
|
|
required this.accelX,
|
|
required this.accelY,
|
|
required this.accelZ,
|
|
this.rawAccelX,
|
|
this.rawAccelY,
|
|
this.rawAccelZ,
|
|
this.rawGyroX,
|
|
this.rawGyroY,
|
|
this.rawGyroZ,
|
|
});
|
|
}
|
|
|
|
class SensorFusionService with WidgetsBindingObserver {
|
|
final CalibrationService _calibrationService;
|
|
|
|
final _leanAngleController = StreamController<double>.broadcast();
|
|
final _pitchAngleController = StreamController<double>.broadcast();
|
|
final _gForceController = StreamController<GForceData>.broadcast();
|
|
final _diagnosticsController = StreamController<SensorFusionDiagnostics>.broadcast();
|
|
|
|
Stream<double> get leanAngleStream => _leanAngleController.stream;
|
|
Stream<double> get pitchAngleStream => _pitchAngleController.stream;
|
|
Stream<GForceData> get gForceStream => _gForceController.stream;
|
|
Stream<SensorFusionDiagnostics> get diagnosticsStream => _diagnosticsController.stream;
|
|
|
|
StreamSubscription? _accelSub;
|
|
StreamSubscription? _gyroSub;
|
|
StreamSubscription? _userAccelSub;
|
|
|
|
// Calibration offsets & gyro zero-rate bias
|
|
double _rollOffset = 0.0;
|
|
double _pitchOffset = 0.0;
|
|
double _gyroBiasX = 0.0;
|
|
double _gyroBiasY = 0.0;
|
|
double _gyroBiasZ = 0.0;
|
|
bool _isCalibrated = false;
|
|
bool get isCalibrated => _isCalibrated;
|
|
int _remountAnomalyCount = 0;
|
|
|
|
// Stored baseline gravity vector at calibration for remount detection
|
|
double _calGravityX = 0.0;
|
|
double _calGravityY = 0.0;
|
|
double _calGravityZ = 0.0;
|
|
|
|
// Raw sensor caches (unfiltered for forensics and diagnostics)
|
|
double _latestRawAccelX = 0.0;
|
|
double _latestRawAccelY = 0.0;
|
|
double _latestRawAccelZ = 0.0;
|
|
double _latestRawGyroX = 0.0;
|
|
double _latestRawGyroY = 0.0;
|
|
double _latestRawGyroZ = 0.0;
|
|
double _latestRollRateDegPerSec = 0.0;
|
|
|
|
double get latestRawAccelX => _latestRawAccelX;
|
|
double get latestRawAccelY => _latestRawAccelY;
|
|
double get latestRawAccelZ => _latestRawAccelZ;
|
|
double get latestRawGyroX => _latestRawGyroX;
|
|
double get latestRawGyroY => _latestRawGyroY;
|
|
double get latestRawGyroZ => _latestRawGyroZ;
|
|
double get latestRollRateDegPerSec => _latestRollRateDegPerSec;
|
|
|
|
// Sensor fusion state
|
|
double _currentRoll = 0.0;
|
|
double _currentPitch = 0.0;
|
|
DateTime? _lastGyroTime;
|
|
|
|
// 3D Orthonormal Basis for Vehicle Frame:
|
|
// _rightVector: points to motorcycle right (+X_v)
|
|
// _forwardVector: points to motorcycle forward (+Y_v)
|
|
// _upVector: points to motorcycle up (+Z_v)
|
|
List<double> _rightVector = [1.0, 0.0, 0.0];
|
|
List<double> _forwardVector = [0.0, 0.0, -1.0];
|
|
List<double> _upVector = [0.0, 1.0, 0.0];
|
|
|
|
// Low-pass filtered accelerometer values in vehicle frame to reject engine vibration
|
|
double _filteredVx = 0.0;
|
|
double _filteredVy = 0.0;
|
|
double _filteredVz = 0.0;
|
|
bool _hasInitialAccel = false;
|
|
|
|
bool _isLandscape = false;
|
|
bool _isLandscapeLeft = true;
|
|
MountOrientation _mountOrientation = MountOrientation.portraitUpright;
|
|
MountOrientation get mountOrientation => _mountOrientation;
|
|
|
|
static const double _gravityConst = 9.80665;
|
|
|
|
// Low-pass filter for G-Force
|
|
double _filteredLongitudinalG = 0.0;
|
|
double _filteredLateralG = 0.0;
|
|
static const double _gAlpha = 0.40;
|
|
bool _hasReceivedUserAccel = false;
|
|
|
|
// Diagnostic throttling
|
|
DateTime? _lastDiagnosticLogTime;
|
|
SensorFusionDiagnostics? _latestDiagnostics;
|
|
SensorFusionDiagnostics? get latestDiagnostics => _latestDiagnostics;
|
|
|
|
int _calibrationVersion = 0;
|
|
Future<void>? _initializationFuture;
|
|
Future<void> get initializationDone => _initializationFuture ?? Future.value();
|
|
|
|
SensorFusionService(
|
|
this._calibrationService, {
|
|
Stream<AccelerometerEvent>? accelerometerStream,
|
|
Stream<GyroscopeEvent>? gyroscopeStream,
|
|
Stream<UserAccelerometerEvent>? userAccelerometerStream,
|
|
MountOrientation defaultOrientation = MountOrientation.portraitUpright,
|
|
bool autoStartSensors = true,
|
|
}) {
|
|
_mountOrientation = defaultOrientation;
|
|
_recomputeBasisVectors();
|
|
WidgetsBinding.instance.addObserver(this);
|
|
_initializationFuture = _loadInitialCalibration();
|
|
if (autoStartSensors || accelerometerStream != null || gyroscopeStream != null) {
|
|
_startSensors(
|
|
accelStream: accelerometerStream,
|
|
gyroStream: gyroscopeStream,
|
|
userAccelStream: userAccelerometerStream,
|
|
);
|
|
}
|
|
}
|
|
|
|
void _recomputeBasisVectors() {
|
|
if (_isCalibrated &&
|
|
(_calGravityX != 0.0 || _calGravityY != 0.0 || _calGravityZ != 0.0)) {
|
|
final calNorm = math.sqrt(_calGravityX * _calGravityX +
|
|
_calGravityY * _calGravityY +
|
|
_calGravityZ * _calGravityZ);
|
|
if (calNorm > 3.0) {
|
|
_upVector = [
|
|
_calGravityX / calNorm,
|
|
_calGravityY / calNorm,
|
|
_calGravityZ / calNorm,
|
|
];
|
|
|
|
// Determine nominal right vector based on mount orientation
|
|
List<double> nominalRight;
|
|
switch (_mountOrientation) {
|
|
case MountOrientation.portraitUpright:
|
|
nominalRight = [1.0, 0.0, 0.0];
|
|
break;
|
|
case MountOrientation.landscapeLeftUpright:
|
|
nominalRight = [0.0, -1.0, 0.0];
|
|
break;
|
|
case MountOrientation.landscapeRightUpright:
|
|
nominalRight = [0.0, 1.0, 0.0];
|
|
break;
|
|
case MountOrientation.flat:
|
|
nominalRight = [1.0, 0.0, 0.0];
|
|
break;
|
|
}
|
|
|
|
// Project nominalRight to be strictly orthogonal to _upVector
|
|
final dot = nominalRight[0] * _upVector[0] +
|
|
nominalRight[1] * _upVector[1] +
|
|
nominalRight[2] * _upVector[2];
|
|
final rx = nominalRight[0] - dot * _upVector[0];
|
|
final ry = nominalRight[1] - dot * _upVector[1];
|
|
final rz = nominalRight[2] - dot * _upVector[2];
|
|
final rNorm = math.sqrt(rx * rx + ry * ry + rz * rz);
|
|
if (rNorm > 0.1) {
|
|
_rightVector = [rx / rNorm, ry / rNorm, rz / rNorm];
|
|
} else {
|
|
_rightVector = nominalRight;
|
|
}
|
|
|
|
// Forward vector = Up x Right
|
|
_forwardVector = [
|
|
_upVector[1] * _rightVector[2] - _upVector[2] * _rightVector[1],
|
|
_upVector[2] * _rightVector[0] - _upVector[0] * _rightVector[2],
|
|
_upVector[0] * _rightVector[1] - _upVector[1] * _rightVector[0],
|
|
];
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Nominal orthogonal basis for uncalibrated state
|
|
switch (_mountOrientation) {
|
|
case MountOrientation.portraitUpright:
|
|
_rightVector = [1.0, 0.0, 0.0];
|
|
_forwardVector = [0.0, 0.0, -1.0];
|
|
_upVector = [0.0, 1.0, 0.0];
|
|
break;
|
|
case MountOrientation.landscapeLeftUpright:
|
|
_rightVector = [0.0, -1.0, 0.0];
|
|
_forwardVector = [0.0, 0.0, -1.0];
|
|
_upVector = [1.0, 0.0, 0.0];
|
|
break;
|
|
case MountOrientation.landscapeRightUpright:
|
|
_rightVector = [0.0, 1.0, 0.0];
|
|
_forwardVector = [0.0, 0.0, -1.0];
|
|
_upVector = [-1.0, 0.0, 0.0];
|
|
break;
|
|
case MountOrientation.flat:
|
|
_rightVector = [1.0, 0.0, 0.0];
|
|
_forwardVector = [0.0, 1.0, 0.0];
|
|
_upVector = [0.0, 0.0, 1.0];
|
|
break;
|
|
}
|
|
}
|
|
|
|
@override
|
|
void didChangeMetrics() {
|
|
_updateDisplayOrientation();
|
|
}
|
|
|
|
void _updateDisplayOrientation() {
|
|
try {
|
|
final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
|
|
if (view != null && view.physicalSize.height > 0) {
|
|
final size = view.physicalSize;
|
|
// Only adapt automatically if display dimensions are realistic mobile dimensions
|
|
if (size.width > size.height && size.width < 1500) {
|
|
_isLandscape = true;
|
|
_updateMountOrientation();
|
|
_recomputeBasisVectors();
|
|
} else if (size.height >= size.width) {
|
|
_isLandscape = false;
|
|
_updateMountOrientation();
|
|
_recomputeBasisVectors();
|
|
}
|
|
}
|
|
} catch (_) {}
|
|
}
|
|
|
|
void setOrientation(bool isLandscape, {bool isLandscapeLeft = true}) {
|
|
_isLandscape = isLandscape;
|
|
_isLandscapeLeft = isLandscapeLeft;
|
|
_updateMountOrientation();
|
|
_recomputeBasisVectors();
|
|
}
|
|
|
|
void _updateMountOrientation() {
|
|
if (!_isLandscape) {
|
|
_mountOrientation = MountOrientation.portraitUpright;
|
|
} else if (_isLandscapeLeft) {
|
|
_mountOrientation = MountOrientation.landscapeLeftUpright;
|
|
} else {
|
|
_mountOrientation = MountOrientation.landscapeRightUpright;
|
|
}
|
|
}
|
|
|
|
Future<void> _loadInitialCalibration() async {
|
|
await loadCalibrationForMotorcycle(null);
|
|
}
|
|
|
|
void _checkRemountValidity(double ax, double ay, double az) {
|
|
if (!_isCalibrated) return;
|
|
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;
|
|
|
|
// Normalize current acceleration vector
|
|
final nx = ax / curNorm;
|
|
final ny = ay / curNorm;
|
|
final nz = az / curNorm;
|
|
|
|
// Project current vector onto 3D Vehicle Coordinate Frame:
|
|
// vUp: component along calibrated upright axis
|
|
// vForward: component along motorcycle longitudinal/forward axis
|
|
final vUp = (nx * _upVector[0] + ny * _upVector[1] + nz * _upVector[2]).clamp(-1.0, 1.0);
|
|
final vForward = (nx * _forwardVector[0] + ny * _forwardVector[1] + nz * _forwardVector[2]).clamp(-1.0, 1.0);
|
|
|
|
// Total 3D angular deviation from calibrated upright gravity
|
|
final totalAngleDiffDeg = math.acos(vUp) * (180.0 / math.pi);
|
|
|
|
// Pitch tilt (tilt out of motorcycle roll plane)
|
|
final pitchTiltDeg = (math.asin(vForward)).abs() * (180.0 / math.pi);
|
|
|
|
// Motorcycle Lean & Remount Physics:
|
|
// 1. Roll-axis rotation (up to 65°) is normal motorcycle riding, cornering, and kickstand parking (20°-35°).
|
|
// Deviations within the roll plane with pitchTiltDeg <= 35° and totalAngleDiffDeg <= 68°
|
|
// must NEVER invalidate calibration!
|
|
// 2. True remount / phone removal:
|
|
// - Placed flat on desk (screen up): totalAngleDiffDeg ~ 90°, pitchTiltDeg ~ 90°
|
|
// - Placed in pocket/bag: totalAngleDiffDeg >= 90°
|
|
// - Mount rotated or twisted: totalAngleDiffDeg > 68° or pitchTiltDeg > 38°
|
|
final isImmediateRemount = totalAngleDiffDeg >= 75.0 || pitchTiltDeg >= 42.0;
|
|
final isSuspectedRemount = totalAngleDiffDeg > 68.0 || pitchTiltDeg > 35.0;
|
|
|
|
if (isImmediateRemount) {
|
|
_invalidateRemount(totalAngleDiffDeg);
|
|
} else if (isSuspectedRemount) {
|
|
_remountAnomalyCount++;
|
|
// Require 5 consecutive stationary samples (100ms) to filter out transient bumps or noise
|
|
if (_remountAnomalyCount >= 5) {
|
|
_invalidateRemount(totalAngleDiffDeg);
|
|
}
|
|
} else {
|
|
_remountAnomalyCount = 0;
|
|
}
|
|
}
|
|
|
|
void _invalidateRemount(double angleDiffDeg) {
|
|
debugPrint('[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating calibration.');
|
|
_calibrationVersion++;
|
|
_isCalibrated = false;
|
|
_remountAnomalyCount = 0;
|
|
_rollOffset = 0.0;
|
|
_pitchOffset = 0.0;
|
|
_calGravityX = 0.0;
|
|
_calGravityY = 0.0;
|
|
_calGravityZ = 0.0;
|
|
_recomputeBasisVectors();
|
|
_calibrationService.clearCalibration();
|
|
_emitDiagnostics(0.0);
|
|
}
|
|
|
|
void _startSensors({
|
|
Stream<AccelerometerEvent>? accelStream,
|
|
Stream<GyroscopeEvent>? gyroStream,
|
|
Stream<UserAccelerometerEvent>? userAccelStream,
|
|
}) {
|
|
try {
|
|
final gStream = gyroStream ?? gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20));
|
|
_gyroSub = gStream.listen(
|
|
handleGyroscopeEvent,
|
|
onError: (err) {
|
|
debugPrint('[SensorFusion] Gyroscope stream error: $err');
|
|
},
|
|
cancelOnError: false,
|
|
);
|
|
} catch (err) {
|
|
debugPrint('[SensorFusion] Gyroscope initialization skipped (headless test runner): $err');
|
|
}
|
|
|
|
try {
|
|
final aStream = accelStream ?? accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20));
|
|
_accelSub = aStream.listen(
|
|
handleAccelerometerEvent,
|
|
onError: (err) {
|
|
debugPrint('[SensorFusion] Accelerometer stream error: $err');
|
|
},
|
|
cancelOnError: false,
|
|
);
|
|
} catch (err) {
|
|
debugPrint('[SensorFusion] Accelerometer initialization skipped (headless test runner): $err');
|
|
}
|
|
|
|
try {
|
|
final uStream = userAccelStream ?? userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20));
|
|
_userAccelSub = uStream.listen(
|
|
handleUserAccelerometerEvent,
|
|
onError: (err) {
|
|
debugPrint('[SensorFusion] UserAccelerometer stream error: $err');
|
|
},
|
|
cancelOnError: false,
|
|
);
|
|
} catch (err) {
|
|
debugPrint('[SensorFusion] UserAccelerometer initialization skipped (headless test runner): $err');
|
|
}
|
|
}
|
|
|
|
void handleGyroscopeEvent(GyroscopeEvent event) {
|
|
final now = DateTime.now();
|
|
_latestRawGyroX = event.x;
|
|
_latestRawGyroY = event.y;
|
|
_latestRawGyroZ = event.z;
|
|
|
|
if (_lastGyroTime == null) {
|
|
_lastGyroTime = now;
|
|
return;
|
|
}
|
|
|
|
double dt = now.difference(_lastGyroTime!).inMicroseconds / 1000000.0;
|
|
_lastGyroTime = now;
|
|
|
|
if (dt > 0.1) dt = 0.1;
|
|
if (dt <= 0.0) return;
|
|
|
|
// Compensate for static zero-rate gyro bias
|
|
final unbiasedGx = event.x - _gyroBiasX;
|
|
final unbiasedGy = event.y - _gyroBiasY;
|
|
final unbiasedGz = event.z - _gyroBiasZ;
|
|
|
|
// 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] +
|
|
unbiasedGy * _forwardVector[1] +
|
|
unbiasedGz * _forwardVector[2]) *
|
|
(180.0 / math.pi);
|
|
|
|
final vGyroPitchRate = (unbiasedGx * _rightVector[0] +
|
|
unbiasedGy * _rightVector[1] +
|
|
unbiasedGz * _rightVector[2]) *
|
|
(180.0 / math.pi);
|
|
|
|
_latestRollRateDegPerSec = vGyroRollRate;
|
|
|
|
// Numerical integration of gyro rate
|
|
_currentPitch = (_currentPitch + vGyroPitchRate * dt).clamp(-90.0, 90.0);
|
|
_currentRoll = (_currentRoll + vGyroRollRate * dt).clamp(-90.0, 90.0);
|
|
}
|
|
|
|
void handleAccelerometerEvent(AccelerometerEvent event) {
|
|
_latestRawAccelX = event.x;
|
|
_latestRawAccelY = event.y;
|
|
_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);
|
|
if ((rawNorm - _gravityConst).abs() < 1.0) {
|
|
_checkRemountValidity(event.x, event.y, event.z);
|
|
} else {
|
|
_remountAnomalyCount = 0;
|
|
}
|
|
|
|
// Auto-detect landscape subtype when in landscape
|
|
if (_isLandscape && event.x.abs() > 3.0) {
|
|
final wasLeft = _isLandscapeLeft;
|
|
_isLandscapeLeft = event.x > 0;
|
|
if (wasLeft != _isLandscapeLeft) {
|
|
_updateMountOrientation();
|
|
_recomputeBasisVectors();
|
|
}
|
|
}
|
|
|
|
// 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];
|
|
|
|
// Low-pass filter to reject engine vibration (e.g. Yamaha R6 high-RPM inline-4 vibration)
|
|
if (!_hasInitialAccel) {
|
|
_filteredVx = vx;
|
|
_filteredVy = vy;
|
|
_filteredVz = vz;
|
|
_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;
|
|
}
|
|
|
|
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 accelPitch = math.atan2(_filteredVy, _filteredVz) * (180.0 / math.pi);
|
|
|
|
final gMag = norm / _gravityConst;
|
|
final dynamicDelta = (gMag - 1.0).abs();
|
|
|
|
// Complementary filter:
|
|
// - Static / resting: alpha = 0.88 (converges quickly to gravity reference)
|
|
// - Dynamic cruising: alpha = 0.95 (smooth vibration rejection)
|
|
// - Dynamic cornering: alpha transitions up to 0.995 max (relies on gyro rate during turns)
|
|
// Alpha is strictly capped at 0.995, preventing runaway integration or pinning.
|
|
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);
|
|
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);
|
|
|
|
// Resting convergence: when stationary and upright, gently pull residual drift to 0.0
|
|
if (rollRateAbs < 0.5 && dynamicDelta < 0.03 && accelRoll.abs() < 1.0) {
|
|
_currentRoll = _currentRoll * 0.92;
|
|
}
|
|
|
|
final calibratedLean = (_currentRoll - _rollOffset).clamp(-65.0, 65.0);
|
|
final calibratedPitch = (_currentPitch - _pitchOffset).clamp(-45.0, 45.0);
|
|
|
|
_leanAngleController.add(calibratedLean);
|
|
_pitchAngleController.add(calibratedPitch);
|
|
|
|
// Diagnostics
|
|
_emitDiagnostics(calibratedLean);
|
|
|
|
// Fallback linear acceleration when userAccelerometer is unavailable
|
|
if (!_hasReceivedUserAccel) {
|
|
_deriveFallbackLinearG(vx, vy, vz, norm, accelRoll, 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 userVx = vx - gx;
|
|
final userVy = vy - gy;
|
|
final userVz = vz - gz;
|
|
|
|
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));
|
|
|
|
double rawLongitudinalG = rawLongitudinal / _gravityConst;
|
|
double rawLateralG = rawLateral / _gravityConst;
|
|
|
|
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;
|
|
|
|
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,
|
|
));
|
|
}
|
|
|
|
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 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));
|
|
|
|
double rawLongitudinalG = rawLongitudinal / _gravityConst;
|
|
double rawLateralG = rawLateral / _gravityConst;
|
|
|
|
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;
|
|
|
|
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,
|
|
));
|
|
}
|
|
|
|
void _emitDiagnostics(double finalLean) {
|
|
final now = DateTime.now();
|
|
final direction = finalLean < -0.5 ? 'LEFT' : (finalLean > 0.5 ? 'RIGHT' : 'CENTER');
|
|
final displayedAngle = finalLean.abs();
|
|
|
|
final diag = SensorFusionDiagnostics(
|
|
rawAccelX: _latestRawAccelX,
|
|
rawAccelY: _latestRawAccelY,
|
|
rawAccelZ: _latestRawAccelZ,
|
|
rawGyroX: _latestRawGyroX,
|
|
rawGyroY: _latestRawGyroY,
|
|
rawGyroZ: _latestRawGyroZ,
|
|
transformedRoll: _currentRoll,
|
|
transformedPitch: _currentPitch,
|
|
rollOffset: _rollOffset,
|
|
pitchOffset: _pitchOffset,
|
|
finalLeanAngle: finalLean,
|
|
dashboardDisplayedAngle: displayedAngle,
|
|
dashboardDirection: direction,
|
|
isCalibrated: _isCalibrated,
|
|
mountOrientation: _mountOrientation.name,
|
|
);
|
|
|
|
_latestDiagnostics = diag;
|
|
_diagnosticsController.add(diag);
|
|
|
|
// Throttled debugPrint logging (every 1.5 seconds)
|
|
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');
|
|
}
|
|
}
|
|
|
|
Future<void> loadCalibrationForMotorcycle(int? motorcycleId) async {
|
|
final version = ++_calibrationVersion;
|
|
try {
|
|
final offsets = await _calibrationService.loadCalibrationOffsets(motorcycleId: motorcycleId);
|
|
final mountMode = await _calibrationService.loadMountMode(motorcycleId: motorcycleId);
|
|
if (version != _calibrationVersion) return;
|
|
|
|
_pitchOffset = offsets['pitch'] ?? 0.0;
|
|
_rollOffset = offsets['roll'] ?? 0.0;
|
|
_gyroBiasX = offsets['gyroBiasX'] ?? 0.0;
|
|
_gyroBiasY = offsets['gyroBiasY'] ?? 0.0;
|
|
_gyroBiasZ = offsets['gyroBiasZ'] ?? 0.0;
|
|
final isCal = (offsets['isCalibrated'] ?? 0.0) > 0.5;
|
|
_calGravityX = offsets['accelCalX'] ?? 0.0;
|
|
_calGravityY = offsets['accelCalY'] ?? 0.0;
|
|
_calGravityZ = offsets['accelCalZ'] ?? 0.0;
|
|
|
|
if (mountMode != null) {
|
|
for (final m in MountOrientation.values) {
|
|
if (m.name == mountMode) {
|
|
_mountOrientation = m;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
_isCalibrated = isCal;
|
|
_remountAnomalyCount = 0;
|
|
_recomputeBasisVectors();
|
|
} catch (_) {
|
|
if (version != _calibrationVersion) return;
|
|
_isCalibrated = false;
|
|
_recomputeBasisVectors();
|
|
}
|
|
}
|
|
|
|
Future<void> calibrateUpright({int? motorcycleId}) async {
|
|
++_calibrationVersion;
|
|
_gyroBiasX = _latestRawGyroX;
|
|
_gyroBiasY = _latestRawGyroY;
|
|
_gyroBiasZ = _latestRawGyroZ;
|
|
_calGravityX = _latestRawAccelX;
|
|
_calGravityY = _latestRawAccelY;
|
|
_calGravityZ = _latestRawAccelZ;
|
|
_isCalibrated = true;
|
|
_remountAnomalyCount = 0;
|
|
|
|
// Auto-detect mount orientation from gravity
|
|
final ax = _calGravityX.abs();
|
|
final ay = _calGravityY.abs();
|
|
final az = _calGravityZ.abs();
|
|
if (ax > ay && ax > az) {
|
|
_mountOrientation = _calGravityX > 0
|
|
? MountOrientation.landscapeLeftUpright
|
|
: MountOrientation.landscapeRightUpright;
|
|
} else if (_calGravityZ > 7.5 && ay < 4.0) {
|
|
_mountOrientation = MountOrientation.flat;
|
|
} else {
|
|
_mountOrientation = MountOrientation.portraitUpright;
|
|
}
|
|
|
|
_recomputeBasisVectors();
|
|
|
|
// In newly calibrated frame, current lean and pitch start at zero
|
|
_currentRoll = 0.0;
|
|
_currentPitch = 0.0;
|
|
_rollOffset = 0.0;
|
|
_pitchOffset = 0.0;
|
|
|
|
// Immediately re-emit calibrated 0.0° lean
|
|
_leanAngleController.add(0.0);
|
|
_pitchAngleController.add(0.0);
|
|
_emitDiagnostics(0.0);
|
|
|
|
await _calibrationService.saveCalibrationOffsets(
|
|
0.0,
|
|
0.0,
|
|
gyroBiasX: _gyroBiasX,
|
|
gyroBiasY: _gyroBiasY,
|
|
gyroBiasZ: _gyroBiasZ,
|
|
accelCalX: _calGravityX,
|
|
accelCalY: _calGravityY,
|
|
accelCalZ: _calGravityZ,
|
|
mountMode: _mountOrientation.name,
|
|
isCalibrated: true,
|
|
motorcycleId: motorcycleId,
|
|
);
|
|
}
|
|
|
|
Future<void> resetCalibration({int? motorcycleId}) async {
|
|
_calibrationVersion++;
|
|
_isCalibrated = false;
|
|
_remountAnomalyCount = 0;
|
|
_rollOffset = 0.0;
|
|
_pitchOffset = 0.0;
|
|
_calGravityX = 0.0;
|
|
_calGravityY = 0.0;
|
|
_calGravityZ = 0.0;
|
|
_recomputeBasisVectors();
|
|
await _calibrationService.clearCalibration(motorcycleId: motorcycleId);
|
|
_emitDiagnostics(0.0);
|
|
}
|
|
|
|
void dispose() {
|
|
WidgetsBinding.instance.removeObserver(this);
|
|
_accelSub?.cancel();
|
|
_gyroSub?.cancel();
|
|
_userAccelSub?.cancel();
|
|
_leanAngleController.close();
|
|
_pitchAngleController.close();
|
|
_gForceController.close();
|
|
_diagnosticsController.close();
|
|
}
|
|
}
|