262 lines
8.8 KiB
Dart
262 lines
8.8 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; // Raw m/s^2
|
|
final double accelY; // Raw m/s^2
|
|
final double accelZ; // Raw m/s^2
|
|
|
|
GForceData({
|
|
required this.longitudinal,
|
|
required this.lateral,
|
|
required this.accelX,
|
|
required this.accelY,
|
|
required this.accelZ,
|
|
});
|
|
}
|
|
|
|
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
|
|
double _rollOffset = 0.0;
|
|
double _pitchOffset = 0.0;
|
|
|
|
// 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 _baseAlpha = 0.94; // Base complementary filter gain
|
|
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;
|
|
} catch (_) {}
|
|
}
|
|
|
|
void _startSensors() {
|
|
_gyroSub = gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((GyroscopeEvent event) {
|
|
final now = DateTime.now();
|
|
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;
|
|
|
|
// 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 = event.x * (180.0 / math.pi);
|
|
vGyroRollRate = -event.y * (180.0 / math.pi);
|
|
} else if (_isLandscapeLeft) {
|
|
// Landscape Left
|
|
vGyroPitchRate = event.y * (180.0 / math.pi);
|
|
vGyroRollRate = event.x * (180.0 / math.pi);
|
|
} else {
|
|
// Landscape Right
|
|
vGyroPitchRate = -event.y * (180.0 / math.pi);
|
|
vGyroRollRate = -event.x * (180.0 / math.pi);
|
|
}
|
|
|
|
_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) {
|
|
// 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();
|
|
|
|
// If dynamic force > 0.08g, trust gyro to prevent vehicle acceleration from distorting lean
|
|
double dynamicAlpha = _baseAlpha;
|
|
if (dynamicDelta > 0.08) {
|
|
dynamicAlpha = 0.998;
|
|
} else if (dynamicDelta > 0.04) {
|
|
dynamicAlpha = 0.985;
|
|
}
|
|
|
|
_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,
|
|
));
|
|
});
|
|
}
|
|
|
|
void calibrateUpright() {
|
|
_rollOffset = _currentRoll;
|
|
_pitchOffset = _currentPitch;
|
|
_calibrationService.saveCalibrationOffsets(_pitchOffset, _rollOffset);
|
|
}
|
|
|
|
void dispose() {
|
|
WidgetsBinding.instance.removeObserver(this);
|
|
_accelSub?.cancel();
|
|
_gyroSub?.cancel();
|
|
_userAccelSub?.cancel();
|
|
_leanAngleController.close();
|
|
_pitchAngleController.close();
|
|
_gForceController.close();
|
|
}
|
|
}
|