imu: the WT61 on any USB-UART adapter, in the car's frame, on yukart/imu/*

Finds the module itself (every non-modem adapter, 115200 then 9600), so the
adapter can be swapped. Mounting by --forward/--right; attitude from a
complementary filter on gravity (gyro through motion, accelerometer near 1 g);
gravity removed from acceleration; yaw rate left-positive. Never writes to
the module. dialout via SupplementaryGroups.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz

Cargo.toml

@@ -2,7 +2,7 @@
 name = "svc-core"
 version = "0.1.0"
 edition = "2021"
-description = "Acquisition and processing cluster: recorder, decoder, gnss, orin-status and obd-poller now; can-bridge later"
+description = "Acquisition and processing cluster: recorder, decoder, gnss, imu, orin-status and obd-poller now; can-bridge later"
 publish = false
 
 [[bin]]
@@ -25,6 +25,10 @@ path = "src/decoder/main.rs"
 name = "gnss"
 path = "src/gnss/main.rs"
 
+[[bin]]
+name = "imu"
+path = "src/imu/main.rs"
+
 [[bin]]
 name = "bus-bench"
 path = "src/bus-bench/main.rs"

README.md

@@ -10,7 +10,7 @@ This file serves as the minimalist portal to other docs
 ## What this repo is about
 
 The acquisition and processing cluster on the Orin, developed and deployed as one
-unit: `recorder`, `decoder`, `gnss`, `orin-status` and `obd-poller` now; `can-bridge` as it is
+unit: `recorder`, `decoder`, `gnss`, `imu`, `orin-status` and `obd-poller` now; `can-bridge` as it is
 needed.
 
 | Binary | Installed as | Does |
