diff --git a/CLAUDE.md b/CLAUDE.md index 2631638..d3a8c4e 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -346,7 +346,14 @@ RX: GPIO4 ← GPS TX **Why UART0 for GPS?** Console output is disabled in `sdkconfig.defaults` to free UART0. All logging goes through USB-JTAG instead. -### IMU Calibration (main.rs:387-420) +### IMU Calibration (system.rs:66-128) + +**Startup Procedure:** +1. Affix device securely to vehicle +2. Park on a flat, level surface with motor OFF (ICE) or stationary (EV) +3. Power on the device +4. Wait for yellow LED blinks to complete (~10 seconds) +5. Then drive off **Process:** 1. Device must be completely stationary on level surface @@ -360,6 +367,28 @@ RX: GPIO4 ← GPS TX **Critical:** Any movement during calibration corrupts biases → poor EKF performance. +### Mount Calibration (calibration.rs) + +**Purpose:** Determines the yaw offset between the device's forward axis and the vehicle's forward axis. The device can be mounted in any horizontal orientation. + +**How it works:** When braking hard in a straight line, the acceleration vector points directly backward relative to the vehicle. The angle of this vector in the device's body frame *is* the mounting offset. + +**Procedure:** +1. Complete IMU calibration (device startup) +2. Trigger mount calibration via web UI (`/api/calibrate` endpoint) +3. Drive forward above 3 m/s (~11 km/h) +4. Brake hard in a straight line (>0.25g deceleration) +5. Hold brake for ~0.5 seconds while going straight + +**Calibration States:** +- `WaitingForMotion`: Drive forward to start +- `WaitingForBrake`: Now brake hard in a straight line +- `Collecting`: Gathering brake samples +- `Complete`: Offset computed and saved to NVS +- `Failed`: Timeout or inconsistent samples (retry) + +**Persistence:** Mount calibration is stored in NVS flash and survives reboots. Only needs to be done once per mounting position. + ### Telemetry Rate (main.rs:35) ```rust diff --git a/sensors/blackbox/src/calibration.rs b/sensors/blackbox/src/calibration.rs new file mode 100644 index 0000000..d664825 --- /dev/null +++ b/sensors/blackbox/src/calibration.rs @@ -0,0 +1,446 @@ +//! Mount calibration for determining device orientation relative to vehicle +//! +//! The device can be mounted in any horizontal orientation. This module +//! determines the yaw offset between the device's forward axis and the +//! vehicle's forward axis using a simple brake calibration maneuver. +//! +//! The insight: when braking hard in a straight line, the acceleration +//! vector points directly backward relative to the vehicle. The angle +//! of this vector in the device's body frame *is* the mounting offset. +//! +//! No GPS required. No magnetometer. No EKF. Just physics. + +#![allow(dead_code)] // Some methods reserved for future use + +use core::f32::consts::PI; + +/// Calibration configuration thresholds +#[derive(Debug, Clone, Copy)] +pub struct CalibrationConfig { + /// Minimum deceleration to detect braking (m/s²) + pub min_decel: f32, + /// Maximum yaw rate during braking to ensure straight line (rad/s) + pub max_yaw_rate: f32, + /// Minimum speed before we start looking for braking (m/s) + pub min_speed: f32, + /// Duration to collect samples during braking (seconds) + pub sample_duration: f32, + /// Maximum angle variance in samples (radians) + pub max_angle_variance: f32, + /// Timeout waiting for motion (seconds) + pub motion_timeout: f32, + /// Timeout waiting for brake (seconds) + pub brake_timeout: f32, +} + +impl Default for CalibrationConfig { + fn default() -> Self { + Self { + min_decel: 2.5, // ~0.25g - moderate braking + max_yaw_rate: 0.15, // ~8.6°/s - reasonably straight + min_speed: 3.0, // ~11 km/h - clearly moving forward + sample_duration: 0.5, // Half second of samples + max_angle_variance: 0.175, // ~10° - samples should agree + motion_timeout: 30.0, // 30 seconds to start moving + brake_timeout: 30.0, // 30 seconds to brake after moving + } + } +} + +/// Persistent mount calibration data +#[derive(Debug, Clone, Copy)] +pub struct MountCalibration { + /// Yaw offset from device frame to vehicle frame (radians) + /// To convert device heading to vehicle heading: vehicle_yaw = device_yaw - offset + pub yaw_offset: f32, + /// Whether calibration has been performed + pub is_calibrated: bool, +} + +impl Default for MountCalibration { + fn default() -> Self { + Self { + yaw_offset: 0.0, + is_calibrated: false, + } + } +} + +impl MountCalibration { + /// Create a new calibration with a known offset + pub fn new(yaw_offset: f32) -> Self { + Self { + yaw_offset: normalize_angle(yaw_offset), + is_calibrated: true, + } + } + + /// Apply the calibration offset to a device-frame yaw to get vehicle-frame yaw + pub fn device_to_vehicle_yaw(&self, device_yaw: f32) -> f32 { + if self.is_calibrated { + normalize_angle(device_yaw - self.yaw_offset) + } else { + device_yaw + } + } + + /// Apply the calibration to rotate a 2D vector from device frame to vehicle frame + pub fn device_to_vehicle_accel(&self, ax_device: f32, ay_device: f32) -> (f32, f32) { + if !self.is_calibrated { + return (ax_device, ay_device); + } + + let cos_off = self.yaw_offset.cos(); + let sin_off = self.yaw_offset.sin(); + + // Rotate by negative offset (device → vehicle) + let ax_vehicle = ax_device * cos_off + ay_device * sin_off; + let ay_vehicle = -ax_device * sin_off + ay_device * cos_off; + + (ax_vehicle, ay_vehicle) + } +} + +/// Calibration state machine states +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum CalibrationState { + /// Not calibrating, waiting for trigger + Idle, + /// Waiting for vehicle to start moving + WaitingForMotion, + /// Vehicle moving, waiting for hard brake + WaitingForBrake, + /// Collecting acceleration samples during braking + Collecting, + /// Calibration completed successfully + Complete, + /// Calibration failed + Failed, +} + +/// Result of calibration attempt +#[derive(Debug, Clone)] +pub enum CalibrationResult { + /// Still in progress + InProgress(CalibrationState), + /// Completed successfully with computed offset + Success(MountCalibration), + /// Failed with reason + Failure(CalibrationError), +} + +/// Calibration failure reasons +#[derive(Debug, Clone, Copy)] +pub enum CalibrationError { + /// Timed out waiting for vehicle motion + MotionTimeout, + /// Timed out waiting for braking + BrakeTimeout, + /// Samples were too inconsistent (user turned while braking?) + InconsistentSamples, + /// Not enough valid samples collected + InsufficientSamples, + /// Calibration was cancelled + Cancelled, +} + +/// Collected sample during braking +#[derive(Debug, Clone, Copy)] +struct BrakeSample { + /// Body-frame X acceleration (gravity removed) + ax: f32, + /// Body-frame Y acceleration (gravity removed) + ay: f32, + /// Timestamp in milliseconds + timestamp_ms: u32, +} + +/// The calibration engine +pub struct MountCalibrator { + config: CalibrationConfig, + state: CalibrationState, + state_start_ms: u32, + samples: [BrakeSample; 64], + sample_count: usize, + computed_offset: Option, +} + +impl MountCalibrator { + pub fn new(config: CalibrationConfig) -> Self { + Self { + config, + state: CalibrationState::Idle, + state_start_ms: 0, + samples: [BrakeSample { + ax: 0.0, + ay: 0.0, + timestamp_ms: 0, + }; 64], + sample_count: 0, + computed_offset: None, + } + } + + /// Get current calibration state + pub fn state(&self) -> CalibrationState { + self.state + } + + /// Check if calibration is in progress + pub fn is_active(&self) -> bool { + matches!( + self.state, + CalibrationState::WaitingForMotion + | CalibrationState::WaitingForBrake + | CalibrationState::Collecting + ) + } + + /// Trigger calibration to begin + pub fn trigger(&mut self, now_ms: u32) { + self.state = CalibrationState::WaitingForMotion; + self.state_start_ms = now_ms; + self.sample_count = 0; + self.computed_offset = None; + } + + /// Cancel calibration in progress + pub fn cancel(&mut self) { + self.state = CalibrationState::Failed; + self.sample_count = 0; + } + + /// Reset to idle state + pub fn reset(&mut self) { + self.state = CalibrationState::Idle; + self.sample_count = 0; + self.computed_offset = None; + } + + /// Update calibration state machine + /// + /// # Arguments + /// * `ax_body` - X acceleration in body frame, gravity removed (m/s²) + /// * `ay_body` - Y acceleration in body frame, gravity removed (m/s²) + /// * `wz` - Yaw rate (rad/s) + /// * `speed` - Vehicle speed from GPS or EKF (m/s) + /// * `now_ms` - Current timestamp in milliseconds + /// + /// # Returns + /// Current calibration result + pub fn update( + &mut self, + ax_body: f32, + ay_body: f32, + wz: f32, + speed: f32, + now_ms: u32, + ) -> CalibrationResult { + let elapsed_ms = now_ms.wrapping_sub(self.state_start_ms); + let elapsed_s = elapsed_ms as f32 / 1000.0; + + match self.state { + CalibrationState::Idle => CalibrationResult::InProgress(self.state), + + CalibrationState::WaitingForMotion => { + // Check timeout + if elapsed_s > self.config.motion_timeout { + self.state = CalibrationState::Failed; + return CalibrationResult::Failure(CalibrationError::MotionTimeout); + } + + // Check if we have sufficient forward motion + if speed > self.config.min_speed { + self.state = CalibrationState::WaitingForBrake; + self.state_start_ms = now_ms; + } + + CalibrationResult::InProgress(self.state) + } + + CalibrationState::WaitingForBrake => { + // Check timeout + if elapsed_s > self.config.brake_timeout { + self.state = CalibrationState::Failed; + return CalibrationResult::Failure(CalibrationError::BrakeTimeout); + } + + // Compute total horizontal acceleration magnitude + let accel_mag = (ax_body * ax_body + ay_body * ay_body).sqrt(); + + // Check for braking: high decel, low yaw rate, still moving + let is_decelerating = accel_mag > self.config.min_decel; + let is_straight = wz.abs() < self.config.max_yaw_rate; + let still_moving = speed > self.config.min_speed * 0.5; + + if is_decelerating && is_straight && still_moving { + self.state = CalibrationState::Collecting; + self.state_start_ms = now_ms; + self.sample_count = 0; + } + + CalibrationResult::InProgress(self.state) + } + + CalibrationState::Collecting => { + let accel_mag = (ax_body * ax_body + ay_body * ay_body).sqrt(); + let is_still_braking = accel_mag > self.config.min_decel * 0.7; // Slight hysteresis + let is_straight = wz.abs() < self.config.max_yaw_rate; + + // If braking stopped or turned, check if we have enough samples + if !is_still_braking || !is_straight { + return self.finalize_calibration(); + } + + // Collect sample + if self.sample_count < self.samples.len() { + self.samples[self.sample_count] = BrakeSample { + ax: ax_body, + ay: ay_body, + timestamp_ms: now_ms, + }; + self.sample_count += 1; + } + + // Check if we have enough duration + if elapsed_s >= self.config.sample_duration { + return self.finalize_calibration(); + } + + CalibrationResult::InProgress(self.state) + } + + CalibrationState::Complete => { + if let Some(offset) = self.computed_offset { + CalibrationResult::Success(MountCalibration::new(offset)) + } else { + CalibrationResult::InProgress(self.state) + } + } + + CalibrationState::Failed => CalibrationResult::Failure(CalibrationError::Cancelled), + } + } + + /// Compute the calibration from collected samples + fn finalize_calibration(&mut self) -> CalibrationResult { + if self.sample_count < 5 { + self.state = CalibrationState::Failed; + return CalibrationResult::Failure(CalibrationError::InsufficientSamples); + } + + // Compute mean acceleration vector + let mut sum_ax = 0.0; + let mut sum_ay = 0.0; + + for i in 0..self.sample_count { + sum_ax += self.samples[i].ax; + sum_ay += self.samples[i].ay; + } + + let mean_ax = sum_ax / self.sample_count as f32; + let mean_ay = sum_ay / self.sample_count as f32; + + // Compute angle of mean vector + let mean_angle = mean_ay.atan2(mean_ax); + + // Check consistency: all samples should point roughly the same direction + let mut max_deviation = 0.0f32; + for i in 0..self.sample_count { + let sample_angle = self.samples[i].ay.atan2(self.samples[i].ax); + let deviation = angle_difference(sample_angle, mean_angle).abs(); + max_deviation = max_deviation.max(deviation); + } + + if max_deviation > self.config.max_angle_variance { + self.state = CalibrationState::Failed; + return CalibrationResult::Failure(CalibrationError::InconsistentSamples); + } + + // The acceleration during braking points BACKWARD relative to vehicle forward + // So the mounting offset is the brake angle plus 180 degrees + let yaw_offset = normalize_angle(mean_angle + PI); + + self.computed_offset = Some(yaw_offset); + self.state = CalibrationState::Complete; + + CalibrationResult::Success(MountCalibration::new(yaw_offset)) + } + + /// Get computed offset if calibration is complete + pub fn get_offset(&self) -> Option { + self.computed_offset + } +} + +/// Normalize angle to [-π, π] +fn normalize_angle(mut angle: f32) -> f32 { + while angle > PI { + angle -= 2.0 * PI; + } + while angle < -PI { + angle += 2.0 * PI; + } + angle +} + +/// Compute shortest angular difference between two angles +fn angle_difference(a: f32, b: f32) -> f32 { + normalize_angle(a - b) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_normalize_angle() { + assert!((normalize_angle(0.0) - 0.0).abs() < 1e-6); + assert!((normalize_angle(PI) - PI).abs() < 1e-6); + assert!((normalize_angle(-PI) - (-PI)).abs() < 1e-6); + assert!((normalize_angle(2.0 * PI) - 0.0).abs() < 1e-6); + assert!((normalize_angle(3.0 * PI) - PI).abs() < 1e-6); + } + + #[test] + fn test_calibration_no_offset() { + // Device mounted aligned with vehicle + // Braking produces acceleration in -X direction (backward) + let cal = MountCalibration::new(0.0); + let (vx, vy) = cal.device_to_vehicle_accel(-3.0, 0.0); + assert!((vx - (-3.0)).abs() < 1e-6); + assert!(vy.abs() < 1e-6); + } + + #[test] + fn test_calibration_90_degree_offset() { + // Device mounted rotated 90 degrees clockwise (offset = -π/2) + // Device X points to vehicle right, Device Y points to vehicle backward + // In device frame, braking (vehicle -X) appears as +Y (device Y = -vehicle X) + // Should transform to -X in vehicle frame + let cal = MountCalibration::new(-PI / 2.0); + let (vx, vy) = cal.device_to_vehicle_accel(0.0, 3.0); + assert!((vx - (-3.0)).abs() < 0.01); + assert!(vy.abs() < 0.01); + } + + #[test] + fn test_offset_from_braking_aligned() { + // Device aligned: braking shows as -X in device frame + let brake_ax = -3.0; + let brake_ay = 0.0; + let brake_angle = brake_ay.atan2(brake_ax); // Should be PI (pointing backward) + let offset = normalize_angle(brake_angle + PI); // Should be ~0 + assert!(offset.abs() < 0.01); + } + + #[test] + fn test_offset_from_braking_rotated() { + // Device rotated 45 degrees: braking shows as (-2.12, -2.12) in device frame + let brake_ax = -2.12; + let brake_ay = -2.12; + let brake_angle = brake_ay.atan2(brake_ax); // Should be -3π/4 + let offset = normalize_angle(brake_angle + PI); // Should be π/4 (45 degrees) + assert!((offset - PI / 4.0).abs() < 0.1); + } +} diff --git a/sensors/blackbox/src/main.rs b/sensors/blackbox/src/main.rs index 63f1803..8ec6955 100644 --- a/sensors/blackbox/src/main.rs +++ b/sensors/blackbox/src/main.rs @@ -1,9 +1,11 @@ mod binary_telemetry; +mod calibration; mod config; mod gps; mod imu; mod mode; mod mqtt; +mod nvs_storage; mod rgb_led; mod system; mod udp_stream; @@ -12,6 +14,7 @@ mod wifi; use std::sync::Arc; +use calibration::{CalibrationConfig, CalibrationResult, MountCalibrator}; use config::{SystemConfig, WifiModeConfig}; use esp_idf_hal::{ delay::FreeRtos, @@ -53,6 +56,11 @@ fn main() { let sysloop = EspSystemEventLoop::take().unwrap(); let nvs = EspDefaultNvsPartition::take().ok(); + // Create NVS storage for persistent settings (uses NVS partition) + let mut nvs_store = nvs + .as_ref() + .and_then(|p| nvs_storage::NvsStorage::new(p.clone()).ok()); + // Create LED for status indication let led = RgbLed::new(peripherals.rmt.channel0, peripherals.pins.gpio8) .expect("Failed to initialize RGB LED"); @@ -258,6 +266,26 @@ fn main() { let mut estimator = StateEstimator::new(); let mut publisher = TelemetryPublisher::new(udp_stream, mqtt_opt, telemetry_state.clone()); + // Create mount calibrator for determining device orientation relative to vehicle + let mut mount_calibrator = MountCalibrator::new(CalibrationConfig::default()); + + // Load mount calibration from NVS if available + let mut mount_calibration = nvs_store + .as_ref() + .and_then(|s| s.load_mount_calibration()) + .unwrap_or_default(); + + // Update server state with loaded calibration + if mount_calibration.is_calibrated { + if let Some(ref state) = telemetry_state { + state.set_mount_calibration(mount_calibration); + } + info!( + "Loaded mount calibration from NVS: {:.1}°", + mount_calibration.yaw_offset.to_degrees() + ); + } + // Main loop timing let mut last_telemetry_ms = 0u32; let mut last_heap_check_ms = 0u32; @@ -321,6 +349,27 @@ fn main() { } status_mgr.led_mut().set_low().unwrap(); } + + // Check for mount calibration request from web UI + if state.take_mount_calibration_request() && !mount_calibrator.is_active() { + info!(">>> Mount calibration requested - drive forward then brake hard!"); + mount_calibrator.trigger(now_ms); + } + + // Update mount calibration state for web UI + state.update_mount_calibration_state(mount_calibrator.state()); + + // Debug: log calibration state periodically when active + static mut LAST_CAL_LOG_MS: u32 = 0; + if mount_calibrator.is_active() && now_ms.wrapping_sub(unsafe { LAST_CAL_LOG_MS }) > 1000 + { + unsafe { LAST_CAL_LOG_MS = now_ms }; + info!( + ">>> Mount cal state: {:?}, EKF_RESET_DONE={}", + mount_calibrator.state(), + unsafe { EKF_RESET_DONE } + ); + } } // Periodic diagnostics (serial log every 5s) @@ -427,16 +476,80 @@ fn main() { // Poll IMU and update EKF prediction (only after GPS warmup AND reset) if let Some((dt, _)) = sensors.poll_imu() { + let (ax_corr, ay_corr, az_corr) = sensors.imu_parser.get_accel_corrected(); + let (ax_b, ay_b, az_b) = remove_gravity( + ax_corr, + ay_corr, + az_corr, + sensors.imu_parser.data().roll, + sensors.imu_parser.data().pitch, + ); + + // Update mount calibrator with body-frame acceleration (gravity removed) + // Use EKF speed when available (updates at IMU rate), fall back to GPS speed + if mount_calibrator.is_active() { + let speed = if unsafe { EKF_RESET_DONE } { + // EKF is running - use its velocity estimate (fresh, 200Hz) + let (vx, vy) = estimator.ekf.velocity(); + (vx * vx + vy * vy).sqrt() + } else { + // Before EKF init - use raw GPS speed directly + sensors.gps_parser.last_fix().speed / 3.6 // Convert km/h to m/s + }; + + // Debug: log speed and accel periodically + static mut LAST_SPEED_LOG_MS: u32 = 0; + if now_ms.wrapping_sub(unsafe { LAST_SPEED_LOG_MS }) > 500 { + unsafe { LAST_SPEED_LOG_MS = now_ms }; + let accel_mag = (ax_b * ax_b + ay_b * ay_b).sqrt(); + info!( + ">>> Cal: spd={:.1}m/s accel={:.2}m/s² wz={:.2}rad/s state={:?}", + speed, + accel_mag, + sensors.imu_parser.data().wz, + mount_calibrator.state() + ); + } + + match mount_calibrator.update( + ax_b, + ay_b, + sensors.imu_parser.data().wz, + speed, + now_ms, + ) { + CalibrationResult::Success(cal) => { + info!( + ">>> Mount calibration SUCCESS! Offset: {:.1}°", + cal.yaw_offset.to_degrees() + ); + mount_calibration = cal; + + // Store in server state for UI display + if let Some(ref state) = telemetry_state { + state.set_mount_calibration(cal); + } + + // Persist to NVS so it survives reboot + if let Some(ref mut storage) = nvs_store { + if let Err(e) = storage.save_mount_calibration(&cal) { + info!(">>> Failed to save calibration to NVS: {:?}", e); + } + } + + // Reset calibrator to idle so user can recalibrate if needed + mount_calibrator.reset(); + } + CalibrationResult::Failure(e) => { + info!(">>> Mount calibration FAILED: {:?}", e); + mount_calibrator.reset(); + } + CalibrationResult::InProgress(_) => {} // Keep going + } + } + // Only run EKF prediction after reset is done if unsafe { EKF_RESET_DONE } { - let (ax_corr, ay_corr, az_corr) = sensors.imu_parser.get_accel_corrected(); - let (ax_b, ay_b, az_b) = remove_gravity( - ax_corr, - ay_corr, - az_corr, - sensors.imu_parser.data().roll, - sensors.imu_parser.data().pitch, - ); let (ax_e, ay_e) = body_to_earth( ax_b, ay_b, @@ -447,9 +560,10 @@ fn main() { ); estimator.predict(ax_e, ay_e, sensors.imu_parser.data().wz, dt); - // Update yaw from magnetometer + // Update yaw from magnetometer, applying mount calibration offset let yaw_mag = sensors.imu_parser.data().yaw.to_radians(); - estimator.update_yaw(yaw_mag); + let yaw_vehicle = mount_calibration.device_to_vehicle_yaw(yaw_mag); + estimator.update_yaw(yaw_vehicle); } } diff --git a/sensors/blackbox/src/nvs_storage.rs b/sensors/blackbox/src/nvs_storage.rs new file mode 100644 index 0000000..cf40d1a --- /dev/null +++ b/sensors/blackbox/src/nvs_storage.rs @@ -0,0 +1,86 @@ +//! Non-volatile storage for mount calibration. + +use esp_idf_svc::nvs::{EspNvs, EspNvsPartition, NvsDefault}; +use log::info; + +const NAMESPACE: &str = "blackbox"; +const KEY_MOUNT_OFFSET: &str = "mount_off"; +const KEY_MOUNT_VALID: &str = "mount_ok"; + +/// NVS-backed persistent storage for calibration data +pub struct NvsStorage { + nvs: EspNvs, +} + +impl NvsStorage { + pub fn new(partition: EspNvsPartition) -> Result { + let nvs = EspNvs::new(partition, NAMESPACE, true)?; + info!("NVS storage initialized"); + Ok(Self { nvs }) + } + + fn get_u8(&self, key: &str) -> Option { + self.nvs.get_u8(key).ok().flatten() + } + + fn set_u8(&mut self, key: &str, val: u8) -> Result<(), esp_idf_svc::sys::EspError> { + self.nvs.set_u8(key, val) + } + + fn get_f32(&self, key: &str) -> Option { + let mut buf = [0u8; 4]; + let bytes = self.nvs.get_raw(key, &mut buf).ok().flatten()?; + if bytes.len() != 4 { + return None; + } + let val = f32::from_le_bytes(buf); + if val.is_nan() { + return None; + } + Some(val) + } + + fn set_f32(&mut self, key: &str, val: f32) -> Result<(), esp_idf_svc::sys::EspError> { + self.nvs.set_raw(key, &val.to_le_bytes())?; + Ok(()) + } + + /// Load mount calibration from NVS. Returns None if not calibrated or invalid. + pub fn load_mount_calibration(&self) -> Option { + if self.get_u8(KEY_MOUNT_VALID) != Some(1) { + return None; + } + + let offset = self.get_f32(KEY_MOUNT_OFFSET)?; + + // Sanity check: offset should be in [-π, π] + if offset.abs() > core::f32::consts::PI + 0.1 { + info!("NVS: Invalid mount offset {:.2}, ignoring", offset); + return None; + } + + info!("NVS: Loaded mount offset: {:.1}°", offset.to_degrees()); + Some(crate::calibration::MountCalibration::new(offset)) + } + + /// Save mount calibration to NVS. + pub fn save_mount_calibration( + &mut self, + cal: &crate::calibration::MountCalibration, + ) -> Result<(), esp_idf_svc::sys::EspError> { + if !cal.is_calibrated { + self.set_u8(KEY_MOUNT_VALID, 0)?; + info!("NVS: Cleared mount calibration"); + return Ok(()); + } + + self.set_f32(KEY_MOUNT_OFFSET, cal.yaw_offset)?; + self.set_u8(KEY_MOUNT_VALID, 1)?; + + info!( + "NVS: Saved mount offset: {:.1}°", + cal.yaw_offset.to_degrees() + ); + Ok(()) + } +} diff --git a/sensors/blackbox/src/system.rs b/sensors/blackbox/src/system.rs index afd533c..9bb9a3e 100644 --- a/sensors/blackbox/src/system.rs +++ b/sensors/blackbox/src/system.rs @@ -20,7 +20,7 @@ use crate::{ websocket_server::TelemetryServerState, }; -const CALIB_SAMPLES: usize = 150; +const CALIB_SAMPLES: usize = 500; /// System-level errors #[derive(Debug)] diff --git a/sensors/blackbox/src/websocket_server.rs b/sensors/blackbox/src/websocket_server.rs index 9370634..1281d17 100644 --- a/sensors/blackbox/src/websocket_server.rs +++ b/sensors/blackbox/src/websocket_server.rs @@ -1,8 +1,10 @@ use std::sync::{ - atomic::{AtomicBool, AtomicU32, Ordering}, + atomic::{AtomicBool, AtomicU32, AtomicU8, Ordering}, Arc, Mutex, }; +use crate::calibration::{CalibrationState, MountCalibration}; + /// HTTP telemetry server for dashboard and settings /// Uses HTTP polling for reliable updates without thread blocking use esp_idf_svc::http::server::{Configuration, EspHttpServer}; @@ -44,12 +46,18 @@ pub struct TelemetryServerState { telemetry_data: Mutex>, /// Packet counter for clients to detect updates packet_counter: AtomicU32, - /// Calibration requested flag + /// IMU bias calibration requested flag calibration_requested: AtomicBool, /// Mode detection settings mode_settings: Mutex, /// Settings changed flag settings_changed: AtomicBool, + /// Mount calibration requested flag + mount_calibration_requested: AtomicBool, + /// Mount calibration state (0=Idle, 1=WaitingForMotion, 2=WaitingForBrake, 3=Collecting, 4=Complete, 5=Failed) + mount_calibration_state: AtomicU8, + /// Current mount calibration + mount_calibration: Mutex, } impl TelemetryServerState { @@ -60,6 +68,9 @@ impl TelemetryServerState { calibration_requested: AtomicBool::new(false), mode_settings: Mutex::new(ModeSettings::default()), settings_changed: AtomicBool::new(false), + mount_calibration_requested: AtomicBool::new(false), + mount_calibration_state: AtomicU8::new(0), + mount_calibration: Mutex::new(MountCalibration::default()), } } @@ -122,6 +133,52 @@ impl TelemetryServerState { pub fn packet_count(&self) -> u32 { self.packet_counter.load(Ordering::SeqCst) } + + /// Request mount calibration from the UI + pub fn request_mount_calibration(&self) { + self.mount_calibration_requested + .store(true, Ordering::SeqCst); + } + + /// Check if mount calibration was requested and clear the flag + pub fn take_mount_calibration_request(&self) -> bool { + self.mount_calibration_requested + .swap(false, Ordering::SeqCst) + } + + /// Update mount calibration state (called from main loop) + pub fn update_mount_calibration_state(&self, state: CalibrationState) { + let state_u8 = match state { + CalibrationState::Idle => 0, + CalibrationState::WaitingForMotion => 1, + CalibrationState::WaitingForBrake => 2, + CalibrationState::Collecting => 3, + CalibrationState::Complete => 4, + CalibrationState::Failed => 5, + }; + self.mount_calibration_state + .store(state_u8, Ordering::SeqCst); + } + + /// Get mount calibration state + pub fn get_mount_calibration_state(&self) -> u8 { + self.mount_calibration_state.load(Ordering::SeqCst) + } + + /// Store completed mount calibration + pub fn set_mount_calibration(&self, cal: MountCalibration) { + if let Ok(mut guard) = self.mount_calibration.lock() { + *guard = cal; + } + } + + /// Get current mount calibration + pub fn get_mount_calibration(&self) -> MountCalibration { + self.mount_calibration + .lock() + .map(|g| *g) + .unwrap_or_default() + } } impl Default for TelemetryServerState { @@ -243,6 +300,14 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0 .cfg-btn{flex:1;padding:10px;border:none;border-radius:6px;font-size:11px;font-weight:600;background:#1a1a24;color:#666;cursor:pointer;transition:all .2s} .cfg-btn:active{transform:scale(0.97)} .cfg-btn.cfg-save{background:linear-gradient(135deg,#1e3a5f,#1a2d4a);color:#60a5fa} +.mount-section{background:#111;border-radius:10px;padding:12px;margin-top:10px;border:1px solid #1a1a24} +.mount-row{display:flex;align-items:center;justify-content:space-between;gap:10px} +.mount-status{font-size:12px;color:#666} +.mount-status .state{color:#60a5fa;font-weight:600} +.mount-status .offset{color:#22c55e;font-weight:600} +.mount-btn{padding:10px 16px;border:none;border-radius:6px;font-size:11px;background:#1e3a5f;color:#60a5fa;cursor:pointer;text-transform:uppercase;letter-spacing:1px} +.mount-btn:disabled{background:#1a1a24;color:#444;cursor:not-allowed} +.mount-btn.active{background:#5f1e1e;color:#f87171;animation:pulse 1s infinite}
00:00
--
@@ -294,6 +359,13 @@ body{font-family:-apple-system,system-ui,sans-serif;background:#0a0a0f;color:#f0
+
+
Mount Calibration
+
+
Not Calibrated
Offset: 0.0°
+ +
+
@@ -571,8 +643,35 @@ selectPreset('city'); } } +// Mount calibration functions +const MOUNT_STATES={idle:'Ready',waiting_motion:'Drive Forward...',waiting_brake:'Brake Hard!',collecting:'Collecting...',complete:'Calibrated',failed:'Failed'}; +async function startMountCal(){ +try{ +await fetch('/api/mount_calibrate?action=start'); +$('mount-btn').className='mount-btn active';$('mount-btn').disabled=true; +}catch(e){alert('Failed: '+e.message)} +} +async function pollMount(){ +try{ +const r=await fetch('/api/mount_calibrate'); +const m=await r.json(); +const active=m.state!=='idle'&&m.state!=='complete'&&m.state!=='failed'; +// Show calibration status: "Calibrated" if done, otherwise show current state +if(m.calibrated&&!active){ +$('mount-state').textContent='Calibrated'; +$('mount-state').style.color='#22c55e'; +}else{ +$('mount-state').textContent=MOUNT_STATES[m.state]||m.state; +$('mount-state').style.color=m.state==='failed'?'#ef4444':'#60a5fa'; +} +$('mount-offset').textContent=m.offset_deg.toFixed(1); +$('mount-btn').className=active?'mount-btn active':'mount-btn'; +$('mount-btn').disabled=active; +}catch(e){} +} + // Load settings, then start polling (self-scheduling for max throughput) -loadCfg().then(()=>poll()); +loadCfg().then(()=>{poll();setInterval(pollMount,500)}) "#; impl TelemetryServer { @@ -582,8 +681,8 @@ impl TelemetryServer { let server_config = Configuration { http_port: port, - max_uri_handlers: 7, /* Dashboard, telemetry, status, calibrate, settings GET, - * settings SET */ + max_uri_handlers: 8, /* Dashboard, telemetry, status, calibrate, mount_calibrate, + * settings GET, settings SET */ max_open_sockets: 8, // HTTP only - no long-lived WebSocket connections stack_size: 10240, ..Default::default() @@ -673,7 +772,7 @@ impl TelemetryServer { }, )?; - // Calibration trigger endpoint + // IMU bias calibration trigger endpoint let state_calib = state.clone(); server.fn_handler( "/api/calibrate", @@ -694,6 +793,61 @@ impl TelemetryServer { }, )?; + // Mount calibration endpoint - trigger and status + let state_mount = state.clone(); + server.fn_handler( + "/api/mount_calibrate", + esp_idf_svc::http::Method::Get, + move |req| -> Result<(), esp_idf_svc::io::EspIOError> { + let uri = req.uri(); + let action = uri.split('?').nth(1).and_then(|q| { + q.split('&').find_map(|p| { + let mut parts = p.split('='); + if parts.next() == Some("action") { + parts.next() + } else { + None + } + }) + }); + + if action == Some("start") { + info!(">>> Mount calibration requested via API"); + state_mount.request_mount_calibration(); + } + + // Return current state + let cal_state = state_mount.get_mount_calibration_state(); + let cal = state_mount.get_mount_calibration(); + let state_name = match cal_state { + 0 => "idle", + 1 => "waiting_motion", + 2 => "waiting_brake", + 3 => "collecting", + 4 => "complete", + 5 => "failed", + _ => "unknown", + }; + let json = format!( + r#"{{"state":"{}","calibrated":{},"offset_deg":{:.1}}}"#, + state_name, + cal.is_calibrated, + cal.yaw_offset.to_degrees() + ); + + let mut response = req.into_response( + 200, + None, + &[ + ("Content-Type", "application/json"), + ("Access-Control-Allow-Origin", "*"), + ], + )?; + response.write_all(json.as_bytes())?; + Ok(()) + }, + )?; + // Get current settings let state_get = state.clone(); server.fn_handler("/api/settings", esp_idf_svc::http::Method::Get, move |req| -> Result<(), esp_idf_svc::io::EspIOError> {