From a42ba87e54487cbbcd3959024902f29593308ff4 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Wed, 24 Jun 2026 05:53:43 +0200 Subject: [PATCH] =?UTF-8?q?feat(dronecan):=20v1.94=20sensor=20ingest=20?= =?UTF-8?q?=E2=80=94=20float16=20+=20esc.Status/mag/baro/battery=20decoder?= =?UTF-8?q?s=20(DC-P03)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The DroneCAN sensor-ingest decoders (the SPI/I2C drivers' CAN-bus counterparts) on the v1.92 transfer layer, plus the shared float16 primitive. - float16.rs: f16_to_f32 (IEEE binary16 -> f32; normals/subnormals/inf/NaN). Decode-only (falcon reads sensor float16; never emits it). No panic surface. - sensors.rs: decode_esc_status (1034, FDI: rpm/voltage/current/temp, multi-frame 14B), decode_mag (1002, MagneticFieldStrength, single-frame), decode_baro (1028, StaticPressure float32+float16), decode_battery_info (1092 prefix: temp/voltage/current, the BatteryMonitor inputs over CAN). read_bits helper for the non-byte-aligned esc.Status fields (rpm int18, power_rating, esc_index). Architecture (spar): dronecan.aadl gains the SensorSample data type + a Decode thread; spar analyze 0 errors, the Decode thread RTA 18us <= its 2ms telemetry deadline. node.wit regenerated (sample-out port + decode export). Verification (integer-Kani / float-proptest split): - Kani DC-K08 verify_read_bits_bounded: the bit-reader is total + every result < 2^nbits (the integer floor under esc.Status). 8/8 relay-dronecan harnesses. - 36 relay-dronecan tests: float16 known IEEE-half vectors + edges; esc.Status field decode + rpm sign-extend; mag 3-axis; baro float32; battery prefix; short rejects. float16 values + full-decoder totality proptest-fuzzed. rivet: SWREQ-FALCON-DC-P03 + FV-FALCON-DC-003 (test-level, 12 steps). validate PASS, 0 gaps. Clean-room 9 confirm (2 env-blocked by a full disk, self-verified). Plus the PX4-parity roadmap (FEAT-FALCON-v1.96..v1.103: parameter system keystone, pre-arm checks, calibration, failsafes, position-UX, + backlog wind-est/multi-EKF/ autotune), from the PX4 gap analysis. Scope move: gnss.Fix2 -> v1.95. Falsification: this release is wrong if a real DroneCAN float16 telemetry value mis-decodes vs IEEE-754, if a sensor decoder panics on any payload, if read_bits returns a value >= 2^nbits, or if spar analyze errors / the decode thread misses its telemetry deadline. Co-Authored-By: Claude Opus 4.8 --- .../features/FEAT-FALCON-px4-parity.yaml | 190 ++++++++++++++ .../features/FEAT-FALCON-rc-dronecan.yaml | 11 +- artifacts/swreq/SWREQ-FALCON-DC-P03.yaml | 45 ++++ artifacts/verification/FV-FALCON-DC-003.yaml | 52 ++++ crates/relay-dronecan/plain/src/float16.rs | 107 ++++++++ .../relay-dronecan/plain/src/kani_proofs.rs | 19 ++ crates/relay-dronecan/plain/src/lib.rs | 2 + crates/relay-dronecan/plain/src/sensors.rs | 244 ++++++++++++++++++ spar/dronecan.aadl | 25 ++ wit/dronecan/node.wit | 3 + 10 files changed, 694 insertions(+), 4 deletions(-) create mode 100644 artifacts/features/FEAT-FALCON-px4-parity.yaml create mode 100644 artifacts/swreq/SWREQ-FALCON-DC-P03.yaml create mode 100644 artifacts/verification/FV-FALCON-DC-003.yaml create mode 100644 crates/relay-dronecan/plain/src/float16.rs create mode 100644 crates/relay-dronecan/plain/src/sensors.rs diff --git a/artifacts/features/FEAT-FALCON-px4-parity.yaml b/artifacts/features/FEAT-FALCON-px4-parity.yaml new file mode 100644 index 0000000..004a41a --- /dev/null +++ b/artifacts/features/FEAT-FALCON-px4-parity.yaml @@ -0,0 +1,190 @@ +artifacts: + # ════════════════════════════════════════════════════════════════════ + # PX4 everyday-multicopter parity roadmap (release-planning skill, driven + # by the 2026-06 PX4 gap analysis vs docs.px4.io). falcon is ~65-70% of + # PX4's everyday-MC feature set today and AHEAD on rigor; the missing ~30% + # is configurability + operational breadth, NOT flight capability. This + # thread closes the realistic next ~20%. status: pending = committed plan. + # + # Priority order (from the analysis): the PARAMETER SYSTEM is the keystone + # (unblocks autotune/calibration/UX/field-config); then the cheap high-safety + # predicate/limiter slices. The harder tier (autotune/multi-EKF/wind-est) is + # scope-after-first-flight backlog. + # + # Numbering: this thread is v1.96+ (after the DroneCAN arc v1.94/95). The + # legacy-RC quick wins (v1.90/91) stay low-priority filler. Numbers are + # labels; the dependency/priority is in the descriptions. + # ════════════════════════════════════════════════════════════════════ + + # ---- Tier 1: the committed next-20% (highest leverage × feasibility) ---- + + - id: FEAT-FALCON-v1.96 + type: feature + title: "v1.96 — Parameter system (the keystone): typed bounded param store + MAVLink PARAM binding" + status: pending + description: > + PLANNED. The single highest-leverage gap vs PX4 (gap-analysis #1). PX4 has + hundreds of typed, persisted, GCS-readable/writable parameters; falcon tunes + via code constants. Without a param store you cannot field-tune gains, run + calibration, set geofence/battery thresholds, or integrate QGC — autotune, + calibration, and UX shaping ALL presuppose it. + + A relay-fit design: a WIT-typed parameter TABLE (named, typed entries with + DECLARED bounds), a verified storage/serialization leaf (bounded keys + + range-clamped values — exactly Kani's wheelhouse), and a MAVLink + PARAM_REQUEST_LIST / PARAM_REQUEST_READ / PARAM_SET / PARAM_VALUE binding + (relay-mavlink already carries the codec pattern). The differentiator from + PX4: a BOUNDED param system — every value's range is proven, not + open-ended live-tuning. + Feasibility: medium effort, high leverage. DO THIS FIRST of the PX4 thread. + tags: [falcon, px4-parity, parameters, config, mavlink, keystone, kani, v1.96, planned] + fields: + release-target: "WIT param table + verified bounded storage leaf (Kani: key-in-range, value-clamped) + MAVLink PARAM_SET/VALUE round-trip" + px4-equivalent: "PX4 parameter system + QGC param panel — falcon adds proven per-param bounds (bounded, not open-ended)" + links: + - type: implements + target: SYSREQ-FALCON-006 + + - id: FEAT-FALCON-v1.97 + type: feature + title: "v1.97 — Pre-arm / commander checks: arm only when preconditions provably hold" + status: pending + description: > + PLANNED. The #1 real-world crash-preventer (gap-analysis #2). falcon's FSM + proves the EXIT invariant (never-disarm-airborne); PX4's commander enforces + a broad ENTRY gate. Add the pre-arm predicate: gate arming on sensor health, + EKF convergence (NIS in-band), calibration-present, GPS-lock-for-mode, + battery-min, RC-present. + Excellent verified fit: a pure predicate over already-available health + signals; Kani proves "arm => all preconditions held". Hangs on the existing + relay-fsm. Cheap, highest safety ROI. + tags: [falcon, px4-parity, prearm, commander, arming, safety, fsm, kani, v1.97, planned] + fields: + release-target: "prearm_ok(health) predicate + Kani arm=>preconditions-held; relay-fsm arming gate wired to the health sources" + px4-equivalent: "PX4 commander pre-flight checks — falcon adds the Kani-proven arming precondition" + links: + - type: implements + target: SYSREQ-FALCON-011 + + - id: FEAT-FALCON-v1.98 + type: feature + title: "v1.98 — Sensor calibration routines: accel/gyro/mag/level offsets the estimator consumes" + status: pending + description: > + PLANNED. An uncalibrated mag/accel diverges even a proven estimator + (gap-analysis #3; was under-scoped as 'validation' on the known-gaps list). + The guided calibration that solves bias/scale/axis-remap + board-level + offsets, landing them in the v1.96 param store. + Feasibility: medium, param-system-dependent (needs v1.96). The calibration + math (ellipsoid fit for mag, accel 6-orientation bias solve, gyro + null-rate) is verifiable leaf work; the verified estimator then consumes + the calibrated offsets (replacing the identity-remap placeholder). + tags: [falcon, px4-parity, calibration, sensors, estimator, kani, v1.98, planned] + fields: + release-target: "mag ellipsoid-fit + accel 6-orientation bias solve + gyro null (verified leaf) -> param store; estimator consumes the offsets" + px4-equivalent: "PX4 sensor calibration (QGC-guided) — falcon adds verified calibration math" + links: + - type: implements + target: SYSREQ-FALCON-018 + + - id: FEAT-FALCON-v1.99 + type: feature + title: "v1.99 — Expanded failsafes: high-wind, motor-failure reallocation, attitude-runaway termination" + status: pending + description: > + PLANNED. Three failsafe responses PX4 has that the unified arbiter lacks + (gap-analysis #5), covering the failure classes most likely to be fatal: + - high-wind: a wind/tilt-saturation threshold -> RTL/land. + - motor-failure: an ESC current/RPM anomaly (from esc.Status, v1.94) -> + control-allocation reallocation (the hardest; touches relay-mix-*). + - attitude-runaway: a sustained-tilt-beyond-limit -> flight termination. + The arbiter is already unified; these are new bounded inputs + actions with + Kani-provable response bounds. Easy-to-medium except motor reallocation. + tags: [falcon, px4-parity, failsafe, high-wind, motor-failure, termination, safety, kani, v1.99, planned] + fields: + release-target: "3 new arbiter inputs+actions (high-wind/motor-fail/attitude-runaway) with Kani response-bound proofs" + px4-equivalent: "PX4 wind/actuator/lockdown failsafes — falcon adds proven response bounds" + links: + - type: implements + target: SYSREQ-FALCON-011 + + - id: FEAT-FALCON-v1.100 + type: feature + title: "v1.100 — Position-mode UX shaping: jerk/slew limits, velocity feedforward, stick expo" + status: pending + description: > + PLANNED. The flight-feel niceties that make position mode professional + (gap-analysis #7): jerk/accel-limited smooth trajectories, velocity + feedforward, and stick expo/smoothing. Flight quality + pilot acceptance, + not safety. + Feasibility: easy, verified-friendly — bounded slew/jerk limiters are + textbook Kani-boundable leaves; the knobs live in the v1.96 param store. + tags: [falcon, px4-parity, position-mode, ux, slew, feedforward, kani, v1.100, planned] + fields: + release-target: "bounded jerk/slew limiters + velocity-FF + stick-expo (Kani output-bounded); knobs in the param store" + px4-equivalent: "PX4 MPC_* trajectory shaping + stick curves — falcon adds Kani-bounded limiters" + links: + - type: implements + target: SYSREQ-FALCON-006 + + # ---- Tier 2: harder backlog (scope AFTER first flight; not yet committed) ---- + + - id: FEAT-FALCON-v1.101 + type: feature + title: "v1.101 — Multicopter wind estimation (airspeed-less, drag-fusion) [backlog: estimator extension]" + status: pending + description: > + BACKLOG (harder; scope after single-EKF flight is proven). PX4's EKF2 + estimates MC wind via drag-specific-force fusion (no airspeed sensor); + falcon's IEKF carries no wind states (gap-analysis #4). A natural estimator + extension — the IEKF can carry wind states + a clean drag model — that + improves position hold in wind and ENABLES the v1.99 high-wind failsafe. + Medium-hard with a Lyapunov/consistency proof; a credible 'verified wind + estimation' differentiator. + tags: [falcon, px4-parity, wind-estimation, iekf, drag, backlog, harder, v1.101, planned] + fields: + release-target: "IEKF wind-state extension + drag-specific-force fusion + consistency proof; feeds the high-wind failsafe" + px4-equivalent: "PX4 EKF2 MC wind estimation — falcon adds a verified wind estimator" + links: + - type: implements + target: SYSREQ-FALCON-017 + + - id: FEAT-FALCON-v1.102 + type: feature + title: "v1.102 — Multi-EKF instance selector / sensor voting [backlog: redundancy]" + status: pending + description: > + BACKLOG (medium-hard; scope after single-EKF flight). PX4 runs N EKF + instances on different IMU/mag combos and votes by consistency, catching + GRADUAL sensor degradation single-instance NIS gating sees late + (gap-analysis #3g/#8). falcon already has the per-instance consistency + metric (NIS); the missing piece is the multi-instance selector + redundant + sensor inputs. The selector logic is verifiable; the cost is N x estimator + compute + redundant hardware. + tags: [falcon, px4-parity, multi-ekf, voting, redundancy, fdi, backlog, harder, v1.102, planned] + fields: + release-target: "N-instance EKF selector keyed on NIS consistency (verified selector) + redundant sensor wiring" + px4-equivalent: "PX4 EKF2 multi-instance + sensor voting — falcon adds a verified selector over its NIS metric" + links: + - type: implements + target: SYSREQ-FALCON-018 + + - id: FEAT-FALCON-v1.103 + type: feature + title: "v1.103 — In-flight PID autotune (system identification) [backlog: param-dependent, safety-sensitive]" + status: pending + description: > + BACKLOG (hard; param-system-dependent + safety-sensitive; scope after + params + calibration). PX4 runs in-flight SISO sysid (2-pole/2-zero, + injecting roll/pitch/yaw disturbances over ~40s) then computes gains + (gap-analysis #1/3a) — the single biggest USABILITY gap (lets non-experts + get a flyable tune). The sysid/least-squares core is verifiable leaf work, + but the in-air excitation + apply-on-land loop is safety-sensitive and + needs the v1.96 param store. Worth scoping LAST of the thread. + tags: [falcon, px4-parity, autotune, sysid, tuning, backlog, harder, v1.103, planned] + fields: + release-target: "SISO sysid (verified least-squares) + bounded in-air excitation + apply-on-land; gains -> param store" + px4-equivalent: "PX4 multicopter autotune — falcon adds verified sysid + bounded excitation" + links: + - type: implements + target: SYSREQ-FALCON-006 diff --git a/artifacts/features/FEAT-FALCON-rc-dronecan.yaml b/artifacts/features/FEAT-FALCON-rc-dronecan.yaml index bebb239..95d494d 100644 --- a/artifacts/features/FEAT-FALCON-rc-dronecan.yaml +++ b/artifacts/features/FEAT-FALCON-rc-dronecan.yaml @@ -133,8 +133,8 @@ artifacts: - id: FEAT-FALCON-v1.94 type: feature - title: "v1.94 — DroneCAN sensor ingest (DC-P03): gnss.Fix2 + mag + baro + BatteryInfo decoders" - status: pending + title: "v1.94 — DroneCAN sensor ingest (DC-P03): float16 + esc.Status + mag + baro + BatteryInfo decoders" + status: implemented description: > PLANNED. The DroneCAN sensor-ingest decoders, completing the peripheral family over CAN (the SPI/I2C drivers' CAN-bus counterparts): @@ -166,8 +166,11 @@ artifacts: title: "v1.95 — DroneCAN node pump (DC-P04): DroneCanNode async loop + NodeStatus heartbeat" status: pending description: > - PLANNED. The runtime that ties the transfer layer + decoders to the CanBus - seam: DroneCanNode — an async pump (mirrors UbxReader) that + PLANNED. Also folds in gnss.Fix2 (DTID 1063, the variable covariance-tail + message moved from v1.94): decode the flight-relevant fixed fields + (lat/lon 1e-8 deg, height, sats, fix status). Then the runtime that ties + the transfer layer + decoders to the CanBus seam: + DroneCanNode — an async pump (mirrors UbxReader) that reads frames, drives the TransferReassembler, dispatches completed transfers to the per-DTID decoders, and broadcasts the node's own NodeStatus heartbeat at 1 Hz. Static node-ID in this slice; the dynamic diff --git a/artifacts/swreq/SWREQ-FALCON-DC-P03.yaml b/artifacts/swreq/SWREQ-FALCON-DC-P03.yaml new file mode 100644 index 0000000..27591de --- /dev/null +++ b/artifacts/swreq/SWREQ-FALCON-DC-P03.yaml @@ -0,0 +1,45 @@ +artifacts: + - id: SWREQ-FALCON-DC-P03 + type: sw-req + title: "DC-P03: DroneCAN sensor ingest — esc.Status/mag/baro/battery decoders + float16 primitive" + status: implemented + description: > + Falcon shall decode the DroneCAN v0 sensor-ingest messages (the SPI/I2C + drivers' CAN-bus counterparts) on the v1.92 transfer layer, plus the shared + IEEE-754 half-precision primitive the telemetry uses: + - float16 (binary16) -> f32 decode (f16_to_f32): handles normals, + subnormals, ±inf, NaN, ±0. Decode-only (falcon reads sensor float16; + it never emits it). Total (pure arithmetic, no panic surface). + - esc.Status (DTID 1034): error_count/voltage/current/temperature/rpm/ + power_rating/esc_index — the FDI source (multi-frame, 14-byte layout). + - MagneticFieldStrength (DTID 1002): ahrs_id + 3x float16 (external mag). + - StaticPressure (DTID 1028): float32 pressure + float16 variance (baro). + - BatteryInfo (DTID 1092) flight-relevant PREFIX: temperature/voltage/ + current (the BatteryMonitor inputs over CAN; the SoC/model tail is not + decoded). + + The architecture (spar/dronecan.aadl) gains the SensorSample data type + a + Decode thread; spar RTA confirms its 18us response meets the 2ms telemetry + deadline. The node WIT gains the sample-out port + decode export. + + Behaviour (crates/relay-dronecan): every decoder returns None on a short + payload and NEVER panics for any bytes; esc.Status bit-fields are masked to + width (power_rating <= 127, esc_index <= 31); rpm sign-extends from int18. + The read_bits helper (non-byte-aligned esc.Status fields) is total and + every result < 2^nbits. + tags: [falcon, dronecan, sensors, esc-status, mag, baro, battery, float16, spar, kani, v1.94] + fields: + req-type: safety + priority: must + verification-criteria: > + relay-dronecan: float16 known-vector decode (0x3C00->1.0, 0xC000->-2.0, + ±inf, NaN, subnormals); esc.Status field decode + rpm sign-extend + short + reject; mag 3-axis, baro float32, battery prefix decode; short-payload + rejects. Kani DC-K08 read_bits bounded (total, result < 2^nbits — the + integer floor under esc.Status). The float16 VALUES + full-decoder + totality are proptest-fuzzed (f32 Kani-intractable). spar/dronecan.aadl + analyzes 0 errors with the Decode thread RTA 18us <= 2ms; node WIT in sync. + SCOPE: gnss.Fix2 (variable covariance tail) is the v1.95 follow-on. + links: + - type: derives-from + target: SYSREQ-FALCON-008 diff --git a/artifacts/verification/FV-FALCON-DC-003.yaml b/artifacts/verification/FV-FALCON-DC-003.yaml new file mode 100644 index 0000000..8a234c0 --- /dev/null +++ b/artifacts/verification/FV-FALCON-DC-003.yaml @@ -0,0 +1,52 @@ +artifacts: + - id: FV-FALCON-DC-003 + type: sw-verification + title: "DroneCAN sensor ingest — float16 + esc.Status/mag/baro/battery decoders, Kani read_bits bound (DC-P03, v1.94)" + status: implemented + description: > + Verifies SWREQ-FALCON-DC-P03. relay-dronecan gains the float16 primitive + (float16.rs) and the sensor-ingest decoders (sensors.rs). Test-level + evidence (pulseengine.eu#89), each step a real named test/harness: + - float16::tests::known_half_vectors: canonical IEEE-half patterns decode + exactly (0x3C00->1.0, 0x4000->2.0, 0xC000->-2.0, 0x7C00->+inf, NaN). + - float16::tests::smallest_normal_and_subnormal: 2^-14 / 2^-24 edges. + - float16::proptests::decode_total_and_matches_reference: never panics + for ANY bit pattern; inf/NaN/finite classification is consistent. + - sensors::tests::esc_status_decodes_fields: error_count/voltage/current/ + temperature/rpm at the right offsets (float16 + bit-packed rpm). + - sensors::tests::esc_status_rpm_negative_sign_extends: int18 rpm sign. + - sensors::tests::esc_status_rejects_short / short_payloads_reject_not_panic: + short payloads -> None, no panic. + - sensors::tests::mag_decodes_three_axes: ahrs_id + 3x float16. + - sensors::tests::baro_decodes_pressure_f32: float32 pressure + float16. + - sensors::tests::battery_prefix_decodes_voltage_current: the + BatteryMonitor inputs (temp/voltage/current) over CAN. + - sensors::proptests::sensor_decoders_never_panic: ALL decoders over + arbitrary bytes never panic; esc.Status fields within width. + - Kani DC-K08 verify_read_bits_bounded: the bit-reader is total and every + result < 2^nbits (the integer floor under esc.Status). + Architecture: spar analyze --root Falcon_DroneCAN::DroneCanNode.Fmu reports + 0 errors with the Decode thread RTA 18us <= 2ms telemetry deadline; + wit/dronecan/node.wit (now with sample-out + decode export) in sync. + ORACLE: `cargo test -p relay-dronecan` green (36 tests); `cargo kani -p + relay-dronecan` VERIFICATION SUCCESSFUL for all eight harnesses. + tags: [verification, falcon, dronecan, sensors, float16, relay-dronecan, spar, kani, "v1.94"] + fields: + method: formal-verification + steps: + - run: "cargo test -p relay-dronecan float16::tests::known_half_vectors" + - run: "cargo test -p relay-dronecan float16::tests::smallest_normal_and_subnormal" + - run: "cargo test -p relay-dronecan float16::proptests::decode_total_and_matches_reference" + - run: "cargo test -p relay-dronecan sensors::tests::esc_status_decodes_fields" + - run: "cargo test -p relay-dronecan sensors::tests::esc_status_rpm_negative_sign_extends" + - run: "cargo test -p relay-dronecan sensors::tests::esc_status_rejects_short" + - 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 test -p relay-dronecan sensors::tests::short_payloads_reject_not_panic" + - run: "cargo test -p relay-dronecan sensors::proptests::sensor_decoders_never_panic" + # Kani DC-K08 — gated by the kani.yml matrix (relay-dronecan entry) + - run: "cargo kani -p relay-dronecan --harness verify_read_bits_bounded" + links: + - type: verifies + target: SWREQ-FALCON-DC-P03 diff --git a/crates/relay-dronecan/plain/src/float16.rs b/crates/relay-dronecan/plain/src/float16.rs new file mode 100644 index 0000000..7059a50 --- /dev/null +++ b/crates/relay-dronecan/plain/src/float16.rs @@ -0,0 +1,107 @@ +//! IEEE 754 half-precision (binary16) decode — the shared primitive for the +//! DroneCAN sensor telemetry (voltage/current/temperature) carried as float16, +//! LSB-first. Decode-only: falcon reads sensor float16 (in); it never emits it +//! (esc.RawCommand is int14). Pure arithmetic — no panic surface, so totality is +//! trivial; value correctness is pinned by known IEEE-half vectors + proptest. + +/// Decode an IEEE 754 binary16 bit pattern to f32. Total: never panics for any +/// `u16` (subnormals, ±inf, NaN, ±0 all handled). +pub fn f16_to_f32(bits: u16) -> f32 { + let sign = if bits & 0x8000 != 0 { -1.0f32 } else { 1.0f32 }; + let exp = (bits >> 10) & 0x1F; + let frac = bits & 0x03FF; + let mag = match exp { + 0 => { + // zero or subnormal: frac * 2^-24 + (frac as f32) * (1.0 / 16_777_216.0) + } + 0x1F => { + // inf (frac == 0) or NaN + if frac == 0 { + f32::INFINITY + } else { + f32::NAN + } + } + _ => { + // normal: (1 + frac/1024) * 2^(exp-15) + (1.0 + (frac as f32) / 1024.0) * exp2(exp as i32 - 15) + } + }; + sign * mag +} + +/// 2^n for a bounded exponent (normal float16 exponents are -14..=15). Table-free +/// repeated multiply keeps it no_std-clean (no libm dependency). +#[inline] +fn exp2(n: i32) -> f32 { + let mut v = 1.0f32; + if n >= 0 { + let mut i = 0; + while i < n { + v *= 2.0; + i += 1; + } + } else { + let mut i = 0; + while i < -n { + v *= 0.5; + i += 1; + } + } + v +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn known_half_vectors() { + // canonical IEEE-754 binary16 patterns + assert_eq!(f16_to_f32(0x0000), 0.0); + assert_eq!(f16_to_f32(0x3C00), 1.0); + assert_eq!(f16_to_f32(0x4000), 2.0); + assert_eq!(f16_to_f32(0xC000), -2.0); + assert_eq!(f16_to_f32(0x3800), 0.5); + assert_eq!(f16_to_f32(0x4900), 10.0); + assert!((f16_to_f32(0x3555) - 0.333_25).abs() < 1e-3); // ~1/3 + assert!(f16_to_f32(0x7C00).is_infinite() && f16_to_f32(0x7C00) > 0.0); + assert!(f16_to_f32(0xFC00).is_infinite() && f16_to_f32(0xFC00) < 0.0); + assert!(f16_to_f32(0x7E00).is_nan()); + } + + #[test] + fn smallest_normal_and_subnormal() { + // smallest positive normal 2^-14, smallest subnormal 2^-24 + assert!((f16_to_f32(0x0400) - 2.0f32.powi(-14)).abs() < 1e-12); + assert!((f16_to_f32(0x0001) - 2.0f32.powi(-24)).abs() < 1e-12); + } +} + +#[cfg(test)] +mod proptests { + use super::*; + use proptest::prelude::*; + + proptest! { + /// f16_to_f32 never panics for ANY bit pattern, and finite results match + /// the reference within half-precision tolerance for the normal range. + #[test] + fn decode_total_and_matches_reference(bits in 0u16..=0xFFFF) { + let v = f16_to_f32(bits); + // never panics (reaching here proves it); special values consistent + let exp = (bits >> 10) & 0x1F; + if exp == 0x1F { + let frac = bits & 0x3FF; + if frac == 0 { + prop_assert!(v.is_infinite()); + } else { + prop_assert!(v.is_nan()); + } + } else { + prop_assert!(v.is_finite()); + } + } + } +} diff --git a/crates/relay-dronecan/plain/src/kani_proofs.rs b/crates/relay-dronecan/plain/src/kani_proofs.rs index 714eb13..441cb54 100644 --- a/crates/relay-dronecan/plain/src/kani_proofs.rs +++ b/crates/relay-dronecan/plain/src/kani_proofs.rs @@ -8,6 +8,7 @@ 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}; @@ -137,3 +138,21 @@ fn verify_encode_single_frame_bounded() { None => assert!(n > 7), } } + +/// 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). +#[kani::proof] +fn verify_read_bits_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)); + } +} diff --git a/crates/relay-dronecan/plain/src/lib.rs b/crates/relay-dronecan/plain/src/lib.rs index 0f356d9..33b6cc3 100644 --- a/crates/relay-dronecan/plain/src/lib.rs +++ b/crates/relay-dronecan/plain/src/lib.rs @@ -24,8 +24,10 @@ #![forbid(unsafe_code)] pub mod crc; +pub mod float16; pub mod id; pub mod msg; +pub mod sensors; pub mod tail; pub mod transfer; diff --git a/crates/relay-dronecan/plain/src/sensors.rs b/crates/relay-dronecan/plain/src/sensors.rs new file mode 100644 index 0000000..b60ac13 --- /dev/null +++ b/crates/relay-dronecan/plain/src/sensors.rs @@ -0,0 +1,244 @@ +//! DroneCAN v0 sensor-ingest decoders (DC-P03) — the SPI/I2C drivers' CAN-bus +//! counterparts. Pure fixed-offset DSDL decoders over reassembled transfer +//! payloads (multi-frame via the v1.92 reassembler); the float16 telemetry uses +//! the shared [`crate::float16`] primitive. +//! +//! Decode-in only (falcon reads these): esc.Status (FDI: RPM/voltage/current/ +//! temp), MagneticFieldStrength (external mag over CAN), StaticPressure (external +//! baro), BatteryInfo prefix (power monitor over CAN). gnss.Fix2 (variable +//! covariance tail) is the v1.95 follow-on. + +use crate::float16::f16_to_f32; + +/// Data-type ids of the sensor messages this module decodes. +pub const DTID_ESC_STATUS: u16 = 1034; +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 { + pub error_count: u32, + pub voltage: f32, + pub current: f32, + pub temperature: f32, + pub rpm: i32, + pub power_rating_pct: u8, + pub esc_index: u8, +} + +/// Decode esc.Status (14-byte fixed layout). `None` if shorter. Total: never +/// panics; bit-fields masked to width (power_rating <= 127, esc_index <= 31). +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 { + 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, + }) +} + +/// 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. +pub fn decode_mag(p: &[u8]) -> Option { + if p.len() < 7 { + 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]])), + ], + }) +} + +/// uavcan.equipment.air_data.StaticPressure (DTID 1028) — external baro over CAN. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct StaticPressure { + pub pressure_pa: f32, + pub variance: f32, +} + +/// Decode StaticPressure (6-byte single-frame: float32 + float16). `None` if +/// shorter. Total. +pub fn decode_baro(p: &[u8]) -> Option { + if p.len() < 6 { + return None; + } + Some(StaticPressure { + pressure_pa: f32::from_le_bytes([p[0], p[1], p[2], p[3]]), + variance: f16_to_f32(u16::from_le_bytes([p[4], p[5]])), + }) +} + +/// uavcan.equipment.power.BatteryInfo (DTID 1092) flight-relevant PREFIX — +/// temperature/voltage/current (the BatteryMonitor inputs over CAN). The SoC / +/// capacity / model-name tail is not decoded. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct BatteryTelemetry { + pub temperature: f32, + pub voltage: f32, + pub current: f32, +} + +/// Decode the BatteryInfo prefix (3x float16). `None` if shorter than 6 bytes. +/// Total. +pub fn decode_battery_info(p: &[u8]) -> Option { + if p.len() < 6 { + return None; + } + Some(BatteryTelemetry { + temperature: f16_to_f32(u16::from_le_bytes([p[0], p[1]])), + voltage: f16_to_f32(u16::from_le_bytes([p[2], p[3]])), + current: f16_to_f32(u16::from_le_bytes([p[4], p[5]])), + }) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[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 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_eq!(s.rpm, 5000); + } + + #[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); + } + } + assert_eq!(decode_esc_status(&p).unwrap().rpm, -1); + } + + #[test] + fn esc_status_rejects_short() { + assert_eq!(decode_esc_status(&[0; 13]), None); + } + + #[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 m = decode_mag(&p).unwrap(); + assert_eq!(m.ahrs_id, 1); + assert_eq!(m.field_ga, [1.0, -2.0, 0.5]); + } + + #[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 b = decode_baro(&p).unwrap(); + assert_eq!(b.pressure_pa, 101325.0); + assert_eq!(b.variance, 1.0); + } + + #[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 bt = decode_battery_info(&p).unwrap(); + assert_eq!(bt.temperature, 10.0); + assert_eq!(bt.voltage, 20.0); + assert_eq!(bt.current, 2.0); + } + + #[test] + fn short_payloads_reject_not_panic() { + assert_eq!(decode_mag(&[0; 6]), None); + assert_eq!(decode_baro(&[0; 5]), None); + assert_eq!(decode_battery_info(&[0; 5]), None); + } +} + +#[cfg(test)] +mod proptests { + use super::*; + 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. + #[test] + fn sensor_decoders_never_panic(bytes in proptest::collection::vec(any::(), 0..40)) { + if let Some(s) = decode_esc_status(&bytes) { + prop_assert!(s.power_rating_pct <= 127); + prop_assert!(s.esc_index <= 31); + } + let _ = decode_mag(&bytes); + let _ = decode_baro(&bytes); + let _ = decode_battery_info(&bytes); + } + } +} diff --git a/spar/dronecan.aadl b/spar/dronecan.aadl index 0976c5b..8cfaed9 100644 --- a/spar/dronecan.aadl +++ b/spar/dronecan.aadl @@ -74,6 +74,16 @@ public Data_Model::Data_Representation => Struct; end EscRawCommand; + data SensorSample + -- A decoded DroneCAN sensor-ingest message (DC-P03): esc.Status (1034, + -- RPM/voltage/current/temp, the FDI source), MagneticFieldStrength (1002), + -- StaticPressure (1028), or BatteryInfo (1092, the power-monitor prefix). + -- The float16 telemetry uses the shared f16_to_f32 primitive; the decoders + -- are pure fixed-offset bodies over the reassembled transfer payload. + properties + Data_Model::Data_Representation => Struct; + end SensorSample; + -- ══════════════════════════════════════════════════════════════ -- The transfer-reassembly process — frames in, transfers out. -- ══════════════════════════════════════════════════════════════ @@ -84,6 +94,7 @@ public transfer_out: out data port Transfer; -- a completed, CRC-checked transfer status_out: out data port NodeStatus; -- this node's own 1 Hz heartbeat command_in: in data port EscRawCommand; -- the mixer's per-motor throttles (DC-P02) + sample_out: out data port SensorSample; -- a decoded sensor-ingest message (DC-P03) -- The mitigating sinks: the transfer-CRC kills a corrupted multi-frame -- (ValueError), the fixed-buffer bound kills an oversized transfer -- (SequenceError), and the toggle + transfer-id state kills a re-ordered or @@ -109,6 +120,7 @@ public reassemble: thread Reassemble; -- per-frame: tail decode, accumulate, CRC-check, emit heartbeat: thread Heartbeat; -- broadcast this node's NodeStatus at 1 Hz command: thread Command; -- encode + emit esc.RawCommand at the control rate + decode: thread Decode; -- decode a sensor-ingest transfer (DC-P03) end TransferReassembler.Fixed; thread Reassemble @@ -146,6 +158,18 @@ public Compute_Execution_Time => 2 us .. 8 us; end Command; + thread Decode + -- Decode a completed sensor-ingest transfer (esc.Status / mag / baro / + -- battery) into a SensorSample for the estimator + FDI. Dispatched per + -- completed transfer; bounded by the telemetry rate (the fastest, esc.Status, + -- is ~50 Hz per ESC). The fixed-offset decoders + f16_to_f32 are the bodies; + -- read_bits is Kani-bounded (DC-K08), the float16 values proptest-gated. + properties + Dispatch_Protocol => Sporadic; + Period => 2000 us; -- min interarrival floor (~500 Hz aggregate telemetry) + Compute_Execution_Time => 2 us .. 6 us; + end Decode; + -- ══════════════════════════════════════════════════════════════ -- The CAN bus — the fault surface (bus-off / noise / overrun). -- ══════════════════════════════════════════════════════════════ @@ -209,6 +233,7 @@ public Actual_Processor_Binding => (reference (cpu)) applies to node.reassemble; Actual_Processor_Binding => (reference (cpu)) applies to node.heartbeat; Actual_Processor_Binding => (reference (cpu)) applies to node.command; + Actual_Processor_Binding => (reference (cpu)) applies to node.decode; end DroneCanNode.Fmu; end Falcon_DroneCAN; diff --git a/wit/dronecan/node.wit b/wit/dronecan/node.wit index 080e848..d8a1c75 100644 --- a/wit/dronecan/node.wit +++ b/wit/dronecan/node.wit @@ -5,12 +5,14 @@ interface node-ports { type canframe = list; type escrawcommand = list; type nodestatus = list; + type sensorsample = list; type transfer = list; frames-in: func() -> canframe; set-transfer-out: func(val: transfer); set-status-out: func(val: nodestatus); command-in: func() -> escrawcommand; + set-sample-out: func(val: sensorsample); } world node-world { @@ -18,4 +20,5 @@ world node-world { export reassemble: func(); export heartbeat: func(); export command: func(); + export decode: func(); }