@@ -20,6 +20,7 @@ needed.
 | `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 |
 | `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/*`) |
 
 ```bash
@@ -32,6 +33,7 @@ zstdcat can-*.log.zst | yukart-decoder --replay   # what it would publish for a
 yukart-obd-poller --elm ADDR --once        # on the Orin, service stopped: adapter, protocol,
                                            # 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-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

deploy/yukart-imu.service

@@ -0,0 +1,19 @@
+[Unit]
+Description=Yukart imu: the WT61 on a USB-UART adapter, as the car's motion on the bus
+Documentation=gitea:yukart/svc-core
+# Tier 2. Ordered after the broker and nothing else: it finds the adapter by itself,
+# whenever it appears.
+After=mosquitto.service
+StartLimitIntervalSec=0
+
+[Service]
+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.
+ExecStart=/usr/local/bin/yukart-imu
+Restart=always
+RestartSec=2
+
+[Install]
+WantedBy=multi-user.target

scripts/deploy-imu.sh

@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+# Build imu natively on the development Orin and install it on the car Orin as
+# yukart-imu: scripts/deploy-native.sh does the work.
+#
+#   usage: scripts/deploy-imu.sh [host]       (default: mikkeli-orin-nano)
+exec "$(dirname "$0")/deploy-native.sh" imu "$@"

src/imu/frame.rs

@@ -0,0 +1,220 @@
+//! From the module's axes to the car's, and from raw motion to what contracts' `imu_*`
+//! signals mean: roll, pitch, gravity-free acceleration and yaw rate.
+//!
+//! Car frame: x forward, y right, z down (right-handed). The mounting says which module
+//! axis points forward and which points right; down follows. Attitude comes from a
+//! complementary filter on the gravity direction: the gyro carries it through motion,
+//! the accelerometer pulls it back whenever the total force is close to 1 g. The
+//! module's own angles are not used: they assume the module's own mounting.
+
+/// A signed module axis: 0 = X, 1 = Y, 2 = Z.
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct Axis {
+    index: usize,
+    sign: f64,
+}
+
+impl Axis {
+    /// "+x", "-z", "y" (plus implied).
+    pub fn parse(s: &str) -> Option<Axis> {
+        let s = s.trim().to_ascii_lowercase();
+        let (sign, rest) = match s.strip_prefix('-') {
+            Some(r) => (-1.0, r),
+            None => (1.0, s.strip_prefix('+').unwrap_or(&s)),
+        };
+        let index = match rest {
+            "x" => 0,
+            "y" => 1,
+            "z" => 2,
+            _ => return None,
+        };
+        Some(Axis { index, sign })
+    }
+
+    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] {
+    [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 {
+    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
+}
+
+fn norm(a: [f64; 3]) -> f64 {
+    dot(a, a).sqrt()
+}
+
+/// Module axes → car axes: rows forward, right, down (= forward × right).
+#[derive(Debug, Clone, Copy)]
+pub struct Mount([[f64; 3]; 3]);
+
+impl Mount {
+    /// None when the two axes are the same line.
+    pub fn new(forward: Axis, right: Axis) -> Option<Mount> {
+        if forward.index == right.index {
+            return None;
+        }
+        let (f, r) = (forward.unit(), right.unit());
+        Some(Mount([f, r, cross(f, r)]))
+    }
+
+    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)]
+    }
+}
+
+/// One output: the `imu_*` signals.
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct Motion {
+    pub roll_deg: f64,
+    pub pitch_deg: f64,
+    pub accel_lon_g: f64,
+    pub accel_lat_g: f64,
+    pub yaw_rate_dps: f64,
+}
+
+pub struct Attitude {
+    mount: Mount,
+    /// Gravity's direction in the car frame, unit length, pointing down. None until the first sample.
+    down: Option<[f64; 3]>,
+    /// Latest angular rate, car frame, rad/s.
+    rate: [f64; 3],
+    /// Seconds for the accelerometer to pull a disturbed attitude most of the way back.
+    tau: f64,
+}
+
+impl Attitude {
+    pub fn new(mount: Mount) -> Self {
+        Attitude { mount, down: None, rate: [0.0; 3], tau: 2.0 }
+    }
+
+    pub fn gyro(&mut self, dps: [f64; 3]) {
+        self.rate = self.mount.to_car(dps).map(f64::to_radians);
+    }
+
+    /// One accelerometer sample (specific force, g, module axes), `dt` seconds after the last.
+    pub fn accel(&mut self, g: [f64; 3], dt: f64) -> Option<Motion> {
+        let f = self.mount.to_car(g);
+        let n = norm(f);
+        if n < 1e-6 {
+            return None;
+        }
+        // At rest the accelerometer reads 1 g upwards, so gravity is the opposite of f.
+        let measured = f.map(|x| -x / n);
+        let mut d = match self.down {
+            None => measured,
+            Some(d) => {
+                // A world-fixed vector seen from a rotating body turns the other way: dd/dt = -ω × d.
+                let w = cross(self.rate, d);
+                let mut p = [d[0] - w[0] * dt, d[1] - w[1] * dt, d[2] - w[2] * dt];
+                let pn = norm(p);
+                p = p.map(|x| x / pn);
+                if (n - 1.0).abs() < 0.15 {
+                    let k = (dt / self.tau).clamp(0.0, 1.0);
+                    p = [0, 1, 2].map(|i| p[i] * (1.0 - k) + measured[i] * k);
+                    let pn = norm(p);
+                    p = p.map(|x| x / pn);
+                }
+                p
+            }
+        };
+        if d.iter().any(|x| !x.is_finite()) {
+            d = measured;
+        }
+        self.down = Some(d);
+        // Kinematic acceleration = specific force + gravity (1 g along `down`).
+        let a = [f[0] + d[0], f[1] + d[1], f[2] + d[2]];
+        Some(Motion {
+            // Right side down: gravity leans onto +y. Nose up: gravity leans onto -x.
+            roll_deg: d[1].atan2(d[2]).to_degrees(),
+            pitch_deg: (-d[0]).atan2((d[1] * d[1] + d[2] * d[2]).sqrt()).to_degrees(),
+            accel_lon_g: a[0],
+            accel_lat_g: a[1],
+            // About the down axis, positive is clockwise from above; contracts count a left turn positive.
+            yaw_rate_dps: -self.rate[2].to_degrees(),
+        })
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    /// The datasheet's axes, module flat and face up: X right, Y forward, Z up.
+    fn flat() -> Mount {
+        Mount::new(Axis::parse("+y").unwrap(), Axis::parse("+x").unwrap()).unwrap()
+    }
+
+    fn close(a: f64, b: f64) -> bool {
+        (a - b).abs() < 1e-6
+    }
+
+    #[test]
+    fn axes() {
+        assert_eq!(Axis::parse("-Z"), Some(Axis { index: 2, sign: -1.0 }));
+        assert_eq!(Axis::parse("x"), Some(Axis { index: 0, sign: 1.0 }));
+        assert_eq!(Axis::parse("w"), None);
+        assert!(Mount::new(Axis::parse("+x").unwrap(), Axis::parse("-x").unwrap()).is_none());
+        // Flat: module up (+z) is car up, so car down is module -z.
+        assert_eq!(flat().to_car([0.0, 0.0, 1.0]), [0.0, 0.0, -1.0]);
+    }
+
+    #[test]
+    fn level_and_still_reads_zero() {
+        let mut a = Attitude::new(flat());
+        let m = a.accel([0.0, 0.0, 1.0], 0.01).unwrap();
+        assert!(close(m.roll_deg, 0.0) && close(m.pitch_deg, 0.0));
+        assert!(close(m.accel_lon_g, 0.0) && close(m.accel_lat_g, 0.0));
+    }
+
+    #[test]
+    fn tilts_have_the_contract_signs() {
+        // Nose up θ: in the car frame gravity is (-sin θ, 0, cos θ) and the accelerometer reads
+        // the opposite. The flat mount's inverse gives the module's reading.
+        let t = 10f64.to_radians();
+        let f_car = [t.sin(), 0.0, -t.cos()];
+        let module = [f_car[1], f_car[0], -f_car[2]]; // x = right, y = forward, z = up
+        let m = Attitude::new(flat()).accel(module, 0.01).unwrap();
+        assert!((m.pitch_deg - 10.0).abs() < 1e-6, "{m:?}");
+        // Right side down 5°: down leans onto +y (right).
+        let r = 5f64.to_radians();
+        let f_car = [0.0, -r.sin(), -r.cos()];
+        let module = [f_car[1], f_car[0], -f_car[2]];
+        let m = Attitude::new(flat()).accel(module, 0.01).unwrap();
+        assert!((m.roll_deg - 5.0).abs() < 1e-6, "{m:?}");
+    }
+
+    #[test]
+    fn acceleration_and_yaw_signs() {
+        let mut a = Attitude::new(flat());
+        a.accel([0.0, 0.0, 1.0], 0.01);
+        // Speeding up at 0.2 g: the force pushes forward (module +Y). A right turn's force pushes right (+X).
+        let m = a.accel([0.1, 0.2, 1.0], 0.01).unwrap();
+        assert!((m.accel_lon_g - 0.2).abs() < 0.01 && (m.accel_lat_g - 0.1).abs() < 0.01, "{m:?}");
+        // Turning left: counter-clockwise from above, about module +Z (up) by the right-hand rule.
+        a.gyro([0.0, 0.0, 30.0]);
+        let m = a.accel([0.0, 0.0, 1.0], 0.01).unwrap();
+        assert!(close(m.yaw_rate_dps, 30.0), "{m:?}");
+    }
+
+    #[test]
+    fn gyro_carries_a_roll_the_accelerometer_has_not_seen() {
+        let mut a = Attitude::new(flat());
+        a.accel([0.0, 0.0, 1.0], 0.01);
+        // Rolling right at 20°/s for 1 s: about the forward axis (module +Y), right side down.
+        a.gyro([0.0, 20.0, 0.0]);
+        let mut m = None;
+        for _ in 0..100 {
+            m = a.accel([0.0, 0.0, 1.0], 0.01); // accelerometer still says level: it disagrees
+        }
+        let roll = m.unwrap().roll_deg;
+        // The gyro's 20° wins most of it back over the 2 s accelerometer pull.
+        assert!(roll > 10.0 && roll < 20.0, "{roll}");
+    }
+}

src/imu/main.rs

@@ -0,0 +1,324 @@
+//! imu — the WT61 (plus) on a USB-UART adapter, as the car's motion on the bus.
+//!
+//! Finds the module by itself: every USB-UART adapter that is not the modem, at
+//! 115200 then 9600 baud, until one sends WT61 packets; no device name is fixed, so
+//! the adapter can be swapped. Turns acceleration and angular rate into the car's
+//! frame (`--forward` and `--right` name the module axes that point that way) and
+//! publishes contracts' `imu_*` signals on `yukart/imu/<signal>`, retained, at most
+//! every 40 ms: roll, pitch, gravity-free longitudinal and lateral g, yaw rate.
+//! Names and units from `signals.json` (source `imu`).
+//!
+//! 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 frame;
+mod serial;
+mod wt61;
+
+use std::collections::HashMap;
+use std::process::ExitCode;
+use std::sync::atomic::{AtomicBool, Ordering};
+use std::sync::Arc;
+use std::thread;
+use std::time::{Duration, Instant};
+
+use rumqttc::{Client, Event, LastWill, MqttOptions, Packet as MqttPacket, QoS};
+use serde_json::{json, Value};
+
+use frame::{Attitude, Axis, Motion, Mount};
+use wt61::{Packet, Parser};
+
+const USAGE: &str = "\
+usage: imu [options]
+  --broker HOST:PORT   MQTT broker                                   (127.0.0.1:1883)
+  --device PATH        the adapter, instead of trying each           (any)
+  --baud N             its speed, instead of trying 115200 and 9600  (any)
+  --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)
+  --once               find the module, print 3 s of readings as JSON, exit (no broker)
+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"));
+const SYSTEM_TOPIC: &str = "yukart/system/imu";
+
+fn now_us() -> i64 {
+    let d = std::time::SystemTime::now().duration_since(std::time::UNIX_EPOCH).unwrap_or_default();
+    d.as_micros() as i64
+}
+
+fn mono_ns() -> i64 {
+    let mut ts: libc::timespec = unsafe { std::mem::zeroed() };
+    unsafe { libc::clock_gettime(libc::CLOCK_MONOTONIC, &mut ts) };
+    ts.tv_sec as i64 * 1_000_000_000 + ts.tv_nsec as i64
+}
+
+/// Units of the imu signals, from the registry: never authored here (contracts guideline 1).
+fn units() -> HashMap<String, String> {
+    let v: Value = serde_json::from_str(REGISTRY).expect("contracts signals.json is valid JSON");
+    v["signals"]
+        .as_object()
+        .expect("signals.json has a signals object")
+        .iter()
+        .filter(|(_, s)| s["source"] == "imu")
+        .map(|(k, s)| (k.clone(), s["unit"].as_str().unwrap_or("").to_string()))
+        .collect()
+}
+
+fn signals(m: &Motion) -> [(&'static str, Value); 5] {
+    let r = |v: f64, places: i32| {
+        let f = 10f64.powi(places);
+        json!((v * f).round() / f)
+    };
+    [
+        ("imu_roll_deg", r(m.roll_deg, 1)),
+        ("imu_pitch_deg", r(m.pitch_deg, 1)),
+        ("imu_accel_lon_g", r(m.accel_lon_g, 3)),
+        ("imu_accel_lat_g", r(m.accel_lat_g, 3)),
+        ("imu_yaw_rate_dps", r(m.yaw_rate_dps, 1)),
+    ]
+}
+
+struct Opts {
+    broker: String,
+    device: Option<String>,
+    baud: Option<u32>,
+    mount: Mount,
+    every: Duration,
+    once: bool,
+}
+
+fn parse() -> Result<Opts, String> {
+    let (mut forward, mut right) = (Axis::parse("+y").unwrap(), Axis::parse("+x").unwrap());
+    let mut o = Opts {
+        broker: "127.0.0.1:1883".into(),
+        device: None,
+        baud: None,
+        mount: Mount::new(forward, right).unwrap(),
+        every: Duration::from_millis(40),
+        once: false,
+    };
+    let mut args = std::env::args().skip(1);
+    while let Some(a) = args.next() {
+        if a == "--once" {
+            o.once = true;
+            continue;
+        }
+        let v = args.next().ok_or(format!("{a} needs a value"))?;
+        let axis = || Axis::parse(&v).ok_or(format!("{a}: not an axis: {v} (+x, -y, z, ...)"));
+        match a.as_str() {
+            "--broker" => o.broker = v.clone(),
+            "--device" => o.device = Some(v.clone()),
+            "--baud" => o.baud = Some(v.parse().map_err(|_| format!("--baud: not a number: {v}"))?),
+            "--forward" => forward = axis()?,
+            "--right" => right = axis()?,
+            "--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")?;
+    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 {
+    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 last_chunk = Instant::now();
+    let mut heard = Instant::now();
+    while !stop() {
+        let n = match port.read(&mut buf) {
+            Ok(n) => n,
+            Err(e) => return format!("read: {e}"),
+        };
+        if n == 0 {
+            if heard.elapsed() > Duration::from_secs(3) {
+                return "no packets for 3 s".into();
+            }
+            continue;
+        }
+        parser.push(&buf[..n], &mut packets);
+        counts.bad = parser.bad;
+        // The adapter delivers in chunks: spread the chunk's time over its accelerometer packets.
+        let accels = packets.iter().filter(|p| matches!(p, Packet::Accel(_))).count().max(1);
+        let dt = (last_chunk.elapsed().as_secs_f64() / accels as f64).clamp(0.001, 0.1);
+        last_chunk = Instant::now();
+        for p in packets.drain(..) {
+            heard = Instant::now();
+            match p {
+                Packet::Gyro(g) => {
+                    counts.gyro += 1;
+                    att.gyro(g);
+                }
+                Packet::Accel(a) => {
+                    counts.accel += 1;
+                    if let Some(m) = att.accel(a, dt) {
+                        out(m, &counts);
+                    }
+                }
+                Packet::Angle(_) => counts.angle += 1,
+            }
+        }
+    }
+    "stopped".into()
+}
+
+#[derive(Default, Clone, Copy)]
+struct Counts {
+    accel: u64,
+    gyro: u64,
+    angle: u64,
+    bad: u64,
+}
+
+fn once(o: &Opts) -> ExitCode {
+    let Some((path, baud)) = serial::find(o.device.as_deref(), o.baud) else {
+        eprintln!("imu: no WT61 on {:?} at {:?}", o.device.clone().map(|d| vec![d]).unwrap_or_else(serial::adapters), serial::BAUDS);
+        return ExitCode::FAILURE;
+    };
+    let port = match serial::Port::open(&path, baud) {
+        Ok(p) => p,
+        Err(e) => {
+            eprintln!("imu: {path}: {e}");
+            return ExitCode::FAILURE;
+        }
+    };
+    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| {
+        last = Some(m);
+        counts = *c;
+    });
+    let secs = t.elapsed().as_secs_f64();
+    println!(
+        "{}",
+        serde_json::to_string_pretty(&json!({
+            "device": path, "baud": baud,
+            "rate_hz": { "accel": (counts.accel as f64 / secs).round(), "gyro": (counts.gyro as f64 / secs).round(), "angle": (counts.angle as f64 / secs).round() },
+            "bad_checksums": counts.bad,
+            "signals": last.map(|m| signals(&m).into_iter().map(|(k, v)| (k.to_string(), v)).collect::<serde_json::Map<_, _>>()),
+        }))
+        .unwrap()
+    );
+    ExitCode::SUCCESS
+}
+
+fn main() -> ExitCode {
+    let o = match parse() {
+        Ok(o) => o,
+        Err(e) => {
+            eprintln!("imu: {e}\n{USAGE}");
+            return ExitCode::from(2);
+        }
+    };
+    if o.once {
+        return once(&o);
+    }
+    let units = units();
+
+    let Some((host, port)) = o.broker.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::<u16>().ok()?)))
+    else {
+        eprintln!("imu: --broker must be HOST:PORT\n{USAGE}");
+        return ExitCode::from(2);
+    };
+    let mut opts = MqttOptions::new("imu", host, port);
+    opts.set_keep_alive(Duration::from_secs(5));
+    opts.set_last_will(LastWill::new(
+        SYSTEM_TOPIC,
+        json!({ "state": "stopping", "detail": "connection lost" }).to_string(),
+        QoS::AtLeastOnce,
+        true,
+    ));
+    let (client, mut connection) = Client::new(opts, 512);
+    let connected = Arc::new(AtomicBool::new(false));
+    {
+        let connected = connected.clone();
+        thread::spawn(move || {
+            for ev in connection.iter() {
+                match ev {
+                    Ok(Event::Incoming(MqttPacket::ConnAck(_))) => {
+                        eprintln!("imu: connected to the broker");
+                        connected.store(true, Ordering::Relaxed);
+                    }
+                    Ok(_) => {}
+                    Err(e) => {
+                        if connected.swap(false, Ordering::Relaxed) {
+                            eprintln!("imu: broker: {e}; retrying");
+                        }
+                        thread::sleep(Duration::from_secs(1));
+                    }
+                }
+            }
+        });
+    }
+    let state = |s: Value| {
+        if connected.load(Ordering::Relaxed) {
+            let _ = client.try_publish(SYSTEM_TOPIC, QoS::AtLeastOnce, true, s.to_string());
+        }
+    };
+
+    eprintln!("imu {} publishing to {}", env!("CARGO_PKG_VERSION"), o.broker);
+    let mut last_detail = String::new();
+    loop {
+        let Some((path, baud)) = serial::find(o.device.as_deref(), o.baud) else {
+            let d = format!("no WT61 on {:?}", o.device.clone().map(|d| vec![d]).unwrap_or_else(serial::adapters));
+            if d != last_detail {
+                eprintln!("imu: {d}; retrying");
+                last_detail = d.clone();
+            }
+            thread::sleep(Duration::from_millis(500));
+            state(json!({ "state": "degraded", "detail": d }));
+            thread::sleep(Duration::from_secs(2));
+            continue;
+        };
+        let port = match serial::Port::open(&path, baud) {
+            Ok(p) => p,
+            Err(e) => {
+                eprintln!("imu: {path}: {e}");
+                thread::sleep(Duration::from_secs(2));
+                continue;
+            }
+        };
+        eprintln!("imu: WT61 on {path} at {baud} baud");
+        last_detail.clear();
+        let mut last_pub = Instant::now() - o.every;
+        let mut window = (Instant::now(), Counts::default());
+        let why = run(&port, o.mount, &|| false, |m, c| {
+            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,
+                    "rate_hz": ((c.accel - window.1.accel) as f64 / secs).round(),
+                    "bad_checksums": c.bad } }));
+                window = (Instant::now(), *c);
+            }
+            if last_pub.elapsed() < o.every || !connected.load(Ordering::Relaxed) {
+                return;
+            }
+            last_pub = Instant::now();
+            for (name, value) in signals(&m) {
+                let unit = units.get(name).unwrap_or_else(|| panic!("signal {name} is not an imu signal in contracts signals.json"));
+                let env = json!({ "timestamp": now_us(), "monotonic": mono_ns(), "value": value, "unit": unit });
+                let _ = client.try_publish(format!("yukart/imu/{name}"), QoS::AtMostOnce, true, env.to_string());
+            }
+        });
+        eprintln!("imu: {path}: {why}; looking again");
+        state(json!({ "state": "degraded", "detail": format!("{path}: {why}") }));
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn every_signal_is_registered() {
+        let u = super::units();
+        let m = super::Motion { roll_deg: 0.0, pitch_deg: 0.0, accel_lon_g: 0.0, accel_lat_g: 0.0, yaw_rate_dps: 0.0 };
+        for (k, _) in super::signals(&m) {
+            assert!(u.contains_key(k), "{k} not an imu signal in signals.json");
+        }
+    }
+}

src/imu/serial.rs

@@ -0,0 +1,124 @@
+//! A raw serial port (termios), and finding the WT61 among the USB serial adapters.
+//! No device name is fixed: the adapter may change (CP2102 now, maybe a PL2303 later),
+//! so every adapter that is not the modem is tried at each speed until one sends
+//! WT61 packets.
+
+use std::ffi::CString;
+use std::io;
+use std::time::{Duration, Instant};
+
+use crate::wt61::{Packet, Parser};
+
+/// Drivers of plain USB-UART adapters. The SIM7600's ports (`option`) are never opened:
+/// they belong to ModemManager.
+const ADAPTER_DRIVERS: [&str; 4] = ["cp210x", "pl2303", "ftdi_sio", "ch341"];
+/// Fastest first: the module's setting since 2026-10-14, then its other speed.
+pub const BAUDS: [u32; 2] = [115200, 9600];
+
+pub struct Port {
+    fd: i32,
+}
+
+impl Drop for Port {
+    fn drop(&mut self) {
+        unsafe { libc::close(self.fd) };
+    }
+}
+
+fn speed(baud: u32) -> Option<libc::speed_t> {
+    Some(match baud {
+        9600 => libc::B9600,
+        19200 => libc::B19200,
+        38400 => libc::B38400,
+        57600 => libc::B57600,
+        115200 => libc::B115200,
+        230400 => libc::B230400,
+        _ => return None,
+    })
+}
+
+impl Port {
+    /// Raw 8N1, reads returning after at most 0.2 s.
+    pub fn open(path: &str, baud: u32) -> io::Result<Self> {
+        let sp = speed(baud).ok_or_else(|| io::Error::other(format!("unsupported speed {baud}")))?;
+        let c = CString::new(path).map_err(|e| io::Error::new(io::ErrorKind::InvalidInput, e))?;
+        let fd = unsafe { libc::open(c.as_ptr(), libc::O_RDWR | libc::O_NOCTTY | libc::O_CLOEXEC) };
+        if fd < 0 {
+            return Err(io::Error::last_os_error());
+        }
+        let port = Port { fd };
+        unsafe {
+            let mut t: libc::termios = std::mem::zeroed();
+            if libc::tcgetattr(fd, &mut t) < 0 {
+                return Err(io::Error::last_os_error());
+            }
+            libc::cfmakeraw(&mut t);
+            t.c_cflag |= libc::CLOCAL | libc::CREAD;
+            t.c_cflag &= !(libc::CSTOPB | libc::PARENB | libc::CRTSCTS);
+            t.c_cc[libc::VMIN] = 0;
+            t.c_cc[libc::VTIME] = 2;
+            libc::cfsetispeed(&mut t, sp);
+            libc::cfsetospeed(&mut t, sp);
+            if libc::tcsetattr(fd, libc::TCSANOW, &t) < 0 {
+                return Err(io::Error::last_os_error());
+            }
+            libc::tcflush(fd, libc::TCIFLUSH);
+        }
+        Ok(port)
+    }
+
+    /// Whatever arrived; empty after 0.2 s of silence. An error means the adapter went away.
+    pub fn read(&self, buf: &mut [u8]) -> io::Result<usize> {
+        let n = unsafe { libc::read(self.fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len()) };
+        if n < 0 { Err(io::Error::last_os_error()) } else { Ok(n as usize) }
+    }
+}
+
+/// `/dev/ttyUSBn` of every USB-UART adapter present, in name order.
+pub fn adapters() -> Vec<String> {
+    let mut v: Vec<String> = std::fs::read_dir("/sys/bus/usb-serial/devices")
+        .into_iter()
+        .flatten()
+        .flatten()
+        .filter(|e| {
+            std::fs::read_link(e.path().join("driver"))
+                .ok()
+                .and_then(|d| d.file_name().map(|n| ADAPTER_DRIVERS.contains(&n.to_string_lossy().as_ref())))
+                .unwrap_or(false)
+        })
+        .map(|e| format!("/dev/{}", e.file_name().to_string_lossy()))
+        .collect();
+    v.sort();
+    v
+}
+
+/// Does `path` at `baud` carry WT61 packets? Listens up to `within`.
+pub fn is_wt61(path: &str, baud: u32, within: Duration) -> bool {
+    let Ok(port) = Port::open(path, baud) else { return false };
+    let (mut parser, mut out, mut buf) = (Parser::default(), Vec::<Packet>::new(), [0u8; 512]);
+    let t = Instant::now();
+    while t.elapsed() < within {
+        match port.read(&mut buf) {
+            Ok(n) => parser.push(&buf[..n], &mut out),
+            Err(_) => return false,
+        }
+        if out.len() >= 3 {
+            return true;
+        }
+    }
+    false
+}
+
+/// The first (adapter, speed) carrying WT61 packets, or a fixed device tried at each speed.
+pub fn find(device: Option<&str>, baud: Option<u32>) -> Option<(String, u32)> {
+    let paths = device.map(|d| vec![d.to_string()]).unwrap_or_else(adapters);
+    let bauds: Vec<u32> = baud.map(|b| vec![b]).unwrap_or_else(|| BAUDS.to_vec());
+    for p in &paths {
+        for &b in &bauds {
+            if is_wt61(p, b, Duration::from_millis(1500)) {
+                return Some((p.clone(), b));
+            }
+        }
+    }
+    None
+}

src/imu/wt61.rs

@@ -0,0 +1,92 @@
+//! The WT61's serial stream (the plain WT61 and the WT61 plus alike): 11-byte packets,
+//! `0x55`, a type, three little-endian int16 values and a temperature, then a checksum
+//! (the low byte of the sum of the first ten). Types: 0x51 acceleration, 0x52 angular
+//! rate, 0x53 angle (the module's own fusion, in its own frame; not used for the car).
+
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub enum Packet {
+    /// Specific force, g, module axes.
+    Accel([f64; 3]),
+    /// Angular rate, degree/s, module axes.
+    Gyro([f64; 3]),
+    /// Roll, pitch, yaw in degrees, as the module computes them.
+    Angle([f64; 3]),
+}
+
+fn values(p: &[u8]) -> [f64; 3] {
+    let v = |i: usize| i16::from_le_bytes([p[2 + 2 * i], p[3 + 2 * i]]) as f64 / 32768.0;
+    [v(0), v(1), v(2)]
+}
+
+/// Splits a byte stream into packets, resynchronising after noise or a bad checksum.
+#[derive(Default)]
+pub struct Parser {
+    buf: Vec<u8>,
+    /// Packets dropped for a bad checksum.
+    pub bad: u64,
+}
+
+impl Parser {
+    pub fn push(&mut self, bytes: &[u8], out: &mut Vec<Packet>) {
+        self.buf.extend_from_slice(bytes);
+        let mut i = 0;
+        while i + 11 <= self.buf.len() {
+            let p = &self.buf[i..i + 11];
+            if p[0] != 0x55 || !(0x51..=0x53).contains(&p[1]) {
+                i += 1;
+                continue;
+            }
+            if p[..10].iter().fold(0u8, |s, b| s.wrapping_add(*b)) != p[10] {
+                self.bad += 1;
+                i += 1;
+                continue;
+            }
+            let v = values(p);
+            out.push(match p[1] {
+                0x51 => Packet::Accel(v.map(|x| x * 16.0)),
+                0x52 => Packet::Gyro(v.map(|x| x * 2000.0)),
+                _ => Packet::Angle(v.map(|x| x * 180.0)),
+            });
+            i += 11;
+        }
+        self.buf.drain(..i);
+        if self.buf.len() > 4096 {
+            self.buf.clear(); // nothing parsed in 4 KB: not a WT61, or the wrong speed
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    pub fn packet(kind: u8, v: [i16; 4]) -> Vec<u8> {
+        let mut p = vec![0x55, kind];
+        for x in v {
+            p.extend_from_slice(&x.to_le_bytes());
+        }
+        let sum = p.iter().fold(0u8, |s, b| s.wrapping_add(*b));
+        p.push(sum);
+        p
+    }
+
+    #[test]
+    fn parses_resyncs_and_counts_bad_checksums() {
+        // As read on the car Orin, 2026-10-14: the module on its side, X down.
+        let mut stream = vec![0x13, 0x55, 0x00]; // noise first
+        stream.extend(packet(0x51, [-2005, 57, -340, 2985]));
+        let mut broken = packet(0x52, [1, 2, 3, 0]);
+        broken[10] ^= 0xFF;
+        stream.extend(broken);
+        stream.extend(packet(0x53, [16384, 0, -16384, 0]));
+        let (mut p, mut out) = (Parser::default(), Vec::new());
+        // In two pieces, split mid-packet.
+        p.push(&stream[..20], &mut out);
+        p.push(&stream[20..], &mut out);
+        assert_eq!(p.bad, 1);
+        assert_eq!(out.len(), 2);
+        let Packet::Accel(a) = out[0] else { panic!("{:?}", out[0]) };
+        assert!((a[0] + 0.979).abs() < 0.001 && (a[2] + 0.166).abs() < 0.001);
+        assert_eq!(out[1], Packet::Angle([90.0, 0.0, -90.0]));
+    }
+}