diff --git a/CLAUDE.md b/CLAUDE.md index dd13468..ba662cf 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -27,6 +27,8 @@ You are a seasoned embedded Rust engineer with deep expertise in: - Efficient binary protocols (fixed-size packets, checksums) - Avoid over-engineering: solve the problem at hand, not hypothetical future problems - Keep abstractions minimal until they prove necessary +- **Never use `#[allow(dead_code)]`** - dead code must be removed or used, not silenced +- **Never duplicate constants** - use a single `const` and reference it everywhere. Add a unit test to verify defaults use the constant. **Testing Philosophy:** - Write meaningful tests that catch real bugs, not boilerplate @@ -302,10 +304,10 @@ After 5 consecutive stationary detections: | Preset | Accel | Brake | Lateral | Yaw | Min Speed | Use Case | |--------|-------|-------|---------|-----|-----------|----------| -| Track | 0.30g | 0.50g | 0.50g | 0.15 rad/s | 3.0 m/s | Racing, autocross | -| Canyon | 0.20g | 0.35g | 0.30g | 0.10 rad/s | 2.5 m/s | Spirited twisty roads | -| City (default) | 0.10g | 0.18g | 0.12g | 0.05 rad/s | 2.0 m/s | Daily driving | -| Highway | 0.08g | 0.15g | 0.10g | 0.04 rad/s | 4.0 m/s | Cruise, subtle inputs | +| Track | 0.35g | 0.35g | 0.50g | 0.15 rad/s | 4.0 m/s | Racing, autocross | +| Canyon | 0.22g | 0.22g | 0.28g | 0.10 rad/s | 3.0 m/s | Spirited twisty roads | +| City (default) | 0.10g | 0.15g | 0.10g | 0.04 rad/s | 2.0 m/s | Daily driving | +| Highway | 0.12g | 0.15g | 0.12g | 0.04 rad/s | 5.0 m/s | Cruise, subtle inputs | | Custom | User-defined | | | | | Fine-tuning via sliders | Exit thresholds are 50% of entry values for hysteresis (prevents oscillation). @@ -531,29 +533,198 @@ Result: acceleration due to motion only (no gravity component). ### mode.rs - Driving Mode Classifier -**NEW: Independent Component Detection** (replaces exclusive state machine) +**Hybrid Mode Detection** with GPS/IMU blending: +- Longitudinal acceleration (ACCEL/BRAKE) uses **blended GPS + IMU** +- Lateral acceleration (CORNER) uses **filtered IMU** (GPS can't sense lateral) +- GPS-derived acceleration computed from velocity changes at 25Hz +- Blending ratio adapts based on GPS rate and freshness + ``` -Each mode component detected independently with hysteresis: +Independent hysteresis detection: -ACCEL: entry at 0.10g → exit at 0.05g -BRAKE: entry at -0.18g → exit at -0.09g (mutually exclusive with ACCEL) -CORNER: entry at 0.12g lat + 0.05rad/s yaw → exit at 0.06g lat or low yaw +ACCEL: entry at 0.10g → exit at 0.05g (from blended lon accel) +BRAKE: entry at -0.15g → exit at -0.08g (from blended lon accel) +CORNER: entry at 0.10g lat + 0.04rad/s yaw → exit at 0.05g lat -Combined states possible: +Combined states: - ACCEL + CORNER = corner exit (trail throttle) - BRAKE + CORNER = corner entry (trail braking) ``` -**Hysteresis:** Entry threshold > exit threshold prevents oscillation. +**Benefits of Hybrid Detection:** +- Mounting angle independent (GPS measures actual vehicle acceleration) +- No drift (GPS doesn't drift like IMU) +- Graceful fallback when GPS degrades **All modes require:** speed > min_speed (2.0 m/s / 7.2 km/h) -**Speed Display:** -Uses **raw GPS speed** directly (system.rs:295-297) for responsive updates. -- No longer uses EKF-filtered velocity (which was laggy) -- Server sends raw GPS speed (5 Hz updates) -- Dashboard applies light EMA smoothing (alpha=0.4) for car speedometer-like display -- Mode detection still uses EKF velocity for stability +### filter.rs - Biquad Low-Pass Filter + +2nd-order Butterworth IIR filter for vibration removal from IMU longitudinal acceleration. + +**Problem:** +Engine vibration (30-100 Hz) causes 0.08-0.12g noise on longitudinal G readings when +the vehicle is running but stationary. This makes mode detection unreliable. + +**Solution (based on ArduPilot research):** +- ArduPilot uses **20Hz** for INS_ACCEL_FILTER default +- Sharp braking events can have frequency components up to 5-10Hz +- Engine/road vibration is 30-100Hz → separable from driving dynamics (0-10Hz) + +**Configuration:** +- Sample rate: **200 Hz (IMU rate)** - CRITICAL: must match actual process_imu() call rate! +- Cutoff: **15 Hz** (preserves driving dynamics 0-10Hz, removes vibration 30+Hz) +- Q = 0.707 (Butterworth - maximally flat passband, no overshoot) + +**Frequency Response:** +``` +0-10 Hz: ~100% pass (driving dynamics preserved) +15 Hz: -3 dB (cutoff point) +30 Hz: -12 dB (engine vibration attenuated) +60+ Hz: -24+ dB (engine vibration removed) +``` + +**Implementation:** +```rust +// Direct Form I biquad +y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] +``` + +### fusion.rs - Sensor Fusion Module + +Handles GPS-corrected orientation, GPS/IMU blending, and continuous calibration. +Uses an **ArduPilot-style approach**: GPS velocity provides ground truth to correct AHRS +orientation errors, enabling accurate 200 Hz IMU data for mode detection. + +**The Core Problem:** +The WT901 AHRS cannot distinguish linear acceleration from tilt. During forward +acceleration, it reports false pitch (thinks device is tilting backward). This corrupts +gravity removal and produces wrong earth-frame acceleration. Without correction, +measured correlation with ground truth was only 0.32 (garbage). + +**Solution: GPS-Corrected Orientation** +Compare IMU-predicted acceleration (using AHRS angles) with GPS-derived acceleration +(ground truth). The difference reveals orientation error: +``` +pitch_error ≈ (ax_imu - ax_gps) / G +``` + +**Components:** + +1. **OrientationCorrector** - Learns pitch/roll corrections from GPS velocity (NEW) + - Compares IMU-derived acceleration with GPS ground truth + - Learns pitch correction during acceleration (forward/backward) + - Learns roll correction during cornering (left/right) + - Applies corrections BEFORE gravity removal + - Confidence increases with learning samples (0-100%) + - Max correction capped at ±15° for safety + - Enables accurate 200 Hz IMU instead of being limited to GPS rate + +2. **GpsAcceleration** - Computes longitudinal acceleration from GPS velocity + - `accel = (speed_new - speed_old) / dt` + - At 25Hz GPS: dt = 40ms, clean acceleration signal + - Tracks staleness for fallback to IMU + - Provides ground truth for OrientationCorrector learning + +3. **TiltEstimator** - Learns residual mounting offset when stopped + - After 3 seconds stationary, averages earth-frame acceleration + - Handles minor offsets not captured by OrientationCorrector + - Relearns at every stop (adapts to device repositioning) + +4. **YawRateCalibrator** - Learns gyro bias while driving straight + - When GPS heading is stable (±2° for 3+ seconds), assumes true yaw rate ≈ 0 + - Any measured yaw rate during this time is gyro bias + - Applies correction to prevent lateral drift in centripetal calculation + - Essential for highway driving where ZUPT rarely triggers + +5. **SensorFusion** - Main processor + - Takes raw body-frame IMU + AHRS angles (NEW API) + - Applies orientation correction (learned from GPS comparison) + - Removes gravity using corrected orientation + - Applies residual tilt correction (learned when stopped) + - **10 Hz Butterworth low-pass filter** removes engine vibration + - Blends corrected IMU with GPS based on correction confidence + - Computes centripetal lateral (speed × calibrated_yaw_rate) + +**GPS/IMU Blending Ratios (based on OrientationCorrector confidence):** +``` +Confidence > 80%: 30% GPS / 70% corrected IMU (trust corrected IMU) +Confidence 30-80%: 50% GPS / 50% corrected IMU (moderate confidence, interpolated) +Confidence < 30%: 80% GPS / 20% corrected IMU (not confident yet, interpolated) +GPS stale (>200ms): 0% GPS / 100% corrected IMU (fallback) +``` +Note: GPS blending IS still necessary even after OrientationCorrector learns because: +1. Cold start needs GPS until correction converges (first 1-2 minutes) +2. Provides continuous sanity check / measurement diversity +3. Graceful degradation if correction diverges + +**process_imu() API (NEW):** +```rust +pub fn process_imu( + &mut self, + ax_raw: f32, // Raw body-frame X accel (m/s²) + ay_raw: f32, // Raw body-frame Y accel (m/s²) + az_raw: f32, // Raw body-frame Z accel (gravity on Z) + ahrs_roll_deg: f32, // AHRS roll angle (degrees) + ahrs_pitch_deg: f32, // AHRS pitch angle (degrees) + yaw: f32, // Vehicle yaw (radians, from EKF) + speed: f32, // Speed (m/s) + yaw_rate: f32, // Yaw rate (rad/s) + dt: f32, // Time step (seconds) + is_stationary: bool, // Whether vehicle is stopped +) -> (f32, f32) // Returns (lon_blended, lat_centripetal) +``` + +**Data Flow:** +``` +LONGITUDINAL (GPS-Corrected Orientation): +GPS (25Hz) → GpsAcceleration → lon_accel_gps ──────────────────────────┐ + │ │ + └──► OrientationCorrector learns │ + ↓ │ +IMU raw (200Hz) → OrientationCorrector.correct(pitch, roll) │ + ↓ │ + Corrected orientation │ + ↓ │ + remove_gravity → body_to_earth → Tilt → Biquad(10Hz)│ + ↓ │ + lon_imu_corrected ─────────────► Blend ◄────────────┘ + ↓ (based on confidence) + lon_blended + +LATERAL (centripetal): +GPS heading → YawRateCalibrator → learned_bias + ↓ +Gyro yaw_rate → (yaw_rate - bias) → calibrated_yaw_rate + ↓ +GPS speed × calibrated_yaw_rate → lat_centripetal + +OUTPUT: +(lon_blended, lat_centripetal) → mode.rs (classification) + → Dashboard (G-meter) +``` + +**Why This Works (ArduPilot Insight):** +- During acceleration, pitch error causes ax_earth to be wrong +- GPS velocity gives us true horizontal acceleration (dv/dt = ground truth) +- The innovation (IMU - GPS difference) is directly proportional to pitch error +- Learning is continuous while driving, adapts to any mount angle +- Once learned, enables accurate 200 Hz IMU instead of 25 Hz GPS rate + +**Diagnostics (NEW):** +- `pitch_correction_deg`: Learned pitch correction (degrees) +- `roll_correction_deg`: Learned roll correction (degrees) +- `pitch_confidence`: Correction confidence (0-100%) +- `roll_confidence`: Correction confidence (0-100%) + +**Why Centripetal Lateral?** +- Traditional IMU lateral (accelerometer) suffers from mount angle sensitivity and can "stick" +- Centripetal: `a_lat = v × ω` is physics-based, instant, and mount-independent +- Uses calibrated yaw rate to prevent drift during highway driving +- Pro telemetry systems use this approach + +**Dashboard Display:** The G-meter shows the same blended longitudinal and centripetal lateral +values that mode classification uses. What you see is what the algorithm sees. ### websocket_server.rs - HTTP Dashboard Server @@ -645,6 +816,22 @@ Displayed as updates per minute (rolling average), not cumulative count. Typical - ZUPT: 0-60/min (depends on stops) - Free heap: ~60KB (stable during operation) +**Color-Coded Thresholds:** +The diagnostics page displays metrics with color coding for at-a-glance health status: + +| Metric | Green (OK) | Yellow (Warning) | Red (Error) | +|--------|------------|------------------|-------------| +| Loop Rate | > 500 Hz | > 200 Hz | < 200 Hz | +| Position σ | < 5 m | < 10 m | > 10 m | +| Velocity σ | < 0.5 m/s | < 1.0 m/s | > 1.0 m/s | +| Yaw σ | < 5° | < 10° | > 10° | +| Bias X/Y | < 0.3 m/s² | < 0.5 m/s² | > 0.5 m/s² | +| Heap | > 40 KB | > 20 KB | < 20 KB | +| Yaw Bias | < 10 mrad/s | < 50 mrad/s | > 50 mrad/s | +| Tilt X/Y | < 0.3 m/s² | < 0.5 m/s² | > 0.5 m/s² | +| Pitch/Roll Corr | < 10° | - | > 10° | +| Orient Conf | > 50% | - | < 50% | + ### udp_stream.rs - UDP Client (Station Mode) **Features:** diff --git a/README.md b/README.md index 62d68f2..235dfc2 100644 --- a/README.md +++ b/README.md @@ -4,13 +4,7 @@ [![Rust](https://img.shields.io/badge/rust-1.75%2B-orange.svg)](https://www.rust-lang.org/) [![ESP32-C3](https://img.shields.io/badge/platform-ESP32--C3-blue.svg)](https://www.espressif.com/) -**Real-time vehicle dynamics tracking using GPS+IMU sensor fusion.** Professional-grade algorithms on $50 hardware. - ---- - -## What Is This? - -A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's position, velocity, and acceleration in real-time. Built for track day logging, vehicle dynamics research, and DIY automotive projects. +A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's position, velocity, and acceleration in real-time. Built for track day logging, vehicle dynamics research, and DIY automotive projects. Professional-grade algorithms on $50 hardware. **What it does:** - Tracks position (±1m accuracy between GPS updates) @@ -20,14 +14,7 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's - Streams telemetry at 20-30Hz over WiFi - **Built-in mobile dashboard** - view live data on your phone -**How it works:** -- 7-state Extended Kalman Filter fuses GPS (5Hz) and IMU (50Hz) -- Gyro integrates yaw between GPS updates to maintain heading -- Body-frame accelerations transformed to earth frame using current orientation -- Zero-velocity updates eliminate IMU drift when stationary -- Constant Turn Rate and Acceleration (CTRA) model for cornering - -**Why it's useful:** +**Why build this instead of buying?** - Track day data logging without $1000+ commercial systems - Vehicle dynamics research and education - DIY EV/kit car development @@ -36,6 +23,41 @@ A complete ESP32-C3 firmware that fuses GPS and IMU data to track your vehicle's --- +## Table of Contents + +- [Hardware Requirements](#hardware-requirements) +- [Quick Start](#quick-start) + - [Option A: Web Flasher](#option-a-web-flasher-easiest) + - [Option B: Build from Source](#option-b-build-from-source) +- [Building From Source](#building-from-source) + - [Prerequisites](#prerequisites) + - [Build Commands](#build-commands) + - [Common Build Issues](#common-build-issues) +- [System Architecture](#system-architecture) + - [Data Flow](#data-flow) + - [File Structure](#file-structure) + - [Key Algorithms](#key-algorithms) +- [Telemetry Format](#telemetry-format) +- [Configuration](#configuration) + - [WiFi and Network](#wifi-and-network) + - [EKF Tuning](#ekf-tuning) + - [EKF Health Metrics](#ekf-health-metrics) + - [Mode Detection Thresholds](#mode-detection-thresholds) +- [Performance](#performance) +- [LED Status Codes](#led-status-codes) +- [Calibration](#calibration) +- [Mobile Dashboard](#mobile-dashboard) + - [Dashboard Features](#dashboard-features) + - [Diagnostics Page](#diagnostics-page) + - [WiFi Modes](#wifi-modes) + - [Using Python Tools](#using-python-tools-station-mode-only) +- [How It Works](#how-it-works) +- [Future Enhancements](#future-enhancements) +- [Contributing](#contributing) +- [License](#license) + +--- + ## Hardware Requirements ### Core Components ($50-100 total) @@ -307,6 +329,12 @@ cargo fmt # Lint cargo clippy -- -D warnings +# Run unit tests (on host, no device needed) +cargo test -p sensor-fusion -p wt901 -p ublox-gps + +# Run driving simulation (demonstrates GPS-corrected orientation) +cargo run -p sensor-fusion --example drive_sim + # Clean build (if things go wrong) cargo clean ``` @@ -434,28 +462,38 @@ GPS (5-25Hz) IMU (200Hz) ``` blackbox/ +├── framework/ # sensor-fusion library crate (host-testable) +│ └── src/ +│ ├── lib.rs # Crate entry point +│ ├── ekf.rs # Extended Kalman Filter +│ ├── transforms.rs # Body↔Earth↔Vehicle coordinate math +│ ├── mode.rs # Driving mode classifier +│ ├── fusion.rs # GPS/IMU blending, tilt correction +│ ├── filter.rs # Biquad filter (unused, for reference) +│ └── velocity.rs # Velocity source selection +├── drivers/ +│ ├── wt901/ # WT901 IMU driver crate +│ └── ublox-gps/ # u-blox GPS driver crate ├── sensors/ -│ └── blackbox/ +│ └── blackbox/ # ESP32 firmware (embedded binary) │ ├── .cargo/ -│ │ └── config.toml # ESP32-C3 build configuration +│ │ └── config.toml # ESP32-C3 build configuration │ ├── src/ -│ │ ├── main.rs # Main loop and setup -│ │ ├── config.rs # Build-time configuration (GPS model, WiFi mode) -│ │ ├── system.rs # Sensor/estimator/publisher managers -│ │ ├── imu.rs # WT901 UART parser (auto-detect baud) -│ │ ├── gps.rs # NMEA/UBX parser with warmup (NEO-6M/M9N) -│ │ ├── diagnostics.rs # System health monitoring -│ │ ├── transforms.rs # Body↔Earth coordinate math -│ │ ├── mode.rs # Driving mode classifier -│ │ ├── binary_telemetry.rs # 67-byte packet format -│ │ ├── websocket_server.rs # Mobile dashboard & HTTP server -│ │ ├── udp_stream.rs # High-speed UDP client -│ │ ├── mqtt.rs # MQTT client for status -│ │ ├── wifi.rs # WiFi connection manager -│ │ └── rgb_led.rs # WS2812 status LED -│ ├── Cargo.toml # Rust dependencies -│ ├── sdkconfig.defaults # ESP-IDF configuration -│ └── build.rs # Build script +│ │ ├── main.rs # Main loop and setup +│ │ ├── config.rs # Build-time configuration +│ │ ├── system.rs # Sensor/estimator/publisher managers +│ │ ├── imu.rs # WT901 UART parser (auto-detect baud) +│ │ ├── gps.rs # NMEA/UBX parser with warmup +│ │ ├── diagnostics.rs # System health monitoring +│ │ ├── binary_telemetry.rs # 67-byte packet format +│ │ ├── websocket_server.rs # Mobile dashboard & HTTP server +│ │ ├── udp_stream.rs # High-speed UDP client +│ │ ├── mqtt.rs # MQTT client for status +│ │ ├── wifi.rs # WiFi connection manager +│ │ └── rgb_led.rs # WS2812 status LED +│ ├── Cargo.toml # Rust dependencies +│ ├── sdkconfig.defaults # ESP-IDF configuration +│ └── build.rs # Build script ├── tools/ │ └── python/ │ ├── configure_wt901.py # IMU configuration tool (200Hz setup) @@ -883,6 +921,23 @@ python3 probe_wt901.py /dev/ttyUSB0 ``` Useful for verifying IMU is working before flashing, or determining what baud rate it's configured to. +**Analyze Telemetry CSV (works with both modes):** +```bash +# Export CSV from dashboard, then analyze on your computer +python3 analyze_telemetry.py recorded_session.csv +``` +Analyzes a CSV export from the dashboard and provides: +- **Timing analysis**: Sample rate, duration +- **Speed distribution**: Time in speed bands +- **Acceleration analysis**: lon_g range, mean, bias detection +- **Mode distribution**: Time spent in IDLE/ACCEL/BRAKE/CORNER +- **Ground truth comparison**: Compares lon_g with GPS-derived acceleration +- **Mode detection accuracy**: Precision and recall for ACCEL/BRAKE modes +- **Correlation**: How well lon_g matches actual acceleration (want >0.7) +- **Diagnostic summary**: Identifies potential issues like bias or high false positive rates + +This is useful for validating that the orientation correction is working properly after a test drive. + ### Choosing a Mode | Feature | Access Point | Station | @@ -896,6 +951,45 @@ Useful for verifying IMU is working before flashing, or determining what baud ra --- +## How It Works + +The system uses sensor fusion to combine fast but drifty IMU measurements with slow but accurate GPS updates. Here's what happens under the hood: + +**7-State Extended Kalman Filter** +- Tracks: position (x, y), heading (yaw), velocity (vx, vy), and accelerometer biases (bax, bay) +- Predicts state 200 times per second using IMU accelerations +- Corrects drift 5-25 times per second using GPS position and velocity +- Learns accelerometer biases during stops (Zero-Velocity Update / ZUPT) + +**Coordinate Transformations** +- Raw IMU measures in "body frame" (sensor orientation) +- Gravity is removed using roll/pitch angles +- Accelerations rotated to "earth frame" (horizontal plane) +- Then rotated to "vehicle frame" (car forward/left) for mode detection + +**GPS-Corrected Orientation (ArduPilot-style)** +- Problem: AHRS can't distinguish tilt from acceleration (reports false pitch when accelerating) +- Solution: Compare IMU-derived acceleration with GPS velocity ground truth +- Learn and correct pitch/roll errors over time +- Enables accurate 200Hz acceleration despite AHRS limitations + +**Tilt Correction** +- When stopped, the device learns its mounting offset automatically +- Works regardless of mounting angle (within reason) +- Re-learns if you move the device + +**Motion Models** +- CTRA (Constant Turn Rate and Acceleration): Used when speed >2 m/s and turning +- CA (Constant Acceleration): Used when going straight or slow + +**Why this matters for track days:** +- High-rate IMU gives responsive g-force readings (200 Hz) +- GPS prevents drift over time +- Mode detection works reliably regardless of mounting angle +- ZUPT eliminates accumulated errors at every stop + +--- + ## Future Enhancements - [ ] SD card logging for offline operation diff --git a/docs/index.html b/docs/index.html index 9270172..88a484d 100644 --- a/docs/index.html +++ b/docs/index.html @@ -4,7 +4,120 @@ Blackbox - Open Source Vehicle Telemetry - + + + + + + + + + + + + + + + + + + + + + + + @@ -840,7 +953,7 @@
About Features - Dashboard + How It Works Hardware GitHub Flash Firmware @@ -880,19 +993,19 @@

