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:
+5
-1
@@ -2,7 +2,7 @@
|
||||
name = "svc-core"
|
||||
version = "0.1.0"
|
||||
edition = "2021"
|
||||
description = "Acquisition and processing cluster: recorder, decoder, orin-status and obd-poller now; can-bridge and gnss later"
|
||||
description = "Acquisition and processing cluster: recorder, decoder, gnss, orin-status and obd-poller now; can-bridge later"
|
||||
publish = false
|
||||
|
||||
[[bin]]
|
||||
@@ -21,6 +21,10 @@ path = "src/obd-poller/main.rs"
|
||||
name = "decoder"
|
||||
path = "src/decoder/main.rs"
|
||||
|
||||
[[bin]]
|
||||
name = "gnss"
|
||||
path = "src/gnss/main.rs"
|
||||
|
||||
[[bin]]
|
||||
name = "bus-bench"
|
||||
path = "src/bus-bench/main.rs"
|
||||
|
||||
@@ -10,25 +10,28 @@ 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`, `decoder`, `orin-status` and `obd-poller` now; `can-bridge` and `gnss` as they are
|
||||
unit: `recorder`, `decoder`, `gnss`, `orin-status` and `obd-poller` now; `can-bridge` as it is
|
||||
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 |
|
||||
| `recorder` | `yukart-recorder.service` | Raw CAN + GNSS to disk, in sessions, under a storage bound; every `yukart/vehicle/*` value (decoded and OBD-II) 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; **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/*`) |
|
||||
| `obd-poller` | `yukart-obd-poller.service` | OBD-II mode 01 on the K-line through the Bluetooth ELM327 (address in `/etc/default/yukart-elm`), the single writer to the car; never on CAN. The `obd_*` signals, calibration references (`yukart/vehicle/*`): rpm, speed, throttle, load, MAF, timing, O2 every round; temperatures and trims every 5 s. Lets go of the adapter after a minute of ECU silence |
|
||||
| `gnss` | `yukart-gnss.service` | gpsd's fix as single values on `yukart/gnss/*`: fix, speed, heading, position, altitude, satellites |
|
||||
| `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, the ELM's Bluetooth link, 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)
|
||||
scripts/deploy-native.sh BIN # build BIN on the development Orin, glibc check, install;
|
||||
# deploy-{decoder,gnss,obd-poller,orin-status}.sh wrap it.
|
||||
# Redeploy after any contracts 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-obd-poller --elm ADDR --once # on the Orin, service stopped: adapter, protocol,
|
||||
# supported PIDs and one value each
|
||||
yukart-gnss --once # the next fix as JSON, no broker
|
||||
yukart-orin-status --once # on an Orin: one reading as JSON, no broker
|
||||
yukart-recorder ctl status # on the Orin
|
||||
yukart-recorder ctl start "cold start" # manual session, ends only on stop
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
# The ELM327 (OBD-II on the K-line, over Bluetooth serial): its address, read by
|
||||
# yukart-obd-poller (which connects) and yukart-orin-status (which only watches).
|
||||
# Installed to /etc/default/yukart-elm by scripts/deploy-native.sh when absent;
|
||||
# edit it there for another adapter, then restart both services.
|
||||
ELM_ADDR=00:0C:BF:50:AF:3F
|
||||
@@ -0,0 +1,15 @@
|
||||
[Unit]
|
||||
Description=Yukart gnss: the receiver's fix on the bus, as single values
|
||||
Documentation=gitea:yukart/svc-core
|
||||
# Tier 2. Ordered after the broker and gpsd; it reconnects to either by itself.
|
||||
After=mosquitto.service gpsd.service
|
||||
StartLimitIntervalSec=0
|
||||
|
||||
[Service]
|
||||
User=mikkeli
|
||||
ExecStart=/usr/local/bin/yukart-gnss
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
@@ -1,14 +1,15 @@
|
||||
[Unit]
|
||||
Description=Yukart obd-poller: OBD-II mode 01 polling, the single CAN writer
|
||||
Description=Yukart obd-poller: OBD-II over the K-line through the Bluetooth ELM327
|
||||
Documentation=gitea:yukart/svc-core
|
||||
# Tier 2. Ordered after the broker and nothing else: the adapter comes and goes, and
|
||||
# the poller only sends while the bus is visibly alive.
|
||||
After=mosquitto.service
|
||||
# Tier 2. Ordered after the broker and Bluetooth: the adapter comes and goes, and the
|
||||
# poller connects, releases and retries by itself.
|
||||
After=mosquitto.service bluetooth.service
|
||||
StartLimitIntervalSec=0
|
||||
|
||||
[Service]
|
||||
User=mikkeli
|
||||
ExecStart=/usr/local/bin/yukart-obd-poller
|
||||
EnvironmentFile=/etc/default/yukart-elm
|
||||
ExecStart=/usr/local/bin/yukart-obd-poller --elm ${ELM_ADDR}
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
|
||||
@@ -9,7 +9,9 @@ StartLimitIntervalSec=0
|
||||
User=mikkeli
|
||||
# The UPS INA219 is on /dev/i2c-7, group i2c.
|
||||
SupplementaryGroups=i2c
|
||||
ExecStart=/usr/local/bin/yukart-orin-status
|
||||
# The ELM's address, for elm_connected; without the file nothing is reported.
|
||||
EnvironmentFile=-/etc/default/yukart-elm
|
||||
ExecStart=/usr/local/bin/yukart-orin-status --elm ${ELM_ADDR}
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
|
||||
+8
-6
@@ -3,12 +3,14 @@ type: guidelines
|
||||
---
|
||||
# Core services guidelines
|
||||
|
||||
## 1. One CAN writer: `obd-poller`
|
||||
- `obd-poller` is the only thing that transmits on the car's bus (project-yukart
|
||||
decision 3): OBD-II requests, one at a time, only while the bus is visibly
|
||||
alive. Anything else that transmits is a bug; the recorder only reads.
|
||||
- `can_pcan` is no longer listen-only (2026-10-12), so the adapter acknowledges
|
||||
frames like any scan tool. Going back is one line in orin-nano-env-setup's
|
||||
## 1. One writer: `obd-poller`, and never on CAN
|
||||
- `obd-poller` is the only thing that sends anything to the car (project-yukart
|
||||
decisions 3 and 30): OBD-II requests, one at a time, on the K-line through the
|
||||
Bluetooth ELM327, with the ELM's protocol fixed so it never probes CAN.
|
||||
- Nothing in svc-core transmits on CAN. The recorder and the decoder only read;
|
||||
anything else that writes to `can_pcan` is a bug.
|
||||
- `can_pcan` still acknowledges frames like any scan tool (listen-only off since
|
||||
2026-10-12, left so as harmless). Going back is one line in orin-nano-env-setup's
|
||||
`/etc/default/can-can_pcan`.
|
||||
|
||||
## 2. Capture never waits
|
||||
|
||||
@@ -1,42 +1,6 @@
|
||||
#!/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.
|
||||
# Build decoder natively on the development Orin and install it on the car Orin as
|
||||
# yukart-decoder: scripts/deploy-native.sh does the work.
|
||||
#
|
||||
# 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'
|
||||
exec "$(dirname "$0")/deploy-native.sh" decoder "$@"
|
||||
|
||||
Executable
+6
@@ -0,0 +1,6 @@
|
||||
#!/usr/bin/env bash
|
||||
# Build gnss natively on the development Orin and install it on the car Orin as
|
||||
# yukart-gnss: scripts/deploy-native.sh does the work.
|
||||
#
|
||||
# usage: scripts/deploy-gnss.sh [host] (default: mikkeli-orin-nano)
|
||||
exec "$(dirname "$0")/deploy-native.sh" gnss "$@"
|
||||
Executable
+55
@@ -0,0 +1,55 @@
|
||||
#!/usr/bin/env bash
|
||||
# Build one svc-core binary natively on the development Orin, check it against the
|
||||
# car's glibc, and install it on the car Orin as the yukart-<bin> service.
|
||||
#
|
||||
# usage: scripts/deploy-native.sh BIN [host] (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/. Signal names, units and layouts are
|
||||
# compiled in from contracts' signals.json, so a registry change needs a redeploy.
|
||||
# Native because std::process::Command cross-compiled against the workstation's
|
||||
# newer glibc links GLIBC_2.39 symbols the car cannot load (check-glibc.sh gates it).
|
||||
#
|
||||
# A service that reads /etc/default/yukart-elm (the ELM327's address) gets the
|
||||
# repo's copy installed when the host has none; an existing one is left alone.
|
||||
set -euo pipefail
|
||||
|
||||
BIN="${1:?usage: scripts/deploy-native.sh BIN [host]}"
|
||||
HOST="${2:-mikkeli-orin-nano}"
|
||||
BUILD_HOST="${BUILD_HOST:-mikkeli-orin-nano-2}"
|
||||
cd "$(dirname "$0")/.."
|
||||
UNIT="deploy/yukart-$BIN.service"
|
||||
[ -f "$UNIT" ] || { echo "no $UNIT" >&2; exit 2; }
|
||||
OUT="$(mktemp -d)/$BIN"
|
||||
|
||||
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 $BIN 2>&1 | tail -1
|
||||
git log --oneline -1"
|
||||
scp -q "$BUILD_HOST:dev/yukart/svc-core/target/release/$BIN" "$OUT"
|
||||
scripts/check-glibc.sh "$OUT"
|
||||
|
||||
# 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 "$OUT" "$HOST:infra/svc-core/$BIN.new"
|
||||
scp -q "$UNIT" "$HOST:infra/svc-core/yukart-$BIN.service"
|
||||
if grep -q /etc/default/yukart-elm "$UNIT"; then
|
||||
scp -q deploy/yukart-elm.default "$HOST:infra/svc-core/yukart-elm.default"
|
||||
fi
|
||||
ssh "$HOST" "set -e
|
||||
mv ~/infra/svc-core/$BIN.new ~/infra/svc-core/$BIN
|
||||
sudo ln -sfn \"\$HOME/infra/svc-core/$BIN\" /usr/local/bin/yukart-$BIN
|
||||
sudo install -m 0644 ~/infra/svc-core/yukart-$BIN.service /etc/systemd/system/yukart-$BIN.service
|
||||
if [ -f ~/infra/svc-core/yukart-elm.default ] && [ ! -f /etc/default/yukart-elm ]; then
|
||||
sudo install -m 0644 ~/infra/svc-core/yukart-elm.default /etc/default/yukart-elm
|
||||
fi
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable yukart-$BIN.service >/dev/null
|
||||
sudo systemctl restart yukart-$BIN.service
|
||||
sleep 1
|
||||
systemctl --no-pager --lines=5 status yukart-$BIN.service"
|
||||
@@ -1,31 +1,6 @@
|
||||
#!/usr/bin/env bash
|
||||
# Build the obd-poller for aarch64, check it against the car's glibc, and install it
|
||||
# on an Orin as the yukart-obd-poller service.
|
||||
# Build obd-poller natively on the development Orin and install it on the car Orin as
|
||||
# yukart-obd-poller: scripts/deploy-native.sh does the work.
|
||||
#
|
||||
# usage: scripts/deploy-obd-poller.sh [host] (default: mikkeli-orin-nano)
|
||||
#
|
||||
# Cross-compiles from the workstation (the documented fallback): the development
|
||||
# Orin is the preferred native build host. Either way check-glibc.sh gates it.
|
||||
set -euo pipefail
|
||||
|
||||
HOST="${1:-mikkeli-orin-nano}"
|
||||
cd "$(dirname "$0")/.."
|
||||
TARGET=aarch64-unknown-linux-gnu
|
||||
BIN="target/$TARGET/release/obd-poller"
|
||||
|
||||
cargo build --release --target "$TARGET" --bin obd-poller
|
||||
scripts/check-glibc.sh "$BIN"
|
||||
|
||||
# Binary lives under ~/infra per the host repo's guidelines; /usr/local/bin gets a symlink.
|
||||
ssh "$HOST" 'mkdir -p ~/infra/svc-core'
|
||||
scp -q "$BIN" "$HOST:infra/svc-core/obd-poller.new"
|
||||
scp -q deploy/yukart-obd-poller.service "$HOST:infra/svc-core/yukart-obd-poller.service"
|
||||
ssh "$HOST" 'set -e
|
||||
mv ~/infra/svc-core/obd-poller.new ~/infra/svc-core/obd-poller
|
||||
sudo ln -sfn "$HOME/infra/svc-core/obd-poller" /usr/local/bin/yukart-obd-poller
|
||||
sudo install -m 0644 ~/infra/svc-core/yukart-obd-poller.service /etc/systemd/system/yukart-obd-poller.service
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable yukart-obd-poller.service >/dev/null
|
||||
sudo systemctl restart yukart-obd-poller.service
|
||||
sleep 1
|
||||
systemctl --no-pager --lines=5 status yukart-obd-poller.service'
|
||||
exec "$(dirname "$0")/deploy-native.sh" obd-poller "$@"
|
||||
|
||||
@@ -1,42 +1,6 @@
|
||||
#!/usr/bin/env bash
|
||||
# Build orin-status natively on the development Orin, check it against the car's
|
||||
# glibc, and install it on the car Orin as the yukart-orin-status service.
|
||||
# Build orin-status natively on the development Orin and install it on the car Orin as
|
||||
# yukart-orin-status: scripts/deploy-native.sh does the work.
|
||||
#
|
||||
# usage: scripts/deploy-orin-status.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/. Native on purpose: orin-status runs
|
||||
# mmcli, and std::process::Command cross-compiled against the workstation's newer
|
||||
# glibc links GLIBC_2.39 symbols the car cannot load.
|
||||
set -euo pipefail
|
||||
|
||||
HOST="${1:-mikkeli-orin-nano}"
|
||||
BUILD_HOST="${BUILD_HOST:-mikkeli-orin-nano-2}"
|
||||
cd "$(dirname "$0")/.."
|
||||
BIN="$(mktemp -d)/orin-status"
|
||||
|
||||
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 orin-status 2>&1 | tail -1
|
||||
git log --oneline -1'
|
||||
scp -q "$BUILD_HOST:dev/yukart/svc-core/target/release/orin-status" "$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/orin-status.new"
|
||||
scp -q deploy/yukart-orin-status.service "$HOST:infra/svc-core/yukart-orin-status.service"
|
||||
ssh "$HOST" 'set -e
|
||||
mv ~/infra/svc-core/orin-status.new ~/infra/svc-core/orin-status
|
||||
sudo ln -sfn "$HOME/infra/svc-core/orin-status" /usr/local/bin/yukart-orin-status
|
||||
sudo install -m 0644 ~/infra/svc-core/yukart-orin-status.service /etc/systemd/system/yukart-orin-status.service
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable yukart-orin-status.service >/dev/null
|
||||
sudo systemctl restart yukart-orin-status.service
|
||||
sleep 1
|
||||
systemctl --no-pager --lines=5 status yukart-orin-status.service'
|
||||
exec "$(dirname "$0")/deploy-native.sh" orin-status "$@"
|
||||
|
||||
@@ -0,0 +1,288 @@
|
||||
//! gnss — the receiver's fix on the bus, as single values for displays.
|
||||
//!
|
||||
//! Watches gpsd and publishes each position report (TPV) as envelopes on
|
||||
//! `yukart/gnss/<signal>`, retained: fix quality, speed and heading over ground,
|
||||
//! latitude, longitude and altitude; the satellite count comes from SKY reports.
|
||||
//! Names and units from contracts' `signals.json` (source `gnss`). A value the fix
|
||||
//! does not support (no fix, 2D altitude, heading at a standstill) is published as
|
||||
//! null, which the envelope defines as "known, no value now".
|
||||
//!
|
||||
//! GNSS speed is the reference beside the decoded wheel speed (project-yukart
|
||||
//! decision 30). The recorder keeps gpsd's own reports; this is only for the bus.
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::io::{BufRead, BufReader, Write};
|
||||
use std::net::{TcpStream, ToSocketAddrs};
|
||||
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: gnss [options]
|
||||
--broker HOST:PORT MQTT broker (127.0.0.1:1883)
|
||||
--gpsd HOST:PORT gpsd (127.0.0.1:2947)
|
||||
--once print the signals of the next fix as JSON and exit (no broker)";
|
||||
|
||||
const REGISTRY: &str = include_str!(concat!(env!("CARGO_MANIFEST_DIR"), "/../contracts/schema/signals.json"));
|
||||
const SYSTEM_TOPIC: &str = "yukart/system/gnss";
|
||||
/// Below this, gpsd's track is noise: heading is published as null.
|
||||
const STILL_MPS: f64 = 0.5;
|
||||
|
||||
fn now_us() -> i64 {
|
||||
let d = std::time::SystemTime::now().duration_since(std::time::UNIX_EPOCH).unwrap_or_default();
|
||||
d.as_micros() as i64
|
||||
}
|
||||
|
||||
fn mono_ns() -> i64 {
|
||||
let mut ts: libc::timespec = unsafe { std::mem::zeroed() };
|
||||
unsafe { libc::clock_gettime(libc::CLOCK_MONOTONIC, &mut ts) };
|
||||
ts.tv_sec as i64 * 1_000_000_000 + ts.tv_nsec as i64
|
||||
}
|
||||
|
||||
/// Units of the gnss signals, 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()
|
||||
.filter(|(_, s)| s["source"] == "gnss")
|
||||
.map(|(k, s)| (k.clone(), s["unit"].as_str().unwrap_or("").to_string()))
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn round(v: f64, places: i32) -> Value {
|
||||
let f = 10f64.powi(places);
|
||||
serde_json::Number::from_f64((v * f).round() / f).map(Value::Number).unwrap_or(Value::Null)
|
||||
}
|
||||
|
||||
/// The signals in one gpsd report; empty for reports that carry none.
|
||||
fn signals(report: &Value) -> Vec<(&'static str, Value)> {
|
||||
match report["class"].as_str() {
|
||||
Some("TPV") => {
|
||||
let mode = report["mode"].as_u64().unwrap_or(0);
|
||||
let has = |min: u64, k: &str| report[k].as_f64().filter(|_| mode >= min);
|
||||
let speed = has(2, "speed");
|
||||
vec![
|
||||
("gnss_fix", json!(match mode { 2 => "2d", 3 => "3d", _ => "none" })),
|
||||
("gnss_speed_kph", speed.map(|v| round(v * 3.6, 1)).unwrap_or(Value::Null)),
|
||||
(
|
||||
"gnss_heading_deg",
|
||||
has(2, "track").filter(|_| speed.is_some_and(|s| s >= STILL_MPS)).map(|v| round(v, 1)).unwrap_or(Value::Null),
|
||||
),
|
||||
("gnss_lat", has(2, "lat").map(|v| round(v, 7)).unwrap_or(Value::Null)),
|
||||
("gnss_lon", has(2, "lon").map(|v| round(v, 7)).unwrap_or(Value::Null)),
|
||||
("gnss_alt_m", has(3, "altMSL").or(has(3, "alt")).map(|v| round(v, 1)).unwrap_or(Value::Null)),
|
||||
]
|
||||
}
|
||||
Some("SKY") => {
|
||||
// Newer gpsd sends uSat; older ones only the satellite list.
|
||||
let used = report["uSat"].as_u64().or_else(|| {
|
||||
report["satellites"].as_array().map(|s| s.iter().filter(|x| x["used"] == true).count() as u64)
|
||||
});
|
||||
used.map(|n| vec![("gnss_satellites", json!(n))]).unwrap_or_default()
|
||||
}
|
||||
_ => Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Reads gpsd forever, reconnecting; hands each report's signals to `out`.
|
||||
fn watch(addr: &str, linked: &AtomicBool, mut out: impl FnMut(Vec<(&'static str, Value)>) -> bool) {
|
||||
let mut reported = false;
|
||||
loop {
|
||||
let conn = addr
|
||||
.to_socket_addrs()
|
||||
.ok()
|
||||
.and_then(|mut a| a.next())
|
||||
.ok_or_else(|| std::io::Error::other("unresolvable"))
|
||||
.and_then(|a| TcpStream::connect_timeout(&a, Duration::from_secs(2)));
|
||||
let mut stream = match conn {
|
||||
Ok(s) => s,
|
||||
Err(e) => {
|
||||
if !reported {
|
||||
eprintln!("gnss: gpsd {addr}: {e}; retrying");
|
||||
reported = true;
|
||||
}
|
||||
linked.store(false, Ordering::Relaxed);
|
||||
thread::sleep(Duration::from_secs(3));
|
||||
continue;
|
||||
}
|
||||
};
|
||||
let _ = stream.set_read_timeout(Some(Duration::from_secs(5)));
|
||||
if stream.write_all(b"?WATCH={\"enable\":true,\"json\":true};\n").is_err() {
|
||||
continue;
|
||||
}
|
||||
eprintln!("gnss: gpsd {addr}: watching");
|
||||
reported = false;
|
||||
linked.store(true, Ordering::Relaxed);
|
||||
for line in BufReader::new(stream).lines() {
|
||||
let Ok(line) = line else { break };
|
||||
let Ok(report) = serde_json::from_str::<Value>(&line) else { continue };
|
||||
let s = signals(&report);
|
||||
if !s.is_empty() && !out(s) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
eprintln!("gnss: gpsd {addr}: connection lost");
|
||||
linked.store(false, Ordering::Relaxed);
|
||||
thread::sleep(Duration::from_secs(1));
|
||||
}
|
||||
}
|
||||
|
||||
fn main() -> ExitCode {
|
||||
let mut broker = "127.0.0.1:1883".to_string();
|
||||
let mut gpsd = "127.0.0.1:2947".to_string();
|
||||
let mut once = false;
|
||||
let mut args = std::env::args().skip(1);
|
||||
while let Some(a) = args.next() {
|
||||
match a.as_str() {
|
||||
"--once" => once = true,
|
||||
"--broker" => broker = args.next().unwrap_or_default(),
|
||||
"--gpsd" => gpsd = args.next().unwrap_or_default(),
|
||||
_ => {
|
||||
eprintln!("gnss: unknown option {a}\n{USAGE}");
|
||||
return ExitCode::from(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
let units = units();
|
||||
let linked = Arc::new(AtomicBool::new(false));
|
||||
|
||||
if once {
|
||||
let mut got = serde_json::Map::new();
|
||||
watch(&gpsd, &linked, |s| {
|
||||
let tpv = s.iter().any(|(k, _)| *k == "gnss_fix");
|
||||
got.extend(s.into_iter().map(|(k, v)| (k.to_string(), v)));
|
||||
!tpv
|
||||
});
|
||||
println!("{}", serde_json::to_string_pretty(&got).unwrap());
|
||||
return ExitCode::SUCCESS;
|
||||
}
|
||||
|
||||
let Some((host, port)) = broker.rsplit_once(':').and_then(|(h, p)| Some((h.to_string(), p.parse::<u16>().ok()?)))
|
||||
else {
|
||||
eprintln!("gnss: --broker must be HOST:PORT\n{USAGE}");
|
||||
return ExitCode::from(2);
|
||||
};
|
||||
let mut opts = MqttOptions::new("gnss", host, port);
|
||||
opts.set_keep_alive(Duration::from_secs(5));
|
||||
opts.set_last_will(LastWill::new(
|
||||
SYSTEM_TOPIC,
|
||||
json!({ "state": "stopping", "detail": "connection lost" }).to_string(),
|
||||
QoS::AtLeastOnce,
|
||||
true,
|
||||
));
|
||||
let (client, mut connection) = Client::new(opts, 256);
|
||||
let connected = Arc::new(AtomicBool::new(false));
|
||||
{
|
||||
let connected = connected.clone();
|
||||
thread::spawn(move || {
|
||||
for ev in connection.iter() {
|
||||
match ev {
|
||||
Ok(Event::Incoming(Packet::ConnAck(_))) => {
|
||||
eprintln!("gnss: connected to the broker");
|
||||
connected.store(true, Ordering::Relaxed);
|
||||
}
|
||||
Ok(_) => {}
|
||||
Err(e) => {
|
||||
if connected.swap(false, Ordering::Relaxed) {
|
||||
eprintln!("gnss: broker: {e}; retrying");
|
||||
}
|
||||
thread::sleep(Duration::from_secs(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// The service's own state, from its own thread: gpsd can go quiet for minutes.
|
||||
{
|
||||
let fix = Arc::new(std::sync::Mutex::new(String::from("none")));
|
||||
let (st_client, st_connected, st_linked, last_fix) = (client.clone(), connected.clone(), linked.clone(), fix.clone());
|
||||
thread::spawn(move || {
|
||||
let (client, connected, linked) = (st_client, st_connected, st_linked);
|
||||
let mut last = String::new();
|
||||
let mut at = Instant::now();
|
||||
loop {
|
||||
let state = if !linked.load(Ordering::Relaxed) {
|
||||
json!({ "state": "degraded", "detail": "gpsd not reachable" })
|
||||
} else {
|
||||
json!({ "state": "ready", "detail": { "fix": *last_fix.lock().unwrap() } })
|
||||
}
|
||||
.to_string();
|
||||
if connected.load(Ordering::Relaxed) && (state != last || at.elapsed() > Duration::from_secs(60)) {
|
||||
if state != last {
|
||||
eprintln!("gnss: {state}");
|
||||
}
|
||||
let _ = client.try_publish(SYSTEM_TOPIC, QoS::AtLeastOnce, true, state.clone());
|
||||
last = state;
|
||||
at = Instant::now();
|
||||
}
|
||||
thread::sleep(Duration::from_secs(1));
|
||||
}
|
||||
});
|
||||
eprintln!("gnss {} watching {gpsd}, publishing to {broker}", env!("CARGO_PKG_VERSION"));
|
||||
watch(&gpsd, &linked, move |s| {
|
||||
for (name, value) in s {
|
||||
if name == "gnss_fix" {
|
||||
*fix.lock().unwrap() = value.as_str().unwrap_or("none").to_string();
|
||||
}
|
||||
if connected.load(Ordering::Relaxed) {
|
||||
let unit = units.get(name).unwrap_or_else(|| panic!("signal {name} is not a gnss signal in contracts signals.json"));
|
||||
let env = json!({ "timestamp": now_us(), "monotonic": mono_ns(), "value": value, "unit": unit });
|
||||
let _ = client.try_publish(format!("yukart/gnss/{name}"), QoS::AtMostOnce, true, env.to_string());
|
||||
}
|
||||
}
|
||||
true
|
||||
});
|
||||
}
|
||||
ExitCode::SUCCESS
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn get(s: &[(&str, Value)], k: &str) -> Value {
|
||||
s.iter().find(|(n, _)| *n == k).map(|(_, v)| v.clone()).unwrap()
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_signal_is_registered() {
|
||||
let u = units();
|
||||
let tpv = signals(&json!({"class": "TPV", "mode": 3, "lat": 1.0, "lon": 2.0, "altMSL": 3.0, "speed": 1.0, "track": 4.0}));
|
||||
let sky = signals(&json!({"class": "SKY", "uSat": 9}));
|
||||
for (k, _) in tpv.iter().chain(sky.iter()) {
|
||||
assert!(u.contains_key(*k), "{k} not in signals.json as a gnss signal");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_fix_becomes_values() {
|
||||
// From the 2026-10-13 checklist session.
|
||||
let s = signals(&json!({"class":"TPV","mode":3,"lat":36.6033299,"lon":139.95439805,"altMSL":162.0,"speed":1.67,"track":87.25}));
|
||||
assert_eq!(get(&s, "gnss_fix"), json!("3d"));
|
||||
assert_eq!(get(&s, "gnss_speed_kph"), json!(6.0));
|
||||
assert_eq!(get(&s, "gnss_heading_deg"), json!(87.3));
|
||||
assert_eq!(get(&s, "gnss_lat"), json!(36.6033299));
|
||||
assert_eq!(get(&s, "gnss_alt_m"), json!(162.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn missing_fix_and_standstill_are_null() {
|
||||
let s = signals(&json!({"class":"TPV","mode":1}));
|
||||
assert_eq!(get(&s, "gnss_fix"), json!("none"));
|
||||
assert_eq!(get(&s, "gnss_speed_kph"), Value::Null);
|
||||
let s = signals(&json!({"class":"TPV","mode":2,"lat":1.0,"lon":2.0,"alt":5.0,"speed":0.1,"track":200.0}));
|
||||
assert_eq!(get(&s, "gnss_alt_m"), Value::Null); // 2D: no altitude
|
||||
assert_eq!(get(&s, "gnss_heading_deg"), Value::Null); // standing still
|
||||
assert_eq!(get(&s, "gnss_speed_kph"), json!(0.4));
|
||||
let s = signals(&json!({"class":"SKY","satellites":[{"used":true},{"used":false},{"used":true}]}));
|
||||
assert_eq!(get(&s, "gnss_satellites"), json!(2));
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
+136
-143
@@ -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)
|
||||
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)
|
||||
--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)
|
||||
--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,67 +71,27 @@ 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;
|
||||
};
|
||||
match self.last {
|
||||
Some((prev, _)) if n != prev => self.last = Some((n, Instant::now())),
|
||||
None => self.last = Some((n, Instant::now() - Duration::from_secs(10))),
|
||||
_ => {}
|
||||
}
|
||||
self.last.map(|(_, t)| t.elapsed() < Duration::from_secs(1)).unwrap_or(false)
|
||||
}
|
||||
}
|
||||
|
||||
struct Poller {
|
||||
sock: ObdSocket,
|
||||
timeout: Duration,
|
||||
}
|
||||
|
||||
impl Poller {
|
||||
/// One mode 01 request; the first reply to it from any ECU: (receive time µs, ECU id, data bytes).
|
||||
fn ask(&self, pid: u8) -> std::io::Result<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>> {
|
||||
/// 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;
|
||||
while let Some((_, _, d)) = self.ask(mask)? {
|
||||
if d.len() < 4 {
|
||||
break;
|
||||
}
|
||||
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,
|
||||
};
|
||||
let s = pids::supported_in(mask, &d);
|
||||
let more = s.contains(&(mask + 0x20));
|
||||
all.extend(s.into_iter().filter(|p| p % 0x20 != 0));
|
||||
@@ -134,17 +100,18 @@ impl Poller {
|
||||
}
|
||||
mask += 0x20;
|
||||
}
|
||||
Ok(all)
|
||||
}
|
||||
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" });
|
||||
if elm.is_none() {
|
||||
if Instant::now() < next_connect {
|
||||
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 }),
|
||||
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));
|
||||
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 }))
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
//! The Bluetooth link to the ELM327 (the OBD-II adapter on the K-line), as BlueZ sees
|
||||
//! it. Only watched, never opened: the adapter takes one connection, and obd-poller
|
||||
//! holds it while the ECU answers.
|
||||
|
||||
use std::process::Command;
|
||||
|
||||
use serde_json::{json, Value};
|
||||
|
||||
pub struct Elm {
|
||||
/// "00:0C:BF:50:AF:3F"; None when no adapter is configured.
|
||||
addr: Option<String>,
|
||||
}
|
||||
|
||||
/// `bluetoothctl info` says "Connected: yes" while any link to the device is up,
|
||||
/// the poller's serial link included. An unknown device is simply not connected.
|
||||
fn connected(addr: &str) -> bool {
|
||||
Command::new("bluetoothctl")
|
||||
.args(["info", addr])
|
||||
.output()
|
||||
.map(|o| String::from_utf8_lossy(&o.stdout).lines().any(|l| l.trim() == "Connected: yes"))
|
||||
.unwrap_or(false)
|
||||
}
|
||||
|
||||
impl Elm {
|
||||
pub fn new(addr: &str) -> Self {
|
||||
Elm { addr: Some(addr.trim().to_string()).filter(|a| !a.is_empty()) }
|
||||
}
|
||||
|
||||
pub fn read(&mut self, out: &mut Vec<(&'static str, Value)>) {
|
||||
if let Some(a) = &self.addr {
|
||||
out.push(("elm_connected", json!(connected(a))));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,7 +1,8 @@
|
||||
//! orin-status — the Orin's own health on the bus, once a second.
|
||||
//!
|
||||
//! Temperatures, CPU and GPU load, the shared memory pool, module power, the fan,
|
||||
//! network links (WiFi, LTE, the USB link to the tablet), the CAN interface and the UPS battery, each
|
||||
//! network links (WiFi, LTE, the USB link to the tablet), the CAN interface, the Bluetooth
|
||||
//! link to the OBD-II adapter and the UPS battery, each
|
||||
//! as an envelope on `yukart/host/<signal>`, retained. Names and units come from
|
||||
//! contracts' `signals.json` (source `host`).
|
||||
//!
|
||||
@@ -10,6 +11,7 @@
|
||||
//! `yukart/system/orin-status`, with a last will for when it vanishes.
|
||||
|
||||
mod can;
|
||||
mod elm;
|
||||
mod host;
|
||||
mod net;
|
||||
mod ups;
|
||||
@@ -27,6 +29,7 @@ const USAGE: &str = "\
|
||||
usage: orin-status [options]
|
||||
--broker HOST:PORT MQTT broker (127.0.0.1:1883)
|
||||
--can IFACE CAN interface to report (can_pcan)
|
||||
--elm ADDR OBD-II adapter's Bluetooth address to report; '' for none ('')
|
||||
--i2c DEV I²C bus of the UPS INA219 (/dev/i2c-7)
|
||||
--ups-addr ADDR INA219 address, hex (41)
|
||||
--once print one reading as JSON and exit (no broker)";
|
||||
@@ -60,6 +63,7 @@ struct Sources {
|
||||
host: host::Host,
|
||||
net: net::Net,
|
||||
can: can::Can,
|
||||
elm: elm::Elm,
|
||||
ups: ups::Ups,
|
||||
}
|
||||
|
||||
@@ -71,6 +75,7 @@ impl Sources {
|
||||
self.host.read(&mut out);
|
||||
self.net.read(&mut out);
|
||||
self.can.read(&mut out);
|
||||
self.elm.read(&mut out);
|
||||
if !self.ups.read(&mut out) {
|
||||
failed.push("ups");
|
||||
}
|
||||
@@ -82,6 +87,7 @@ fn main() -> ExitCode {
|
||||
let mut broker = "127.0.0.1:1883".to_string();
|
||||
let mut i2c = "/dev/i2c-7".to_string();
|
||||
let mut can_iface = "can_pcan".to_string();
|
||||
let mut elm_addr = String::new();
|
||||
let mut addr: u16 = 0x41;
|
||||
let mut once = false;
|
||||
let mut args = std::env::args().skip(1);
|
||||
@@ -91,6 +97,7 @@ fn main() -> ExitCode {
|
||||
"--broker" => broker = args.next().unwrap_or_default(),
|
||||
"--i2c" => i2c = args.next().unwrap_or_default(),
|
||||
"--can" => can_iface = args.next().unwrap_or_default(),
|
||||
"--elm" => elm_addr = args.next().unwrap_or_default(),
|
||||
"--ups-addr" => match args.next().and_then(|v| u16::from_str_radix(v.trim_start_matches("0x"), 16).ok()) {
|
||||
Some(v) => addr = v,
|
||||
None => {
|
||||
@@ -106,7 +113,7 @@ fn main() -> ExitCode {
|
||||
}
|
||||
|
||||
let units = units();
|
||||
let mut src = Sources { host: host::Host::discover(), net: net::Net::start(), can: can::Can::new(&can_iface), ups: ups::Ups::new(&i2c, addr) };
|
||||
let mut src = Sources { host: host::Host::discover(), net: net::Net::start(), can: can::Can::new(&can_iface), elm: elm::Elm::new(&elm_addr), ups: ups::Ups::new(&i2c, addr) };
|
||||
|
||||
if once {
|
||||
src.read(); // rates and CPU load need two readings
|
||||
|
||||
Reference in New Issue
Block a user