diff --git a/CLAUDE.md b/CLAUDE.md index ba94932..0555441 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -28,6 +28,13 @@ You are a seasoned embedded Rust engineer with deep expertise in: - Avoid over-engineering: solve the problem at hand, not hypothetical future problems - Keep abstractions minimal until they prove necessary +**SOLID Principles:** +- **Single Responsibility**: Each module/struct should have one reason to change. Keep sensors, protocols, and business logic separate. +- **Open/Closed**: Design for extension without modification. Use traits to allow new sensor types or protocols without changing core code. +- **Liskov Substitution**: Implementations of traits must be substitutable. If `GpsSensor` trait exists, any implementation should work interchangeably. +- **Interface Segregation**: Prefer small, focused traits over large ones. Don't force implementations to depend on methods they don't use. +- **Dependency Inversion**: High-level modules should depend on abstractions (traits), not concrete implementations. Pass dependencies via constructors or function parameters. + **Testing Philosophy:** - Write meaningful tests that catch real bugs, not boilerplate - **Every test MUST have assertions** (`assert!`, `assert_eq!`, etc.) - a test without assertions proves nothing @@ -645,6 +652,132 @@ Displayed as updates per minute (rolling average), not cumulative count. Typical - ZUPT: 0-60/min (depends on stops) - Free heap: ~60KB (stable during operation) +### settings.rs - NVS Settings Persistence + +**Purpose:** +Persists mode detection thresholds to ESP32's Non-Volatile Storage (NVS) so they survive power cycles. + +**Functions:** +- `load_settings(nvs_partition)`: Loads `ModeConfig` from NVS, returns `None` if not found +- `save_settings(nvs_partition, config)`: Saves `ModeConfig` to NVS, returns success/failure + +**NVS Schema:** +- **Namespace:** `bb_cfg` +- **Keys (max 15 chars for NVS):** + - `acc` - acceleration entry threshold (f32) + - `acc_x` - acceleration exit threshold (f32) + - `brk` - brake entry threshold (f32) + - `brk_x` - brake exit threshold (f32) + - `lat` - lateral entry threshold (f32) + - `lat_x` - lateral exit threshold (f32) + - `yaw` - yaw rate threshold (f32) + - `spd` - minimum speed (f32) + +**Storage Format:** +Values stored as raw 4-byte little-endian floats (`f32::to_le_bytes()`). + +**Usage in main.rs:** +```rust +// On boot: load saved settings +let saved_config = nvs.as_ref().and_then(|p| settings::load_settings(p.clone())); +if let Some(cfg) = saved_config { + estimator.mode_classifier.update_config(cfg); +} + +// On settings change: save to NVS +if let Some(ref nvs) = nvs_for_settings { + settings::save_settings(nvs.clone(), &new_config); +} +``` + +**Note:** `alpha` (EMA filter) is NOT persisted - always uses mode.rs default (0.35). + +### Autotune System + +**Overview:** +Autotune learns mode detection thresholds by driving in the selected mode. Each profile (City, Canyon, Track, Highway) is calibrated independently - no scaling between profiles. + +**Key Design:** +- User selects which mode to calibrate (Track/Canyon/City/Highway) +- Drives in that style for 10-20 minutes +- Thresholds computed directly from that driving style +- No arbitrary scaling factors between profiles +- Each mode calibrated by actually driving in that mode + +**Data Flow:** +``` +Select Mode → Drive → Event Detection → IQR Filter → Median → NVS + localStorage + │ │ │ │ │ │ + └─►Track └─►ax,ay,wz └─►ACCEL/BRAKE/ └─►Outlier └─►P50×0.7 └─►bb_cfg + CORNER bins rejection (active) +``` + +**Event Detection Algorithm:** +```javascript +// Parameters tuned for 25Hz polling to match 200Hz mode.rs dynamics +const EMA_ALPHA = 0.85; // Adjusted for sample rate mismatch +const NOISE_FLOOR = 0.08; // 0.08g - reject road bumps +const EVENT_MIN_DURATION = 200; // ms - reject transients +const EVENT_COOLDOWN = 400; // ms - gap between events +const MIN_SPEED_KMH = 7.2; // 2.0 m/s + +// EMA filter matching mode.rs dynamics +emaLonSim = (1 - EMA_ALPHA) * emaLonSim + EMA_ALPHA * lonG; + +// Event detection with noise floor +if (emaLonSim > NOISE_FLOOR && speed > MIN_SPEED_KMH) { + // Start/continue ACCEL event, track peak +} +// BRAKE: emaLonSim < -NOISE_FLOOR +// TURN: |emaLatSim| > NOISE_FLOOR && |emaYawSim| > 0.02 && sign consistent +``` + +**Threshold Calculation:** +```javascript +// IQR outlier rejection +function filterOutliers(arr) { + const q1 = percentile(arr, 25), q3 = percentile(arr, 75); + const iqr = q3 - q1; + return arr.filter(v => v >= q1 - 1.5*iqr && v <= q3 + 1.5*iqr); +} + +// Compute thresholds from collected events +const peaks = filterOutliers(events.map(e => e.peak)); +const P50 = median(peaks); +const entry = P50 * 0.7; // 70% of median +const exit = entry * 0.5; // 50% of entry (hysteresis) +``` + +**NVS + localStorage Storage:** +```javascript +// On Apply: +// 1. Send to ESP32 → saves to NVS (bb_cfg namespace) +await fetch('/api/settings/set?acc=...&brake=...'); + +// 2. Save to browser localStorage for selected mode +let profiles = JSON.parse(localStorage.getItem('bb_profiles') || '{}'); +profiles[selectedMode] = {...calibratedProfile, calibrated: new Date().toISOString()}; +profiles.lastMode = selectedMode; +localStorage.setItem('bb_profiles', JSON.stringify(profiles)); +``` + +**Boot Behavior:** +- ESP32 reads thresholds from NVS on boot +- If NVS has valid data → applies to mode classifier +- If NVS empty → uses City defaults (mode.rs `ModeConfig::default()`) + +**Confidence Levels:** +- HIGH: n ≥ 15 events per category +- MEDIUM: n = 8-14 events +- LOW: n = 3-7 events +- INSUFFICIENT: n < 3 + +**Integration with Dashboard:** +- User calibrates each mode by selecting it and driving in that style +- Clicking preset loads calibrated values (or defaults if uncalibrated) +- Export button downloads comprehensive JSON for analysis +- Progress bar shows real-time event capture during calibration + ### udp_stream.rs - UDP Client (Station Mode) **Features:** diff --git a/README.md b/README.md index 62d68f2..dc6ec5c 100644 --- a/README.md +++ b/README.md @@ -711,6 +711,113 @@ You can trigger recalibration from the dashboard: --- +## Autotune (Threshold Calibration) + +The **Autotune** feature learns your vehicle's characteristics by driving in the mode you want to calibrate. Each profile (City, Canyon, Track, Highway) is calibrated independently - no arbitrary scaling. Calibrated settings persist across power cycles via NVS. + +### Why Autotune? + +Default presets use generic thresholds, but every vehicle and driver is different: +- A sports car can pull 1.0g lateral; a minivan might max at 0.5g +- Track driving produces different g-forces than city driving +- Calibrate each mode by actually driving in that style + +### How to Use Autotune + +1. **Open the dashboard** at `http://192.168.71.1` +2. **Tap "Autotune"** in the menu +3. **Select the mode to calibrate** (City, Canyon, Track, or Highway) +4. **Wait for GPS lock** (green GPS status required) +5. **Tap "Start Calibration"** and drive in that style for 10-20 minutes +6. **Collect events**: accelerations, brakes, and turns +7. **Review thresholds** - computed from YOUR driving +8. **Tap "Apply"** to save to ESP32 (NVS) and browser + +### Mode-Specific Calibration + +Unlike previous versions that scaled from a single baseline, each mode is now calibrated independently: + +| Mode | Calibration Drive Style | +|------|------------------------| +| **City** | Normal commute - stop lights, lane changes, parking | +| **Canyon** | Spirited mountain roads - tighter turns, harder braking | +| **Track** | Racing/track day - aggressive accel, hard braking, high-g turns | +| **Highway** | Highway cruising - gentle lane changes, gradual accel/brake | + +**Calibrate each mode separately** by driving in that style. This eliminates guesswork from scaling factors. + +### Algorithm Details + +Autotune uses physics-informed signal processing matching `mode.rs`: + +**Event Detection:** +- **EMA filtering** at α=0.85 (tuned for 25Hz polling to match 200Hz firmware) +- **Noise floor** of 0.08g rejects road bumps and vibration +- **Duration filter** of 200ms ensures intentional inputs +- **Cooldown** of 400ms between events prevents double-counting +- **Sign consistency** for turns: lateral G and yaw rate must agree + +**Threshold Calculation:** +1. Collect peak G-forces from each event type (accel, brake, turn) +2. Apply **IQR outlier rejection** (1.5× interquartile range) +3. Compute **median (P50)** of filtered peaks +4. Entry threshold = **70% of median** +5. Exit threshold = **50% of entry** (hysteresis) + +**min_speed** is fixed at 2.0 m/s (7.2 km/h) - physics doesn't change with driving style. + +### NVS Persistence + +Calibrated settings persist in ESP32's Non-Volatile Storage: + +**On Apply:** +- Thresholds sent to ESP32 via `/api/settings/set` +- ESP32 saves to NVS namespace `bb_cfg` +- Browser saves profile to `localStorage` for dashboard + +**On Boot:** +- ESP32 reads thresholds from NVS +- If valid, applies to mode classifier automatically +- If NVS is empty/invalid, uses **City defaults** (acc=0.10g, brake=0.18g, lat=0.12g) +- No manual configuration needed after calibration + +**Storage Keys:** `acc`, `acc_x`, `brk`, `brk_x`, `lat`, `lat_x`, `yaw`, `spd` + +### Confidence Levels + +| Events | Confidence | Recommendation | +|--------|------------|----------------| +| 3-7 each | Low | Keep driving - thresholds may be noisy | +| 8-14 each | Medium | Good for most uses | +| 15+ each | High | Robust outlier rejection, accurate medians | + +### Export Data + +Exports JSON with mode-specific calibration data: +```json +{ + "version": 2, + "mode": "track", + "exportDate": "2024-01-15T10:30:00Z", + "calibrated": { "acc": 0.35, "brake": 0.50, ... }, + "events": { + "accel": [{"peak": 0.45, "dur": 450, "dv": 18.3}, ...], + "brake": [...], + "turn": [...] + } +} +``` + +### Tips for Best Results + +- **Drive in the style you're calibrating** - Track mode = aggressive, City = normal +- **Calibrate each mode separately** - switch modes and recalibrate +- **Include variety** - different turn directions, brake intensities +- **More driving = better** - 15+ events each gives high confidence +- **GPS lock required** - ensures accurate speed filtering + +--- + ## Mobile Dashboard The firmware includes a built-in web dashboard that runs directly on the ESP32. No external server needed - just connect your phone and view live telemetry. diff --git a/docs/index.html b/docs/index.html index 9270172..f9cc4e5 100644 --- a/docs/index.html +++ b/docs/index.html @@ -986,6 +986,16 @@