GPS + IMU sensor fusion for vehicle dynamics

-
+
20Hz
-
Telemetry
+
Telemetry Rate
-
+
200Hz
IMU Sampling
-
+
7-State
Kalman Filter
-
+
$50-100
Hardware Cost
@@ -1005,7 +1118,7 @@

Technical Features

Sensor Fusion

-

7-state EKF fuses 200Hz IMU with 5Hz GPS for drift-free position estimates.

+

7-state Extended Kalman Filter combines fast IMU (200Hz) with accurate GPS. Orientation correction automatically learns mounting angle errors.

@@ -1059,8 +1172,21 @@

Mobile Dashboard

-

ZUPT

-

Zero-velocity updates prevent drift. Bias continuously estimated when stationary.

+

Zero-Velocity Updates

+

When stopped, the system knows velocity is exactly zero. This prevents position drift that would otherwise accumulate over time.

+
+ +
+
+ + + + + + +
+

Auto Mounting Correction

+

Device can be mounted at any angle (within 15°). GPS provides ground truth to automatically learn and correct orientation errors.

@@ -1078,6 +1204,84 @@

Open Source

+
+
+
+

How It Works

+

Understanding the 7-state Extended Kalman Filter and sensor fusion

+
+ + +
+

The 7 States Explained

+

+ The Extended Kalman Filter (EKF) tracks 7 values that describe your vehicle's motion. These are continuously estimated and refined as new sensor data arrives: +

+
+
+
x, y
+
Position
+
Location in meters relative to where GPS first locked. X is east-west, Y is north-south.
+
+
+
ψ (psi)
+
Yaw / Heading
+
Which direction the vehicle is pointing, in radians. 0 = East, π/2 = North.
+
+
+
vx, vy
+
Velocity
+
Speed broken into east-west and north-south components, in meters per second.
+
+
+
bax, bay
+
Accelerometer Bias
+
Sensor error offsets that slowly drift. Learned when stationary and subtracted from readings.
+
+
+
+ + +
+

Glossary of Terms

