332 lines
11 KiB
Dart
332 lines
11 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;
|
|
});
|
|
|
|
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();
|
|
|
|
Stream<double> get leanAngleStream => _leanAngleController.stream;
|
|
Stream<double> get pitchAngleStream => _pitchAngleController.stream;
|
|
Stream<GForceData> get gForceStream => _gForceController.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;
|
|
|
|
// Raw sensor caches (unfiltered for forensics)
|
|
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; // The lean angle
|
|
double _currentPitch = 0.0;
|
|
DateTime? _lastGyroTime;
|
|
|
|
bool _isLandscape = false;
|
|
bool _isLandscapeLeft = true;
|
|
|
|
static const double _gravityConst = 9.80665;
|
|
|
|
// Low-pass filter for G-Force to suppress motorcycle vibration spikes
|
|
double _filteredLongitudinalG = 0.0;
|
|
double _filteredLateralG = 0.0;
|
|
static const double _gAlpha = 0.40; // Fast responsive dynamic smoothing
|
|
|
|
SensorFusionService(this._calibrationService) {
|
|
WidgetsBinding.instance.addObserver(this);
|
|
_updateDisplayOrientation();
|
|
_loadInitialCalibration();
|
|
_startSensors();
|
|
}
|
|
|
|
@override
|
|
void didChangeMetrics() {
|
|
_updateDisplayOrientation();
|
|
}
|
|
|
|
void _updateDisplayOrientation() {
|
|
try {
|
|
final view = WidgetsBinding.instance.platformDispatcher.views.firstOrNull;
|
|
if (view != null) {
|
|
final size = view.physicalSize;
|
|
_isLandscape = size.width > size.height;
|
|
}
|
|
} catch (_) {}
|
|
}
|
|
|
|
void setOrientation(bool isLandscape, {bool isLandscapeLeft = true}) {
|
|
_isLandscape = isLandscape;
|
|
_isLandscapeLeft = isLandscapeLeft;
|
|
}
|
|
|
|
Future<void> _loadInitialCalibration() async {
|
|
try {
|
|
final offsets = await _calibrationService.loadCalibrationOffsets();
|
|
_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;
|
|
} catch (_) {}
|
|
}
|
|
|
|
void _startSensors() {
|
|
_gyroSub = gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((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;
|
|
|
|
// Clamp dt to prevent runaway integration during gaps/app pauses
|
|
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;
|
|
|
|
// Transform gyro rates into Vehicle Coordinate Frame based on screen orientation
|
|
double vGyroRollRate; // Rotation around vehicle forward axis (lean rate)
|
|
double vGyroPitchRate; // Rotation around vehicle transverse axis (pitch rate)
|
|
|
|
if (!_isLandscape) {
|
|
// Portrait
|
|
vGyroPitchRate = unbiasedGx * (180.0 / math.pi);
|
|
vGyroRollRate = -unbiasedGy * (180.0 / math.pi);
|
|
} else if (_isLandscapeLeft) {
|
|
// Landscape Left
|
|
vGyroPitchRate = unbiasedGy * (180.0 / math.pi);
|
|
vGyroRollRate = unbiasedGx * (180.0 / math.pi);
|
|
} else {
|
|
// Landscape Right
|
|
vGyroPitchRate = -unbiasedGy * (180.0 / math.pi);
|
|
vGyroRollRate = -unbiasedGx * (180.0 / math.pi);
|
|
}
|
|
|
|
_latestRollRateDegPerSec = vGyroRollRate;
|
|
|
|
_currentPitch = (_currentPitch + vGyroPitchRate * dt).clamp(-90.0, 90.0);
|
|
_currentRoll = (_currentRoll + vGyroRollRate * dt).clamp(-90.0, 90.0);
|
|
});
|
|
|
|
_accelSub = accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((AccelerometerEvent event) {
|
|
_latestRawAccelX = event.x;
|
|
_latestRawAccelY = event.y;
|
|
_latestRawAccelZ = event.z;
|
|
|
|
// Auto-detect landscape sub-type if in landscape
|
|
if (_isLandscape && event.x.abs() > 3.0) {
|
|
_isLandscapeLeft = event.x > 0;
|
|
}
|
|
|
|
// Map raw accelerometer to Vehicle Frame (vx = Right, vy = Forward, vz = Up)
|
|
double vx, vy, vz;
|
|
if (!_isLandscape) {
|
|
vx = event.x;
|
|
vy = event.y;
|
|
vz = event.z;
|
|
} else if (_isLandscapeLeft) {
|
|
vx = -event.y;
|
|
vy = event.x;
|
|
vz = event.z;
|
|
} else {
|
|
vx = event.y;
|
|
vy = -event.x;
|
|
vz = event.z;
|
|
}
|
|
|
|
// Calculate total acceleration magnitude
|
|
final norm = math.sqrt(vx * vx + vy * vy + vz * vz);
|
|
if (norm < 0.1) return;
|
|
|
|
// Compute static gravity-based angles in vehicle frame
|
|
final accelRoll = -math.atan2(vx, math.sqrt(vy * vy + vz * vz)) * (180.0 / math.pi);
|
|
final accelPitch = math.atan2(-vy, vz) * (180.0 / math.pi);
|
|
|
|
final gMag = norm / _gravityConst;
|
|
final dynamicDelta = (gMag - 1.0).abs();
|
|
|
|
// Continuous dynamic adaptation: smoothly shifts reliance to gyro when total G deviates from 1.0g
|
|
final dynamicAlpha = (0.94 + 1.2 * dynamicDelta).clamp(0.94, 0.998);
|
|
|
|
_currentRoll = (dynamicAlpha * _currentRoll + (1.0 - dynamicAlpha) * accelRoll).clamp(-90.0, 90.0);
|
|
_currentPitch = (dynamicAlpha * _currentPitch + (1.0 - dynamicAlpha) * accelPitch).clamp(-90.0, 90.0);
|
|
|
|
final calibratedLean = (_currentRoll - _rollOffset).clamp(-65.0, 65.0);
|
|
final calibratedPitch = (_currentPitch - _pitchOffset).clamp(-45.0, 45.0);
|
|
|
|
_leanAngleController.add(calibratedLean);
|
|
_pitchAngleController.add(calibratedPitch);
|
|
});
|
|
|
|
_userAccelSub = userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((UserAccelerometerEvent event) {
|
|
double vxUser, vyUser, vzUser;
|
|
if (!_isLandscape) {
|
|
vxUser = event.x;
|
|
vyUser = event.y;
|
|
vzUser = event.z;
|
|
} else if (_isLandscapeLeft) {
|
|
vxUser = -event.y;
|
|
vyUser = event.x;
|
|
vzUser = event.z;
|
|
} else {
|
|
vxUser = event.y;
|
|
vyUser = -event.x;
|
|
vzUser = event.z;
|
|
}
|
|
|
|
// Project phone linear acceleration vector into motorcycle body frame
|
|
final pitchRad = (_currentPitch - _pitchOffset) * (math.pi / 180.0);
|
|
final rollRad = (_currentRoll - _rollOffset) * (math.pi / 180.0);
|
|
|
|
// Forward/longitudinal acceleration (along motorcycle centerline)
|
|
final rawLongitudinal = (vyUser * math.cos(pitchRad) - vzUser * math.sin(pitchRad));
|
|
// Lateral acceleration (cornering)
|
|
final rawLateral = (vxUser * math.cos(rollRad) + vzUser * math.sin(rollRad));
|
|
|
|
double rawLongitudinalG = rawLongitudinal / _gravityConst;
|
|
double rawLateralG = rawLateral / _gravityConst;
|
|
|
|
// Deadband / noise gate for resting phone on table
|
|
if (rawLongitudinalG.abs() < 0.02) rawLongitudinalG = 0.0;
|
|
if (rawLateralG.abs() < 0.02) rawLateralG = 0.0;
|
|
|
|
// Dynamic smoothing: suppress engine high-frequency vibration while preserving vehicle dynamics
|
|
_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,
|
|
));
|
|
});
|
|
}
|
|
|
|
Future<void> loadCalibrationForMotorcycle(int? motorcycleId) async {
|
|
try {
|
|
final offsets = await _calibrationService.loadCalibrationOffsets(motorcycleId: motorcycleId);
|
|
_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;
|
|
} catch (_) {}
|
|
}
|
|
|
|
void calibrateUpright({int? motorcycleId}) {
|
|
_rollOffset = _currentRoll;
|
|
_pitchOffset = _currentPitch;
|
|
_gyroBiasX = _latestRawGyroX;
|
|
_gyroBiasY = _latestRawGyroY;
|
|
_gyroBiasZ = _latestRawGyroZ;
|
|
_calibrationService.saveCalibrationOffsets(
|
|
_pitchOffset,
|
|
_rollOffset,
|
|
gyroBiasX: _gyroBiasX,
|
|
gyroBiasY: _gyroBiasY,
|
|
gyroBiasZ: _gyroBiasZ,
|
|
motorcycleId: motorcycleId,
|
|
);
|
|
}
|
|
|
|
void dispose() {
|
|
WidgetsBinding.instance.removeObserver(this);
|
|
_accelSub?.cancel();
|
|
_gyroSub?.cancel();
|
|
_userAccelSub?.cancel();
|
|
_leanAngleController.close();
|
|
_pitchAngleController.close();
|
|
_gForceController.close();
|
|
}
|
|
}
|