448 lines
15 KiB
Rust
448 lines
15 KiB
Rust
//! Forklift/AGV proximity detection — ADR-041 Category 5 Industrial module.
|
|
//!
|
|
//! Detects dangerous proximity between pedestrians and forklifts/AGVs using
|
|
//! CSI signal characteristics:
|
|
//!
|
|
//! - **Forklift signature**: high-amplitude, low-frequency (<0.3 Hz) phase
|
|
//! modulation combined with motor vibration harmonics. Large metal bodies
|
|
//! produce distinctive broadband amplitude increases.
|
|
//! - **Human signature**: moderate amplitude, higher-frequency (0.5-2 Hz)
|
|
//! phase modulation from gait.
|
|
//! - **Co-occurrence alert**: When both signatures are simultaneously present,
|
|
//! emit proximity warnings with distance category.
|
|
//!
|
|
//! Budget: S (<5 ms per frame). Event IDs 500-502.
|
|
|
|
#[cfg(not(feature = "std"))]
|
|
use libm::sqrtf;
|
|
#[cfg(feature = "std")]
|
|
fn sqrtf(x: f32) -> f32 { x.sqrt() }
|
|
|
|
/// Maximum subcarriers to process.
|
|
const MAX_SC: usize = 32;
|
|
|
|
/// Phase history depth for frequency analysis (1 second at 20 Hz).
|
|
const PHASE_HISTORY: usize = 20;
|
|
|
|
/// Amplitude threshold ratio for forklift (large metal body).
|
|
/// Forklift amplitude is typically 2-5x baseline.
|
|
const FORKLIFT_AMP_RATIO: f32 = 2.5;
|
|
|
|
/// Motion energy threshold for human presence near vehicle.
|
|
const HUMAN_MOTION_THRESH: f32 = 0.15;
|
|
|
|
/// Low-frequency dominance ratio: fraction of energy below 0.3 Hz.
|
|
/// Forklifts have >60% of energy in low frequencies.
|
|
const LOW_FREQ_RATIO_THRESH: f32 = 0.55;
|
|
|
|
/// Variance threshold for motor vibration harmonics.
|
|
const VIBRATION_VAR_THRESH: f32 = 0.08;
|
|
|
|
/// Debounce frames before emitting vehicle detection.
|
|
const VEHICLE_DEBOUNCE: u8 = 4;
|
|
|
|
/// Debounce frames before emitting proximity alert.
|
|
const PROXIMITY_DEBOUNCE: u8 = 2;
|
|
|
|
/// Cooldown frames after proximity alert.
|
|
const ALERT_COOLDOWN: u16 = 40;
|
|
|
|
/// Distance categories based on signal strength.
|
|
const DIST_CRITICAL: f32 = 4.0; // amplitude ratio > 4.0 = very close
|
|
const DIST_WARNING: f32 = 3.0; // amplitude ratio > 3.0 = close
|
|
// Below WARNING = caution
|
|
|
|
/// Event IDs (500-series: Industrial).
|
|
pub const EVENT_PROXIMITY_WARNING: i32 = 500;
|
|
pub const EVENT_VEHICLE_DETECTED: i32 = 501;
|
|
pub const EVENT_HUMAN_NEAR_VEHICLE: i32 = 502;
|
|
|
|
/// Forklift proximity detector.
|
|
pub struct ForkliftProximityDetector {
|
|
/// Per-subcarrier baseline amplitude (calibrated).
|
|
baseline_amp: [f32; MAX_SC],
|
|
/// Phase history ring buffer for frequency analysis.
|
|
phase_history: [[f32; MAX_SC]; PHASE_HISTORY],
|
|
phase_hist_idx: usize,
|
|
phase_hist_len: usize,
|
|
/// Calibration state.
|
|
calib_amp_sum: [f32; MAX_SC],
|
|
calib_count: u32,
|
|
calibrated: bool,
|
|
/// Vehicle detection state.
|
|
vehicle_present: bool,
|
|
vehicle_debounce: u8,
|
|
vehicle_amp_ratio: f32,
|
|
/// Proximity alert state.
|
|
proximity_debounce: u8,
|
|
cooldown: u16,
|
|
/// Frame counter.
|
|
frame_count: u32,
|
|
}
|
|
|
|
impl ForkliftProximityDetector {
|
|
pub const fn new() -> Self {
|
|
Self {
|
|
baseline_amp: [0.0; MAX_SC],
|
|
phase_history: [[0.0; MAX_SC]; PHASE_HISTORY],
|
|
phase_hist_idx: 0,
|
|
phase_hist_len: 0,
|
|
calib_amp_sum: [0.0; MAX_SC],
|
|
calib_count: 0,
|
|
calibrated: false,
|
|
vehicle_present: false,
|
|
vehicle_debounce: 0,
|
|
vehicle_amp_ratio: 0.0,
|
|
proximity_debounce: 0,
|
|
cooldown: 0,
|
|
frame_count: 0,
|
|
}
|
|
}
|
|
|
|
/// Process one CSI frame.
|
|
///
|
|
/// # Arguments
|
|
/// - `phases`: per-subcarrier phase values
|
|
/// - `amplitudes`: per-subcarrier amplitude values
|
|
/// - `variance`: per-subcarrier variance values
|
|
/// - `motion_energy`: host-reported motion energy
|
|
/// - `presence`: host-reported presence flag (0/1)
|
|
/// - `n_persons`: host-reported person count
|
|
///
|
|
/// Returns events as `(event_id, value)` pairs.
|
|
pub fn process_frame(
|
|
&mut self,
|
|
phases: &[f32],
|
|
amplitudes: &[f32],
|
|
variance: &[f32],
|
|
motion_energy: f32,
|
|
presence: i32,
|
|
n_persons: i32,
|
|
) -> &[(i32, f32)] {
|
|
let n_sc = phases.len().min(amplitudes.len()).min(variance.len()).min(MAX_SC);
|
|
if n_sc < 4 {
|
|
return &[];
|
|
}
|
|
|
|
self.frame_count += 1;
|
|
|
|
if self.cooldown > 0 {
|
|
self.cooldown -= 1;
|
|
}
|
|
|
|
// Store phase history.
|
|
for i in 0..n_sc {
|
|
self.phase_history[self.phase_hist_idx][i] = phases[i];
|
|
}
|
|
self.phase_hist_idx = (self.phase_hist_idx + 1) % PHASE_HISTORY;
|
|
if self.phase_hist_len < PHASE_HISTORY {
|
|
self.phase_hist_len += 1;
|
|
}
|
|
|
|
static mut EVENTS: [(i32, f32); 4] = [(0, 0.0); 4];
|
|
let mut n_events = 0usize;
|
|
|
|
// Calibration phase: 100 frames (~5 seconds).
|
|
if !self.calibrated {
|
|
for i in 0..n_sc {
|
|
self.calib_amp_sum[i] += amplitudes[i];
|
|
}
|
|
self.calib_count += 1;
|
|
if self.calib_count >= 100 {
|
|
let n = self.calib_count as f32;
|
|
for i in 0..n_sc {
|
|
self.baseline_amp[i] = self.calib_amp_sum[i] / n;
|
|
if self.baseline_amp[i] < 0.01 {
|
|
self.baseline_amp[i] = 0.01;
|
|
}
|
|
}
|
|
self.calibrated = true;
|
|
}
|
|
return unsafe { &EVENTS[..0] };
|
|
}
|
|
|
|
// --- Step 1: Detect forklift/AGV signature ---
|
|
let amp_ratio = self.compute_amplitude_ratio(amplitudes, n_sc);
|
|
let low_freq_dominant = self.check_low_frequency_dominance(n_sc);
|
|
let vibration_sig = self.compute_vibration_signature(variance, n_sc);
|
|
|
|
let is_vehicle = amp_ratio > FORKLIFT_AMP_RATIO
|
|
&& low_freq_dominant
|
|
&& vibration_sig > VIBRATION_VAR_THRESH;
|
|
|
|
if is_vehicle {
|
|
self.vehicle_debounce = self.vehicle_debounce.saturating_add(1);
|
|
} else {
|
|
self.vehicle_debounce = self.vehicle_debounce.saturating_sub(1);
|
|
}
|
|
|
|
let was_vehicle = self.vehicle_present;
|
|
self.vehicle_present = self.vehicle_debounce >= VEHICLE_DEBOUNCE;
|
|
self.vehicle_amp_ratio = amp_ratio;
|
|
|
|
// Emit vehicle detected on transition.
|
|
if self.vehicle_present && !was_vehicle && n_events < 4 {
|
|
unsafe {
|
|
EVENTS[n_events] = (EVENT_VEHICLE_DETECTED, amp_ratio);
|
|
}
|
|
n_events += 1;
|
|
}
|
|
|
|
// --- Step 2: Check human presence near vehicle ---
|
|
let human_present = (presence > 0 || n_persons > 0)
|
|
&& motion_energy > HUMAN_MOTION_THRESH;
|
|
|
|
if self.vehicle_present && human_present {
|
|
self.proximity_debounce = self.proximity_debounce.saturating_add(1);
|
|
|
|
// Emit human-near-vehicle event on transition (debounce threshold reached).
|
|
if self.proximity_debounce == PROXIMITY_DEBOUNCE && n_events < 4 {
|
|
unsafe {
|
|
EVENTS[n_events] = (EVENT_HUMAN_NEAR_VEHICLE, motion_energy);
|
|
}
|
|
n_events += 1;
|
|
}
|
|
|
|
// Emit proximity warning with distance category.
|
|
if self.proximity_debounce >= PROXIMITY_DEBOUNCE
|
|
&& self.cooldown == 0
|
|
&& n_events < 4
|
|
{
|
|
let dist_cat = if amp_ratio > DIST_CRITICAL {
|
|
0.0 // critical
|
|
} else if amp_ratio > DIST_WARNING {
|
|
1.0 // warning
|
|
} else {
|
|
2.0 // caution
|
|
};
|
|
unsafe {
|
|
EVENTS[n_events] = (EVENT_PROXIMITY_WARNING, dist_cat);
|
|
}
|
|
n_events += 1;
|
|
self.cooldown = ALERT_COOLDOWN;
|
|
}
|
|
} else {
|
|
self.proximity_debounce = 0;
|
|
}
|
|
|
|
unsafe { &EVENTS[..n_events] }
|
|
}
|
|
|
|
/// Compute mean amplitude ratio vs baseline across subcarriers.
|
|
fn compute_amplitude_ratio(&self, amplitudes: &[f32], n_sc: usize) -> f32 {
|
|
let mut ratio_sum = 0.0f32;
|
|
let mut count = 0u32;
|
|
for i in 0..n_sc {
|
|
if self.baseline_amp[i] > 0.01 {
|
|
ratio_sum += amplitudes[i] / self.baseline_amp[i];
|
|
count += 1;
|
|
}
|
|
}
|
|
if count == 0 { 1.0 } else { ratio_sum / count as f32 }
|
|
}
|
|
|
|
/// Check if phase modulation is dominated by low frequencies (<0.3 Hz).
|
|
/// Uses simple energy ratio: variance of phase differences (proxy for
|
|
/// high-frequency content) vs total phase variance.
|
|
fn check_low_frequency_dominance(&self, n_sc: usize) -> bool {
|
|
if self.phase_hist_len < 6 {
|
|
return false;
|
|
}
|
|
|
|
// Compute total phase variance and high-frequency component.
|
|
let mut total_var = 0.0f32;
|
|
let mut hf_energy = 0.0f32;
|
|
let mut count = 0u32;
|
|
|
|
for sc in 0..n_sc.min(MAX_SC) {
|
|
// Compute mean phase for this subcarrier.
|
|
let mut sum = 0.0f32;
|
|
for t in 0..self.phase_hist_len {
|
|
let idx = (self.phase_hist_idx + PHASE_HISTORY - self.phase_hist_len + t) % PHASE_HISTORY;
|
|
sum += self.phase_history[idx][sc];
|
|
}
|
|
let mean = sum / self.phase_hist_len as f32;
|
|
|
|
// Total variance.
|
|
let mut var = 0.0f32;
|
|
for t in 0..self.phase_hist_len {
|
|
let idx = (self.phase_hist_idx + PHASE_HISTORY - self.phase_hist_len + t) % PHASE_HISTORY;
|
|
let d = self.phase_history[idx][sc] - mean;
|
|
var += d * d;
|
|
}
|
|
total_var += var;
|
|
|
|
// High-frequency: variance of first differences (approximates >1Hz).
|
|
let mut diff_var = 0.0f32;
|
|
for t in 1..self.phase_hist_len {
|
|
let idx0 = (self.phase_hist_idx + PHASE_HISTORY - self.phase_hist_len + t - 1) % PHASE_HISTORY;
|
|
let idx1 = (self.phase_hist_idx + PHASE_HISTORY - self.phase_hist_len + t) % PHASE_HISTORY;
|
|
let d = self.phase_history[idx1][sc] - self.phase_history[idx0][sc];
|
|
diff_var += d * d;
|
|
}
|
|
hf_energy += diff_var;
|
|
count += 1;
|
|
}
|
|
|
|
if count == 0 || total_var < 0.001 {
|
|
return false;
|
|
}
|
|
|
|
// Low frequency ratio: if high-freq energy is small relative to total.
|
|
let lf_ratio = 1.0 - (hf_energy / (total_var + 0.001));
|
|
lf_ratio > LOW_FREQ_RATIO_THRESH
|
|
}
|
|
|
|
/// Compute vibration signature from variance pattern.
|
|
/// Motor vibration produces elevated, relatively uniform variance.
|
|
fn compute_vibration_signature(&self, variance: &[f32], n_sc: usize) -> f32 {
|
|
let mut sum = 0.0f32;
|
|
for i in 0..n_sc {
|
|
sum += variance[i];
|
|
}
|
|
sum / n_sc as f32
|
|
}
|
|
|
|
/// Whether a vehicle is currently detected.
|
|
pub fn is_vehicle_present(&self) -> bool {
|
|
self.vehicle_present
|
|
}
|
|
|
|
/// Current amplitude ratio (proxy for vehicle proximity).
|
|
pub fn amplitude_ratio(&self) -> f32 {
|
|
self.vehicle_amp_ratio
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
fn make_detector_calibrated() -> ForkliftProximityDetector {
|
|
let mut det = ForkliftProximityDetector::new();
|
|
let phases = [0.0f32; 16];
|
|
let amps = [1.0f32; 16];
|
|
let var = [0.01f32; 16];
|
|
for _ in 0..100 {
|
|
det.process_frame(&phases, &s, &var, 0.0, 0, 0);
|
|
}
|
|
assert!(det.calibrated);
|
|
det
|
|
}
|
|
|
|
#[test]
|
|
fn test_init_state() {
|
|
let det = ForkliftProximityDetector::new();
|
|
assert!(!det.calibrated);
|
|
assert!(!det.is_vehicle_present());
|
|
assert_eq!(det.frame_count, 0);
|
|
}
|
|
|
|
#[test]
|
|
fn test_calibration() {
|
|
let mut det = ForkliftProximityDetector::new();
|
|
let phases = [0.0f32; 16];
|
|
let amps = [2.0f32; 16];
|
|
let var = [0.01f32; 16];
|
|
|
|
for _ in 0..99 {
|
|
det.process_frame(&phases, &s, &var, 0.0, 0, 0);
|
|
}
|
|
assert!(!det.calibrated);
|
|
|
|
det.process_frame(&phases, &s, &var, 0.0, 0, 0);
|
|
assert!(det.calibrated);
|
|
// Baseline should be ~2.0.
|
|
assert!((det.baseline_amp[0] - 2.0).abs() < 0.01);
|
|
}
|
|
|
|
#[test]
|
|
fn test_no_alert_quiet_scene() {
|
|
let mut det = make_detector_calibrated();
|
|
let phases = [0.0f32; 16];
|
|
let amps = [1.0f32; 16];
|
|
let var = [0.01f32; 16];
|
|
|
|
for _ in 0..50 {
|
|
let events = det.process_frame(&phases, &s, &var, 0.0, 0, 0);
|
|
assert!(events.is_empty(), "no events expected in quiet scene");
|
|
}
|
|
assert!(!det.is_vehicle_present());
|
|
}
|
|
|
|
#[test]
|
|
fn test_vehicle_detection() {
|
|
let mut det = make_detector_calibrated();
|
|
// Build up phase history first with slow-changing phases (low freq).
|
|
let var_high = [0.12f32; 16];
|
|
|
|
let mut vehicle_detected = false;
|
|
for frame in 0..30 {
|
|
// High amplitude + slow phase change + high variance = forklift.
|
|
let phase_val = 0.1 * (frame as f32); // slow ramp => low frequency
|
|
let phases = [phase_val; 16];
|
|
let amps = [3.5f32; 16]; // 3.5x baseline of 1.0
|
|
let events = det.process_frame(&phases, &s, &var_high, 0.0, 0, 0);
|
|
for &(et, _) in events {
|
|
if et == EVENT_VEHICLE_DETECTED {
|
|
vehicle_detected = true;
|
|
}
|
|
}
|
|
}
|
|
assert!(vehicle_detected, "vehicle should be detected with high amp + low freq + vibration");
|
|
}
|
|
|
|
#[test]
|
|
fn test_proximity_warning() {
|
|
let mut det = make_detector_calibrated();
|
|
let var_high = [0.12f32; 16];
|
|
|
|
let mut proximity_warned = false;
|
|
for frame in 0..40 {
|
|
let phase_val = 0.1 * (frame as f32);
|
|
let phases = [phase_val; 16];
|
|
let amps = [4.5f32; 16]; // very high = critical distance
|
|
// Human present + vehicle present => proximity warning.
|
|
let events = det.process_frame(&phases, &s, &var_high, 0.5, 1, 1);
|
|
for &(et, val) in events {
|
|
if et == EVENT_PROXIMITY_WARNING {
|
|
proximity_warned = true;
|
|
// Distance category 0 = critical (amp_ratio > 4.0).
|
|
assert!(val == 0.0 || val == 1.0 || val == 2.0);
|
|
}
|
|
}
|
|
}
|
|
assert!(proximity_warned, "proximity warning should fire when vehicle + human co-occur");
|
|
}
|
|
|
|
#[test]
|
|
fn test_cooldown_prevents_flood() {
|
|
let mut det = make_detector_calibrated();
|
|
let var_high = [0.12f32; 16];
|
|
|
|
let mut alert_count = 0u32;
|
|
for frame in 0..100 {
|
|
let phase_val = 0.1 * (frame as f32);
|
|
let phases = [phase_val; 16];
|
|
let amps = [4.0f32; 16];
|
|
let events = det.process_frame(&phases, &s, &var_high, 0.5, 1, 1);
|
|
for &(et, _) in events {
|
|
if et == EVENT_PROXIMITY_WARNING {
|
|
alert_count += 1;
|
|
}
|
|
}
|
|
}
|
|
// With ALERT_COOLDOWN=40, in 100 frames we should get at most ~3 alerts.
|
|
assert!(alert_count <= 4, "cooldown should limit alert rate, got {}", alert_count);
|
|
}
|
|
|
|
#[test]
|
|
fn test_amplitude_ratio_computation() {
|
|
let det = make_detector_calibrated();
|
|
// Baseline is 1.0, test with 3.0 amplitude.
|
|
let amps = [3.0f32; 16];
|
|
let ratio = det.compute_amplitude_ratio(&s, 16);
|
|
assert!((ratio - 3.0).abs() < 0.1, "amplitude ratio should be ~3.0, got {}", ratio);
|
|
}
|
|
}
|