From 6a7de037737833f881b18703568f7f2cc3f637f0 Mon Sep 17 00:00:00 2001 From: Mikkeli Date: Mon, 12 Oct 2026 23:25:03 +0900 Subject: [PATCH] obd-poller: OBD-II mode 01, the single CAN writer; recorder keeps its values obd-poller asks 0x7DF for the nine obd2 signals, only while the bus is alive, and publishes envelopes stamped with the reply's kernel time. The recorder subscribes to yukart/vehicle/* and writes vehicle.ndjson beside the frames. Tested against a fake ECU on vcan0. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz --- Cargo.toml | 6 +- README.md | 7 +- deploy/yukart-obd-poller.service | 16 ++ docs/GUIDELINES.md | 11 +- docs/impl/plans/INITIAL.md | 39 ++- scripts/deploy-obd-poller.sh | 31 +++ scripts/deploy-recorder.sh | 2 +- src/obd-poller/main.rs | 391 +++++++++++++++++++++++++++++++ src/obd-poller/pids.rs | 59 +++++ src/obd-poller/socket.rs | 110 +++++++++ src/recorder/main.rs | 27 ++- src/recorder/session.rs | 15 ++ src/recorder/sources.rs | 45 ++++ 13 files changed, 747 insertions(+), 12 deletions(-) create mode 100644 deploy/yukart-obd-poller.service create mode 100755 scripts/deploy-obd-poller.sh create mode 100644 src/obd-poller/main.rs create mode 100644 src/obd-poller/pids.rs create mode 100644 src/obd-poller/socket.rs diff --git a/Cargo.toml b/Cargo.toml index 5147fdf..0ef6ef6 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 and orin-status now; can-bridge, obd-poller, gnss, decoder later" +description = "Acquisition and processing cluster: recorder, orin-status and obd-poller now; can-bridge, gnss, decoder later" publish = false [[bin]] @@ -13,6 +13,10 @@ path = "src/recorder/main.rs" name = "orin-status" path = "src/orin-status/main.rs" +[[bin]] +name = "obd-poller" +path = "src/obd-poller/main.rs" + [[bin]] name = "bus-bench" path = "src/bus-bench/main.rs" diff --git a/README.md b/README.md index dbbb191..db35484 100644 --- a/README.md +++ b/README.md @@ -10,18 +10,21 @@ 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` and `orin-status` now; `can-bridge`, `obd-poller`, `gnss` and `decoder` as they are +unit: `recorder`, `orin-status` and `obd-poller` now; `can-bridge`, `gnss` and `decoder` as they are needed. | Binary | Installed as | Does | |---|---|---| -| `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound | +| `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound; OBD-II values from the bus beside them | | `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 polling, the single CAN writer: rpm, speed, throttle, load, MAP, MAF ~10 Hz; temperatures and fuel 1 Hz (`yukart/vehicle/*`) | | `orin-status` | `yukart-orin-status.service` | The Orin's health on the bus, once a second: temperatures, CPU/GPU load, memory, power, fan, WiFi/LTE/tablet links, UPS battery (`yukart/host/*`) | ```bash scripts/deploy-recorder.sh # build, glibc check, install on the car Orin scripts/deploy-orin-status.sh # build on the development Orin, glibc check, install +scripts/deploy-obd-poller.sh # build, glibc check, install on the car Orin +yukart-obd-poller --once # on the Orin, service stopped: what the ECU answers 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-obd-poller.service b/deploy/yukart-obd-poller.service new file mode 100644 index 0000000..02b244f --- /dev/null +++ b/deploy/yukart-obd-poller.service @@ -0,0 +1,16 @@ +[Unit] +Description=Yukart obd-poller: OBD-II mode 01 polling, the single CAN writer +Documentation=gitea:yukart/svc-core +# Tier 2. Ordered after the broker and nothing else: the adapter comes and goes, and +# the poller only sends while the bus is visibly alive. +After=mosquitto.service +StartLimitIntervalSec=0 + +[Service] +User=mikkeli +ExecStart=/usr/local/bin/yukart-obd-poller +Restart=always +RestartSec=2 + +[Install] +WantedBy=multi-user.target diff --git a/docs/GUIDELINES.md b/docs/GUIDELINES.md index 2dc802b..bd5352f 100644 --- a/docs/GUIDELINES.md +++ b/docs/GUIDELINES.md @@ -3,10 +3,13 @@ type: guidelines --- # Core services guidelines -## 1. Listen-only until `obd-poller` exists -- Nothing here opens a CAN socket for writing. The recorder only reads, and the - host brings `can_pcan` up listen-only. -- `obd-poller` will be the single writer. Until it lands, any transmit is a bug. +## 1. One CAN writer: `obd-poller` +- `obd-poller` is the only thing that transmits on the car's bus (project-yukart + decision 3): OBD-II requests, one at a time, only while the bus is visibly + alive. Anything else that transmits is a bug; the recorder only reads. +- `can_pcan` is no longer listen-only (2026-10-12), so the adapter acknowledges + frames like any scan tool. Going back is one line in orin-nano-env-setup's + `/etc/default/can-can_pcan`. ## 2. Capture never waits - A frame missed is gone. Producers hand frames to the writer without blocking; diff --git a/docs/impl/plans/INITIAL.md b/docs/impl/plans/INITIAL.md index d528a67..da8a80d 100644 --- a/docs/impl/plans/INITIAL.md +++ b/docs/impl/plans/INITIAL.md @@ -22,9 +22,10 @@ bus. Below. ## Recorder Reads SocketCAN and gpsd directly. It is tier 1, and the broker is a single point -of failure that raw capture should not share. When `can-bridge` exists, the -recorder still owns raw capture; it subscribes to the bus only for decoded -signals. +of failure that raw capture should not share (decision 25). When `can-bridge` +exists, the recorder still owns raw capture; it subscribes to the bus only for +`yukart/vehicle/*` (`--broker`, `''` to disable), and a broker that is down costs +those envelopes, never a frame. ### Session triggers @@ -60,6 +61,9 @@ is the intended second client: `ctl mark crank` for a recovered input loss, and can-0001.log.zst candump -L, closed chunks (5 min or 64 MB), zstd can-0002.log the open chunk, plain gnss.ndjson {"rx": , "msg": } + vehicle.ndjson {"rx": …, "signal": …, "env": } for every message on + yukart/vehicle/* (OBD-II now); "retained": true for the + broker's stored copy, which may predate the session events.ndjson marks, can_link, gnss_link, clock_step, drops, storage_bound ``` @@ -110,6 +114,35 @@ enough to time individual frames against position. - **Copy-off.** Recordings stay on the car until the parked-at-home step in the power plan, or a manual `rsync`. +## obd-poller + +OBD-II mode 01 (SAE J1979) on `can_pcan`, the single CAN writer. Tier 2. + +- **Requests** go to the functional address `0x7DF`, one at a time; the first + single-frame reply from `0x7E8`–`0x7EF` answers it (kernel filter: nothing else + reaches the process). 100 ms timeout, 10 ms between requests. +- **Discovery** walks the supported-PID bitmasks (`01 00`, `01 20`, …) whenever the + bus wakes; only supported PIDs are polled. +- **Schedule:** rpm, speed, throttle, load, MAP and MAF every round (~10–13 Hz each + with a 4 ms ECU); coolant, intake air and fuel level once a second. +- **Only while the bus is alive:** the interface's received-frame count moved in + the last second. A request into a sleeping bus gets no ACK, and the adapter would + resend it until error-passive. +- **Output:** envelopes on `yukart/vehicle/`, retained; names, units and the + `obd2` source checked against contracts at build. `timestamp` is the reply's + kernel receive time, identical to its line in the recorder's CAN log. +- **State** on `yukart/system/obd-poller`: `ready` with supported PIDs, achieved + rates and unanswered count every 5 s; `degraded` with the reason (bus idle, no + ECU answers, interface errors); last will `stopping`. +- `--once`: discover, read each supported PID once, print JSON with reply times. +- **Not yet:** multi-frame replies (ISO-TP flow control), so no DTC reading (modes + 03/07/0A) and no MUT-III `21 xx` requests. The kernel has no `can-isotp`; flow + control will be done by hand. + +Tested 2026-10-12 against a fake ECU on `vcan0`: 7 PIDs discovered and polled, +540 requests and 540 replies in the recorder's log, every envelope's timestamp +equal to its reply frame's. + ## orin-status The Orin's own health as envelopes on `yukart/host/`, retained, once a diff --git a/scripts/deploy-obd-poller.sh b/scripts/deploy-obd-poller.sh new file mode 100755 index 0000000..270e179 --- /dev/null +++ b/scripts/deploy-obd-poller.sh @@ -0,0 +1,31 @@ +#!/usr/bin/env bash +# Build the obd-poller for aarch64, check it against the car's glibc, and install it +# on an Orin as the yukart-obd-poller service. +# +# usage: scripts/deploy-obd-poller.sh [host] (default: mikkeli-orin-nano) +# +# Cross-compiles from the workstation (the documented fallback): the development +# Orin is the preferred native build host. Either way check-glibc.sh gates it. +set -euo pipefail + +HOST="${1:-mikkeli-orin-nano}" +cd "$(dirname "$0")/.." +TARGET=aarch64-unknown-linux-gnu +BIN="target/$TARGET/release/obd-poller" + +cargo build --release --target "$TARGET" --bin obd-poller +scripts/check-glibc.sh "$BIN" + +# Binary lives under ~/infra per the host repo's guidelines; /usr/local/bin gets a symlink. +ssh "$HOST" 'mkdir -p ~/infra/svc-core' +scp -q "$BIN" "$HOST:infra/svc-core/obd-poller.new" +scp -q deploy/yukart-obd-poller.service "$HOST:infra/svc-core/yukart-obd-poller.service" +ssh "$HOST" 'set -e + mv ~/infra/svc-core/obd-poller.new ~/infra/svc-core/obd-poller + sudo ln -sfn "$HOME/infra/svc-core/obd-poller" /usr/local/bin/yukart-obd-poller + sudo install -m 0644 ~/infra/svc-core/yukart-obd-poller.service /etc/systemd/system/yukart-obd-poller.service + sudo systemctl daemon-reload + sudo systemctl enable yukart-obd-poller.service >/dev/null + sudo systemctl restart yukart-obd-poller.service + sleep 1 + systemctl --no-pager --lines=5 status yukart-obd-poller.service' diff --git a/scripts/deploy-recorder.sh b/scripts/deploy-recorder.sh index 7441ec6..f48c31c 100755 --- a/scripts/deploy-recorder.sh +++ b/scripts/deploy-recorder.sh @@ -13,7 +13,7 @@ cd "$(dirname "$0")/.." TARGET=aarch64-unknown-linux-gnu BIN="target/$TARGET/release/recorder" -cargo build --release --target "$TARGET" +cargo build --release --target "$TARGET" --bin recorder scripts/check-glibc.sh "$BIN" # Binary lives under ~/infra per the host repo's guidelines; /usr/local/bin gets a symlink. diff --git a/src/obd-poller/main.rs b/src/obd-poller/main.rs new file mode 100644 index 0000000..5246229 --- /dev/null +++ b/src/obd-poller/main.rs @@ -0,0 +1,391 @@ +//! obd-poller — active OBD-II mode 01 polling, the single CAN writer (decision 3). +//! +//! Asks the engine ECU for standard values (SAE J1979, functional request 0x7DF, +//! replies on 0x7E8–0x7EF) one at a time, and publishes each reply as an envelope on +//! `yukart/vehicle/`, retained; names and units from contracts' +//! `signals.json` (source `obd2`). The envelope's `timestamp` is the reply's kernel +//! receive time, so it lines up with the recorder's raw frames, which hold the same +//! request and reply. These values are the ground truth for decoding the car's own +//! broadcast frames. +//! +//! It only sends while the bus is visibly alive (the interface received frames in +//! the last second). A request into a sleeping bus gets no acknowledgement, and the +//! adapter would retry it until it goes error-passive. + +mod pids; +mod socket; + +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, NetworkOptions, Packet, QoS}; +use serde_json::{json, Value}; + +use pids::{Pid, PIDS}; +use socket::ObdSocket; + +const USAGE: &str = "\ +usage: obd-poller [options] + --iface NAME CAN interface (can_pcan) + --broker HOST:PORT MQTT broker (127.0.0.1:1883) + --gap-ms N pause between requests (10) + --timeout-ms N give up on a reply after this long (100) + --slow-ms N period of the slow PIDs (temperatures, fuel) (1000) + --once discover, read every supported PID once, print JSON, exit (no broker)"; + +const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json")); +const SYSTEM_TOPIC: &str = "yukart/system/obd-poller"; +const REQUEST_ID: u32 = 0x7DF; + +pub 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 polled signals, from the registry: never authored here (contracts guideline 1). +fn units() -> HashMap<&'static str, String> { + let v: Value = serde_json::from_str(REGISTRY).expect("contracts signals.json is valid JSON"); + PIDS.iter() + .map(|p| { + let s = &v["signals"][p.signal]; + assert_eq!(s["source"], "obd2", "signal {} is not an obd2 signal in signals.json", p.signal); + (p.signal, s["unit"].as_str().unwrap_or("").to_string()) + }) + .collect() +} + +struct Opts { + iface: String, + broker: String, + gap: Duration, + timeout: Duration, + slow: Duration, + once: bool, +} + +/// Is anything else talking? Received-frame count moved within the last second. +struct BusWatch { + path: String, + last: Option<(u64, Instant)>, +} + +impl BusWatch { + fn alive(&mut self) -> bool { + let Some(n) = std::fs::read_to_string(&self.path).ok().and_then(|s| s.trim().parse::().ok()) else { + self.last = None; + return false; + }; + match self.last { + Some((prev, _)) if n != prev => self.last = Some((n, Instant::now())), + None => self.last = Some((n, Instant::now() - Duration::from_secs(10))), + _ => {} + } + self.last.map(|(_, t)| t.elapsed() < Duration::from_secs(1)).unwrap_or(false) + } +} + +struct Poller { + sock: ObdSocket, + timeout: Duration, +} + +impl Poller { + /// One mode 01 request; the first reply to it from any ECU: (receive time µs, ECU id, data bytes). + fn ask(&self, pid: u8) -> std::io::Result)>> { + self.sock.send(REQUEST_ID, &[0x02, 0x01, pid, 0x55, 0x55, 0x55, 0x55, 0x55])?; + let deadline = Instant::now() + self.timeout; + loop { + let left = deadline.saturating_duration_since(Instant::now()); + if left.is_zero() { + return Ok(None); + } + let Some(r) = self.sock.recv(left)? else { return Ok(None) }; + // Single frame: [length, 0x41, pid, A, B, ...]. Anything else is not our answer. + let len = (r.data[0] & 0x0F) as usize; + if r.data[0] >> 4 == 0 && len >= 2 && len <= 7 && r.data[1] == 0x41 && r.data[2] == pid { + return Ok(Some((r.ts_us, r.id, r.data[3..1 + len].to_vec()))); + } + } + } + + /// Supported PIDs, walking the 0x00 / 0x20 / 0x40 ... bitmasks. Empty: no ECU answered. + fn discover(&self) -> std::io::Result> { + let mut all = Vec::new(); + let mut mask = 0x00u8; + loop { + let Some((_, _, d)) = self.ask(mask)? else { break }; + if d.len() < 4 { + break; + } + let s = pids::supported_in(mask, &d); + let more = s.contains(&(mask + 0x20)); + all.extend(s.into_iter().filter(|p| p % 0x20 != 0)); + if !more || mask >= 0xC0 { + break; + } + mask += 0x20; + } + Ok(all) + } +} + +fn parse() -> Result { + let mut o = Opts { + iface: "can_pcan".into(), + broker: "127.0.0.1:1883".into(), + gap: Duration::from_millis(10), + timeout: Duration::from_millis(100), + slow: Duration::from_millis(1000), + 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 ms = || v.parse::().map(Duration::from_millis).map_err(|_| format!("{a}: not a number: {v}")); + match a.as_str() { + "--iface" => o.iface = v.clone(), + "--broker" => o.broker = v.clone(), + "--gap-ms" => o.gap = ms()?, + "--timeout-ms" => o.timeout = ms()?, + "--slow-ms" => o.slow = ms()?, + _ => return Err(format!("unknown option {a}")), + } + } + Ok(o) +} + +fn once(o: &Opts) -> ExitCode { + let mut watch = BusWatch { path: format!("/sys/class/net/{}/statistics/rx_packets", o.iface), last: None }; + let t = Instant::now(); + while !watch.alive() { + if t.elapsed() > Duration::from_secs(3) { + eprintln!("obd-poller: {}: no traffic; is the ignition on?", o.iface); + return ExitCode::FAILURE; + } + thread::sleep(Duration::from_millis(100)); + } + let p = match ObdSocket::open(&o.iface) { + Ok(sock) => Poller { sock, timeout: o.timeout }, + Err(e) => { + eprintln!("obd-poller: {}: {e}", o.iface); + return ExitCode::FAILURE; + } + }; + let run = || -> std::io::Result { + let supported = p.discover()?; + let mut values = serde_json::Map::new(); + for pid in PIDS.iter().filter(|x| supported.contains(&x.pid)) { + let t = Instant::now(); + if let Some((_, ecu, d)) = p.ask(pid.pid)?.filter(|r| r.2.len() >= pid.bytes) { + values.insert(pid.signal.into(), json!({ "value": (pid.decode)(&d), "ecu": format!("{ecu:03X}"), + "reply_ms": t.elapsed().as_secs_f64() * 1e3 })); + } + } + Ok(json!({ + "supported": supported.iter().map(|x| format!("{x:02X}")).collect::>(), + "values": values, + })) + }; + match run() { + Ok(v) => { + println!("{}", serde_json::to_string_pretty(&v).unwrap()); + ExitCode::SUCCESS + } + Err(e) => { + eprintln!("obd-poller: {e}"); + ExitCode::FAILURE + } + } +} + +fn main() -> ExitCode { + let o = match parse() { + Ok(o) => o, + Err(e) => { + eprintln!("obd-poller: {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!("obd-poller: --broker must be HOST:PORT\n{USAGE}"); + return ExitCode::from(2); + }; + let mut opts = MqttOptions::new("obd-poller", 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, 256); + let mut net = NetworkOptions::new(); + net.set_tcp_nodelay(true); // svc-core BUS-BENCH.md: Nagle adds a ~40 ms tail + connection.eventloop.set_network_options(net); + let connected = Arc::new(AtomicBool::new(false)); + let announce = Arc::new(AtomicBool::new(false)); + { + let (connected, announce) = (connected.clone(), announce.clone()); + thread::spawn(move || { + for ev in connection.iter() { + match ev { + Ok(Event::Incoming(Packet::ConnAck(_))) => { + eprintln!("obd-poller: connected to the broker"); + connected.store(true, Ordering::Relaxed); + announce.store(true, Ordering::Relaxed); + } + Ok(_) => {} + Err(e) => { + if connected.swap(false, Ordering::Relaxed) { + eprintln!("obd-poller: broker: {e}; retrying"); + } + thread::sleep(Duration::from_secs(1)); + } + } + } + }); + } + + eprintln!("obd-poller {} on {}, publishing to {}", env!("CARGO_PKG_VERSION"), o.iface, o.broker); + let mut watch = BusWatch { path: format!("/sys/class/net/{}/statistics/rx_packets", o.iface), last: None }; + let mut poller: Option = None; + let mut supported: Option> = None; + let mut last_slow = Instant::now() - o.slow; + let mut next_discover = Instant::now(); + let mut counts: HashMap<&'static str, u32> = HashMap::new(); + let mut misses = 0u32; + let mut window = Instant::now(); + let mut state = json!({ "state": "starting" }); + let mut last_state = String::new(); + let mut last_announce = Instant::now(); + + loop { + // The service's own state: on change, on reconnect, and every 5 s with the achieved rates. + let s = state.to_string(); + if connected.load(Ordering::Relaxed) + && (s != last_state || announce.swap(false, Ordering::Relaxed) || last_announce.elapsed() > Duration::from_secs(5)) + { + // Log transitions only; the rates change every 5 s. + let kind = |j: &str| serde_json::from_str::(j).ok().map(|v| (v["state"].clone(), v["detail"].is_string().then(|| v["detail"].clone()))); + if kind(&s) != kind(&last_state) { + eprintln!("obd-poller: {s}"); + } + let _ = client.try_publish(SYSTEM_TOPIC, QoS::AtLeastOnce, true, s.clone()); + last_state = s; + last_announce = Instant::now(); + } + + if !watch.alive() { + // Asleep, ignition off, or unplugged: say nothing on the bus, and rediscover once it wakes. + poller = None; + supported = None; + state = json!({ "state": "degraded", "detail": "bus idle: not polling" }); + thread::sleep(Duration::from_millis(250)); + continue; + } + if poller.is_none() { + match ObdSocket::open(&o.iface) { + Ok(sock) => poller = Some(Poller { sock, timeout: o.timeout }), + Err(e) => { + state = json!({ "state": "degraded", "detail": format!("{}: {e}", o.iface) }); + thread::sleep(Duration::from_secs(1)); + continue; + } + } + } + let p = poller.as_ref().unwrap(); + + if supported.is_none() { + if Instant::now() < next_discover { + thread::sleep(Duration::from_millis(100)); + continue; + } + match p.discover() { + Ok(s) if !s.is_empty() => { + let polled: Vec<&str> = PIDS.iter().filter(|x| s.contains(&x.pid)).map(|x| x.signal).collect(); + eprintln!("obd-poller: supported PIDs {:02X?}; polling {polled:?}", s); + supported = Some(s); + misses = 0; + } + Ok(_) => { + state = json!({ "state": "degraded", "detail": "no ECU answers 01 00; retrying in 5 s" }); + next_discover = Instant::now() + Duration::from_secs(5); + } + Err(e) => { + state = json!({ "state": "degraded", "detail": format!("{}: {e}", o.iface) }); + poller = None; + next_discover = Instant::now() + Duration::from_secs(1); + } + } + continue; + } + + // One round: the fast PIDs, and the slow ones when their period is up. + let slow_due = last_slow.elapsed() >= o.slow; + if slow_due { + last_slow = Instant::now(); + } + let sup = supported.as_ref().unwrap(); + let round: Vec<&Pid> = PIDS.iter().filter(|x| sup.contains(&x.pid) && (x.fast || slow_due)).collect(); + let mut failed = false; + for pid in round { + match p.ask(pid.pid) { + Ok(Some((ts, _, d))) if d.len() >= pid.bytes => { + *counts.entry(pid.signal).or_default() += 1; + if connected.load(Ordering::Relaxed) { + let v = (pid.decode)(&d); + let env = json!({ "timestamp": ts, "monotonic": mono_ns(), + "value": (v * 100.0).round() / 100.0, "unit": units[pid.signal] }); + let _ = client.try_publish(format!("yukart/vehicle/{}", pid.signal), QoS::AtMostOnce, true, env.to_string()); + } + } + Ok(_) => misses += 1, + Err(e) => { + // ENOBUFS (tx queue full: nothing acknowledges), ENETDOWN, ENODEV. + state = json!({ "state": "degraded", "detail": format!("{}: {e}", o.iface) }); + poller = None; + failed = true; + thread::sleep(Duration::from_millis(500)); + break; + } + } + thread::sleep(o.gap); + } + if failed { + continue; + } + + if window.elapsed() >= Duration::from_secs(5) { + let secs = window.elapsed().as_secs_f64(); + let rates: serde_json::Map = + counts.iter().map(|(k, n)| (k.to_string(), json!((*n as f64 / secs * 10.0).round() / 10.0))).collect(); + // An odd unanswered request is normal; none answered at all is not. + state = json!({ "state": if counts.is_empty() { "degraded" } else { "ready" }, + "detail": { "supported": sup.iter().map(|x| format!("{x:02X}")).collect::>(), + "rates_hz": rates, "unanswered": misses } }); + counts.clear(); + misses = 0; + window = Instant::now(); + } + } +} diff --git a/src/obd-poller/pids.rs b/src/obd-poller/pids.rs new file mode 100644 index 0000000..4a1d570 --- /dev/null +++ b/src/obd-poller/pids.rs @@ -0,0 +1,59 @@ +//! The OBD-II mode 01 PIDs polled, and how each reply becomes a value (SAE J1979). +//! Signal names and units belong to contracts' `signals.json` (source `obd2`). + +pub struct Pid { + pub pid: u8, + pub signal: &'static str, + /// Reply data bytes A, B, ... needed. + pub bytes: usize, + pub decode: fn(&[u8]) -> f64, + /// Polled every round; the rest about once a second. + pub fast: bool, +} + +fn ab(d: &[u8]) -> f64 { + (d[0] as f64) * 256.0 + d[1] as f64 +} + +pub const PIDS: &[Pid] = &[ + Pid { pid: 0x0C, signal: "engine_rpm", bytes: 2, decode: |d| ab(d) / 4.0, fast: true }, + Pid { pid: 0x0D, signal: "vehicle_speed_kph", bytes: 1, decode: |d| d[0] as f64, fast: true }, + Pid { pid: 0x11, signal: "throttle_pct", bytes: 1, decode: |d| d[0] as f64 * 100.0 / 255.0, fast: true }, + Pid { pid: 0x04, signal: "engine_load_pct", bytes: 1, decode: |d| d[0] as f64 * 100.0 / 255.0, fast: true }, + Pid { pid: 0x0B, signal: "intake_pressure_kpa", bytes: 1, decode: |d| d[0] as f64, fast: true }, + Pid { pid: 0x10, signal: "maf_gps", bytes: 2, decode: |d| ab(d) / 100.0, fast: true }, + Pid { pid: 0x05, signal: "coolant_temp_c", bytes: 1, decode: |d| d[0] as f64 - 40.0, fast: false }, + Pid { pid: 0x0F, signal: "intake_air_temp_c", bytes: 1, decode: |d| d[0] as f64 - 40.0, fast: false }, + Pid { pid: 0x2F, signal: "fuel_level_pct", bytes: 1, decode: |d| d[0] as f64 * 100.0 / 255.0, fast: false }, +]; + +/// The bitmask PIDs (0x00, 0x20, ...): bit 7 of byte A is the PID one above the mask's own. +pub fn supported_in(mask_pid: u8, d: &[u8]) -> Vec { + let bits = u32::from_be_bytes([d[0], d[1], d[2], d[3]]); + (0..32).filter(|i| bits & (1 << (31 - i)) != 0).map(|i| mask_pid + 1 + i as u8).collect() +} + +#[cfg(test)] +mod tests { + use super::*; + + fn by(pid: u8) -> &'static Pid { + PIDS.iter().find(|p| p.pid == pid).unwrap() + } + + #[test] + fn decodes_j1979_examples() { + assert_eq!((by(0x0C).decode)(&[0x0D, 0x34]), 845.0); // 0x0D34 / 4 + assert_eq!((by(0x05).decode)(&[0x7B]), 83.0); + assert_eq!((by(0x10).decode)(&[0x01, 0xF4]), 5.0); + assert!(((by(0x11).decode)(&[0xFF]) - 100.0).abs() < 1e-9); + } + + #[test] + fn reads_the_supported_bitmask() { + // 0xBE1FA813: a common real-world reply to 01 00. + let s = supported_in(0x00, &[0xBE, 0x1F, 0xA8, 0x13]); + assert_eq!(&s[..6], &[0x01, 0x03, 0x04, 0x05, 0x06, 0x07]); + assert!(s.contains(&0x0C) && s.contains(&0x0D) && s.contains(&0x20)); + } +} diff --git a/src/obd-poller/socket.rs b/src/obd-poller/socket.rs new file mode 100644 index 0000000..267b2e3 --- /dev/null +++ b/src/obd-poller/socket.rs @@ -0,0 +1,110 @@ +//! A SocketCAN socket for OBD-II request/response: sends requests, and receives only +//! the diagnostic response IDs (0x7E8–0x7EF), each with the kernel's receive time. +//! +//! The bus's own ~900 frames/s never reach this process: the kernel filter drops +//! them. Other sockets on the interface (the recorder) still see the requests sent +//! here, through SocketCAN's local loopback, and the ECU's replies. + +use std::ffi::CString; +use std::io; +use std::mem::{size_of, zeroed}; +use std::time::Duration; + +pub struct Response { + /// CLOCK_REALTIME at reception, microseconds. + pub ts_us: i64, + pub id: u32, + pub data: [u8; 8], +} + +pub struct ObdSocket { + fd: i32, +} + +impl Drop for ObdSocket { + fn drop(&mut self) { + unsafe { libc::close(self.fd) }; + } +} + +fn setopt(fd: i32, level: i32, name: i32, val: &T) -> io::Result<()> { + let r = unsafe { + libc::setsockopt(fd, level, name, val as *const T as *const libc::c_void, size_of::() as u32) + }; + if r < 0 { Err(io::Error::last_os_error()) } else { Ok(()) } +} + +impl ObdSocket { + pub fn open(iface: &str) -> io::Result { + let name = CString::new(iface).map_err(|e| io::Error::new(io::ErrorKind::InvalidInput, e))?; + let idx = unsafe { libc::if_nametoindex(name.as_ptr()) }; + if idx == 0 { + return Err(io::Error::last_os_error()); + } + let fd = unsafe { libc::socket(libc::PF_CAN, libc::SOCK_RAW | libc::SOCK_CLOEXEC, libc::CAN_RAW) }; + if fd < 0 { + return Err(io::Error::last_os_error()); + } + let sock = ObdSocket { fd }; + // Standard 11-bit IDs 0x7E8..0x7EF only: the eight possible ECU responders. + let filter = libc::can_filter { can_id: 0x7E8, can_mask: 0x7F8 | libc::CAN_EFF_FLAG | libc::CAN_RTR_FLAG }; + setopt(fd, libc::SOL_CAN_RAW, libc::CAN_RAW_FILTER, &filter)?; + setopt(fd, libc::SOL_SOCKET, libc::SO_TIMESTAMP, &1i32)?; + let mut addr: libc::sockaddr_can = unsafe { zeroed() }; + addr.can_family = libc::AF_CAN as u16; + addr.can_ifindex = idx as i32; + let r = unsafe { + libc::bind(fd, &addr as *const _ as *const libc::sockaddr, size_of::() as u32) + }; + if r < 0 { + return Err(io::Error::last_os_error()); + } + Ok(sock) + } + + pub fn send(&self, id: u32, data: &[u8; 8]) -> io::Result<()> { + let mut f: libc::can_frame = unsafe { zeroed() }; + f.can_id = id; + f.can_dlc = 8; + f.data = *data; + let n = unsafe { libc::write(self.fd, &f as *const _ as *const libc::c_void, size_of::()) }; + if n < 0 { Err(io::Error::last_os_error()) } else { Ok(()) } + } + + /// The next response within `timeout`, or None. + pub fn recv(&self, timeout: Duration) -> io::Result> { + let tv = libc::timeval { tv_sec: timeout.as_secs() as _, tv_usec: timeout.subsec_micros().max(1) as _ }; + setopt(self.fd, libc::SOL_SOCKET, libc::SO_RCVTIMEO, &tv)?; + let mut frame: libc::can_frame = unsafe { zeroed() }; + let mut iov = libc::iovec { + iov_base: &mut frame as *mut _ as *mut libc::c_void, + iov_len: size_of::(), + }; + let mut cbuf = [0u64; 8]; + let mut msg: libc::msghdr = unsafe { zeroed() }; + msg.msg_iov = &mut iov; + msg.msg_iovlen = 1; + msg.msg_control = cbuf.as_mut_ptr() as *mut libc::c_void; + msg.msg_controllen = size_of::<[u64; 8]>() as _; + let n = unsafe { libc::recvmsg(self.fd, &mut msg, 0) }; + if n < 0 { + let e = io::Error::last_os_error(); + return match e.kind() { + io::ErrorKind::WouldBlock | io::ErrorKind::TimedOut | io::ErrorKind::Interrupted => Ok(None), + _ => Err(e), + }; + } + let mut ts_us = crate::now_us(); + unsafe { + let mut c = libc::CMSG_FIRSTHDR(&msg); + while !c.is_null() { + if (*c).cmsg_level == libc::SOL_SOCKET && (*c).cmsg_type == libc::SCM_TIMESTAMP { + let tv = std::ptr::read_unaligned(libc::CMSG_DATA(c) as *const libc::timeval); + ts_us = tv.tv_sec as i64 * 1_000_000 + tv.tv_usec as i64; + } + c = libc::CMSG_NXTHDR(&msg, c); + } + } + Ok(Some(Response { ts_us, id: frame.can_id & libc::CAN_SFF_MASK, data: frame.data })) + } +} diff --git a/src/recorder/main.rs b/src/recorder/main.rs index 3f4a794..376615c 100644 --- a/src/recorder/main.rs +++ b/src/recorder/main.rs @@ -1,7 +1,9 @@ //! recorder — persists raw CAN and GNSS to disk, session by session. //! //! 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. +//! 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 +//! and are recorded when it is there. //! //! Sessions start when CAN frames arrive (auto) or on `ctl start` (manual), and an //! auto session ends after `--idle` seconds without frames: ignition off, adapter @@ -35,6 +37,7 @@ usage: --iface NAME CAN interface (can_pcan) --root DIR recordings directory (/var/lib/yukart/recordings) --gpsd HOST:PORT gpsd, '' to disable (127.0.0.1:2947) + --broker HOST:PORT MQTT, for yukart/vehicle/*; '' to disable (127.0.0.1:1883) --ctl PATH control socket (/run/yukart-recorder/ctl.sock) --idle SECS end an auto session after this long without frames (15) --min-secs SECS discard auto sessions shorter than this with no marks (3) @@ -50,6 +53,7 @@ struct Config { iface: String, root: PathBuf, gpsd: String, + broker: String, ctl: String, idle: Duration, min_secs: u64, @@ -63,6 +67,7 @@ fn parse_run(args: &[String]) -> Result { iface: "can_pcan".into(), root: "/var/lib/yukart/recordings".into(), gpsd: "127.0.0.1:2947".into(), + broker: "127.0.0.1:1883".into(), ctl: "/run/yukart-recorder/ctl.sock".into(), idle: Duration::from_secs(15), min_secs: 3, @@ -82,6 +87,7 @@ fn parse_run(args: &[String]) -> Result { "--iface" => c.iface = v.clone(), "--root" => c.root = v.into(), "--gpsd" => c.gpsd = v.clone(), + "--broker" => c.broker = v.clone(), "--ctl" => c.ctl = v.clone(), "--idle" => c.idle = Duration::from_secs(num()?), "--min-secs" => c.min_secs = num()?, @@ -139,6 +145,7 @@ struct Recorder { last_tpv: Option<(f64, String)>, can_link: (bool, String), gnss_link: (bool, String), + bus_link: (bool, String), kernel_drops: u64, queue_drops: Arc, queue_drops_seen: u64, @@ -156,6 +163,7 @@ impl Recorder { } let _ = s.event("can_link", json!({ "up": self.can_link.0, "detail": self.can_link.1 })); let _ = s.event("gnss_link", json!({ "up": self.gnss_link.0, "detail": self.gnss_link.1 })); + let _ = s.event("bus_link", json!({ "up": self.bus_link.0, "detail": self.bus_link.1 })); if let Some((rx, line)) = &self.last_tpv { let _ = s.gnss(*rx, line, true); } @@ -192,6 +200,7 @@ impl Recorder { "can": { "up": self.can_link.0, "detail": self.can_link.1, "last_frame_age_s": self.last_frame.map(|t| t.elapsed().as_secs_f64()) }, "gnss": { "up": self.gnss_link.0, "detail": self.gnss_link.1 }, + "bus": { "up": self.bus_link.0, "detail": self.bus_link.1 }, "storage": { "root": self.cfg.root, "used_bytes": storage::usage(&self.cfg.root), "free_bytes": storage::free_bytes(&self.cfg.root), "cap_bytes": self.cfg.bound.cap_bytes }, @@ -282,6 +291,18 @@ impl Recorder { let _ = s.event("gnss_link", json!({ "up": up, "detail": detail })); } } + Event::Vehicle(rx, signal, payload, retained) => { + if let Some(s) = &mut self.session { + let _ = s.vehicle(rx, &signal, &payload, retained); + } + } + Event::BusLink(up, detail) => { + eprintln!("bus: {detail}"); + self.bus_link = (up, detail.clone()); + if let Some(s) = &mut self.session { + let _ = s.event("bus_link", json!({ "up": up, "detail": detail })); + } + } Event::Ctl(cmd, reply) => { let out = self.ctl(&cmd); let _ = reply.send(out); @@ -316,6 +337,9 @@ fn run(cfg: Config) -> ExitCode { if !cfg.gpsd.is_empty() { sources::spawn_gnss(cfg.gpsd.clone(), tx.clone(), stop.clone()); } + if !cfg.broker.is_empty() { + sources::spawn_bus(cfg.broker.clone(), tx.clone(), stop.clone()); + } if let Some(dir) = Path::new(&cfg.ctl).parent() { let _ = std::fs::create_dir_all(dir); } @@ -336,6 +360,7 @@ fn run(cfg: Config) -> ExitCode { last_tpv: None, can_link: (false, "not yet opened".into()), gnss_link: (false, "not yet connected".into()), + bus_link: (false, "not yet connected".into()), kernel_drops: 0, queue_drops, queue_drops_seen: 0, diff --git a/src/recorder/session.rs b/src/recorder/session.rs index ad52c02..1e13a77 100644 --- a/src/recorder/session.rs +++ b/src/recorder/session.rs @@ -6,6 +6,7 @@ //! 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) //! events.ndjson marks, clock steps, link changes, drops //! ``` //! @@ -42,6 +43,7 @@ pub struct Session { chunk_bytes: u64, can: BufWriter, gnss: BufWriter, + vehicle: BufWriter, events: BufWriter, line: String, pub frames: u64, @@ -118,6 +120,7 @@ impl Session { let s = Session { can: append(&dir.join(chunk_name(1)))?, gnss: append(&dir.join("gnss.ndjson"))?, + vehicle: append(&dir.join("vehicle.ndjson"))?, events: append(&dir.join("events.ndjson"))?, dir, name, trigger, meta, iface: iface.to_string(), @@ -156,6 +159,17 @@ impl Session { writeln!(self.gnss, "{v}") } + /// One envelope from the bus. `retained`: the broker's stored copy, delivered on + /// (re)subscribe, which may predate this session. + pub fn vehicle(&mut self, rx: f64, signal: &str, payload: &str, retained: bool) -> io::Result<()> { + let env = serde_json::from_str::(payload).unwrap_or_else(|_| Value::String(payload.to_string())); + let mut v = json!({ "rx": rx, "signal": signal, "env": env }); + if retained { + v["retained"] = json!(true); + } + writeln!(self.vehicle, "{v}") + } + pub fn event(&mut self, kind: &str, mut extra: Value) -> io::Result<()> { if !extra.is_object() { extra = json!({}); @@ -181,6 +195,7 @@ impl Session { pub fn sync(&mut self) -> io::Result<()> { sync(&mut self.can)?; sync(&mut self.gnss)?; + sync(&mut self.vehicle)?; sync(&mut self.events) } diff --git a/src/recorder/sources.rs b/src/recorder/sources.rs index 17c6b80..ac33246 100644 --- a/src/recorder/sources.rs +++ b/src/recorder/sources.rs @@ -21,6 +21,9 @@ pub enum Event { CanLink(bool, String), Gnss(f64, String), GnssLink(bool, String), + /// An envelope on yukart/vehicle/: receive time, signal, payload, retained. + Vehicle(f64, String, String, bool), + BusLink(bool, String), Ctl(String, mpsc::Sender), } @@ -124,6 +127,48 @@ pub fn spawn_gnss(addr: String, tx: SyncSender, stop: Arc) { }).expect("spawn gnss thread"); } +/// Envelopes from the broker, for the record only: raw capture never waits on this. +pub fn spawn_bus(addr: String, tx: SyncSender, stop: Arc) { + let Some((host, port)) = addr.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::().ok()?))) else { + let _ = tx.send(Event::BusLink(false, format!("bad broker address {addr}"))); + return; + }; + thread::Builder::new().name("bus".into()).spawn(move || { + let mut opts = rumqttc::MqttOptions::new("recorder", host, port); + opts.set_keep_alive(Duration::from_secs(5)); + let (client, mut connection) = rumqttc::Client::new(opts, 64); + let mut net = rumqttc::NetworkOptions::new(); + net.set_tcp_nodelay(true); + connection.eventloop.set_network_options(net); + let mut up = false; + for ev in connection.iter() { + if stop.load(Ordering::Relaxed) { + return; + } + match ev { + 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/+"))); + } + Ok(rumqttc::Event::Incoming(rumqttc::Packet::Publish(p))) => { + let signal = p.topic.rsplit('/').next().unwrap_or_default().to_string(); + let payload = String::from_utf8_lossy(&p.payload).into_owned(); + // Never block the event loop on the writer; CAN frames have priority in the queue. + let _ = tx.try_send(Event::Vehicle(clock::realtime(), signal, payload, p.retain)); + } + Ok(_) => {} + Err(e) => { + if std::mem::take(&mut up) { + let _ = tx.send(Event::BusLink(false, format!("{addr}: {e}"))); + } + thread::sleep(Duration::from_secs(1)); + } + } + } + }).expect("spawn bus thread"); +} + /// One command per connection, one reply line back. pub fn spawn_ctl(path: String, tx: SyncSender) -> std::io::Result<()> { let _ = std::fs::remove_file(&path);