Commercial Systems

Examples: AiM Solo 2 DL (~$1,200), Racelogic VBOX Sport (~$1,800), Motec systems ($3,000+)

+ +
+

What about phone apps?

+

+ Apps like Harry's LapTimer use your phone's 1Hz GPS—one position per second. At 100 km/h, that's 28 meters between updates. Racing lines are interpolated guesses. Lap times are ±0.3s at best. No sensor fusion, just raw GPS dots. +

+

+ Blackbox with 25Hz GPS captures position every 1-2 meters. The EKF fuses 200Hz IMU between GPS updates for continuous tracking—the same approach in commercial systems. Phone apps are fine for casual use. Dedicated hardware is for actual accuracy. +

+
@@ -1063,6 +1073,21 @@

ZUPT

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

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

Autotune

+

Learn vehicle-specific thresholds by driving in each mode. Calibrate Track, City, Canyon, and Highway independently.

+
+
@@ -1127,6 +1152,10 @@

Dashboard Features

Driving presets: Track, Canyon, City, Highway, Custom +
  • + + Autotune: calibrate thresholds to your vehicle +
  • Record & export to CSV diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 7af904f..9cc3bc4 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -6,6 +6,7 @@ mod imu; mod mode; mod mqtt; mod rgb_led; +mod settings; mod system; mod udp_stream; mod websocket_server; @@ -69,6 +70,9 @@ fn main() { let sysloop = EspSystemEventLoop::take().unwrap(); let nvs = EspDefaultNvsPartition::take().ok(); + // Keep a clone of NVS partition for settings operations + let nvs_for_settings = nvs.as_ref().cloned(); + // Create LED for status indication let led = RgbLed::new(peripherals.rmt.channel0, peripherals.pins.gpio8) .expect("Failed to initialize RGB LED"); @@ -440,6 +444,30 @@ fn main() { // Create state estimator and telemetry publisher let mut estimator = StateEstimator::new(); + + // Load active mode from NVS and apply its settings + let active_mode = nvs_for_settings + .as_ref() + .map(|p| settings::load_active_mode(p.clone())) + .unwrap_or(settings::DrivingMode::City); + + let mode_config = nvs_for_settings + .as_ref() + .map(|p| settings::load_mode_settings(p.clone(), active_mode)) + .unwrap_or_else(|| active_mode.default_config()); + + info!("Active mode: {} - applying settings", active_mode.name()); + estimator.mode_classifier.update_config(mode_config); + + // Update telemetry state with current mode and settings + if let Some(ref state) = telemetry_state { + state.set_current_mode(active_mode.into()); + state.set_settings(mode_config.into()); + // Clear any pending flags since we just loaded, not user-changed + let _ = state.take_settings_change(); + let _ = state.take_mode_change(); + } + let mut publisher = TelemetryPublisher::new(udp_stream, mqtt_opt, telemetry_state.clone()); // Main loop timing @@ -489,9 +517,16 @@ fn main() { lat_thr: s.lat_thr, lat_exit: s.lat_exit, yaw_thr: s.yaw_thr, - alpha: 0.25, // Keep default smoothing + alpha: 0.35, // EMA smoothing factor }; estimator.mode_classifier.update_config(new_config); + + // Persist to NVS for current mode + if let Some(ref nvs) = nvs_for_settings { + let driving_mode: settings::DrivingMode = state.get_current_mode().into(); + settings::save_mode_settings(nvs.clone(), driving_mode, &new_config); + } + info!( ">>> Mode config updated: acc={:.2}g/{:.2}g, brake={:.2}g/{:.2}g, lat={:.2}g/{:.2}g, yaw={:.3}rad/s, min_spd={:.1}m/s", s.acc_thr, s.acc_exit, s.brake_thr, s.brake_exit, s.lat_thr, s.lat_exit, s.yaw_thr, s.min_speed @@ -506,6 +541,48 @@ fn main() { } status_mgr.led_mut().set_low().unwrap(); } + + // Check for mode change request from web UI + if let Some(new_mode) = state.take_mode_change() { + let driving_mode: settings::DrivingMode = new_mode.into(); + + // Load settings for the new mode from NVS + let new_config = if let Some(ref nvs) = nvs_for_settings { + settings::load_mode_settings(nvs.clone(), driving_mode) + } else { + driving_mode.default_config() + }; + + // Apply to mode classifier + estimator.mode_classifier.update_config(new_config); + + // Save the new active mode to NVS + if let Some(ref nvs) = nvs_for_settings { + settings::save_active_mode(nvs.clone(), driving_mode); + } + + // Update telemetry state + state.set_current_mode(new_mode); + state.set_settings(new_config.into()); + + info!( + ">>> Mode changed to {}: acc={:.2}g, brake={:.2}g, lat={:.2}g, yaw={:.3}rad/s", + driving_mode.name(), + new_config.acc_thr, + new_config.brake_thr, + new_config.lat_thr, + new_config.yaw_thr + ); + + // Visual confirmation: 3 cyan-white alternating flashes + for _ in 0..3 { + status_mgr.led_mut().cyan().unwrap(); + FreeRtos::delay_ms(80); + status_mgr.led_mut().white().unwrap(); + FreeRtos::delay_ms(80); + } + status_mgr.led_mut().set_low().unwrap(); + } } // Periodic diagnostics (serial log every 5s) diff --git a/sensors/blackbox/src/settings.rs b/sensors/blackbox/src/settings.rs new file mode 100644 index 0000000..abe3346 --- /dev/null +++ b/sensors/blackbox/src/settings.rs @@ -0,0 +1,261 @@ +//! NVS-backed settings persistence for mode detection thresholds +//! Supports multiple driving profiles (track, canyon, city, highway, custom) + +use esp_idf_svc::nvs::{EspNvs, EspNvsPartition, NvsDefault}; +use log::{info, warn}; + +use crate::mode::ModeConfig; + +const NAMESPACE: &str = "bb_cfg"; + +// Key for active mode (max 15 chars for NVS keys) +const KEY_MODE: &str = "mode"; + +/// Driving mode profiles +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum DrivingMode { + Track, + Canyon, + City, + Highway, + Custom, +} + +impl DrivingMode { + /// Get the NVS key prefix for this mode (single char to save space) + fn prefix(&self) -> &'static str { + match self { + DrivingMode::Track => "t", + DrivingMode::Canyon => "n", // 'n' for caNyon (c is taken by city) + DrivingMode::City => "c", + DrivingMode::Highway => "h", + DrivingMode::Custom => "x", + } + } + + /// Get mode name for logging/display + pub fn name(&self) -> &'static str { + match self { + DrivingMode::Track => "track", + DrivingMode::Canyon => "canyon", + DrivingMode::City => "city", + DrivingMode::Highway => "highway", + DrivingMode::Custom => "custom", + } + } + + /// Parse mode from string + pub fn from_str(s: &str) -> Option { + match s.to_lowercase().as_str() { + "track" => Some(DrivingMode::Track), + "canyon" => Some(DrivingMode::Canyon), + "city" => Some(DrivingMode::City), + "highway" => Some(DrivingMode::Highway), + "custom" => Some(DrivingMode::Custom), + _ => None, + } + } + + /// Get default config for this mode + pub fn default_config(&self) -> ModeConfig { + match self { + DrivingMode::Track => ModeConfig { + acc_thr: 0.30, + acc_exit: 0.15, + brake_thr: -0.50, + brake_exit: -0.25, + lat_thr: 0.50, + lat_exit: 0.25, + yaw_thr: 0.15, + min_speed: 3.0, + alpha: 0.35, + }, + DrivingMode::Canyon => ModeConfig { + acc_thr: 0.20, + acc_exit: 0.10, + brake_thr: -0.35, + brake_exit: -0.17, + lat_thr: 0.30, + lat_exit: 0.15, + yaw_thr: 0.10, + min_speed: 2.5, + alpha: 0.35, + }, + DrivingMode::City => ModeConfig { + acc_thr: 0.10, + acc_exit: 0.05, + brake_thr: -0.18, + brake_exit: -0.09, + lat_thr: 0.12, + lat_exit: 0.06, + yaw_thr: 0.05, + min_speed: 2.0, + alpha: 0.35, + }, + DrivingMode::Highway => ModeConfig { + acc_thr: 0.08, + acc_exit: 0.04, + brake_thr: -0.15, + brake_exit: -0.07, + lat_thr: 0.10, + lat_exit: 0.05, + yaw_thr: 0.04, + min_speed: 4.0, + alpha: 0.35, + }, + DrivingMode::Custom => ModeConfig { + // Custom defaults to City + acc_thr: 0.10, + acc_exit: 0.05, + brake_thr: -0.18, + brake_exit: -0.09, + lat_thr: 0.12, + lat_exit: 0.06, + yaw_thr: 0.05, + min_speed: 2.0, + alpha: 0.35, + }, + } + } +} + +/// Load the active mode from NVS, defaults to City +pub fn load_active_mode(nvs_partition: EspNvsPartition) -> DrivingMode { + let nvs = match EspNvs::new(nvs_partition, NAMESPACE, true) { + Ok(nvs) => nvs, + Err(_) => return DrivingMode::City, + }; + + let mut buf = [0u8; 16]; + match nvs.get_raw(KEY_MODE, &mut buf) { + Ok(Some(data)) => { + // Find null terminator or use full length + let len = data.iter().position(|&b| b == 0).unwrap_or(data.len()); + let s = core::str::from_utf8(&data[..len]).unwrap_or("city"); + DrivingMode::from_str(s).unwrap_or(DrivingMode::City) + } + _ => DrivingMode::City, + } +} + +/// Save the active mode to NVS +pub fn save_active_mode(nvs_partition: EspNvsPartition, mode: DrivingMode) -> bool { + let mut nvs = match EspNvs::new(nvs_partition, NAMESPACE, true) { + Ok(nvs) => nvs, + Err(e) => { + warn!("Failed to open NVS for mode save: {:?}", e); + return false; + } + }; + + nvs.set_raw(KEY_MODE, mode.name().as_bytes()).is_ok() +} + +/// Load settings for a specific mode from NVS +/// Returns the mode's default config if not found in NVS +pub fn load_mode_settings( + nvs_partition: EspNvsPartition, + mode: DrivingMode, +) -> ModeConfig { + let nvs = match EspNvs::new(nvs_partition, NAMESPACE, true) { + Ok(nvs) => nvs, + Err(_) => return mode.default_config(), + }; + + let prefix = mode.prefix(); + + // Try to load all values - if any missing, use mode defaults + let acc = read_f32_prefixed(&nvs, prefix, "acc"); + let acc_exit = read_f32_prefixed(&nvs, prefix, "acc_x"); + let brake = read_f32_prefixed(&nvs, prefix, "brk"); + let brake_exit = read_f32_prefixed(&nvs, prefix, "brk_x"); + let lat = read_f32_prefixed(&nvs, prefix, "lat"); + let lat_exit = read_f32_prefixed(&nvs, prefix, "lat_x"); + let yaw = read_f32_prefixed(&nvs, prefix, "yaw"); + let min_speed = read_f32_prefixed(&nvs, prefix, "spd"); + + // If all values present, use them; otherwise use defaults + if let (Some(a), Some(ae), Some(b), Some(be), Some(l), Some(le), Some(y), Some(s)) = ( + acc, acc_exit, brake, brake_exit, lat, lat_exit, yaw, min_speed, + ) { + info!( + "Loaded {} profile from NVS: acc={:.2}, brake={:.2}, lat={:.2}, yaw={:.3}", + mode.name(), + a, + b, + l, + y + ); + ModeConfig { + acc_thr: a, + acc_exit: ae, + brake_thr: b, + brake_exit: be, + lat_thr: l, + lat_exit: le, + yaw_thr: y, + min_speed: s, + alpha: 0.35, + } + } else { + info!("Using default {} profile (not found in NVS)", mode.name()); + mode.default_config() + } +} + +/// Save settings for a specific mode to NVS +pub fn save_mode_settings( + nvs_partition: EspNvsPartition, + mode: DrivingMode, + config: &ModeConfig, +) -> bool { + let mut nvs = match EspNvs::new(nvs_partition, NAMESPACE, true) { + Ok(nvs) => nvs, + Err(e) => { + warn!("Failed to open NVS for settings save: {:?}", e); + return false; + } + }; + + let prefix = mode.prefix(); + + let success = write_f32_prefixed(&mut nvs, prefix, "acc", config.acc_thr) + && write_f32_prefixed(&mut nvs, prefix, "acc_x", config.acc_exit) + && write_f32_prefixed(&mut nvs, prefix, "brk", config.brake_thr) + && write_f32_prefixed(&mut nvs, prefix, "brk_x", config.brake_exit) + && write_f32_prefixed(&mut nvs, prefix, "lat", config.lat_thr) + && write_f32_prefixed(&mut nvs, prefix, "lat_x", config.lat_exit) + && write_f32_prefixed(&mut nvs, prefix, "yaw", config.yaw_thr) + && write_f32_prefixed(&mut nvs, prefix, "spd", config.min_speed); + + if success { + info!( + "Saved {} profile to NVS: acc={:.2}, brake={:.2}, lat={:.2}, yaw={:.3}", + mode.name(), + config.acc_thr, + config.brake_thr, + config.lat_thr, + config.yaw_thr + ); + } else { + warn!("Failed to save {} profile to NVS", mode.name()); + } + + success +} + +// === Helper functions === + +fn read_f32_prefixed(nvs: &EspNvs, prefix: &str, key: &str) -> Option { + let mut buf = [0u8; 4]; + let full_key = format!("{}_{}", prefix, key); + match nvs.get_raw(&full_key, &mut buf) { + Ok(Some(_)) => Some(f32::from_le_bytes(buf)), + _ => None, + } +} + +fn write_f32_prefixed(nvs: &mut EspNvs, prefix: &str, key: &str, val: f32) -> bool { + let full_key = format!("{}_{}", prefix, key); + nvs.set_raw(&full_key, &val.to_le_bytes()).is_ok() +} diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 1e94344..cfab7d5 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -1,5 +1,5 @@ use std::sync::{ - atomic::{AtomicBool, AtomicU32, Ordering}, + atomic::{AtomicBool, AtomicU32, AtomicU8, Ordering}, Arc, Mutex, }; @@ -9,7 +9,7 @@ use esp_idf_svc::http::server::{Configuration, EspHttpServer}; use esp_idf_svc::io::Write; use log::info; -use crate::diagnostics::DiagnosticsState; +use crate::{diagnostics::DiagnosticsState, mode::ModeConfig, settings::DrivingMode}; /// Mode detection settings (matching mode.rs ModeConfig) #[derive(Clone, Copy)] @@ -24,6 +24,63 @@ pub struct ModeSettings { pub min_speed: f32, // Minimum speed for mode detection (m/s) } +/// Current driving mode (for API display) +#[derive(Clone, Copy, PartialEq)] +pub enum CurrentMode { + Track, + Canyon, + City, + Highway, + Custom, +} + +impl CurrentMode { + pub fn as_str(&self) -> &'static str { + match self { + CurrentMode::Track => "track", + CurrentMode::Canyon => "canyon", + CurrentMode::City => "city", + CurrentMode::Highway => "highway", + CurrentMode::Custom => "custom", + } + } + + pub fn from_str(s: &str) -> Option { + match s.to_lowercase().as_str() { + "track" => Some(CurrentMode::Track), + "canyon" => Some(CurrentMode::Canyon), + "city" => Some(CurrentMode::City), + "highway" => Some(CurrentMode::Highway), + "custom" => Some(CurrentMode::Custom), + _ => None, + } + } +} + +impl From for CurrentMode { + fn from(mode: DrivingMode) -> Self { + match mode { + DrivingMode::Track => CurrentMode::Track, + DrivingMode::Canyon => CurrentMode::Canyon, + DrivingMode::City => CurrentMode::City, + DrivingMode::Highway => CurrentMode::Highway, + DrivingMode::Custom => CurrentMode::Custom, + } + } +} + +impl From for DrivingMode { + fn from(mode: CurrentMode) -> Self { + match mode { + CurrentMode::Track => DrivingMode::Track, + CurrentMode::Canyon => DrivingMode::Canyon, + CurrentMode::City => DrivingMode::City, + CurrentMode::Highway => DrivingMode::Highway, + CurrentMode::Custom => DrivingMode::Custom, + } + } +} + impl Default for ModeSettings { fn default() -> Self { // Matches city preset - sensitive defaults for street driving @@ -40,6 +97,21 @@ impl Default for ModeSettings { } } +impl From for ModeSettings { + fn from(cfg: ModeConfig) -> Self { + Self { + acc_thr: cfg.acc_thr, + acc_exit: cfg.acc_exit, + brake_thr: cfg.brake_thr, + brake_exit: cfg.brake_exit, + lat_thr: cfg.lat_thr, + lat_exit: cfg.lat_exit, + yaw_thr: cfg.yaw_thr, + min_speed: cfg.min_speed, + } + } +} + /// Shared state for telemetry server pub struct TelemetryServerState { /// Latest telemetry packet (raw bytes) @@ -52,6 +124,10 @@ pub struct TelemetryServerState { mode_settings: Mutex, /// Settings changed flag settings_changed: AtomicBool, + /// Current driving mode (0=track, 1=canyon, 2=city, 3=highway, 4=custom) + current_mode: AtomicU8, + /// Mode change requested (new mode value, 255 = no change) + mode_change_requested: AtomicU8, /// Optional diagnostics state (shared with main loop) diagnostics_state: Option>, } @@ -64,6 +140,8 @@ impl TelemetryServerState { calibration_requested: AtomicBool::new(false), mode_settings: Mutex::new(ModeSettings::default()), settings_changed: AtomicBool::new(false), + current_mode: AtomicU8::new(2), // Default to City + mode_change_requested: AtomicU8::new(255), // No change diagnostics_state: None, } } @@ -76,10 +154,61 @@ impl TelemetryServerState { calibration_requested: AtomicBool::new(false), mode_settings: Mutex::new(ModeSettings::default()), settings_changed: AtomicBool::new(false), + current_mode: AtomicU8::new(2), // Default to City + mode_change_requested: AtomicU8::new(255), // No change diagnostics_state: Some(diagnostics), } } + /// Get current mode + pub fn get_current_mode(&self) -> CurrentMode { + match self.current_mode.load(Ordering::SeqCst) { + 0 => CurrentMode::Track, + 1 => CurrentMode::Canyon, + 2 => CurrentMode::City, + 3 => CurrentMode::Highway, + 4 => CurrentMode::Custom, + _ => CurrentMode::City, + } + } + + /// Set current mode (called when mode is loaded from NVS) + pub fn set_current_mode(&self, mode: CurrentMode) { + let val = match mode { + CurrentMode::Track => 0, + CurrentMode::Canyon => 1, + CurrentMode::City => 2, + CurrentMode::Highway => 3, + CurrentMode::Custom => 4, + }; + self.current_mode.store(val, Ordering::SeqCst); + } + + /// Request a mode change (called from API) + pub fn request_mode_change(&self, mode: CurrentMode) { + let val = match mode { + CurrentMode::Track => 0, + CurrentMode::Canyon => 1, + CurrentMode::City => 2, + CurrentMode::Highway => 3, + CurrentMode::Custom => 4, + }; + self.mode_change_requested.store(val, Ordering::SeqCst); + } + + /// Take mode change request (called from main loop) + pub fn take_mode_change(&self) -> Option { + let val = self.mode_change_requested.swap(255, Ordering::SeqCst); + match val { + 0 => Some(CurrentMode::Track), + 1 => Some(CurrentMode::Canyon), + 2 => Some(CurrentMode::City), + 3 => Some(CurrentMode::Highway), + 4 => Some(CurrentMode::Custom), + _ => None, + } + } + /// Get diagnostics state reference pub fn diagnostics(&self) -> Option<&Arc> { self.diagnostics_state.as_ref() @@ -267,7 +396,7 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0 .cfg-btn.cfg-save{background:linear-gradient(135deg,#1e3a5f,#1a2d4a);color:#60a5fa} -
    DIAG00:00
    --
    +
    Mode
    IDLE
    @@ -332,18 +461,25 @@ const $=id=>document.getElementById(id); const cv=$('gfc'),ctx=cv.getContext('2d'); const CX=70,CY=70,R=55,SCL=R/2; -// Preset definitions based on real-world G-forces (tested values): -// City: gentle-normal inputs (0.10-0.20g accel, 0.15-0.30g brake, 0.10-0.25g lateral) -// Highway: mostly cruising, higher speed threshold to filter parking maneuvers -// 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'}, +// Default presets (must match settings.rs DrivingMode::default_config()) +// Each mode has independent thresholds - calibrate via Autotune page +const DEFAULT_PRESETS={ +track:{acc:0.30,acc_exit:0.15,brake:0.50,brake_exit:0.25,lat:0.50,lat_exit:0.25,yaw:0.15,min_speed:3.0,desc:'Racing/track days'}, +canyon:{acc:0.20,acc_exit:0.10,brake:0.35,brake_exit:0.17,lat:0.30,lat_exit:0.15,yaw:0.10,min_speed:2.5,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'} +highway:{acc:0.08,acc_exit:0.04,brake:0.15,brake_exit:0.07,lat:0.10,lat_exit:0.05,yaw:0.04,min_speed:4.0,desc:'Highway cruising'} }; +// Get presets - prefer calibrated profiles from localStorage, fall back to defaults +function getPresets(){ +const calib=localStorage.getItem('bb_profiles'); +if(calib){ +try{return JSON.parse(calib).profiles}catch(e){} +} +return DEFAULT_PRESETS; +} +const PRESETS=getPresets(); + 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')} function resetGMax(){maxL=maxR=maxA=maxB=0;$('maxL').textContent=$('maxR').textContent=$('maxA').textContent=$('maxB').textContent='0.0'} @@ -501,6 +637,29 @@ $('s-yaw').value=p.yaw;$('v-yaw').textContent=p.yaw.toFixed(3); $('s-minspd').value=p.min_speed;$('v-minspd').textContent=p.min_speed.toFixed(1); } +// Get calibration data from localStorage +// Schema: { profiles: { track: {...}, city: {...}, ... }, lastMode: 'track', date: '...' } +function getCalibrationData(){ +try{ +const data=JSON.parse(localStorage.getItem('bb_profiles')||'null'); +return data; +}catch(e){return null} +} +function isCalibrated(){ +const data=getCalibrationData(); +return data&&data.profiles&&Object.keys(data.profiles).length>0; +} +// Get autotune profile for current preset (used by Custom mode) +function getAutotuneProfile(){ +const data=getCalibrationData(); +if(data&&data.profiles){ +// Return last calibrated mode's profile, or first available +const mode=data.lastMode||Object.keys(data.profiles)[0]; +return data.profiles[mode]||null; +} +return null; +} + // Select preset and apply function selectPreset(name){ currentPreset=name; @@ -512,12 +671,20 @@ b.classList.toggle('active',b.dataset.preset===name); const isCustom=name==='custom'; $('preset-summary').classList.toggle('hidden',isCustom); $('cfg-sliders').classList.toggle('show',isCustom); -// If not custom, apply preset and send to ESP32 +// If not custom, apply preset and send to ESP32 (includes mode switch) if(!isCustom&&PRESETS[name]){ const p=PRESETS[name]; updateSummary(p); applyPresetToSliders(p); -sendSettings(p); +sendSettings(p,name); +}else if(isCustom){ +// For custom, try to load from autotune profile +const at=getAutotuneProfile(); +if(at){ +const p={acc:at.acc,acc_exit:at.acc_exit,brake:at.brake,brake_exit:at.brake_exit,lat:at.lat,lat_exit:at.lat_exit,yaw:at.yaw,min_speed:at.min_speed}; +applyPresetToSliders(p); +sendSettings(p,'custom'); +} } } @@ -531,10 +698,13 @@ applyPresetToSliders(PRESETS.city); } } -// Send settings to ESP32 -async function sendSettings(p){ +// Send settings to ESP32 (includes mode change for correct NVS slot) +async function sendSettings(p,mode){ const brake=-Math.abs(p.brake),brake_exit=-Math.abs(p.brake_exit); try{ +// First switch to target mode if specified +if(mode){await fetch('/api/settings/set?mode='+mode);await new Promise(r=>setTimeout(r,150))} +// Then send settings const url='/api/settings/set?acc='+p.acc+'&acc_exit='+p.acc_exit+'&brake='+brake+'&brake_exit='+brake_exit+'&lat='+p.lat+'&lat_exit='+p.lat_exit+'&yaw='+p.yaw+'&min_speed='+p.min_speed; await fetch(url); }catch(e){console.log('Settings send failed:',e)} @@ -546,10 +716,10 @@ var acc=parseFloat($('s-acc').value),accexit=parseFloat($('s-accexit').value),br if(accexit>=acc){alert('Accel Exit must be < Accel Entry');return} if(brakeexit>=brake){alert('Brake Exit must be < Brake Entry');return} if(latexit>=lat){alert('Lateral Exit must be < Lateral Entry');return} -// Send +// Send (with current preset mode for NVS storage) const p={acc,acc_exit:accexit,brake,brake_exit:brakeexit,lat,lat_exit:latexit,yaw,min_speed:minspd}; try{ -await sendSettings(p); +await sendSettings(p,currentPreset); var btn=document.querySelector('.cfg-save'); btn.textContent='Applied';btn.style.background='#10b981'; setTimeout(()=>{btn.textContent='Apply';btn.style.background=''},1500); @@ -574,6 +744,21 @@ if(Math.abs(s.acc-p.acc)<0.01&&Math.abs(s.lat-p.lat)<0.01&&Math.abs(s.min_speed- matched=name;break; } } +// Update preset buttons to show calibration status +const calib=getCalibrationData(); +document.querySelectorAll('.preset-btn:not(.custom)').forEach(btn=>{ +const name=btn.dataset.preset; +if(calib&&calib.profiles){ +// Show calibrated indicator +btn.innerHTML=name.charAt(0).toUpperCase()+name.slice(1)+' '; +btn.title='Calibrated '+new Date(calib.date).toLocaleDateString(); +} +}); +// Update custom button +const customBtn=document.querySelector('.preset-btn.custom'); +if(customBtn){ +customBtn.textContent='Custom'; +} currentPreset=matched; // Update UI document.querySelectorAll('.preset-btn').forEach(b=>{ @@ -716,6 +901,707 @@ update(); "#; +/// Auto-tune page for calibration assistance +/// Uses EVENT DETECTION with full physics validation including: +/// - GPS heading vs gyro yaw rate +/// - Centripetal acceleration check (a_lat = v × ω) +/// - Impulse validation (ΔV = G × t) +/// - Event duration tracking +/// - Report export and localStorage save +const AUTOTUNE_HTML: &str = r#" + +Blackbox AutoTune + + + +
    + +
    +
    Select Mode to Calibrate
    +
    + + + + +
    +
    Daily street driving - calibrate during normal commute
    + +
    +
    📋 How It Works
    +
    +
      +
    1. Select the mode you want to calibrate above
    2. +
    3. Press Start Calibration below
    4. +
    5. Drive in that style for 10-20 minutes
    6. +
    7. The system detects your accel/brake/turn events
    8. +
    9. Press Apply to save thresholds for this mode
    10. +
    +
    +Tip: Calibrate each mode separately by driving in that style. City = commute, Track = aggressive, Highway = gentle cruising. +
    +
    +
    +
    + +
    +
    Event Detection0:00
    +
    +
    Ready
    +GPS: -- +
    +
    +
    0
    peak --
    Accels
    +
    0
    peak --
    Brakes
    +
    0
    peak --
    Turns
    +
    +
    +
    0.00
    Lon G
    +
    0.00
    Lat G
    +
    0.00
    v×ω G
    +
    0
    km/h
    +
    +
    +
    Drive to collect events
    +
    + + + +
    + + +
    + + +
    + +"#; + impl TelemetryServer { /// Create and start the telemetry server with HTTP polling /// @@ -730,8 +1616,9 @@ impl TelemetryServer { let server_config = Configuration { http_port: port, - max_uri_handlers: 9, /* Dashboard, telemetry, status, calibrate, settings GET, - * settings SET, diagnostics page, diagnostics API */ + max_uri_handlers: 10, /* Dashboard, autotune, telemetry, status, calibrate, + * settings GET, settings SET, diagnostics page, diagnostics + * API */ max_open_sockets: 8, // HTTP only - no long-lived WebSocket connections stack_size: 10240, ..Default::default() @@ -764,6 +1651,27 @@ impl TelemetryServer { }, )?; + // Serve the auto-tune calibration page + server.fn_handler( + "/autotune", + esp_idf_svc::http::Method::Get, + |req| -> Result<(), esp_idf_svc::io::EspIOError> { + let html_bytes = AUTOTUNE_HTML.as_bytes(); + let content_length = html_bytes.len().to_string(); + let mut response = req.into_response( + 200, + None, + &[ + ("Content-Type", "text/html; charset=utf-8"), + ("Content-Length", &content_length), + ("Connection", "close"), + ], + )?; + response.write_all(html_bytes)?; + Ok(()) + }, + )?; + // WebSocket removed - using HTTP polling for reliability and to eliminate // thread blocking @@ -847,9 +1755,10 @@ impl TelemetryServer { let state_get = state.clone(); server.fn_handler("/api/settings", esp_idf_svc::http::Method::Get, move |req| -> Result<(), esp_idf_svc::io::EspIOError> { let s = state_get.get_settings(); + let mode = state_get.get_current_mode(); let json = format!( - r#"{{"acc":{:.2},"acc_exit":{:.2},"brake":{:.2},"brake_exit":{:.2},"lat":{:.2},"lat_exit":{:.2},"yaw":{:.3},"min_speed":{:.1}}}"#, - s.acc_thr, s.acc_exit, s.brake_thr, s.brake_exit, s.lat_thr, s.lat_exit, s.yaw_thr, s.min_speed + r#"{{"mode":"{}","acc":{:.2},"acc_exit":{:.2},"brake":{:.2},"brake_exit":{:.2},"lat":{:.2},"lat_exit":{:.2},"yaw":{:.3},"min_speed":{:.1}}}"#, + mode.as_str(), s.acc_thr, s.acc_exit, s.brake_thr, s.brake_exit, s.lat_thr, s.lat_exit, s.yaw_thr, s.min_speed ); let content_length = json.len().to_string(); let mut response = req.into_response( @@ -871,36 +1780,54 @@ impl TelemetryServer { server.fn_handler("/api/settings/set", esp_idf_svc::http::Method::Get, move |req| -> Result<(), esp_idf_svc::io::EspIOError> { let uri = req.uri(); let mut settings = state_set.get_settings(); + let mut mode_change: Option = None; // Parse query parameters if let Some(query) = uri.split('?').nth(1) { for param in query.split('&') { let mut parts = param.split('='); if let (Some(key), Some(val)) = (parts.next(), parts.next()) { - if let Ok(v) = val.parse::() { - match key { - "acc" => settings.acc_thr = v, - "acc_exit" => settings.acc_exit = v, - "brake" => settings.brake_thr = v, - "brake_exit" => settings.brake_exit = v, - "lat" => settings.lat_thr = v, - "lat_exit" => settings.lat_exit = v, - "yaw" => settings.yaw_thr = v, - "min_speed" => settings.min_speed = v, - _ => {} + match key { + "mode" => { + if let Some(m) = CurrentMode::from_str(val) { + mode_change = Some(m); + } + } + _ => { + if let Ok(v) = val.parse::() { + match key { + "acc" => settings.acc_thr = v, + "acc_exit" => settings.acc_exit = v, + "brake" => settings.brake_thr = v, + "brake_exit" => settings.brake_exit = v, + "lat" => settings.lat_thr = v, + "lat_exit" => settings.lat_exit = v, + "yaw" => settings.yaw_thr = v, + "min_speed" => settings.min_speed = v, + _ => {} + } + } } } } } } - state_set.set_settings(settings); - info!("Settings updated: acc={:.2}, acc_exit={:.2}, brake={:.2}, brake_exit={:.2}, lat={:.2}, lat_exit={:.2}, yaw={:.3}, min_speed={:.1}", - settings.acc_thr, settings.acc_exit, settings.brake_thr, settings.brake_exit, settings.lat_thr, settings.lat_exit, settings.yaw_thr, settings.min_speed); + // Handle mode change (main loop will load settings for new mode from NVS) + if let Some(new_mode) = mode_change { + state_set.request_mode_change(new_mode); + info!("Mode change requested: {}", new_mode.as_str()); + } else { + // Regular settings update for current mode + state_set.set_settings(settings); + info!("Settings updated: acc={:.2}, brake={:.2}, lat={:.2}, yaw={:.3}", + settings.acc_thr, settings.brake_thr, settings.lat_thr, settings.yaw_thr); + } + let current_mode = state_set.get_current_mode(); let json = format!( - r#"{{"status":"ok","acc":{:.2},"acc_exit":{:.2},"brake":{:.2},"brake_exit":{:.2},"lat":{:.2},"lat_exit":{:.2},"yaw":{:.3},"min_speed":{:.1}}}"#, - settings.acc_thr, settings.acc_exit, settings.brake_thr, settings.brake_exit, settings.lat_thr, settings.lat_exit, settings.yaw_thr, settings.min_speed + r#"{{"status":"ok","mode":"{}","acc":{:.2},"acc_exit":{:.2},"brake":{:.2},"brake_exit":{:.2},"lat":{:.2},"lat_exit":{:.2},"yaw":{:.3},"min_speed":{:.1}}}"#, + current_mode.as_str(), settings.acc_thr, settings.acc_exit, settings.brake_thr, settings.brake_exit, settings.lat_thr, settings.lat_exit, settings.yaw_thr, settings.min_speed ); let content_length = json.len().to_string(); let mut response = req.into_response( @@ -1001,3 +1928,93 @@ impl TelemetryServer { self.state.clone() } } + +#[cfg(test)] +mod tests { + use super::*; + + /// Validates localStorage schema consistency between dashboard and autotune + /// pages. + /// + /// Both pages MUST use the same schema for bb_profiles: + /// ```json + /// { + /// "profiles": { "track": {...}, "city": {...}, ... }, + /// "lastMode": "track", + /// "date": "2025-01-11T..." + /// } + /// ``` + /// + /// This test catches the bug where autotune saved flat profiles but + /// dashboard expected a `.profiles` wrapper, breaking the main page. + #[test] + fn test_localstorage_schema_consistency() { + // Dashboard page must read from .profiles property + assert!( + DASHBOARD_HTML.contains(".profiles"), + "Dashboard must access .profiles property from localStorage" + ); + assert!( + DASHBOARD_HTML.contains("getCalibrationData()"), + "Dashboard must use getCalibrationData() helper" + ); + assert!( + DASHBOARD_HTML.contains("getAutotuneProfile()"), + "Dashboard must define getAutotuneProfile() function" + ); + + // Autotune page must save with .profiles wrapper + assert!( + AUTOTUNE_HTML.contains("stored.profiles[selectedMode]"), + "Autotune must save to stored.profiles[mode], not stored[mode]" + ); + assert!( + AUTOTUNE_HTML.contains("if(!stored.profiles)stored.profiles={}"), + "Autotune must initialize .profiles object if missing" + ); + + // Both pages must use the same localStorage key + let dashboard_key_count = DASHBOARD_HTML.matches("bb_profiles").count(); + let autotune_key_count = AUTOTUNE_HTML.matches("bb_profiles").count(); + assert!( + dashboard_key_count > 0, + "Dashboard must use 'bb_profiles' localStorage key" + ); + assert!( + autotune_key_count > 0, + "Autotune must use 'bb_profiles' localStorage key" + ); + + // Schema documentation must be present in both pages + assert!( + DASHBOARD_HTML.contains("Schema:"), + "Dashboard must document localStorage schema in comments" + ); + assert!( + AUTOTUNE_HTML.contains("Schema:"), + "Autotune must document localStorage schema in comments" + ); + } + + /// Validates that required JavaScript functions are defined in dashboard + #[test] + fn test_required_js_functions_defined() { + // Dashboard must define these functions + assert!( + DASHBOARD_HTML.contains("function getCalibrationData()"), + "Dashboard must define getCalibrationData()" + ); + assert!( + DASHBOARD_HTML.contains("function getAutotuneProfile()"), + "Dashboard must define getAutotuneProfile()" + ); + assert!( + DASHBOARD_HTML.contains("function isCalibrated()"), + "Dashboard must define isCalibrated()" + ); + assert!( + DASHBOARD_HTML.contains("function getPresets()"), + "Dashboard must define getPresets()" + ); + } +}