obd-poller over the K-line via the Bluetooth ELM327; gnss publisher; elm_connected
obd-poller drops its CAN socket: the Z27AG answers OBD-II only on ISO 9141-2. It holds a raw RFCOMM link to the ELM (no pairing, no root), fixes the protocol (ATSP3) so nothing probes CAN, publishes the obd_* signals, and releases the adapter after a minute of ECU silence so it can sleep. gnss: gpsd's fix as yukart/gnss/<signal>. orin-status: elm_connected from BlueZ. One native deploy script (deploy-native.sh BIN) behind the per-service ones; /etc/default/yukart-elm holds the adapter's address. 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,177 @@
|
||||
//! An ELM327 over classic Bluetooth serial (RFCOMM): the Orin's only way onto the
|
||||
//! car's K-line (project-yukart decision 30). No pairing, no `rfcomm bind`, no root:
|
||||
//! a plain socket to the adapter's serial channel.
|
||||
//!
|
||||
//! The ELM speaks text: a command, `\r`, then the reply and a `>` prompt. Its
|
||||
//! protocol is fixed at start (ISO 9141-2 by default), so it never runs the
|
||||
//! auto-search, which would send OBD requests onto the CAN bus first.
|
||||
|
||||
use std::io::{self, Read, Write};
|
||||
use std::mem::size_of;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
const AF_BLUETOOTH: i32 = 31;
|
||||
const BTPROTO_RFCOMM: i32 = 3;
|
||||
|
||||
#[repr(C)]
|
||||
struct SockaddrRc {
|
||||
family: u16,
|
||||
/// Little-endian: the address's last byte first.
|
||||
bdaddr: [u8; 6],
|
||||
channel: u8,
|
||||
}
|
||||
|
||||
/// "00:0C:BF:50:AF:3F" → the kernel's byte order.
|
||||
pub fn parse_addr(s: &str) -> Option<[u8; 6]> {
|
||||
let b: Vec<u8> = s.split(':').map(|x| u8::from_str_radix(x, 16).ok()).collect::<Option<_>>()?;
|
||||
let mut a: [u8; 6] = b.try_into().ok()?;
|
||||
a.reverse();
|
||||
Some(a)
|
||||
}
|
||||
|
||||
struct Rfcomm {
|
||||
fd: i32,
|
||||
}
|
||||
|
||||
impl Drop for Rfcomm {
|
||||
fn drop(&mut self) {
|
||||
unsafe { libc::close(self.fd) };
|
||||
}
|
||||
}
|
||||
|
||||
fn timeo(fd: i32, opt: i32, t: Duration) -> io::Result<()> {
|
||||
let tv = libc::timeval { tv_sec: t.as_secs() as _, tv_usec: t.subsec_micros() as _ };
|
||||
let r = unsafe {
|
||||
libc::setsockopt(fd, libc::SOL_SOCKET, opt, &tv as *const _ as *const libc::c_void, size_of::<libc::timeval>() as u32)
|
||||
};
|
||||
if r < 0 { Err(io::Error::last_os_error()) } else { Ok(()) }
|
||||
}
|
||||
|
||||
impl Rfcomm {
|
||||
fn connect(bdaddr: [u8; 6], channel: u8, timeout: Duration) -> io::Result<Self> {
|
||||
let fd = unsafe { libc::socket(AF_BLUETOOTH, libc::SOCK_STREAM | libc::SOCK_CLOEXEC, BTPROTO_RFCOMM) };
|
||||
if fd < 0 {
|
||||
return Err(io::Error::last_os_error());
|
||||
}
|
||||
let s = Rfcomm { fd };
|
||||
timeo(fd, libc::SO_SNDTIMEO, timeout)?; // bounds connect() too
|
||||
let addr = SockaddrRc { family: AF_BLUETOOTH as u16, bdaddr, channel };
|
||||
let r = unsafe { libc::connect(fd, &addr as *const _ as *const libc::sockaddr, size_of::<SockaddrRc>() as u32) };
|
||||
if r < 0 {
|
||||
return Err(io::Error::last_os_error());
|
||||
}
|
||||
Ok(s)
|
||||
}
|
||||
}
|
||||
|
||||
impl Read for Rfcomm {
|
||||
fn read(&mut self, buf: &mut [u8]) -> io::Result<usize> {
|
||||
let n = unsafe { libc::read(self.fd, buf.as_mut_ptr() as *mut libc::c_void, buf.len()) };
|
||||
if n < 0 { Err(io::Error::last_os_error()) } else { Ok(n as usize) }
|
||||
}
|
||||
}
|
||||
|
||||
impl Write for Rfcomm {
|
||||
fn write(&mut self, buf: &[u8]) -> io::Result<usize> {
|
||||
let n = unsafe { libc::write(self.fd, buf.as_ptr() as *const libc::c_void, buf.len()) };
|
||||
if n < 0 { Err(io::Error::last_os_error()) } else { Ok(n as usize) }
|
||||
}
|
||||
fn flush(&mut self) -> io::Result<()> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
pub struct Elm {
|
||||
link: Rfcomm,
|
||||
/// What `ATI` said, e.g. "ELM327 v2.2".
|
||||
pub version: String,
|
||||
}
|
||||
|
||||
/// The data bytes of a mode 01 reply to `pid` in the ELM's text (spaces off, headers
|
||||
/// off): the first line reading `41<pid>…`. K-line replies can be preceded by
|
||||
/// "SEARCHING..." or "BUS INIT: ...OK", and several ECUs may answer; the first wins.
|
||||
pub fn parse_reply(text: &str, pid: u8) -> Option<Vec<u8>> {
|
||||
let want = format!("41{pid:02X}");
|
||||
text.lines().map(|l| l.trim().replace(' ', "")).find(|l| l.starts_with(&want) && l.len() % 2 == 0).and_then(|l| {
|
||||
(want.len()..l.len()).step_by(2).map(|i| u8::from_str_radix(&l[i..i + 2], 16).ok()).collect()
|
||||
})
|
||||
}
|
||||
|
||||
impl Elm {
|
||||
/// Connects and sets the adapter up: echo, linefeeds, spaces and headers off,
|
||||
/// adaptive timing, and the protocol (`ATSP<protocol>`, 3 = ISO 9141-2).
|
||||
pub fn open(addr: [u8; 6], channel: u8, protocol: char) -> io::Result<Self> {
|
||||
let link = Rfcomm::connect(addr, channel, Duration::from_secs(10))?;
|
||||
let mut e = Elm { link, version: String::new() };
|
||||
e.cmd("ATZ", Duration::from_secs(5))?;
|
||||
for c in ["ATE0", "ATL0", "ATS0", "ATH0", "ATAT1"] {
|
||||
let r = e.cmd(c, Duration::from_secs(2))?;
|
||||
if !r.contains("OK") {
|
||||
return Err(io::Error::other(format!("{c}: {r:?}")));
|
||||
}
|
||||
}
|
||||
let r = e.cmd(&format!("ATSP{protocol}"), Duration::from_secs(2))?;
|
||||
if !r.contains("OK") {
|
||||
return Err(io::Error::other(format!("ATSP{protocol}: {r:?}")));
|
||||
}
|
||||
e.version = e.cmd("ATI", Duration::from_secs(2))?;
|
||||
Ok(e)
|
||||
}
|
||||
|
||||
/// One command; the reply up to the prompt, without it. Times out as an error:
|
||||
/// an adapter that stops prompting is gone, not slow.
|
||||
pub fn cmd(&mut self, c: &str, timeout: Duration) -> io::Result<String> {
|
||||
self.link.write_all(format!("{c}\r").as_bytes())?;
|
||||
let deadline = Instant::now() + timeout;
|
||||
let mut buf = Vec::new();
|
||||
let mut chunk = [0u8; 256];
|
||||
while !buf.contains(&b'>') {
|
||||
let left = deadline.saturating_duration_since(Instant::now());
|
||||
if left.is_zero() {
|
||||
return Err(io::Error::new(io::ErrorKind::TimedOut, format!("{c}: no prompt within {timeout:?}")));
|
||||
}
|
||||
timeo(self.link.fd, libc::SO_RCVTIMEO, left.max(Duration::from_millis(1)))?;
|
||||
match self.link.read(&mut chunk) {
|
||||
Ok(0) => return Err(io::Error::new(io::ErrorKind::UnexpectedEof, "adapter closed the link")),
|
||||
Ok(n) => buf.extend_from_slice(&chunk[..n]),
|
||||
Err(e) if matches!(e.kind(), io::ErrorKind::WouldBlock | io::ErrorKind::TimedOut) => {}
|
||||
Err(e) => return Err(e),
|
||||
}
|
||||
}
|
||||
let text = String::from_utf8_lossy(&buf).replace('>', "").replace('\r', "\n");
|
||||
Ok(text.lines().map(str::trim).filter(|l| !l.is_empty()).collect::<Vec<_>>().join("\n"))
|
||||
}
|
||||
|
||||
/// One mode 01 request: Ok((receive time µs, data bytes)), or Err(what the adapter
|
||||
/// said instead: "NO DATA", "BUS INIT: ...ERROR", "UNABLE TO CONNECT", …) when the
|
||||
/// car gave no usable answer. The outer error means the link to the adapter broke.
|
||||
pub fn ask(&mut self, pid: u8) -> io::Result<Result<(i64, Vec<u8>), String>> {
|
||||
// The first request after the ignition comes on runs the K-line's 5-baud init: seconds.
|
||||
let r = self.cmd(&format!("01{pid:02X}"), Duration::from_secs(8))?;
|
||||
let ts = crate::now_us();
|
||||
Ok(parse_reply(&r, pid).map(|d| (ts, d)).ok_or(r))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn addresses() {
|
||||
assert_eq!(parse_addr("00:0C:BF:50:AF:3F"), Some([0x3F, 0xAF, 0x50, 0xBF, 0x0C, 0x00]));
|
||||
assert_eq!(parse_addr("00:0C:BF:50:AF"), None);
|
||||
assert_eq!(parse_addr("nope"), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn replies() {
|
||||
// Seen on the Z27AG, 2026-10-13 (headers were on then: 48 6B 00 … checksum).
|
||||
assert_eq!(parse_reply("SEARCHING...\n4100BE1FB010", 0x00), Some(vec![0xBE, 0x1F, 0xB0, 0x10]));
|
||||
assert_eq!(parse_reply("BUS INIT: ...OK\n410C0000", 0x0C), Some(vec![0, 0]));
|
||||
assert_eq!(parse_reply("41 0C 12 DC", 0x0C), Some(vec![0x12, 0xDC]));
|
||||
assert_eq!(parse_reply("41055A", 0x0C), None); // another PID's reply
|
||||
assert_eq!(parse_reply("NO DATA", 0x0C), None);
|
||||
assert_eq!(parse_reply("410C12D", 0x0C), None); // torn
|
||||
}
|
||||
}
|
||||
+147
-154
@@ -1,19 +1,22 @@
|
||||
//! obd-poller — active OBD-II mode 01 polling, the single CAN writer (decision 3).
|
||||
//! obd-poller — OBD-II mode 01 over the K-line, through a Bluetooth ELM327.
|
||||
//!
|
||||
//! 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
|
||||
//! The Z27AG answers OBD-II only on the K-line (ISO 9141-2), not on CAN, and the
|
||||
//! ELM is the Orin's only way onto it (project-yukart decision 30). The poller holds
|
||||
//! the adapter's Bluetooth serial link, asks the engine ECU for standard values
|
||||
//! (SAE J1979) one at a time, and publishes each reply as an envelope on
|
||||
//! `yukart/vehicle/<signal>`, 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.
|
||||
//! `signals.json` (source `obd2`, every name `obd_`). These are calibration
|
||||
//! references for the decoded CAN signals, recorded beside the frames; displays
|
||||
//! take the CAN values.
|
||||
//!
|
||||
//! 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.
|
||||
//! The ELM's protocol is fixed (`ATSP3`), so it never searches, and never sends an
|
||||
//! OBD request onto the CAN bus: nothing in Yukart transmits on CAN.
|
||||
//!
|
||||
//! When the ECU stops answering (ignition off) for a minute, the poller lets go of
|
||||
//! the link so the adapter can sleep, and tries again once a minute.
|
||||
|
||||
mod elm;
|
||||
mod pids;
|
||||
mod socket;
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::process::ExitCode;
|
||||
@@ -26,20 +29,23 @@ use rumqttc::{Client, Event, LastWill, MqttOptions, NetworkOptions, Packet, QoS}
|
||||
use serde_json::{json, Value};
|
||||
|
||||
use pids::{Pid, PIDS};
|
||||
use socket::ObdSocket;
|
||||
use elm::Elm;
|
||||
|
||||
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)
|
||||
usage: obd-poller --elm ADDR [options]
|
||||
--elm ADDR the ELM327's Bluetooth address, e.g. 00:0C:BF:50:AF:3F (required)
|
||||
--channel N its serial (RFCOMM) channel (1)
|
||||
--protocol P ELM protocol number; 3 = ISO 9141-2 (K-line) (3)
|
||||
--broker HOST:PORT MQTT broker (127.0.0.1:1883)
|
||||
--slow-ms N period of the slow PIDs (temperatures, trims) (5000)
|
||||
--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;
|
||||
/// The ECU silent this long: release the adapter so it can sleep.
|
||||
const RELEASE_AFTER: Duration = Duration::from_secs(60);
|
||||
/// Retry a released or unreachable adapter this often.
|
||||
const RECONNECT_EVERY: Duration = Duration::from_secs(60);
|
||||
|
||||
pub fn now_us() -> i64 {
|
||||
let d = std::time::SystemTime::now().duration_since(std::time::UNIX_EPOCH).unwrap_or_default();
|
||||
@@ -65,86 +71,47 @@ fn units() -> HashMap<&'static str, String> {
|
||||
}
|
||||
|
||||
struct Opts {
|
||||
iface: String,
|
||||
elm: [u8; 6],
|
||||
elm_text: String,
|
||||
channel: u8,
|
||||
protocol: char,
|
||||
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::<u64>().ok()) else {
|
||||
self.last = None;
|
||||
return false;
|
||||
/// Supported PIDs, walking the 0x00 / 0x20 / 0x40 ... bitmasks. Err: what the
|
||||
/// adapter said when the ECU did not answer the first one.
|
||||
fn discover(e: &mut Elm) -> std::io::Result<Result<Vec<u8>, String>> {
|
||||
let mut all = Vec::new();
|
||||
let mut mask = 0x00u8;
|
||||
loop {
|
||||
let d = match e.ask(mask)? {
|
||||
Ok((_, d)) if d.len() >= 4 => d,
|
||||
Ok(_) => break,
|
||||
Err(said) if mask == 0 => return Ok(Err(said)),
|
||||
Err(_) => break,
|
||||
};
|
||||
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))),
|
||||
_ => {}
|
||||
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;
|
||||
}
|
||||
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<Option<(i64, u32, Vec<u8>)>> {
|
||||
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 && (2..=7).contains(&len) && 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<Vec<u8>> {
|
||||
let mut all = Vec::new();
|
||||
let mut mask = 0x00u8;
|
||||
while let Some((_, _, d)) = self.ask(mask)? {
|
||||
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)
|
||||
mask += 0x20;
|
||||
}
|
||||
Ok(Ok(all))
|
||||
}
|
||||
|
||||
fn parse() -> Result<Opts, String> {
|
||||
let mut elm = None;
|
||||
let mut o = Opts {
|
||||
iface: "can_pcan".into(),
|
||||
elm: [0; 6],
|
||||
elm_text: String::new(),
|
||||
channel: 1,
|
||||
protocol: '3',
|
||||
broker: "127.0.0.1:1883".into(),
|
||||
gap: Duration::from_millis(10),
|
||||
timeout: Duration::from_millis(100),
|
||||
slow: Duration::from_millis(1000),
|
||||
slow: Duration::from_millis(5000),
|
||||
once: false,
|
||||
};
|
||||
let mut args = std::env::args().skip(1);
|
||||
@@ -154,47 +121,42 @@ fn parse() -> Result<Opts, String> {
|
||||
continue;
|
||||
}
|
||||
let v = args.next().ok_or(format!("{a} needs a value"))?;
|
||||
let ms = || v.parse::<u64>().map(Duration::from_millis).map_err(|_| format!("{a}: not a number: {v}"));
|
||||
match a.as_str() {
|
||||
"--iface" => o.iface = v.clone(),
|
||||
"--elm" => {
|
||||
elm = Some(elm::parse_addr(&v).ok_or(format!("--elm: not a Bluetooth address: {v:?}"))?);
|
||||
o.elm_text = v.clone();
|
||||
}
|
||||
"--channel" => o.channel = v.parse().map_err(|_| format!("--channel: not a number: {v}"))?,
|
||||
"--protocol" => o.protocol = v.chars().next().filter(|c| c.is_ascii_hexdigit() && v.len() == 1).ok_or(format!("--protocol: one hex digit, not {v}"))?,
|
||||
"--broker" => o.broker = v.clone(),
|
||||
"--gap-ms" => o.gap = ms()?,
|
||||
"--timeout-ms" => o.timeout = ms()?,
|
||||
"--slow-ms" => o.slow = ms()?,
|
||||
"--slow-ms" => o.slow = v.parse::<u64>().map(Duration::from_millis).map_err(|_| format!("--slow-ms: not a number: {v}"))?,
|
||||
_ => return Err(format!("unknown option {a}")),
|
||||
}
|
||||
}
|
||||
o.elm = elm.ok_or("--elm is required (the deploy sets it from /etc/default/yukart-elm)")?;
|
||||
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<Value> {
|
||||
let supported = p.discover()?;
|
||||
let run = || -> Result<Value, String> {
|
||||
let mut e = Elm::open(o.elm, o.channel, o.protocol).map_err(|e| format!("{}: {e}", o.elm_text))?;
|
||||
let supported = discover(&mut e).map_err(|e| e.to_string())?.map_err(|said| format!("the ECU does not answer: {said:?}"))?;
|
||||
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 }));
|
||||
match e.ask(pid.pid).map_err(|e| e.to_string())? {
|
||||
Ok((_, d)) if d.len() >= pid.bytes => {
|
||||
values.insert(pid.signal.into(), json!({ "value": (pid.decode)(&d), "reply_ms": (t.elapsed().as_secs_f64() * 1e3).round() }));
|
||||
}
|
||||
Ok(_) => {}
|
||||
Err(said) => {
|
||||
values.insert(pid.signal.into(), json!({ "said": said }));
|
||||
}
|
||||
}
|
||||
}
|
||||
Ok(json!({
|
||||
"adapter": e.version,
|
||||
"protocol": e.cmd("ATDP", Duration::from_secs(2)).unwrap_or_default(),
|
||||
"supported": supported.iter().map(|x| format!("{x:02X}")).collect::<Vec<_>>(),
|
||||
"values": values,
|
||||
}))
|
||||
@@ -265,12 +227,14 @@ fn main() -> ExitCode {
|
||||
});
|
||||
}
|
||||
|
||||
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<Poller> = None;
|
||||
eprintln!("obd-poller {} via the ELM at {} (protocol {}), publishing to {}", env!("CARGO_PKG_VERSION"), o.elm_text, o.protocol, o.broker);
|
||||
let mut elm: Option<Elm> = None;
|
||||
let mut next_connect = Instant::now();
|
||||
let mut supported: Option<Vec<u8>> = None;
|
||||
let mut last_slow = Instant::now() - o.slow;
|
||||
let mut next_discover = Instant::now();
|
||||
// When the ECU last answered (or the link came up): a minute of silence releases the adapter.
|
||||
let mut heard = Instant::now();
|
||||
let mut last_slow = Instant::now() - o.slow;
|
||||
let mut counts: HashMap<&'static str, u32> = HashMap::new();
|
||||
let mut misses = 0u32;
|
||||
let mut window = Instant::now();
|
||||
@@ -294,46 +258,58 @@ fn main() -> ExitCode {
|
||||
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 }),
|
||||
if elm.is_none() {
|
||||
if Instant::now() < next_connect {
|
||||
thread::sleep(Duration::from_millis(250));
|
||||
continue;
|
||||
}
|
||||
match Elm::open(o.elm, o.channel, o.protocol) {
|
||||
Ok(e) => {
|
||||
eprintln!("obd-poller: {} connected: {}", o.elm_text, e.version);
|
||||
elm = Some(e);
|
||||
supported = None;
|
||||
next_discover = Instant::now();
|
||||
heard = Instant::now();
|
||||
}
|
||||
Err(e) => {
|
||||
state = json!({ "state": "degraded", "detail": format!("{}: {e}", o.iface) });
|
||||
thread::sleep(Duration::from_secs(1));
|
||||
continue;
|
||||
state = json!({ "state": "degraded", "detail": format!("ELM {}: {e} (unplugged, asleep or out of range); retrying in {} s", o.elm_text, RECONNECT_EVERY.as_secs()) });
|
||||
next_connect = Instant::now() + RECONNECT_EVERY;
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
let e = elm.as_mut().unwrap();
|
||||
|
||||
if heard.elapsed() >= RELEASE_AFTER {
|
||||
// Ignition off: let go, so the adapter can sleep instead of draining the battery.
|
||||
elm = None;
|
||||
supported = None;
|
||||
next_connect = Instant::now() + RECONNECT_EVERY;
|
||||
state = json!({ "state": "degraded", "detail": format!("ECU silent for {} s: adapter released; retrying in {} s", RELEASE_AFTER.as_secs(), RECONNECT_EVERY.as_secs()) });
|
||||
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() => {
|
||||
match discover(e) {
|
||||
Ok(Ok(s)) => {
|
||||
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);
|
||||
heard = Instant::now();
|
||||
misses = 0;
|
||||
}
|
||||
Ok(_) => {
|
||||
state = json!({ "state": "degraded", "detail": "no ECU answers 01 00; retrying in 5 s" });
|
||||
Ok(Err(said)) => {
|
||||
state = json!({ "state": "degraded", "detail": format!("ECU not answering ({}): ignition off?", said.replace('\n', " ")) });
|
||||
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);
|
||||
Err(err) => {
|
||||
state = json!({ "state": "degraded", "detail": format!("ELM {}: {err}", o.elm_text) });
|
||||
elm = None;
|
||||
next_connect = Instant::now() + Duration::from_secs(2);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
@@ -346,31 +322,40 @@ fn main() -> ExitCode {
|
||||
}
|
||||
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 => {
|
||||
let mut answered = 0;
|
||||
let mut broken = None;
|
||||
for pid in &round {
|
||||
match e.ask(pid.pid) {
|
||||
Ok(Ok((ts, d))) if d.len() >= pid.bytes => {
|
||||
answered += 1;
|
||||
*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] });
|
||||
"value": (v * 1000.0).round() / 1000.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));
|
||||
Err(err) => {
|
||||
broken = Some(err);
|
||||
break;
|
||||
}
|
||||
}
|
||||
thread::sleep(o.gap);
|
||||
}
|
||||
if failed {
|
||||
if let Some(err) = broken {
|
||||
state = json!({ "state": "degraded", "detail": format!("ELM {}: {err}", o.elm_text) });
|
||||
elm = None;
|
||||
next_connect = Instant::now() + Duration::from_secs(2);
|
||||
continue;
|
||||
}
|
||||
if answered > 0 {
|
||||
heard = Instant::now();
|
||||
} else if !round.is_empty() {
|
||||
// A whole round unanswered: the ECU went away. Rediscover once it is back.
|
||||
supported = None;
|
||||
next_discover = Instant::now() + Duration::from_secs(5);
|
||||
state = json!({ "state": "degraded", "detail": "ECU stopped answering: ignition off?" });
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -378,9 +363,8 @@ fn main() -> ExitCode {
|
||||
let secs = window.elapsed().as_secs_f64();
|
||||
let rates: serde_json::Map<String, Value> =
|
||||
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::<Vec<_>>(),
|
||||
"detail": { "adapter": e.version, "supported": sup.iter().map(|x| format!("{x:02X}")).collect::<Vec<_>>(),
|
||||
"rates_hz": rates, "unanswered": misses } });
|
||||
counts.clear();
|
||||
misses = 0;
|
||||
@@ -388,3 +372,12 @@ fn main() -> ExitCode {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
#[test]
|
||||
fn every_pid_is_an_obd2_signal_in_the_registry() {
|
||||
let u = super::units();
|
||||
assert!(u.keys().all(|k| k.starts_with("obd_")));
|
||||
}
|
||||
}
|
||||
|
||||
+28
-9
@@ -1,5 +1,5 @@
|
||||
//! 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`).
|
||||
//! Signal names and units belong to contracts' `signals.json` (source `obd2`, every name `obd_`).
|
||||
|
||||
pub struct Pid {
|
||||
pub pid: u8,
|
||||
@@ -15,15 +15,25 @@ fn ab(d: &[u8]) -> f64 {
|
||||
(d[0] as f64) * 256.0 + d[1] as f64
|
||||
}
|
||||
|
||||
/// Fuel trim: 128 is zero, each step 100/128 %.
|
||||
fn trim(d: &[u8]) -> f64 {
|
||||
(d[0] as f64 - 128.0) * 100.0 / 128.0
|
||||
}
|
||||
|
||||
pub const PIDS: &[Pid] = &[
|
||||
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 },
|
||||
Pid { pid: 0x0C, signal: "obd_engine_rpm", bytes: 2, decode: |d| ab(d) / 4.0, fast: true },
|
||||
Pid { pid: 0x0D, signal: "obd_vehicle_speed_kph", bytes: 1, decode: |d| d[0] as f64, fast: true },
|
||||
Pid { pid: 0x11, signal: "obd_throttle_pct", bytes: 1, decode: |d| d[0] as f64 * 100.0 / 255.0, fast: true },
|
||||
Pid { pid: 0x04, signal: "obd_engine_load_pct", bytes: 1, decode: |d| d[0] as f64 * 100.0 / 255.0, fast: true },
|
||||
Pid { pid: 0x0B, signal: "obd_intake_pressure_kpa", bytes: 1, decode: |d| d[0] as f64, fast: true },
|
||||
Pid { pid: 0x10, signal: "obd_maf_gps", bytes: 2, decode: |d| ab(d) / 100.0, fast: true },
|
||||
Pid { pid: 0x0E, signal: "obd_timing_advance_deg", bytes: 1, decode: |d| d[0] as f64 / 2.0 - 64.0, fast: true },
|
||||
Pid { pid: 0x14, signal: "obd_o2_b1s1_v", bytes: 1, decode: |d| d[0] as f64 / 200.0, fast: true },
|
||||
Pid { pid: 0x06, signal: "obd_stft_pct", bytes: 1, decode: trim, fast: false },
|
||||
Pid { pid: 0x07, signal: "obd_ltft_pct", bytes: 1, decode: trim, fast: false },
|
||||
Pid { pid: 0x05, signal: "obd_coolant_temp_c", bytes: 1, decode: |d| d[0] as f64 - 40.0, fast: false },
|
||||
Pid { pid: 0x0F, signal: "obd_intake_air_temp_c", bytes: 1, decode: |d| d[0] as f64 - 40.0, fast: false },
|
||||
Pid { pid: 0x2F, signal: "obd_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.
|
||||
@@ -45,10 +55,19 @@ mod tests {
|
||||
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);
|
||||
// The 2026-10-13 probe over the ELM: 410C12DC, 41055A.
|
||||
assert_eq!((by(0x0C).decode)(&[0x12, 0xDC]), 1207.0);
|
||||
assert_eq!((by(0x05).decode)(&[0x5A]), 50.0);
|
||||
assert_eq!((by(0x0E).decode)(&[0x94]), 10.0);
|
||||
assert_eq!((by(0x06).decode)(&[0x80]), 0.0);
|
||||
assert_eq!((by(0x14).decode)(&[0x5A, 0xFF]), 0.45);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reads_the_supported_bitmask() {
|
||||
// The Z27AG's own reply over the K-line, 2026-10-13.
|
||||
let z = supported_in(0x00, &[0xBE, 0x1F, 0xB0, 0x10]);
|
||||
assert_eq!(z, vec![0x01, 0x03, 0x04, 0x05, 0x06, 0x07, 0x0C, 0x0D, 0x0E, 0x0F, 0x10, 0x11, 0x13, 0x14, 0x1C]);
|
||||
// 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]);
|
||||
|
||||
@@ -1,110 +0,0 @@
|
||||
//! 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<T>(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::<T>() as u32)
|
||||
};
|
||||
if r < 0 { Err(io::Error::last_os_error()) } else { Ok(()) }
|
||||
}
|
||||
|
||||
impl ObdSocket {
|
||||
pub fn open(iface: &str) -> 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 = 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::<libc::sockaddr_can>() 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::<libc::can_frame>()) };
|
||||
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<Option<Response>> {
|
||||
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::<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);
|
||||
}
|
||||
}
|
||||
Ok(Some(Response { ts_us, id: frame.can_id & libc::CAN_SFF_MASK, data: frame.data }))
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user