From 54171aaf273f14af0f01981bd7e409757d76444c Mon Sep 17 00:00:00 2001 From: Mikkeli Date: Tue, 13 Oct 2026 21:31:52 +0900 Subject: [PATCH] decoder: broadcast frames to yukart/vehicle/* from contracts' can blocks Kernel-filtered read-only socket; booleans on change, numbers <= 20/s, every live value refreshed once a second, stale after 1 s of silence. --replay runs a candump log through the same path, --list prints the layouts. obd-poller drops engine_rpm, now a z27ag signal. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz --- Cargo.toml | 6 +- README.md | 8 +- deploy/yukart-decoder.service | 16 ++ scripts/deploy-decoder.sh | 42 ++++ src/decoder/layout.rs | 152 +++++++++++++ src/decoder/main.rs | 393 ++++++++++++++++++++++++++++++++++ src/decoder/socket.rs | 96 +++++++++ src/obd-poller/pids.rs | 2 - 8 files changed, 710 insertions(+), 5 deletions(-) create mode 100644 deploy/yukart-decoder.service create mode 100755 scripts/deploy-decoder.sh create mode 100644 src/decoder/layout.rs create mode 100644 src/decoder/main.rs create mode 100644 src/decoder/socket.rs diff --git a/Cargo.toml b/Cargo.toml index 0ef6ef6..d6d8a13 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, orin-status and obd-poller now; can-bridge, gnss, decoder later" +description = "Acquisition and processing cluster: recorder, decoder, orin-status and obd-poller now; can-bridge and gnss later" publish = false [[bin]] @@ -17,6 +17,10 @@ path = "src/orin-status/main.rs" name = "obd-poller" path = "src/obd-poller/main.rs" +[[bin]] +name = "decoder" +path = "src/decoder/main.rs" + [[bin]] name = "bus-bench" path = "src/bus-bench/main.rs" diff --git a/README.md b/README.md index c0bbd47..ac45f2c 100644 --- a/README.md +++ b/README.md @@ -10,19 +10,23 @@ 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`, `orin-status` and `obd-poller` now; `can-bridge`, `gnss` and `decoder` as they are +unit: `recorder`, `decoder`, `orin-status` and `obd-poller` now; `can-bridge` and `gnss` as they are needed. | Binary | Installed as | Does | |---|---|---| | `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound; OBD-II values from the bus beside them | +| `decoder` | `yukart-decoder.service` | The car's own broadcast frames to named signals (`yukart/vehicle/*`): every `z27ag` signal in contracts' `signals.json`, laid out by its `can` block. Booleans on change, numbers ≤ 20/s, each refreshed once a second; reads only | | `bus-bench` | not installed: a test tool | Raw CAN and decoded-signal load over MQTT, measured: loss, latency, broker CPU ([report](docs/impl/reports/BUS-BENCH.md)) | -| `obd-poller` | `yukart-obd-poller.service` | OBD-II mode 01 polling, the single CAN writer: rpm, speed, throttle, load, MAP, MAF ~10 Hz; temperatures and fuel 1 Hz (`yukart/vehicle/*`) | +| `obd-poller` | `yukart-obd-poller.service` | OBD-II mode 01 polling, the single CAN writer; **disabled**, the Z27AG has no OBD-II on CAN: 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 (with SSID)/LTE/tablet links, the CAN interface, 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-decoder.sh # same, for the decoder (after any signals.json change) +yukart-decoder --list # the signals it decodes, and where +zstdcat can-*.log.zst | yukart-decoder --replay # what it would publish for a recording 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 diff --git a/deploy/yukart-decoder.service b/deploy/yukart-decoder.service new file mode 100644 index 0000000..f188026 --- /dev/null +++ b/deploy/yukart-decoder.service @@ -0,0 +1,16 @@ +[Unit] +Description=Yukart decoder: the car's broadcast CAN frames to named signals on the bus +Documentation=gitea:yukart/svc-core +# Tier 2. Ordered after the broker and nothing else: the adapter comes and goes, and +# the decoder reopens the interface by itself. It only reads the bus. +After=mosquitto.service +StartLimitIntervalSec=0 + +[Service] +User=mikkeli +ExecStart=/usr/local/bin/yukart-decoder +Restart=always +RestartSec=2 + +[Install] +WantedBy=multi-user.target diff --git a/scripts/deploy-decoder.sh b/scripts/deploy-decoder.sh new file mode 100755 index 0000000..619bbf4 --- /dev/null +++ b/scripts/deploy-decoder.sh @@ -0,0 +1,42 @@ +#!/usr/bin/env bash +# Build the decoder natively on the development Orin, check it against the car's +# glibc, and install it on the car Orin as the yukart-decoder service. +# +# usage: scripts/deploy-decoder.sh [host] (default: mikkeli-orin-nano) +# env: BUILD_HOST (default: mikkeli-orin-nano-2) +# +# Builds what is pushed, not the working tree: the build host pulls master of +# svc-core and contracts under ~/dev/yukart/; the signal layouts are compiled in +# from contracts' signals.json, so a registry change needs a redeploy. Native like +# orin-status, on the preferred build host (check-glibc.sh gates it either way). +set -euo pipefail + +HOST="${1:-mikkeli-orin-nano}" +BUILD_HOST="${BUILD_HOST:-mikkeli-orin-nano-2}" +cd "$(dirname "$0")/.." +BIN="$(mktemp -d)/decoder" + +ssh "$BUILD_HOST" 'set -e + mkdir -p ~/dev/yukart && cd ~/dev/yukart + for r in svc-core contracts; do + [ -d $r ] || git clone -q http://192.168.2.199:3005/yukart/$r.git + git -C $r pull -q --ff-only + done + cd svc-core && ~/.cargo/bin/cargo build --release --bin decoder 2>&1 | tail -1 + git log --oneline -1' +scp -q "$BUILD_HOST:dev/yukart/svc-core/target/release/decoder" "$BIN" +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/decoder.new" +scp -q deploy/yukart-decoder.service "$HOST:infra/svc-core/yukart-decoder.service" +ssh "$HOST" 'set -e + mv ~/infra/svc-core/decoder.new ~/infra/svc-core/decoder + sudo ln -sfn "$HOME/infra/svc-core/decoder" /usr/local/bin/yukart-decoder + sudo install -m 0644 ~/infra/svc-core/yukart-decoder.service /etc/systemd/system/yukart-decoder.service + sudo systemctl daemon-reload + sudo systemctl enable yukart-decoder.service >/dev/null + sudo systemctl restart yukart-decoder.service + sleep 1 + systemctl --no-pager --lines=5 status yukart-decoder.service' diff --git a/src/decoder/layout.rs b/src/decoder/layout.rs new file mode 100644 index 0000000..aaabea0 --- /dev/null +++ b/src/decoder/layout.rs @@ -0,0 +1,152 @@ +//! Where each decoded signal sits in its frame, read from contracts' `signals.json` +//! (`source: z27ag`, the `can` block). Nothing here names an ID or a bit: a layout +//! corrected in the registry is corrected here at the next build (contracts guideline 6). + +use serde_json::Value; + +#[derive(Debug, Clone, PartialEq)] +pub enum Kind { + Bool, + Number, +} + +#[derive(Debug, Clone)] +pub struct Layout { + pub signal: String, + pub id: u32, + pub byte: usize, + /// 1, 8 or 16. + pub length: u32, + /// Length 1 only: 0 = LSB of `byte`. + pub bit: u32, + /// Length 16 only: true = `byte` is the most significant. + pub big: bool, + pub scale: f64, + pub offset: f64, + pub kind: Kind, +} + +impl Layout { + /// Bytes the frame must carry for this signal to be read. + pub fn needs(&self) -> usize { + self.byte + (self.length as usize + 7) / 8 + } + + /// The signal's value from a frame's data, or None when the frame is too short. + pub fn value(&self, data: &[u8]) -> Option { + if data.len() < self.needs() { + return None; + } + let raw: u32 = match self.length { + 1 => (data[self.byte] as u32 >> self.bit) & 1, + 8 => data[self.byte] as u32, + _ => { + let (a, b) = (data[self.byte] as u32, data[self.byte + 1] as u32); + if self.big { a << 8 | b } else { b << 8 | a } + } + }; + Some(match self.kind { + Kind::Bool => Value::Bool(raw != 0), + Kind::Number => { + let v = raw as f64 * self.scale + self.offset; + // Three decimals: 0.4 × 98 should read 39.2, not 39.200000000000003. + let v = (v * 1000.0).round() / 1000.0; + serde_json::Number::from_f64(v).map(Value::Number).unwrap_or(Value::Null) + } + }) + } +} + +/// Every `z27ag` signal's layout. Errors name the signal and what is wrong with it. +pub fn load(registry: &str) -> Result, String> { + let v: Value = serde_json::from_str(registry).map_err(|e| format!("signals.json: {e}"))?; + let sigs = v["signals"].as_object().ok_or("signals.json has no signals object")?; + let mut out = Vec::new(); + for (name, s) in sigs { + if s["source"] != "z27ag" { + continue; + } + let bad = |what: &str| format!("signal {name}: {what}"); + let c = s.get("can").ok_or_else(|| bad("source z27ag but no can block"))?; + let id = c["id"] + .as_str() + .and_then(|h| u32::from_str_radix(h.trim_start_matches("0x"), 16).ok()) + .filter(|&i| i <= 0x7FF) + .ok_or_else(|| bad("can.id must be an 11-bit hex string"))?; + let byte = c["byte"].as_u64().filter(|&b| b < 8).ok_or_else(|| bad("can.byte must be 0-7"))? as usize; + let length = c["length"].as_u64().ok_or_else(|| bad("can.length missing"))? as u32; + let (bit, big) = match length { + 1 => (c["bit"].as_u64().filter(|&b| b < 8).ok_or_else(|| bad("length 1 needs can.bit 0-7"))? as u32, true), + 8 => (0, true), + 16 => match c["order"].as_str() { + Some("big") => (0, true), + Some("little") => (0, false), + _ => return Err(bad("length 16 needs can.order big or little")), + }, + _ => return Err(bad("can.length must be 1, 8 or 16")), + }; + let kind = match s["type"].as_str() { + Some("boolean") if length == 1 => Kind::Bool, + Some("number") if length > 1 => Kind::Number, + _ => return Err(bad("type must be boolean for length 1, number otherwise")), + }; + let l = Layout { + signal: name.clone(), + id, + byte, + length, + bit, + big, + scale: c["scale"].as_f64().unwrap_or(1.0), + offset: c["offset"].as_f64().unwrap_or(0.0), + kind, + }; + if l.needs() > 8 { + return Err(bad("runs past byte 7")); + } + out.push(l); + } + Ok(out) +} + +#[cfg(test)] +mod tests { + use super::*; + use serde_json::json; + + const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json")); + + fn one(can: Value, typ: &str) -> Result, String> { + load(&json!({ "signals": { "x": { "unit": "", "type": typ, "source": "z27ag", "can": can } } }).to_string()) + } + + #[test] + fn the_registry_loads() { + let l = load(REGISTRY).expect("every z27ag signal has a valid can block"); + assert!(l.iter().any(|l| l.signal == "engine_rpm")); + } + + #[test] + fn values() { + let rpm = &one(json!({"id": "0x210", "byte": 4, "length": 16, "order": "big"}), "number").unwrap()[0]; + assert_eq!(rpm.value(&[0, 0x60, 8, 0x34, 0x03, 0x63]), Some(json!(867.0))); + assert_eq!(rpm.value(&[0, 0x60, 8, 0x34, 0x03]), None); + let le = &one(json!({"id": "0x235", "byte": 0, "length": 16, "order": "little", "scale": 0.05}), "number").unwrap()[0]; + assert_eq!(le.value(&[0x4C, 0]), Some(json!(3.8))); + let pedal = &one(json!({"id": "0x210", "byte": 0, "length": 8, "scale": 0.4}), "number").unwrap()[0]; + assert_eq!(pedal.value(&[0xFA]), Some(json!(100.0))); + assert_eq!(pedal.value(&[98]), Some(json!(39.2))); + let brake = &one(json!({"id": "0x231", "byte": 4, "bit": 1, "length": 1}), "boolean").unwrap()[0]; + assert_eq!(brake.value(&[0, 0, 0, 0, 0x02]), Some(json!(true))); + assert_eq!(brake.value(&[0, 0, 0, 0, 0xFD]), Some(json!(false))); + } + + #[test] + fn bad_layouts_are_refused() { + assert!(one(json!({"id": "0x210", "byte": 4, "length": 16}), "number").is_err()); // no order + assert!(one(json!({"id": "0x210", "byte": 7, "length": 16, "order": "big"}), "number").is_err()); + assert!(one(json!({"id": "0x231", "byte": 4, "length": 1}), "boolean").is_err()); // no bit + assert!(one(json!({"id": "0x231", "byte": 4, "bit": 1, "length": 1}), "number").is_err()); + assert!(one(json!({"id": "0x800", "byte": 0, "length": 8}), "number").is_err()); + } +} diff --git a/src/decoder/main.rs b/src/decoder/main.rs new file mode 100644 index 0000000..be8a699 --- /dev/null +++ b/src/decoder/main.rs @@ -0,0 +1,393 @@ +//! decoder — the car's own broadcast frames to named signals on the bus. +//! +//! Every `source: z27ag` signal in contracts' `signals.json` is decoded from its +//! `can` block and published on `yukart/vehicle/`, retained, as an envelope. +//! The registry is the only place a layout is written (contracts guideline 6); this +//! service has no IDs of its own. It only reads the bus. +//! +//! Booleans are published on every change, so an indicator's flashes come through +//! edge for edge. Numbers are published on change, at most every 50 ms (20/s). +//! Every live value is also republished once a second, so a steady one (the handbrake +//! on) never looks stale to svc-gw, which judges staleness by arrival. A +//! signal whose frame stops for a second is republished once with `quality: stale`; +//! the next frame makes it ok again. The service's own state goes on +//! `yukart/system/decoder`, with a last will for when it vanishes. +//! +//! `--replay` reads a `candump -L` log on stdin and prints what would be published, +//! with the log's own times: the same code path, without a car. + +mod layout; +mod socket; + +use std::collections::HashMap; +use std::io::BufRead; +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, QoS}; +use serde_json::{json, Value}; + +const USAGE: &str = "\ +usage: decoder [options] + --broker HOST:PORT MQTT broker (127.0.0.1:1883) + --can IFACE CAN interface to read (can_pcan) + --replay decode a candump -L log from stdin, print what would be + published as JSON lines, and exit (no broker, no bus) + --list print the decoded signals and their layouts, and exit"; + +const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json")); +const SYSTEM_TOPIC: &str = "yukart/system/decoder"; +/// Numbers are published at most this often (µs). +const NUMBER_EVERY_US: i64 = 50_000; +/// A frame silent this long makes its signals stale (µs). +const STALE_AFTER_US: i64 = 1_000_000; +/// A live value unchanged this long is republished (µs). +const REFRESH_US: i64 = 1_000_000; + +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 by signal name, 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() + .map(|(k, s)| (k.clone(), s["unit"].as_str().unwrap_or("").to_string())) + .collect() +} + +/// One value to publish: signal index, value, stale or not, time of the frame (µs). +struct Out { + sig: usize, + value: Value, + stale: bool, + ts_us: i64, +} + +struct Sig { + layout: layout::Layout, + last: Option, + last_pub_us: i64, + pending: Option<(Value, i64)>, + seen_us: i64, + stale: bool, +} + +/// The publishing policy, separate from where frames come from and where values go. +struct Decoder { + sigs: Vec, + by_id: HashMap>, +} + +impl Decoder { + fn new(layouts: Vec) -> Self { + let mut by_id: HashMap> = HashMap::new(); + for (i, l) in layouts.iter().enumerate() { + by_id.entry(l.id).or_default().push(i); + } + let sigs = layouts + .into_iter() + .map(|layout| Sig { layout, last: None, last_pub_us: 0, pending: None, seen_us: 0, stale: false }) + .collect(); + Decoder { sigs, by_id } + } + + fn ids(&self) -> Vec { + let mut v: Vec = self.by_id.keys().copied().collect(); + v.sort(); + v + } + + fn frame(&mut self, id: u32, data: &[u8], ts_us: i64, out: &mut Vec) { + let Some(idx) = self.by_id.get(&id) else { return }; + for &i in idx { + let s = &mut self.sigs[i]; + let Some(v) = s.layout.value(data) else { continue }; + s.seen_us = ts_us; + let changed = s.last.as_ref() != Some(&v); + let due = s.layout.kind == layout::Kind::Bool || ts_us - s.last_pub_us >= NUMBER_EVERY_US; + if s.stale || (changed && due) { + s.stale = false; + s.pending = None; + s.last = Some(v.clone()); + s.last_pub_us = ts_us; + out.push(Out { sig: i, value: v, stale: false, ts_us }); + } else if changed { + s.pending = Some((v, ts_us)); + } else { + s.pending = None; // changed and changed back inside the window + } + } + } + + /// Throttled values that are now due, and signals whose frames went silent. + fn tick(&mut self, now_us: i64, out: &mut Vec) { + for (i, s) in self.sigs.iter_mut().enumerate() { + if let Some((v, ts)) = s.pending.take() { + if now_us - s.last_pub_us >= NUMBER_EVERY_US { + s.last = Some(v.clone()); + s.last_pub_us = now_us; + out.push(Out { sig: i, value: v, stale: false, ts_us: ts }); + } else { + s.pending = Some((v, ts)); + } + } + if !s.stale && s.seen_us > 0 && now_us - s.seen_us > STALE_AFTER_US { + s.stale = true; + s.pending = None; + if let Some(v) = &s.last { + out.push(Out { sig: i, value: v.clone(), stale: true, ts_us: s.seen_us }); + } + } else if !s.stale && s.pending.is_none() && now_us - s.last_pub_us >= REFRESH_US { + if let Some(v) = &s.last { + s.last_pub_us = now_us; + out.push(Out { sig: i, value: v.clone(), stale: false, ts_us: s.seen_us }); + } + } + } + } +} + +fn layouts() -> Vec { + match layout::load(REGISTRY) { + Ok(l) => l, + Err(e) => panic!("contracts signals.json: {e}"), + } +} + +fn replay() -> ExitCode { + let mut d = Decoder::new(layouts()); + let mut out = Vec::new(); + let names: Vec = d.sigs.iter().map(|s| s.layout.signal.clone()).collect(); + let mut last_tick = 0i64; + for line in std::io::stdin().lock().lines() { + let Ok(line) = line else { break }; + // (1791878450.945008) can_pcan 139#3333230044040000 + let mut f = line.split_whitespace(); + let (Some(ts), Some(_), Some(frame)) = (f.next(), f.next(), f.next()) else { continue }; + let Some(ts) = ts.trim_matches(|c| c == '(' || c == ')').parse::().ok() else { continue }; + let Some((id, hex)) = frame.split_once('#') else { continue }; + let (Ok(id), Some(data)) = (u32::from_str_radix(id, 16), (0..hex.len() / 2).map(|i| u8::from_str_radix(&hex[2 * i..2 * i + 2], 16).ok()).collect::>>()) else { continue }; + let ts_us = (ts * 1e6) as i64; + d.frame(id, &data, ts_us, &mut out); + if ts_us - last_tick >= 10_000 { + d.tick(ts_us, &mut out); + last_tick = ts_us; + } + for o in out.drain(..) { + let mut e = json!({ "t": o.ts_us as f64 / 1e6, "signal": names[o.sig], "value": o.value }); + if o.stale { + e["quality"] = json!("stale"); + } + println!("{e}"); + } + } + ExitCode::SUCCESS +} + +fn main() -> ExitCode { + let mut broker = "127.0.0.1:1883".to_string(); + let mut iface = "can_pcan".to_string(); + let mut args = std::env::args().skip(1); + while let Some(a) = args.next() { + match a.as_str() { + "--replay" => return replay(), + "--list" => { + for l in layouts() { + println!("{:<20} 0x{:03X} byte {} {}", l.signal, l.id, l.byte, match l.length { + 1 => format!("bit {}", l.bit), + n => format!("{n} bits{} ×{} +{}", if n == 16 { if l.big { " big" } else { " little" } } else { "" }, l.scale, l.offset), + }); + } + return ExitCode::SUCCESS; + } + "--broker" => broker = args.next().unwrap_or_default(), + "--can" => iface = args.next().unwrap_or_default(), + _ => { + eprintln!("decoder: unknown option {a}\n{USAGE}"); + return ExitCode::from(2); + } + } + } + + let units = units(); + let mut d = Decoder::new(layouts()); + let names: Vec = d.sigs.iter().map(|s| s.layout.signal.clone()).collect(); + let topics: Vec = names.iter().map(|n| format!("yukart/vehicle/{n}")).collect(); + let unit_of: Vec = names + .iter() + .map(|n| units.get(n).unwrap_or_else(|| panic!("signal {n} is not in contracts signals.json")).clone()) + .collect(); + + let Some((host, port)) = broker.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::().ok()?))) + else { + eprintln!("decoder: --broker must be HOST:PORT\n{USAGE}"); + return ExitCode::from(2); + }; + let mut opts = MqttOptions::new("decoder", 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, 1024); + + // The event loop runs on its own thread and reconnects forever; publishing never waits on it. + 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!("decoder: connected to the broker"); + connected.store(true, Ordering::Relaxed); + announce.store(true, Ordering::Relaxed); + } + Ok(_) => {} + Err(e) => { + if connected.swap(false, Ordering::Relaxed) { + eprintln!("decoder: broker: {e}; retrying"); + } + thread::sleep(Duration::from_secs(1)); + } + } + } + }); + } + + eprintln!("decoder {} on {iface}: {} signals from {} frame IDs, publishing to {broker}", env!("CARGO_PKG_VERSION"), names.len(), d.ids().len()); + let ids = d.ids(); + let mut sock: Option = None; + let mut out = Vec::new(); + let mut last_state = String::new(); + let mut last_announce = Instant::now(); + let mut last_open_try = Instant::now() - Duration::from_secs(10); + let mut last_tick = Instant::now(); + let mut last_frame_us = 0i64; + loop { + if sock.is_none() && last_open_try.elapsed() >= Duration::from_secs(2) { + last_open_try = Instant::now(); + match socket::RxSocket::open(&iface, &ids, Duration::from_millis(100)) { + Ok(s) => { + eprintln!("decoder: {iface} open"); + sock = Some(s); + } + Err(_) => {} // absent or down: the state says so, retried every 2 s + } + } + match sock.as_ref().map(|s| s.recv()) { + Some(Ok(Some(f))) => { + last_frame_us = f.ts_us; + d.frame(f.id, &f.data, f.ts_us, &mut out); + } + Some(Ok(None)) => {} + Some(Err(e)) => { + eprintln!("decoder: {iface}: {e}; reopening"); + sock = None; + } + None => thread::sleep(Duration::from_millis(100)), + } + if last_tick.elapsed() >= Duration::from_millis(10) { + last_tick = Instant::now(); + d.tick(now_us(), &mut out); + } + for o in out.drain(..) { + let mut e = json!({ "timestamp": o.ts_us, "monotonic": mono_ns(), "value": o.value, "unit": unit_of[o.sig] }); + if o.stale { + e["quality"] = json!("stale"); + } + // try_publish: a full queue drops a value rather than stalling the reads. + let _ = client.try_publish(&topics[o.sig], QoS::AtMostOnce, true, e.to_string()); + } + + if connected.load(Ordering::Relaxed) { + let state = if sock.is_none() { + json!({ "state": "degraded", "detail": format!("{iface} absent or down") }) + } else if now_us() - last_frame_us > STALE_AFTER_US { + json!({ "state": "degraded", "detail": "bus silent" }) + } else { + json!({ "state": "ready" }) + } + .to_string(); + if state != last_state || announce.swap(false, Ordering::Relaxed) || last_announce.elapsed() > Duration::from_secs(60) { + if state != last_state { + eprintln!("decoder: {state}"); + } + let _ = client.try_publish(SYSTEM_TOPIC, QoS::AtLeastOnce, true, state.clone()); + last_state = state; + last_announce = Instant::now(); + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn dec() -> Decoder { + Decoder::new(layout::load(REGISTRY).unwrap()) + } + + fn named<'a>(d: &'a Decoder, out: &'a [Out]) -> Vec<(&'a str, Value, bool)> { + out.iter().map(|o| (d.sigs[o.sig].layout.signal.as_str(), o.value.clone(), o.stale)).collect() + } + + #[test] + fn numbers_are_throttled_and_flushed() { + let mut d = dec(); + let id = d.sigs.iter().find(|s| s.layout.signal == "engine_rpm").unwrap().layout.id; + let mut out = Vec::new(); + let rpm = |r: u16| { + let mut f = [0u8; 8]; + let s = d.sigs.iter().find(|s| s.layout.signal == "engine_rpm").unwrap(); + let b = if s.layout.big { r.to_be_bytes() } else { r.to_le_bytes() }; + f[s.layout.byte..s.layout.byte + 2].copy_from_slice(&b); + f + }; + let (a, b) = (rpm(800), rpm(820)); + d.frame(id, &a, 1_000_000, &mut out); + d.frame(id, &b, 1_020_000, &mut out); // 20 ms later: held back + assert_eq!(out.iter().filter(|o| d.sigs[o.sig].layout.signal == "engine_rpm").count(), 1); + out.clear(); + d.tick(1_060_000, &mut out); + assert!(named(&d, &out).contains(&("engine_rpm", json!(820.0), false))); + } + + #[test] + fn silence_makes_stale_and_a_frame_makes_ok() { + let mut d = dec(); + let s = d.sigs.iter().find(|s| s.layout.signal == "brake_pedal").unwrap().layout.clone(); + let mut f = [0u8; 8]; + f[s.byte] = 1 << s.bit; + let mut out = Vec::new(); + d.frame(s.id, &f, 1_000_000, &mut out); + d.frame(s.id, &f, 1_600_000, &mut out); + out.clear(); + d.tick(2_000_100, &mut out); // frames still coming, a second unchanged: republished + assert!(named(&d, &out).contains(&("brake_pedal", json!(true), false))); + out.clear(); + d.tick(2_700_000, &mut out); // then silent past a second: stale + assert!(named(&d, &out).contains(&("brake_pedal", json!(true), true))); + out.clear(); + d.frame(s.id, &f, 2_800_000, &mut out); + assert!(named(&d, &out).contains(&("brake_pedal", json!(true), false))); + } +} diff --git a/src/decoder/socket.rs b/src/decoder/socket.rs new file mode 100644 index 0000000..7127238 --- /dev/null +++ b/src/decoder/socket.rs @@ -0,0 +1,96 @@ +//! A receive-only SocketCAN socket, filtered in the kernel to the frame IDs the +//! registry decodes: the other frames of the bus's ~900/s never reach this process. +//! It never writes (guideline 1: obd-poller is the only CAN writer). + +use std::ffi::CString; +use std::io; +use std::mem::{size_of, zeroed}; +use std::time::Duration; + +pub struct Frame { + /// CLOCK_REALTIME at reception, microseconds. + pub ts_us: i64, + pub id: u32, + pub data: Vec, +} + +pub struct RxSocket { + fd: i32, +} + +impl Drop for RxSocket { + fn drop(&mut self) { + unsafe { libc::close(self.fd) }; + } +} + +fn setopt(fd: i32, level: i32, name: i32, val: *const T, len: usize) -> io::Result<()> { + let r = unsafe { libc::setsockopt(fd, level, name, val as *const libc::c_void, len as u32) }; + if r < 0 { Err(io::Error::last_os_error()) } else { Ok(()) } +} + +impl RxSocket { + pub fn open(iface: &str, ids: &[u32], timeout: Duration) -> 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 = RxSocket { fd }; + // Exact standard IDs only: no extended, no remote frames. + let mask = libc::CAN_SFF_MASK | libc::CAN_EFF_FLAG | libc::CAN_RTR_FLAG; + let filters: Vec = ids.iter().map(|&id| libc::can_filter { can_id: id, can_mask: mask }).collect(); + setopt(fd, libc::SOL_CAN_RAW, libc::CAN_RAW_FILTER, filters.as_ptr(), filters.len() * size_of::())?; + setopt(fd, libc::SOL_SOCKET, libc::SO_TIMESTAMP, &1i32, size_of::())?; + let tv = libc::timeval { tv_sec: timeout.as_secs() as _, tv_usec: timeout.subsec_micros().max(1) as _ }; + setopt(fd, libc::SOL_SOCKET, libc::SO_RCVTIMEO, &tv, size_of::())?; + 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) + } + + /// The next frame within the socket's timeout, or None. An error means the + /// interface went away (or down): reopen. + pub fn recv(&self) -> io::Result> { + 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); + } + } + let len = (frame.can_dlc as usize).min(8); + Ok(Some(Frame { ts_us, id: frame.can_id & libc::CAN_SFF_MASK, data: frame.data[..len].to_vec() })) + } +} diff --git a/src/obd-poller/pids.rs b/src/obd-poller/pids.rs index 4a1d570..0e7b2eb 100644 --- a/src/obd-poller/pids.rs +++ b/src/obd-poller/pids.rs @@ -16,7 +16,6 @@ fn ab(d: &[u8]) -> 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 }, @@ -43,7 +42,6 @@ mod tests { #[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);