1028 lines
33 KiB
Dart
1028 lines
33 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;
|
|
final double eventRateHz;
|
|
final double longitudinalG;
|
|
final double lateralG;
|
|
|
|
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,
|
|
this.eventRateHz = 50.0,
|
|
this.longitudinalG = 0.0,
|
|
this.lateralG = 0.0,
|
|
});
|
|
|
|
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,
|
|
'eventRateHz': eventRateHz,
|
|
'longitudinalG': longitudinalG,
|
|
'lateralG': lateralG,
|
|
};
|
|
}
|
|
|
|
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;
|
|
int _remountReturnCount = 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 _eventRateHz = 50.0;
|
|
int _sensorEventCounter = 0;
|
|
DateTime? _lastRateWindow;
|
|
|
|
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;
|
|
double get eventRateHz => _eventRateHz;
|
|
|
|
// 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;
|
|
|
|
double _speedMph = 0.0;
|
|
double get speedMph => _speedMph;
|
|
void updateVehicleSpeed(double speedMph) {
|
|
_speedMph = speedMph;
|
|
}
|
|
|
|
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();
|
|
}
|
|
|
|
@override
|
|
void didChangeAppLifecycleState(AppLifecycleState state) {
|
|
if (state == AppLifecycleState.resumed) {
|
|
_updateDisplayOrientation();
|
|
_ensureSensorsListening();
|
|
if (!_isCalibrated) {
|
|
loadCalibrationForMotorcycle(null);
|
|
}
|
|
}
|
|
}
|
|
|
|
void _ensureSensorsListening() {
|
|
if (_accelSub == null || _gyroSub == null) {
|
|
_startSensors();
|
|
}
|
|
}
|
|
|
|
void _updateDisplayOrientation() {
|
|
try {
|
|
final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
|
|
if (view != null && view.physicalSize.height > 0) {
|
|
final size = view.physicalSize;
|
|
if (size.width > size.height) {
|
|
_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) {
|
|
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 calibrated upright gravity axis
|
|
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) {
|
|
// Automatic re-lock: if the device returns to the motorcycle mount (within 25° of calibrated gravity)
|
|
if (totalAngleDiffDeg < 25.0) {
|
|
_remountReturnCount++;
|
|
if (_remountReturnCount >= 15) {
|
|
_isCalibrated = true;
|
|
_remountAnomalyCount = 0;
|
|
_remountReturnCount = 0;
|
|
_recomputeBasisVectors();
|
|
_emitDiagnostics(0.0);
|
|
}
|
|
} else {
|
|
_remountReturnCount = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
_remountReturnCount = 0;
|
|
|
|
// Do not invalidate calibration while the vehicle is actively in motion or experiencing dynamic rotation
|
|
if (_speedMph > 2.0 || _latestRollRateDegPerSec.abs() > 3.0) {
|
|
_remountAnomalyCount = 0;
|
|
return;
|
|
}
|
|
|
|
// 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 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 must NEVER invalidate calibration.
|
|
// 2. Cockpit mounts: Handlebar mounts (QuadLock, RAM) are commonly pitched backwards 35°-55° facing the rider.
|
|
// 3. True remount / phone removal:
|
|
// - 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 isSuspectedRemount = totalAngleDiffDeg > 70.0 || pitchTiltDeg > 55.0;
|
|
|
|
if (isImmediateRemount) {
|
|
_invalidateRemount(totalAngleDiffDeg);
|
|
} else if (isSuspectedRemount) {
|
|
_remountAnomalyCount++;
|
|
// Require 50 consecutive stationary samples (1000ms) to filter out transient bumps, fork dive, or vibrations
|
|
if (_remountAnomalyCount >= 50) {
|
|
_invalidateRemount(totalAngleDiffDeg);
|
|
}
|
|
} else {
|
|
_remountAnomalyCount = 0;
|
|
}
|
|
}
|
|
|
|
void _invalidateRemount(double angleDiffDeg) {
|
|
debugPrint(
|
|
'[SensorFusion] Remount detected: device orientation shifted ${angleDiffDeg.toStringAsFixed(1)}° from saved calibration. Invalidating runtime calibration.',
|
|
);
|
|
_calibrationVersion++;
|
|
_isCalibrated = false;
|
|
_remountAnomalyCount = 0;
|
|
_remountReturnCount = 0;
|
|
_rollOffset = 0.0;
|
|
_pitchOffset = 0.0;
|
|
_recomputeBasisVectors();
|
|
_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) {
|
|
_sensorEventCounter++;
|
|
final now = DateTime.now();
|
|
if (_lastRateWindow == null) {
|
|
_lastRateWindow = now;
|
|
} else {
|
|
final elapsedMs = now.difference(_lastRateWindow!).inMilliseconds;
|
|
if (elapsedMs >= 1000) {
|
|
_eventRateHz = (_sensorEventCounter * 1000.0) / elapsedMs;
|
|
_sensorEventCounter = 0;
|
|
_lastRateWindow = now;
|
|
}
|
|
}
|
|
|
|
_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,
|
|
eventRateHz: _eventRateHz,
|
|
longitudinalG: _filteredLongitudinalG,
|
|
lateralG: _filteredLateralG,
|
|
);
|
|
|
|
_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;
|
|
_remountReturnCount = 0;
|
|
_recomputeBasisVectors();
|
|
if (_isCalibrated) {
|
|
_leanAngleController.add(0.0);
|
|
_pitchAngleController.add(0.0);
|
|
_emitDiagnostics(0.0);
|
|
}
|
|
} 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;
|
|
_remountReturnCount = 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, 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);
|
|
_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;
|
|
_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);
|
|
}
|
|
|
|
void dispose() {
|
|
WidgetsBinding.instance.removeObserver(this);
|
|
_accelSub?.cancel();
|
|
_gyroSub?.cancel();
|
|
_userAccelSub?.cancel();
|
|
_leanAngleController.close();
|
|
_pitchAngleController.close();
|
|
_gForceController.close();
|
|
_diagnosticsController.close();
|
|
}
|
|
}
|