From 26d5e3cbe4d0755e4c177b62cbfee5d20357bf18 Mon Sep 17 00:00:00 2001 From: Mikkeli Date: Wed, 14 Oct 2026 13:28:40 +0900 Subject: [PATCH] imu: raw samples on yukart/raw/imu, recorded as imu.ndjson; calibration from a session --calibrate SESSION [--write]: down from two level stretches (turning round cancels the slope), forward from straight pull-aways and stops; saved to /var/lib/yukart/imu/mount.json (StateDirectory), which the service reloads on change. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz --- README.md | 5 +- deploy/yukart-imu.service | 5 +- src/imu/calibrate.rs | 230 ++++++++++++++++++++++++++++++++++++++ src/imu/frame.rs | 46 +++++++- src/imu/main.rs | 134 ++++++++++++++++++++-- src/recorder/main.rs | 7 +- src/recorder/session.rs | 13 ++- src/recorder/sources.rs | 9 +- 8 files changed, 429 insertions(+), 20 deletions(-) create mode 100644 src/imu/calibrate.rs diff --git a/README.md b/README.md index 3cdd992..43f4363 100644 --- a/README.md +++ b/README.md @@ -15,12 +15,12 @@ needed. | Binary | Installed as | Does | |---|---|---| -| `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound; every `yukart/vehicle/*` value (decoded and OBD-II) beside them | +| `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound; every `yukart/vehicle/*` value (decoded and OBD-II) and every raw IMU sample (`yukart/raw/imu`) beside them | | `decoder` | `yukart-decoder.service` | The car's own broadcast frames to named signals (`yukart/vehicle/*`): every `z27ag` signal in contracts' `signals.json`, laid out by its `can` block. Sent on change (booleans at once, numbers at most every 50 ms); a value that has not changed is re-sent once a second, so svc-gw never marks a steady one stale; reads only | | `bus-bench` | not installed: a test tool | Raw CAN and decoded-signal load over MQTT, measured: loss, latency, broker CPU ([report](docs/impl/reports/BUS-BENCH.md)) | | `obd-poller` | `yukart-obd-poller.service` | OBD-II mode 01 on the K-line through the Bluetooth ELM327 (address in `/etc/default/yukart-elm`), the single writer to the car; never on CAN. The `obd_*` signals, calibration references (`yukart/vehicle/*`): rpm, speed, throttle, load, MAF, timing, O2 every round; temperatures and trims every 5 s. Lets go of the adapter after a minute of ECU silence | | `gnss` | `yukart-gnss.service` | gpsd's fix as single values on `yukart/gnss/*`: fix, speed, heading, position, altitude, satellites | -| `imu` | `yukart-imu.service` | The WT61 (plus) on any USB-UART adapter, found by itself (adapter and speed), turned into the car's frame (`--forward`, `--right`): roll, pitch, gravity-free longitudinal and lateral g, yaw rate on `yukart/imu/*`, at most every 40 ms. Never writes to the module | +| `imu` | `yukart-imu.service` | The WT61 (plus) on any USB-UART adapter, found by itself (adapter and speed), turned into the car's frame (`--forward`, `--right`): roll, pitch, gravity-free longitudinal and lateral g, yaw rate on `yukart/imu/*`, at most every 40 ms; every sample raw on `yukart/raw/imu` for the recorder. Mounting from `/var/lib/yukart/imu/mount.json` when calibrated (`--calibrate SESSION --write`, from checklist steps 27–30). Never writes to the module | | `orin-status` | `yukart-orin-status.service` | The Orin's health on the bus, once a second: temperatures, CPU/GPU load, memory, power, fan, WiFi (with SSID)/LTE/tablet links, the CAN interface, the ELM's Bluetooth link, UPS battery (`yukart/host/*`). Powers the Orin off cleanly when the UPS pack stays below 11.0 V while discharging for 30 s | ```bash @@ -36,6 +36,7 @@ yukart-obd-poller --elm ADDR --once # on the Orin, service stopped: adapt # supported PIDs and one value each yukart-gnss --once # the next fix as JSON, no broker yukart-imu --once # find the WT61, 3 s of readings as JSON, no broker +yukart-imu --calibrate SESSION_DIR [--write] # measure the mounting from checklist steps 27–30 yukart-orin-status --once # on an Orin: one reading as JSON, no broker yukart-recorder ctl status # on the Orin yukart-recorder ctl start "cold start" # manual session, ends only on stop diff --git a/deploy/yukart-imu.service b/deploy/yukart-imu.service index 764718d..8967063 100644 --- a/deploy/yukart-imu.service +++ b/deploy/yukart-imu.service @@ -10,7 +10,10 @@ StartLimitIntervalSec=0 User=mikkeli # USB serial adapters belong to dialout: the one right it needs, no sudo. SupplementaryGroups=dialout -# Mounting: add --forward AXIS --right AXIS once the module has its place in the car. +# /var/lib/yukart/imu, owned by the service user: the measured mounting (mount.json), +# written by `yukart-imu --calibrate SESSION --write`. Until one exists, the defaults +# (module flat, Y forward) or --forward/--right apply. +StateDirectory=yukart/imu ExecStart=/usr/local/bin/yukart-imu Restart=always RestartSec=2 diff --git a/src/imu/calibrate.rs b/src/imu/calibrate.rs new file mode 100644 index 0000000..9966728 --- /dev/null +++ b/src/imu/calibrate.rs @@ -0,0 +1,230 @@ +//! The mounting, measured from a recorded session: `imu --calibrate SESSION_DIR`. +//! +//! Uses the raw samples (`imu.ndjson`, module axes) inside the checklist's marked +//! stretches (`events.ndjson`, contracts' `checklists/z27ag-captures.json`, section +//! "IMU calibration"): +//! +//! - **Down** from the two level stretches, parked facing one way ("imu level A") and +//! turned round ("imu level B"). Averaged, a slope's tilt cancels, since turning +//! round reverses it; the difference between them is reported as the slope. +//! One stretch alone still works, slope included. +//! - **Forward** from straight-line pull-aways ("imu pull away", forward) and stops +//! ("imu brake", backward): the force's horizontal part, signed by which stretch it +//! is in, summed. Without them, forward is the `--forward` axis laid flat, as before. +//! +//! The result is the car's forward and down in module axes, written to the mount file +//! the service loads (`--mount-file`). + +use std::path::Path; + +use serde_json::{json, Value}; + +use crate::frame::{dot, norm, unit, Mount}; + +/// One sample: receive time (s, realtime), specific force (g), rate (deg/s), module axes. +#[derive(Debug, Clone, Copy)] +pub struct Sample { + pub t: f64, + pub accel: [f64; 3], + pub gyro: [f64; 3], +} + +/// A marked stretch: its label (the mark text without the trailing start/end) and times. +#[derive(Debug, Clone)] +pub struct Stretch { + pub label: String, + pub t0: f64, + pub t1: f64, +} + +fn vec3(v: &Value) -> Option<[f64; 3]> { + let a = v.as_array()?; + Some([a.first()?.as_f64()?, a.get(1)?.as_f64()?, a.get(2)?.as_f64()?]) +} + +pub fn load_samples(dir: &Path) -> Result, String> { + let text = std::fs::read_to_string(dir.join("imu.ndjson")).map_err(|e| format!("imu.ndjson: {e}"))?; + Ok(text + .lines() + .filter_map(|l| serde_json::from_str::(l).ok()) + .filter_map(|v| { + let m = &v["msg"]; + Some(Sample { t: v["rx"].as_f64()?, accel: vec3(&m["accel"])?, gyro: vec3(&m["gyro"])? }) + }) + .collect()) +} + +/// "start"/"end" mark pairs, as the app's checklist writes them ("29 imu pull away #2 start"). +pub fn load_stretches(dir: &Path) -> Result, String> { + let text = std::fs::read_to_string(dir.join("events.ndjson")).map_err(|e| format!("events.ndjson: {e}"))?; + let mut open: Vec<(String, f64)> = Vec::new(); + let mut out = Vec::new(); + for v in text.lines().filter_map(|l| serde_json::from_str::(l).ok()) { + if v["kind"] != "mark" { + continue; + } + let (Some(text), Some(t)) = (v["text"].as_str(), v["rt"].as_f64()) else { continue }; + if let Some(label) = text.strip_suffix(" start") { + open.push((label.to_string(), t)); + } else if let Some(label) = text.strip_suffix(" end") { + if let Some(i) = open.iter().position(|(l, _)| l == label) { + let (_, t0) = open.remove(i); + out.push(Stretch { label: label.to_string(), t0, t1: t }); + } + } else if let Some(label) = text.strip_suffix(" abort") { + open.retain(|(l, _)| l != label); + } + } + Ok(out) +} + +fn within<'a>(samples: &'a [Sample], st: &Stretch, trim: f64) -> impl Iterator { + let (a, b) = (st.t0 + trim, st.t1 - trim.min(0.5)); + samples.iter().filter(move |s| s.t >= a && s.t <= b) +} + +fn mean(v: &[[f64; 3]]) -> Option<[f64; 3]> { + if v.is_empty() { + return None; + } + let n = v.len() as f64; + Some([0, 1, 2].map(|i| v.iter().map(|x| x[i]).sum::() / n)) +} + +fn angle_deg(a: [f64; 3], b: [f64; 3]) -> f64 { + (dot(unit(a), unit(b)).clamp(-1.0, 1.0)).acos().to_degrees() +} + +pub struct Calibration { + pub mount: Mount, + pub report: Value, +} + +/// `fallback_forward`: the configured forward axis, used when no driving was captured. +pub fn calibrate(samples: &[Sample], stretches: &[Stretch], fallback_forward: [f64; 3]) -> Result { + let has = |s: &Stretch, k: &str| s.label.contains(k); + + // Down: the mean force while still is "up"; down is its opposite. + let mut ups = Vec::new(); + let mut level = Vec::new(); + for key in ["imu level A", "imu level B"] { + let Some(st) = stretches.iter().rfind(|s| has(s, key)) else { continue }; + let still: Vec<[f64; 3]> = within(samples, st, 1.0).map(|s| s.accel).collect(); + let rate: Vec = within(samples, st, 1.0).map(|s| norm(s.gyro)).collect(); + let Some(m) = mean(&still) else { continue }; + let max_rate = rate.iter().cloned().fold(0.0, f64::max); + level.push(json!({ "stretch": st.label, "samples": still.len(), "g": (norm(m) * 1000.0).round() / 1000.0, + "max_rate_dps": (max_rate * 10.0).round() / 10.0 })); + if max_rate > 5.0 { + return Err(format!("'{}': turning at up to {max_rate:.1} deg/s; the car must be still", st.label)); + } + ups.push(unit(m)); + } + let up = match ups.as_slice() { + [] => return Err("no 'imu level A' or 'imu level B' stretch with samples".into()), + [a] => *a, + [a, b, ..] => unit([a[0] + b[0], a[1] + b[1], a[2] + b[2]]), + }; + let slope = (ups.len() >= 2).then(|| angle_deg(ups[0], ups[1]) / 2.0); + let down = up.map(|x| -x); + + // Forward: horizontal force in pull-aways (+) and stops (-). + let mut sum = [0.0; 3]; + let mut used = 0usize; + let mut dirs = Vec::new(); + for st in stretches { + let sign = if has(st, "imu pull away") { + 1.0 + } else if has(st, "imu brake") { + -1.0 + } else { + continue; + }; + for s in within(samples, st, 0.0) { + let h = [0, 1, 2].map(|i| s.accel[i] - dot(s.accel, up) * up[i]); + if norm(h) < 0.08 { + continue; // too gentle to say anything + } + let h = h.map(|x| x * sign); + sum = [sum[0] + h[0], sum[1] + h[1], sum[2] + h[2]]; + dirs.push(h); + used += 1; + } + } + let (forward, forward_from) = if used >= 20 { + (sum, format!("{used} samples of pull-aways and stops")) + } else { + (fallback_forward, format!("the --forward axis, laid flat ({used} usable driving samples, need 20)")) + }; + let mount = Mount::from_vectors(forward, down).ok_or("forward and down came out parallel")?; + let f = mount.rows()[0]; + let spread = if dirs.is_empty() { + None + } else { + Some(dirs.iter().map(|d| angle_deg(*d, f)).sum::() / dirs.len() as f64) + }; + let r3 = |v: [f64; 3]| v.map(|x| (x * 10000.0).round() / 10000.0); + let rows = mount.rows(); + Ok(Calibration { + mount, + report: json!({ + "forward": r3(rows[0]), "right": r3(rows[1]), "down": r3(rows[2]), + "level": level, + "slope_deg": slope.map(|x| (x * 10.0).round() / 10.0), + "forward_from": forward_from, + "forward_spread_deg": spread.map(|x| (x * 10.0).round() / 10.0), + }), + }) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn st(label: &str, t0: f64, t1: f64) -> Stretch { + Stretch { label: label.into(), t0, t1 } + } + + /// Samples of one constant force over [t0, t1). + fn steady(t0: f64, t1: f64, accel: [f64; 3]) -> Vec { + (0..((t1 - t0) * 100.0) as usize).map(|i| Sample { t: t0 + i as f64 / 100.0, accel, gyro: [0.0; 3] }).collect() + } + + #[test] + fn slope_cancels_and_forward_is_found() { + // Module upright on its edge: module +Z is the car's forward, module +X is up. + // Parked on a 4° slope, nose up; then turned round, nose down. + let (s4, c4) = (4f64.to_radians().sin(), 4f64.to_radians().cos()); + let mut samples = steady(0.0, 10.0, [c4, 0.0, s4]); // up mostly +X, leaning onto forward (+Z) + samples.extend(steady(20.0, 30.0, [c4, 0.0, -s4])); + // Pulling away (force forward, +Z) and braking (force backward, -Z), level road. + samples.extend(steady(40.0, 43.0, [1.0, 0.0, 0.2])); + samples.extend(steady(50.0, 53.0, [1.0, 0.0, -0.3])); + let stretches = vec![ + st("27 imu level A", 0.0, 10.0), + st("28 imu level B", 20.0, 30.0), + st("29 imu pull away #1", 40.0, 43.0), + st("30 imu brake #1", 50.0, 53.0), + ]; + let r = calibrate(&samples, &stretches, [0.0, 1.0, 0.0]).unwrap(); + let rows = r.mount.rows(); + let close = |a: [f64; 3], b: [f64; 3]| (0..3).all(|i| (a[i] - b[i]).abs() < 1e-6); + assert!(close(rows[2], [-1.0, 0.0, 0.0]), "down {:?}", rows[2]); + assert!(close(rows[0], [0.0, 0.0, 1.0]), "forward {:?}", rows[0]); + assert!((r.report["slope_deg"].as_f64().unwrap() - 4.0).abs() < 0.05, "{}", r.report); + // And the car frame comes out level and still for a level, still module reading. + let car = r.mount.to_car([1.0, 0.0, 0.0]); + assert!(close(car, [0.0, 0.0, -1.0]), "{car:?}"); + } + + #[test] + fn without_driving_forward_falls_back_and_motion_is_refused() { + let samples = steady(0.0, 10.0, [0.0, 0.0, 1.0]); + let r = calibrate(&samples, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0]).unwrap(); + assert!(r.report["forward_from"].as_str().unwrap().starts_with("the --forward axis")); + let mut moving = steady(0.0, 10.0, [0.0, 0.0, 1.0]); + moving.iter_mut().for_each(|s| s.gyro = [0.0, 0.0, 20.0]); + assert!(calibrate(&moving, &[st("27 imu level A", 0.0, 10.0)], [0.0, 1.0, 0.0]).is_err()); + assert!(calibrate(&samples, &[], [0.0, 1.0, 0.0]).is_err()); + } +} diff --git a/src/imu/frame.rs b/src/imu/frame.rs index 63c0773..5f3c1b5 100644 --- a/src/imu/frame.rs +++ b/src/imu/frame.rs @@ -31,25 +31,30 @@ impl Axis { Some(Axis { index, sign }) } - fn unit(self) -> [f64; 3] { + pub fn unit(self) -> [f64; 3] { let mut u = [0.0; 3]; u[self.index] = self.sign; u } } -fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] { +pub fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] { [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 dot(a: [f64; 3], b: [f64; 3]) -> f64 { +pub fn dot(a: [f64; 3], b: [f64; 3]) -> f64 { a[0] * b[0] + a[1] * b[1] + a[2] * b[2] } -fn norm(a: [f64; 3]) -> f64 { +pub fn norm(a: [f64; 3]) -> f64 { dot(a, a).sqrt() } +pub fn unit(a: [f64; 3]) -> [f64; 3] { + let n = norm(a); + a.map(|x| x / n) +} + /// Module axes → car axes: rows forward, right, down (= forward × right). #[derive(Debug, Clone, Copy)] pub struct Mount([[f64; 3]; 3]); @@ -64,6 +69,25 @@ impl Mount { Some(Mount([f, r, cross(f, r)])) } + /// From a calibration: the car's forward and down directions in module axes. + /// Forward is made exactly perpendicular to down; right completes the frame. + /// None when they are (nearly) parallel or not finite. + pub fn from_vectors(forward: [f64; 3], down: [f64; 3]) -> Option { + let d = unit(down); + let f = [0, 1, 2].map(|i| forward[i] - dot(forward, d) * d[i]); + if norm(f).is_nan() || norm(f) <= 1e-3 || d.iter().any(|x| !x.is_finite()) { + return None; + } + let f = unit(f); + // Car frame x forward, y right, z down: y = z × x. + Some(Mount([f, cross(d, f), d])) + } + + /// Rows: forward, right, down, each in module axes. + pub fn rows(&self) -> [[f64; 3]; 3] { + self.0 + } + pub fn to_car(self, v: [f64; 3]) -> [f64; 3] { [dot(self.0[0], v), dot(self.0[1], v), dot(self.0[2], v)] } @@ -165,6 +189,20 @@ mod tests { assert_eq!(flat().to_car([0.0, 0.0, 1.0]), [0.0, 0.0, -1.0]); } + #[test] + fn from_vectors_matches_the_axis_form() { + // Flat mount: forward +Y, down -Z. + let m = Mount::from_vectors([0.0, 1.0, 0.0], [0.0, 0.0, -1.0]).unwrap(); + for v in [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] { + let (a, b) = (m.to_car(v), flat().to_car(v)); + assert!((0..3).all(|i| close(a[i], b[i])), "{a:?} vs {b:?}"); + } + // A slightly tilted forward is squared up against down. + let m = Mount::from_vectors([0.0, 1.0, 0.1], [0.0, 0.0, -1.0]).unwrap(); + assert!(close(m.rows()[0][2], 0.0)); + assert!(Mount::from_vectors([0.0, 0.0, 1.0], [0.0, 0.0, -1.0]).is_none()); + } + #[test] fn level_and_still_reads_zero() { let mut a = Attitude::new(flat()); diff --git a/src/imu/main.rs b/src/imu/main.rs index 5d0f900..8e5495a 100644 --- a/src/imu/main.rs +++ b/src/imu/main.rs @@ -8,10 +8,20 @@ //! every 40 ms: roll, pitch, gravity-free longitudinal and lateral g, yaw rate. //! Names and units from `signals.json` (source `imu`). //! +//! Every sample also goes out raw (module axes, uncalibrated) on `yukart/raw/imu`, which +//! the recorder keeps in each session (`imu.ndjson`), so a later calibration can +//! reprocess earlier drives. +//! +//! Calibration: `--calibrate SESSION_DIR` measures the mounting from a recorded session's +//! marked stretches (calibrate.rs) and, with `--write`, saves it to the mount file. The +//! service uses the mount file when there is one, instead of `--forward`/`--right`, and +//! picks up a new one within seconds. +//! //! Standalone: it needs the broker and nothing else, and never writes to the module //! (its rate and speed are set once, by hand: orin-nano-env-setup `docs/BOM.md`). //! State on `yukart/system/imu`, with a last will. +mod calibrate; mod frame; mod serial; mod wt61; @@ -37,7 +47,10 @@ usage: imu [options] --forward AXIS module axis pointing forward: +x, -y, ... (+y) --right AXIS module axis pointing right (+x) --every-ms N publish at most this often (40) + --mount-file PATH measured mounting; overrides --forward/--right (/var/lib/yukart/imu/mount.json) --once find the module, print 3 s of readings as JSON, exit (no broker) + --calibrate DIR measure the mounting from a recorded session, print it, exit + --write with --calibrate: also save it to the mount file The defaults are the datasheet's drawing: module flat, face up, Y forward, X right."; const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json")); @@ -85,8 +98,27 @@ struct Opts { device: Option, baud: Option, mount: Mount, + forward: Axis, every: Duration, once: bool, + mount_file: String, + calibrate: Option, + write: bool, +} + +/// The mount file's mounting and its modification time, when there is a valid one. +fn mount_file(path: &str) -> Option<(Mount, std::time::SystemTime)> { + let meta = std::fs::metadata(path).ok()?; + let v: Value = serde_json::from_str(&std::fs::read_to_string(path).ok()?).ok()?; + let vec = |k: &str| -> Option<[f64; 3]> { + let a = v[k].as_array()?; + Some([a.first()?.as_f64()?, a.get(1)?.as_f64()?, a.get(2)?.as_f64()?]) + }; + Some((Mount::from_vectors(vec("forward")?, vec("down")?)?, meta.modified().ok()?)) +} + +fn modified(path: &str) -> Option { + std::fs::metadata(path).and_then(|m| m.modified()).ok() } fn parse() -> Result { @@ -96,13 +128,17 @@ fn parse() -> Result { device: None, baud: None, mount: Mount::new(forward, right).unwrap(), + forward, every: Duration::from_millis(40), once: false, + mount_file: "/var/lib/yukart/imu/mount.json".into(), + calibrate: None, + write: false, }; let mut args = std::env::args().skip(1); while let Some(a) = args.next() { - if a == "--once" { - o.once = true; + if a == "--once" || a == "--write" { + if a == "--once" { o.once = true } else { o.write = true } continue; } let v = args.next().ok_or(format!("{a} needs a value"))?; @@ -113,20 +149,32 @@ fn parse() -> Result { "--baud" => o.baud = Some(v.parse().map_err(|_| format!("--baud: not a number: {v}"))?), "--forward" => forward = axis()?, "--right" => right = axis()?, + "--mount-file" => o.mount_file = v.clone(), + "--calibrate" => o.calibrate = Some(v.clone()), "--every-ms" => o.every = Duration::from_millis(v.parse().map_err(|_| format!("--every-ms: not a number: {v}"))?), _ => return Err(format!("unknown option {a}")), } } o.mount = Mount::new(forward, right).ok_or("--forward and --right must be different axes")?; + o.forward = forward; Ok(o) } -/// Reads one port until it fails or goes quiet; hands each new attitude to `out`, and -/// packet counts to `stats`. Returns why it stopped. -fn run(port: &serial::Port, mount: Mount, stop: &dyn Fn() -> bool, mut out: impl FnMut(Motion, &Counts)) -> String { +/// One raw sample, module axes: the accelerometer packet with the latest rate and angle. +#[derive(Clone, Copy)] +struct Raw { + accel: [f64; 3], + gyro: [f64; 3], + angle: [f64; 3], +} + +/// Reads one port until it fails, goes quiet or `stop` says so; hands each new attitude, +/// with its raw sample and the packet counts, to `out`. Returns why it stopped. +fn run(port: &serial::Port, mount: Mount, stop: &mut dyn FnMut() -> bool, mut out: impl FnMut(Motion, Raw, &Counts)) -> String { let (mut parser, mut packets, mut buf) = (Parser::default(), Vec::new(), [0u8; 1024]); let mut att = Attitude::new(mount); let mut counts = Counts::default(); + let (mut gyro, mut angle) = ([0.0; 3], [0.0; 3]); let mut last_chunk = Instant::now(); let mut heard = Instant::now(); while !stop() { @@ -151,21 +199,63 @@ fn run(port: &serial::Port, mount: Mount, stop: &dyn Fn() -> bool, mut out: impl match p { Packet::Gyro(g) => { counts.gyro += 1; + gyro = g; att.gyro(g); } Packet::Accel(a) => { counts.accel += 1; if let Some(m) = att.accel(a, dt) { - out(m, &counts); + out(m, Raw { accel: a, gyro, angle }, &counts); } } - Packet::Angle(_) => counts.angle += 1, + Packet::Angle(v) => { + counts.angle += 1; + angle = v; + } } } } "stopped".into() } +/// `--calibrate DIR [--write]`: measure the mounting from a recorded session. +fn calibrate_session(o: &Opts, dir: &str) -> ExitCode { + let dir = std::path::Path::new(dir); + let result = calibrate::load_samples(dir) + .and_then(|s| Ok((s, calibrate::load_stretches(dir)?))) + .and_then(|(s, st)| calibrate::calibrate(&s, &st, o.forward.unit())); + let c = match result { + Ok(c) => c, + Err(e) => { + eprintln!("imu: calibration from {}: {e}", dir.display()); + return ExitCode::FAILURE; + } + }; + // Where each module axis points in the car (x forward, y right, z down): a sanity check by eye. + for (name, v) in [("X", [1.0, 0.0, 0.0]), ("Y", [0.0, 1.0, 0.0]), ("Z", [0.0, 0.0, 1.0])] { + let car = c.mount.to_car(v).map(|x| (x * 100.0).round() / 100.0); + eprintln!("imu: module {name} points {car:?} in the car (forward, right, down)"); + } + let mut report = c.report; + report["session"] = json!(dir.file_name().map(|n| n.to_string_lossy().to_string())); + report["made"] = json!(now_us() / 1_000_000); + println!("{}", serde_json::to_string_pretty(&report).unwrap()); + if o.write { + // Write-temp-then-rename: the service never reads half a file. + let tmp = format!("{}.tmp", o.mount_file); + let res = std::fs::write(&tmp, serde_json::to_string_pretty(&report).unwrap() + "\n") + .and_then(|_| std::fs::rename(&tmp, &o.mount_file)); + match res { + Ok(()) => eprintln!("imu: written to {}; the service picks it up within seconds", o.mount_file), + Err(e) => { + eprintln!("imu: {}: {e}", o.mount_file); + return ExitCode::FAILURE; + } + } + } + ExitCode::SUCCESS +} + #[derive(Default, Clone, Copy)] struct Counts { accel: u64, @@ -189,7 +279,7 @@ fn once(o: &Opts) -> ExitCode { let t = Instant::now(); let mut last = None; let mut counts = Counts::default(); - run(&port, o.mount, &|| t.elapsed() > Duration::from_secs(3), |m, c| { + run(&port, o.mount, &mut || t.elapsed() > Duration::from_secs(3), |m, _, c| { last = Some(m); counts = *c; }); @@ -218,6 +308,9 @@ fn main() -> ExitCode { if o.once { return once(&o); } + if let Some(dir) = &o.calibrate { + return calibrate_session(&o, dir); + } let units = units(); let Some((host, port)) = o.broker.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::().ok()?))) @@ -285,13 +378,34 @@ fn main() -> ExitCode { }; eprintln!("imu: WT61 on {path} at {baud} baud"); last_detail.clear(); + // The measured mounting when there is one; reread whenever the file changes. + let (mount, from) = match mount_file(&o.mount_file) { + Some((m, _)) => (m, o.mount_file.as_str()), + None => (o.mount, "--forward/--right"), + }; + eprintln!("imu: mounting from {from}"); + let seen = modified(&o.mount_file); + let mut checked = Instant::now(); + let mut mount_changed = || { + if checked.elapsed() < Duration::from_secs(2) { + return false; + } + checked = Instant::now(); + modified(&o.mount_file) != seen + }; let mut last_pub = Instant::now() - o.every; let mut window = (Instant::now(), Counts::default()); - let why = run(&port, o.mount, &|| false, |m, c| { + let why = run(&port, mount, &mut mount_changed, |m, raw, c| { + // Every sample, raw, for the recorder: not retained, never throttled. + if connected.load(Ordering::Relaxed) { + let r = |v: [f64; 3]| v.map(|x| (x * 10000.0).round() / 10000.0); + let msg = json!({ "mono": mono_ns(), "t": now_us(), "accel": r(raw.accel), "gyro": r(raw.gyro), "angle": r(raw.angle) }); + let _ = client.try_publish("yukart/raw/imu", QoS::AtMostOnce, false, msg.to_string()); + } if window.0.elapsed() >= Duration::from_secs(5) { let secs = window.0.elapsed().as_secs_f64(); state(json!({ "state": "ready", "detail": { - "device": path, "baud": baud, + "device": path, "baud": baud, "mounting": from, "rate_hz": ((c.accel - window.1.accel) as f64 / secs).round(), "bad_checksums": c.bad } })); window = (Instant::now(), *c); diff --git a/src/recorder/main.rs b/src/recorder/main.rs index 376615c..0916e6b 100644 --- a/src/recorder/main.rs +++ b/src/recorder/main.rs @@ -2,7 +2,7 @@ //! //! Reads SocketCAN and gpsd directly, not through the broker: capture is tier 1 and //! must not depend on anything that can be late or down (decision 25). Envelopes on -//! `yukart/vehicle/*` (OBD-II replies, later decoded signals) come from the broker +//! `yukart/vehicle/*` (decoded and OBD-II signals) and `yukart/raw/imu` come from the broker //! and are recorded when it is there. //! //! Sessions start when CAN frames arrive (auto) or on `ctl start` (manual), and an @@ -296,6 +296,11 @@ impl Recorder { let _ = s.vehicle(rx, &signal, &payload, retained); } } + Event::Imu(rx, payload) => { + if let Some(s) = &mut self.session { + let _ = s.imu(rx, &payload); + } + } Event::BusLink(up, detail) => { eprintln!("bus: {detail}"); self.bus_link = (up, detail.clone()); diff --git a/src/recorder/session.rs b/src/recorder/session.rs index 1e13a77..446dd72 100644 --- a/src/recorder/session.rs +++ b/src/recorder/session.rs @@ -6,7 +6,8 @@ //! can-0001.log.zst candump -L text, closed chunks compressed //! can-0002.log the open chunk, plain text, fdatasync'd every second //! gnss.ndjson every gpsd report, with the local receive time -//! vehicle.ndjson every envelope on yukart/vehicle/* (OBD-II replies now, decoded signals later) +//! vehicle.ndjson every envelope on yukart/vehicle/* (decoded CAN signals, OBD-II references) +//! imu.ndjson every raw IMU sample on yukart/raw/imu (module axes), with the local receive time //! events.ndjson marks, clock steps, link changes, drops //! ``` //! @@ -44,6 +45,7 @@ pub struct Session { can: BufWriter, gnss: BufWriter, vehicle: BufWriter, + imu: BufWriter, events: BufWriter, line: String, pub frames: u64, @@ -121,6 +123,7 @@ impl Session { can: append(&dir.join(chunk_name(1)))?, gnss: append(&dir.join("gnss.ndjson"))?, vehicle: append(&dir.join("vehicle.ndjson"))?, + imu: append(&dir.join("imu.ndjson"))?, events: append(&dir.join("events.ndjson"))?, dir, name, trigger, meta, iface: iface.to_string(), @@ -170,6 +173,13 @@ impl Session { writeln!(self.vehicle, "{v}") } + /// One raw IMU sample (module axes, not calibrated): kept so a later calibration can + /// reprocess this session. + pub fn imu(&mut self, rx: f64, payload: &str) -> io::Result<()> { + let msg = serde_json::from_str::(payload).unwrap_or_else(|_| Value::String(payload.to_string())); + writeln!(self.imu, "{}", json!({ "rx": rx, "msg": msg })) + } + pub fn event(&mut self, kind: &str, mut extra: Value) -> io::Result<()> { if !extra.is_object() { extra = json!({}); @@ -196,6 +206,7 @@ impl Session { sync(&mut self.can)?; sync(&mut self.gnss)?; sync(&mut self.vehicle)?; + sync(&mut self.imu)?; sync(&mut self.events) } diff --git a/src/recorder/sources.rs b/src/recorder/sources.rs index ac33246..5fd0c36 100644 --- a/src/recorder/sources.rs +++ b/src/recorder/sources.rs @@ -23,6 +23,8 @@ pub enum Event { GnssLink(bool, String), /// An envelope on yukart/vehicle/: receive time, signal, payload, retained. Vehicle(f64, String, String, bool), + /// One raw IMU sample on yukart/raw/imu: receive time, payload. + Imu(f64, String), BusLink(bool, String), Ctl(String, mpsc::Sender), } @@ -149,7 +151,12 @@ pub fn spawn_bus(addr: String, tx: SyncSender, stop: Arc) { Ok(rumqttc::Event::Incoming(rumqttc::Packet::ConnAck(_))) => { up = true; let _ = client.try_subscribe("yukart/vehicle/+", rumqttc::QoS::AtMostOnce); - let _ = tx.send(Event::BusLink(true, format!("{addr}: subscribed to yukart/vehicle/+"))); + let _ = client.try_subscribe("yukart/raw/imu", rumqttc::QoS::AtMostOnce); + let _ = tx.send(Event::BusLink(true, format!("{addr}: subscribed to yukart/vehicle/+, yukart/raw/imu"))); + } + Ok(rumqttc::Event::Incoming(rumqttc::Packet::Publish(p))) if p.topic == "yukart/raw/imu" => { + let payload = String::from_utf8_lossy(&p.payload).into_owned(); + let _ = tx.try_send(Event::Imu(clock::realtime(), payload)); } Ok(rumqttc::Event::Incoming(rumqttc::Packet::Publish(p))) => { let signal = p.topic.rsplit('/').next().unwrap_or_default().to_string();