diff --git a/Cargo.toml b/Cargo.toml index e1340ae..d972ae8 100644 --- a/Cargo.toml +++ b/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" diff --git a/README.md b/README.md index 7808691..354560a 100644 --- a/README.md +++ b/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 diff --git a/deploy/yukart-imu.service b/deploy/yukart-imu.service new file mode 100644 index 0000000..959262f --- /dev/null +++ b/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 diff --git a/scripts/deploy-imu.sh b/scripts/deploy-imu.sh new file mode 100755 index 0000000..62b850d --- /dev/null +++ b/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 "$@" diff --git a/src/imu/frame.rs b/src/imu/frame.rs new file mode 100644 index 0000000..63c0773 --- /dev/null +++ b/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 { + 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 { + 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 { + 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}"); + } +} diff --git a/src/imu/main.rs b/src/imu/main.rs new file mode 100644 index 0000000..6043107 --- /dev/null +++ b/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/`, 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 { + 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, + baud: Option, + mount: Mount, + every: Duration, + once: bool, +} + +fn parse() -> Result { + 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::>()), + })) + .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::().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"); + } + } +} diff --git a/src/imu/serial.rs b/src/imu/serial.rs new file mode 100644 index 0000000..79e5f84 --- /dev/null +++ b/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 { + 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 { + 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 { + 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 { + let mut v: Vec = 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::::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) -> Option<(String, u32)> { + let paths = device.map(|d| vec![d.to_string()]).unwrap_or_else(adapters); + let bauds: Vec = 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 +} diff --git a/src/imu/wt61.rs b/src/imu/wt61.rs new file mode 100644 index 0000000..595b7a2 --- /dev/null +++ b/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, + /// Packets dropped for a bad checksum. + pub bad: u64, +} + +impl Parser { + pub fn push(&mut self, bytes: &[u8], out: &mut Vec) { + 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 { + 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])); + } +}