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 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01DDwfd6Ki8UfG4hK6W5YpJz
This commit is contained in:
@@ -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/<signal>`, 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<String, String> {
|
||||
let v: Value = serde_json::from_str(REGISTRY).expect("contracts signals.json is valid JSON");
|
||||
v["signals"]
|
||||
.as_object()
|
||||
.expect("signals.json has a signals object")
|
||||
.iter()
|
||||
.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<Value>,
|
||||
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<Sig>,
|
||||
by_id: HashMap<u32, Vec<usize>>,
|
||||
}
|
||||
|
||||
impl Decoder {
|
||||
fn new(layouts: Vec<layout::Layout>) -> Self {
|
||||
let mut by_id: HashMap<u32, Vec<usize>> = 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<u32> {
|
||||
let mut v: Vec<u32> = self.by_id.keys().copied().collect();
|
||||
v.sort();
|
||||
v
|
||||
}
|
||||
|
||||
fn frame(&mut self, id: u32, data: &[u8], ts_us: i64, out: &mut Vec<Out>) {
|
||||
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<Out>) {
|
||||
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<layout::Layout> {
|
||||
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<String> = 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::<f64>().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::<Option<Vec<u8>>>()) 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<String> = d.sigs.iter().map(|s| s.layout.signal.clone()).collect();
|
||||
let topics: Vec<String> = names.iter().map(|n| format!("yukart/vehicle/{n}")).collect();
|
||||
let unit_of: Vec<String> = 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::<u16>().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<socket::RxSocket> = 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)));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user