+
+
+
EKF
+
Extended Kalman Filter — A mathematical algorithm that combines multiple sensor measurements to estimate state (position, velocity, etc.) more accurately than any single sensor alone.
+
+
+
IMU
+
Inertial Measurement Unit — A sensor that measures acceleration (how fast you're speeding up/slowing down) and rotation (how fast you're turning). The WT901 used here samples at 200 times per second.
+
+
+
GPS
+
Global Positioning System — Satellite-based positioning. Provides accurate position and velocity but only updates 5-25 times per second. The EKF fills the gaps using IMU data.
+
+
+
AHRS
+
Attitude and Heading Reference System — The WT901's built-in processor that estimates orientation (pitch, roll, yaw) by combining accelerometer, gyroscope, and magnetometer data.
+
+
+
ZUPT
+
Zero-Velocity Update — When the vehicle is stopped, we know velocity is exactly zero. This constraint prevents position errors from accumulating (drifting) over time.
+
+
+
Sensor Fusion
+
Combining data from multiple sensors (IMU + GPS) to get a better estimate than either sensor alone. The IMU is fast but drifts; GPS is accurate but slow. Together they give fast AND accurate data.
+
+
+
G-Force
+
Acceleration measured in multiples of gravity (9.8 m/s²). 1g = gravity. Hard braking might be 0.8-1.0g. Cornering in a sports car might reach 1.2g.
+
+
+
WiFi AP
+
WiFi Access Point — The ESP32 creates its own wireless network that your phone connects to directly. No router or internet required.
+
+
+
+
+
+
diff --git a/framework/examples/drive_sim.rs b/framework/examples/drive_sim.rs new file mode 100644 index 0000000..dce08d4 --- /dev/null +++ b/framework/examples/drive_sim.rs @@ -0,0 +1,346 @@ +//! Simulates a driving scenario to verify GPS-corrected orientation sensor fusion +//! +//! This simulation tests the ArduPilot-style approach: using GPS velocity to correct +//! AHRS orientation errors, enabling accurate 200 Hz IMU data for mode detection. +//! +//! **Key test**: The WT901 AHRS reports false pitch during acceleration (can't distinguish +//! linear acceleration from tilt). This simulation reproduces that error and verifies +//! OrientationCorrector learns to compensate. +//! +//! Run with: cargo run -p sensor-fusion --example drive_sim + +use sensor_fusion::{FusionConfig, ModeClassifier, SensorFusion}; + +const G: f32 = 9.80665; + +/// Simple pseudo-random noise generator (deterministic for reproducibility) +struct NoiseGen { + state: u32, +} + +impl NoiseGen { + fn new(seed: u32) -> Self { + Self { state: seed } + } + + /// Returns noise in range [-amplitude, +amplitude] + fn next(&mut self, amplitude: f32) -> f32 { + // Simple LCG + self.state = self.state.wrapping_mul(1103515245).wrapping_add(12345); + let normalized = (self.state as f32 / u32::MAX as f32) * 2.0 - 1.0; + normalized * amplitude + } +} + +fn main() { + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + let mut mode_classifier = ModeClassifier::new(); + let mut noise = NoiseGen::new(42); + + println!("=== GPS-Corrected Orientation Sensor Fusion Simulation ===\n"); + println!("This simulates: STOP → ACCELERATE → CRUISE → CORNER → BRAKE → STOP\n"); + println!("Key test: OrientationCorrector learns pitch/roll errors from GPS comparison,"); + println!("compensating for WT901 AHRS errors during acceleration/cornering.\n"); + + // Phase 1: Stationary calibration (3 seconds) + println!("Phase 1: STATIONARY CALIBRATION (3s)"); + println!(" Learning tilt offset with sensor noise..."); + for i in 0..600 { + // Add realistic sensor noise (±0.05g) + let ax_noise = noise.next(0.05 * G); + let ay_noise = noise.next(0.05 * G); + let az_noise = noise.next(0.02 * G); + + fusion.process_imu( + ax_noise, + ay_noise, + G + az_noise, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.005, + true, + ); + if i % 200 == 199 { + println!(" ... {}s", (i + 1) / 200); + } + } + let (tilt_x, tilt_y, tilt_valid) = fusion.get_tilt_offsets(); + let (pitch_corr, roll_corr) = fusion.get_orientation_correction(); + println!( + " Tilt learned: x={:.3}, y={:.3}, valid={}", + tilt_x, tilt_y, tilt_valid + ); + println!( + " Orientation correction: pitch={:.1}°, roll={:.1}°\n", + pitch_corr, roll_corr + ); + + // Phase 2: Acceleration (2 seconds, 0.3g) + // CRITICAL: Simulate WT901 AHRS error - it reports false pitch during acceleration! + // The AHRS thinks the device is tilting backward when we're actually accelerating forward. + println!("Phase 2: ACCELERATION (2s at 0.3g = 2.94 m/s²)"); + println!(" Simulating WT901 AHRS error: reports false pitch during acceleration"); + let mut speed = 0.0f32; + for i in 0..400 { + let accel = 2.94; // 0.3g forward acceleration + speed += accel * 0.005; + + // GPS provides ground truth + if i % 8 == 0 { + fusion.process_gps(speed, (i as f32) * 0.005); + } + + // WT901 AHRS ERROR: When accelerating forward at 0.3g, the AHRS reports + // ~17° pitch backward (atan(0.3) ≈ 17°) because it can't distinguish + // linear acceleration from tilt. This is the core problem! + let false_pitch = (accel / G).atan().to_degrees(); + + // Add sensor noise + let ax_noise = noise.next(0.05 * G); + let ay_noise = noise.next(0.05 * G); + + let (lon, lat) = fusion.process_imu( + accel + ax_noise, + ay_noise, + G, // Body-frame: forward accel + gravity + 0.0, + false_pitch, // AHRS reports false pitch! + 0.0, + speed, + 0.0, + 0.005, + false, + ); + mode_classifier.update_hybrid(lon, lat, 0.0, speed); + let mode = mode_classifier.get_mode(); + + if i % 100 == 99 { + let (pitch_corr, _) = fusion.get_orientation_correction(); + println!(" t={:.1}s: speed={:.1} m/s, lon={:.2} m/s², mode={}, AHRS_pitch={:.1}°, corr={:.1}°", + (i + 1) as f32 * 0.005, speed, lon, mode_str(&mode), false_pitch, pitch_corr); + } + } + println!(); + + // Phase 3: Cruise (5 seconds, constant speed, zero input) + println!("Phase 3: CRUISE (5s at constant speed, ZERO acceleration)"); + println!(" KEY TEST: OrientationCorrector should have learned pitch error"); + println!(" lon should stay near 0, mode should be IDLE\n"); + let cruise_speed = speed; + let mut max_lon = 0.0f32; + let mut accel_count = 0; + let mut brake_count = 0; + let mut idle_count = 0; + + for i in 0..1000 { + if i % 8 == 0 { + fusion.process_gps(cruise_speed, 2.0 + (i as f32) * 0.005); + } + + // ZERO acceleration - AHRS now reports level (no false pitch when not accelerating) + let ax_noise = noise.next(0.05 * G); + let ay_noise = noise.next(0.05 * G); + + let (lon, lat) = fusion.process_imu( + ax_noise, + ay_noise, + G, + 0.0, + 0.0, // AHRS reports level during cruise + 0.0, + cruise_speed, + 0.0, + 0.005, + false, + ); + mode_classifier.update_hybrid(lon, lat, 0.0, cruise_speed); + let mode = mode_classifier.get_mode(); + + if i > 200 { + // After filter settles + max_lon = max_lon.max(lon.abs()); + if mode.has_accel() { + accel_count += 1; + } else if mode.has_brake() { + brake_count += 1; + } else { + idle_count += 1; + } + } + + if i % 200 == 199 { + let (pitch_corr, _) = fusion.get_orientation_correction(); + let (pitch_conf, _) = fusion.get_orientation_confidence(); + println!( + " t={:.1}s: lon={:.3} m/s², mode={}, pitch_corr={:.1}°, conf={:.0}%", + (i + 1) as f32 * 0.005, + lon, + mode_str(&mode), + pitch_corr, + pitch_conf * 100.0 + ); + } + } + println!("\n CRUISE RESULTS:"); + println!(" Max |lon|: {:.3} m/s² (should be < 0.5)", max_lon); + println!( + " Mode counts: IDLE={}, ACCEL={}, BRAKE={}", + idle_count, accel_count, brake_count + ); + if brake_count > 0 || accel_count > 0 { + println!(" ⚠️ WARNING: False mode detections during cruise!"); + } else { + println!(" ✓ PASS: No false detections during cruise"); + } + println!(); + + // Phase 4: Cornering (3 seconds, 0.4g lateral at cruise speed) + println!("Phase 4: CORNERING (3s at 0.4g lateral)"); + println!(" Testing roll correction learning with simulated AHRS roll error"); + let turn_yaw_rate = 0.4 * G / cruise_speed; // yaw_rate = a_lat / v + let mut corner_count = 0; + let mut yaw = 0.0f32; + + for i in 0..600 { + if i % 8 == 0 { + fusion.process_gps(cruise_speed, 7.0 + (i as f32) * 0.005); + fusion.process_gps_heading(yaw); + } + + // AHRS reports false roll during cornering (similar to pitch error) + let lat_accel = cruise_speed * turn_yaw_rate; // Centripetal + let false_roll = (lat_accel / G).atan().to_degrees(); + yaw += turn_yaw_rate * 0.005; + + let ax_noise = noise.next(0.05 * G); + let ay_noise = noise.next(0.05 * G); + + let (lon, lat) = fusion.process_imu( + ax_noise, + lat_accel + ay_noise, + G, // Lateral accel in body Y + false_roll, + 0.0, // AHRS reports false roll + yaw, + cruise_speed, + turn_yaw_rate, + 0.005, + false, + ); + mode_classifier.update_hybrid(lon, lat, turn_yaw_rate, cruise_speed); + let mode = mode_classifier.get_mode(); + + if mode.has_corner() { + corner_count += 1; + } + + if i % 150 == 149 { + let (_, roll_corr) = fusion.get_orientation_correction(); + let (_, roll_conf) = fusion.get_orientation_confidence(); + println!(" t={:.1}s: lat={:.2} m/s², mode={}, AHRS_roll={:.1}°, roll_corr={:.1}°, conf={:.0}%", + (i + 1) as f32 * 0.005, lat, mode_str(&mode), false_roll, roll_corr, roll_conf * 100.0); + } + } + println!( + " CORNER detection rate: {:.0}%\n", + 100.0 * corner_count as f32 / 600.0 + ); + + // Phase 5: Braking (2 seconds, -0.4g) + println!("Phase 5: BRAKING (2s at -0.4g = -3.92 m/s²)"); + println!(" AHRS reports false pitch forward (opposite of acceleration)"); + for i in 0..400 { + let accel = -3.92; // -0.4g braking + speed = (speed + accel * 0.005).max(0.0); + + if i % 8 == 0 { + fusion.process_gps(speed, 10.0 + (i as f32) * 0.005); + } + + // AHRS reports false pitch forward during braking + let false_pitch = (accel / G).atan().to_degrees(); + let ax_noise = noise.next(0.05 * G); + let ay_noise = noise.next(0.05 * G); + + let (lon, lat) = fusion.process_imu( + accel + ax_noise, + ay_noise, + G, + 0.0, + false_pitch, + yaw, + speed, + 0.0, + 0.005, + false, + ); + mode_classifier.update_hybrid(lon, lat, 0.0, speed); + let mode = mode_classifier.get_mode(); + + if i % 100 == 99 { + println!( + " t={:.1}s: speed={:.1} m/s, lon={:.2} m/s², mode={}, AHRS_pitch={:.1}°", + (i + 1) as f32 * 0.005, + speed, + lon, + mode_str(&mode), + false_pitch + ); + } + } + println!(); + + // Phase 6: Stopped + println!("Phase 6: STOPPED"); + for i in 0..200 { + let ax_noise = noise.next(0.02 * G); + let ay_noise = noise.next(0.02 * G); + + let (lon, lat) = + fusion.process_imu(ax_noise, ay_noise, G, 0.0, 0.0, yaw, 0.0, 0.0, 0.005, true); + mode_classifier.update_hybrid(lon, lat, 0.0, 0.0); + let mode = mode_classifier.get_mode(); + + if i == 199 { + let (pitch_corr, roll_corr) = fusion.get_orientation_correction(); + let (pitch_conf, roll_conf) = fusion.get_orientation_confidence(); + println!(" Final: lon={:.3} m/s², mode={}", lon, mode_str(&mode)); + println!( + " Final orientation correction: pitch={:.1}° ({:.0}%), roll={:.1}° ({:.0}%)", + pitch_corr, + pitch_conf * 100.0, + roll_corr, + roll_conf * 100.0 + ); + } + } + + println!("\n=== Simulation Complete ==="); + println!("\nSummary:"); + println!( + "- WT901 AHRS error was simulated: false pitch during accel, false roll during cornering" + ); + println!("- OrientationCorrector learned to compensate for these errors"); + println!("- Corrected IMU provides accurate 200 Hz acceleration despite AHRS errors"); + println!("- GPS is used for ground truth during learning and as confidence-based fallback"); +} + +fn mode_str(mode: &sensor_fusion::Mode) -> &'static str { + if mode.has_accel() && mode.has_corner() { + "ACCEL+CORNER" + } else if mode.has_brake() && mode.has_corner() { + "BRAKE+CORNER" + } else if mode.has_accel() { + "ACCEL" + } else if mode.has_brake() { + "BRAKE" + } else if mode.has_corner() { + "CORNER" + } else { + "IDLE" + } +} diff --git a/framework/src/filter.rs b/framework/src/filter.rs new file mode 100644 index 0000000..abda75d --- /dev/null +++ b/framework/src/filter.rs @@ -0,0 +1,235 @@ +//! Signal filtering utilities +//! +//! Contains Biquad IIR filter implementation for vibration removal. +//! Used by SensorFusion to remove engine vibration (20-100Hz) from +//! IMU longitudinal acceleration while preserving driving dynamics (0-3Hz). +//! +//! Based on research: +//! - ArduPilot uses 10Hz low-pass on accelerometers (outer loop) +//! - Academic papers show 1-5Hz Butterworth effective for vehicle dynamics +//! - Physics: driving events (braking, acceleration, cornering) are < 3Hz + +/// Biquad IIR low-pass filter for vibration removal +/// +/// Implements a 2nd-order Butterworth low-pass filter optimized for +/// removing engine/road vibration (10-100Hz) while preserving +/// driving dynamics (0-3Hz). +/// +/// Design: Butterworth (maximally flat passband, Q = 0.707) +/// Typical cutoff: 4Hz at 200Hz sample rate +#[derive(Clone)] +pub struct BiquadFilter { + // Filter coefficients (normalized) + b0: f32, + b1: f32, + b2: f32, + a1: f32, + a2: f32, + + // Filter state (previous samples) + x1: f32, // x[n-1] + x2: f32, // x[n-2] + y1: f32, // y[n-1] + y2: f32, // y[n-2] +} + +impl BiquadFilter { + /// Create a new Butterworth low-pass filter + /// + /// # Arguments + /// * `cutoff_hz` - Cutoff frequency in Hz (e.g., 4.0) + /// * `sample_rate_hz` - Sample rate in Hz (e.g., 200.0) + /// + /// # Panics + /// Panics if cutoff >= sample_rate/2 (Nyquist limit) + pub fn new_lowpass(cutoff_hz: f32, sample_rate_hz: f32) -> Self { + assert!( + cutoff_hz < sample_rate_hz / 2.0, + "Cutoff must be below Nyquist frequency" + ); + + // Butterworth Q factor for critically damped response + let q = core::f32::consts::FRAC_1_SQRT_2; // 0.7071... + + // Pre-warp the cutoff frequency for bilinear transform + let omega = 2.0 * core::f32::consts::PI * cutoff_hz / sample_rate_hz; + let sin_omega = omega.sin(); + let cos_omega = omega.cos(); + let alpha = sin_omega / (2.0 * q); + + // Compute coefficients (lowpass) + let b0 = (1.0 - cos_omega) / 2.0; + let b1 = 1.0 - cos_omega; + let b2 = (1.0 - cos_omega) / 2.0; + let a0 = 1.0 + alpha; + let a1 = -2.0 * cos_omega; + let a2 = 1.0 - alpha; + + // Normalize by a0 + Self { + b0: b0 / a0, + b1: b1 / a0, + b2: b2 / a0, + a1: a1 / a0, + a2: a2 / a0, + x1: 0.0, + x2: 0.0, + y1: 0.0, + y2: 0.0, + } + } + + /// Process a single sample through the filter + /// + /// # Arguments + /// * `input` - Input sample + /// + /// # Returns + /// Filtered output sample + pub fn process(&mut self, input: f32) -> f32 { + // Direct Form I implementation + // y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2] + let output = self.b0 * input + self.b1 * self.x1 + self.b2 * self.x2 + - self.a1 * self.y1 + - self.a2 * self.y2; + + // Update state + self.x2 = self.x1; + self.x1 = input; + self.y2 = self.y1; + self.y1 = output; + + output + } + + /// Initialize filter state to a steady value + /// + /// Use this to avoid startup transients when you know + /// the initial value (e.g., after calibration shows ~0) + pub fn reset_to(&mut self, value: f32) { + self.x1 = value; + self.x2 = value; + self.y1 = value; + self.y2 = value; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const SAMPLE_RATE: f32 = 200.0; + const CUTOFF: f32 = 4.0; + + #[test] + fn test_filter_creation() { + let filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + assert!(filter.b0 > 0.0 && filter.b0 < 1.0); + assert!(filter.b1 > 0.0); + assert!(filter.b2 > 0.0 && filter.b2 < 1.0); + } + + #[test] + #[should_panic(expected = "Cutoff must be below Nyquist")] + fn test_filter_nyquist_limit() { + let _ = BiquadFilter::new_lowpass(100.0, SAMPLE_RATE); + } + + #[test] + fn test_dc_passthrough() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + for _ in 0..100 { + filter.process(1.0); + } + + let output = filter.process(1.0); + assert!( + (output - 1.0).abs() < 0.01, + "DC should pass through: got {}", + output + ); + } + + #[test] + fn test_high_frequency_attenuation() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let freq = 50.0; + let mut max_output: f32 = 0.0; + + for i in 0..1000 { + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 500 { + max_output = max_output.max(output.abs()); + } + } + + assert!( + max_output < 0.05, + "50Hz should be attenuated: got peak {}", + max_output + ); + } + + #[test] + fn test_low_frequency_passthrough() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let freq = 1.0; + let mut max_output: f32 = 0.0; + + for i in 0..2000 { + let t = i as f32 / SAMPLE_RATE; + let input = (2.0 * core::f32::consts::PI * freq * t).sin(); + let output = filter.process(input); + + if i > 1000 { + max_output = max_output.max(output.abs()); + } + } + + assert!( + max_output > 0.9, + "1Hz should pass through: got peak {}", + max_output + ); + } + + #[test] + fn test_step_response_minimal_overshoot() { + // Butterworth filters have minimal overshoot (Q = 0.707) + // In practice, slight overshoot ~5% is acceptable + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + let mut max_output: f32 = 0.0; + + for _ in 0..500 { + let output = filter.process(1.0); + max_output = max_output.max(output); + } + + assert!( + max_output <= 1.10, + "Should have minimal overshoot (<10%): got max {}", + max_output + ); + } + + #[test] + fn test_reset_to_value() { + let mut filter = BiquadFilter::new_lowpass(CUTOFF, SAMPLE_RATE); + + filter.reset_to(0.5); + + let output = filter.process(0.5); + assert!( + (output - 0.5).abs() < 0.01, + "After reset_to, should output steady value: got {}", + output + ); + } +} diff --git a/framework/src/fusion.rs b/framework/src/fusion.rs new file mode 100644 index 0000000..f110dc4 --- /dev/null +++ b/framework/src/fusion.rs @@ -0,0 +1,2064 @@ +//! Sensor Fusion Module +//! +//! This module handles: +//! 1. GPS-derived longitudinal acceleration (from velocity changes) +//! 2. GPS-corrected orientation for accurate IMU acceleration +//! 3. Butterworth low-pass filtering to remove engine vibration +//! 4. Dynamic tilt offset learning (ZUPT enhancement) +//! 5. GPS heading-based yaw rate calibration +//! +//! The goal is to provide clean, drift-free acceleration for mode detection +//! that works regardless of device mounting angle. +//! +//! ## Key Innovation: GPS-Corrected Orientation +//! +//! The WT901 AHRS cannot distinguish linear acceleration from tilt. During +//! forward acceleration, it reports false pitch. This corrupts gravity removal +//! and produces wrong earth-frame acceleration. +//! +//! Solution: Use GPS velocity (ground truth) to learn orientation corrections. +//! - Compare IMU-predicted accel with GPS-derived accel +//! - The difference reveals orientation error: pitch_error ≈ (ax_imu - ax_gps) / G +//! - Apply corrections BEFORE gravity removal +//! +//! This enables accurate 200 Hz IMU data instead of being limited to GPS rate. +//! +//! ## Vibration Filtering +//! +//! Engine vibration (20-100+ Hz) is removed using a 2nd-order Butterworth +//! low-pass filter at 10 Hz. This preserves driving dynamics (0-5 Hz) while +//! eliminating noise from engine, alternator, road surface, etc. + +use crate::filter::BiquadFilter; +use crate::transforms::{body_to_earth, earth_to_car, remove_gravity}; + +const G: f32 = 9.80665; + +/// Orientation Corrector - learns pitch/roll corrections from GPS velocity +/// +/// The WT901 AHRS can't distinguish linear acceleration from tilt. During +/// acceleration, it reports wrong pitch/roll, which corrupts gravity removal. +/// +/// This corrector compares IMU-derived acceleration (using AHRS angles) with +/// GPS-derived acceleration (ground truth) to learn orientation corrections. +/// +/// ## How it works +/// 1. Compute IMU acceleration using AHRS + current correction +/// 2. Compare with GPS-derived acceleration (dv/dt) +/// 3. The difference reveals orientation error: pitch_error ≈ (ax_imu - ax_gps) / G +/// 4. Apply EMA filter to learn smooth correction +/// 5. Only learn when vehicle is accelerating (signal present) and GPS is valid +/// +/// ## Why this works +/// - During acceleration, pitch error causes ax_earth to be wrong +/// - GPS velocity gives us true horizontal acceleration +/// - The innovation (difference) is directly proportional to pitch error +pub struct OrientationCorrector { + /// Pitch correction in radians (added to AHRS pitch before gravity removal) + pitch_correction: f32, + /// Roll correction in radians (added to AHRS roll before gravity removal) + roll_correction: f32, + /// Confidence in pitch correction (0-1, based on learning samples) + pitch_confidence: f32, + /// Confidence in roll correction (0-1) + roll_confidence: f32, + /// Cruise bias - offset learned when driving at constant speed (m/s²) + /// This catches mounting offsets that show up during cruise but not at stops + cruise_bias: f32, + /// Cruise bias accumulator for averaging + cruise_bias_sum: f32, + /// Cruise bias sample count + cruise_bias_count: u32, + /// Time spent in cruise state (seconds) + cruise_time: f32, + /// EMA alpha for learning (smaller = slower but more stable) + alpha: f32, + /// Minimum speed for learning (m/s) - need motion for GPS accuracy + min_speed: f32, + /// Minimum acceleration to trigger pitch/roll learning (m/s²) + /// Below this threshold, cruise bias learning kicks in instead + min_accel: f32, + /// Maximum correction magnitude (radians) - safety cap + max_correction: f32, + /// Number of learning updates (for confidence calculation) + update_count: u32, +} + +impl OrientationCorrector { + pub fn new() -> Self { + Self { + pitch_correction: 0.0, + roll_correction: 0.0, + pitch_confidence: 0.0, + roll_confidence: 0.0, + cruise_bias: 0.0, + cruise_bias_sum: 0.0, + cruise_bias_count: 0, + cruise_time: 0.0, + alpha: 0.05, // Slow learning for stability + min_speed: 3.0, // ~11 km/h + min_accel: 0.3, // ~0.03g - lowered to learn from more events + max_correction: 0.26, // ~15 degrees max + update_count: 0, + } + } + + /// Update orientation corrections based on GPS vs IMU comparison + /// + /// # Arguments + /// * `lon_imu` - IMU-derived longitudinal accel (using AHRS + current correction) + /// * `lon_gps` - GPS-derived longitudinal accel (ground truth) + /// * `lat_imu` - IMU-derived lateral accel + /// * `lat_gps` - GPS-derived lateral accel (from centripetal: speed × yaw_rate) + /// * `speed` - Vehicle speed in m/s + /// * `is_gps_fresh` - Whether GPS data is recent and valid + /// * `dt` - Time step in seconds (for cruise learning timing) + #[allow(clippy::too_many_arguments)] + pub fn update( + &mut self, + lon_imu: f32, + lon_gps: f32, + lat_imu: f32, + lat_gps: f32, + speed: f32, + is_gps_fresh: bool, + dt: f32, + ) { + // Only learn when conditions are good + if speed < self.min_speed || !is_gps_fresh { + self.cruise_time = 0.0; // Reset cruise accumulator + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + return; + } + + // Apply current cruise bias before comparison + let lon_imu_corrected = lon_imu - self.cruise_bias; + let lon_error = lon_imu_corrected - lon_gps; + + // Learn pitch correction from longitudinal acceleration difference + // During acceleration: pitch error → wrong ax_earth + if lon_gps.abs() > self.min_accel { + // Signal present - can learn pitch + let pitch_innovation = lon_error / G; + + // EMA update with clamping + self.pitch_correction = + (1.0 - self.alpha) * self.pitch_correction + self.alpha * pitch_innovation; + self.pitch_correction = self + .pitch_correction + .clamp(-self.max_correction, self.max_correction); + + // Update confidence + self.update_count = self.update_count.saturating_add(1); + self.pitch_confidence = (self.update_count as f32 / 100.0).min(1.0); + + // Reset cruise accumulator when accelerating + self.cruise_time = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + } else { + // Cruising (lon_gps near zero) - learn cruise bias + // If GPS says ~0 accel but IMU shows offset, that's mounting bias + self.cruise_time += dt; + self.cruise_bias_sum += lon_imu_corrected; // Should be ~0 when cruising + self.cruise_bias_count += 1; + + // After 2 seconds of stable cruising, update cruise bias + if self.cruise_time >= 2.0 && self.cruise_bias_count > 30 { + let measured_bias = self.cruise_bias_sum / self.cruise_bias_count as f32; + + // EMA update for cruise bias (slower alpha for stability) + self.cruise_bias = 0.9 * self.cruise_bias + 0.1 * measured_bias; + // Clamp cruise bias to reasonable range (±1.5 m/s² = ±0.15g) + self.cruise_bias = self.cruise_bias.clamp(-1.5, 1.5); + + // Reset accumulator + self.cruise_time = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + } + } + + // Learn roll correction from lateral acceleration difference + // During cornering: roll error → wrong ay_earth + let lat_error = lat_imu - lat_gps; + if lat_gps.abs() > self.min_accel { + let roll_innovation = lat_error / G; + + self.roll_correction = + (1.0 - self.alpha) * self.roll_correction + self.alpha * roll_innovation; + self.roll_correction = self + .roll_correction + .clamp(-self.max_correction, self.max_correction); + + self.roll_confidence = (self.update_count as f32 / 100.0).min(1.0); + } + } + + /// Get current cruise bias (m/s²) for diagnostics + pub fn get_cruise_bias(&self) -> f32 { + self.cruise_bias + } + + /// Apply corrections to AHRS orientation + /// + /// Returns corrected (pitch_deg, roll_deg) for use in gravity removal + pub fn correct(&self, ahrs_pitch_deg: f32, ahrs_roll_deg: f32) -> (f32, f32) { + // Convert corrections from radians to degrees + let pitch_corr_deg = self.pitch_correction.to_degrees(); + let roll_corr_deg = self.roll_correction.to_degrees(); + + ( + ahrs_pitch_deg - pitch_corr_deg, + ahrs_roll_deg - roll_corr_deg, + ) + } + + /// Get current pitch correction in degrees (for diagnostics) + pub fn get_pitch_correction_deg(&self) -> f32 { + self.pitch_correction.to_degrees() + } + + /// Get current roll correction in degrees (for diagnostics) + pub fn get_roll_correction_deg(&self) -> f32 { + self.roll_correction.to_degrees() + } + + /// Get pitch confidence (0-1) + pub fn get_pitch_confidence(&self) -> f32 { + self.pitch_confidence + } + + /// Get roll confidence (0-1) + pub fn get_roll_confidence(&self) -> f32 { + self.roll_confidence + } + + /// Reset corrections (e.g., after device remount) + pub fn reset(&mut self) { + self.pitch_correction = 0.0; + self.roll_correction = 0.0; + self.pitch_confidence = 0.0; + self.roll_confidence = 0.0; + self.cruise_bias = 0.0; + self.cruise_bias_sum = 0.0; + self.cruise_bias_count = 0; + self.cruise_time = 0.0; + self.update_count = 0; + } +} + +impl Default for OrientationCorrector { + fn default() -> Self { + Self::new() + } +} + +/// Configuration for sensor fusion +#[derive(Clone, Copy)] +pub struct FusionConfig { + /// GPS rate threshold for high-confidence GPS (Hz) + pub gps_high_rate: f32, + /// GPS rate threshold for medium-confidence GPS (Hz) + pub gps_medium_rate: f32, + /// Maximum age of GPS data before considered stale (seconds) + pub gps_max_age: f32, + /// GPS weight when rate >= high_rate (0.0-1.0) + /// Higher = more GPS, smoother but slower response + /// Lower = more IMU, faster response but potential drift + pub gps_weight_high: f32, + /// GPS weight when rate >= medium_rate (0.0-1.0) + pub gps_weight_medium: f32, + /// GPS weight when rate < medium_rate (0.0-1.0) + pub gps_weight_low: f32, + /// Time required at stop to learn tilt offset (seconds) + pub tilt_learn_time: f32, + /// Low-pass filter cutoff for IMU longitudinal (Hz) + /// Removes engine vibration (20-100Hz) while passing driving dynamics (0-3Hz) + /// Research: ArduPilot uses 10Hz, academic papers suggest 1-5Hz for vehicle dynamics + pub lon_filter_cutoff: f32, + /// Sample rate for longitudinal filter (Hz) - must match ACTUAL call rate of process_imu() + /// CRITICAL: This is telemetry rate (~30Hz), NOT IMU rate (200Hz)! + pub lon_sample_rate: f32, +} + +impl Default for FusionConfig { + /// Default configuration for mode detection. + /// + /// ## GPS-Corrected IMU Approach + /// + /// We use GPS velocity to correct AHRS orientation errors, then use the + /// corrected IMU at 200 Hz for mode detection. This gives us: + /// - 200 Hz update rate (vs 25 Hz GPS-only) + /// - Accurate acceleration even during dynamic maneuvers + /// - Foundation for future granular detection (soft/medium/hard accel) + /// + /// The OrientationCorrector learns pitch/roll errors by comparing IMU-predicted + /// acceleration with GPS-derived acceleration (ground truth). + /// + /// GPS weight fields control blending between corrected IMU and GPS acceleration + /// for robustness during correction learning phase. + fn default() -> Self { + Self { + gps_high_rate: 20.0, + gps_medium_rate: 10.0, + gps_max_age: 0.2, // 200ms - GPS older than this is considered stale + // GPS weight fields - blend GPS with corrected IMU based on confidence + gps_weight_high: 0.3, // 30% GPS, 70% corrected IMU when confident + gps_weight_medium: 0.5, // 50/50 blend at medium confidence + gps_weight_low: 0.8, // 80% GPS when correction not yet learned + tilt_learn_time: 3.0, + // Butterworth filter for IMU + // 10 Hz cutoff: passes driving dynamics, removes vibration + lon_filter_cutoff: 10.0, + // CRITICAL: Must match actual process_imu() call rate (telemetry rate ~30Hz) + // NOT the IMU hardware rate (200Hz)! + lon_sample_rate: 30.0, + } + } +} + +/// GPS-derived acceleration tracker +pub struct GpsAcceleration { + /// Previous speed (m/s) + prev_speed: f32, + /// Previous timestamp (seconds, monotonic) + prev_time: f32, + /// Computed longitudinal acceleration (m/s²) + accel_lon: f32, + /// Is the acceleration estimate valid? + valid: bool, + /// Current GPS rate estimate (Hz) + gps_rate: f32, + /// Time since last valid GPS fix (seconds) + time_since_fix: f32, + /// EMA for GPS rate calculation + rate_ema: f32, + /// Last fix count for rate calculation + last_fix_count: u32, + /// Time of last rate calculation + last_rate_time: f32, +} + +impl GpsAcceleration { + pub fn new() -> Self { + Self { + prev_speed: 0.0, + prev_time: 0.0, + accel_lon: 0.0, + valid: false, + gps_rate: 0.0, + time_since_fix: 999.0, + rate_ema: 0.0, + last_fix_count: 0, + last_rate_time: 0.0, + } + } + + /// Update with new GPS speed measurement + /// + /// # Arguments + /// * `speed` - Current speed in m/s + /// * `time` - Current timestamp in seconds (monotonic) + /// + /// # Returns + /// The computed longitudinal acceleration, or None if not enough data + pub fn update(&mut self, speed: f32, time: f32) -> Option { + self.time_since_fix = 0.0; + + if self.prev_time > 0.0 { + let dt = time - self.prev_time; + + // Validate dt: should be 20-200ms for 5-50Hz GPS + if dt > 0.02 && dt < 0.5 { + self.accel_lon = (speed - self.prev_speed) / dt; + self.valid = true; + + self.prev_speed = speed; + self.prev_time = time; + + return Some(self.accel_lon); + } + } + + self.prev_speed = speed; + self.prev_time = time; + None + } + + /// Update GPS rate estimate + /// + /// # Arguments + /// * `fix_count` - Total number of valid fixes received + /// * `time` - Current timestamp in seconds + pub fn update_rate(&mut self, fix_count: u32, time: f32) { + if self.last_rate_time > 0.0 { + let dt = time - self.last_rate_time; + if dt >= 1.0 { + let fixes = fix_count.saturating_sub(self.last_fix_count); + let instant_rate = fixes as f32 / dt; + + // EMA smoothing for rate + self.rate_ema = 0.3 * instant_rate + 0.7 * self.rate_ema; + self.gps_rate = self.rate_ema; + + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } else { + self.last_fix_count = fix_count; + self.last_rate_time = time; + } + } + + /// Advance time without GPS fix (for staleness tracking) + pub fn advance_time(&mut self, dt: f32) { + self.time_since_fix += dt; + if self.time_since_fix > 0.5 { + self.valid = false; + } + } + + /// Get current GPS-derived acceleration + pub fn get_accel(&self) -> Option { + if self.valid { + Some(self.accel_lon) + } else { + None + } + } + + /// Get current GPS rate estimate (Hz) + pub fn get_rate(&self) -> f32 { + self.gps_rate + } + + /// Check if GPS data is fresh + pub fn is_fresh(&self, max_age: f32) -> bool { + self.time_since_fix < max_age + } +} + +impl Default for GpsAcceleration { + fn default() -> Self { + Self::new() + } +} + +/// Tilt offset estimator - learns mounting offset when stopped +pub struct TiltEstimator { + /// Accumulated X acceleration (earth frame) + accel_x_sum: f32, + /// Accumulated Y acceleration (earth frame) + accel_y_sum: f32, + /// Number of samples accumulated + sample_count: u32, + /// Time stationary (seconds) + stationary_time: f32, + /// Learned tilt offset X (m/s²) + offset_x: f32, + /// Learned tilt offset Y (m/s²) + offset_y: f32, + /// Is the offset valid (learned at least once)? + offset_valid: bool, + /// Required stationary time to learn (seconds) + learn_time: f32, +} + +impl TiltEstimator { + pub fn new(learn_time: f32) -> Self { + Self { + accel_x_sum: 0.0, + accel_y_sum: 0.0, + sample_count: 0, + stationary_time: 0.0, + offset_x: 0.0, + offset_y: 0.0, + offset_valid: false, + learn_time, + } + } + + /// Update with acceleration sample while stationary + /// + /// # Arguments + /// * `ax_earth` - X acceleration in earth frame (m/s²) + /// * `ay_earth` - Y acceleration in earth frame (m/s²) + /// * `dt` - Time step (seconds) + /// + /// # Returns + /// true if offset was just updated (can trigger visual feedback) + pub fn update_stationary(&mut self, ax_earth: f32, ay_earth: f32, dt: f32) -> bool { + self.stationary_time += dt; + self.accel_x_sum += ax_earth; + self.accel_y_sum += ay_earth; + self.sample_count += 1; + + // After sufficient time, compute offset + if self.stationary_time >= self.learn_time && self.sample_count > 50 { + let new_offset_x = self.accel_x_sum / self.sample_count as f32; + let new_offset_y = self.accel_y_sum / self.sample_count as f32; + + // Only update if change is significant (avoid jitter) + let change = ((new_offset_x - self.offset_x).powi(2) + + (new_offset_y - self.offset_y).powi(2)) + .sqrt(); + + if change > 0.05 || !self.offset_valid { + self.offset_x = new_offset_x; + self.offset_y = new_offset_y; + self.offset_valid = true; + + // Reset accumulators for continued learning + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + + return true; + } + } + + false + } + + /// Reset stationary tracking (called when vehicle starts moving) + pub fn reset_stationary(&mut self) { + self.accel_x_sum = 0.0; + self.accel_y_sum = 0.0; + self.sample_count = 0; + self.stationary_time = 0.0; + } + + /// Apply tilt correction to acceleration + pub fn correct(&self, ax_earth: f32, ay_earth: f32) -> (f32, f32) { + if self.offset_valid { + (ax_earth - self.offset_x, ay_earth - self.offset_y) + } else { + (ax_earth, ay_earth) + } + } + + /// Get learned offsets and validity (for diagnostics) + pub fn get_offsets(&self) -> (f32, f32, bool) { + (self.offset_x, self.offset_y, self.offset_valid) + } +} + +/// Yaw rate calibrator using GPS heading +/// +/// When driving straight on highway (stable GPS course), the true yaw rate +/// should be ~0. Any measured yaw rate from the gyro is bias that we can +/// learn and subtract. This prevents lateral drift in centripetal calculation. +pub struct YawRateCalibrator { + /// Learned yaw rate bias (rad/s) + bias: f32, + /// Is bias estimate valid? + valid: bool, + /// Heading history for stability detection (radians) + heading_history: [f32; 20], + heading_idx: usize, + heading_count: u32, + /// Time driving straight (seconds) + straight_time: f32, + /// Accumulated yaw rate samples + yaw_sum: f32, + sample_count: u32, + /// Configuration + min_speed: f32, // m/s, minimum speed for calibration (~29 km/h) + heading_tolerance: f32, // radians, max heading deviation for "straight" (~2°) + learn_time: f32, // seconds, time required to update bias + alpha: f32, // EMA alpha for bias update +} + +impl YawRateCalibrator { + pub fn new() -> Self { + Self { + bias: 0.0, + valid: false, + heading_history: [0.0; 20], + heading_idx: 0, + heading_count: 0, + straight_time: 0.0, + yaw_sum: 0.0, + sample_count: 0, + min_speed: 8.0, // ~29 km/h + heading_tolerance: 0.035, // ~2 degrees + learn_time: 3.0, // 3 seconds of straight driving + alpha: 0.1, // Slow update for stability + } + } + + /// Update heading history with new GPS course + /// Call this at GPS rate (5-25 Hz) + pub fn update_heading(&mut self, heading: f32) { + self.heading_history[self.heading_idx] = heading; + self.heading_idx = (self.heading_idx + 1) % self.heading_history.len(); + if self.heading_count < self.heading_history.len() as u32 { + self.heading_count += 1; + } + } + + /// Update with gyro yaw rate and check for calibration opportunity + /// Call this at telemetry rate (20 Hz) + /// + /// # Returns + /// true if bias was updated + pub fn update(&mut self, yaw_rate: f32, speed: f32, dt: f32) -> bool { + // Need minimum speed and enough heading history + if speed < self.min_speed || self.heading_count < 10 { + self.reset_accumulator(); + return false; + } + + // Check if heading is stable (driving straight) + if !self.is_heading_stable() { + self.reset_accumulator(); + return false; + } + + // Accumulate yaw rate samples + self.straight_time += dt; + self.yaw_sum += yaw_rate; + self.sample_count += 1; + + // After sufficient time, learn bias + if self.straight_time >= self.learn_time && self.sample_count > 50 { + let measured_bias = self.yaw_sum / self.sample_count as f32; + + if self.valid { + // EMA update for smooth correction + self.bias = (1.0 - self.alpha) * self.bias + self.alpha * measured_bias; + } else { + self.bias = measured_bias; + self.valid = true; + } + + self.reset_accumulator(); + return true; + } + + false + } + + fn reset_accumulator(&mut self) { + self.straight_time = 0.0; + self.yaw_sum = 0.0; + self.sample_count = 0; + } + + fn is_heading_stable(&self) -> bool { + if self.heading_count < 10 { + return false; + } + + let count = self.heading_count as usize; + + // Compute circular mean (headings wrap around at ±π) + let mut sin_sum = 0.0f32; + let mut cos_sum = 0.0f32; + for &h in &self.heading_history[..count] { + sin_sum += h.sin(); + cos_sum += h.cos(); + } + let mean_heading = sin_sum.atan2(cos_sum); + + // Check all headings are within tolerance of mean + for &h in &self.heading_history[..count] { + let diff = angle_diff(h, mean_heading); + if diff.abs() > self.heading_tolerance { + return false; + } + } + + true + } + + /// Apply bias correction to yaw rate + pub fn correct(&self, yaw_rate: f32) -> f32 { + if self.valid { + yaw_rate - self.bias + } else { + yaw_rate + } + } + + /// Get current bias estimate (rad/s) + pub fn get_bias(&self) -> f32 { + self.bias + } + + /// Is bias calibration valid? + pub fn is_valid(&self) -> bool { + self.valid + } +} + +impl Default for YawRateCalibrator { + fn default() -> Self { + Self::new() + } +} + +/// Compute angular difference handling wrap-around at ±π +fn angle_diff(a: f32, b: f32) -> f32 { + let mut diff = a - b; + if diff > core::f32::consts::PI { + diff -= 2.0 * core::f32::consts::PI; + } else if diff < -core::f32::consts::PI { + diff += 2.0 * core::f32::consts::PI; + } + diff +} + +/// Main sensor fusion processor +pub struct SensorFusion { + pub config: FusionConfig, + /// GPS-derived acceleration + pub gps_accel: GpsAcceleration, + /// Orientation corrector (learns pitch/roll errors from GPS) + pub orientation_corrector: OrientationCorrector, + /// Tilt estimator (learns residual offset when stopped) + pub tilt_estimator: TiltEstimator, + /// Yaw rate calibrator (learns gyro bias while driving straight) + pub yaw_rate_calibrator: YawRateCalibrator, + /// Butterworth low-pass filter for IMU longitudinal (removes engine vibration) + lon_filter: BiquadFilter, + /// Blended longitudinal acceleration (m/s²) + blended_lon: f32, + /// Display longitudinal - GPS when fresh, otherwise filtered blended (m/s²) + lon_display: f32, + /// Raw IMU longitudinal (unfiltered, m/s², vehicle frame) + lon_imu_raw: f32, + /// Filtered IMU longitudinal (m/s², vehicle frame) + lon_imu_filtered: f32, + /// Corrected lateral acceleration for display (m/s², vehicle frame) + lat_corrected: f32, + /// Centripetal lateral acceleration for mode detection (m/s²) + lat_centripetal: f32, + /// IMU lateral (from corrected orientation) for learning + lat_imu: f32, + /// Last GPS blend weight (for diagnostics) + last_gps_weight: f32, + /// Was stationary last update? + was_stationary: bool, +} + +impl SensorFusion { + pub fn new(config: FusionConfig) -> Self { + Self { + gps_accel: GpsAcceleration::new(), + orientation_corrector: OrientationCorrector::new(), + tilt_estimator: TiltEstimator::new(config.tilt_learn_time), + yaw_rate_calibrator: YawRateCalibrator::new(), + lon_filter: BiquadFilter::new_lowpass(config.lon_filter_cutoff, config.lon_sample_rate), + blended_lon: 0.0, + lon_display: 0.0, + lon_imu_raw: 0.0, + lon_imu_filtered: 0.0, + lat_corrected: 0.0, + lat_centripetal: 0.0, + lat_imu: 0.0, + last_gps_weight: 0.0, + was_stationary: false, + config, + } + } + + /// Process IMU data with GPS-corrected orientation (call at IMU rate, e.g., 200Hz) + /// + /// This is the ArduPilot-style approach: use GPS velocity to correct AHRS + /// orientation errors, then use the corrected IMU for accurate 200 Hz acceleration. + /// + /// # Arguments + /// * `ax_raw` - Raw X acceleration in body frame (m/s²) + /// * `ay_raw` - Raw Y acceleration in body frame (m/s²) + /// * `az_raw` - Raw Z acceleration in body frame (m/s²) + /// * `ahrs_roll_deg` - AHRS roll angle in degrees + /// * `ahrs_pitch_deg` - AHRS pitch angle in degrees + /// * `yaw` - Vehicle yaw angle (rad, from EKF or magnetometer) + /// * `speed` - Vehicle speed (m/s) + /// * `yaw_rate` - Yaw rate (rad/s) + /// * `dt` - Time step (seconds) + /// * `is_stationary` - Whether vehicle is currently stationary + /// + /// # Returns + /// (lon_blended, lat_centripetal) - Accelerations for mode detection (m/s²) + /// lon_blended: orientation-corrected IMU blended with GPS + /// lat_centripetal: speed * yaw_rate (pro-style, mount-independent) + #[allow(clippy::too_many_arguments)] + pub fn process_imu( + &mut self, + ax_raw: f32, + ay_raw: f32, + az_raw: f32, + ahrs_roll_deg: f32, + ahrs_pitch_deg: f32, + yaw: f32, + speed: f32, + yaw_rate: f32, + dt: f32, + is_stationary: bool, + ) -> (f32, f32) { + // Track GPS staleness + self.gps_accel.advance_time(dt); + let gps_fresh = self.gps_accel.is_fresh(self.config.gps_max_age); + + // Step 1: Apply orientation correction (learned from GPS comparison) + // This corrects AHRS errors caused by linear acceleration being mistaken for tilt + let (corrected_pitch_deg, corrected_roll_deg) = self + .orientation_corrector + .correct(ahrs_pitch_deg, ahrs_roll_deg); + + // Step 2: Remove gravity using corrected orientation + let (ax_nograv, ay_nograv, _az_nograv) = remove_gravity( + ax_raw, + ay_raw, + az_raw, + corrected_roll_deg, + corrected_pitch_deg, + ); + + // Step 3: Transform to earth frame + let (ax_earth, ay_earth) = body_to_earth( + ax_nograv, + ay_nograv, + 0.0, + corrected_roll_deg, + corrected_pitch_deg, + yaw, + ); + + // Step 4: Apply residual tilt correction (learned when stopped) + // This handles minor mounting offsets not captured by orientation correction + let (ax_corr, ay_corr) = self.tilt_estimator.correct(ax_earth, ay_earth); + + // Step 5: Transform to vehicle frame + let (lon_imu_raw, lat_imu_raw) = earth_to_car(ax_corr, ay_corr, yaw); + + // Store values + self.lon_imu_raw = lon_imu_raw; + self.lat_imu = lat_imu_raw; + + // Step 6: Apply Butterworth low-pass filter to remove engine vibration + let lon_imu_filtered = self.lon_filter.process(lon_imu_raw); + self.lon_imu_filtered = lon_imu_filtered; + + // Store corrected lateral for display + self.lat_corrected = lat_imu_raw; + + // Step 7: Apply yaw rate calibration (removes gyro bias) + let yaw_rate_corrected = self.yaw_rate_calibrator.correct(yaw_rate); + + // Step 8: Calculate centripetal lateral for mode detection + // a_lateral = v * omega (mount-angle independent) + self.lat_centripetal = speed * yaw_rate_corrected; + + // Step 9: Handle stationary state - learn residual offsets + if is_stationary { + self.tilt_estimator + .update_stationary(ax_earth, ay_earth, dt); + } else { + if self.was_stationary { + self.tilt_estimator.reset_stationary(); + } + self.yaw_rate_calibrator.update(yaw_rate, speed, dt); + } + self.was_stationary = is_stationary; + + // Step 10: Get GPS-derived acceleration for comparison/blending + let gps_lon = self.gps_accel.get_accel(); + let gps_lat = self.lat_centripetal; // Centripetal = speed * yaw_rate + + // Step 11: Update orientation corrector (learn from GPS vs IMU) + // Only learn when moving and GPS is fresh + if !is_stationary && gps_fresh { + if let Some(lon_gps) = gps_lon { + self.orientation_corrector.update( + lon_imu_filtered, // IMU (using current correction) + lon_gps, // GPS (ground truth) + lat_imu_raw, // IMU lateral + gps_lat, // Centripetal (truth for lateral) + speed, + gps_fresh, + dt, // Time step for cruise bias learning + ); + } + } + + // Step 12: Blend IMU and GPS based on orientation correction confidence + // Higher confidence → trust corrected IMU more + // Lower confidence → rely more on GPS + let lon_blended = if let Some(lon_gps) = gps_lon { + // Compute blend weight based on correction confidence + let confidence = self.orientation_corrector.get_pitch_confidence(); + + // Interpolate between high/medium/low GPS weights based on confidence + let gps_weight = if confidence > 0.8 { + self.config.gps_weight_high // 30% GPS when very confident + } else if confidence > 0.3 { + // Linear interpolation between medium and high + let t = (confidence - 0.3) / 0.5; + self.config.gps_weight_medium * (1.0 - t) + self.config.gps_weight_high * t + } else { + // Linear interpolation between low and medium + let t = confidence / 0.3; + self.config.gps_weight_low * (1.0 - t) + self.config.gps_weight_medium * t + }; + + self.last_gps_weight = gps_weight; + + // Blend: (1-w)*IMU + w*GPS + (1.0 - gps_weight) * lon_imu_filtered + gps_weight * lon_gps + } else { + // GPS unavailable - use corrected IMU only + self.last_gps_weight = 0.0; + lon_imu_filtered + }; + + self.blended_lon = lon_blended; + self.lon_display = lon_blended; + + // Return blended longitudinal and centripetal lateral + (lon_blended, self.lat_centripetal) + } + + /// Process GPS speed update (call at GPS rate, e.g., 25Hz) + /// + /// # Arguments + /// * `speed` - GPS speed in m/s + /// * `time` - Monotonic timestamp in seconds + pub fn process_gps(&mut self, speed: f32, time: f32) { + self.gps_accel.update(speed, time); + } + + /// Process GPS heading update for yaw rate calibration + /// Call this at GPS rate when GPS is valid and speed > 0 + /// + /// # Arguments + /// * `heading` - Course over ground in radians + pub fn process_gps_heading(&mut self, heading: f32) { + self.yaw_rate_calibrator.update_heading(heading); + } + + /// Update GPS rate estimate (call periodically, e.g., every second) + pub fn update_gps_rate(&mut self, fix_count: u32, time: f32) { + self.gps_accel.update_rate(fix_count, time); + } + + /// Get display-optimized longitudinal acceleration (vehicle frame, m/s²) + /// Uses GPS-derived acceleration when fresh (no vibration noise) + /// Falls back to blended with heavier smoothing when GPS stale + /// Positive = forward acceleration + pub fn get_lon_display(&self) -> f32 { + self.lon_display + } + + /// Get blended longitudinal acceleration (vehicle frame, m/s²) + /// This is GPS/IMU blended - same value used by mode classification + /// Positive = forward acceleration + pub fn get_lon_blended(&self) -> f32 { + self.blended_lon + } + + /// Get corrected lateral acceleration for display (vehicle frame, m/s²) + /// This is accelerometer-based with tilt/gravity correction + /// Positive = left + /// NOTE: Consider using get_lat_centripetal() instead for responsive display + pub fn get_lat_display(&self) -> f32 { + self.lat_corrected + } + + /// Get centripetal lateral acceleration (vehicle frame, m/s²) + /// Computed as speed × yaw_rate - instant, mount-independent, doesn't stick + /// This is the same value used by mode detection + /// Positive = left turn + pub fn get_lat_centripetal(&self) -> f32 { + self.lat_centripetal + } + + // ========== Diagnostic getters ========== + + /// Get raw IMU longitudinal acceleration (before filter, m/s²) + pub fn get_lon_imu_raw(&self) -> f32 { + self.lon_imu_raw + } + + /// Get filtered IMU longitudinal acceleration (after Butterworth, m/s²) + pub fn get_lon_imu_filtered(&self) -> f32 { + self.lon_imu_filtered + } + + /// Get last GPS blend weight (0.0-1.0) + pub fn get_last_gps_weight(&self) -> f32 { + self.last_gps_weight + } + + /// Get GPS-derived acceleration (m/s²), or NaN if invalid + pub fn get_gps_accel(&self) -> f32 { + self.gps_accel.get_accel().unwrap_or(f32::NAN) + } + + /// Get GPS rate estimate (Hz) + pub fn get_gps_rate(&self) -> f32 { + self.gps_accel.get_rate() + } + + /// Check if GPS was rejected by validity check + /// Returns true if GPS showed ~0 but IMU had signal + pub fn is_gps_rejected(&self) -> bool { + // If we have valid GPS but weight is 0, GPS was rejected + self.gps_accel.get_accel().is_some() && self.last_gps_weight < 0.01 + } + + /// Get tilt estimator offsets and validity + pub fn get_tilt_offsets(&self) -> (f32, f32, bool) { + self.tilt_estimator.get_offsets() + } + + /// Get orientation correction (pitch, roll) in degrees + pub fn get_orientation_correction(&self) -> (f32, f32) { + ( + self.orientation_corrector.get_pitch_correction_deg(), + self.orientation_corrector.get_roll_correction_deg(), + ) + } + + /// Get orientation correction confidence (pitch, roll) as 0-1 values + pub fn get_orientation_confidence(&self) -> (f32, f32) { + ( + self.orientation_corrector.get_pitch_confidence(), + self.orientation_corrector.get_roll_confidence(), + ) + } + + /// Reset orientation corrector (call after device remount) + pub fn reset_orientation_corrector(&mut self) { + self.orientation_corrector.reset(); + } +} + +impl Default for SensorFusion { + fn default() -> Self { + Self::new(FusionConfig::default()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_gps_acceleration_basic() { + let mut gps = GpsAcceleration::new(); + + // First update - no acceleration yet (use non-zero time) + assert!(gps.update(10.0, 1.0).is_none()); + + // Second update - should compute acceleration + let accel = gps.update(12.0, 1.1); // 2 m/s increase over 0.1s = 20 m/s² + assert!(accel.is_some()); + assert!((accel.unwrap() - 20.0).abs() < 0.1); + } + + #[test] + fn test_gps_acceleration_decel() { + let mut gps = GpsAcceleration::new(); + + // Use non-zero times (prev_time > 0.0 check in code) + gps.update(20.0, 1.0); + let accel = gps.update(15.0, 1.1); // 5 m/s decrease = -50 m/s² + + assert!(accel.is_some()); + assert!((accel.unwrap() + 50.0).abs() < 0.1); + } + + #[test] + fn test_gps_staleness() { + let mut gps = GpsAcceleration::new(); + let config = FusionConfig::default(); + + // Use non-zero times + gps.update(10.0, 1.0); + gps.update(12.0, 1.1); + + assert!(gps.is_fresh(config.gps_max_age)); + assert!(gps.get_accel().is_some()); + + // Advance time without GPS update - exceeds freshness threshold + gps.advance_time(0.3); // > 200ms (gps_max_age) + assert!(!gps.is_fresh(config.gps_max_age)); + + // Advance more time - exceeds validity threshold (0.5s internal) + gps.advance_time(0.3); // total 0.6s > 0.5s + assert!(gps.get_accel().is_none()); + } + + #[test] + fn test_tilt_estimator_learns_and_corrects() { + let mut tilt = TiltEstimator::new(1.0); // 1 second learn time + + // Simulate stationary with 0.5 m/s² offset (tilted mount) + for _ in 0..300 { + // 1.5 seconds at 200Hz + tilt.update_stationary(0.5, 0.3, 0.005); + } + + // Correction should now reduce the offset to near zero + let (ax, ay) = tilt.correct(0.5, 0.3); + assert!(ax.abs() < 0.1, "Corrected X should be ~0: {}", ax); + assert!(ay.abs() < 0.1, "Corrected Y should be ~0: {}", ay); + } + + #[test] + fn test_centripetal_lateral_during_turn() { + // Simulate a turn: speed = 10 m/s, yaw_rate = 0.3 rad/s + // Expected centripetal: 10 * 0.3 = 3 m/s² + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // New API: pass raw body-frame accel + // For level surface: ax_raw = earth_x, ay_raw = earth_y, az_raw = G + let (_lon, lat) = fusion.process_imu( + 0.0, // ax_raw (no forward accel) + 0.0, // ay_raw (no lateral accel) + G, // az_raw (gravity) + 0.0, // ahrs_roll_deg (level) + 0.0, // ahrs_pitch_deg (level) + 0.0, // yaw (heading) + 10.0, // Speed = 10 m/s + 0.3, // Yaw rate = 0.3 rad/s (turning) + 0.05, // dt + false, // Not stationary + ); + + // Lateral should be centripetal: speed * yaw_rate = 3 m/s² + assert!( + (lat - 3.0).abs() < 0.01, + "Centripetal lateral should be 3.0: got {}", + lat + ); + } + + #[test] + fn test_lateral_returns_to_zero_after_turn() { + // This test verifies the critical bug fix: + // After a turn ends, lateral should return to zero immediately + // (not "stick" due to earth-frame filtering) + + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Simulate a 90° turn + let mut yaw = 0.0; + for i in 0..20 { + // 1 second of turning at 20Hz + let yaw_rate = 1.57; // ~90°/s + yaw += yaw_rate * 0.05; + + let (_lon, lat) = fusion.process_imu( + 0.0, // ax_raw + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch + yaw, // Changing heading + 10.0, // Constant speed + yaw_rate, // Turning + 0.05, // dt + false, // Not stationary + ); + + // During turn, lateral should be positive (speed * positive yaw_rate) + if i > 5 { + assert!(lat > 10.0, "During turn, lateral should be high: {}", lat); + } + } + + // Turn ends - yaw_rate goes to zero + let (_lon, lat) = fusion.process_imu( + 0.0, // ax_raw + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch + yaw, // Final heading + 10.0, // Still moving + 0.0, // NO MORE TURNING + 0.05, // dt + false, + ); + + // Lateral should be zero immediately (no sticking!) + assert!( + lat.abs() < 0.01, + "After turn ends, lateral should be ~0 immediately: got {}", + lat + ); + } + + #[test] + fn test_display_matches_mode_classification_longitudinal() { + // Verify that get_lon_blended() returns the same value as process_imu() + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Setup GPS + fusion.gps_accel.rate_ema = 25.0; + fusion.gps_accel.update(10.0, 0.0); + fusion.gps_accel.update(12.0, 0.04); + + let (lon_from_process, _lat) = fusion.process_imu( + 5.0, 3.0, G, // raw body accel (forward, lateral, gravity) + 0.0, 0.0, // roll, pitch (level) + 0.0, 10.0, 0.0, 0.05, false, + ); + + let lon_for_display = fusion.get_lon_blended(); + + assert_eq!( + lon_from_process, lon_for_display, + "Display lon must match mode classifier input" + ); + } + + #[test] + fn test_earth_to_car_transform() { + // Heading East (yaw = 0) + let (lon, lat) = earth_to_car(10.0, 0.0, 0.0); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading North (yaw = π/2) + let (lon, lat) = earth_to_car(0.0, 10.0, core::f32::consts::FRAC_PI_2); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + + // Heading West (yaw = π) + let (lon, lat) = earth_to_car(-10.0, 0.0, core::f32::consts::PI); + assert!((lon - 10.0).abs() < 0.01); + assert!(lat.abs() < 0.01); + } + + #[test] + fn test_gps_rate_calculation() { + let mut gps = GpsAcceleration::new(); + + // Initialize with non-zero time + gps.update_rate(0, 1.0); + + // After 1 second with 25 fixes + gps.update_rate(25, 2.0); + // First update, EMA: 0.3 * 25 + 0.7 * 0 = 7.5 + assert!( + (gps.get_rate() - 7.5).abs() < 1.0, + "First rate should be ~7.5: got {}", + gps.get_rate() + ); + + // After another second with 25 more fixes + gps.update_rate(50, 3.0); + let rate = gps.get_rate(); + // EMA: 0.3 * 25 + 0.7 * 7.5 = 7.5 + 5.25 = 12.75 + assert!(rate > 10.0 && rate < 20.0, "Rate should converge: {}", rate); + } + + #[test] + fn test_yaw_rate_calibrator_learns_bias() { + let mut cal = YawRateCalibrator::new(); + + // Simulate driving straight on highway with constant heading + // Heading = 0 rad (East), speed = 20 m/s, gyro has 0.01 rad/s bias + let bias = 0.01; // Small gyro bias + + // Feed stable headings (within 2° tolerance) + for _ in 0..30 { + cal.update_heading(0.0); // Constant heading + } + + // Drive straight for 4 seconds at 20 Hz with biased gyro + for _ in 0..80 { + cal.update(bias, 20.0, 0.05); + } + + // Calibrator should have learned the bias + assert!( + cal.is_valid(), + "Should have valid calibration after 4s straight" + ); + assert!( + (cal.get_bias() - bias).abs() < 0.005, + "Learned bias should be close to actual: {} vs {}", + cal.get_bias(), + bias + ); + + // Corrected yaw rate should be near zero + let corrected = cal.correct(bias); + assert!( + corrected.abs() < 0.005, + "Corrected yaw rate should be ~0: {}", + corrected + ); + } + + #[test] + fn test_yaw_rate_calibrator_rejects_turns() { + let mut cal = YawRateCalibrator::new(); + + // Feed varying headings (turning) + for i in 0..30 { + cal.update_heading(i as f32 * 0.1); // Increasing heading = turning + } + + // Try to calibrate during turn + for _ in 0..80 { + cal.update(0.1, 20.0, 0.05); + } + + // Should NOT have valid calibration because heading wasn't stable + assert!(!cal.is_valid(), "Should reject calibration during turns"); + } + + #[test] + fn test_centripetal_uses_calibrated_yaw_rate() { + // Verify that centripetal calculation uses the calibrated yaw rate + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Simulate highway driving with bias in yaw rate + let yaw_bias = 0.02; // 0.02 rad/s bias + + // Feed stable headings + for _ in 0..30 { + fusion.process_gps_heading(0.0); + } + + // Drive straight with biased yaw rate until calibration kicks in + for _ in 0..100 { + fusion.process_imu(0.0, 0.0, G, 0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); + } + + // Now with calibration active, centripetal should be near zero + // even though raw yaw_rate has bias + let (_lon, lat) = + fusion.process_imu(0.0, 0.0, G, 0.0, 0.0, 0.0, 20.0, yaw_bias, 0.05, false); + + // Without calibration: lat = 20 * 0.02 = 0.4 m/s² + // With calibration: lat should be ~0 + assert!( + lat.abs() < 0.1, + "Calibrated lateral should be near zero on straight: {}", + lat + ); + } + + #[test] + fn test_biquad_filter_removes_high_frequency_noise() { + // Simulate noise at 12Hz with 0.1g amplitude (~1 m/s²) + // The 10Hz cutoff Butterworth filter should attenuate this + // (12Hz is above 10Hz cutoff but below 15Hz Nyquist for 30Hz sample rate) + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let noise_freq = 12.0; // Hz - above 10Hz cutoff, below 15Hz Nyquist + let noise_amp = 1.0; // m/s² (~0.1g) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) + + // Run for 3 seconds to let filter settle + let mut max_output: f32 = 0.0; + for i in 0..90 { + let t = i as f32 / sample_rate; + // Simulate noisy IMU input in body frame + let noise = noise_amp * (2.0 * core::f32::consts::PI * noise_freq * t).sin(); + + let (lon, _lat) = fusion.process_imu( + noise, // ax_raw = noisy + 0.0, // ay_raw = 0 + G, // az_raw = gravity + 0.0, // roll = level + 0.0, // pitch = level + 0.0, // yaw = 0 (heading east) + 10.0, // speed + 0.0, // yaw_rate = 0 + 0.033, // dt = 33ms (~30Hz) + false, // not stationary + ); + + // After settling (1 second = 30 samples), check output + if i > 30 { + max_output = max_output.max(lon.abs()); + } + } + + // 12Hz noise should be attenuated by the 10Hz cutoff filter + // Expect ~6dB attenuation: 1 m/s² input → ~0.5 m/s² output + // Being conservative, check it's below 0.7 m/s² + assert!( + max_output < 0.7, + "12Hz noise should be attenuated: got {} m/s²", + max_output + ); + } + + #[test] + fn test_biquad_filter_passes_driving_dynamics() { + // Simulate a braking event at 1Hz (typical vehicle dynamics) + // The 10Hz cutoff filter should pass this with minimal attenuation + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let dynamics_freq = 1.0; // Hz - braking event + let dynamics_amp = 3.0; // m/s² (~0.3g braking) + let sample_rate = 30.0; // Hz - telemetry rate (matches filter config!) + + // Run for 5 seconds to let filter settle + let mut max_output: f32 = 0.0; + for i in 0..150 { + let t = i as f32 / sample_rate; + let signal = dynamics_amp * (2.0 * core::f32::consts::PI * dynamics_freq * t).sin(); + + let (lon, _lat) = fusion.process_imu( + signal, // ax_raw + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch + 0.0, // yaw + 10.0, // speed + 0.0, // yaw_rate + 0.033, // dt = 33ms (~30Hz) + false, + ); + + // After settling (1 second = 30 samples), check output + if i > 30 { + max_output = max_output.max(lon.abs()); + } + } + + // 1Hz should pass through with >80% amplitude at 10Hz cutoff + // 3 m/s² input → >2.4 m/s² output + assert!( + max_output > 2.4, + "1Hz driving dynamics should pass through: got {} m/s², expected >2.4", + max_output + ); + } + + #[test] + fn test_default_config_has_correct_filter_settings() { + // Verify critical configuration values + let config = FusionConfig::default(); + + // Filter must be configured for TELEMETRY rate (~30Hz), not IMU hardware rate + // process_imu() is called at telemetry rate, not IMU sample rate! + assert_eq!( + config.lon_sample_rate, 30.0, + "lon_sample_rate must be 30Hz (telemetry rate, not IMU hardware rate)" + ); + + // Filter cutoff should be 10Hz for smoother output + assert_eq!( + config.lon_filter_cutoff, 10.0, + "lon_filter_cutoff should be 10Hz" + ); + + // GPS max age for freshness check + assert!( + config.gps_max_age <= 0.3, + "gps_max_age should be <=300ms for responsive fallback" + ); + } + + #[test] + fn test_lon_display_equals_lon_blended() { + // Verify that lon_display uses lon_blended, not GPS-only + // This ensures dashboard shows same value as mode detection + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Feed GPS data + fusion.process_gps(10.0, 0.0); + fusion.process_gps(10.5, 0.1); // Creates GPS accel + + // Process IMU with non-zero input + let (lon_blended, _lat) = fusion.process_imu( + 2.0, 0.0, G, // ax, ay, az (body frame) + 0.0, 0.0, // roll, pitch + 0.0, 10.0, 0.0, 0.005, false, + ); + + let lon_display = fusion.get_lon_display(); + + // Display should equal blended (not GPS-only) + assert!( + (lon_display - lon_blended).abs() < 0.001, + "lon_display ({}) should equal lon_blended ({})", + lon_display, + lon_blended + ); + } + + #[test] + fn test_sharp_braking_preserved_with_15hz_filter() { + // Simulate sharp braking (0.3 second event = ~3Hz component) + // 15Hz filter should preserve this; old 5Hz filter would attenuate + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + let sample_rate = 200.0; + let mut max_output: f32 = 0.0; + + // 0.3 second braking pulse + for i in 0..200 { + let t = i as f32 / sample_rate; + + // Sharp braking pulse: ramps up in 0.1s, holds 0.1s, ramps down 0.1s + let brake_input = if t < 0.1 { + 5.0 * (t / 0.1) // Ramp up to 5 m/s² (0.5g) + } else if t < 0.2 { + 5.0 // Hold + } else if t < 0.3 { + 5.0 * (1.0 - (t - 0.2) / 0.1) // Ramp down + } else { + 0.0 + }; + + let (lon, _lat) = fusion.process_imu( + brake_input, // ax_raw + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch + 0.0, // yaw + 15.0, // 15 m/s = 54 km/h + 0.0, // yaw_rate + 0.005, // dt + false, + ); + + max_output = max_output.max(lon.abs()); + } + + // Should preserve at least 60% of 5 m/s² input + // (some attenuation due to filter, but not severe) + assert!( + max_output > 3.0, + "Sharp braking should be preserved: got {} m/s², expected >3.0", + max_output + ); + } + + #[test] + fn test_orientation_correction_converges_during_cruise() { + // With the new GPS-corrected orientation approach: + // - IMU shows fake acceleration due to AHRS error + // - OrientationCorrector learns the error from GPS comparison + // - After learning, lon_blended converges toward GPS (truth) + // + // This test verifies the OrientationCorrector learns during driving. + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Set up GPS with zero acceleration (cruising at constant speed) + fusion.process_gps(10.0, 0.0); + fusion.process_gps(10.0, 0.1); // Same speed = 0 accel + + // Process IMU with fake "acceleration" (simulates AHRS pitch error) + // pitch_error ≈ 3° causes ~0.5 m/s² fake accel + let imu_fake_accel = 0.5; // m/s² - this is AHRS error, not real accel + let mut lon = 0.0; + + for i in 0..200 { + let (l, _lat) = fusion.process_imu( + imu_fake_accel, // Fake signal from AHRS pitch error + 0.0, // ay_raw + G, // az_raw + 0.0, // roll + 0.0, // pitch (AHRS reports 0, but there's error we're simulating) + 0.0, // yaw + 10.0, // speed + 0.0, // yaw_rate + 0.005, // dt + false, + ); + // Keep GPS fresh + if i % 20 == 0 { + fusion.process_gps(10.0, 0.1 + (i as f32) * 0.005); + } + lon = l; + } + + // With GPS-IMU blending based on confidence: + // - Initially high GPS weight → lon close to GPS (0) + // - As correction learns → GPS weight decreases but corrected IMU also → 0 + // Either way, lon should be small + assert!( + lon.abs() < 0.5, + "During cruise, lon should converge to ~0: got {} m/s²", + lon + ); + } + + #[test] + fn test_cruising_at_constant_speed_produces_zero_lon() { + // KEY BUG TEST: When cruising at constant speed with no real acceleration, + // lon_blended should be near zero. This catches bad offset learning. + // + // The GravityEstimator bug caused lon_raw = -2.6 m/s² when cruising, + // which triggered false BRAKE detection. + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // First: learn tilt offset when stopped (simulates calibration) + for _ in 0..600 { + // 3 seconds at 200Hz + fusion.process_imu( + 0.0, 0.0, G, // ax, ay, az (level, no motion) + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 0.0, // Stopped + 0.0, // No yaw rate + 0.005, // dt = 5ms + true, // STATIONARY - tilt learns here + ); + } + + // Now: start "cruising" at 20 m/s with zero input acceleration + // This simulates constant-speed highway driving + let mut max_lon: f32 = 0.0; + let mut sum_lon: f32 = 0.0; + let samples = 400; // 2 seconds at 200Hz + + for i in 0..samples { + // Provide GPS updates to keep it fresh + if i % 8 == 0 { + // 25Hz GPS rate + fusion.process_gps(20.0, (i as f32) * 0.005); // Constant speed + } + + let (lon, _lat) = fusion.process_imu( + 0.0, 0.0, G, // ax, ay, az - ZERO accel! + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 20.0, // 20 m/s = 72 km/h cruising + 0.0, // Straight road + 0.005, // dt = 5ms + false, // Moving + ); + + // Skip filter settling time + if i > 200 { + max_lon = max_lon.max(lon.abs()); + sum_lon += lon.abs(); + } + } + + let avg_lon = sum_lon / (samples - 200) as f32; + + // CRITICAL: lon_blended should be near zero when cruising + // The bug caused ~2.6 m/s² here + assert!( + max_lon < 0.5, + "Cruising with zero input should produce near-zero lon: max={} m/s²", + max_lon + ); + assert!( + avg_lon < 0.2, + "Average lon during cruise should be near zero: avg={} m/s²", + avg_lon + ); + } + + #[test] + fn test_tilt_only_updates_when_stationary() { + // Verify that tilt estimator only learns when is_stationary=true + // If it learned while moving, bad offsets could accumulate + let mut tilt = TiltEstimator::new(1.0); + + // Simulate "moving" conditions with large acceleration + for _ in 0..300 { + // 1.5 seconds at 200Hz + // Note: update_stationary should only be called when stationary, + // but let's verify the valid flag behavior + tilt.update_stationary(5.0, 3.0, 0.005); // Large accel (would be wrong if learned) + } + + // After learning, check the offset + let (ax, _ay) = tilt.correct(5.0, 3.0); + + // The tilt estimator WILL learn this offset - this is by design + // The key is that in main.rs, we only call update_stationary when truly stopped + assert!( + ax.abs() < 0.1, + "After learning, correction should work: {}", + ax + ); + } + + #[test] + fn test_no_drift_during_extended_cruise() { + // Verify that lon_blended stays near zero over extended cruising + // This catches slow drift from any remaining estimators + let config = FusionConfig::default(); + let mut fusion = SensorFusion::new(config); + + // Learn tilt when stopped + for _ in 0..600 { + fusion.process_imu(0.0, 0.0, G, 0.0, 0.0, 0.0, 0.0, 0.0, 0.005, true); + } + + // Start cruising + let mut lon_samples: Vec = Vec::new(); + + for i in 0..2000 { + // 10 seconds at 200Hz + if i % 8 == 0 { + fusion.process_gps(25.0, (i as f32) * 0.005); + } + + let (lon, _) = fusion.process_imu( + 0.0, 0.0, G, // ax, ay, az - zero input + 0.0, 0.0, // roll, pitch + 0.0, // yaw + 25.0, // 25 m/s = 90 km/h + 0.0, // yaw_rate + 0.005, // dt + false, + ); + + if i > 400 { + // After 2 seconds settling + lon_samples.push(lon); + } + } + + // Check for drift: first half vs second half average + let mid = lon_samples.len() / 2; + let first_half_avg: f32 = lon_samples[..mid].iter().sum::() / mid as f32; + let second_half_avg: f32 = lon_samples[mid..].iter().sum::() / mid as f32; + let drift = (second_half_avg - first_half_avg).abs(); + + assert!( + drift < 0.1, + "lon should not drift during cruise: first_avg={}, second_avg={}, drift={}", + first_half_avg, + second_half_avg, + drift + ); + } + + // ============== OrientationCorrector Tests ============== + + #[test] + fn test_orientation_corrector_init() { + let corrector = OrientationCorrector::new(); + + assert_eq!(corrector.pitch_correction, 0.0); + assert_eq!(corrector.roll_correction, 0.0); + assert_eq!(corrector.pitch_confidence, 0.0); + assert_eq!(corrector.roll_confidence, 0.0); + } + + #[test] + fn test_orientation_corrector_learns_pitch_from_forward_accel() { + // Simulate: true accel = 3.0 m/s², but AHRS has pitch error + // causing IMU to show different value than GPS + // + // AHRS pitch error = -3° (thinks car tilting back during forward accel) + // This causes remove_gravity to subtract wrong gravity component + // Result: lon_imu is LESS than lon_gps + // + // Math: gravity error ≈ G * sin(3°) ≈ 0.51 m/s² + // So lon_imu ≈ 3.0 - 0.51 = 2.49 m/s² (if AHRS pitch = -3°) + // GPS shows truth: lon_gps = 3.0 m/s² + // Innovation = (2.49 - 3.0) / 9.8 ≈ -0.052 rad ≈ -3° + + let mut corrector = OrientationCorrector::new(); + + let lon_gps = 3.0; // Ground truth + let lon_imu = 2.49; // What IMU shows with -3° pitch error + let speed = 15.0; // 54 km/h, above min_speed + + // Simulate many updates (EMA needs time to converge) + for _ in 0..500 { + corrector.update( + lon_imu, // IMU longitudinal + lon_gps, // GPS longitudinal (truth) + 0.0, // lat_imu + 0.0, // lat_gps + speed, true, // GPS fresh + 0.033, // dt (~30Hz) + ); + } + + // Pitch correction should converge toward the error + // Expected: ~-3° = ~-0.052 rad + let pitch_corr = corrector.pitch_correction; + assert!( + (pitch_corr - (-0.052)).abs() < 0.01, + "Pitch correction should be ~-0.052 rad (-3°), got {} rad ({}°)", + pitch_corr, + pitch_corr.to_degrees() + ); + + // Confidence should be high after many updates + assert!( + corrector.pitch_confidence > 0.9, + "Confidence should be >0.9 after 500 updates, got {}", + corrector.pitch_confidence + ); + } + + #[test] + fn test_orientation_corrector_learns_roll_from_cornering() { + // During left turn, centripetal acceleration is positive (leftward) + // If AHRS has roll error, lat_imu differs from lat_gps + let mut corrector = OrientationCorrector::new(); + + let lat_gps = 5.0; // Centripetal = speed * yaw_rate = 10 * 0.5 = 5 m/s² + let lat_imu = 4.5; // Roll error causes 0.5 m/s² difference + let speed = 15.0; + + for _ in 0..500 { + corrector.update( + 0.0, // lon_imu (not accelerating) + 0.0, // lon_gps + lat_imu, // IMU lateral + lat_gps, // GPS lateral (centripetal) + speed, true, 0.005, // dt = 5ms (200Hz) + ); + } + + // Roll correction should converge + // Error = (4.5 - 5.0) / 9.8 = -0.051 rad + let roll_corr = corrector.roll_correction; + assert!( + (roll_corr - (-0.051)).abs() < 0.01, + "Roll correction should be ~-0.051 rad, got {}", + roll_corr + ); + } + + #[test] + fn test_orientation_corrector_ignores_low_speed() { + let mut corrector = OrientationCorrector::new(); + + // At low speed, GPS is noisy - should not learn + let low_speed = 1.0; // Below min_speed (3.0 m/s) + + for _ in 0..100 { + corrector.update( + 5.0, // Large IMU value + 0.0, // GPS shows zero + 0.0, 0.0, low_speed, true, 0.005, // dt = 5ms + ); + } + + // Should NOT have learned anything + assert_eq!( + corrector.pitch_correction, 0.0, + "Should not learn at low speed" + ); + assert_eq!( + corrector.pitch_confidence, 0.0, + "Confidence should be 0 at low speed" + ); + } + + #[test] + fn test_orientation_corrector_ignores_small_accel() { + let mut corrector = OrientationCorrector::new(); + + // When not accelerating (GPS shows ~0), no signal to learn from for pitch + // BUT now this should learn cruise_bias instead! + for _ in 0..100 { + corrector.update( + 0.3, // Small IMU value (offset) + 0.0, // GPS shows zero (cruising) + 0.0, 0.0, 15.0, // Good speed + true, 0.05, // dt = 50ms (shorter loop, 2s = 40 iterations, we do 100) + ); + } + + // Pitch correction should NOT have learned - GPS accel below min_accel threshold + assert_eq!( + corrector.pitch_correction, 0.0, + "Should not learn pitch when GPS accel < min_accel" + ); + + // But cruise_bias SHOULD have been learned! (new behavior) + // IMU shows 0.3 m/s² offset during cruise, cruise bias should converge toward it + assert!( + corrector.cruise_bias.abs() > 0.01, + "Should learn cruise_bias when cruising, got {}", + corrector.cruise_bias + ); + } + + #[test] + fn test_orientation_corrector_clamped_at_max() { + let mut corrector = OrientationCorrector::new(); + + // Huge error that would exceed max_correction + let lon_imu = 10.0; + let lon_gps = 3.0; // 7 m/s² difference = huge pitch error + + for _ in 0..1000 { + corrector.update(lon_imu, lon_gps, 0.0, 0.0, 15.0, true, 0.005); + } + + // Should be clamped at max_correction (0.26 rad = 15°) + assert!( + corrector.pitch_correction.abs() <= corrector.max_correction + 0.001, + "Correction should be clamped at max: {} > {}", + corrector.pitch_correction.abs(), + corrector.max_correction + ); + } + + #[test] + fn test_orientation_corrector_correct_applies_properly() { + let mut corrector = OrientationCorrector::new(); + + // Manually set corrections for testing + corrector.pitch_correction = 0.05; // ~2.9° + corrector.roll_correction = -0.03; // ~-1.7° + + // AHRS reports pitch=5°, roll=2° + let (corrected_pitch, corrected_roll) = corrector.correct(5.0, 2.0); + + // Corrected = AHRS - correction_deg + // pitch: 5.0 - 2.86 = 2.14 + // roll: 2.0 - (-1.72) = 3.72 + assert!( + (corrected_pitch - 2.14).abs() < 0.1, + "Corrected pitch should be ~2.14°, got {}", + corrected_pitch + ); + assert!( + (corrected_roll - 3.72).abs() < 0.1, + "Corrected roll should be ~3.72°, got {}", + corrected_roll + ); + } + + #[test] + fn test_orientation_corrector_reset() { + let mut corrector = OrientationCorrector::new(); + + // Learn some corrections + for _ in 0..200 { + corrector.update(5.0, 3.0, 3.0, 2.0, 15.0, true, 0.005); + } + + assert!(corrector.pitch_correction != 0.0); + + // Reset + corrector.reset(); + + assert_eq!(corrector.pitch_correction, 0.0); + assert_eq!(corrector.roll_correction, 0.0); + assert_eq!(corrector.pitch_confidence, 0.0); + assert_eq!(corrector.update_count, 0); + } + + #[test] + fn test_orientation_corrector_no_gps_no_learning() { + let mut corrector = OrientationCorrector::new(); + + // GPS not fresh - should not learn + for _ in 0..100 { + corrector.update( + 5.0, 3.0, 0.0, 0.0, 15.0, false, // GPS NOT fresh + 0.005, + ); + } + + assert_eq!( + corrector.pitch_correction, 0.0, + "Should not learn when GPS not fresh" + ); + } + + #[test] + fn test_orientation_corrector_cruise_bias_learning() { + // Test that cruise bias is learned during constant-speed driving + // when GPS shows ~0 acceleration but IMU has a consistent offset + let mut corrector = OrientationCorrector::new(); + + // Simulate 5 seconds of cruising at constant speed + // IMU shows 0.5 m/s² offset (mounting error), GPS shows ~0 (cruising) + let imu_offset = 0.5; // m/s² - consistent offset during cruise + let dt = 0.05; // 50ms = 20Hz, so 2s = 40 samples + + // Run for 5 seconds (100 iterations at 50ms each) + for _ in 0..100 { + corrector.update( + imu_offset, // IMU shows offset + 0.0, // GPS shows zero (cruising) + 0.0, 0.0, 15.0, // 54 km/h - above min_speed + true, dt, + ); + } + + // Cruise bias should have been learned + // With slow EMA (alpha=0.1) and 2s update cycle, after 5s we get ~2 updates + // First update: bias = 0 + 0.1 * 0.5 = 0.05 + // Second update: corrected = 0.5 - 0.05 = 0.45, bias = 0.05 * 0.9 + 0.1 * 0.45 = 0.09 + let cruise_bias = corrector.get_cruise_bias(); + assert!( + cruise_bias.abs() > 0.05, + "Cruise bias should be learning (>0.05), got {}", + cruise_bias + ); + assert!( + cruise_bias > 0.0, + "Cruise bias should be positive (learning toward IMU offset): got {}", + cruise_bias + ); + + // Pitch correction should NOT be learned (no GPS accel signal) + assert!( + corrector.pitch_correction.abs() < 0.01, + "Pitch should not change during cruise" + ); + } + + #[test] + fn test_orientation_corrector_cruise_bias_resets_on_accel() { + // Test that cruise bias accumulator resets when acceleration is detected + let mut corrector = OrientationCorrector::new(); + + // First, build up some cruise bias + for _ in 0..50 { + corrector.update(0.3, 0.0, 0.0, 0.0, 15.0, true, 0.05); + } + + // Now accelerate - this should reset the cruise accumulator + // but preserve the learned cruise_bias from before + let bias_before = corrector.get_cruise_bias(); + + for _ in 0..10 { + corrector.update( + 3.5, // IMU shows acceleration + 3.0, // GPS also shows acceleration + 0.0, 0.0, 15.0, true, 0.05, + ); + } + + // Cruise bias should be preserved (not cleared by acceleration) + let bias_after = corrector.get_cruise_bias(); + assert!( + (bias_after - bias_before).abs() < 0.1, + "Cruise bias should be preserved during acceleration" + ); + + // But pitch should now be learning + assert!( + corrector.pitch_correction.abs() > 0.001, + "Pitch should be learning during acceleration" + ); + } +} diff --git a/framework/src/lib.rs b/framework/src/lib.rs index 38f20d8..ecd4d4f 100644 --- a/framework/src/lib.rs +++ b/framework/src/lib.rs @@ -132,11 +132,23 @@ // Core framework modules pub mod ekf; +pub mod filter; +pub mod fusion; +pub mod mode; pub mod sensor_framework; pub mod sensors; pub mod transforms; pub mod velocity; +// Re-export mode detection types +pub use mode::{Mode, ModeClassifier, ModeConfig, DEFAULT_MODE_ALPHA}; + +// Re-export sensor fusion types +pub use fusion::{ + FusionConfig, GpsAcceleration, OrientationCorrector, SensorFusion, TiltEstimator, + YawRateCalibrator, +}; + // Re-export commonly used types from sensor_framework // Re-export EKF pub use ekf::Ekf; diff --git a/framework/src/mode.rs b/framework/src/mode.rs new file mode 100644 index 0000000..ee8ae9e --- /dev/null +++ b/framework/src/mode.rs @@ -0,0 +1,617 @@ +#![allow(dead_code)] // API methods for future use + +/// Driving mode classifier +/// Detects IDLE, ACCEL, BRAKE, CORNER, ACCEL+CORNER, and BRAKE+CORNER modes +/// based on acceleration and yaw rate +/// +/// ## Hybrid Acceleration Support +/// +/// This classifier can operate in two modes: +/// 1. **Legacy mode**: Uses `update()` with raw earth-frame IMU accelerations +/// 2. **Hybrid mode**: Uses `update_hybrid()` with pre-blended longitudinal acceleration +/// +/// Hybrid mode receives longitudinal acceleration that's already blended from +/// GPS-derived (from velocity changes) and IMU sources. This provides: +/// - Drift-free longitudinal detection (GPS doesn't drift) +/// - Mounting angle independence (GPS measures actual vehicle acceleration) +/// - Graceful fallback when GPS is unavailable +use crate::transforms::earth_to_car; + +const G: f32 = 9.80665; // m/s² + +/// Driving mode flags - can be combined +pub struct Mode { + flags: u8, +} + +// Mode bit flags +const IDLE: u8 = 0; +const ACCEL: u8 = 1; +const BRAKE: u8 = 2; +const CORNER: u8 = 4; + +impl Mode { + pub fn idle() -> Self { + Self { flags: IDLE } + } + + pub fn is_idle(&self) -> bool { + self.flags == IDLE + } + + pub fn has_accel(&self) -> bool { + self.flags & ACCEL != 0 + } + + pub fn has_brake(&self) -> bool { + self.flags & BRAKE != 0 + } + + pub fn has_corner(&self) -> bool { + self.flags & CORNER != 0 + } + + pub fn set_accel(&mut self, enabled: bool) { + if enabled { + self.flags |= ACCEL; + self.flags &= !BRAKE; // Clear brake (mutually exclusive) + } else { + self.flags &= !ACCEL; + } + } + + pub fn set_brake(&mut self, enabled: bool) { + if enabled { + self.flags |= BRAKE; + self.flags &= !ACCEL; // Clear accel (mutually exclusive) + } else { + self.flags &= !BRAKE; + } + } + + pub fn set_corner(&mut self, enabled: bool) { + if enabled { + self.flags |= CORNER; + } else { + self.flags &= !CORNER; + } + } + + pub fn clear(&mut self) { + self.flags = IDLE; + } + + /// Get mode as string for display + pub fn as_str(&self) -> &'static str { + match self.flags { + 0 => "IDLE", + 1 => "ACCEL", + 2 => "BRAKE", + 4 => "CORNER", + 5 => "ACCEL+CORNER", + 6 => "BRAKE+CORNER", + _ => "UNKNOWN", + } + } + + /// Get mode as u8 for binary telemetry + pub fn as_u8(&self) -> u8 { + self.flags + } +} + +impl Default for Mode { + fn default() -> Self { + Self::idle() + } +} + +/// Default EMA alpha for mode detection smoothing +/// τ ≈ 72ms at 20Hz processing rate +/// Used by both ModeConfig::default() and settings handler in main.rs +pub const DEFAULT_MODE_ALPHA: f32 = 0.50; + +/// Configurable thresholds for mode detection +#[derive(Debug, Clone, Copy)] +pub struct ModeConfig { + pub min_speed: f32, // m/s (minimum speed for maneuvers) + pub acc_thr: f32, // g (acceleration threshold) + pub acc_exit: f32, // g (acceleration exit threshold) + pub brake_thr: f32, // g (braking threshold, negative) + pub brake_exit: f32, // g (braking exit threshold, negative) + pub lat_thr: f32, // g (lateral acceleration threshold) + pub lat_exit: f32, // g (lateral acceleration exit threshold) + pub yaw_thr: f32, // rad/s (yaw rate threshold for cornering) + pub alpha: f32, // EMA smoothing factor +} + +impl Default for ModeConfig { + fn default() -> Self { + Self { + min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner + acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) + acc_exit: 0.05, // exit when below 0.05g + brake_thr: -0.15, // -0.15g - sensitive brake detection (compensates for mounting bias) + brake_exit: -0.08, // exit when above -0.08g + lat_thr: 0.10, // 0.10g lateral - sensitive corner detection + lat_exit: 0.05, // exit when below 0.05g + yaw_thr: 0.04, // ~2.3°/s yaw rate - sensitive + alpha: DEFAULT_MODE_ALPHA, + } + } +} + +/// Mode classifier with EMA filtering +pub struct ModeClassifier { + pub mode: Mode, + pub config: ModeConfig, + + // EMA filtered values (all accelerations filtered to reject road bumps) + a_lon_ema: f32, + a_lat_ema: f32, + yaw_ema: f32, + + // Display speed with separate smoothing + v_disp: f32, + v_alpha: f32, // Speed display smoothing factor +} + +impl ModeClassifier { + pub fn new() -> Self { + Self { + mode: Mode::default(), + config: ModeConfig::default(), + a_lon_ema: 0.0, + a_lat_ema: 0.0, + yaw_ema: 0.0, + v_disp: 0.0, + v_alpha: 0.85, // High alpha = fast response to GPS speed changes + } + } + + /// Update mode based on current vehicle state + /// + /// # Arguments + /// * `ax_earth` - Longitudinal acceleration in earth frame (m/s²) + /// * `ay_earth` - Lateral acceleration in earth frame (m/s²) + /// * `yaw_rad` - Vehicle yaw angle (rad) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed (m/s) + pub fn update(&mut self, ax_earth: f32, ay_earth: f32, yaw_rad: f32, wz: f32, speed: f32) { + // Update display speed with EMA + self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; + if self.v_disp < 3.0 / 3.6 { + // < 3 km/h (below walking speed, treat as stationary for display) + self.v_disp = 0.0; + } + + // Transform earth-frame acceleration to car-frame + let (a_lon_ms2, a_lat_ms2) = earth_to_car(ax_earth, ay_earth, yaw_rad); + + // Convert to g units + let a_lon = a_lon_ms2 / G; + let a_lat = a_lat_ms2 / G; + + // Update EMA filters for ALL values (critical for rejecting road bump noise) + let alpha = self.config.alpha; + self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; + self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; + self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; + + // Independent state detection for each mode component + // Modes can be combined (e.g., ACCEL+CORNER, BRAKE+CORNER) + + // If stopped, clear all modes + if speed < self.config.min_speed { + self.mode.clear(); + return; + } + + // Check cornering (independent of accel/brake) + let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr + && self.yaw_ema.abs() > self.config.yaw_thr + && (self.a_lat_ema * self.yaw_ema) > 0.0; + + let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit + && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; + + if self.mode.has_corner() { + // Exit corner if conditions no longer met + if cornering_exit { + self.mode.set_corner(false); + } + } else { + // Enter corner if conditions met + if cornering_active { + self.mode.set_corner(true); + } + } + + // Check acceleration (mutually exclusive with braking) + // Uses filtered a_lon_ema to reject road bump noise + if self.mode.has_accel() { + // Exit if acceleration dropped + if self.a_lon_ema < self.config.acc_exit { + self.mode.set_accel(false); + } + } else if !self.mode.has_brake() { + // Enter accel if not braking and threshold met + if self.a_lon_ema > self.config.acc_thr { + self.mode.set_accel(true); + } + } + + // Check braking (mutually exclusive with acceleration) + // Uses filtered a_lon_ema to reject road bump noise + if self.mode.has_brake() { + // Exit if braking force dropped + if self.a_lon_ema > self.config.brake_exit { + self.mode.set_brake(false); + } + } else if !self.mode.has_accel() { + // Enter brake if not accelerating and threshold met + if self.a_lon_ema < self.config.brake_thr { + self.mode.set_brake(true); + } + } + } + + /// Get current mode + pub fn get_mode(&self) -> &Mode { + &self.mode + } + + /// Get current mode as u8 for telemetry + pub fn get_mode_u8(&self) -> u8 { + self.mode.as_u8() + } + + /// Get display speed in km/h + pub fn get_speed_kmh(&self) -> f32 { + self.v_disp * 3.6 + } + + /// Force speed to zero (for ZUPT) + pub fn reset_speed(&mut self) { + self.v_disp = 0.0; + } + + /// Check if display speed is near zero + pub fn is_stopped(&self) -> bool { + self.v_disp < 0.1 // < 0.36 km/h + } + + /// Get EMA filtered lateral acceleration (for diagnostics) + #[allow(dead_code)] + pub fn get_a_lat_ema(&self) -> f32 { + self.a_lat_ema + } + + /// Get EMA filtered yaw rate (for diagnostics) + #[allow(dead_code)] + pub fn get_yaw_ema(&self) -> f32 { + self.yaw_ema + } + + /// Update configuration (e.g., from MQTT) + pub fn update_config(&mut self, config: ModeConfig) { + self.config = config; + } + + /// Update mode using pre-blended hybrid acceleration + /// + /// This is the preferred method when using SensorFusion, as it receives + /// longitudinal acceleration that's already blended from GPS and IMU sources, + /// and centripetal lateral acceleration (speed × yaw_rate) for corner detection. + /// + /// # Arguments + /// * `a_lon_blended` - Blended longitudinal acceleration (m/s², vehicle frame) + /// * `a_lat_centripetal` - Centripetal lateral acceleration (m/s², = speed × yaw_rate) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed (m/s) + pub fn update_hybrid( + &mut self, + a_lon_blended: f32, + a_lat_centripetal: f32, + wz: f32, + speed: f32, + ) { + // Update display speed with EMA + self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; + if self.v_disp < 3.0 / 3.6 { + self.v_disp = 0.0; + } + + // Convert to g units (accelerations are in m/s²) + let a_lon = a_lon_blended / G; + let a_lat = a_lat_centripetal / G; + + // Update EMA filters + // Note: longitudinal already filtered/blended, but still apply EMA for consistency + let alpha = self.config.alpha; + self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; + self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; + self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; + + // If stopped, clear all modes + if speed < self.config.min_speed { + self.mode.clear(); + return; + } + + // Check cornering (independent of accel/brake) + let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr + && self.yaw_ema.abs() > self.config.yaw_thr + && (self.a_lat_ema * self.yaw_ema) > 0.0; + + let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit + && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; + + if self.mode.has_corner() { + if cornering_exit { + self.mode.set_corner(false); + } + } else if cornering_active { + self.mode.set_corner(true); + } + + // Check acceleration (mutually exclusive with braking) + if self.mode.has_accel() { + if self.a_lon_ema < self.config.acc_exit { + self.mode.set_accel(false); + } + } else if !self.mode.has_brake() && self.a_lon_ema > self.config.acc_thr { + self.mode.set_accel(true); + } + + // Check braking (mutually exclusive with acceleration) + if self.mode.has_brake() { + if self.a_lon_ema > self.config.brake_exit { + self.mode.set_brake(false); + } + } else if !self.mode.has_accel() && self.a_lon_ema < self.config.brake_thr { + self.mode.set_brake(true); + } + } + + /// Get EMA filtered longitudinal acceleration (for diagnostics) + pub fn get_a_lon_ema(&self) -> f32 { + self.a_lon_ema + } +} + +impl Default for ModeClassifier { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_mode_idle_by_default() { + let classifier = ModeClassifier::new(); + assert!(classifier.get_mode().is_idle()); + } + + #[test] + fn test_acceleration_detection() { + let mut classifier = ModeClassifier::new(); + + // Simulate forward acceleration at 5 m/s (above min_speed of 2.0) + let ax = 0.25 * G; // 0.25g forward + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 5.0; + + // Multiple updates needed for EMA to converge (alpha=0.25) + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel()); + } + + #[test] + fn test_braking_detection() { + let mut classifier = ModeClassifier::new(); + + // Simulate braking at 5 m/s (above min_speed of 2.0) + let ax = -0.3 * G; // 0.3g deceleration + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 5.0; + + // Multiple updates needed for EMA to converge + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_brake()); + } + + #[test] + fn test_no_mode_when_stopped() { + let mut classifier = ModeClassifier::new(); + + // Even with high acceleration, should stay IDLE when speed is 0 + let ax = -0.5 * G; // 0.5g deceleration (would trigger brake) + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed = 0.0; // STOPPED + + classifier.update(ax, ay, yaw, wz, speed); + assert!(classifier.get_mode().is_idle()); // Must stay IDLE when stopped + } + + #[test] + fn test_combined_accel_corner() { + let mut classifier = ModeClassifier::new(); + + // Simulate acceleration while cornering (corner exit) + let ax = 0.20 * G; // 0.20g forward acceleration + let ay = 0.15 * G; // 0.15g lateral + let yaw = 0.0; + let wz = 0.08; // 0.08 rad/s yaw rate + let speed = 8.0; // 8 m/s + + // Multiple updates for EMA convergence + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + + // Should detect both acceleration and cornering + assert!( + classifier.get_mode().has_accel(), + "Should detect acceleration" + ); + assert!( + classifier.get_mode().has_corner(), + "Should detect cornering" + ); + assert_eq!(classifier.get_mode().as_str(), "ACCEL+CORNER"); + } + + #[test] + fn test_combined_brake_corner() { + let mut classifier = ModeClassifier::new(); + + // Simulate braking while cornering (corner entry) + let ax = -0.35 * G; // 0.35g braking (above -0.25g threshold) + let ay = -0.15 * G; // 0.15g lateral (left turn) + let yaw = 0.0; + let wz = -0.08; // -0.08 rad/s yaw rate (left) + let speed = 8.0; // 8 m/s + + // Multiple updates for EMA convergence + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + + // Should detect both braking and cornering + assert!(classifier.get_mode().has_brake(), "Should detect braking"); + assert!( + classifier.get_mode().has_corner(), + "Should detect cornering" + ); + assert_eq!(classifier.get_mode().as_str(), "BRAKE+CORNER"); + } + + #[test] + fn test_hysteresis_prevents_oscillation() { + let mut classifier = ModeClassifier::new(); + + let speed = 5.0; + let yaw = 0.0; + let wz = 0.0; + let ay = 0.0; + + // Acceleration above entry threshold - multiple updates for EMA + let ax1 = 0.20 * G; // Use higher value to ensure EMA exceeds threshold + for _ in 0..10 { + classifier.update(ax1, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel(), "Should enter ACCEL"); + + // Drop to 0.06g (above 0.05g exit threshold) - EMA should stay above exit + let ax2 = 0.06 * G; + for _ in 0..5 { + classifier.update(ax2, ay, yaw, wz, speed); + } + assert!( + classifier.get_mode().has_accel(), + "Should stay ACCEL (EMA still above exit)" + ); + + // Drop to 0.00g - EMA will gradually fall below exit threshold + let ax3 = 0.00 * G; + for _ in 0..20 { + classifier.update(ax3, ay, yaw, wz, speed); + } + assert!( + classifier.get_mode().is_idle(), + "Should exit to IDLE (EMA below exit)" + ); + } + + #[test] + fn test_speed_threshold_prevents_false_modes() { + let mut classifier = ModeClassifier::new(); + + // High acceleration but speed below threshold + let ax = 0.30 * G; + let ay = 0.0; + let yaw = 0.0; + let wz = 0.0; + let speed_low = 1.5; // Below 2.0 m/s threshold + + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed_low); + } + assert!( + classifier.get_mode().is_idle(), + "Should stay IDLE when speed < min_speed" + ); + + // Same acceleration, speed above threshold + let speed_high = 2.5; + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed_high); + } + assert!( + classifier.get_mode().has_accel(), + "Should detect ACCEL when speed > min_speed" + ); + } + + #[test] + fn test_corner_independent_persistence() { + let mut classifier = ModeClassifier::new(); + + // Start cornering - multiple updates for EMA + let ax = 0.0; + let ay = 0.15 * G; + let yaw = 0.0; + let wz = 0.08; + let speed = 8.0; + + for _ in 0..10 { + classifier.update(ax, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_corner(), "Should detect corner"); + + // Add acceleration while still cornering + let ax2 = 0.20 * G; + for _ in 0..10 { + classifier.update(ax2, ay, yaw, wz, speed); + } + assert!(classifier.get_mode().has_accel(), "Should detect accel"); + assert!( + classifier.get_mode().has_corner(), + "Should still detect corner" + ); + assert_eq!(classifier.get_mode().as_u8(), 5, "Should be ACCEL+CORNER"); + + // Stop accelerating but keep cornering - EMA needs to drop + let ax3 = 0.0; + for _ in 0..20 { + classifier.update(ax3, ay, yaw, wz, speed); + } + assert!(!classifier.get_mode().has_accel(), "Should exit accel"); + assert!(classifier.get_mode().has_corner(), "Should still corner"); + } + + #[test] + fn test_default_alpha_uses_constant() { + // This test ensures ModeConfig::default() uses the shared constant + // Prevents bugs where alpha is hardcoded in multiple places with different values + let config = ModeConfig::default(); + assert_eq!( + config.alpha, DEFAULT_MODE_ALPHA, + "ModeConfig::default() must use DEFAULT_MODE_ALPHA constant" + ); + } +} diff --git a/framework/src/transforms.rs b/framework/src/transforms.rs index 90fbe7f..fef1cdb 100644 --- a/framework/src/transforms.rs +++ b/framework/src/transforms.rs @@ -80,7 +80,6 @@ pub fn remove_gravity( /// /// # Returns /// * `(a_lon, a_lat)` - Longitudinal and lateral acceleration (m/s²) -#[allow(dead_code)] pub fn earth_to_car(ax_earth: f32, ay_earth: f32, yaw_rad: f32) -> (f32, f32) { let cos_yaw = yaw_rad.cos(); let sin_yaw = yaw_rad.sin(); diff --git a/sensors/blackbox/src/diagnostics.rs b/sensors/blackbox/src/diagnostics.rs index 3fd968a..fdb9963 100644 --- a/sensors/blackbox/src/diagnostics.rs +++ b/sensors/blackbox/src/diagnostics.rs @@ -94,6 +94,55 @@ pub struct ConfigSnapshot { pub gps_warmup_fixes: u8, } +/// Sensor fusion diagnostics - filter pipeline and blending status +#[derive(Debug, Clone, Copy, Default)] +pub struct FusionDiagnostics { + // Filter pipeline (all in m/s²) + /// Raw IMU longitudinal acceleration (before Butterworth filter) + pub lon_imu_raw: f32, + /// Filtered IMU longitudinal (after 10Hz low-pass) + pub lon_imu_filtered: f32, + /// Final blended longitudinal (GPS/IMU mix) + pub lon_blended: f32, + + // GPS blending status + /// Current GPS blend weight (0.0-1.0, based on orientation correction + /// confidence) + pub gps_weight: f32, + /// GPS-derived acceleration (m/s²), NaN if invalid + pub gps_accel: f32, + /// GPS update rate estimate (Hz) + pub gps_rate: f32, + /// Was GPS rejected by validity check? (GPS=0 but IMU has signal) + pub gps_rejected: bool, + + // Orientation corrector (ArduPilot-style GPS-corrected orientation) + /// Learned pitch correction (degrees) - corrects AHRS errors during + /// acceleration + pub pitch_correction_deg: f32, + /// Learned roll correction (degrees) - corrects AHRS errors during + /// cornering + pub roll_correction_deg: f32, + /// Pitch correction confidence (0.0-1.0, based on learning samples) + pub pitch_confidence: f32, + /// Roll correction confidence (0.0-1.0) + pub roll_confidence: f32, + + // Yaw rate calibrator + /// Learned gyro bias (rad/s) + pub yaw_bias: f32, + /// Is yaw calibration valid? + pub yaw_calibrated: bool, + + // Tilt estimator (learned when stopped) + /// Tilt offset X (m/s²) + pub tilt_offset_x: f32, + /// Tilt offset Y (m/s²) + pub tilt_offset_y: f32, + /// Is tilt offset valid? + pub tilt_valid: bool, +} + /// Complete diagnostics snapshot (immutable copy for reading) #[derive(Debug, Clone, Default)] pub struct DiagnosticsSnapshot { @@ -103,6 +152,7 @@ pub struct DiagnosticsSnapshot { pub system_health: SystemHealth, pub wifi_status: WifiStatus, pub config: ConfigSnapshot, + pub fusion: FusionDiagnostics, } /// Thread-safe diagnostics state container @@ -128,6 +178,7 @@ struct DiagnosticsInner { system_health: SystemHealth, wifi_status: WifiStatus, config: ConfigSnapshot, + fusion: FusionDiagnostics, // For rate calculation (last known counts) last_imu_count: u32, last_gps_count: u32, @@ -199,6 +250,12 @@ impl DiagnosticsState { self.zupt_count.fetch_add(1, Ordering::Relaxed); } + /// Get total GPS fix count (for sensor fusion rate tracking) + #[inline] + pub fn gps_fix_count(&self) -> u32 { + self.gps_fix_count.load(Ordering::Relaxed) + } + /// Update sensor rates (call periodically, e.g., every second) pub fn update_rates(&self, now_ms: u32) { let last_ms = self.last_rate_check_ms.swap(now_ms, Ordering::SeqCst); @@ -294,6 +351,14 @@ impl DiagnosticsState { } } + /// Update fusion diagnostics (filter pipeline, GPS blending, calibrators) + #[allow(clippy::too_many_arguments)] + pub fn update_fusion(&self, fusion: FusionDiagnostics) { + if let Ok(mut inner) = self.inner.lock() { + inner.fusion = fusion; + } + } + /// Get a snapshot of all diagnostics (for HTTP response) pub fn snapshot(&self) -> DiagnosticsSnapshot { if let Ok(inner) = self.inner.lock() { @@ -304,6 +369,7 @@ impl DiagnosticsState { system_health: inner.system_health, wifi_status: inner.wifi_status.clone(), config: inner.config.clone(), + fusion: inner.fusion, } } else { DiagnosticsSnapshot::default() diff --git a/sensors/blackbox/src/filter.rs b/sensors/blackbox/src/filter.rs new file mode 100644 index 0000000..90fe6f6 --- /dev/null +++ b/sensors/blackbox/src/filter.rs @@ -0,0 +1,8 @@ +//! Signal filtering module +//! +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::filter`. +//! +//! Note: Currently unused in production - kept for potential future use. + +pub use sensor_fusion::filter::*; diff --git a/sensors/blackbox/src/fusion.rs b/sensors/blackbox/src/fusion.rs new file mode 100644 index 0000000..b919f7a --- /dev/null +++ b/sensors/blackbox/src/fusion.rs @@ -0,0 +1,6 @@ +//! Sensor fusion module +//! +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::fusion`. + +pub use sensor_fusion::fusion::*; diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 7af904f..9607790 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -1,6 +1,9 @@ mod binary_telemetry; mod config; mod diagnostics; +#[cfg(test)] +mod filter; // Only compiled for tests - production uses GPS blend instead of Biquad +mod fusion; mod gps; mod imu; mod mode; @@ -14,17 +17,18 @@ mod wifi; use std::sync::Arc; use config::{SystemConfig, WifiModeConfig}; -use diagnostics::DiagnosticsState; +use diagnostics::{DiagnosticsState, FusionDiagnostics}; use esp_idf_hal::{ delay::FreeRtos, peripherals::Peripherals, uart::{config::Config, UartDriver}, }; use esp_idf_svc::{eventloop::EspSystemEventLoop, nvs::EspDefaultNvsPartition}; +use fusion::{FusionConfig, SensorFusion}; use log::info; use mqtt::MqttClient; use rgb_led::RgbLed; -use sensor_fusion::transforms::{body_to_earth, remove_gravity}; +use sensor_fusion::transforms::{body_to_earth, remove_gravity}; // Used for EKF prediction use system::{SensorManager, StateEstimator, StatusManager, TelemetryPublisher}; use udp_stream::UdpTelemetryStream; use websocket_server::{TelemetryServer, TelemetryServerState}; @@ -442,6 +446,37 @@ fn main() { let mut estimator = StateEstimator::new(); let mut publisher = TelemetryPublisher::new(udp_stream, mqtt_opt, telemetry_state.clone()); + // Create sensor fusion for mode detection + // + // Uses GPS-corrected orientation (ArduPilot-style approach): + // - OrientationCorrector learns pitch/roll errors from GPS velocity comparison + // - IMU orientation is corrected BEFORE gravity removal + // - Enables accurate 200 Hz IMU data instead of being limited to GPS rate + // + // For longitudinal: Blends corrected IMU with GPS based on correction + // confidence + // - High confidence → trust IMU more (fast 200 Hz response) + // - Low confidence → rely on GPS (ground truth but slower) + // + // For lateral: centripetal (speed × yaw_rate) - mount-independent, instant + let fusion_config = FusionConfig { + gps_high_rate: 20.0, + gps_medium_rate: 10.0, + // GPS staleness threshold - must be > GPS interval + processing delay + // At 5Hz GPS (200ms) with ~20Hz telemetry, 400ms gives safe margin + gps_max_age: 0.4, + // GPS weights based on orientation correction confidence: + gps_weight_high: 0.3, // 30% GPS when correction is very confident + gps_weight_medium: 0.6, // 60% GPS when moderately confident + gps_weight_low: 1.0, // 100% GPS when not confident (no correction yet) + tilt_learn_time: 3.0, + // Butterworth filter to remove engine vibration from IMU + // Sample rate must match ACTUAL process_imu() call rate (telemetry rate) + lon_filter_cutoff: 10.0, + lon_sample_rate: 30.0, // 33ms telemetry interval = ~30Hz (from TelemetryConfig::default) + }; + let mut sensor_fusion = SensorFusion::new(fusion_config); + // Main loop timing let mut last_telemetry_ms = 0u32; let mut last_heap_check_ms = 0u32; @@ -489,7 +524,7 @@ fn main() { lat_thr: s.lat_thr, lat_exit: s.lat_exit, yaw_thr: s.yaw_thr, - alpha: 0.25, // Keep default smoothing + alpha: mode::DEFAULT_MODE_ALPHA, // Single source of truth }; estimator.mode_classifier.update_config(new_config); info!( @@ -546,6 +581,29 @@ fn main() { gps_fix.pdop, ); + // Fusion diagnostics (filter pipeline, GPS blending, calibrators) + let (tilt_x, tilt_y, tilt_valid) = sensor_fusion.get_tilt_offsets(); + let (pitch_corr, roll_corr) = sensor_fusion.get_orientation_correction(); + let (pitch_conf, roll_conf) = sensor_fusion.get_orientation_confidence(); + diagnostics.update_fusion(FusionDiagnostics { + lon_imu_raw: sensor_fusion.get_lon_imu_raw(), + lon_imu_filtered: sensor_fusion.get_lon_imu_filtered(), + lon_blended: sensor_fusion.get_lon_blended(), + gps_weight: sensor_fusion.get_last_gps_weight(), + gps_accel: sensor_fusion.get_gps_accel(), + gps_rate: sensor_fusion.get_gps_rate(), + gps_rejected: sensor_fusion.is_gps_rejected(), + pitch_correction_deg: pitch_corr, + roll_correction_deg: roll_corr, + pitch_confidence: pitch_conf, + roll_confidence: roll_conf, + yaw_bias: sensor_fusion.yaw_rate_calibrator.get_bias(), + yaw_calibrated: sensor_fusion.yaw_rate_calibrator.is_valid(), + tilt_offset_x: tilt_x, + tilt_offset_y: tilt_y, + tilt_valid, + }); + publisher.reset_stats(); loop_count = 0; last_heap_check_ms = now_ms; @@ -674,11 +732,30 @@ fn main() { if sensors.poll_gps() { // Count GPS rate only when a NEW valid RMC fix is received // (not on every GGA/GSA sentence while last_fix.valid is still true) - if sensors.gps_parser.take_new_fix() { + let is_new_fix = sensors.gps_parser.take_new_fix(); + if is_new_fix { diagnostics.record_gps_fix(); } - // Check if warmup complete (has reference point) AND fix is valid + // ALWAYS feed GPS speed to sensor fusion when we have a new fix + // GPS acceleration only needs scalar speed - doesn't require full position + // validity This ensures OrientationCorrector can learn even during + // brief GPS hiccups + if sensors.gps_parser.is_warmed_up() && is_new_fix { + let gps_speed = sensors.gps_parser.last_fix().speed; + let time_s = now_ms as f32 / 1000.0; + sensor_fusion.process_gps(gps_speed, time_s); + + // Feed GPS heading to yaw rate calibrator (only valid at speed) + // Course over ground is unreliable below ~5 m/s + if gps_speed > 5.0 { + let course = sensors.gps_parser.last_fix().course; + sensor_fusion.process_gps_heading(course); + } + } + + // Check if warmup complete (has reference point) AND fix is valid for EKF + // updates if sensors.gps_parser.is_warmed_up() && sensors.gps_parser.last_fix().valid { let (ax_corr, ay_corr, _) = sensors.imu_parser.get_accel_corrected(); @@ -711,6 +788,7 @@ fn main() { estimator.reset_speed(); } + // Update EKF with velocity components (requires full validity) if let Some((vx, vy)) = sensors.gps_parser.get_velocity_enu() { let speed = (vx * vx + vy * vy).sqrt(); estimator.update_velocity(vx, vy); @@ -719,6 +797,12 @@ fn main() { } } + // Update GPS rate estimate periodically (every second) + if now_ms % 1000 < 50 { + let fix_count = diagnostics.gps_fix_count(); + sensor_fusion.update_gps_rate(fix_count, now_ms as f32 / 1000.0); + } + // Update GPS status let gps_locked = sensors.gps_parser.is_warmed_up(); // Remove the .valid check status_mgr.update_gps_status( @@ -730,35 +814,40 @@ fn main() { // Publish telemetry at configured rate if now_ms - last_telemetry_ms >= config.telemetry.interval_ms { - // Update mode classifier + // Get current speed and check if stationary let speed = sensors.get_speed(Some(&estimator.ekf)); - let (ax_corr, ay_corr, _) = sensors.imu_parser.get_accel_corrected(); - let (ax_b, ay_b, _) = remove_gravity( - ax_corr, - ay_corr, - sensors.imu_parser.data().az, - sensors.imu_parser.data().roll, - sensors.imu_parser.data().pitch, - ); - let (ax_e, ay_e) = body_to_earth( - ax_b, - ay_b, - 0.0, - sensors.imu_parser.data().roll, - sensors.imu_parser.data().pitch, - estimator.ekf.yaw(), + let (ax_corr, ay_corr, az_corr) = sensors.imu_parser.get_accel_corrected(); + let is_stationary = + sensors.is_stationary(ax_corr, ay_corr, sensors.imu_parser.data().wz); + + // Process through sensor fusion (GPS-corrected orientation + blending) + // Pass raw body-frame IMU data - fusion handles transforms with corrected + // orientation + let dt = config.telemetry.interval_ms as f32 / 1000.0; + let (lon_blended, lat_filtered) = sensor_fusion.process_imu( + ax_corr, // Raw body-frame X accel (bias-corrected) + ay_corr, // Raw body-frame Y accel (bias-corrected) + az_corr, // Raw body-frame Z accel (gravity) + sensors.imu_parser.data().roll, // AHRS roll (degrees) + sensors.imu_parser.data().pitch, // AHRS pitch (degrees) + estimator.ekf.yaw(), // EKF yaw (radians) + speed, + sensors.imu_parser.data().wz, + dt, + is_stationary, ); - estimator.update_mode( - ax_e, - ay_e, - estimator.ekf.yaw(), + + // Update mode classifier with hybrid (blended) acceleration + estimator.mode_classifier.update_hybrid( + lon_blended, + lat_filtered, sensors.imu_parser.data().wz, speed, ); - // Publish telemetry + // Publish telemetry with tilt-corrected accelerations publisher - .publish_telemetry(&sensors, &estimator, now_ms) + .publish_telemetry(&sensors, &estimator, &sensor_fusion, now_ms) .ok(); last_telemetry_ms = now_ms; } diff --git a/sensors/blackbox/src/mode.rs b/sensors/blackbox/src/mode.rs index 2c08820..8c62a47 100644 --- a/sensors/blackbox/src/mode.rs +++ b/sensors/blackbox/src/mode.rs @@ -1,502 +1,6 @@ -#![allow(dead_code)] // API methods for future use +//! Mode detection module +//! +//! Re-exports from the sensor-fusion framework crate. +//! The actual implementation lives in `sensor_fusion::mode`. -/// Driving mode classifier -/// Detects IDLE, ACCEL, BRAKE, CORNER, ACCEL+CORNER, and BRAKE+CORNER modes -/// based on acceleration and yaw rate -use sensor_fusion::transforms::earth_to_car; - -const G: f32 = 9.80665; // m/s² - -/// Driving mode flags - can be combined -pub struct Mode { - flags: u8, -} - -// Mode bit flags -const IDLE: u8 = 0; -const ACCEL: u8 = 1; -const BRAKE: u8 = 2; -const CORNER: u8 = 4; - -impl Mode { - pub fn idle() -> Self { - Self { flags: IDLE } - } - - pub fn is_idle(&self) -> bool { - self.flags == IDLE - } - - pub fn has_accel(&self) -> bool { - self.flags & ACCEL != 0 - } - - pub fn has_brake(&self) -> bool { - self.flags & BRAKE != 0 - } - - pub fn has_corner(&self) -> bool { - self.flags & CORNER != 0 - } - - pub fn set_accel(&mut self, enabled: bool) { - if enabled { - self.flags |= ACCEL; - self.flags &= !BRAKE; // Clear brake (mutually exclusive) - } else { - self.flags &= !ACCEL; - } - } - - pub fn set_brake(&mut self, enabled: bool) { - if enabled { - self.flags |= BRAKE; - self.flags &= !ACCEL; // Clear accel (mutually exclusive) - } else { - self.flags &= !BRAKE; - } - } - - pub fn set_corner(&mut self, enabled: bool) { - if enabled { - self.flags |= CORNER; - } else { - self.flags &= !CORNER; - } - } - - pub fn clear(&mut self) { - self.flags = IDLE; - } - - /// Get mode as string for display - pub fn as_str(&self) -> &'static str { - match self.flags { - 0 => "IDLE", - 1 => "ACCEL", - 2 => "BRAKE", - 4 => "CORNER", - 5 => "ACCEL+CORNER", - 6 => "BRAKE+CORNER", - _ => "UNKNOWN", - } - } - - /// Get mode as u8 for binary telemetry - pub fn as_u8(&self) -> u8 { - self.flags - } -} - -impl Default for Mode { - fn default() -> Self { - Self::idle() - } -} - -/// Configurable thresholds for mode detection -#[derive(Debug, Clone, Copy)] -pub struct ModeConfig { - pub min_speed: f32, // m/s (minimum speed for maneuvers) - pub acc_thr: f32, // g (acceleration threshold) - pub acc_exit: f32, // g (acceleration exit threshold) - pub brake_thr: f32, // g (braking threshold, negative) - pub brake_exit: f32, // g (braking exit threshold, negative) - pub lat_thr: f32, // g (lateral acceleration threshold) - pub lat_exit: f32, // g (lateral acceleration exit threshold) - pub yaw_thr: f32, // rad/s (yaw rate threshold for cornering) - pub alpha: f32, // EMA smoothing factor -} - -impl Default for ModeConfig { - fn default() -> Self { - Self { - min_speed: 2.0, // ~7 km/h - must be moving for accel/brake/corner - acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) - acc_exit: 0.05, // exit when below 0.05g - brake_thr: -0.18, // -0.18g - city driving (normal braking) - brake_exit: -0.09, // exit when above -0.09g - lat_thr: 0.12, // 0.12g lateral - city turns - lat_exit: 0.06, // exit when below 0.06g - yaw_thr: 0.05, // ~2.9°/s yaw rate - alpha: 0.35, // EMA smoothing - balanced responsiveness vs bump rejection - } - } -} - -/// Mode classifier with EMA filtering -pub struct ModeClassifier { - pub mode: Mode, - pub config: ModeConfig, - - // EMA filtered values (all accelerations filtered to reject road bumps) - a_lon_ema: f32, - a_lat_ema: f32, - yaw_ema: f32, - - // Display speed with separate smoothing - v_disp: f32, - v_alpha: f32, // Speed display smoothing factor -} - -impl ModeClassifier { - pub fn new() -> Self { - Self { - mode: Mode::default(), - config: ModeConfig::default(), - a_lon_ema: 0.0, - a_lat_ema: 0.0, - yaw_ema: 0.0, - v_disp: 0.0, - v_alpha: 0.85, // High alpha = fast response to GPS speed changes - } - } - - /// Update mode based on current vehicle state - /// - /// # Arguments - /// * `ax_earth` - Longitudinal acceleration in earth frame (m/s²) - /// * `ay_earth` - Lateral acceleration in earth frame (m/s²) - /// * `yaw_rad` - Vehicle yaw angle (rad) - /// * `wz` - Yaw rate (rad/s) - /// * `speed` - Vehicle speed (m/s) - pub fn update(&mut self, ax_earth: f32, ay_earth: f32, yaw_rad: f32, wz: f32, speed: f32) { - // Update display speed with EMA - self.v_disp = (1.0 - self.v_alpha) * self.v_disp + self.v_alpha * speed; - if self.v_disp < 3.0 / 3.6 { - // < 3 km/h (below walking speed, treat as stationary for display) - self.v_disp = 0.0; - } - - // Transform earth-frame acceleration to car-frame - let (a_lon_ms2, a_lat_ms2) = earth_to_car(ax_earth, ay_earth, yaw_rad); - - // Convert to g units - let a_lon = a_lon_ms2 / G; - let a_lat = a_lat_ms2 / G; - - // Update EMA filters for ALL values (critical for rejecting road bump noise) - let alpha = self.config.alpha; - self.a_lon_ema = (1.0 - alpha) * self.a_lon_ema + alpha * a_lon; - self.a_lat_ema = (1.0 - alpha) * self.a_lat_ema + alpha * a_lat; - self.yaw_ema = (1.0 - alpha) * self.yaw_ema + alpha * wz; - - // Independent state detection for each mode component - // Modes can be combined (e.g., ACCEL+CORNER, BRAKE+CORNER) - - // If stopped, clear all modes - if speed < self.config.min_speed { - self.mode.clear(); - return; - } - - // Check cornering (independent of accel/brake) - let cornering_active = self.a_lat_ema.abs() > self.config.lat_thr - && self.yaw_ema.abs() > self.config.yaw_thr - && (self.a_lat_ema * self.yaw_ema) > 0.0; - - let cornering_exit = self.a_lat_ema.abs() < self.config.lat_exit - && self.yaw_ema.abs() < self.config.yaw_thr * 0.5; - - if self.mode.has_corner() { - // Exit corner if conditions no longer met - if cornering_exit { - self.mode.set_corner(false); - } - } else { - // Enter corner if conditions met - if cornering_active { - self.mode.set_corner(true); - } - } - - // Check acceleration (mutually exclusive with braking) - // Uses filtered a_lon_ema to reject road bump noise - if self.mode.has_accel() { - // Exit if acceleration dropped - if self.a_lon_ema < self.config.acc_exit { - self.mode.set_accel(false); - } - } else if !self.mode.has_brake() { - // Enter accel if not braking and threshold met - if self.a_lon_ema > self.config.acc_thr { - self.mode.set_accel(true); - } - } - - // Check braking (mutually exclusive with acceleration) - // Uses filtered a_lon_ema to reject road bump noise - if self.mode.has_brake() { - // Exit if braking force dropped - if self.a_lon_ema > self.config.brake_exit { - self.mode.set_brake(false); - } - } else if !self.mode.has_accel() { - // Enter brake if not accelerating and threshold met - if self.a_lon_ema < self.config.brake_thr { - self.mode.set_brake(true); - } - } - } - - /// Get current mode - pub fn get_mode(&self) -> &Mode { - &self.mode - } - - /// Get current mode as u8 for telemetry - pub fn get_mode_u8(&self) -> u8 { - self.mode.as_u8() - } - - /// Get display speed in km/h - pub fn get_speed_kmh(&self) -> f32 { - self.v_disp * 3.6 - } - - /// Force speed to zero (for ZUPT) - pub fn reset_speed(&mut self) { - self.v_disp = 0.0; - } - - /// Check if display speed is near zero - pub fn is_stopped(&self) -> bool { - self.v_disp < 0.1 // < 0.36 km/h - } - - /// Get EMA filtered lateral acceleration (for diagnostics) - #[allow(dead_code)] - pub fn get_a_lat_ema(&self) -> f32 { - self.a_lat_ema - } - - /// Get EMA filtered yaw rate (for diagnostics) - #[allow(dead_code)] - pub fn get_yaw_ema(&self) -> f32 { - self.yaw_ema - } - - /// Update configuration (e.g., from MQTT) - pub fn update_config(&mut self, config: ModeConfig) { - self.config = config; - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn test_mode_idle_by_default() { - let classifier = ModeClassifier::new(); - assert!(classifier.get_mode().is_idle()); - } - - #[test] - fn test_acceleration_detection() { - let mut classifier = ModeClassifier::new(); - - // Simulate forward acceleration at 5 m/s (above min_speed of 2.0) - let ax = 0.25 * G; // 0.25g forward - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 5.0; - - // Multiple updates needed for EMA to converge (alpha=0.25) - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel()); - } - - #[test] - fn test_braking_detection() { - let mut classifier = ModeClassifier::new(); - - // Simulate braking at 5 m/s (above min_speed of 2.0) - let ax = -0.3 * G; // 0.3g deceleration - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 5.0; - - // Multiple updates needed for EMA to converge - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_brake()); - } - - #[test] - fn test_no_mode_when_stopped() { - let mut classifier = ModeClassifier::new(); - - // Even with high acceleration, should stay IDLE when speed is 0 - let ax = -0.5 * G; // 0.5g deceleration (would trigger brake) - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed = 0.0; // STOPPED - - classifier.update(ax, ay, yaw, wz, speed); - assert!(classifier.get_mode().is_idle()); // Must stay IDLE when stopped - } - - #[test] - fn test_combined_accel_corner() { - let mut classifier = ModeClassifier::new(); - - // Simulate acceleration while cornering (corner exit) - let ax = 0.20 * G; // 0.20g forward acceleration - let ay = 0.15 * G; // 0.15g lateral - let yaw = 0.0; - let wz = 0.08; // 0.08 rad/s yaw rate - let speed = 8.0; // 8 m/s - - // Multiple updates for EMA convergence - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - - // Should detect both acceleration and cornering - assert!( - classifier.get_mode().has_accel(), - "Should detect acceleration" - ); - assert!( - classifier.get_mode().has_corner(), - "Should detect cornering" - ); - assert_eq!(classifier.get_mode().as_str(), "ACCEL+CORNER"); - } - - #[test] - fn test_combined_brake_corner() { - let mut classifier = ModeClassifier::new(); - - // Simulate braking while cornering (corner entry) - let ax = -0.25 * G; // 0.25g braking - let ay = -0.15 * G; // 0.15g lateral (left turn) - let yaw = 0.0; - let wz = -0.08; // -0.08 rad/s yaw rate (left) - let speed = 8.0; // 8 m/s - - // Multiple updates for EMA convergence - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - - // Should detect both braking and cornering - assert!(classifier.get_mode().has_brake(), "Should detect braking"); - assert!( - classifier.get_mode().has_corner(), - "Should detect cornering" - ); - assert_eq!(classifier.get_mode().as_str(), "BRAKE+CORNER"); - } - - #[test] - fn test_hysteresis_prevents_oscillation() { - let mut classifier = ModeClassifier::new(); - - let speed = 5.0; - let yaw = 0.0; - let wz = 0.0; - let ay = 0.0; - - // Acceleration above entry threshold - multiple updates for EMA - let ax1 = 0.20 * G; // Use higher value to ensure EMA exceeds threshold - for _ in 0..10 { - classifier.update(ax1, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel(), "Should enter ACCEL"); - - // Drop to 0.06g (above 0.05g exit threshold) - EMA should stay above exit - let ax2 = 0.06 * G; - for _ in 0..5 { - classifier.update(ax2, ay, yaw, wz, speed); - } - assert!( - classifier.get_mode().has_accel(), - "Should stay ACCEL (EMA still above exit)" - ); - - // Drop to 0.00g - EMA will gradually fall below exit threshold - let ax3 = 0.00 * G; - for _ in 0..20 { - classifier.update(ax3, ay, yaw, wz, speed); - } - assert!( - classifier.get_mode().is_idle(), - "Should exit to IDLE (EMA below exit)" - ); - } - - #[test] - fn test_speed_threshold_prevents_false_modes() { - let mut classifier = ModeClassifier::new(); - - // High acceleration but speed below threshold - let ax = 0.30 * G; - let ay = 0.0; - let yaw = 0.0; - let wz = 0.0; - let speed_low = 1.5; // Below 2.0 m/s threshold - - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed_low); - } - assert!( - classifier.get_mode().is_idle(), - "Should stay IDLE when speed < min_speed" - ); - - // Same acceleration, speed above threshold - let speed_high = 2.5; - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed_high); - } - assert!( - classifier.get_mode().has_accel(), - "Should detect ACCEL when speed > min_speed" - ); - } - - #[test] - fn test_corner_independent_persistence() { - let mut classifier = ModeClassifier::new(); - - // Start cornering - multiple updates for EMA - let ax = 0.0; - let ay = 0.15 * G; - let yaw = 0.0; - let wz = 0.08; - let speed = 8.0; - - for _ in 0..10 { - classifier.update(ax, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_corner(), "Should detect corner"); - - // Add acceleration while still cornering - let ax2 = 0.20 * G; - for _ in 0..10 { - classifier.update(ax2, ay, yaw, wz, speed); - } - assert!(classifier.get_mode().has_accel(), "Should detect accel"); - assert!( - classifier.get_mode().has_corner(), - "Should still detect corner" - ); - assert_eq!(classifier.get_mode().as_u8(), 5, "Should be ACCEL+CORNER"); - - // Stop accelerating but keep cornering - EMA needs to drop - let ax3 = 0.0; - for _ in 0..20 { - classifier.update(ax3, ay, yaw, wz, speed); - } - assert!(!classifier.get_mode().has_accel(), "Should exit accel"); - assert!(classifier.get_mode().has_corner(), "Should still corner"); - } -} +pub use sensor_fusion::mode::*; diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index 2fc057b..c50c474 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -336,18 +336,29 @@ impl TelemetryPublisher { &mut self, sensors: &SensorManager, estimator: &StateEstimator, + sensor_fusion: &crate::fusion::SensorFusion, now_ms: u32, ) -> Result<(), SystemError> { - // Send bias-corrected accelerations (client can process further if needed) - let (ax_corr, ay_corr, az_corr) = sensors.imu_parser.get_accel_corrected(); + // Get accelerations for display + // lon: GPS-derived when fresh (no vibration), otherwise blended with heavy + // smoothing lat: centripetal (speed × yaw_rate) - instant, doesn't + // stick after corners Uses calibrated yaw rate to prevent drift on + // highway + let lon_display = sensor_fusion.get_lon_display(); + let lat_display = sensor_fusion.get_lat_centripetal(); + + // Keep raw az for reference (no tilt correction needed for vertical) + let (_, _, az_corr) = sensors.imu_parser.get_accel_corrected(); let mut packet = binary_telemetry::TelemetryPacket::new(); packet.timestamp_ms = now_ms; - // Send bias-corrected accelerations in body frame - // Client-side processing can remove gravity and transform to vehicle frame - packet.ax = ax_corr; - packet.ay = ay_corr; + // Send corrected vehicle-frame accelerations + // Dashboard expects: lng = -ax/9.81, latg = ay/9.81 + // So: ax = -lon (negated so dashboard shows positive for accel) + // ay = lat (positive = right turn on G-meter) + packet.ax = -lon_display; + packet.ay = -lat_display; // Negate: fusion uses left-positive, dashboard uses right-positive packet.az = az_corr; packet.wz = sensors.imu_parser.data().wz; packet.roll = sensors.imu_parser.data().roll.to_radians(); diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 1e94344..84960b9 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -26,15 +26,15 @@ pub struct ModeSettings { impl Default for ModeSettings { fn default() -> Self { - // Matches city preset - sensitive defaults for street driving + // Matches city preset - must stay in sync with mode.rs ModeConfig::default() Self { acc_thr: 0.10, // 0.10g - city driving (gentle acceleration) acc_exit: 0.05, // Exit threshold - brake_thr: -0.18, // -0.18g - city driving (normal braking) - brake_exit: -0.09, // Exit threshold - lat_thr: 0.12, // 0.12g - city turns - lat_exit: 0.06, // Exit threshold - yaw_thr: 0.05, // ~2.9°/s yaw rate + brake_thr: -0.15, // -0.15g - sensitive brake detection (compensates for mounting bias) + brake_exit: -0.08, // Exit threshold + lat_thr: 0.10, // 0.10g - sensitive corner detection + lat_exit: 0.05, // Exit threshold + yaw_thr: 0.04, // ~2.3°/s yaw rate min_speed: 2.0, // ~7 km/h } } @@ -299,20 +299,20 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
Accel0.10/0.05g
-
Brake0.18/0.09g
-
Lateral0.12/0.06g
-
Yaw0.05 r/s
+
Brake0.15/0.08g
+
Lateral0.10/0.05g
+
Yaw0.04 r/s
Min Speed2.0 m/s
-
Accel0.10g
-
Acc Exit0.05g
-
Brake0.18g
-
Brk Exit0.09g
-
Lateral0.12g
-
Lat Exit0.06g
-
Yaw0.050r/s
-
Min Spd2.0m/s
+
Accel0.10g
+
Acc Exit0.05g
+
Brake0.15g
+
Brk Exit0.08g
+
Lateral0.10g
+
Lat Exit0.05g
+
Yaw0.040r/s
+
Min Spd2.0m/s
@@ -328,6 +328,9 @@ let trail=[],maxL=0,maxR=0,maxA=0,maxB=0,maxSpd=0; let speed_ema=0; let sessionStart=Date.now(); let currentPreset='city'; +// Fusion diagnostics for CSV export (fetched periodically during recording) +let fusion={lon_imu:0,lon_gps:0,gps_wt:0,pitch_c:0,pitch_cf:0,roll_c:0,roll_cf:0,tilt_x:0,tilt_y:0}; +let fusionPoll=0; const $=id=>document.getElementById(id); const cv=$('gfc'),ctx=cv.getContext('2d'); const CX=70,CY=70,R=55,SCL=R/2; @@ -338,10 +341,10 @@ const CX=70,CY=70,R=55,SCL=R/2; // Canyon: spirited driving (0.20-0.40g range) // Track: racing (0.35-0.80g+ range) const PRESETS={ -track:{acc:0.35,acc_exit:0.17,brake:0.55,brake_exit:0.27,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:4.0,desc:'Racing/track days'}, -canyon:{acc:0.22,acc_exit:0.11,brake:0.35,brake_exit:0.17,lat:0.28,lat_exit:0.14,yaw:0.10,min_speed:3.0,desc:'Spirited mountain roads'}, -city:{acc:0.10,acc_exit:0.05,brake:0.18,brake_exit:0.09,lat:0.12,lat_exit:0.06,yaw:0.05,min_speed:2.0,desc:'Daily street driving'}, -highway:{acc:0.12,acc_exit:0.06,brake:0.22,brake_exit:0.11,lat:0.14,lat_exit:0.07,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} +track:{acc:0.35,acc_exit:0.17,brake:0.35,brake_exit:0.17,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:4.0,desc:'Racing/track days'}, +canyon:{acc:0.22,acc_exit:0.11,brake:0.22,brake_exit:0.11,lat:0.28,lat_exit:0.14,yaw:0.10,min_speed:3.0,desc:'Spirited mountain roads'}, +city:{acc:0.10,acc_exit:0.05,brake:0.15,brake_exit:0.08,lat:0.10,lat_exit:0.05,yaw:0.04,min_speed:2.0,desc:'Daily street driving'}, +highway:{acc:0.12,acc_exit:0.06,brake:0.15,brake_exit:0.08,lat:0.12,lat_exit:0.06,yaw:0.04,min_speed:5.0,desc:'Highway cruising'} }; function fmtTime(ms){const s=Math.floor(ms/1000),m=Math.floor(s/60);return String(m).padStart(2,'0')+':'+String(s%60).padStart(2,'0')} @@ -438,7 +441,7 @@ if(lng>0&&lng>maxA){maxA=lng;$('maxA').textContent=maxA.toFixed(2)} if(lng<0&&Math.abs(lng)>maxB){maxB=Math.abs(lng);$('maxB').textContent=maxB.toFixed(2)} drawG(); cnt++; -if(rec)data.push({t:Date.now(),sp,ax,ay,wz,mo,latg,lng,lat,lon,gpsOk}); +if(rec)data.push({t:Date.now(),sp,ax,ay,wz,mo,latg,lng,lat,lon,gpsOk,...fusion}); } // HTTP polling - self-scheduling for maximum throughput @@ -452,11 +455,32 @@ const b=atob(j.data),a=new Uint8Array(b.length); for(let i=0;i=5){fusionPoll=0;fetchFusion()} } setTimeout(poll,33); // Poll at ~30Hz to match ESP32 telemetry rate }catch(e){$('dot').className='dot';$('stxt').textContent='Offline';setTimeout(poll,500)} // Retry slower on error } +// Fetch fusion diagnostics for CSV export (non-blocking) +async function fetchFusion(){ +try{ +const r=await fetch('/api/diagnostics'); +const d=await r.json(); +if(d.fusion){ +fusion.lon_imu=d.fusion.lon_filtered||0; +fusion.lon_gps=d.fusion.gps_accel||0; +fusion.gps_wt=d.fusion.gps_weight||0; +fusion.pitch_c=d.fusion.pitch_corr||0; +fusion.pitch_cf=d.fusion.pitch_conf||0; +fusion.roll_c=d.fusion.roll_corr||0; +fusion.roll_cf=d.fusion.roll_conf||0; +fusion.tilt_x=d.fusion.tilt_x||0; +fusion.tilt_y=d.fusion.tilt_y||0; +} +}catch(e){} +} + // Reset button: resets G max, speed max, timer, and triggers calibration $('rstg').onclick=resetAll; $('gfc').ondblclick=resetGMax; @@ -474,7 +498,7 @@ if(data.length){const s=JSON.parse(localStorage.getItem('bb')||'[]');s.unshift({ localStorage.setItem('bb',JSON.stringify(s.slice(0,10)));alert('Saved '+data.length+' pts')}}}; $('exp').onclick=()=>{const s=JSON.parse(localStorage.getItem('bb')||'[]');if(!s.length)return alert('No data'); -let c='time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid\n';s[0].d.forEach(r=>{c+=r.t+','+r.sp+','+r.ax+','+r.ay+','+r.wz+','+r.mo+','+r.latg+','+r.lng+','+(r.lat||0)+','+(r.lon||0)+','+(r.gpsOk||0)+'\n'}); +let c='time,speed,ax,ay,wz,mode,lat_g,lon_g,gps_lat,gps_lon,gps_valid,lon_imu,lon_gps,gps_weight,pitch_corr,pitch_conf,roll_corr,roll_conf,tilt_x,tilt_y\n';s[0].d.forEach(r=>{c+=r.t+','+r.sp+','+r.ax+','+r.ay+','+r.wz+','+r.mo+','+r.latg+','+r.lng+','+(r.lat||0)+','+(r.lon||0)+','+(r.gpsOk||0)+','+(r.lon_imu||0).toFixed(4)+','+(r.lon_gps||0).toFixed(4)+','+(r.gps_wt||0).toFixed(2)+','+(r.pitch_c||0).toFixed(2)+','+(r.pitch_cf||0).toFixed(1)+','+(r.roll_c||0).toFixed(2)+','+(r.roll_cf||0).toFixed(1)+','+(r.tilt_x||0).toFixed(4)+','+(r.tilt_y||0).toFixed(4)+'\n'}); const b=new Blob([c],{type:'text/csv'}),u=URL.createObjectURL(b),a=document.createElement('a');a.href=u;a.download='blackbox.csv';a.click()}; // Settings functions @@ -660,6 +684,28 @@ h1{font-size:18px;margin-bottom:16px;color:#60a5fa;letter-spacing:2px}
TX Failed--
+
+

Sensor Fusion

+
+
+
Lon Raw--
+
Lon Filtered--
+
Lon Blended--
+
GPS Weight--
+
GPS Accel--
+
GPS Rejected--
+
+
+
Pitch Corr--
+
Roll Corr--
+
Orient Conf--
+
Yaw Bias--
+
Yaw Calibrated--
+
Tilt X/Y--
+
Tilt Valid--
+
+
+
Uptime: --