From de0bc959b096a9283ec61539702476b3238e4536 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Thu, 25 Jun 2026 07:29:00 +0200 Subject: [PATCH] =?UTF-8?q?fix(dronecan):=20correct=20DSDL=20bit-serializa?= =?UTF-8?q?tion=20=E2=80=94=20conformance=20bug=20caught=20by=20pydronecan?= =?UTF-8?q?=20reference=20vectors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A flight-critical wire-conformance bug in the shipped v1.92-94 decoders, found by validating against pydronecan (the reference impl PX4/ArduPilot's libcanard conforms to) — exactly the external oracle a self-constructed round-trip test cannot be. THE BUG: the hand-rolled bit-field decoders used LSB-first packing, but DroneCAN v0 DSDL uses be_from_le_bits (8-bit-chunk reversal: byte-aligned fields are little-endian — which is why the float16/u32 decodes were already correct — but sub-byte/multi-byte bit fields are MSB-first-per-stream-byte). Against the canonical reference: - NodeStatus health/mode/sub_mode: read 1/6/4 instead of 2/3/1. - esc.RawCommand int14 (FLIGHT-CRITICAL): [8191,0,-8192,4096] read as [-1241,899,0,16] — falcon would have sent garbage throttles to a real ESC. - esc.Status rpm/power_rating/esc_index: wrong bit fields. - MagneticFieldStrength: a phantom ahrs_id (that field is in MagneticFieldStrength2/1003), shifting the field by a byte. WHY EVERYTHING MISSED IT: 40 tests, 9 Kani harnesses, clean-room 10/10 all passed — they proved robustness + self-consistency (no panic, bounded, encode∘decode = identity), all true and all blind to a spec mismatch, because every test used the same wrong convention for both encode and decode. The textbook limit of self-referential testing. THE FIX: - crate::dsdl — a faithful port of be_from_le_bits / le_from_be_bits (the DSDL bit codec), bounds-guarded + total. Kani DC-K08 verify_dsdl_read_bounded. - decode_node_status / encode+decode_raw_command (msg.rs), decode_esc_status + decode_mag (sensors.rs) rewritten to use it. Byte-aligned float16/u32 paths unchanged (already correct). The buggy LSB-first read_bits removed. - CONFORMANCE tests: canonical pydronecan payloads (0403020199efbe NodeStatus, ff7c0000080010 RawCommand, etc.) decoded + asserted to their reference field values; encoders asserted to reproduce the canonical bytes. scripts/gen- dronecan-vectors.py regenerates them. 42 tests (was 36 + the conformance vectors), 9/9 Kani SUCCESSFUL, clippy clean, no external breakage (relay-dronecan is a leaf). rivet FV-FALCON-DC-CONFORMANCE verifies DC-P02/P03 on the conformance axis; validate PASS, 0 gaps. NOTE: encode_node_status (in the un-merged v1.95) needs the same dsdl::write_uint treatment when v1.95 lands — tracked. Falsification: this fix is wrong if a relay-dronecan decoder disagrees with a pydronecan-encoded canonical frame for any message in the conformance test set. Co-Authored-By: Claude Opus 4.8 --- .../FV-FALCON-DC-CONFORMANCE.yaml | 60 +++++++ crates/relay-dronecan/plain/src/dsdl.rs | 149 ++++++++++++++++++ .../relay-dronecan/plain/src/kani_proofs.rs | 28 ++-- crates/relay-dronecan/plain/src/lib.rs | 1 + crates/relay-dronecan/plain/src/msg.rs | 71 +++++---- crates/relay-dronecan/plain/src/sensors.rs | 116 +++++--------- scripts/gen-dronecan-vectors.py | 67 ++++++++ 7 files changed, 366 insertions(+), 126 deletions(-) create mode 100644 artifacts/verification/FV-FALCON-DC-CONFORMANCE.yaml create mode 100644 crates/relay-dronecan/plain/src/dsdl.rs create mode 100644 scripts/gen-dronecan-vectors.py diff --git a/artifacts/verification/FV-FALCON-DC-CONFORMANCE.yaml b/artifacts/verification/FV-FALCON-DC-CONFORMANCE.yaml new file mode 100644 index 0000000..9e47460 --- /dev/null +++ b/artifacts/verification/FV-FALCON-DC-CONFORMANCE.yaml @@ -0,0 +1,60 @@ +artifacts: + - id: FV-FALCON-DC-CONFORMANCE + type: sw-verification + title: "DroneCAN wire conformance — pydronecan reference-vector validation (fixes the DSDL bit-order bug)" + status: implemented + description: > + Verifies SWREQ-FALCON-DC-P02 + SWREQ-FALCON-DC-P03 along the CONFORMANCE + axis (does the decode match the real DSDL?), which the prior FV-FALCON-DC-002 + /003 (robustness + self-consistency) could not: every self-constructed + round-trip test passed because encode + decode shared the same WRONG bit + order. + + THE BUG (found 2026-06 by running external reference vectors): the + hand-rolled bit-field decoders used LSB-first packing, but DroneCAN v0 DSDL + uses `be_from_le_bits` (8-bit-chunk reversal — byte-aligned fields are + little-endian, sub-byte/multi-byte bit fields are MSB-first-per-stream-byte). + Affected, against the pydronecan reference (the impl PX4/ArduPilot libcanard + conforms to): + - NodeStatus health/mode/sub_mode: read 1/6/4 instead of 2/3/1. + - esc.RawCommand int14 (FLIGHT-CRITICAL): [8191,0,-8192,4096] read as + [-1241,899,0,16] — falcon would have sent garbage throttles to a real ESC. + - esc.Status rpm/power_rating/esc_index: wrong bit fields. + - MagneticFieldStrength: a phantom ahrs_id (that field is in + MagneticFieldStrength2/1003), shifting the whole field by a byte. + + THE FIX: a faithful DSDL bit codec (crate::dsdl, be_from_le_bits / + le_from_be_bits) replacing the LSB-first readers; the byte-aligned float16/ + u32 paths (already correct) are unchanged. Validated against canonical + pydronecan payloads embedded as CONFORMANCE tests; the vectors are + regenerable via scripts/gen-dronecan-vectors.py. + + Evidence (crates/relay-dronecan), each a real named test/harness: + - msg::tests::node_status_decodes_fields (canonical 0403020199efbe). + - msg::tests::raw_command_matches_pydronecan_reference (canonical + ff7c0000080010 decodes [8191,0,-8192,4096]; encode reproduces the bytes). + - sensors::tests::esc_status_decodes_fields (070000000049003c885c8813190c). + - sensors::tests::mag_decodes_three_axes (003c00c00038, no ahrs_id). + - sensors::tests::baro_decodes_pressure_f32 / battery_prefix_decodes_voltage_current. + - dsdl::tests::raw_command_int14_against_reference / write_reproduces_reference_bytes. + - Kani DC-K08 verify_dsdl_read_bounded (the codec is total + result < 2^w). + ORACLE: `cargo test -p relay-dronecan` green (42 tests); the pydronecan + vectors decode to their canonical field values. + tags: [verification, falcon, dronecan, conformance, pydronecan, reference-vectors, relay-dronecan, kani, fix] + fields: + method: formal-verification + steps: + - run: "cargo test -p relay-dronecan dsdl::tests::raw_command_int14_against_reference" + - run: "cargo test -p relay-dronecan dsdl::tests::write_reproduces_reference_bytes" + - run: "cargo test -p relay-dronecan msg::tests::node_status_decodes_fields" + - run: "cargo test -p relay-dronecan msg::tests::raw_command_matches_pydronecan_reference" + - run: "cargo test -p relay-dronecan sensors::tests::esc_status_decodes_fields" + - run: "cargo test -p relay-dronecan sensors::tests::mag_decodes_three_axes" + - run: "cargo test -p relay-dronecan sensors::tests::baro_decodes_pressure_f32" + - run: "cargo test -p relay-dronecan sensors::tests::battery_prefix_decodes_voltage_current" + - run: "cargo kani -p relay-dronecan --harness verify_dsdl_read_bounded" + links: + - type: verifies + target: SWREQ-FALCON-DC-P02 + - type: verifies + target: SWREQ-FALCON-DC-P03 diff --git a/crates/relay-dronecan/plain/src/dsdl.rs b/crates/relay-dronecan/plain/src/dsdl.rs new file mode 100644 index 0000000..dde9c2a --- /dev/null +++ b/crates/relay-dronecan/plain/src/dsdl.rs @@ -0,0 +1,149 @@ +//! Faithful port of the DroneCAN v0 (UAVCAN v0) DSDL bit serialization. +//! +//! THE conformance core. The wire convention (pydronecan's `be_from_le_bits` / +//! `le_from_be_bits`, the same the PX4/ArduPilot libcanard implementations use): +//! the bit stream is filled MSB-first within each byte, and a primitive of width +//! `w` is stored byte-**little-endian** with each byte emitted MSB-first. +//! +//! Consequence — and the bug this module fixes: a BYTE-ALIGNED field reduces to +//! `from_le_bytes` (which is why the float16/u32 decodes were already correct), +//! but a SUB-BYTE or BIT-MISALIGNED field (NodeStatus health/mode, esc.RawCommand +//! int14, esc.Status rpm) is NOT simple LSB-first packing — the earlier +//! hand-rolled `pos/8, 1<<(pos%8)` readers were wrong against the real DSDL, and +//! every self-constructed round-trip test missed it because encode and decode +//! shared the same wrong convention. Validated here against pydronecan reference +//! vectors (`tests::pydronecan_reference_vectors` in the message modules). +//! +//! All reads/writes are bounds-guarded — total, never panic. + +/// One stream bit at global position `p` (MSB-first within each byte). +/// Out-of-range -> 0. +#[inline] +fn stream_bit(payload: &[u8], p: usize) -> u64 { + let byte = p / 8; + if byte >= payload.len() { + return 0; + } + ((payload[byte] >> (7 - (p % 8))) & 1) as u64 +} + +/// Read a `w`-bit unsigned DSDL primitive at bit offset `o` (w <= 64). +/// `be_from_le_bits`: the value's bytes are little-endian, each byte MSB-first +/// in the stream — i.e. 8-bit chunks of the field are reversed. Total. +pub fn read_uint(payload: &[u8], o: usize, w: usize) -> u64 { + let mut val: u64 = 0; + let nchunks = w.div_ceil(8); + let mut chunk = nchunks; + // most-significant chunk (highest stream offset) first + while chunk > 0 { + chunk -= 1; + let start = chunk * 8; + let end = if start + 8 < w { start + 8 } else { w }; + let mut c: u64 = 0; + let mut i = start; + while i < end { + c = (c << 1) | stream_bit(payload, o + i); + i += 1; + } + val = (val << (end - start)) | c; + } + val +} + +/// Read a `w`-bit signed DSDL primitive (two's complement), sign-extended to i64. +pub fn read_int(payload: &[u8], o: usize, w: usize) -> i64 { + let v = read_uint(payload, o, w); + if w == 0 || w >= 64 { + return v as i64; + } + if v & (1 << (w - 1)) != 0 { + (v as i64) - (1i64 << w) + } else { + v as i64 + } +} + +/// Write a `w`-bit unsigned value at bit offset `o` (`le_from_be_bits`, the +/// inverse of [`read_uint`]). The value's little-endian bytes are emitted +/// MSB-first per stream byte. Out-of-range writes are dropped (total). Only the +/// low `w` bits of `value` are used. +pub fn write_uint(payload: &mut [u8], o: usize, w: usize, value: u64) { + let nchunks = w.div_ceil(8); + let mut chunk = 0; + while chunk < nchunks { + let start = chunk * 8; + let end = if start + 8 < w { start + 8 } else { w }; + let cbits = end - start; + let byte = (value >> (chunk * 8)) & 0xFF; + let mut k = 0; + while k < cbits { + let bit = (byte >> (cbits - 1 - k)) & 1; // MSB-first within the chunk + let p = o + start + k; + let pb = p / 8; + if pb < payload.len() { + let mask = 1u8 << (7 - (p % 8)); + if bit != 0 { + payload[pb] |= mask; + } else { + payload[pb] &= !mask; + } + } + k += 1; + } + chunk += 1; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn byte_aligned_matches_le_bytes() { + // a byte-aligned u32 read == from_le_bytes (proves the float16/u32 path + // was always correct). + let p = [0x04, 0x03, 0x02, 0x01]; + assert_eq!(read_uint(&p, 0, 32), 0x0102_0304); + } + + #[test] + fn nodestatus_subbyte_fields() { + // byte 4 = 0x99 packs health=2 / mode=3 / sub_mode=1 (MSB-first), per the + // pydronecan canonical NodeStatus 0403020199efbe. + let p = [0x04, 0x03, 0x02, 0x01, 0x99, 0xef, 0xbe]; + assert_eq!(read_uint(&p, 32, 2), 2); // health + assert_eq!(read_uint(&p, 34, 3), 3); // mode + assert_eq!(read_uint(&p, 37, 3), 1); // sub_mode + } + + #[test] + fn raw_command_int14_against_reference() { + // pydronecan canonical RawCommand [8191,0,-8192,4096] = ff7c0000080010 + let p = [0xff, 0x7c, 0x00, 0x00, 0x08, 0x00, 0x10]; + assert_eq!(read_int(&p, 0, 14), 8191); + assert_eq!(read_int(&p, 14, 14), 0); + assert_eq!(read_int(&p, 28, 14), -8192); + assert_eq!(read_int(&p, 42, 14), 4096); + } + + #[test] + fn write_then_read_round_trips() { + // the codec is self-inverse for any offset/width/value in range + for &(o, w, v) in &[(0usize, 14usize, 8191u64), (14, 14, 0x2000), (32, 2, 2), (34, 3, 5), (5, 18, 0x3ABCD)] { + let mut buf = [0u8; 16]; + write_uint(&mut buf, o, w, v); + assert_eq!(read_uint(&buf, o, w), v & ((1u64 << w) - 1), "o={o} w={w}"); + } + } + + #[test] + fn write_reproduces_reference_bytes() { + // encoding [8191,0,-8192,4096] must reproduce the pydronecan bytes + let mut buf = [0u8; 7]; + write_uint(&mut buf, 0, 14, 8191); + write_uint(&mut buf, 14, 14, 0); + write_uint(&mut buf, 28, 14, (-8192i64 as u64) & 0x3FFF); + write_uint(&mut buf, 42, 14, 4096); + assert_eq!(buf, [0xff, 0x7c, 0x00, 0x00, 0x08, 0x00, 0x10]); + } +} diff --git a/crates/relay-dronecan/plain/src/kani_proofs.rs b/crates/relay-dronecan/plain/src/kani_proofs.rs index 441cb54..ebb5380 100644 --- a/crates/relay-dronecan/plain/src/kani_proofs.rs +++ b/crates/relay-dronecan/plain/src/kani_proofs.rs @@ -6,9 +6,9 @@ //! target (totality + bounds), out of Verus's productive range. #![cfg(kani)] +use crate::dsdl; use crate::id::{decode_message_id, encode_message_id, MessageId}; use crate::msg::{decode_node_status, encode_raw_command, MAX_ESC}; -use crate::sensors::read_bits; use crate::tail::{decode_tail, encode_tail}; use crate::transfer::{encode_single_frame, CanFrame, Reassembler, MAX_PAYLOAD}; @@ -139,20 +139,18 @@ fn verify_encode_single_frame_bounded() { } } -/// DC-K08 — the sensor bit-reader is total and range-bounded: for ANY payload, -/// bit offset, and width (<= 18, the widest esc.Status field), `read_bits` never -/// panics / never indexes out of bounds, and the result is `< 2^nbits`. The -/// integer floor under the esc.Status decode (the float16 telemetry values are -/// proptest-gated — Kani on f32 is intractable). +/// DC-K08 — the DSDL bit-codec is total and range-bounded: for ANY payload, bit +/// offset, and width (<= 18, the widest esc.Status bit-field), `dsdl::read_uint` +/// never panics / never indexes out of bounds, and the result is `< 2^w`. The +/// integer floor under every DSDL bit-field decode (the conformance core; the +/// float16 telemetry values are proptest-gated — Kani on f32 is intractable). #[kani::proof] -fn verify_read_bits_bounded() { +fn verify_dsdl_read_bounded() { let payload: [u8; 14] = kani::any(); - let bit_off: usize = kani::any(); - kani::assume(bit_off <= 112); // within the 14-byte (112-bit) esc.Status frame - let nbits: u32 = kani::any(); - kani::assume(nbits <= 18); - let v = read_bits(&payload, bit_off, nbits); - if nbits < 32 { - assert!(v < (1u32 << nbits)); - } + let o: usize = kani::any(); + kani::assume(o <= 112); // within the 14-byte (112-bit) esc.Status frame + let w: usize = kani::any(); + kani::assume(w >= 1 && w <= 18); + let v = dsdl::read_uint(&payload, o, w); + assert!(v < (1u64 << w)); } diff --git a/crates/relay-dronecan/plain/src/lib.rs b/crates/relay-dronecan/plain/src/lib.rs index 33b6cc3..03fe69e 100644 --- a/crates/relay-dronecan/plain/src/lib.rs +++ b/crates/relay-dronecan/plain/src/lib.rs @@ -24,6 +24,7 @@ #![forbid(unsafe_code)] pub mod crc; +pub mod dsdl; pub mod float16; pub mod id; pub mod msg; diff --git a/crates/relay-dronecan/plain/src/msg.rs b/crates/relay-dronecan/plain/src/msg.rs index e8b7073..bc4355b 100644 --- a/crates/relay-dronecan/plain/src/msg.rs +++ b/crates/relay-dronecan/plain/src/msg.rs @@ -13,6 +13,8 @@ //! one mandatory message): node uptime/health/mode — the bus-presence + //! node-health source for FDI. +use crate::dsdl; + /// Data-type id of uavcan.protocol.NodeStatus. pub const DTID_NODE_STATUS: u16 = 341; /// Data-type id of uavcan.equipment.esc.RawCommand. @@ -45,10 +47,13 @@ pub fn decode_node_status(payload: &[u8]) -> Option { return None; } Some(NodeStatus { + // byte-aligned fields are little-endian (== DSDL be_from_le_bits); the + // sub-byte fields use the DSDL bit codec (MSB-first within the byte) — + // the conformance fix vs the canonical pydronecan encoding. uptime_sec: u32::from_le_bytes([payload[0], payload[1], payload[2], payload[3]]), - health: payload[4] & 0x03, - mode: (payload[4] >> 2) & 0x07, - sub_mode: (payload[4] >> 5) & 0x07, + health: dsdl::read_uint(payload, 32, 2) as u8, + mode: dsdl::read_uint(payload, 34, 3) as u8, + sub_mode: dsdl::read_uint(payload, 37, 3) as u8, vendor_status: u16::from_le_bytes([payload[5], payload[6]]), }) } @@ -67,18 +72,9 @@ pub fn encode_raw_command(channels: &[i16], out: &mut [u8]) -> usize { for b in out[..nbytes].iter_mut() { *b = 0; } - let mut bit = 0usize; - for &c in channels.iter().take(n) { - let v = (c.clamp(ESC_CMD_MIN, ESC_CMD_MAX) as u16) & 0x3FFF; // 14-bit two's complement - let mut k = 0; - while k < 14 { - if v & (1 << k) != 0 { - let pos = bit + k; - out[pos / 8] |= 1 << (pos % 8); - } - k += 1; - } - bit += 14; + for (i, &c) in channels.iter().take(n).enumerate() { + let v = (c.clamp(ESC_CMD_MIN, ESC_CMD_MAX) as u16 & 0x3FFF) as u64; // int14 two's complement + dsdl::write_uint(out, i * 14, 14, v); // DSDL bit packing (conformance fix) } nbytes } @@ -97,20 +93,9 @@ pub struct RawCommand { pub fn decode_raw_command(payload: &[u8]) -> RawCommand { let n = (payload.len() * 8 / 14).min(MAX_ESC); let mut channels = [0i16; MAX_ESC]; - let mut bit = 0usize; - for ch in channels.iter_mut().take(n) { - let mut v: u16 = 0; - let mut k = 0; - while k < 14 { - let pos = bit + k; - if pos / 8 < payload.len() && payload[pos / 8] & (1 << (pos % 8)) != 0 { - v |= 1 << k; - } - k += 1; - } - // sign-extend the 14-bit two's complement to i16 - *ch = if v & 0x2000 != 0 { (v | 0xC000) as i16 } else { v as i16 }; - bit += 14; + for (i, ch) in channels.iter_mut().take(n).enumerate() { + // DSDL int14, sign-extended (conformance fix) + *ch = dsdl::read_int(payload, i * 14, 14) as i16; } RawCommand { channels, count: n } } @@ -119,19 +104,35 @@ pub fn decode_raw_command(payload: &[u8]) -> RawCommand { mod tests { use super::*; + /// CONFORMANCE: the canonical pydronecan NodeStatus(uptime=0x01020304, + /// health=2, mode=3, sub_mode=1, vendor=0xBEEF) is `0403020199efbe` — + /// byte 4 = 0x99 (MSB-first sub-byte fields). Catches the LSB-first bug. #[test] fn node_status_decodes_fields() { - // uptime 0x01020304, byte4 = health 2 | mode 5<<2 | sub_mode 1<<5 = - // 0b001_101_10 = 0x36 ; vendor 0xBEEF - let payload = [0x04, 0x03, 0x02, 0x01, 0b0010_1110, 0xEF, 0xBE]; + let payload = [0x04, 0x03, 0x02, 0x01, 0x99, 0xEF, 0xBE]; let ns = decode_node_status(&payload).unwrap(); assert_eq!(ns.uptime_sec, 0x0102_0304); - assert_eq!(ns.health, 0b10); // bits 0-1 - assert_eq!(ns.mode, 0b011); // bits 2-4 - assert_eq!(ns.sub_mode, 0b001); // bits 5-7 + assert_eq!(ns.health, 2); + assert_eq!(ns.mode, 3); + assert_eq!(ns.sub_mode, 1); assert_eq!(ns.vendor_status, 0xBEEF); } + /// CONFORMANCE (flight-critical): the canonical pydronecan RawCommand + /// [8191, 0, -8192, 4096] is `ff7c0000080010`. Decoding it must recover the + /// commands; encoding must reproduce the bytes. This is the bug that would + /// have sent garbage throttles to a real ESC. + #[test] + fn raw_command_matches_pydronecan_reference() { + let canonical = [0xff, 0x7c, 0x00, 0x00, 0x08, 0x00, 0x10]; + let decoded = decode_raw_command(&canonical); + assert_eq!(&decoded.channels[..4], &[8191, 0, -8192, 4096]); + let mut buf = [0u8; 7]; + let n = encode_raw_command(&[8191, 0, -8192, 4096], &mut buf); + assert_eq!(n, 7); + assert_eq!(buf, canonical); + } + #[test] fn node_status_rejects_short_payload() { assert_eq!(decode_node_status(&[0; 6]), None); diff --git a/crates/relay-dronecan/plain/src/sensors.rs b/crates/relay-dronecan/plain/src/sensors.rs index b60ac13..8445eb9 100644 --- a/crates/relay-dronecan/plain/src/sensors.rs +++ b/crates/relay-dronecan/plain/src/sensors.rs @@ -8,6 +8,7 @@ //! baro), BatteryInfo prefix (power monitor over CAN). gnss.Fix2 (variable //! covariance tail) is the v1.95 follow-on. +use crate::dsdl; use crate::float16::f16_to_f32; /// Data-type ids of the sensor messages this module decodes. @@ -16,22 +17,6 @@ pub const DTID_MAG: u16 = 1002; // uavcan.equipment.ahrs.MagneticFieldStrength pub const DTID_BARO: u16 = 1028; // uavcan.equipment.air_data.StaticPressure pub const DTID_BATTERY: u16 = 1092; // uavcan.equipment.power.BatteryInfo -/// Read `nbits` (<= 32) starting at LSB-first `bit_off` from `payload`. Bounded: -/// bits past the end read as 0, so it never panics and the result is always -/// `< 2^nbits` (Kani DC-K08). Used for the non-byte-aligned esc.Status fields. -pub(crate) fn read_bits(payload: &[u8], bit_off: usize, nbits: u32) -> u32 { - let mut v = 0u32; - let mut i = 0u32; - while i < nbits && i < 32 { - let pos = bit_off + i as usize; - if pos / 8 < payload.len() && (payload[pos / 8] >> (pos % 8)) & 1 != 0 { - v |= 1 << i; - } - i += 1; - } - v -} - /// uavcan.equipment.esc.Status (DTID 1034) — per-ESC telemetry; the FDI source. #[derive(Clone, Copy, Debug, PartialEq)] pub struct EscStatus { @@ -50,44 +35,40 @@ pub fn decode_esc_status(p: &[u8]) -> Option { if p.len() < 14 { return None; } - let rpm_raw = read_bits(p, 80, 18); - // sign-extend the 18-bit two's-complement rpm to i32 - let rpm = if rpm_raw & 0x0002_0000 != 0 { - (rpm_raw | 0xFFFC_0000) as i32 - } else { - rpm_raw as i32 - }; Some(EscStatus { + // byte-aligned: little-endian (== DSDL). Bit-misaligned: the DSDL codec. error_count: u32::from_le_bytes([p[0], p[1], p[2], p[3]]), voltage: f16_to_f32(u16::from_le_bytes([p[4], p[5]])), current: f16_to_f32(u16::from_le_bytes([p[6], p[7]])), temperature: f16_to_f32(u16::from_le_bytes([p[8], p[9]])), - rpm, - power_rating_pct: read_bits(p, 98, 7) as u8, - esc_index: read_bits(p, 105, 5) as u8, + rpm: dsdl::read_int(p, 80, 18) as i32, + power_rating_pct: dsdl::read_uint(p, 98, 7) as u8, + esc_index: dsdl::read_uint(p, 105, 5) as u8, }) } /// uavcan.equipment.ahrs.MagneticFieldStrength (DTID 1002) — external mag over CAN. #[derive(Clone, Copy, Debug, PartialEq)] pub struct MagField { - pub ahrs_id: u8, /// Magnetic field in gauss, [x, y, z]. pub field_ga: [f32; 3], } -/// Decode MagneticFieldStrength (7-byte single-frame: u8 + 3x float16). `None` -/// if shorter. Total. +/// Decode MagneticFieldStrength (6-byte fixed prefix: 3x float16). `None` if +/// shorter. Total. +/// +/// CONFORMANCE FIX: the DSDL has NO `ahrs_id` (that is MagneticFieldStrength2, +/// DTID 1003); `magnetic_field_ga[3]` starts at byte 0. The earlier decoder read +/// a phantom ahrs_id at byte 0 and shifted the field by one byte. pub fn decode_mag(p: &[u8]) -> Option { - if p.len() < 7 { + if p.len() < 6 { return None; } Some(MagField { - ahrs_id: p[0], field_ga: [ - f16_to_f32(u16::from_le_bytes([p[1], p[2]])), - f16_to_f32(u16::from_le_bytes([p[3], p[4]])), - f16_to_f32(u16::from_le_bytes([p[5], p[6]])), + f16_to_f32(u16::from_le_bytes([p[0], p[1]])), + f16_to_f32(u16::from_le_bytes([p[2], p[3]])), + f16_to_f32(u16::from_le_bytes([p[4], p[5]])), ], }) } @@ -138,39 +119,27 @@ pub fn decode_battery_info(p: &[u8]) -> Option { mod tests { use super::*; + /// CONFORMANCE: pydronecan canonical esc.Status(error_count=7, voltage=10.0, + /// current=1.0, temperature=290.0, rpm=5000, power_rating=50, esc_index=3) = + /// `070000000049003c885c8813190c`. The rpm/power_rating/esc_index bit-fields + /// use the DSDL codec (the LSB-first read_bits was wrong). #[test] fn esc_status_decodes_fields() { - let mut p = [0u8; 14]; - p[..4].copy_from_slice(&7u32.to_le_bytes()); // error_count = 7 - p[4..6].copy_from_slice(&0x4900u16.to_le_bytes()); // voltage = 10.0 - p[6..8].copy_from_slice(&0x3C00u16.to_le_bytes()); // current = 1.0 - p[8..10].copy_from_slice(&0x4000u16.to_le_bytes()); // temperature = 2.0 - // rpm at bit 80 (byte 10) = 5000; power_rating bit 98; esc_index bit 105 - let rpm: u32 = 5000; - for b in 0..18 { - if rpm & (1 << b) != 0 { - let pos = 80 + b; - p[pos / 8] |= 1 << (pos % 8); - } - } + let p = [0x07, 0, 0, 0, 0x00, 0x49, 0x00, 0x3c, 0x88, 0x5c, 0x88, 0x13, 0x19, 0x0c]; let s = decode_esc_status(&p).unwrap(); assert_eq!(s.error_count, 7); assert_eq!(s.voltage, 10.0); assert_eq!(s.current, 1.0); - assert_eq!(s.temperature, 2.0); + assert!((s.temperature - 290.0).abs() < 1.0); // float16 precision assert_eq!(s.rpm, 5000); + assert_eq!(s.power_rating_pct, 50); + assert_eq!(s.esc_index, 3); } + /// CONFORMANCE: pydronecan esc.Status with rpm=-1 = `...ffffc000`. #[test] fn esc_status_rpm_negative_sign_extends() { - let mut p = [0u8; 14]; - let neg18 = 0x3FFFFu32; // 18-bit -1 - for b in 0..18 { - if neg18 & (1 << b) != 0 { - let pos = 80 + b; - p[pos / 8] |= 1 << (pos % 8); - } - } + let p = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0xff, 0xff, 0xc0, 0x00]; assert_eq!(decode_esc_status(&p).unwrap().rpm, -1); } @@ -179,43 +148,38 @@ mod tests { assert_eq!(decode_esc_status(&[0; 13]), None); } + /// CONFORMANCE: pydronecan MagneticFieldStrength([1.0,-2.0,0.5]) = + /// `003c00c00038` — field_ga starts at byte 0, NO ahrs_id. #[test] fn mag_decodes_three_axes() { - let mut p = [0u8; 7]; - p[0] = 1; // ahrs_id - p[1..3].copy_from_slice(&0x3C00u16.to_le_bytes()); // 1.0 - p[3..5].copy_from_slice(&0xC000u16.to_le_bytes()); // -2.0 - p[5..7].copy_from_slice(&0x3800u16.to_le_bytes()); // 0.5 + let p = [0x00, 0x3c, 0x00, 0xc0, 0x00, 0x38]; let m = decode_mag(&p).unwrap(); - assert_eq!(m.ahrs_id, 1); assert_eq!(m.field_ga, [1.0, -2.0, 0.5]); } + /// CONFORMANCE: pydronecan StaticPressure(101325.0, variance 1.0) = `80e6c547003c`. #[test] fn baro_decodes_pressure_f32() { - let mut p = [0u8; 6]; - p[..4].copy_from_slice(&101325.0f32.to_le_bytes()); // sea-level Pa - p[4..6].copy_from_slice(&0x3C00u16.to_le_bytes()); // variance 1.0 + let p = [0x80, 0xe6, 0xc5, 0x47, 0x00, 0x3c]; let b = decode_baro(&p).unwrap(); assert_eq!(b.pressure_pa, 101325.0); assert_eq!(b.variance, 1.0); } + /// CONFORMANCE: pydronecan BatteryInfo(temp=300, voltage=22.2, current=5.0) + /// prefix = `b05c8d4d0045...`. #[test] fn battery_prefix_decodes_voltage_current() { - let mut p = [0u8; 6]; - p[0..2].copy_from_slice(&0x4900u16.to_le_bytes()); // temp 10.0 - p[2..4].copy_from_slice(&0x4D00u16.to_le_bytes()); // voltage ~20.0 - p[4..6].copy_from_slice(&0x4000u16.to_le_bytes()); // current 2.0 + let p = [0xb0, 0x5c, 0x8d, 0x4d, 0x00, 0x45]; let bt = decode_battery_info(&p).unwrap(); - assert_eq!(bt.temperature, 10.0); - assert_eq!(bt.voltage, 20.0); - assert_eq!(bt.current, 2.0); + assert!((bt.temperature - 300.0).abs() < 1.0); + assert!((bt.voltage - 22.2).abs() < 0.05); + assert!((bt.current - 5.0).abs() < 0.05); } #[test] fn short_payloads_reject_not_panic() { - assert_eq!(decode_mag(&[0; 6]), None); + assert_eq!(decode_mag(&[0; 5]), None); // mag now needs 6 bytes (no ahrs_id) assert_eq!(decode_baro(&[0; 5]), None); assert_eq!(decode_battery_info(&[0; 5]), None); } @@ -227,9 +191,9 @@ mod proptests { use proptest::prelude::*; proptest! { - /// All sensor decoders never panic for ANY payload (the float16 path - /// included); read_bits stays in range. The full-path totality companion - /// to the Kani read_bits proof. + /// All sensor decoders never panic for ANY payload (the float16 + DSDL + /// bit-codec paths included); the bit-fields stay in range. The full-path + /// totality companion to the Kani dsdl-read proof. #[test] fn sensor_decoders_never_panic(bytes in proptest::collection::vec(any::(), 0..40)) { if let Some(s) = decode_esc_status(&bytes) { diff --git a/scripts/gen-dronecan-vectors.py b/scripts/gen-dronecan-vectors.py new file mode 100644 index 0000000..5bf0b25 --- /dev/null +++ b/scripts/gen-dronecan-vectors.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +"""Generate authoritative DroneCAN v0 reference vectors for relay-dronecan tests. + +Uses pydronecan (the reference implementation PX4/ArduPilot's libcanard conforms +to) to produce canonical on-wire payloads for the messages relay-dronecan +decodes. These vectors are embedded as `CONFORMANCE` tests in the message modules +(msg.rs / sensors.rs) — the external oracle that caught the LSB-first bit-order +bug a self-constructed round-trip test could not. + +Setup: python3 -m venv /tmp/dcvec && /tmp/dcvec/bin/pip install dronecan +Run: /tmp/dcvec/bin/python scripts/gen-dronecan-vectors.py + +Each line: . The decoder must reproduce +the field values from the hex; the encoder (where falcon emits) must reproduce +the hex from the field values. +""" +from dronecan import uavcan + + +def wire(m): + """Canonical on-wire payload (TAO serialization for the top-level message).""" + b = m._pack(True) + b += "0" * ((8 - len(b) % 8) % 8) + return bytes(int(b[i:i + 8], 2) for i in range(0, len(b), 8)) + + +def rt(cls, by): + """Round-trip: decode the bytes back, to confirm they are canonical.""" + m = cls() + m._unpack("".join(f"{x:08b}" for x in by), True) + return m + + +def emit(tag, by, desc): + print(f"{tag:14s} {by.hex():46s} {desc}") + + +# uavcan.protocol.NodeStatus (341) — sub-byte fields health/mode/sub_mode +m = uavcan.protocol.NodeStatus() +m.uptime_sec, m.health, m.mode, m.sub_mode, m.vendor_specific_status_code = 0x01020304, 2, 3, 1, 0xBEEF +emit("node_status", wire(m), "uptime=0x01020304 health=2 mode=3 sub_mode=1 vendor=0xBEEF") + +# uavcan.equipment.esc.RawCommand (1030) — int14 array (FLIGHT-CRITICAL, falcon emits) +m = uavcan.equipment.esc.RawCommand() +m.cmd = [8191, 0, -8192, 4096] +emit("raw_command", wire(m), "cmd=[8191, 0, -8192, 4096]") + +# uavcan.equipment.esc.Status (1034) — float16 + int18 rpm bit-field +m = uavcan.equipment.esc.Status() +m.error_count, m.voltage, m.current, m.temperature = 7, 10.0, 1.0, 290.0 +m.rpm, m.power_rating_pct, m.esc_index = 5000, 50, 3 +emit("esc_status", wire(m), "error_count=7 V=10.0 I=1.0 T=290.0 rpm=5000 power_rating=50 esc_index=3") + +# uavcan.equipment.ahrs.MagneticFieldStrength (1002) — float16[3] from byte 0, NO ahrs_id +m = uavcan.equipment.ahrs.MagneticFieldStrength() +m.magnetic_field_ga = [1.0, -2.0, 0.5] +emit("mag", wire(m), "field_ga=[1.0, -2.0, 0.5]") + +# uavcan.equipment.air_data.StaticPressure (1028) — float32 + float16 +m = uavcan.equipment.air_data.StaticPressure() +m.static_pressure, m.static_pressure_variance = 101325.0, 1.0 +emit("baro", wire(m), "pressure=101325.0 variance=1.0") + +# uavcan.equipment.power.BatteryInfo (1092) — prefix temperature/voltage/current +m = uavcan.equipment.power.BatteryInfo() +m.temperature, m.voltage, m.current = 300.0, 22.2, 5.0 +emit("battery", wire(m), "temperature=300.0 voltage=22.2 current=5.0 (prefix)")