From da8d5d537e9db3ce540207fd3e489b5cc9acbc87 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Thu, 16 Jul 2026 04:28:09 +0200 Subject: [PATCH] =?UTF-8?q?feat(falcon):=20v1.121=20=E2=80=94=20dual-M9N?= =?UTF-8?q?=20GNSS=20blend/failover=20+=20TF02=20terrain-relative=20landin?= =?UTF-8?q?g=20(GNSS-P02=20+=20RANGEDRV-P01)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two hardware-register slices for the first vehicle, both built by first UN-ORPHANING verified leaves (the recurring pattern): GNSS-P02 — falcon-gnss-ubx dual.rs: per-receiver health gating (fix, sats, accuracy ceiling+floor, innovation gate vs the estimator), inverse-variance blending, failover within one fix interval, and a debounced LATCHED divergence flag. Sqrt-free decision path (squared gates). Kani GNSS-K05 proved selector totality over ALL inputs — and caught a real NaN: a denormal reported accuracy overflowed the 1/acc² blend weight into ∞−∞ (fixed with accuracy floor + position sanity bound). FlightCore runs the selector when the backend has two lanes (read_gnss_dual seam, default None); the previously-ORPHANED relay-iekf SpoofMonitor is now fed the position innovation, and nav_compromised() (spoof ∨ divergence) blocks pre-arm. RANGEDRV-P01 — relay-flowrange tf02.rs: Benewake TF02-Pro 9-byte frame decode cross-validated against TWO independent parsers (ArduPilot AP_RangeFinder_Benewake + PX4 tfmini_parser semantics quoted in the module doc), strength/envelope quality gate IN the decoder, streaming resync scanner. Kani FLOWRANGE-K03/K04 (decode totality + gate soundness over all 9-byte patterns; scanner totality) — K03 caught 10 × 0.01f32 = 0.099999994 escaping the envelope (fixed: divide, the correctly-rounded quotient makes the bounds exact). The previously- ORPHANED range_to_altitude is now the LANDING-PHASE altitude reference: terrain-relative AGL (the PX4 terrain-estimator role, NOT z-state fusion — three absolute references would fight), innovation- gated so an obstacle overflight can never fold the touchdown logic. Oracles (falcon-core, +5 tests): failover holds the estimate (<1 m step, no reset); divergence latches + blocks arming + estimate stays with the consistent receiver; 40-trial LCG dispersion campaign (drop / accuracy-collapse / divergence-walk / nominal — flag in 100% of walk trials, 0 false flags, no estimate steps); biased-vertical landing (baro AND GNSS-z lying 2 m high — the sim now models receiver BIAS, which zero-mean noise cannot) touches down within 15 cm of TRUE ground with range assist and provably NOT without; obstacle short-return rejected by the gate, altitude estimate unfolded. Also: CI clippy gate extended with relay-batt, relay-flowrange, falcon-gnss-ubx (they were outside the linted crate list); a pre-existing match-destructure lint in relay-flowrange fixed; LoggingBackend forwards the two new backend reads (the v1.120 wrapper-default lesson; raw-lane logging = schema-v2, #277). SWREQ-FALCON-GNSS-P02 + SWREQ-FALCON-RANGEDRV-P01 → implemented (the verify PR follows per the two-commit rule). Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01HvusAXYbHLyv3uTzfBcMbG --- .github/workflows/ci.yml | 1 + Cargo.lock | 2 + artifacts/swreq/SWREQ-FALCON-GNSS-P02.yaml | 2 +- .../swreq/SWREQ-FALCON-RANGEDRV-P01.yaml | 2 +- crates/falcon-core/Cargo.toml | 2 + .../falcon-core/plain/src/blackbox_backend.rs | 13 + crates/falcon-core/plain/src/lib.rs | 442 +++++++++++++++++- crates/falcon-gnss-ubx/src/dual.rs | 312 +++++++++++++ crates/falcon-gnss-ubx/src/kani_proofs.rs | 43 ++ crates/falcon-gnss-ubx/src/lib.rs | 2 + .../relay-flowrange/plain/src/kani_proofs.rs | 28 ++ crates/relay-flowrange/plain/src/lib.rs | 11 +- crates/relay-flowrange/plain/src/tf02.rs | 218 +++++++++ 13 files changed, 1058 insertions(+), 20 deletions(-) create mode 100644 crates/falcon-gnss-ubx/src/dual.rs create mode 100644 crates/relay-flowrange/plain/src/tf02.rs diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0a0b9e4..c38ee14 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -65,6 +65,7 @@ jobs: -p falcon-param -p relay-fsm -p relay-preflight -p relay-calib \ -p falcon-core -p relay-mix-quad \ -p relay-arm -p relay-adrc -p relay-geo -p relay-iekf \ + -p relay-batt -p relay-flowrange -p falcon-gnss-ubx \ --all-targets -- -D warnings test: diff --git a/Cargo.lock b/Cargo.lock index 87bfd3b..8a9f5cf 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -271,9 +271,11 @@ dependencies = [ name = "falcon-core" version = "0.1.0" dependencies = [ + "falcon-gnss-ubx", "relay-adrc", "relay-batt", "relay-calib", + "relay-flowrange", "relay-fsm", "relay-geo", "relay-iekf", diff --git a/artifacts/swreq/SWREQ-FALCON-GNSS-P02.yaml b/artifacts/swreq/SWREQ-FALCON-GNSS-P02.yaml index 5177ba6..faba59f 100644 --- a/artifacts/swreq/SWREQ-FALCON-GNSS-P02.yaml +++ b/artifacts/swreq/SWREQ-FALCON-GNSS-P02.yaml @@ -2,7 +2,7 @@ artifacts: - id: SWREQ-FALCON-GNSS-P02 type: sw-req title: "GNSS-P02 — dual-receiver blending and failover (2× u-blox M9N)" - status: proposed + status: implemented release: falcon-v1.121.0 description: > The estimator input shall support TWO GNSS receivers (the first diff --git a/artifacts/swreq/SWREQ-FALCON-RANGEDRV-P01.yaml b/artifacts/swreq/SWREQ-FALCON-RANGEDRV-P01.yaml index fb8fd85..78c4cbc 100644 --- a/artifacts/swreq/SWREQ-FALCON-RANGEDRV-P01.yaml +++ b/artifacts/swreq/SWREQ-FALCON-RANGEDRV-P01.yaml @@ -2,7 +2,7 @@ artifacts: - id: SWREQ-FALCON-RANGEDRV-P01 type: sw-req title: "RANGEDRV-P01 — Benewake TF02 Pro rangefinder driver + landing/terrain assist" - status: proposed + status: implemented release: falcon-v1.121.0 description: > The stack shall decode the Benewake TF02 Pro LiDAR rangefinder diff --git a/crates/falcon-core/Cargo.toml b/crates/falcon-core/Cargo.toml index 32cc088..04fc774 100644 --- a/crates/falcon-core/Cargo.toml +++ b/crates/falcon-core/Cargo.toml @@ -21,6 +21,8 @@ relay-fsm = { path = "../relay-fsm" } relay-preflight = { path = "../relay-preflight" } relay-calib = { path = "../relay-calib" } relay-batt = { path = "../relay-batt" } +relay-flowrange = { path = "../relay-flowrange" } +falcon-gnss-ubx = { path = "../falcon-gnss-ubx" } relay-log = { path = "../relay-log" } relay-param = { path = "../relay-param" } diff --git a/crates/falcon-core/plain/src/blackbox_backend.rs b/crates/falcon-core/plain/src/blackbox_backend.rs index 2a77b7e..7a49d96 100644 --- a/crates/falcon-core/plain/src/blackbox_backend.rs +++ b/crates/falcon-core/plain/src/blackbox_backend.rs @@ -100,6 +100,19 @@ impl FlightBackend for LoggingBackend<'_, B, L> { fn read_battery_v(&mut self) -> f32 { self.inner.read_battery_v() } + fn read_gnss_dual( + &mut self, + ) -> Option<(Option, Option)> + { + // Forwarded, NOT logged: the TickRecord carries the SELECTED position + // (what the estimator consumed); per-receiver raw lanes are part of + // the schema-v2 slice (#277). + self.inner.read_gnss_dual() + } + fn read_range(&mut self) -> Option { + // Forwarded, NOT logged yet (schema-v2, #277). + self.inner.read_range() + } fn read_battery_i(&mut self) -> Option { // Forwarded, NOT logged yet: battery samples are not in the v1.118 // TickRecord schema (extending it regenerates the shipped golden — diff --git a/crates/falcon-core/plain/src/lib.rs b/crates/falcon-core/plain/src/lib.rs index 5f254ad..9b2db08 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -73,6 +73,26 @@ pub trait FlightBackend { fn read_battery_i(&mut self) -> Option { None } + /// BOTH GNSS receivers' fixes for this cycle (GNSS-P02, v1.121: the + /// first vehicle carries 2× M9N). `None` = the backend has no dual-GNSS + /// concept — the core falls back to the single [`read_position`] lane. + /// With `Some`, the core's dual-receiver selector (health gating, + /// inverse-variance blending, one-interval failover, latched divergence + /// flag) feeds the estimator instead. + fn read_gnss_dual( + &mut self, + ) -> Option<(Option, Option)> + { + None + } + /// Downward rangefinder distance (m), already wire-decoded and + /// quality-gated at the driver layer (TF02-Pro: relay-flowrange tf02, + /// RANGEDRV-P01). The core tilt-compensates it, innovation-gates it + /// against the estimated altitude, and fuses it as a tight vertical + /// anchor for the landing phase. Default `None`. + fn read_range(&mut self) -> Option { + None + } /// Barometric altitude (NED z, metres; negative = up), or `None` if no /// barometer. An INDEPENDENT vertical source the core fuses so altitude /// survives GPS-vertical loss (v1.20). Default `None`. @@ -134,6 +154,19 @@ pub struct FlightCore { /// into the verified IEKF as a vertical anchor (a position update whose z is /// the baro), so altitude AND its rate survive GPS-vertical loss. baro_var: f32, + /// Latest gated terrain-relative AGL from the rangefinder (m), or `None` + /// (v1.121, RANGEDRV-P01): tilt-compensated, innovation-gated. The + /// landing law's altitude reference when present — centimetre truth + /// exactly where the baro/GNSS metres of error matter most. + range_agl: Option, + /// Dual-receiver GNSS selector (GNSS-P02, v1.121): health gating, + /// inverse-variance blending, one-interval failover, latched divergence. + dual_gnss: falcon_gnss_ubx::dual::DualGnss, + /// GNSS anti-spoof CUSUM (relay-iekf, previously an ORPHANED verified + /// leaf): fed the position innovation each GNSS update; a sustained + /// directional walk-off latches. OR-ed with the selector divergence into + /// [`nav_compromised`](Self::nav_compromised). + spoof: relay_iekf::SpoofMonitor, /// Single-rotor-out fault detection & isolation (relay-iekf CUSUM on the /// per-rotor commanded-vs-achieved effectiveness residual). On isolation, /// `failed_motor` latches and the control law switches to reduced-attitude + @@ -215,6 +248,11 @@ impl FlightCore { pos_int: [0.0; 2], pos_int_max: 1.5, baro_var: 0.05, // ≈ (0.2 m)² baro noise; trusted less than a clean GPS-z + range_agl: None, + dual_gnss: falcon_gnss_ubx::dual::DualGnss::new(), + // threshold 60 m·steps, drift slack 0.15 m/step: rejects zero-mean + // GNSS noise, latches on a sustained directional walk-off. + spoof: relay_iekf::SpoofMonitor::new(60.0, 0.15), // CUSUM thresholds matching the SITL-verified fault-tolerance chain. fdi: RotorFaultDetector::new(0.5, 0.1), failed_motor: None, @@ -280,6 +318,24 @@ impl FlightCore { /// The isolated failed rotor (latched), or `None` — the supervisor lands the /// vehicle when this is set (a 3-rotor quad cannot navigate; v1.103). + /// The latest gated terrain-relative AGL (m) from the rangefinder + /// (RANGEDRV-P01) — the land detector's ground-proximity input. + pub fn range_agl(&self) -> Option { + self.range_agl + } + + /// Navigation-integrity flag (GNSS-P02): the anti-spoof CUSUM latched a + /// sustained innovation walk-off, OR the dual-receiver selector latched + /// receiver divergence. Consumed by pre-arm (and any failsafe policy). + pub fn nav_compromised(&self) -> bool { + self.spoof.spoofed() || self.dual_gnss.diverged() + } + + /// The dual-GNSS selector's latched divergence flag alone (GNSS-P02). + pub fn gnss_diverged(&self) -> bool { + self.dual_gnss.diverged() + } + pub fn failed_motor(&self) -> Option { self.failed_motor } @@ -409,6 +465,31 @@ impl FlightCore { /// "Disarmed" vehicle away (the analytic plant's ground had hidden it). /// Mirrors `step`'s estimator block exactly (kept adjacent; drift between /// them is a bug). + /// GNSS estimator input, shared by [`step`](Self::step) and + /// [`step_estimate_only`](Self::step_estimate_only): the dual-receiver + /// selector (GNSS-P02) when the backend has one, else the legacy single + /// lane. Feeds the anti-spoof CUSUM with the position innovation. + fn fuse_gnss(&mut self, b: &mut B) { + if let Some((ga, gb)) = b.read_gnss_dual() { + // The innovation-gate reference engages after warm-up (a cold + // estimate would disqualify honest receivers). + let est_ref = if self.step_count >= self.fdi_warmup_steps * 5 { + Some(self.iekf.state().p) + } else { + None + }; + let dec = self.dual_gnss.update(ga, gb, est_ref); + if let Some(p) = dec.pos { + if let Some(e) = est_ref { + self.spoof.update([p[0] - e[0], p[1] - e[1], p[2] - e[2]]); + } + self.iekf.update_position(p, self.pos_var); + } + } else if let Some(p) = b.read_position() { + self.iekf.update_position(p, self.pos_var); + } + } + pub fn step_estimate_only(&mut self, b: &mut B) { let dt = b.dt(); let raw = b.read_imu(); @@ -416,9 +497,7 @@ impl FlightCore { let accel = self.calib.apply_accel(raw.accel); self.iekf.propagate(IekfImu { gyro, accel }, dt); self.iekf.update_gravity(accel, self.grav_var); - if let Some(p) = b.read_position() { - self.iekf.update_position(p, self.pos_var); - } + self.fuse_gnss(b); if let Some(m) = b.read_mag() { self.iekf.update_magnetometer(self.calib.apply_mag(m), 0.0, self.mag_var); } @@ -446,9 +525,7 @@ impl FlightCore { // ── Estimate ── self.iekf.propagate(IekfImu { gyro, accel }, dt); self.iekf.update_gravity(accel, self.grav_var); - if let Some(p) = b.read_position() { - self.iekf.update_position(p, self.pos_var); - } + self.fuse_gnss(b); if let Some(m) = b.read_mag() { self.iekf.update_magnetometer(self.calib.apply_mag(m), 0.0, self.mag_var); } @@ -490,6 +567,29 @@ impl FlightCore { let e = self.iekf.state(); self.iekf.update_position([e.p[0], e.p[1], bz], self.baro_var); } + // v1.121 — terrain-relative AGL (RANGEDRV-P01): the decoded, + // strength-gated TF02 return, tilt-compensated (relay-flowrange, the + // previously-orphaned verified leaf). NOT fused into the z state — + // three absolute references (biased GNSS-z, biased baro, true range) + // would fight and the estimate would settle metres off. Instead the + // range is the LANDING-PHASE altitude reference directly (the PX4 + // terrain-estimator role): innovation-gated against the estimator's + // AGL so an obstacle overflight (sudden short return) is REJECTED — + // it can never fold the touchdown logic — while a biased-baro/GNSS + // descent still flares off the true ground. + self.range_agl = b.read_range().and_then(|r| { + let e = self.iekf.state(); + // min 0.10 = the TF02 device floor: the touchdown threshold + // (0.12 m) must sit INSIDE the valid band, not in the blind + // zone below the gate (0.15 would go dark 3 cm too early). + relay_flowrange::range_to_altitude(r, e.tilt_rad(), 0.10, 35.0).filter(|agl| { + let est_agl = -e.p[2]; + // gate 3 m: wider than credible baro/GNSS vertical bias (the + // assist must ENGAGE against lying references), narrower + // than an obstacle step worth rejecting. + (agl - est_agl) > -3.0 && (agl - est_agl) < 3.0 + }) + }); let est = self.iekf.state(); // ── Altitude loop ── velocity-based touchdown when landing, else the @@ -499,7 +599,11 @@ impl FlightCore { // RATE (NED z positive = down), independent of the ground-effect // cushion the position loop floats on; cut to idle on touchdown so // the vehicle settles on the surface. - let alt_agl = -est.p[2]; + // v1.121: terrain-relative AGL from the rangefinder when present + // (centimetre truth exactly where the flare needs it); estimator + // AGL otherwise. The gate above guarantees the two never differ + // by an obstacle-sized step. + let alt_agl = self.range_agl.unwrap_or(-est.p[2]); if alt_agl < 0.12 { 0.1 // touched down — idle (the ground holds the vehicle) } else { @@ -878,11 +982,22 @@ impl FlightSupervisor { self.preflight.calibration_present = self.core.calibration() != relay_calib::CalParams::identity(); self.preflight.battery_ok = !(self.batt_state.low || self.batt_state.critical); + // GNSS-P02: a latched receiver divergence or spoof walk-off blocks + // arming (the flag only clears via a ground reset). + if self.core.nav_compromised() { + self.preflight.sensors_healthy = false; + } self.preflight.geofence_loaded = self.fence_radius > 0.0; } /// Why arming would be refused right now (the first failing pre-arm check), /// or `None` if all checks pass — the reason a GCS surfaces to the operator. + /// Shared view of the wrapped [`FlightCore`] (estimator state, FDI, + /// GNSS-integrity flags) — the telemetry/pre-arm read seam. + pub fn core(&self) -> &FlightCore { + &self.core + } + /// The latest estimated battery state (BATTERY-P02): sag-compensated SoC, /// coulomb count, failsafe flags, and the degraded (voltage-only) flag — /// the SYS_STATUS battery seam (map via `relay_batt::sys_status_fields`). @@ -1367,6 +1482,33 @@ pub struct SimBackend { /// The collective thrust command from the last write_motors — the sim's /// battery LOAD (v1.120): drives the current sense read_battery_i exposes. last_collective: f32, + /// GNSS position bias (m, NED) — real receivers carry a slowly-varying + /// meters-class VERTICAL offset (multipath/atmosphere) that zero-mean + /// noise does not model: it does not average out. The biased-vertical + /// landing scenario (RANGEDRV-P01) needs it. + pub gps_pos_bias: [f32; 3], + /// Baro static bias (m, NED z) — the lying-baro landing scenario + /// (RANGEDRV-P01): baro altitude is off by metres exactly at the flare. + pub baro_bias: f32, + /// Rangefinder model (v1.121): when true, read_range returns the TRUE + /// AGL + noise (a decoded, gated TF02 return), overridden by + /// `range_fault` when set (obstacle overflight / false short return). + pub range_enabled: bool, + pub range_noise: f32, + /// Scripted rangefinder override (m) — obstacle injection. + pub range_fault: Option, + /// Dual-GNSS model (v1.121): when true, read_gnss_dual serves two + /// receivers derived from truth + per-receiver noise/offset/health + /// scripts; read_position is then unused by the core. + pub gnss_dual_enabled: bool, + /// Per-receiver scripted fault: None = healthy; Some(fix) overrides. + pub gnss_a_down: bool, + pub gnss_b_down: bool, + /// Additive NED offset on receiver B (divergence/spoof injection). + pub gnss_b_offset: [f32; 2], + /// Reported accuracy per receiver (m). + pub gnss_a_acc: f32, + pub gnss_b_acc: f32, /// Scripted current sense for tests (v1.120): returned by read_battery_i /// when the drain model is off. `None` = no current sense (fallback mode). pub battery_i: Option, @@ -1434,6 +1576,17 @@ impl SimBackend { baro_noise: 0.0, battery_v: 16.0, battery_i: None, + gps_pos_bias: [0.0; 3], + baro_bias: 0.0, + range_enabled: false, + range_noise: 0.0, + range_fault: None, + gnss_dual_enabled: false, + gnss_a_down: false, + gnss_b_down: false, + gnss_b_offset: [0.0, 0.0], + gnss_a_acc: 1.0, + gnss_b_acc: 1.0, battery_drain: false, battery_charge: 1.0, thrust_lapse: 0.0, @@ -1557,14 +1710,21 @@ impl FlightBackend for SimBackend { return None; } // v1.19 — continuous GNSS position noise (Gaussian per axis). + // v1.121 — plus the receiver's slowly-varying bias (multipath / + // atmosphere): a metres-class offset zero-mean noise cannot model. + let bias = self.gps_pos_bias; if p.gps_noise > 0.0 { return Some([ - self.pos[0] + p.gps_noise * self.noise_unit(), - self.pos[1] + p.gps_noise * self.noise_unit(), - self.pos[2] + p.gps_noise * self.noise_unit(), + self.pos[0] + bias[0] + p.gps_noise * self.noise_unit(), + self.pos[1] + bias[1] + p.gps_noise * self.noise_unit(), + self.pos[2] + bias[2] + p.gps_noise * self.noise_unit(), ]); } - Some(self.pos) // full 6-DoF NED position (v1.3) + Some([ + self.pos[0] + bias[0], + self.pos[1] + bias[1], + self.pos[2] + bias[2], + ]) // full 6-DoF NED position (v1.3) + receiver bias (v1.121) } fn read_mag(&mut self) -> Option { let mut m = self.to_body([1.0, 0.0, 0.0]); // NED north → yaw observable @@ -1710,6 +1870,46 @@ impl FlightBackend for SimBackend { fn read_battery_v(&mut self) -> f32 { self.battery_v } + fn read_range(&mut self) -> Option { + if !self.range_enabled { + return None; + } + if let Some(f) = self.range_fault { + return Some(f); + } + let agl = -self.pos[2]; // flat terrain at z = 0 + if !(0.1..=40.0).contains(&agl) { + return None; // outside the TF02 envelope (driver-gated) + } + Some(agl + self.range_noise * self.noise_unit()) + } + fn read_gnss_dual( + &mut self, + ) -> Option<(Option, Option)> + { + use falcon_gnss_ubx::dual::NedFix; + if !self.gnss_dual_enabled { + return None; + } + let mut base = self.pos; + let n = self.path.gps_noise; + base[0] += n * self.noise_unit(); + base[1] += n * self.noise_unit(); + let a = if self.gnss_a_down { + None + } else { + Some(NedFix { pos: base, acc_m: self.gnss_a_acc, sats: 14, fix_ok: true }) + }; + let b = if self.gnss_b_down { + None + } else { + let mut pb = base; + pb[0] += self.gnss_b_offset[0]; + pb[1] += self.gnss_b_offset[1]; + Some(NedFix { pos: pb, acc_m: self.gnss_b_acc, sats: 14, fix_ok: true }) + }; + Some((a, b)) + } fn read_battery_i(&mut self) -> Option { // Physically consistent with the drain model's sag (0.3·collective V // at R = 0.024 Ω): I = 12.5·collective A (25 A at hover). No current @@ -1723,7 +1923,7 @@ impl FlightBackend for SimBackend { } fn read_baro(&mut self) -> Option { if self.baro_enabled { - Some(self.pos[2] + self.baro_noise * self.noise_unit()) // NED z, noisy + Some(self.pos[2] + self.baro_bias + self.baro_noise * self.noise_unit()) // NED z, noisy + bias } else { None } @@ -2538,6 +2738,224 @@ mod tests { ); } + /// GNSS-P02 failover: receiver A dies mid-hover — the estimator input + /// fails over within one fix interval with NO position-estimate step + /// beyond the healthy receiver's accuracy, and no estimator reset. + #[test] + fn dual_gnss_failover_holds_estimate() { + let dt = 0.002f32; + let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + let mut b = SimBackend::new(level, dt); + b.gnss_dual_enabled = true; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-3.0); + for _ in 0..4000 { + core.step(&mut b); + } + let before = core.state().p; + b.gnss_a_down = true; // receiver A dies + let mut max_step = 0.0f32; + let mut prev = before; + for _ in 0..1000 { + core.step(&mut b); + let p = core.state().p; + let d = relay_math::sqrtf((p[0] - prev[0]).powi(2) + (p[1] - prev[1]).powi(2)); + max_step = max_step.max(d); + prev = p; + } + assert!( + max_step < 1.0, + "failover must not step the estimate beyond B's accuracy: {max_step} m" + ); + assert!(!core.nav_compromised(), "a clean failover is not a spoof"); + } + + /// GNSS-P02 divergence: receiver B walks 12 m off. The selector latches + /// divergence, the estimate stays with the truth-consistent receiver, + /// and PRE-ARM blocks (nav_compromised via sensors_healthy). + #[test] + fn gnss_divergence_latches_and_blocks_arming() { + use relay_calib::CalParams; + let dt = 0.004f32; + let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + let mut b = SimBackend::new(level, dt); + b.gnss_dual_enabled = true; + b.ground_contact = true; + let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 50.0, 2.0, 14.0); + sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + for _ in 0..2000 { + sup.step(&mut b); + } + assert_eq!(sup.arm_blocked_reason(), None, "healthy dual-GNSS arms"); + b.gnss_b_offset = [12.0, 0.0]; // B starts lying + for _ in 0..2000 { + sup.step(&mut b); + } + assert!(sup.core().gnss_diverged(), "sustained 12 m split must latch"); + assert!(sup.arm_blocked_reason().is_some(), "divergence blocks arming"); + let e = sup.core().state(); + assert!( + relay_math::fabsf(e.p[0]) < 2.0, + "estimate must stay with the consistent receiver: {} m", + e.p[0] + ); + } + + /// GNSS-P02 dispersion mini-campaign (CI-sized): 40 LCG-dispersed trials + /// over fault type (A dropout / A accuracy collapse / B divergence walk / + /// nominal) and onset. The divergence flag fires in 100% of walk trials + /// and 0% of the others; no trial steps the estimate discontinuously. + #[test] + fn dual_gnss_dispersion_campaign() { + let mut lcg: u32 = 0xC0FFEE; + let mut rnd = move || { + lcg = lcg.wrapping_mul(1664525).wrapping_add(1013904223); + (lcg >> 8) as f32 / 16777216.0 + }; + let dt = 0.004f32; + let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + for trial in 0..40 { + let fault = trial % 4; // 0 nominal, 1 A-drop, 2 A-acc-collapse, 3 B-walk + let onset = 2000 + (rnd() * 1000.0) as usize; + let mut b = SimBackend::new(level, dt); + b.gnss_dual_enabled = true; + b.path.gps_noise = 0.05 + rnd() * 0.1; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-3.0); + let mut prev: Option = None; + let mut max_step = 0.0f32; + for k in 0..5000 { + if k == onset { + match fault { + 1 => b.gnss_a_down = true, + 2 => b.gnss_a_acc = 50.0, // over the health ceiling + 3 => b.gnss_b_offset = [15.0, 0.0], + _ => {} + } + } + core.step(&mut b); + let p = core.state().p; + if let Some(q) = prev { + let d = relay_math::sqrtf((p[0] - q[0]).powi(2) + (p[1] - q[1]).powi(2)); + max_step = max_step.max(d); + } + prev = Some(p); + } + assert!( + max_step < 1.5, + "trial {trial} fault {fault}: estimate stepped {max_step} m" + ); + if fault == 3 { + assert!(core.gnss_diverged(), "trial {trial}: walk must raise the flag"); + } else { + assert!(!core.gnss_diverged(), "trial {trial} fault {fault}: false flag"); + } + } + } + + /// RANGEDRV-P01 landing assist: the baro lies by 2 m (reads HIGH — the + /// dangerous direction: the vehicle believes it is 2 m above its true + /// altitude and would fly into the ground). With the TF02 range anchor + /// the touchdown fires within 15 cm of TRUE ground. + #[test] + fn biased_baro_range_assisted_touchdown() { + let dt = 0.002f32; + let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + // WITH the range anchor: + let mut b = SimBackend::new(level, dt); + b.ground_contact = true; + b.baro_enabled = true; + b.baro_bias = -2.0; // baro reads 2 m HIGHER than truth (NED z minus) + b.gps_pos_bias = [0.0, 0.0, -2.0]; // GNSS vertical carries the same lie + b.range_enabled = true; + b.range_noise = 0.02; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-3.0); + for _ in 0..4000 { + core.step(&mut b); + } + core.set_landing(true); + // Touchdown recognition = the landing law cutting to idle (the + // terrain-relative AGL crossing 0.12), visible as the collective + // dropping to 4×0.1. Record the TRUE altitude at that moment. + let mut true_alt_at_touchdown = None; + for _ in 0..20000 { + core.step(&mut b); + if b.last_collective < 0.45 { + true_alt_at_touchdown = Some(-b.pos[2]); + break; + } + } + let ta = true_alt_at_touchdown.expect("must recognize touchdown"); + assert!( + ta.abs() <= 0.15, + "range-assisted touchdown must fire within 15 cm of true ground: {ta} m" + ); + + // WITHOUT range (the failure this slice fixes): the same biased baro + // leaves the estimate ~2 m high — touchdown never triggers before + // the vehicle is ground-clamped, i.e. it flies INTO the ground. + let mut b2 = SimBackend::new(level, dt); + b2.ground_contact = true; + b2.baro_enabled = true; + b2.baro_bias = -2.0; + b2.gps_pos_bias = [0.0, 0.0, -2.0]; + let mut core2 = FlightCore::new(0.5, 1.0 / dt); + core2.set_altitude(-3.0); + for _ in 0..4000 { + core2.step(&mut b2); + } + core2.set_landing(true); + let mut touch_true_alt = None; + for _ in 0..20000 { + core2.step(&mut b2); + if b2.last_collective < 0.45 { + touch_true_alt = Some(-b2.pos[2]); + break; + } + } + match touch_true_alt { + None => {} // never recognized touchdown — flew into the clamp: the failure + Some(t) => assert!( + t.abs() > 0.15, + "without range the biased-vertical landing should NOT be clean ({t} m) — \ + if it is, the scenario no longer discriminates" + ), + } + } + + /// RANGEDRV-P01 innovation gate: an obstacle overflight (sudden 4 m + /// short return) mid-descent must NOT fold the altitude estimate. + #[test] + fn obstacle_overflight_does_not_fold_altitude() { + let dt = 0.002f32; + let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + let mut b = SimBackend::new(level, dt); + b.ground_contact = true; + b.baro_enabled = true; + b.range_enabled = true; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-8.0); + for _ in 0..6000 { + core.step(&mut b); + } + let est_before = -core.state().p[2]; + // fly over a 5 m obstacle for 0.5 s: the range suddenly reads ~3 m. + b.range_fault = Some(3.0); + for _ in 0..250 { + core.step(&mut b); + } + assert!( + core.range_agl().is_none(), + "the innovation gate must REJECT the obstacle's short return" + ); + let est_after = -core.state().p[2]; + assert!( + (est_after - est_before).abs() < 0.5, + "altitude estimate folded over the obstacle: {est_before} -> {est_after}" + ); + } + /// BATTERY-P02's sag criterion AT THE SUPERVISOR: a 60C punch on a /// half-full pack sags the terminal voltage far below the raw threshold /// (which would have fired the OLD `v < low_batt_v` failsafe) — the diff --git a/crates/falcon-gnss-ubx/src/dual.rs b/crates/falcon-gnss-ubx/src/dual.rs new file mode 100644 index 0000000..ccec654 --- /dev/null +++ b/crates/falcon-gnss-ubx/src/dual.rs @@ -0,0 +1,312 @@ +//! Dual-receiver GNSS selection/blending (GNSS-P02, v1.121). +//! +//! The first vehicle carries 2× u-blox M9N; the estimator input was a +//! single lane. This selector turns two (possibly absent, possibly lying) +//! fixes into ONE estimator input plus a divergence flag: +//! +//! - **Health per receiver**: presence, 3D fix, satellite count, reported +//! accuracy, and an innovation gate against the estimator's position (a +//! jump the filter would reject disqualifies the receiver THIS cycle). +//! - **Inverse-variance blending** when both are healthy — the output sits +//! between the fixes, weighted by reported accuracy. +//! - **Failover within one update** when the active side degrades: the +//! output continues from the healthy receiver; because blending always +//! lies between the two fixes, the step on failover is bounded by the +//! healthy receiver's distance to the blend — within its own accuracy +//! when the receivers agreed. +//! - **Divergence flag** (latched): sustained disagreement beyond the gate +//! raises `diverged` for the spoof monitor and pre-arm checks; the +//! selector then trusts the receiver consistent with the estimator. +//! +//! Pure, `no_std`, total (GNSS-K05): any pair of inputs — including +//! NaN/∞-poisoned fixes — yields a finite selection or an explicit +//! no-fix, never a NaN. + +/// One receiver's fix for one selector cycle, in local NED metres +/// (projection from LLH happens at the driver/integration layer). +#[derive(Clone, Copy, Debug)] +pub struct NedFix { + pub pos: [f32; 3], + /// Reported horizontal accuracy estimate, metres (M9N hAcc). + pub acc_m: f32, + pub sats: u8, + /// 3D fix (or better) reported. + pub fix_ok: bool, +} + +/// Which source produced this cycle's output. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum GnssSource { + A, + B, + Blend, + None, +} + +/// The selector's per-cycle decision. +#[derive(Clone, Copy, Debug)] +pub struct GnssDecision { + /// The estimator input: a finite NED position, or `None` (no usable fix). + pub pos: Option<[f32; 3]>, + /// Accuracy accompanying `pos` (the blend's, or the selected receiver's). + pub acc_m: f32, + pub source: GnssSource, + /// Latched: the two receivers disagreed beyond the divergence gate for + /// `DIVERGE_DEBOUNCE` consecutive dual-healthy cycles — spoof-monitor + /// and pre-arm input. Cleared only by [`DualGnss::reset`]. + pub diverged: bool, +} + +/// Satellite floor for a receiver to count as healthy. +pub const MIN_SATS: u8 = 6; +/// Accuracy ceiling (m) — a receiver reporting worse is unhealthy. +pub const MAX_ACC_M: f32 = 10.0; +/// Accuracy floor (m) — reports below are clamped (an M9N never reports +/// sub-centimetre; a denormal acc would overflow the 1/acc² blend weight +/// into ∞−∞ = NaN — caught by GNSS-K05). +pub const MIN_ACC_M: f32 = 0.01; +/// Position sanity bound (m): a |NED| beyond this is a lying receiver, +/// and bounding it keeps every blend product finite (no f32 overflow). +pub const MAX_POS_M: f32 = 1.0e6; +/// Innovation gate (m) against the estimator position: a fix jumping +/// further than this from the estimate is disqualified this cycle. +pub const INNOVATION_GATE_M: f32 = 15.0; +/// Divergence gate between the two receivers, in multiples of their +/// combined reported accuracy (floored at 5 m absolute). +pub const DIVERGE_ACC_MULT: f32 = 3.0; +pub const DIVERGE_FLOOR_M: f32 = 5.0; +/// Consecutive dual-healthy divergent cycles before the flag latches. +pub const DIVERGE_DEBOUNCE: u32 = 10; + +fn sane3(p: &[f32; 3]) -> bool { + p.iter().all(|v| v.is_finite() && *v >= -MAX_POS_M && *v <= MAX_POS_M) +} + +/// Squared 2D distance — every gate compares SQUARED quantities (order- +/// preserving for non-negatives), keeping sqrt out of the decision path +/// entirely (Kani tractability + no libm on the hot path). +fn dist2d_sq(a: &[f32; 3], b: &[f32; 3]) -> f32 { + let dx = a[0] - b[0]; + let dy = a[1] - b[1]; + dx * dx + dy * dy +} + +/// Health-gate one receiver's fix: present, finite, 3D, enough sats, +/// sane accuracy, and (when an estimate exists) inside the innovation gate. +fn healthy(fix: &Option, est: Option<[f32; 3]>) -> Option { + let f = (*fix)?; + if !f.fix_ok || f.sats < MIN_SATS { + return None; + } + if !sane3(&f.pos) || !f.acc_m.is_finite() || f.acc_m <= 0.0 || f.acc_m > MAX_ACC_M { + return None; + } + let f = NedFix { acc_m: f.acc_m.max(MIN_ACC_M), ..f }; + if let Some(e) = est { + if sane3(&e) && dist2d_sq(&f.pos, &e) > INNOVATION_GATE_M * INNOVATION_GATE_M { + return None; + } + } + Some(f) +} + +/// The dual-receiver selector. One instance per vehicle; call +/// [`update`](Self::update) once per fix interval. +pub struct DualGnss { + diverge_count: u32, + diverged: bool, +} + +impl Default for DualGnss { + fn default() -> Self { + Self::new() + } +} + +impl DualGnss { + pub fn new() -> Self { + DualGnss { diverge_count: 0, diverged: false } + } + + /// Clear the latched divergence flag (ground reset only). + pub fn reset(&mut self) { + self.diverge_count = 0; + self.diverged = false; + } + + /// One selection cycle. `est` is the estimator's current NED position + /// (the innovation-consistency reference), if converged. + pub fn update( + &mut self, + a: Option, + b: Option, + est: Option<[f32; 3]>, + ) -> GnssDecision { + let ha = healthy(&a, est); + let hb = healthy(&b, est); + match (ha, hb) { + (Some(fa), Some(fb)) => { + // Divergence watch (only meaningful with both healthy). + let gate = DIVERGE_FLOOR_M.max(DIVERGE_ACC_MULT * (fa.acc_m + fb.acc_m)); + if dist2d_sq(&fa.pos, &fb.pos) > gate * gate { + self.diverge_count = self.diverge_count.saturating_add(1); + if self.diverge_count >= DIVERGE_DEBOUNCE { + self.diverged = true; + } + } else { + self.diverge_count = 0; + } + if self.diverged { + // Trust the receiver consistent with the estimator when + // one exists; else the better-accuracy one — but never + // blend two stories that contradict each other. + let pick_a = match est { + Some(e) if sane3(&e) => dist2d_sq(&fa.pos, &e) <= dist2d_sq(&fb.pos, &e), + _ => fa.acc_m <= fb.acc_m, + }; + let f = if pick_a { fa } else { fb }; + return GnssDecision { + pos: Some(f.pos), + acc_m: f.acc_m, + source: if pick_a { GnssSource::A } else { GnssSource::B }, + diverged: true, + }; + } + // Inverse-variance blend: w ∝ 1/acc². + let wa = 1.0 / (fa.acc_m * fa.acc_m); + let wb = 1.0 / (fb.acc_m * fb.acc_m); + let wsum = wa + wb; + let mut pos = [0.0f32; 3]; + for (i, p) in pos.iter_mut().enumerate() { + *p = (fa.pos[i] * wa + fb.pos[i] * wb) / wsum; + } + // Blended accuracy: conservatively the better receiver's + // (the true inverse-variance value is smaller; reporting + // min keeps the field sqrt-free and never over-claims). + let acc = if fa.acc_m <= fb.acc_m { fa.acc_m } else { fb.acc_m }; + GnssDecision { pos: Some(pos), acc_m: acc, source: GnssSource::Blend, diverged: false } + } + (Some(f), None) => GnssDecision { + pos: Some(f.pos), + acc_m: f.acc_m, + source: GnssSource::A, + diverged: self.diverged, + }, + (None, Some(f)) => GnssDecision { + pos: Some(f.pos), + acc_m: f.acc_m, + source: GnssSource::B, + diverged: self.diverged, + }, + (None, None) => GnssDecision { + pos: None, + acc_m: f32::MAX, + source: GnssSource::None, + diverged: self.diverged, + }, + } + } + + pub fn diverged(&self) -> bool { + self.diverged + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn fix(x: f32, y: f32, acc: f32) -> Option { + Some(NedFix { pos: [x, y, -10.0], acc_m: acc, sats: 12, fix_ok: true }) + } + + /// Both healthy: the blend lies between the fixes, weighted toward the + /// more accurate receiver, and the blended accuracy beats both. + #[test] + fn blends_toward_the_better_receiver() { + let mut d = DualGnss::new(); + let dec = d.update(fix(0.0, 0.0, 1.0), fix(1.0, 0.0, 2.0), None); + let p = dec.pos.unwrap(); + assert_eq!(dec.source, GnssSource::Blend); + assert!(p[0] > 0.0 && p[0] < 0.5, "weighted toward A (acc 1 vs 2): {}", p[0]); + assert!(dec.acc_m <= 1.0, "blend accuracy never worse than the best receiver"); + assert!(!dec.diverged); + } + + /// Failover within ONE update: receiver A dies; the output continues + /// from B with a step bounded by the healthy receiver's accuracy. + #[test] + fn failover_within_one_fix_interval() { + let mut d = DualGnss::new(); + let before = d.update(fix(0.0, 0.0, 1.0), fix(0.8, 0.0, 1.0), None); + let p0 = before.pos.unwrap(); + // A drops out entirely: + let after = d.update(None, fix(0.8, 0.0, 1.0), None); + assert_eq!(after.source, GnssSource::B); + let p1 = after.pos.unwrap(); + let step = ((p1[0] - p0[0]).powi(2) + (p1[1] - p0[1]).powi(2)).sqrt(); + assert!(step <= 1.0, "failover step {} must stay within B's accuracy", step); + } + + /// Degradation failovers: accuracy collapse and a jump the innovation + /// gate rejects both disqualify a receiver the SAME cycle. + #[test] + fn accuracy_collapse_and_jump_disqualify() { + let mut d = DualGnss::new(); + let dec = d.update( + Some(NedFix { pos: [0.0; 3], acc_m: 50.0, sats: 12, fix_ok: true }), + fix(0.1, 0.0, 1.0), + None, + ); + assert_eq!(dec.source, GnssSource::B, "acc 50 m > ceiling disqualifies A"); + // jump: estimator at origin, A reports 40 m away. + let dec = d.update(fix(40.0, 0.0, 1.0), fix(0.1, 0.0, 1.0), Some([0.0; 3])); + assert_eq!(dec.source, GnssSource::B, "innovation gate rejects the jump"); + } + + /// Divergence: sustained disagreement latches the flag; the selector + /// stops blending and picks the estimator-consistent receiver. + #[test] + fn divergence_latches_and_picks_consistent() { + // A split inside the 15 m innovation gate but beyond the divergence + // gate (5 m floor, 3·(1+1)=6 m): 10 m apart, estimator nearer A. + let mut d = DualGnss::new(); + let mut last = None; + for _ in 0..DIVERGE_DEBOUNCE { + last = Some(d.update(fix(0.0, 0.0, 1.0), fix(10.0, 0.0, 1.0), Some([2.0, 0.0, -10.0]))); + } + let dec = last.unwrap(); + assert!(dec.diverged, "sustained 10 m split must latch divergence"); + assert_eq!(dec.source, GnssSource::A, "picks the estimator-consistent side"); + // and it stays latched through re-agreement: + let dec = d.update(fix(0.0, 0.0, 1.0), fix(0.1, 0.0, 1.0), None); + assert!(dec.diverged, "divergence is latched"); + } + + /// A brief disagreement below the debounce does NOT latch. + #[test] + fn transient_divergence_does_not_latch() { + let mut d = DualGnss::new(); + for _ in 0..(DIVERGE_DEBOUNCE - 1) { + d.update(fix(0.0, 0.0, 1.0), fix(10.0, 0.0, 1.0), None); + } + let dec = d.update(fix(0.0, 0.0, 1.0), fix(0.1, 0.0, 1.0), None); + assert!(!dec.diverged); + assert_eq!(dec.source, GnssSource::Blend); + } + + /// No usable fix on either side: explicit no-fix, never a made-up pos. + #[test] + fn dual_loss_is_no_fix() { + let mut d = DualGnss::new(); + let dec = d.update(None, None, None); + assert!(dec.pos.is_none()); + assert_eq!(dec.source, GnssSource::None); + let dec = d.update( + Some(NedFix { pos: [f32::NAN; 3], acc_m: 1.0, sats: 12, fix_ok: true }), + Some(NedFix { pos: [0.0; 3], acc_m: 1.0, sats: 3, fix_ok: true }), + None, + ); + assert!(dec.pos.is_none(), "NaN pos and 3 sats both unhealthy"); + } +} diff --git a/crates/falcon-gnss-ubx/src/kani_proofs.rs b/crates/falcon-gnss-ubx/src/kani_proofs.rs index eb09b82..7f35282 100644 --- a/crates/falcon-gnss-ubx/src/kani_proofs.rs +++ b/crates/falcon-gnss-ubx/src/kani_proofs.rs @@ -17,3 +17,46 @@ fn verify_parser_total() { let _ = p.push(kani::any()); } } + +/// GNSS-K05 (GNSS-P02) — dual-receiver selector totality: for ANY pair of +/// fixes (every field nondet, incl. NaN/∞ positions and accuracies) with no +/// estimator reference, the selector never panics, and a single-receiver or +/// no-fix decision carries exactly the healthy receiver's (finite) position +/// or an explicit None. The blend path's synthesized position is proptest- +/// gated (nondet f32 blend arithmetic is outside Kani's productive range). +#[kani::proof] +fn verify_dual_selector_total() { + use dual::*; + let mk = || -> Option { + if kani::any() { + Some(NedFix { + pos: [ + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + ], + acc_m: f32::from_bits(kani::any()), + sats: kani::any(), + fix_ok: kani::any(), + }) + } else { + None + } + }; + let a = mk(); + let b = mk(); + let mut d = DualGnss::new(); + let dec = d.update(a, b, None); + match dec.source { + GnssSource::None => assert!(dec.pos.is_none()), + GnssSource::A | GnssSource::B => { + let p = dec.pos.unwrap(); + assert!(p[0].is_finite() && p[1].is_finite() && p[2].is_finite()); + assert!(dec.acc_m.is_finite() && dec.acc_m > 0.0); + } + GnssSource::Blend => { + // reachable only with BOTH healthy; the arithmetic itself is + // proptest territory — totality (no panic) is proven here. + } + } +} diff --git a/crates/falcon-gnss-ubx/src/lib.rs b/crates/falcon-gnss-ubx/src/lib.rs index 42bba00..7069c82 100644 --- a/crates/falcon-gnss-ubx/src/lib.rs +++ b/crates/falcon-gnss-ubx/src/lib.rs @@ -255,6 +255,8 @@ impl UbxReader { } } +pub mod dual; + #[cfg(kani)] mod kani_proofs; diff --git a/crates/relay-flowrange/plain/src/kani_proofs.rs b/crates/relay-flowrange/plain/src/kani_proofs.rs index 9ee2f7c..a684dc3 100644 --- a/crates/relay-flowrange/plain/src/kani_proofs.rs +++ b/crates/relay-flowrange/plain/src/kani_proofs.rs @@ -31,3 +31,31 @@ fn verify_flow_gated() { assert!(out.is_none()); } } + +/// FLOWRANGE-K03 (RANGEDRV-P01) — TF02 decode is TOTAL and its quality gate +/// is SOUND over ALL 9-byte patterns: no panic, and an accepted sample is +/// always inside the 0.1–40 m envelope with reliable strength. Integer-only +/// (the f32 is a cm cast), so fully tractable. +#[kani::proof] +fn verify_tf02_decode_total_gate_sound() { + let frame: [u8; tf02::TF02_FRAME_LEN] = kani::any(); + match tf02::decode_tf02_frame(&frame) { + Ok(s) => { + assert!(s.strength >= tf02::TF02_STRENGTH_MIN && s.strength != 0xFFFF); + assert!(s.distance_m >= 0.1 && s.distance_m <= 40.0); + } + Err(_) => {} + } +} + +/// FLOWRANGE-K04 (RANGEDRV-P01) — the streaming scanner is total and its +/// consumed count never exceeds the input for ANY 16-byte stream. +#[kani::proof] +#[kani::unwind(20)] +fn verify_tf02_scan_total() { + let bytes: [u8; 16] = kani::any(); + let n: usize = kani::any(); + kani::assume(n <= 16); + let (_, used) = tf02::scan_tf02(&bytes[..n]); + assert!(used <= n); +} diff --git a/crates/relay-flowrange/plain/src/lib.rs b/crates/relay-flowrange/plain/src/lib.rs index 46fdff0..9d6d0c8 100644 --- a/crates/relay-flowrange/plain/src/lib.rs +++ b/crates/relay-flowrange/plain/src/lib.rs @@ -77,6 +77,8 @@ pub fn flow_to_velocity( } } +pub mod tf02; + #[cfg(kani)] mod kani_proofs; @@ -135,12 +137,9 @@ mod proptests { /// the altitude never exceeds the range magnitude (|cos| ≤ 1). #[test] fn altitude_gated_and_bounded(r in -100.0f32..100.0, tilt in -3.0f32..3.0) { - match range_to_altitude(r, tilt, 0.2, 30.0) { - Some(a) => { - proptest::prop_assert!((0.2..=30.0).contains(&r)); - proptest::prop_assert!(a.abs() <= r.abs() + 1e-3); - } - None => {} + if let Some(a) = range_to_altitude(r, tilt, 0.2, 30.0) { + proptest::prop_assert!((0.2..=30.0).contains(&r)); + proptest::prop_assert!(a.abs() <= r.abs() + 1e-3); } } } diff --git a/crates/relay-flowrange/plain/src/tf02.rs b/crates/relay-flowrange/plain/src/tf02.rs new file mode 100644 index 0000000..3edbd33 --- /dev/null +++ b/crates/relay-flowrange/plain/src/tf02.rs @@ -0,0 +1,218 @@ +//! Benewake TF02-Pro UART frame decode (RANGEDRV-P01, v1.121). +//! +//! The standard Benewake 9-byte data frame, cross-validated against TWO +//! independent open-source parsers (the external-reference rule for wire +//! decoders — self-round-trip proves robustness, not conformance): +//! +//! - ArduPilot `AP_RangeFinder_Benewake.cpp`: dist LE cm at bytes 2–3 +//! (`(linebuf[3] << 8) | linebuf[2]`), checksum = sum of bytes 0..7 +//! compared to byte 8. +//! - PX4 `tfmini_parser.cpp`: sync `'Y','Y'` (0x59 0x59), dist LE cm, +//! strength at bytes 4–5, `cksm += parserbuf[i]` over the first 8 +//! bytes, 0xFFFF distance = invalid sentinel. +//! - Benewake TF02-Pro manual: strength < 60 or == 65535 ⇒ the distance +//! is unreliable; device envelope 0.1–40 m. +//! +//! Decode is TOTAL over arbitrary bytes (Kani FR-K03/K04) and the quality +//! gate is part of the decoder: a low-strength or out-of-envelope return +//! NEVER reaches the caller as a range. + +/// One decoded, quality-gated TF02-Pro measurement. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Tf02Sample { + /// Gated distance, metres (within the 0.1–40 m device envelope). + pub distance_m: f32, + /// Signal strength (device units, 60..65534 after gating). + pub strength: u16, +} + +/// Why a syntactically valid frame was rejected by the quality gate. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum Tf02Reject { + /// Header or checksum wrong — not a frame (resync and move on). + NotAFrame, + /// The device's own invalid-measurement sentinel (dist == 0xFFFF). + InvalidSentinel, + /// Strength below 60 or the 65535 unreliable marker. + LowStrength, + /// Outside the 0.1–40 m device envelope (spurious return). + OutOfEnvelope, +} + +/// TF02-Pro frame length. +pub const TF02_FRAME_LEN: usize = 9; +/// Frame header byte (twice). +pub const TF02_HEADER: u8 = 0x59; +/// Minimum reliable strength (Benewake manual). +pub const TF02_STRENGTH_MIN: u16 = 60; +/// Device envelope, centimetres. +pub const TF02_DIST_MIN_CM: u16 = 10; +pub const TF02_DIST_MAX_CM: u16 = 4000; + +/// Decode ONE 9-byte frame. Total: any byte pattern yields `Ok` or a +/// specific reject — never a panic, never a NaN/out-of-envelope range. +pub fn decode_tf02_frame(frame: &[u8; TF02_FRAME_LEN]) -> Result { + if frame[0] != TF02_HEADER || frame[1] != TF02_HEADER { + return Err(Tf02Reject::NotAFrame); + } + let mut sum: u8 = 0; + for b in frame.iter().take(TF02_FRAME_LEN - 1) { + sum = sum.wrapping_add(*b); + } + if sum != frame[TF02_FRAME_LEN - 1] { + return Err(Tf02Reject::NotAFrame); + } + let dist_cm = u16::from_le_bytes([frame[2], frame[3]]); + let strength = u16::from_le_bytes([frame[4], frame[5]]); + if dist_cm == 0xFFFF { + return Err(Tf02Reject::InvalidSentinel); + } + if strength < TF02_STRENGTH_MIN || strength == 0xFFFF { + return Err(Tf02Reject::LowStrength); + } + if !(TF02_DIST_MIN_CM..=TF02_DIST_MAX_CM).contains(&dist_cm) { + return Err(Tf02Reject::OutOfEnvelope); + } + // Division, not ×0.01: the f32 quotient is correctly rounded, so the + // envelope bounds are EXACT (10/100 == 0.1f32; 4000/100 == 40.0f32) — + // Kani caught 10 × 0.01f32 = 0.099999994 escaping the envelope. + Ok(Tf02Sample { distance_m: dist_cm as f32 / 100.0, strength }) +} + +/// Streaming resync scanner: find and decode the first valid frame in +/// `bytes`, returning `(result, bytes_consumed)`. On a valid or gated +/// frame, consumes through its end; with no decodable frame, consumes +/// up to the last possible header start so the caller can append more +/// bytes and retry. Total over arbitrary input. +pub fn scan_tf02(bytes: &[u8]) -> (Option>, usize) { + let mut i = 0usize; + while i + TF02_FRAME_LEN <= bytes.len() { + if bytes[i] == TF02_HEADER && bytes[i + 1] == TF02_HEADER { + let mut frame = [0u8; TF02_FRAME_LEN]; + frame.copy_from_slice(&bytes[i..i + TF02_FRAME_LEN]); + let r = decode_tf02_frame(&frame); + if r != Err(Tf02Reject::NotAFrame) { + return (Some(r), i + TF02_FRAME_LEN); + } + // checksum-failed header: skip ONE byte (a real frame may + // start inside the corrupt span), keep scanning. + } + i += 1; + } + (None, i) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Build a frame with the documented checksum (sum of bytes 0..7, + /// low 8 bits) — the SAME arithmetic both external parsers quote. + fn frame(dist_cm: u16, strength: u16, temp_raw: u16) -> [u8; 9] { + let d = dist_cm.to_le_bytes(); + let s = strength.to_le_bytes(); + let t = temp_raw.to_le_bytes(); + let mut f = [0x59, 0x59, d[0], d[1], s[0], s[1], t[0], t[1], 0]; + f[8] = f[..8].iter().fold(0u8, |a, b| a.wrapping_add(*b)); + f + } + + /// Conformance against the externally documented arithmetic: + /// ArduPilot's example decode `(linebuf[3]<<8)|linebuf[2]` in cm. + /// 5.56 m, strength 1080 (a mid-range concrete case worked by hand): + /// dist 556 = 0x022C → bytes 2C 02; strength 1080 = 0x0438 → 38 04; + /// checksum = low byte of 0x59+0x59+0x2C+0x02+0x38+0x04+0+0 = 0x11C → 0x1C. + #[test] + fn decodes_documented_frame_arithmetic() { + let f = [0x59, 0x59, 0x2C, 0x02, 0x38, 0x04, 0x00, 0x00, 0x1C]; + assert_eq!( + decode_tf02_frame(&f), + Ok(Tf02Sample { distance_m: 5.56, strength: 1080 }) + ); + // and the constructor agrees with the hand-worked bytes: + assert_eq!(frame(556, 1080, 0), f); + } + + #[test] + fn rejects_bad_checksum_and_header() { + let mut f = frame(556, 1080, 0); + f[8] ^= 0xFF; + assert_eq!(decode_tf02_frame(&f), Err(Tf02Reject::NotAFrame)); + let mut g = frame(556, 1080, 0); + g[0] = 0x58; + assert_eq!(decode_tf02_frame(&g), Err(Tf02Reject::NotAFrame)); + } + + /// The gate: low strength, unreliable marker, sentinel distance, and + /// out-of-envelope returns NEVER reach the caller as a range. + #[test] + fn quality_gate_rejects() { + assert_eq!(decode_tf02_frame(&frame(556, 59, 0)), Err(Tf02Reject::LowStrength)); + assert_eq!(decode_tf02_frame(&frame(556, 0xFFFF, 0)), Err(Tf02Reject::LowStrength)); + assert_eq!( + decode_tf02_frame(&frame(0xFFFF, 1080, 0)), + Err(Tf02Reject::InvalidSentinel) + ); + assert_eq!(decode_tf02_frame(&frame(5, 1080, 0)), Err(Tf02Reject::OutOfEnvelope)); + assert_eq!(decode_tf02_frame(&frame(4050, 1080, 0)), Err(Tf02Reject::OutOfEnvelope)); + // envelope boundaries are inclusive: + assert!(decode_tf02_frame(&frame(10, 1080, 0)).is_ok()); + assert!(decode_tf02_frame(&frame(4000, 1080, 0)).is_ok()); + } + + /// Resync: garbage → frame → garbage; the scanner finds the frame and + /// reports the right consumed count. + #[test] + fn scanner_resyncs_through_garbage() { + let f = frame(200, 500, 0); + let mut stream = [0u8; 25]; + stream[..7].copy_from_slice(&[0x00, 0x59, 0x12, 0xFF, 0x59, 0x00, 0xAB]); + stream[7..16].copy_from_slice(&f); + let (r, used) = scan_tf02(&stream); + assert_eq!(r, Some(Ok(Tf02Sample { distance_m: 2.0, strength: 500 }))); + assert_eq!(used, 16); + // no frame at all: consumed leaves a potential partial header tail. + let (r, used) = scan_tf02(&stream[..7]); + assert_eq!(r, None); + assert!(used <= 7); + } + + /// A frame straddling a corrupt double-header: the scanner must not + /// get stuck (skip-one-byte resync) and still find the real frame. + #[test] + fn scanner_skips_false_header() { + let f = frame(300, 800, 0); + let mut stream = [0u8; 20]; + stream[0] = 0x59; + stream[1] = 0x59; // false start, checksum will fail + stream[2..11].copy_from_slice(&f); + let (r, _used) = scan_tf02(&stream); + assert_eq!(r, Some(Ok(Tf02Sample { distance_m: 3.0, strength: 800 }))); + } + + mod proptests { + use super::super::*; + use proptest::prelude::*; + + proptest! { + /// Totality + gate soundness over arbitrary bytes: never a + /// panic, and any accepted sample is inside the envelope with + /// reliable strength. + #[test] + fn decode_total_and_gate_sound(bytes in proptest::array::uniform9(any::())) { + if let Ok(s) = decode_tf02_frame(&bytes) { + prop_assert!(s.distance_m >= 0.10 && s.distance_m <= 40.0); + prop_assert!(s.strength >= 60 && s.strength != 0xFFFF); + prop_assert!(s.distance_m.is_finite()); + } + } + + /// Scanner totality: consumed never exceeds input length. + #[test] + fn scan_total(bytes in proptest::collection::vec(any::(), 0..64)) { + let (_, used) = scan_tf02(&bytes); + prop_assert!(used <= bytes.len()); + } + } + } +}