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,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<Value> {
|
||||
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<Vec<Layout>, 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<Vec<Layout>, 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());
|
||||
}
|
||||
}
|
||||
@@ -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)));
|
||||
}
|
||||
}
|
||||
@@ -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<u8>,
|
||||
}
|
||||
|
||||
pub struct RxSocket {
|
||||
fd: i32,
|
||||
}
|
||||
|
||||
impl Drop for RxSocket {
|
||||
fn drop(&mut self) {
|
||||
unsafe { libc::close(self.fd) };
|
||||
}
|
||||
}
|
||||
|
||||
fn setopt<T>(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<Self> {
|
||||
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<libc::can_filter> = 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::<libc::can_filter>())?;
|
||||
setopt(fd, libc::SOL_SOCKET, libc::SO_TIMESTAMP, &1i32, size_of::<i32>())?;
|
||||
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::<libc::timeval>())?;
|
||||
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::<libc::sockaddr_can>() 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<Option<Frame>> {
|
||||
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::<libc::can_frame>() };
|
||||
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() }))
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user