121 lines
4.2 KiB
Dart
121 lines
4.2 KiB
Dart
import 'dart:async';
|
|
import 'dart:math' as math;
|
|
import 'package:flutter_riverpod/flutter_riverpod.dart';
|
|
import 'package:sensors_plus/sensors_plus.dart';
|
|
|
|
final sensorFusionServiceProvider = Provider<SensorFusionService>((ref) {
|
|
return SensorFusionService();
|
|
});
|
|
|
|
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)
|
|
|
|
GForceData({required this.longitudinal, required this.lateral});
|
|
}
|
|
|
|
class SensorFusionService {
|
|
final _leanAngleController = StreamController<double>.broadcast();
|
|
final _gForceController = StreamController<GForceData>.broadcast();
|
|
|
|
Stream<double> get leanAngleStream => _leanAngleController.stream;
|
|
Stream<GForceData> get gForceStream => _gForceController.stream;
|
|
|
|
StreamSubscription? _accelSub;
|
|
StreamSubscription? _gyroSub;
|
|
StreamSubscription? _userAccelSub;
|
|
|
|
// Calibration offsets
|
|
double _rollOffset = 0.0;
|
|
|
|
// Sensor fusion state
|
|
double _currentRoll = 0.0; // The lean angle
|
|
double _currentPitch = 0.0;
|
|
DateTime? _lastUpdate;
|
|
|
|
static const double _alpha = 0.96; // Complementary filter constant
|
|
|
|
SensorFusionService() {
|
|
_startSensors();
|
|
}
|
|
|
|
void _startSensors() {
|
|
_gyroSub = gyroscopeEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((GyroscopeEvent event) {
|
|
final now = DateTime.now();
|
|
if (_lastUpdate == null) {
|
|
_lastUpdate = now;
|
|
return;
|
|
}
|
|
|
|
final dt = now.difference(_lastUpdate!).inMicroseconds / 1000000.0;
|
|
_lastUpdate = now;
|
|
|
|
// Integrate gyro (rad/s to degrees)
|
|
final gx = event.x * (180.0 / math.pi);
|
|
final gy = event.y * (180.0 / math.pi);
|
|
|
|
_currentPitch += gx * dt;
|
|
_currentRoll += gy * dt;
|
|
});
|
|
|
|
_accelSub = accelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((AccelerometerEvent event) {
|
|
// Calculate roll/pitch from accelerometer (accounting for gravity)
|
|
// Assuming phone is mounted roughly vertical or horizontal, we calculate basic roll
|
|
final accelRoll = math.atan2(event.x, math.sqrt(event.y * event.y + event.z * event.z)) * (180.0 / math.pi);
|
|
final accelPitch = math.atan2(-event.y, event.z) * (180.0 / math.pi);
|
|
|
|
// Complementary filter: Combine integrated gyro with absolute accel
|
|
if (_lastUpdate != null) {
|
|
_currentRoll = _alpha * _currentRoll + (1.0 - _alpha) * accelRoll;
|
|
_currentPitch = _alpha * _currentPitch + (1.0 - _alpha) * accelPitch;
|
|
|
|
double calibratedLean = _currentRoll - _rollOffset;
|
|
_leanAngleController.add(calibratedLean);
|
|
}
|
|
});
|
|
|
|
_userAccelSub = userAccelerometerEventStream(samplingPeriod: const Duration(milliseconds: 20)).listen((UserAccelerometerEvent event) {
|
|
// User accelerometer removes gravity.
|
|
// Convert m/s^2 to G's
|
|
const gravity = 9.80665;
|
|
|
|
// We need to rotate these vectors by the current pitch/roll to get true longitudinal/lateral G's
|
|
// relative to the motorcycle, but for a mounted phone, Y is typically longitudinal if vertical,
|
|
// or Z if horizontal. This requires mounting orientation knowledge.
|
|
// Assuming a standard gauge-cluster mount (portrait or landscape).
|
|
// For now, we provide the raw components mapped to standard device axes.
|
|
// E.g., device Y is longitudinal (forward/back), device X is lateral.
|
|
|
|
double longitudinalG = event.y / gravity;
|
|
double lateralG = event.x / gravity;
|
|
|
|
// In a real scenario, these would be transformed by the calibration matrix.
|
|
_gForceController.add(GForceData(
|
|
longitudinal: longitudinalG,
|
|
lateral: lateralG,
|
|
));
|
|
});
|
|
}
|
|
|
|
void calibrateUpright() {
|
|
// Zero out the current lean
|
|
_rollOffset = _currentRoll;
|
|
}
|
|
|
|
void dispose() {
|
|
_accelSub?.cancel();
|
|
_gyroSub?.cancel();
|
|
_userAccelSub?.cancel();
|
|
_leanAngleController.close();
|
|
_gForceController.close();
|
|
}
|
|
}
|