From 4df9f6c1dfd4bbe1cb2b842189903b7e559660ce Mon Sep 17 00:00:00 2001 From: Gava <21977618+broslife@users.noreply.github.com> Date: Fri, 20 Nov 2026 18:38:45 -0300 Subject: [PATCH 1/3] feat(console): turn the gyro's yaw about the player's vertical Held upright, as the console is in handheld, a turn to the side reads mostly on the pad's roll axis, so games that aim with yaw alone moved the camera far less side to side than up and down. The yaw sent is now the turn about the vertical the accelerometer finds (player space, with the Gyro Wiki's 1.41 relaxation). Pitch, roll and gravity stay the pad's own. Every 10 s a moving pad logs the angle it read against the angle the host was given, so drift on the way can be measured. Co-Authored-By: Claude Opus 5.5 --- crates/pf-nx-app/src/runtime/motion.rs | 255 ++++++++++++++++++++++--- crates/pf-nx-app/src/runtime/stream.rs | 7 +- 2 files changed, 231 insertions(+), 31 deletions(-) diff --git a/crates/pf-nx-app/src/runtime/motion.rs b/crates/pf-nx-app/src/runtime/motion.rs index 8554b43..56103a4 100644 --- a/crates/pf-nx-app/src/runtime/motion.rs +++ b/crates/pf-nx-app/src/runtime/motion.rs @@ -1,8 +1,15 @@ //! Each pad's gyro and accelerometer onto its pad on the host, in the wire's units. Pure: samples //! in, rich input out, one call per pad per input tick. //! +//! The yaw sent is the player's: the turn about the vertical the accelerometer finds, so a pad +//! held upright (the console in handheld) turns the camera as far as one held level. Pitch, +//! roll and gravity stay the pad's own. +//! //! The host holds the last rate it got and stamps its own clock on the reports a game reads, so //! a moving pad sends every other tick and the turn stays the same. A pad at rest sends nothing. +//! Every 10 s a moving pad logs the angle it read against the angle the host was given. + +use std::time::{Duration, Instant}; use punktfunk_core::input::gamepad::{MOTION_ACCEL_LSB_PER_G, MOTION_GYRO_LSB_PER_DEG_S}; use punktfunk_core::quic::RichInput; @@ -19,18 +26,90 @@ const STILL_GYRO: u16 = (MOTION_GYRO_LSB_PER_DEG_S / 2) as u16; const REST_SAMPLES: u16 = 25; /// 0.02 g, about 1° of tilt: a resting accelerometer jitters by a few thousandths of a g. const REST_ACCEL: u16 = (MOTION_ACCEL_LSB_PER_G / 50) as u16; +/// Share of the way the held vertical moves to each accelerometer sample: about 200 ms at +/// 250 Hz, slow enough that a swing's own push barely tilts it. +const UP_FOLLOW: f32 = 0.02; +/// The Gyro Wiki's player-space relaxation: a turn about an axis tilted off the vertical still turns +/// in full, capped at the pad's whole yaw-and-roll rate. +const YAW_RELAX: f32 = 1.41; +/// A tick longer than this (a stall, the dial) counts as this long in the summary. +const LONGEST_TICK: Duration = Duration::from_millis(50); +const SUMMARY_EVERY: Duration = Duration::from_secs(10); +/// A window that turned less than this is not logged. +const SUMMARY_MIN_DEG: f32 = 5.0; /// A sample in the wire's counts, `(gyro, accel)`. type Counts = ([i16; 3], [i16; 3]); -fn counts(m: &Motion) -> Counts { +fn counts(gyro: [f32; 3], accel: [f32; 3]) -> Counts { let scale = |v: [f32; 3], per: f32| v.map(|c| (c * per).clamp(-32768.0, 32767.0) as i16); ( - scale(m.gyro, GYRO_LSB_PER_RAD_S), - scale(m.accel, MOTION_ACCEL_LSB_PER_G as f32), + scale(gyro, GYRO_LSB_PER_RAD_S), + scale(accel, MOTION_ACCEL_LSB_PER_G as f32), ) } +/// `gyro` with its yaw replaced by the turn about `up`, the vertical in the pad's frame (where +/// the table pushes). Gravity's share along x is left out, so aiming up or down keeps the yaw. +fn player_space(gyro: [f32; 3], up: [f32; 3]) -> [f32; 3] { + let len = up.iter().map(|c| c * c).sum::().sqrt(); + if len < 0.5 { + return gyro; + } + let [x, y, z] = gyro; + let world = (y * up[1] + z * up[2]) / len; + let yaw = world.signum() * (world.abs() * YAW_RELAX).min((y * y + z * z).sqrt()); + [x, yaw, z] +} + +/// One pad's turn over a summary window, `[yaw, pitch]` in degrees. +#[derive(Clone, Copy, Default)] +struct Sums { + time: Duration, + ticks: u32, + sends: u32, + /// Total turn either way. + travel: [f32; 2], + /// Net turn read off the pad. + read: [f32; 2], + /// Net turn the host was given: each tick at the rate it holds. + given: [f32; 2], +} + +impl Sums { + fn add(&mut self, dt: Duration, read: [f32; 3], held: [i16; 3], sent: bool) { + let s = dt.as_secs_f32(); + let per = MOTION_GYRO_LSB_PER_DEG_S as f32; + // [yaw, pitch] are the wire's y and x. + for (i, axis) in [1, 0].into_iter().enumerate() { + let r = read[axis].to_degrees() * s; + self.travel[i] += r.abs(); + self.read[i] += r; + self.given[i] += f32::from(held[axis]) / per * s; + } + self.time += dt; + self.ticks += 1; + self.sends += u32::from(sent); + } + + fn log(&self, pad: u8) { + let [ty, tp] = self.travel; + if ty.max(tp) < SUMMARY_MIN_DEG { + return; + } + let ([ry, rp], [gy, gp]) = (self.read, self.given); + tracing::info!( + "pad {pad}: gyro over {:.1}s, yaw {ty:.0}° turned, net {ry:+.1}° read {gy:+.1}° given ({:+.1}°); \ + pitch {tp:.0}° turned, net {rp:+.1}° read {gp:+.1}° given ({:+.1}°); {} of {} ticks sent", + self.time.as_secs_f32(), + gy - ry, + gp - rp, + self.sends, + self.ticks, + ); + } +} + /// One pad's motion as the host holds it. #[derive(Clone, Copy, Default)] struct PadMotion { @@ -39,6 +118,10 @@ struct PadMotion { still: u16, /// The previous tick sent, so this one stays home. just_sent: bool, + /// The vertical in the pad's frame, the accelerometer smoothed. + up: Option<[f32; 3]>, + last_tick: Option, + sums: Sums, } #[derive(Default)] @@ -47,18 +130,24 @@ pub struct MotionRouter { } impl MotionRouter { - /// One pad's latest sample, `None` when the pad is gone: the rich input to send, if any. - /// A changed sample goes out unless the previous tick sent. After [`REST_SAMPLES`] still + /// One pad's latest sample at `now`, `None` when the pad is gone: the rich input to send, if + /// any. A changed sample goes out unless the previous tick sent. After [`REST_SAMPLES`] still /// samples the pad sends one zero rate and then only a gravity shift over [`REST_ACCEL`]; /// one sample over [`STILL_GYRO`] makes it live again. A pad's first sample is logged, so a /// run shows the frame hid reports in. - pub fn step(&mut self, pad: u8, sample: Option) -> Option { + pub fn step(&mut self, pad: u8, sample: Option, now: Instant) -> Option { let p = self.pads.get_mut(usize::from(pad))?; let Some(m) = sample else { *p = PadMotion::default(); return None; }; - let (gyro, accel) = counts(&m); + let up = match p.up { + Some(up) => std::array::from_fn(|i| up[i] + (m.accel[i] - up[i]) * UP_FOLLOW), + None => m.accel, + }; + p.up = Some(up); + let turn = player_space(m.gyro, up); + let (gyro, accel) = counts(turn, m.accel); p.still = if gyro.iter().all(|c| c.unsigned_abs() <= STILL_GYRO) { p.still.saturating_add(1) } else { @@ -75,12 +164,21 @@ impl MotionRouter { } Some(_) => (gyro, accel), }; - if std::mem::take(&mut p.just_sent) || p.sent == Some(next) { - return None; + let send = !std::mem::take(&mut p.just_sent) && p.sent != Some(next); + if send { + p.sent = Some(next); + p.just_sent = true; + } + let dt = p + .last_tick + .map_or(Duration::ZERO, |t| now.saturating_duration_since(t).min(LONGEST_TICK)); + p.last_tick = Some(now); + p.sums.add(dt, turn, p.sent.map_or([0; 3], |(g, _)| g), send); + if p.sums.time >= SUMMARY_EVERY { + p.sums.log(pad); + p.sums = Sums::default(); } - p.sent = Some(next); - p.just_sent = true; - Some(RichInput::Motion { + send.then_some(RichInput::Motion { pad, gyro: next.0, accel: next.1, @@ -110,6 +208,8 @@ impl MotionRouter { mod tests { use super::*; + const TICK: Duration = Duration::from_millis(4); + fn still() -> Motion { Motion { gyro: [0.0; 3], @@ -125,9 +225,29 @@ mod tests { } } - /// Each tick's output for `samples` on pad 0. - fn run(r: &mut MotionRouter, samples: impl IntoIterator) -> Vec> { - samples.into_iter().map(|m| r.step(0, Some(m))).collect() + /// A router with its own clock, one tick per call. + struct Rig { + r: MotionRouter, + now: Instant, + } + + impl Rig { + fn new() -> Self { + Self { + r: MotionRouter::default(), + now: Instant::now(), + } + } + + fn step(&mut self, pad: u8, m: Option) -> Option { + self.now += TICK; + self.r.step(pad, m, self.now) + } + + /// Each tick's output for `samples` on pad 0. + fn run(&mut self, samples: impl IntoIterator) -> Vec> { + samples.into_iter().map(|m| self.step(0, Some(m))).collect() + } } fn gyro(out: &RichInput) -> [i16; 3] { @@ -139,7 +259,7 @@ mod tests { #[test] fn samples_go_out_in_the_wire_units_once_each() { - let mut r = MotionRouter::default(); + let mut r = Rig::new(); let turning = Motion { gyro: [std::f32::consts::PI, 0.0, -100.0], ..still() @@ -153,19 +273,71 @@ mod tests { assert!(r.step(1, Some(turning)).is_none(), "an unchanged sample stays home"); } + #[test] + fn a_level_pad_keeps_its_own_yaw() { + let level = [0.0, 1.0, 0.0]; + assert_eq!(player_space([0.1, 0.5, 0.0], level), [0.1, 0.5, 0.0]); + assert_eq!(player_space([0.1, -0.5, 0.0], level), [0.1, -0.5, 0.0]); + } + + #[test] + fn an_upright_pad_turns_as_far_as_a_level_one() { + // Screen toward the player: the table pushes along the wire's -z, and a turn to the + // left about the vertical reads on the pad's z. + let upright = [0.0, 0.0, -1.0]; + assert_eq!(player_space([0.0, 0.0, -0.5], upright), [0.0, 0.5, -0.5]); + assert_eq!(player_space([0.0, 0.0, 0.5], upright), [0.0, -0.5, 0.5]); + } + + #[test] + fn a_tilted_pad_turns_in_full() { + // Tipped 60° back: a 1 rad/s turn about the vertical splits 0.5 on y, 0.87 on z. + let (s, c) = 60f32.to_radians().sin_cos(); + let up = [0.0, c, -s]; + let [_, yaw, _] = player_space([0.0, c, -s], up); + assert!((yaw - 1.0).abs() < 1e-5, "yaw {yaw}"); + // Pitch is the pad's own, whatever the tilt. + assert_eq!(player_space([0.7, 0.0, 0.0], up)[..2], [0.7, 0.0]); + } + + #[test] + fn no_gravity_keeps_the_pad_frame() { + assert_eq!(player_space([0.1, 0.2, 0.3], [0.0; 3]), [0.1, 0.2, 0.3]); + } + + #[test] + fn the_vertical_follows_the_pad_slowly() { + let mut r = Rig::new(); + r.run(std::iter::repeat_n(still(), 10)); + // Stood upright at once and turning: the held vertical is still near level, so the + // first ticks give little yaw, then the full turn once it has caught up. + let upright_turn = Motion { + gyro: [0.0, 0.0, -1.0], + accel: [0.0, 0.0, -1.0], + }; + let out = r.run(std::iter::repeat_n(upright_turn, 500)); + let first = gyro(out[0].as_ref().unwrap())[1]; + let last = out.iter().flatten().last().map(gyro).unwrap()[1]; + assert!(first < last / 2, "first {first}, last {last}"); + assert!( + (i32::from(last) - 1146).abs() <= 2, + "1 rad/s is 1146 counts, got {last}" + ); + } + #[test] fn a_moving_pad_sends_every_other_tick() { - let mut r = MotionRouter::default(); - let out = run(&mut r, (0..100).map(|i| turning(30.0 + i as f32))); + let mut r = Rig::new(); + let out = r.run((0..100).map(|i| turning(30.0 + i as f32))); assert!(out.iter().step_by(2).all(Option::is_some)); assert!(out.iter().skip(1).step_by(2).all(Option::is_none)); } #[test] fn slow_aim_stays_live() { - let mut r = MotionRouter::default(); + let mut r = Rig::new(); // 2 °/s drifting by a count a tick: 40 counts and up, never at rest. - let out = run(&mut r, (0..200).map(|i| turning(2.0 + i as f32 * 0.05))); + let out = r.run((0..200).map(|i| turning(2.0 + i as f32 * 0.05))); let sent: Vec<_> = out.iter().flatten().collect(); assert_eq!(sent.len(), 100); assert!(sent.iter().all(|m| gyro(m)[0] >= 39)); @@ -173,7 +345,7 @@ mod tests { #[test] fn a_resting_pad_sends_one_zero_then_nothing() { - let mut r = MotionRouter::default(); + let mut r = Rig::new(); // ±0.3 °/s and ±0.002 g of noise: still, though no two ticks read the same. let noisy = |i: usize| { let sign: f32 = if i % 2 == 0 { 1.0 } else { -1.0 }; @@ -182,7 +354,7 @@ mod tests { accel: [0.0, 1.0 + 0.002 * sign, 0.0], } }; - let out = run(&mut r, (0..400).map(noisy)); + let out = r.run((0..400).map(noisy)); let (tick, last) = out .iter() .enumerate() @@ -197,8 +369,8 @@ mod tests { #[test] fn a_resting_pad_sends_a_shift_of_gravity() { - let mut r = MotionRouter::default(); - run(&mut r, std::iter::repeat_n(still(), 40)); + let mut r = Rig::new(); + r.run(std::iter::repeat_n(still(), 40)); let small = Motion { accel: [0.0, 1.01, 0.0], ..still() @@ -214,9 +386,36 @@ mod tests { assert_eq!((gyro, accel), ([0; 3], [1000, 9900, 0])); } + #[test] + fn the_sums_match_when_nothing_is_lost() { + let mut r = Rig::new(); + // 45° left, 90° right, 45° left at 90 °/s, then rest: back where it started. + let swing = |deg_s: f32, ticks| { + std::iter::repeat_n( + Motion { + gyro: [0.0, deg_s.to_radians(), 0.0], + ..still() + }, + ticks, + ) + }; + r.run( + std::iter::repeat_n(still(), 10) + .chain(swing(90.0, 125)) + .chain(swing(-90.0, 250)) + .chain(swing(90.0, 125)) + .chain(std::iter::repeat_n(still(), 50)), + ); + let s = r.r.pads[0].sums; + assert!((s.travel[0] - 180.0).abs() < 1.0, "travel {}", s.travel[0]); + assert!(s.read[0].abs() < 0.1, "read {}", s.read[0]); + // Each rate goes out a tick late and the host holds it a tick long: under a degree. + assert!(s.given[0].abs() < 1.0, "given {}", s.given[0]); + } + #[test] fn still_all_stops_the_turning_and_keeps_gravity() { - let mut r = MotionRouter::default(); + let mut r = Rig::new(); let turning = Motion { gyro: [1.0, 0.0, 0.0], ..still() @@ -224,7 +423,7 @@ mod tests { r.step(0, Some(turning)); r.step(2, Some(still())); let mut out = Vec::new(); - r.still_all(&mut out); + r.r.still_all(&mut out); assert_eq!( out, [RichInput::Motion { @@ -234,14 +433,14 @@ mod tests { }] ); out.clear(); - r.still_all(&mut out); + r.r.still_all(&mut out); assert!(out.is_empty()); assert!(r.step(0, Some(turning)).is_some(), "turning again goes out"); } #[test] fn a_pad_that_leaves_starts_over() { - let mut r = MotionRouter::default(); + let mut r = Rig::new(); r.step(0, Some(still())); assert!(r.step(0, None).is_none()); assert!(r.step(0, Some(still())).is_some()); diff --git a/crates/pf-nx-app/src/runtime/stream.rs b/crates/pf-nx-app/src/runtime/stream.rs index 25c36ef..1540e48 100644 --- a/crates/pf-nx-app/src/runtime/stream.rs +++ b/crates/pf-nx-app/src/runtime/stream.rs @@ -139,12 +139,13 @@ fn input_loop( } batch.clear(); pads.poll(&mut batch); - router.step(&batch, Instant::now(), dial_open, kind, &mut wire, &mut routed); + let now = Instant::now(); + router.step(&batch, now, dial_open, kind, &mut wire, &mut routed); if !dial_open { let client = input.client(); for pad in 0..SLOTS as u8 { let Some(sample) = batch.iter().find(|s| s.slot == pad) else { - motion.step(pad, None); + motion.step(pad, None, now); continue; }; if !motion_reaches(kind, sample, client) { @@ -153,7 +154,7 @@ fn input_loop( } continue; } - rich.extend(motion.step(pad, pads.motion(pad))); + rich.extend(motion.step(pad, pads.motion(pad), now)); } pads.touches(&mut fingers); let mode = client.mode(); From d40e7d68174600467d40ea9fc8c8a99cec47cf07 Mon Sep 17 00:00:00 2001 From: Gava <21977618+broslife@users.noreply.github.com> Date: Fri, 20 Nov 2026 19:49:39 -0300 Subject: [PATCH 2/3] fix(console): send the pad's own gyro and check its scale Relink, the game this was tried with, already turns the gyro about the player's vertical itself. The yaw this title rewrote put the same turn on two axes, so the game's estimate of up tipped and the aim sank while turning back to the left. The gyro goes out as the pad reads it again. The 10 s summary now also pairs the gyro's pitch between two steady poses with the tilt gravity measures between them: their ratio is the gyro's scale, which hid names no unit for. Co-Authored-By: Claude Opus 5.5 --- crates/pf-nx-app/src/runtime/motion.rs | 239 ++++++++++++++++--------- 1 file changed, 151 insertions(+), 88 deletions(-) diff --git a/crates/pf-nx-app/src/runtime/motion.rs b/crates/pf-nx-app/src/runtime/motion.rs index 56103a4..a5dc9cf 100644 --- a/crates/pf-nx-app/src/runtime/motion.rs +++ b/crates/pf-nx-app/src/runtime/motion.rs @@ -1,13 +1,12 @@ //! Each pad's gyro and accelerometer onto its pad on the host, in the wire's units. Pure: samples //! in, rich input out, one call per pad per input tick. //! -//! The yaw sent is the player's: the turn about the vertical the accelerometer finds, so a pad -//! held upright (the console in handheld) turns the camera as far as one held level. Pitch, -//! roll and gravity stay the pad's own. -//! //! The host holds the last rate it got and stamps its own clock on the reports a game reads, so //! a moving pad sends every other tick and the turn stays the same. A pad at rest sends nothing. -//! Every 10 s a moving pad logs the angle it read against the angle the host was given. +//! +//! Every 10 s a moving pad logs two checks. The angle it read against the angle the host was +//! given measures what the sending loses. The gyro's pitch between two steady poses against the +//! tilt gravity measures between them pins the gyro's scale (hid names no unit): 1.00 is right. use std::time::{Duration, Instant}; @@ -26,40 +25,42 @@ const STILL_GYRO: u16 = (MOTION_GYRO_LSB_PER_DEG_S / 2) as u16; const REST_SAMPLES: u16 = 25; /// 0.02 g, about 1° of tilt: a resting accelerometer jitters by a few thousandths of a g. const REST_ACCEL: u16 = (MOTION_ACCEL_LSB_PER_G / 50) as u16; -/// Share of the way the held vertical moves to each accelerometer sample: about 200 ms at -/// 250 Hz, slow enough that a swing's own push barely tilts it. -const UP_FOLLOW: f32 = 0.02; -/// The Gyro Wiki's player-space relaxation: a turn about an axis tilted off the vertical still turns -/// in full, capped at the pad's whole yaw-and-roll rate. -const YAW_RELAX: f32 = 1.41; /// A tick longer than this (a stall, the dial) counts as this long in the summary. const LONGEST_TICK: Duration = Duration::from_millis(50); const SUMMARY_EVERY: Duration = Duration::from_secs(10); /// A window that turned less than this is not logged. const SUMMARY_MIN_DEG: f32 = 5.0; +/// A steady pose for the scale check: under 5 °/s (a held hand's tremor is a few) and gravity +/// within 3% of 1 g, so the accelerometer reads tilt and not a swing's push. +const STEADY_RAD_S: f32 = 5.0 * std::f32::consts::PI / 180.0; +const STEADY_G: f32 = 0.03; +/// 40 ms of steady samples in a row make a pose. +const STEADY_TICKS: u16 = 10; +/// A tilt between two poses that the scale check counts: big enough that a degree of noise is +/// a few percent. +const SCALE_MIN_DEG: f32 = 10.0; +/// Over this much gravity on x (about 15° of roll) the pitch is not a tilt about the pad's x. +const SCALE_MAX_ROLL_G: f32 = 0.25; /// A sample in the wire's counts, `(gyro, accel)`. type Counts = ([i16; 3], [i16; 3]); -fn counts(gyro: [f32; 3], accel: [f32; 3]) -> Counts { +fn counts(m: &Motion) -> Counts { let scale = |v: [f32; 3], per: f32| v.map(|c| (c * per).clamp(-32768.0, 32767.0) as i16); ( - scale(gyro, GYRO_LSB_PER_RAD_S), - scale(accel, MOTION_ACCEL_LSB_PER_G as f32), + scale(m.gyro, GYRO_LSB_PER_RAD_S), + scale(m.accel, MOTION_ACCEL_LSB_PER_G as f32), ) } -/// `gyro` with its yaw replaced by the turn about `up`, the vertical in the pad's frame (where -/// the table pushes). Gravity's share along x is left out, so aiming up or down keeps the yaw. -fn player_space(gyro: [f32; 3], up: [f32; 3]) -> [f32; 3] { - let len = up.iter().map(|c| c * c).sum::().sqrt(); - if len < 0.5 { - return gyro; - } - let [x, y, z] = gyro; - let world = (y * up[1] + z * up[2]) / len; - let yaw = world.signum() * (world.abs() * YAW_RELAX).min((y * y + z * z).sqrt()); - [x, yaw, z] +/// The pad's tilt about its x in degrees, from gravity: it grows at the gyro's x rate. +fn tilt(accel: [f32; 3]) -> f32 { + (-accel[2]).atan2(accel[1]).to_degrees() +} + +/// `a - b` in degrees, wrapped into -180..180. +fn wrap_deg(d: f32) -> f32 { + (d + 180.0).rem_euclid(360.0) - 180.0 } /// One pad's turn over a summary window, `[yaw, pitch]` in degrees. @@ -74,6 +75,11 @@ struct Sums { read: [f32; 2], /// Net turn the host was given: each tick at the rate it holds. given: [f32; 2], + /// The scale check's least-squares sums over the tilts between poses: gyro × gravity and + /// gravity², so the scale is their ratio. + gyro_tilt: f32, + tilt_sq: f32, + tilts: u32, } impl Sums { @@ -92,15 +98,23 @@ impl Sums { self.sends += u32::from(sent); } + fn scale(&self) -> Option { + (self.tilts > 0).then(|| self.gyro_tilt / self.tilt_sq) + } + fn log(&self, pad: u8) { let [ty, tp] = self.travel; if ty.max(tp) < SUMMARY_MIN_DEG { return; } let ([ry, rp], [gy, gp]) = (self.read, self.given); + let scale = match self.scale() { + Some(s) => format!("gyro scale {s:.2} over {} tilts", self.tilts), + None => "no tilt to check the scale".to_string(), + }; tracing::info!( "pad {pad}: gyro over {:.1}s, yaw {ty:.0}° turned, net {ry:+.1}° read {gy:+.1}° given ({:+.1}°); \ - pitch {tp:.0}° turned, net {rp:+.1}° read {gp:+.1}° given ({:+.1}°); {} of {} ticks sent", + pitch {tp:.0}° turned, net {rp:+.1}° read {gp:+.1}° given ({:+.1}°); {} of {} ticks sent; {scale}", self.time.as_secs_f32(), gy - ry, gp - rp, @@ -110,6 +124,48 @@ impl Sums { } } +/// The scale check's running state: the gyro's x integrated, and the last steady pose. +#[derive(Clone, Copy, Default)] +struct ScaleCheck { + /// Degrees about x since the pad came, by the gyro. + pitch: f32, + steady: u16, + /// `(tilt by gravity, pitch by the gyro)` at the last steady sample, in degrees. + pose: Option<(f32, f32)>, +} + +impl ScaleCheck { + /// Pairs each new pose with the one before it: a tilt over [`SCALE_MIN_DEG`] between them, + /// the pad not rolled at either, goes into `sums`. + fn step(&mut self, m: &Motion, dt: Duration, sums: &mut Sums) { + self.pitch += m.gyro[0].to_degrees() * dt.as_secs_f32(); + let rate = m.gyro.iter().map(|c| c * c).sum::().sqrt(); + let g = m.accel.iter().map(|c| c * c).sum::().sqrt(); + let level = m.accel[0].abs() < SCALE_MAX_ROLL_G; + if rate >= STEADY_RAD_S || (g - 1.0).abs() >= STEADY_G || !level { + self.steady = 0; + if !level { + self.pose = None; + } + return; + } + self.steady = self.steady.saturating_add(1); + if self.steady < STEADY_TICKS { + return; + } + let now = (tilt(m.accel), self.pitch); + if let (STEADY_TICKS, Some((t0, p0))) = (self.steady, self.pose) { + let by_gravity = wrap_deg(now.0 - t0); + if by_gravity.abs() >= SCALE_MIN_DEG { + sums.gyro_tilt += (now.1 - p0) * by_gravity; + sums.tilt_sq += by_gravity * by_gravity; + sums.tilts += 1; + } + } + self.pose = Some(now); + } +} + /// One pad's motion as the host holds it. #[derive(Clone, Copy, Default)] struct PadMotion { @@ -118,10 +174,9 @@ struct PadMotion { still: u16, /// The previous tick sent, so this one stays home. just_sent: bool, - /// The vertical in the pad's frame, the accelerometer smoothed. - up: Option<[f32; 3]>, last_tick: Option, sums: Sums, + scale: ScaleCheck, } #[derive(Default)] @@ -141,13 +196,7 @@ impl MotionRouter { *p = PadMotion::default(); return None; }; - let up = match p.up { - Some(up) => std::array::from_fn(|i| up[i] + (m.accel[i] - up[i]) * UP_FOLLOW), - None => m.accel, - }; - p.up = Some(up); - let turn = player_space(m.gyro, up); - let (gyro, accel) = counts(turn, m.accel); + let (gyro, accel) = counts(&m); p.still = if gyro.iter().all(|c| c.unsigned_abs() <= STILL_GYRO) { p.still.saturating_add(1) } else { @@ -173,7 +222,8 @@ impl MotionRouter { .last_tick .map_or(Duration::ZERO, |t| now.saturating_duration_since(t).min(LONGEST_TICK)); p.last_tick = Some(now); - p.sums.add(dt, turn, p.sent.map_or([0; 3], |(g, _)| g), send); + p.sums.add(dt, m.gyro, p.sent.map_or([0; 3], |(g, _)| g), send); + p.scale.step(&m, dt, &mut p.sums); if p.sums.time >= SUMMARY_EVERY { p.sums.log(pad); p.sums = Sums::default(); @@ -273,58 +323,6 @@ mod tests { assert!(r.step(1, Some(turning)).is_none(), "an unchanged sample stays home"); } - #[test] - fn a_level_pad_keeps_its_own_yaw() { - let level = [0.0, 1.0, 0.0]; - assert_eq!(player_space([0.1, 0.5, 0.0], level), [0.1, 0.5, 0.0]); - assert_eq!(player_space([0.1, -0.5, 0.0], level), [0.1, -0.5, 0.0]); - } - - #[test] - fn an_upright_pad_turns_as_far_as_a_level_one() { - // Screen toward the player: the table pushes along the wire's -z, and a turn to the - // left about the vertical reads on the pad's z. - let upright = [0.0, 0.0, -1.0]; - assert_eq!(player_space([0.0, 0.0, -0.5], upright), [0.0, 0.5, -0.5]); - assert_eq!(player_space([0.0, 0.0, 0.5], upright), [0.0, -0.5, 0.5]); - } - - #[test] - fn a_tilted_pad_turns_in_full() { - // Tipped 60° back: a 1 rad/s turn about the vertical splits 0.5 on y, 0.87 on z. - let (s, c) = 60f32.to_radians().sin_cos(); - let up = [0.0, c, -s]; - let [_, yaw, _] = player_space([0.0, c, -s], up); - assert!((yaw - 1.0).abs() < 1e-5, "yaw {yaw}"); - // Pitch is the pad's own, whatever the tilt. - assert_eq!(player_space([0.7, 0.0, 0.0], up)[..2], [0.7, 0.0]); - } - - #[test] - fn no_gravity_keeps_the_pad_frame() { - assert_eq!(player_space([0.1, 0.2, 0.3], [0.0; 3]), [0.1, 0.2, 0.3]); - } - - #[test] - fn the_vertical_follows_the_pad_slowly() { - let mut r = Rig::new(); - r.run(std::iter::repeat_n(still(), 10)); - // Stood upright at once and turning: the held vertical is still near level, so the - // first ticks give little yaw, then the full turn once it has caught up. - let upright_turn = Motion { - gyro: [0.0, 0.0, -1.0], - accel: [0.0, 0.0, -1.0], - }; - let out = r.run(std::iter::repeat_n(upright_turn, 500)); - let first = gyro(out[0].as_ref().unwrap())[1]; - let last = out.iter().flatten().last().map(gyro).unwrap()[1]; - assert!(first < last / 2, "first {first}, last {last}"); - assert!( - (i32::from(last) - 1146).abs() <= 2, - "1 rad/s is 1146 counts, got {last}" - ); - } - #[test] fn a_moving_pad_sends_every_other_tick() { let mut r = Rig::new(); @@ -413,6 +411,71 @@ mod tests { assert!(s.given[0].abs() < 1.0, "given {}", s.given[0]); } + /// Steady, then tipped about x by `deg` at 90 °/s with the gyro reading `scale` of the true + /// rate, then steady again; `tilt0` is where it starts. + fn tip(tilt0: f32, deg: f32, scale: f32) -> Vec { + let up = |t: f32| { + let (s, c) = t.to_radians().sin_cos(); + [0.0, c, -s] + }; + let steady = |t: f32| { + std::iter::repeat_n( + Motion { + gyro: [0.0; 3], + accel: up(t), + }, + 20, + ) + }; + let ticks = (deg.abs() / 90.0 / TICK.as_secs_f32()).round() as usize; + let step = deg / ticks as f32; + let moving = (1..=ticks).map(move |i| Motion { + gyro: [(90f32 * deg.signum() * scale).to_radians(), 0.0, 0.0], + accel: up(tilt0 + step * i as f32), + }); + steady(tilt0).chain(moving).chain(steady(tilt0 + deg)).collect() + } + + #[test] + fn tilt_follows_the_gyro_x() { + assert!(tilt([0.0, 1.0, 0.0]).abs() < 1e-4); + let (s, c) = 30f32.to_radians().sin_cos(); + assert!((tilt([0.0, c, -s]) - 30.0).abs() < 1e-3); + assert!((wrap_deg(350.0) + 10.0).abs() < 1e-3); + } + + #[test] + fn a_true_gyro_reads_scale_one() { + let mut r = Rig::new(); + r.run(tip(0.0, 45.0, 1.0).into_iter().chain(tip(45.0, -60.0, 1.0))); + let s = r.r.pads[0].sums; + assert_eq!(s.tilts, 2); + let k = s.scale().unwrap(); + assert!((k - 1.0).abs() < 0.03, "scale {k}"); + } + + #[test] + fn a_weak_gyro_reads_its_scale() { + let mut r = Rig::new(); + r.run(tip(-20.0, 50.0, 0.6)); + let k = r.r.pads[0].sums.scale().unwrap(); + assert!((k - 0.6).abs() < 0.03, "scale {k}"); + } + + #[test] + fn small_or_rolled_tilts_are_left_out() { + let mut r = Rig::new(); + r.run(tip(0.0, 5.0, 1.0)); + assert_eq!(r.r.pads[0].sums.tilts, 0, "5° is under the check"); + let rolled = Motion { + gyro: [0.0; 3], + accel: [0.5, 0.866, 0.0], + }; + r.run(std::iter::repeat_n(rolled, 20)); + r.run(tip(0.0, 40.0, 1.0)); + assert_eq!(r.r.pads[0].sums.tilts, 1, "only the pose after the roll pairs"); + } + #[test] fn still_all_stops_the_turning_and_keeps_gravity() { let mut r = Rig::new(); From 700e5346ef261213392b593fa93cb6d82f769bf4 Mon Sep 17 00:00:00 2001 From: Gava <21977618+broslife@users.noreply.github.com> Date: Fri, 20 Nov 2026 20:21:15 -0300 Subject: [PATCH 3/3] feat(console): check the gyro against gravity on every axis The scale check found no pose in a handheld run: its steady limits (5 deg/s, 3% of 1 g) were tighter than a held console, and it only looked at tilts about x. The 10 s summary now gives each wire axis's turn, net and loss, the mean gravity, how much of the window was calm and near 1 g, and, between steady poses (10 deg/s, 8%), the gyro's response to each axis gravity saw turn about: (1, 0, 0) for x when hid's unit and axes are right. Co-Authored-By: Claude Opus 5.5 --- crates/pf-nx-app/src/runtime/motion.rs | 311 +++++++++++++++---------- 1 file changed, 192 insertions(+), 119 deletions(-) diff --git a/crates/pf-nx-app/src/runtime/motion.rs b/crates/pf-nx-app/src/runtime/motion.rs index a5dc9cf..8f63343 100644 --- a/crates/pf-nx-app/src/runtime/motion.rs +++ b/crates/pf-nx-app/src/runtime/motion.rs @@ -4,9 +4,10 @@ //! The host holds the last rate it got and stamps its own clock on the reports a game reads, so //! a moving pad sends every other tick and the turn stays the same. A pad at rest sends nothing. //! -//! Every 10 s a moving pad logs two checks. The angle it read against the angle the host was -//! given measures what the sending loses. The gyro's pitch between two steady poses against the -//! tilt gravity measures between them pins the gyro's scale (hid names no unit): 1.00 is right. +//! Every 10 s a moving pad logs, per wire axis, the turn it read and what the host was given +//! (what the sending loses), the mean gravity (how the pad was held), and a gravity check: between +//! two steady poses, the turn gravity sees against the gyro's. Each axis gravity saw turn gives +//! the gyro's response to it, `(1.00, 0.00, 0.00)` for x when hid's unit and axes are right. use std::time::{Duration, Instant}; @@ -30,139 +31,185 @@ const LONGEST_TICK: Duration = Duration::from_millis(50); const SUMMARY_EVERY: Duration = Duration::from_secs(10); /// A window that turned less than this is not logged. const SUMMARY_MIN_DEG: f32 = 5.0; -/// A steady pose for the scale check: under 5 °/s (a held hand's tremor is a few) and gravity -/// within 3% of 1 g, so the accelerometer reads tilt and not a swing's push. -const STEADY_RAD_S: f32 = 5.0 * std::f32::consts::PI / 180.0; -const STEADY_G: f32 = 0.03; +/// A steady pose: under 10 °/s (a held hand's tremor is a few) and gravity within 8% of 1 g, +/// so the accelerometer reads mostly gravity and not a swing's push. +const STEADY_RAD_S: f32 = 10.0 * std::f32::consts::PI / 180.0; +const STEADY_G: f32 = 0.08; /// 40 ms of steady samples in a row make a pose. const STEADY_TICKS: u16 = 10; -/// A tilt between two poses that the scale check counts: big enough that a degree of noise is -/// a few percent. -const SCALE_MIN_DEG: f32 = 10.0; -/// Over this much gravity on x (about 15° of roll) the pitch is not a tilt about the pad's x. -const SCALE_MAX_ROLL_G: f32 = 0.25; +/// A turn between two poses the check counts: big enough that a degree of noise is a few +/// percent. The same floor, squared, on an axis's share before its response is logged. +const CHECK_MIN_RAD: f32 = 10.0 * std::f32::consts::PI / 180.0; + +type V3 = [f32; 3]; + +fn dot(a: V3, b: V3) -> f32 { + a[0] * b[0] + a[1] * b[1] + a[2] * b[2] +} + +fn cross(a: V3, b: V3) -> V3 { + [ + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0], + ] +} + +fn norm(a: V3) -> f32 { + dot(a, a).sqrt() +} /// A sample in the wire's counts, `(gyro, accel)`. type Counts = ([i16; 3], [i16; 3]); fn counts(m: &Motion) -> Counts { - let scale = |v: [f32; 3], per: f32| v.map(|c| (c * per).clamp(-32768.0, 32767.0) as i16); + let scale = |v: V3, per: f32| v.map(|c| (c * per).clamp(-32768.0, 32767.0) as i16); ( scale(m.gyro, GYRO_LSB_PER_RAD_S), scale(m.accel, MOTION_ACCEL_LSB_PER_G as f32), ) } -/// The pad's tilt about its x in degrees, from gravity: it grows at the gyro's x rate. -fn tilt(accel: [f32; 3]) -> f32 { - (-accel[2]).atan2(accel[1]).to_degrees() +/// The pad's turn from pose `up0` to pose `up1` as gravity sees it, a rotation vector in radians +/// in the pad's frame: the share across gravity only, since a turn about it moves nothing. +fn turn_by_gravity(up0: V3, up1: V3) -> Option { + let (a, b) = (up0.map(|c| c / norm(up0)), up1.map(|c| c / norm(up1))); + let c = cross(a, b); + let s = norm(c); + let angle = s.atan2(dot(a, b)); + // The pad turns one way, gravity seen from it the other. + (angle >= CHECK_MIN_RAD).then(|| c.map(|v| -v / s * angle)) } -/// `a - b` in degrees, wrapped into -180..180. -fn wrap_deg(d: f32) -> f32 { - (d + 180.0).rem_euclid(360.0) - 180.0 -} - -/// One pad's turn over a summary window, `[yaw, pitch]` in degrees. +/// One pad's motion over a summary window. Angles in degrees, per wire axis. #[derive(Clone, Copy, Default)] struct Sums { time: Duration, ticks: u32, sends: u32, /// Total turn either way. - travel: [f32; 2], + travel: V3, /// Net turn read off the pad. - read: [f32; 2], + read: V3, /// Net turn the host was given: each tick at the rate it holds. - given: [f32; 2], - /// The scale check's least-squares sums over the tilts between poses: gyro × gravity and - /// gravity², so the scale is their ratio. - gyro_tilt: f32, - tilt_sq: f32, - tilts: u32, + given: V3, + /// Gravity summed over the ticks, and its length summed. + accel: V3, + g: f32, + /// Ticks under [`STEADY_RAD_S`], ticks within [`STEADY_G`] of 1 g. + calm: u32, + one_g: u32, + /// The gravity check: per axis gravity saw turn about, the gyro's turn weighted by it, and + /// its weight; their ratio is the gyro's response to that axis. + response: [V3; 3], + weight: V3, + pairs: u32, } impl Sums { - fn add(&mut self, dt: Duration, read: [f32; 3], held: [i16; 3], sent: bool) { + fn add(&mut self, dt: Duration, m: &Motion, held: [i16; 3], sent: bool) { let s = dt.as_secs_f32(); let per = MOTION_GYRO_LSB_PER_DEG_S as f32; - // [yaw, pitch] are the wire's y and x. - for (i, axis) in [1, 0].into_iter().enumerate() { - let r = read[axis].to_degrees() * s; + for i in 0..3 { + let r = m.gyro[i].to_degrees() * s; self.travel[i] += r.abs(); self.read[i] += r; - self.given[i] += f32::from(held[axis]) / per * s; + self.given[i] += f32::from(held[i]) / per * s; + self.accel[i] += m.accel[i]; } + let g = norm(m.accel); + self.g += g; + self.calm += u32::from(norm(m.gyro) < STEADY_RAD_S); + self.one_g += u32::from((g - 1.0).abs() < STEADY_G); self.time += dt; self.ticks += 1; self.sends += u32::from(sent); } - fn scale(&self) -> Option { - (self.tilts > 0).then(|| self.gyro_tilt / self.tilt_sq) + fn pair(&mut self, by_gravity: V3, by_gyro: V3) { + for (axis, w) in by_gravity.into_iter().enumerate() { + for (r, g) in self.response[axis].iter_mut().zip(by_gyro) { + *r += g * w; + } + self.weight[axis] += w * w; + } + self.pairs += 1; + } + + /// The gyro's response to a turn about `axis`, once gravity saw enough of one. + fn response(&self, axis: usize) -> Option { + let w = self.weight[axis]; + (w >= CHECK_MIN_RAD * CHECK_MIN_RAD).then(|| self.response[axis].map(|r| r / w)) } fn log(&self, pad: u8) { - let [ty, tp] = self.travel; - if ty.max(tp) < SUMMARY_MIN_DEG { + if self.travel.iter().all(|t| *t < SUMMARY_MIN_DEG) { return; } - let ([ry, rp], [gy, gp]) = (self.read, self.given); - let scale = match self.scale() { - Some(s) => format!("gyro scale {s:.2} over {} tilts", self.tilts), - None => "no tilt to check the scale".to_string(), - }; + let n = self.ticks.max(1) as f32; + let pct = |c: u32| c as f32 * 100.0 / n; + let [tx, ty, tz] = self.travel; + let [rx, ry, rz] = self.read; + let [lx, ly, lz] = std::array::from_fn::(|i| self.given[i] - self.read[i]); + let [ax, ay, az] = self.accel.map(|a| a / n); + let check = (0..3) + .map(|axis| match self.response(axis) { + Some([x, y, z]) => format!("{}→({x:.2}, {y:.2}, {z:.2})", ["x", "y", "z"][axis]), + None => format!("{}→—", ["x", "y", "z"][axis]), + }) + .collect::>() + .join(" "); tracing::info!( - "pad {pad}: gyro over {:.1}s, yaw {ty:.0}° turned, net {ry:+.1}° read {gy:+.1}° given ({:+.1}°); \ - pitch {tp:.0}° turned, net {rp:+.1}° read {gp:+.1}° given ({:+.1}°); {} of {} ticks sent; {scale}", + "pad {pad}: gyro over {:.1}s; turned x {tx:.0}° y {ty:.0}° z {tz:.0}°; net x {rx:+.1}° y {ry:+.1}° \ + z {rz:+.1}°, lost x {lx:+.1}° y {ly:+.1}° z {lz:+.1}°; gravity ({ax:+.2}, {ay:+.2}, {az:+.2}) \ + |g| {:.3}; calm {:.0}% 1g {:.0}%; {} poses paired, {check}; {} of {} ticks sent", self.time.as_secs_f32(), - gy - ry, - gp - rp, + self.g / n, + pct(self.calm), + pct(self.one_g), + self.pairs, self.sends, self.ticks, ); } } -/// The scale check's running state: the gyro's x integrated, and the last steady pose. +/// The gravity check's running state: the gyro integrated, and the last steady pose. #[derive(Clone, Copy, Default)] -struct ScaleCheck { - /// Degrees about x since the pad came, by the gyro. - pitch: f32, +struct GravityCheck { + /// Radians about each axis since the pad came, by the gyro. + turned: V3, steady: u16, - /// `(tilt by gravity, pitch by the gyro)` at the last steady sample, in degrees. - pose: Option<(f32, f32)>, + /// `(gravity, turned)` at the last steady sample. + pose: Option<(V3, V3)>, } -impl ScaleCheck { - /// Pairs each new pose with the one before it: a tilt over [`SCALE_MIN_DEG`] between them, - /// the pad not rolled at either, goes into `sums`. +impl GravityCheck { + /// Pairs each new pose with the one before it: a turn over [`CHECK_MIN_RAD`] across gravity + /// goes into `sums`, the gyro's turn taken across gravity the same way. fn step(&mut self, m: &Motion, dt: Duration, sums: &mut Sums) { - self.pitch += m.gyro[0].to_degrees() * dt.as_secs_f32(); - let rate = m.gyro.iter().map(|c| c * c).sum::().sqrt(); - let g = m.accel.iter().map(|c| c * c).sum::().sqrt(); - let level = m.accel[0].abs() < SCALE_MAX_ROLL_G; - if rate >= STEADY_RAD_S || (g - 1.0).abs() >= STEADY_G || !level { + let s = dt.as_secs_f32(); + for (t, w) in self.turned.iter_mut().zip(m.gyro) { + *t += w * s; + } + if norm(m.gyro) >= STEADY_RAD_S || (norm(m.accel) - 1.0).abs() >= STEADY_G { self.steady = 0; - if !level { - self.pose = None; - } return; } self.steady = self.steady.saturating_add(1); if self.steady < STEADY_TICKS { return; } - let now = (tilt(m.accel), self.pitch); - if let (STEADY_TICKS, Some((t0, p0))) = (self.steady, self.pose) { - let by_gravity = wrap_deg(now.0 - t0); - if by_gravity.abs() >= SCALE_MIN_DEG { - sums.gyro_tilt += (now.1 - p0) * by_gravity; - sums.tilt_sq += by_gravity * by_gravity; - sums.tilts += 1; + if let (STEADY_TICKS, Some((up0, t0))) = (self.steady, self.pose) { + if let Some(by_gravity) = turn_by_gravity(up0, m.accel) { + let mid = std::array::from_fn(|i| up0[i] / norm(up0) + m.accel[i] / norm(m.accel)); + let mid = mid.map(|c: f32| c / norm(mid)); + let by_gyro: V3 = std::array::from_fn(|i| self.turned[i] - t0[i]); + let along = dot(by_gyro, mid); + sums.pair(by_gravity, std::array::from_fn(|i| by_gyro[i] - mid[i] * along)); } } - self.pose = Some(now); + self.pose = Some((m.accel, self.turned)); } } @@ -176,7 +223,7 @@ struct PadMotion { just_sent: bool, last_tick: Option, sums: Sums, - scale: ScaleCheck, + check: GravityCheck, } #[derive(Default)] @@ -222,8 +269,8 @@ impl MotionRouter { .last_tick .map_or(Duration::ZERO, |t| now.saturating_duration_since(t).min(LONGEST_TICK)); p.last_tick = Some(now); - p.sums.add(dt, m.gyro, p.sent.map_or([0; 3], |(g, _)| g), send); - p.scale.step(&m, dt, &mut p.sums); + p.sums.add(dt, &m, p.sent.map_or([0; 3], |(g, _)| g), send); + p.check.step(&m, dt, &mut p.sums); if p.sums.time >= SUMMARY_EVERY { p.sums.log(pad); p.sums = Sums::default(); @@ -298,6 +345,10 @@ mod tests { fn run(&mut self, samples: impl IntoIterator) -> Vec> { samples.into_iter().map(|m| self.step(0, Some(m))).collect() } + + fn sums(&self) -> Sums { + self.r.pads[0].sums + } } fn gyro(out: &RichInput) -> [i16; 3] { @@ -404,76 +455,98 @@ mod tests { .chain(swing(90.0, 125)) .chain(std::iter::repeat_n(still(), 50)), ); - let s = r.r.pads[0].sums; - assert!((s.travel[0] - 180.0).abs() < 1.0, "travel {}", s.travel[0]); - assert!(s.read[0].abs() < 0.1, "read {}", s.read[0]); + let s = r.sums(); + assert!((s.travel[1] - 180.0).abs() < 1.0, "travel {}", s.travel[1]); + assert!(s.read[1].abs() < 0.1, "read {}", s.read[1]); // Each rate goes out a tick late and the host holds it a tick long: under a degree. - assert!(s.given[0].abs() < 1.0, "given {}", s.given[0]); + assert!(s.given[1].abs() < 1.0, "given {}", s.given[1]); } - /// Steady, then tipped about x by `deg` at 90 °/s with the gyro reading `scale` of the true - /// rate, then steady again; `tilt0` is where it starts. - fn tip(tilt0: f32, deg: f32, scale: f32) -> Vec { - let up = |t: f32| { - let (s, c) = t.to_radians().sin_cos(); - [0.0, c, -s] - }; - let steady = |t: f32| { + /// `v` turned by `deg` about the unit `axis` (Rodrigues). + fn rotate(v: V3, axis: V3, deg: f32) -> V3 { + let (s, c) = deg.to_radians().sin_cos(); + let k = cross(axis, v); + let d = dot(axis, v) * (1.0 - c); + std::array::from_fn(|i| v[i] * c + k[i] * s + axis[i] * d) + } + + /// Steady at gravity `up`, then turned by `deg` about the unit `axis` at 90 °/s while the gyro + /// reads `gyro_of(rate vector)`, then steady again. Returns the samples and the gravity after. + fn turn(up: V3, axis: V3, deg: f32, gyro_of: impl Fn(V3) -> V3) -> (Vec, V3) { + let steady = |up: V3| { std::iter::repeat_n( Motion { gyro: [0.0; 3], - accel: up(t), + accel: up, }, 20, ) }; let ticks = (deg.abs() / 90.0 / TICK.as_secs_f32()).round() as usize; - let step = deg / ticks as f32; - let moving = (1..=ticks).map(move |i| Motion { - gyro: [(90f32 * deg.signum() * scale).to_radians(), 0.0, 0.0], - accel: up(tilt0 + step * i as f32), + let rate = axis.map(|a| (90f32 * deg.signum() * a).to_radians()); + // Seen from the pad, gravity turns the other way. + let moving = (1..=ticks).map(|i| Motion { + gyro: gyro_of(rate), + accel: rotate(up, axis, -deg * i as f32 / ticks as f32), }); - steady(tilt0).chain(moving).chain(steady(tilt0 + deg)).collect() + let end = rotate(up, axis, -deg); + (steady(up).chain(moving).chain(steady(end)).collect(), end) + } + + fn near(a: V3, b: V3) -> bool { + a.iter().zip(b).all(|(x, y)| (x - y).abs() < 0.03) } #[test] - fn tilt_follows_the_gyro_x() { - assert!(tilt([0.0, 1.0, 0.0]).abs() < 1e-4); - let (s, c) = 30f32.to_radians().sin_cos(); - assert!((tilt([0.0, c, -s]) - 30.0).abs() < 1e-3); - assert!((wrap_deg(350.0) + 10.0).abs() < 1e-3); + fn gravity_sees_a_tilt_about_x_and_a_roll_about_z() { + let level = [0.0, 1.0, 0.0]; + let tilt = turn_by_gravity(level, rotate(level, [1.0, 0.0, 0.0], -30.0)).unwrap(); + assert!(near(tilt, [30f32.to_radians(), 0.0, 0.0]), "{tilt:?}"); + let roll = turn_by_gravity(level, rotate(level, [0.0, 0.0, 1.0], -30.0)).unwrap(); + assert!(near(roll, [0.0, 0.0, 30f32.to_radians()]), "{roll:?}"); + assert!(turn_by_gravity(level, rotate(level, [1.0, 0.0, 0.0], 5.0)).is_none()); } #[test] - fn a_true_gyro_reads_scale_one() { + fn a_true_gyro_responds_one_to_one() { let mut r = Rig::new(); - r.run(tip(0.0, 45.0, 1.0).into_iter().chain(tip(45.0, -60.0, 1.0))); - let s = r.r.pads[0].sums; - assert_eq!(s.tilts, 2); - let k = s.scale().unwrap(); - assert!((k - 1.0).abs() < 0.03, "scale {k}"); + let upright = [0.0, 0.0, -1.0]; + // Upright: up and down about x, then a wheel turn about y. + let (a, up) = turn(upright, [1.0, 0.0, 0.0], 40.0, |w| w); + let (b, up) = turn(up, [1.0, 0.0, 0.0], -40.0, |w| w); + let (c, _) = turn(up, [0.0, 1.0, 0.0], 45.0, |w| w); + r.run(a.into_iter().chain(b).chain(c)); + let s = r.sums(); + assert_eq!(s.pairs, 3); + assert!(near(s.response(0).unwrap(), [1.0, 0.0, 0.0]), "{:?}", s.response(0)); + assert!(near(s.response(1).unwrap(), [0.0, 1.0, 0.0]), "{:?}", s.response(1)); + assert!(s.response(2).is_none(), "nothing turned about z across gravity"); } #[test] - fn a_weak_gyro_reads_its_scale() { + fn a_weak_or_crossed_gyro_shows_in_its_response() { let mut r = Rig::new(); - r.run(tip(-20.0, 50.0, 0.6)); - let k = r.r.pads[0].sums.scale().unwrap(); - assert!((k - 0.6).abs() < 0.03, "scale {k}"); + // A gyro at 60% on x that also leaks a third of x onto y. + let (a, _) = turn([0.0, 0.0, -1.0], [1.0, 0.0, 0.0], 45.0, |[x, y, z]| { + [0.6 * x, y + 0.3 * x, z] + }); + r.run(a); + let k = r.sums().response(0).unwrap(); + // The leak's share along gravity is unseen, so y shows most but not all of it. + assert!((k[0] - 0.6).abs() < 0.03 && k[1] > 0.2, "{k:?}"); } #[test] - fn small_or_rolled_tilts_are_left_out() { + fn a_swing_is_no_pose() { let mut r = Rig::new(); - r.run(tip(0.0, 5.0, 1.0)); - assert_eq!(r.r.pads[0].sums.tilts, 0, "5° is under the check"); - let rolled = Motion { - gyro: [0.0; 3], - accel: [0.5, 0.866, 0.0], - }; - r.run(std::iter::repeat_n(rolled, 20)); - r.run(tip(0.0, 40.0, 1.0)); - assert_eq!(r.r.pads[0].sums.tilts, 1, "only the pose after the roll pairs"); + // Gravity at 1.2 g: a push, not a pose, so nothing pairs. + let push = |up: V3| up.map(|c| c * 1.2); + let (a, _) = turn([0.0, 1.0, 0.0], [1.0, 0.0, 0.0], 45.0, |w| w); + r.run(a.into_iter().map(|m| Motion { + accel: push(m.accel), + ..m + })); + assert_eq!(r.sums().pairs, 0); } #[test]