From 2096f58b3f6d44d8c919e4a8552290ada2057ee2 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Thu, 16 Jul 2026 08:21:40 +0200 Subject: [PATCH] =?UTF-8?q?feat(falcon):=20v1.122=20=E2=80=94=20RPM-tracke?= =?UTF-8?q?d=20harmonic=20notches=20+=20pre-arm=20check=20table=20(NOTCH-P?= =?UTF-8?q?01=20+=20PREARM-P03)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit NOTCH-P01 — new relay-notch engine (PX4 IMU_GYRO_DNF / ArduPilot INS_HNTCH / Betaflight RPM-filter parity): per-motor biquad notches at the rotation fundamental + 2nd harmonic, centers tracking ACHIEVED eRPM via the same read_motor_rpm seam the FDI consumes (jess DD-024: bidir-DShot primary). Absent RPM => bit-exact unity bypass (no filtering is safer than a mistracked notch); per-notch Nyquist clamp. Measured (not asserted): >= 20 dB at the tracked fundamental across the hover-to-full band, sweep within 3 dB, < 10 deg added phase at the rate-loop crossover with the FULL 4-motor bank engaged. Kani NOTCH-K01..K03 drove two real fixes: operand sanitization (no intermediate can even COMPUTE a NaN from a poisoned state — CBMC's side-conditions now prove it) and the trig-free band_ok split (libm's rem_pio2_large unwinds >1500 iterations on symbolic args). falcon-core wires the bank into the CONTROL-path gyro ahead of the LPF (estimator stays raw); SimBackend gained rotor-LINE vibration + an rpm_telemetry switch; the closed-loop oracle shows the tracking notch cutting hover motor thrash to < 60% of the bypass baseline under injected rotor vibration (#270's regression-metric shape). PREARM-P03 — the pre-arm breadth as a TABLE (data, not conditionals): 19 rows spanning estimator/nav integrity, configuration consistency, hardware consistency, and safety state, each with a DISTINCT operator-readable STATUSTEXT reason. The legacy six checks are rows 0..=5 (always required); breadth rows gate once DECLARED (a bench without RC is not blocked by a link check it cannot satisfy; a declared row that fails ALWAYS blocks). Kani PREFLIGHT-K03/K04: the gate is exact and MONOTONE (failing any row of an Allowed table can never remain Allowed). Per-row pair tests are table-driven — coverage equals the table length by construction. FlightSupervisor populates every self-feedable row per step (ESC telemetry declare-on-first- sight), integrations feed the rest via set_check; command(Arm) now gates on the table. Cold-boot criterion rides the REAL PARAM-P03 two-slot NVM seam: defaults-fallback cannot arm (distinct reason); after save + genuine reload it can. Gate hygiene: relay-notch enrolled in kani.yml + CI clippy at birth. SWREQ-FALCON-NOTCH-P01 + SWREQ-FALCON-PREARM-P03 -> 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 | 2 +- .github/workflows/kani.yml | 1 + Cargo.lock | 9 + Cargo.toml | 1 + artifacts/swreq/SWREQ-FALCON-NOTCH-P01.yaml | 2 +- artifacts/swreq/SWREQ-FALCON-PREARM-P03.yaml | 2 +- crates/falcon-core/Cargo.toml | 1 + crates/falcon-core/plain/src/lib.rs | 255 +++++++++- crates/relay-notch/Cargo.toml | 21 + .../plain/proptest-regressions/lib.txt | 7 + crates/relay-notch/plain/src/kani_proofs.rs | 72 +++ crates/relay-notch/plain/src/lib.rs | 438 ++++++++++++++++++ .../relay-preflight/plain/src/kani_proofs.rs | 42 ++ crates/relay-preflight/plain/src/lib.rs | 208 +++++++++ 14 files changed, 1050 insertions(+), 11 deletions(-) create mode 100644 crates/relay-notch/Cargo.toml create mode 100644 crates/relay-notch/plain/proptest-regressions/lib.txt create mode 100644 crates/relay-notch/plain/src/kani_proofs.rs create mode 100644 crates/relay-notch/plain/src/lib.rs diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c38ee14..cf86a6d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -65,7 +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 \ + -p relay-batt -p relay-flowrange -p falcon-gnss-ubx -p relay-notch \ --all-targets -- -D warnings test: diff --git a/.github/workflows/kani.yml b/.github/workflows/kani.yml index 7287eeb..e560a35 100644 --- a/.github/workflows/kani.yml +++ b/.github/workflows/kani.yml @@ -63,6 +63,7 @@ jobs: - relay-mix-quad - relay-mavlink - relay-batt + - relay-notch # Flight-critical control + estimator proofs. These harnesses were # authored alongside the engines but never added to this matrix, so # CI never ran them (the same orphaned-proof gap that bit relay-rc diff --git a/Cargo.lock b/Cargo.lock index 8a9f5cf..2fd73b9 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -282,6 +282,7 @@ dependencies = [ "relay-log", "relay-math", "relay-mix-quad", + "relay-notch", "relay-param", "relay-preflight", ] @@ -1096,6 +1097,14 @@ dependencies = [ "proptest", ] +[[package]] +name = "relay-notch" +version = "0.1.0" +dependencies = [ + "proptest", + "relay-math", +] + [[package]] name = "relay-offboard" version = "0.1.0" diff --git a/Cargo.toml b/Cargo.toml index 7f6ba96..9cf3fd6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -27,6 +27,7 @@ members = [ "crates/relay-dronecan", "crates/relay-param", "crates/relay-batt", + "crates/relay-notch", "crates/relay-calib", "crates/relay-log", "crates/relay-mix-multi", diff --git a/artifacts/swreq/SWREQ-FALCON-NOTCH-P01.yaml b/artifacts/swreq/SWREQ-FALCON-NOTCH-P01.yaml index 0ec5310..cd09b51 100644 --- a/artifacts/swreq/SWREQ-FALCON-NOTCH-P01.yaml +++ b/artifacts/swreq/SWREQ-FALCON-NOTCH-P01.yaml @@ -2,7 +2,7 @@ artifacts: - id: SWREQ-FALCON-NOTCH-P01 type: sw-req title: "NOTCH-P01 — RPM-driven harmonic notch filtering on the gyro path" - status: proposed + status: implemented release: falcon-v1.122.0 description: > The rate-loop gyro path shall attenuate per-motor rotational vibration diff --git a/artifacts/swreq/SWREQ-FALCON-PREARM-P03.yaml b/artifacts/swreq/SWREQ-FALCON-PREARM-P03.yaml index b38fb7a..4b59e41 100644 --- a/artifacts/swreq/SWREQ-FALCON-PREARM-P03.yaml +++ b/artifacts/swreq/SWREQ-FALCON-PREARM-P03.yaml @@ -2,7 +2,7 @@ artifacts: - id: SWREQ-FALCON-PREARM-P03 type: sw-req title: "PREARM-P03 — pre-arm check breadth: configuration, estimator, and hardware consistency" - status: proposed + status: implemented release: falcon-v1.122.0 description: > The pre-arm gate (PREARM-P01/P02) shall be extended toward the diff --git a/crates/falcon-core/Cargo.toml b/crates/falcon-core/Cargo.toml index 04fc774..955a734 100644 --- a/crates/falcon-core/Cargo.toml +++ b/crates/falcon-core/Cargo.toml @@ -22,6 +22,7 @@ relay-preflight = { path = "../relay-preflight" } relay-calib = { path = "../relay-calib" } relay-batt = { path = "../relay-batt" } relay-flowrange = { path = "../relay-flowrange" } +relay-notch = { path = "../relay-notch" } 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/lib.rs b/crates/falcon-core/plain/src/lib.rs index 9b2db08..e00f25b 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -167,6 +167,13 @@ pub struct FlightCore { /// directional walk-off latches. OR-ed with the selector divergence into /// [`nav_compromised`](Self::nav_compromised). spoof: relay_iekf::SpoofMonitor, + /// RPM-tracked harmonic notch bank on the CONTROL-path gyro (NOTCH-P01, + /// v1.122): per-motor fundamental + 2nd harmonic, driven by the same + /// read_motor_rpm seam the FDI consumes; clean unity bypass without RPM + /// telemetry. The estimator keeps the raw (calibrated) gyro — the IEKF's + /// gravity/mag updates already reject zero-mean vibration, and filtering + /// the estimator input would add lag where it is not needed. + notch: relay_notch::HarmonicNotchBank<4>, /// 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 + @@ -253,6 +260,7 @@ impl FlightCore { // 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), + notch: relay_notch::HarmonicNotchBank::new(loop_hz), // CUSUM thresholds matching the SITL-verified fault-tolerance chain. fdi: RotorFaultDetector::new(0.5, 0.1), failed_motor: None, @@ -659,8 +667,14 @@ impl FlightCore { a_cmd[1] *= s; } + // v1.122 — RPM-tracked harmonic notches on the CONTROL path, ahead + // of the low-pass (NOTCH-P01): read the achieved RPM once per step + // (shared with the FDI below), track or bypass cleanly. + let rpm_now = b.read_motor_rpm(); + self.notch.update_rpm(rpm_now); + let gyro_ctrl = self.notch.apply(gyro); // advance the (stateful) gyro low-pass every cycle so it stays warm. - let gyro_f = self.gyro_lpf.filter(gyro); + let gyro_f = self.gyro_lpf.filter(gyro_ctrl); // ── Attitude + allocate ── let motors = if let Some(failed) = self.failed_motor { @@ -670,7 +684,7 @@ impl FlightCore { // promoted into the production loop (v1.103). let r = quat_to_rotmat(est.q); let b3_d = thrust_axis_ned(a_cmd).unwrap_or([0.0, 0.0, 1.0]); - let torque = self.geo.moment_reduced(&r, gyro, b3_d); + let torque = self.geo.moment_reduced(&r, gyro_ctrl, b3_d); self.mixer.mix_rotor_out(failed, torque, thrust, ROTOR_OUT_FLOOR) } else { // NORMAL: full-attitude geometric desired-rate → ADRC torque → mix. @@ -725,7 +739,7 @@ impl FlightCore { && self.step_count >= self.fdi_warmup_steps && fdi_steady { - if let Some(rpm) = b.read_motor_rpm() { + if let Some(rpm) = rpm_now { let mut resid = [0.0f32; 4]; let mut i = 0; while i < 4 { @@ -874,6 +888,13 @@ pub struct FlightSupervisor { /// the high-wind detector (v1.101). On reaching WIND_CYCLES it commands RTL /// (the vehicle is fighting a disturbance beyond its control authority). wind_count: u32, + /// The PREARM-P03 check TABLE (v1.122): the legacy six checks mirrored + /// as rows 0..=5 plus the breadth rows (estimator/nav integrity, config + /// consistency, hardware consistency, safety state). Self-populatable + /// rows are refreshed every step; integration-fed rows (RC/GCS link, + /// params-from-NVM, sensor freshness, …) enter via [`set_check`] — + /// setting one DECLARES it required from then on. + check_table: relay_preflight::CheckTable, /// The latest pre-arm / commander check inputs (sensor health, estimator /// convergence, calibration, geofence, battery, failsafe config). Fed by /// `set_preflight`; `command(Arm, …)` gates arming on `arm_check` of these @@ -908,6 +929,18 @@ impl FlightSupervisor { rtl_latched: false, runaway_count: 0, wind_count: 0, + check_table: { + // back-compat: mirror the default-true legacy six (below) — + // a supervisor that has never stepped can still be armed by + // the bring-up tests; the first update_preflight refreshes + // every row with real signals. + let mut t = relay_preflight::CheckTable::new(); + use relay_preflight::CheckId; + for id in CheckId::ALL.iter().take(6) { + t.set(*id, true); + } + t + }, // back-compat default: all checks pass (an integration that feeds real // health via set_preflight tightens this to a fail-safe gate). preflight: relay_preflight::PreflightChecks { @@ -962,6 +995,16 @@ impl FlightSupervisor { /// battery, sensor/RC presence, …); `command(Arm, …)` then gates on them. pub fn set_preflight(&mut self, checks: relay_preflight::PreflightChecks) { self.preflight = checks; + // keep the PREARM-P03 table's legacy rows in lockstep — the arm + // gate reads the TABLE (which embeds these as rows 0..=5). + use relay_preflight::CheckId; + let t = &mut self.check_table; + t.set(CheckId::SensorsHealthy, checks.sensors_healthy); + t.set(CheckId::EstimatorConverged, checks.estimator_converged); + t.set(CheckId::CalibrationPresent, checks.calibration_present); + t.set(CheckId::GeofenceLoaded, checks.geofence_loaded); + t.set(CheckId::BatteryOk, checks.battery_ok); + t.set(CheckId::FailsafeConfigured, checks.failsafe_configured); } /// Derive the pre-arm checks the supervisor can know from its OWN state, each @@ -987,6 +1030,34 @@ impl FlightSupervisor { if self.core.nav_compromised() { self.preflight.sensors_healthy = false; } + // PREARM-P03: mirror the legacy six into the table and refresh every + // row the supervisor can feed itself. + use relay_preflight::CheckId; + let t = &mut self.check_table; + t.set(CheckId::SensorsHealthy, self.preflight.sensors_healthy); + t.set(CheckId::EstimatorConverged, self.preflight.estimator_converged); + t.set(CheckId::CalibrationPresent, self.preflight.calibration_present); + t.set(CheckId::GeofenceLoaded, self.preflight.geofence_loaded); + t.set(CheckId::BatteryOk, self.preflight.battery_ok); + t.set(CheckId::FailsafeConfigured, self.preflight.failsafe_configured); + t.set(CheckId::EstimatorInnovation, !self.core.nav_compromised()); + t.set(CheckId::GnssAgreement, !self.core.gnss_diverged()); + t.set( + CheckId::GeofenceSane, + self.fence_radius.is_finite() + && self.fence_radius > 0.0 + && self.home.iter().all(|v| v.is_finite()), + ); + // Battery must clear the low threshold WITH margin for the takeoff + // draw — arming at low+ε means an RTL seconds after liftoff. + t.set( + CheckId::BatteryTakeoffMargin, + self.batt_state.degraded || self.batt_state.soc >= 0.30, + ); + t.set( + CheckId::NoFailsafeLatched, + !self.rtl_latched && !self.batt_state.low && !self.batt_state.critical, + ); self.preflight.geofence_loaded = self.fence_radius > 0.0; } @@ -1005,6 +1076,24 @@ impl FlightSupervisor { self.batt_state } + /// PREARM-P03 integration seam: feed an integration-owned check row + /// (RC/GCS link, params-from-NVM, calibration age, sensor freshness, + /// rangefinder plausibility). Setting a row DECLARES it — the arm gate + /// is thereafter blocked whenever it fails (monotone, PREFLIGHT-K04). + pub fn set_check(&mut self, id: relay_preflight::CheckId, pass: bool) { + self.check_table.set(id, pass); + } + + /// Why arming would be refused by the PREARM-P03 table (the first + /// failing required row), with the operator STATUSTEXT via + /// `CheckId::reason_text`. `None` = the table allows. + pub fn arm_blocked_check(&self) -> Option { + match relay_preflight::arm_check_table(&self.check_table) { + relay_preflight::TableVerdict::Allowed => None, + relay_preflight::TableVerdict::Blocked(id) => Some(id), + } + } + pub fn arm_blocked_reason(&self) -> Option { match relay_preflight::arm_check(self.preflight) { relay_preflight::ArmVerdict::Allowed => None, @@ -1017,8 +1106,12 @@ impl FlightSupervisor { /// FSM only enters `Armed` when every check passes AND the vehicle is level /// with throttle idle — the Kani-proven entry gate (relay-fsm FSM-K03). pub fn command(&mut self, ev: relay_fsm::Event, level: bool, throttle_low: bool) { - let prearm_ok = - matches!(relay_preflight::arm_check(self.preflight), relay_preflight::ArmVerdict::Allowed); + // v1.122: the PREARM-P03 table is the gate — it embeds the legacy + // six as rows 0..=5 and adds the declared breadth rows. + let prearm_ok = matches!( + relay_preflight::arm_check_table(&self.check_table), + relay_preflight::TableVerdict::Allowed + ); let g = relay_fsm::Gates { level, throttle_low, have_position: true, prearm_ok }; self.fsm.on(ev, g); } @@ -1133,6 +1226,18 @@ impl FlightSupervisor { // current sense) — a throttle-punch sag does not false-trigger, and a // rebounding spent pack cannot hide. self.batt_state = self.batt_est.update(b.dt(), b.read_battery_v(), b.read_battery_i()); + // PREARM-P03 hardware row: ESC/eRPM telemetry alive (the notch's and + // FDI's source). DECLARE-ON-FIRST-SIGHT: a vehicle that has never + // shown eRPM (no bidir-DShot) is not gated on it, but once seen, + // its LOSS blocks arming (one-way, like every declared row). + match b.read_motor_rpm() { + Some(_) => self.check_table.set(relay_preflight::CheckId::EscTelemetry, true), + None => { + if self.check_table.is_required(relay_preflight::CheckId::EscTelemetry) { + self.check_table.set(relay_preflight::CheckId::EscTelemetry, false); + } + } + } let batt_fail = self.batt_state.low || self.batt_state.critical; let breach = dist_home > self.fence_radius || batt_fail; if breach && self.fsm.is_airborne() && self.fsm.mode() != Mode::Land { @@ -1482,6 +1587,15 @@ 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, + /// Rotor-harmonic gyro vibration amplitude (rad/s) — NARROWBAND lines at + /// each motor's rotation frequency (v1.122, NOTCH-P01's contaminant; the + /// broadband `vibration` pathology cannot exercise a notch). + pub rotor_vib: f32, + /// Per-motor vibration phase accumulators (rad). + rotor_vib_phase: [f32; 4], + /// RPM telemetry available? (bidir-DShot present). false = read_motor_rpm + /// returns None: the notch bank must bypass and the FDI loses its source. + pub rpm_telemetry: bool, /// 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 @@ -1576,6 +1690,9 @@ impl SimBackend { baro_noise: 0.0, battery_v: 16.0, battery_i: None, + rotor_vib: 0.0, + rotor_vib_phase: [0.0; 4], + rpm_telemetry: true, gps_pos_bias: [0.0; 3], baro_bias: 0.0, range_enabled: false, @@ -1688,10 +1805,25 @@ impl FlightBackend for SimBackend { // v1.9 — the measured gyro carries the accumulated (drifting) bias the // IEKF bias state must estimate out. v1.18 — plus optional white noise. let gw = self.path.gyro_white; + // v1.122 — per-motor rotational vibration: a LINE at each rotor's + // rotation frequency (phase advanced from the lagged motor state), + // split across roll/pitch like a real airframe transmits it. + let mut vib = [0.0f32; 3]; + if self.rotor_vib > 0.0 { + for m in 0..4 { + let f = self.motor_state[m] * ESC_RPM_FULL / 60.0; + self.rotor_vib_phase[m] += 2.0 * core::f32::consts::PI * f * self.dt; + if self.rotor_vib_phase[m] > 2.0 * core::f32::consts::PI { + self.rotor_vib_phase[m] -= 2.0 * core::f32::consts::PI; + } + vib[0] += self.rotor_vib * relay_math::sinf(self.rotor_vib_phase[m]); + vib[1] += self.rotor_vib * relay_math::cosf(self.rotor_vib_phase[m]); + } + } let gyro = [ - self.omega[0] + self.gyro_bias[0] + gw * self.noise_unit(), - self.omega[1] + self.gyro_bias[1] + gw * self.noise_unit(), - self.omega[2] + self.gyro_bias[2] + gw * self.noise_unit(), + self.omega[0] + self.gyro_bias[0] + vib[0] + gw * self.noise_unit(), + self.omega[1] + self.gyro_bias[1] + vib[1] + gw * self.noise_unit(), + self.omega[2] + self.gyro_bias[2] + vib[2] + gw * self.noise_unit(), ]; ImuSample { accel, gyro } } @@ -1929,6 +2061,9 @@ impl FlightBackend for SimBackend { } } fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + if !self.rpm_telemetry { + return None; + } // ACHIEVED per-rotor RPM from the lagged actual motor state — a failed // rotor reads 0 (it was pinned in `write_motors`). This is the FDI's // commanded-vs-achieved source; with no failure it tracks the command. @@ -2148,6 +2283,11 @@ mod blackbox_replay_tests { } } +#[cfg(test)] +fn falcon_core_tuning_register(store: &mut relay_param::ParamStore<8>) -> bool { + crate::tuning::register_tuning(store) +} + #[cfg(test)] mod tests { use super::*; @@ -2738,6 +2878,105 @@ mod tests { ); } + /// PREARM-P03 integration criterion: a SITL cold boot whose parameter + /// store fell back to defaults (blank NVM) CANNOT arm — with the + /// distinct operator reason — and after a save + genuine NVM load it + /// can. Runs the REAL PARAM-P03 persistence seam (two-slot FRAM image + /// over the ArrayNvm mock), not a hand-set flag. + #[test] + fn defaults_fallback_params_block_arming() { + use relay_param::persist::{load, save, ArrayNvm, Layout, LoadOutcome}; + use relay_param::ParamStore; + use relay_preflight::CheckId; + + 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.ground_contact = true; + let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 50.0, 2.0, 14.0); + sup.set_calibration(relay_calib::CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..relay_calib::CalParams::identity() + }); + + // COLD BOOT: blank NVM → defaults-fallback; the integration reports + // the load outcome into the check table. + let mut nvm: ArrayNvm<2048> = ArrayNvm::new(); + let layout = Layout::new(16); + let mut store: ParamStore<8> = ParamStore::new(); + assert!(falcon_core_tuning_register(&mut store)); + let rep = load(&mut store, &nvm, layout, 1); + assert_eq!(rep.outcome, LoadOutcome::FreshDefaults); + sup.set_check(CheckId::ParamsFromNvm, rep.outcome == LoadOutcome::Loaded); + + for _ in 0..2000 { + sup.step(&mut b); + } + sup.command(relay_fsm::Event::Arm, true, true); + assert_eq!(sup.mode(), relay_fsm::Mode::Disarmed, "defaults-fallback must not arm"); + assert_eq!(sup.arm_blocked_check(), Some(CheckId::ParamsFromNvm)); + assert_eq!( + CheckId::ParamsFromNvm.reason_text(), + "PREARM: params are defaults (no NVM)" + ); + + // field save + reboot-reload: a GENUINE NVM image now loads. + save(&store, &mut nvm, layout, 1).expect("save"); + let mut store2: ParamStore<8> = ParamStore::new(); + assert!(falcon_core_tuning_register(&mut store2)); + let rep2 = load(&mut store2, &nvm, layout, 1); + assert_eq!(rep2.outcome, LoadOutcome::Loaded); + sup.set_check(CheckId::ParamsFromNvm, rep2.outcome == LoadOutcome::Loaded); + sup.step(&mut b); + sup.command(relay_fsm::Event::Arm, true, true); + assert_eq!(sup.mode(), relay_fsm::Mode::Armed, "arms after the NVM load"); + } + + /// NOTCH-P01 closed-loop criterion: hover with per-motor rotor-line + /// vibration injected on the gyro. With RPM telemetry (notch TRACKING) + /// the rate-loop noise — measured as motor-command thrash — is + /// SUBSTANTIALLY below the bypass baseline; and the vehicle holds an + /// upright hover either way (the notch helps, it must not be required + /// for basic stability at this amplitude). + #[test] + fn notch_reduces_rate_loop_noise_under_rotor_vibration() { + fn hover_thrash(rpm_telemetry: bool) -> f32 { + let dt = 0.004f32; // 250 Hz — the flight loop rate + 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.rotor_vib = 0.8; // rad/s per motor line — a rough 1045 airframe + b.rpm_telemetry = rpm_telemetry; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-3.0); + for _ in 0..4000 { + core.step(&mut b); + } + // measure: RMS deviation of motor 0's ACHIEVED state over 8 s of + // settled hover — command thrash is the limit-cycle/vibration + // symptom (#270's regression metric shape). + let n = 2000usize; + let mut sum = 0.0f64; + let mut sum2 = 0.0f64; + for _ in 0..n { + core.step(&mut b); + let m = b.motor_state[0] as f64; + sum += m; + sum2 += m * m; + } + let mean = sum / n as f64; + let var = (sum2 / n as f64 - mean * mean).max(0.0); + assert!(b.tilt() < 0.2, "hover must hold (telemetry={rpm_telemetry})"); + (var.sqrt()) as f32 + } + let with_notch = hover_thrash(true); + let baseline = hover_thrash(false); + assert!( + with_notch < 0.6 * baseline, + "tracking notch must cut motor thrash substantially: \ + with {with_notch:.4} vs bypass {baseline:.4}" + ); + } + /// 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. diff --git a/crates/relay-notch/Cargo.toml b/crates/relay-notch/Cargo.toml new file mode 100644 index 0000000..44d9f16 --- /dev/null +++ b/crates/relay-notch/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "relay-notch" +description = "Relay Notch — RPM-tracked harmonic notch filtering for the rate-loop gyro path (PX4 IMU_GYRO_DNF / ArduPilot INS_HNTCH / Betaflight RPM-filter parity). Per-motor biquad notches at the rotation fundamental + 2nd harmonic, center frequencies tracking achieved eRPM; clean unity bypass when RPM telemetry is absent (no filtering is safer than a mistracked notch); Nyquist-clamped per notch. no_std/no_alloc/forbid(unsafe)." +version.workspace = true +edition.workspace = true +license.workspace = true +repository.workspace = true +rust-version.workspace = true + +[lib] +path = "plain/src/lib.rs" +crate-type = ["rlib"] + +[dependencies] +relay-math = { path = "../relay-math" } + +[dev-dependencies] +proptest.workspace = true + +[lints] +workspace = true diff --git a/crates/relay-notch/plain/proptest-regressions/lib.txt b/crates/relay-notch/plain/proptest-regressions/lib.txt new file mode 100644 index 0000000..6695dda --- /dev/null +++ b/crates/relay-notch/plain/proptest-regressions/lib.txt @@ -0,0 +1,7 @@ +# Seeds for failure cases proptest has generated in the past. It is +# automatically read and these particular cases re-run before any +# novel cases are generated. +# +# It is recommended to check this file in to source control so that +# everyone who runs the test benefits from these saved cases. +cc bf5228648715f7572e4eedb34ba1a89e48d05b634d7009f2fe9416a6b4648fec # shrinks to xs = [2139095040], rpm = [6751, 0, 6751, 6751] diff --git a/crates/relay-notch/plain/src/kani_proofs.rs b/crates/relay-notch/plain/src/kani_proofs.rs new file mode 100644 index 0000000..4d0801f --- /dev/null +++ b/crates/relay-notch/plain/src/kani_proofs.rs @@ -0,0 +1,72 @@ +//! Kani harnesses for relay-notch (plain-only sibling). +//! +//! The verification split (the relay-rc pattern, plus a libm lesson): the +//! trig in `set()` is CBMC-intractable on symbolic arguments (libm's +//! rem_pio2_large unwinds >1500 iterations), so the harnesses keep trig +//! CONCRETE and prove the trig-free properties over all inputs: +//! totality of `apply` for any input AND any (poisoned) state, the +//! never-engage-outside-band safety direction of `set`, and the bank's +//! bit-exact bypass. The engages-in-band liveness direction and the +//! frequency-domain depth/phase are test-measured. +#![cfg(kani)] + +use crate::*; + +/// NOTCH-K01 — `apply` is TOTAL for ANY input bit pattern and ANY internal +/// state (all four state words nondet, coefficients from a concrete +/// engaged tuning): the output is always finite. +#[kani::proof] +fn verify_apply_total_any_state() { + let mut n = Notch::default(); + n.set(70.0, 1000.0); // concrete trig — engaged hover-band notch + assert!(n.is_engaged()); + n.poison_state([ + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + ]); + let x = f32::from_bits(kani::any()); + let y = n.apply(x); + assert!(y.is_finite()); +} + +/// NOTCH-K02 — the band gate over ALL inputs, trig-free by construction +/// (`band_ok` is comparisons only; `set` engages exactly when it holds — +/// the one-line linkage is unit-pinned in set_engagement_matches_band_ok): +/// any non-finite / below-floor / above-Nyquist / fs ≤ 0 pair is rejected, +/// and a DISENGAGED notch's `apply` is bit-exact unity for any input. +#[kani::proof] +fn verify_band_gate_and_disengaged_unity() { + let f0: f32 = kani::any(); + let fs: f32 = kani::any(); + if band_ok(f0, fs) { + assert!(f0.is_finite() && fs.is_finite()); + assert!(fs > 0.0); + assert!(f0 >= MIN_F0_HZ); + assert!(f0 <= MAX_F0_FRAC * fs); + } + let mut n = Notch::default(); + let x = f32::from_bits(kani::any()); + let y = n.apply(x); + assert!(y.to_bits() == x.to_bits(), "disengaged is bit-exact unity"); +} + +/// NOTCH-K03 — the bank's RPM-absent bypass is bit-exact for ANY input +/// (concrete prior tuning; the None transition disengages everything). +#[kani::proof] +fn verify_bank_bypass_bit_exact() { + let mut bank: HarmonicNotchBank<4> = HarmonicNotchBank::new(250.0); + bank.update_rpm(Some([4000, 4100, 3950, 4050])); // concrete trig + bank.update_rpm(None); + assert!(bank.is_bypassed()); + let x = [ + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + f32::from_bits(kani::any()), + ]; + let y = bank.apply(x); + assert!(y[0].to_bits() == x[0].to_bits()); + assert!(y[1].to_bits() == x[1].to_bits()); + assert!(y[2].to_bits() == x[2].to_bits()); +} diff --git a/crates/relay-notch/plain/src/lib.rs b/crates/relay-notch/plain/src/lib.rs new file mode 100644 index 0000000..e190d8e --- /dev/null +++ b/crates/relay-notch/plain/src/lib.rs @@ -0,0 +1,438 @@ +//! relay-notch — RPM-tracked harmonic notch filtering (NOTCH-P01, v1.122). +//! +//! Per-motor rotational vibration is the dominant gyro contaminant on a +//! multirotor, and it is NARROWBAND at the rotation frequency and its +//! harmonics — exactly what a wide low-pass cannot reject without eating +//! the phase margin of the loop it protects. This engine is the +//! PX4 `IMU_GYRO_DNF` / ArduPilot `INS_HNTCH` / Betaflight RPM-filter +//! role: biquad notches per motor at the fundamental + 2nd harmonic, +//! center frequencies tracking ACHIEVED RPM (bidir-DShot eRPM primary, +//! per jess DD-024 — the same `read_motor_rpm` seam the rotor-out FDI +//! consumes). +//! +//! Safety posture: +//! - **Absent RPM ⇒ unity bypass, bit-exact** — no filtering is safer +//! than a mistracked notch. States reset so re-engagement is clean. +//! - **Nyquist clamp per notch** — a harmonic above 0.45·fs bypasses +//! individually (at the 250 Hz flight loop the 2nd harmonic leaves the +//! band above ~4200 RPM; it re-engages when RPM falls back). +//! - **Total** (Kani NOTCH-K01/K02): any input — including NaN/∞ samples +//! or a poisoned internal state — yields a finite output; non-finite +//! arithmetic results reset the offending biquad and pass the (sanitized) +//! input through. +//! +//! Design: RBJ cookbook notch (b = [1, −2cosω₀, 1], a = [1+α, −2cosω₀, +//! 1−α], α = sinω₀/2Q), Direct Form 1 so state has bounded physical +//! meaning. Q = 5 per notch: deep at center (> 20 dB over the tracking +//! band), negligible phase at the rate-loop crossover (measured < 10° +//! in the verification tests, not asserted from arithmetic). + +#![no_std] +#![forbid(unsafe_code)] + +/// Per-notch quality factor. Higher = narrower/deeper but more sensitive +/// to RPM error; 5 matches the reference autopilots' defaults' ballpark. +pub const NOTCH_Q: f32 = 5.0; +/// A notch only engages when its center is below this fraction of fs +/// (Nyquist margin) … +pub const MAX_F0_FRAC: f32 = 0.45; +/// … and above this floor (idle jitter is not worth a notch). +pub const MIN_F0_HZ: f32 = 10.0; +/// Output hard cap (rad/s) — far beyond any physical body rate; the +/// totality backstop for the filter arithmetic. +pub const OUT_CAP: f32 = 200.0; + +#[inline] +fn sane(x: f32) -> f32 { + if x.is_finite() { + x.clamp(-OUT_CAP, OUT_CAP) + } else { + 0.0 + } +} + +/// One RBJ biquad notch (Direct Form 1). +#[derive(Clone, Copy, Debug, Default)] +pub struct Notch { + // normalized coefficients (b0, b1, b2, a1, a2); unity when disengaged + b0: f32, + b1: f32, + b2: f32, + a1: f32, + a2: f32, + x1: f32, + x2: f32, + y1: f32, + y2: f32, + engaged: bool, +} + +/// The engageable-band gate, TRIG-FREE (comparisons only): `set` engages +/// exactly when this holds. Split out so Kani can prove the gate over all +/// inputs without pulling libm's argument reduction into the call graph +/// (symbolic trig unwinds >1500 iterations in CBMC — the libm lesson). +pub fn band_ok(f0_hz: f32, fs_hz: f32) -> bool { + f0_hz.is_finite() + && fs_hz.is_finite() + && fs_hz > 0.0 + && f0_hz >= MIN_F0_HZ + && f0_hz <= MAX_F0_FRAC * fs_hz +} + +impl Notch { + /// (Re)tune to center `f0_hz` at sample rate `fs_hz`. Outside the + /// engageable band (or with nonsense inputs) the notch DISENGAGES — + /// unity passthrough with cleared state. Engagement ≡ [`band_ok`] + /// (the one-line linkage below; the gate itself is Kani-proven). + pub fn set(&mut self, f0_hz: f32, fs_hz: f32) { + if !band_ok(f0_hz, fs_hz) { + self.disengage(); + return; + } + let w0 = 2.0 * core::f32::consts::PI * f0_hz / fs_hz; + let cw = relay_math::cosf(w0); + let sw = relay_math::sinf(w0); + let alpha = sw / (2.0 * NOTCH_Q); + let a0 = 1.0 + alpha; + self.b0 = 1.0 / a0; + self.b1 = -2.0 * cw / a0; + self.b2 = 1.0 / a0; + self.a1 = -2.0 * cw / a0; + self.a2 = (1.0 - alpha) / a0; + if !self.engaged { + self.x1 = 0.0; + self.x2 = 0.0; + self.y1 = 0.0; + self.y2 = 0.0; + } + self.engaged = true; + } + + /// Unity passthrough with cleared state. + pub fn disengage(&mut self) { + *self = Notch::default(); + } + + pub fn is_engaged(&self) -> bool { + self.engaged + } + + /// Kani-only state poisoning: totality must hold for ANY internal + /// state, not just states reachable through sane arithmetic. + #[cfg(kani)] + pub(crate) fn poison_state(&mut self, st: [f32; 4]) { + self.x1 = st[0]; + self.x2 = st[1]; + self.y1 = st[2]; + self.y2 = st[3]; + } + + /// One sample. Total: the output is always finite (a non-finite + /// arithmetic result resets this biquad and passes the sanitized + /// input through). + pub fn apply(&mut self, x: f32) -> f32 { + if !self.engaged { + return x; // bit-exact bypass + } + let xs = sane(x); + // Sanitize the OPERANDS, not just the result: with every state word + // and coefficient finite and bounded, no intermediate can produce a + // NaN/∞ at all (Kani proves the arithmetic side-conditions, not + // only the final assert), and a poisoned state heals in one sample + // instead of glitching through a reset. + let (x1, x2, y1, y2) = (sane(self.x1), sane(self.x2), sane(self.y1), sane(self.y2)); + let y = self.b0 * xs + self.b1 * x1 + self.b2 * x2 - self.a1 * y1 - self.a2 * y2; + let y = sane(y); + self.x1 = xs; + self.x2 = x1; + self.y1 = y; + self.y2 = y1; + y + } +} + +/// The gyro-path bank: `M` motors × 2 harmonics × 3 axes. +pub struct HarmonicNotchBank { + /// notches[axis][motor][harmonic] + notches: [[[Notch; 2]; M]; 3], + fs_hz: f32, + bypassed: bool, +} + +impl HarmonicNotchBank { + pub fn new(fs_hz: f32) -> Self { + HarmonicNotchBank { + notches: [[[Notch::default(); 2]; M]; 3], + fs_hz, + bypassed: true, + } + } + + /// Track the achieved per-motor RPM for this cycle. `None` (no RPM + /// telemetry) ⇒ the WHOLE bank bypasses cleanly until it returns. + pub fn update_rpm(&mut self, rpm: Option<[i32; M]>) { + match rpm { + None => { + if !self.bypassed { + for ax in self.notches.iter_mut() { + for m in ax.iter_mut() { + m[0].disengage(); + m[1].disengage(); + } + } + self.bypassed = true; + } + } + Some(r) => { + self.bypassed = false; + for ax in 0..3 { + for (m, rm) in r.iter().enumerate().take(M) { + let f0 = (*rm).max(0) as f32 / 60.0; // RPM → Hz + self.notches[ax][m][0].set(f0, self.fs_hz); + self.notches[ax][m][1].set(2.0 * f0, self.fs_hz); + } + } + } + } + } + + /// Whole-bank bypass state (RPM telemetry absent). + pub fn is_bypassed(&self) -> bool { + self.bypassed + } + + /// Filter one gyro sample (rad/s, body axes). Bit-exact passthrough + /// while bypassed; finite for ANY input otherwise. + pub fn apply(&mut self, gyro: [f32; 3]) -> [f32; 3] { + if self.bypassed { + return gyro; + } + let mut out = gyro; + for (ax, o) in out.iter_mut().enumerate() { + // sanitize at the bank boundary: with RPM telemetry present we + // own the path, and a non-finite gyro sample must not ride + // through disengaged (Nyquist-clamped) notches untouched. + let mut v = sane(*o); + for m in 0..M { + v = self.notches[ax][m][0].apply(v); + v = self.notches[ax][m][1].apply(v); + } + *o = v; + } + out + } +} + +#[cfg(kani)] +mod kani_proofs; + +#[cfg(test)] +mod tests { + extern crate std; + use super::*; + use std::vec::Vec; + + /// Drive a tuned notch with a unit sinusoid at `f_hz`, return + /// (attenuation_db, phase_deg) measured by DFT at the drive frequency + /// after settling — measured, not asserted from arithmetic. + fn measure(notch_f0: f32, drive_f: f32, fs: f32) -> (f32, f32) { + let mut n = Notch::default(); + n.set(notch_f0, fs); + assert!(n.is_engaged()); + let settle = (10.0 * fs / notch_f0) as usize; + let periods = 50usize; + let nsamp = (periods as f32 * fs / drive_f) as usize; + let mut acc_i = 0.0f64; + let mut acc_q = 0.0f64; + for k in 0..(settle + nsamp) { + let t = k as f32 / fs; + let ph = 2.0 * core::f32::consts::PI * drive_f * t; + let x = relay_math::sinf(ph); + let y = n.apply(x); + if k >= settle { + acc_i += (y * relay_math::sinf(ph)) as f64; + acc_q += (y * relay_math::cosf(ph)) as f64; + } + } + let scale = 2.0 / nsamp as f64; + let (i, q) = (acc_i * scale, acc_q * scale); + let mag = (i * i + q * q).sqrt() as f32; + let att_db = -20.0 * (mag.max(1e-9)).log10(); + let phase_deg = (q.atan2(i) as f32).to_degrees(); + (att_db, phase_deg) + } + + /// ≥ 20 dB at the tracked fundamental across the hover-to-full band + /// (fs = 1000 Hz, the target hardware gyro path rate). + #[test] + fn twenty_db_at_fundamental_across_band() { + let fs = 1000.0; + for rpm in [3000.0f32, 4000.0, 5000.0, 6000.0, 7000.0, 8000.0] { + let f0 = rpm / 60.0; + let (att, _) = measure(f0, f0, fs); + assert!(att >= 20.0, "{rpm} RPM ({f0} Hz): {att:.1} dB < 20 dB"); + } + } + + /// Stepped fine sweep: tracked attenuation stays within 3 dB of the + /// band's nominal (the swept-RPM criterion, measured at 20 points). + #[test] + fn swept_rpm_attenuation_within_3db() { + let fs = 1000.0; + let mut atts = Vec::new(); + for k in 0..20 { + let rpm = 3000.0 + 250.0 * k as f32; // 3000..7750 + let f0 = rpm / 60.0; + let (att, _) = measure(f0, f0, fs); + atts.push(att); + } + let min = atts.iter().cloned().fold(f32::MAX, f32::min); + let max = atts.iter().cloned().fold(f32::MIN, f32::max); + // deep-notch DFT floors can differ hugely in dB; the criterion is + // about the WORST point staying within 3 dB of nominal (20 dB). + assert!(min >= 20.0 - 3.0, "sweep worst point {min:.1} dB (max {max:.1})"); + } + + /// Added phase lag at the rate-loop crossover (≈ 5 Hz for the ADRC + /// band) stays under 10° with the FULL falcon bank engaged at hover + /// RPM — the filter must not destabilize the loop it protects. + #[test] + fn phase_at_crossover_under_10_degrees() { + let fs = 1000.0; + let fc = 5.0; + // full bank: 4 motors near hover (slightly split), both harmonics. + let mut bank: HarmonicNotchBank<4> = HarmonicNotchBank::new(fs); + bank.update_rpm(Some([4000, 4100, 3950, 4050])); + let settle = 2000usize; + let periods = 40usize; + let nsamp = (periods as f32 * fs / fc) as usize; + let mut acc_i = 0.0f64; + let mut acc_q = 0.0f64; + for k in 0..(settle + nsamp) { + let t = k as f32 / fs; + let ph = 2.0 * core::f32::consts::PI * fc * t; + let x = relay_math::sinf(ph); + let y = bank.apply([x, 0.0, 0.0])[0]; + if k >= settle { + acc_i += (y * relay_math::sinf(ph)) as f64; + acc_q += (y * relay_math::cosf(ph)) as f64; + } + } + let phase = ((acc_q).atan2(acc_i) as f32).to_degrees().abs(); + assert!(phase < 10.0, "bank phase at {fc} Hz crossover: {phase:.2}°"); + } + + /// Absent RPM telemetry ⇒ BIT-EXACT unity passthrough. + #[test] + fn bypass_is_bit_exact() { + let mut bank: HarmonicNotchBank<4> = HarmonicNotchBank::new(250.0); + bank.update_rpm(None); + assert!(bank.is_bypassed()); + for k in 0..500 { + let x = [ + relay_math::sinf(k as f32 * 0.37) * 3.0, + f32::from_bits(0x7F80_0000u32.wrapping_sub(k)), // wild bits + -0.0, + ]; + let y = bank.apply(x); + assert_eq!(x[0].to_bits(), y[0].to_bits()); + assert_eq!(x[1].to_bits(), y[1].to_bits()); + assert_eq!(x[2].to_bits(), y[2].to_bits()); + } + // engage, then drop RPM again: bypass returns and is still exact. + bank.update_rpm(Some([4000; 4])); + assert!(!bank.is_bypassed()); + let _ = bank.apply([1.0, 1.0, 1.0]); + bank.update_rpm(None); + let x = [0.123f32, -4.5, 6.7]; + let y = bank.apply(x); + assert_eq!(x[0].to_bits(), y[0].to_bits()); + } + + /// `set` engages EXACTLY per the Kani-proven band_ok gate — the + /// one-line linkage checked over a grid including both boundaries. + #[test] + fn set_engagement_matches_band_ok() { + let mut n = Notch::default(); + for &fs in &[250.0f32, 1000.0] { + for k in 0..200 { + let f0 = k as f32 * 1.5; // 0..300 Hz, crosses both bounds + n.set(f0, fs); + assert_eq!(n.is_engaged(), band_ok(f0, fs), "f0={f0} fs={fs}"); + } + n.set(MIN_F0_HZ, fs); + assert!(n.is_engaged()); + n.set(MAX_F0_FRAC * fs, fs); + assert!(n.is_engaged()); + n.set(f32::NAN, fs); + assert!(!n.is_engaged()); + } + } + + /// Nyquist clamp: at the 250 Hz flight loop a full-throttle 2nd + /// harmonic (267 Hz) must NOT engage; the fundamental (133 Hz) is + /// above 0.45·fs = 112.5 Hz and must not either. At 4000 RPM the + /// fundamental (66.7 Hz) engages and the 2nd (133 Hz) does not. + #[test] + fn nyquist_clamp_per_notch() { + let fs = 250.0; + let mut n = Notch::default(); + n.set(8000.0 / 60.0, fs); // 133 Hz > 112.5 + assert!(!n.is_engaged()); + n.set(4000.0 / 60.0, fs); // 66.7 Hz + assert!(n.is_engaged()); + n.set(2.0 * 4000.0 / 60.0, fs); // 133 Hz again via harmonic path + assert!(!n.is_engaged()); + } + + /// Vibration-rejection at the flight rate: a hover-band rotor line + /// (70 Hz) riding on a 2 Hz body-rate signal, fs = 250 Hz. The bank + /// removes the line and keeps the signal. + #[test] + fn removes_rotor_line_keeps_body_rates() { + let fs = 250.0; + let mut bank: HarmonicNotchBank<4> = HarmonicNotchBank::new(fs); + bank.update_rpm(Some([4200; 4])); // 70 Hz fundamental + let mut in_pow = 0.0f64; + let mut out_pow = 0.0f64; + let mut sig_pow = 0.0f64; + for k in 0..5000 { + let t = k as f32 / fs; + let body = relay_math::sinf(2.0 * core::f32::consts::PI * 2.0 * t); + let vib = 0.8 * relay_math::sinf(2.0 * core::f32::consts::PI * 70.0 * t); + let y = bank.apply([body + vib, 0.0, 0.0])[0]; + if k > 1000 { + in_pow += ((body + vib) * (body + vib)) as f64; + out_pow += ((y - body) * (y - body)) as f64; + sig_pow += (body * body) as f64; + } + } + // residual (output minus true body signal) is far below the + // injected vibration power — and the body signal survived. + assert!(out_pow < 0.05 * in_pow, "residual {out_pow:.1} vs in {in_pow:.1}"); + assert!(sig_pow > 0.0); + } + + mod proptests { + use super::super::*; + use proptest::prelude::*; + + proptest! { + /// Totality: any bit-pattern inputs and any RPM values yield + /// finite outputs while engaged. + #[test] + fn bank_output_always_finite( + xs in proptest::collection::vec(any::(), 1..80), + rpm in proptest::array::uniform4(-20000i32..20000), + ) { + let mut bank: HarmonicNotchBank<4> = HarmonicNotchBank::new(250.0); + bank.update_rpm(Some(rpm)); + for bits in xs { + let x = f32::from_bits(bits); + let y = bank.apply([x, x, x]); + prop_assert!(y.iter().all(|v| v.is_finite())); + } + } + } + } +} diff --git a/crates/relay-preflight/plain/src/kani_proofs.rs b/crates/relay-preflight/plain/src/kani_proofs.rs index d3af3fd..a811052 100644 --- a/crates/relay-preflight/plain/src/kani_proofs.rs +++ b/crates/relay-preflight/plain/src/kani_proofs.rs @@ -50,3 +50,45 @@ fn verify_blocked_reason_is_real() { } } } + +fn any_table() -> CheckTable { + let mut t = CheckTable::new(); + // exercise every combination of declared/undeclared + pass/fail for + // the breadth rows, and pass/fail for the always-required six. + for id in CheckId::ALL { + if kani::any() { + t.set(id, kani::any()); + } + } + t +} + +/// PREFLIGHT-K03 (PREARM-P03) — the table gate is EXACTLY all-required- +/// pass, and a Blocked verdict always names a required, failing row. +#[kani::proof] +fn verify_table_gate_exact() { + let t = any_table(); + match arm_check_table(&t) { + TableVerdict::Allowed => { + for id in CheckId::ALL { + assert!(!t.is_required(id) || t.passed(id)); + } + } + TableVerdict::Blocked(id) => { + assert!(t.is_required(id) && !t.passed(id)); + } + } +} + +/// PREFLIGHT-K04 (PREARM-P03) — MONOTONE: failing any single row of an +/// Allowed table can never remain Allowed (adding a failing check can +/// never ALLOW arming; declaring + failing a new row always blocks). +#[kani::proof] +fn verify_table_gate_monotone() { + let mut t = any_table(); + kani::assume(arm_check_table(&t) == TableVerdict::Allowed); + let which: usize = kani::any(); + kani::assume(which < CHECK_COUNT); + t.set(CheckId::ALL[which], false); + assert!(arm_check_table(&t) != TableVerdict::Allowed); +} diff --git a/crates/relay-preflight/plain/src/lib.rs b/crates/relay-preflight/plain/src/lib.rs index 26169bb..4e34407 100644 --- a/crates/relay-preflight/plain/src/lib.rs +++ b/crates/relay-preflight/plain/src/lib.rs @@ -93,11 +93,170 @@ pub fn arm_check(c: PreflightChecks) -> ArmVerdict { } } +/// The v1.122 check TABLE (PREARM-P03): the pre-arm breadth as DATA, not +/// scattered conditionals — coverage is enumerable (`CheckId::ALL`) and +/// testable row by row. The legacy six checks are rows 0..=5; the gate +/// blocks on the first failing REQUIRED row in table order. Rows a given +/// integration cannot feed yet stay optional (not required) until it +/// declares them via [`CheckTable::set`] — a bench SITL without an RC +/// receiver must not be hard-blocked by a link check it cannot satisfy, +/// but once declared, a failing row ALWAYS blocks (monotone, Kani-proven). +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[repr(usize)] +pub enum CheckId { + // — the legacy six (always required) — + SensorsHealthy = 0, + EstimatorConverged = 1, + CalibrationPresent = 2, + GeofenceLoaded = 3, + BatteryOk = 4, + FailsafeConfigured = 5, + // — estimator / navigation integrity — + EstimatorInnovation = 6, + GnssAgreement = 7, + // — configuration consistency — + ParamsFromNvm = 8, + GeofenceSane = 9, + ThresholdsOrdered = 10, + // — hardware consistency — + SensorsFresh = 11, + EscTelemetry = 12, + BatteryTakeoffMargin = 13, + RangefinderPlausible = 14, + // — calibration / safety state — + CalibrationFresh = 15, + RcLink = 16, + GcsLink = 17, + NoFailsafeLatched = 18, +} + +/// Number of table rows. +pub const CHECK_COUNT: usize = 19; + +impl CheckId { + /// Every row, in gate (priority) order — coverage equals this length + /// by construction. + pub const ALL: [CheckId; CHECK_COUNT] = [ + CheckId::SensorsHealthy, + CheckId::EstimatorConverged, + CheckId::CalibrationPresent, + CheckId::GeofenceLoaded, + CheckId::BatteryOk, + CheckId::FailsafeConfigured, + CheckId::EstimatorInnovation, + CheckId::GnssAgreement, + CheckId::ParamsFromNvm, + CheckId::GeofenceSane, + CheckId::ThresholdsOrdered, + CheckId::SensorsFresh, + CheckId::EscTelemetry, + CheckId::BatteryTakeoffMargin, + CheckId::RangefinderPlausible, + CheckId::CalibrationFresh, + CheckId::RcLink, + CheckId::GcsLink, + CheckId::NoFailsafeLatched, + ]; + + /// Distinct operator-readable reason, sized for a STATUSTEXT payload + /// (MAVLINK-P06 carries these to the GCS on a blocked arm). + pub fn reason_text(self) -> &'static str { + match self { + CheckId::SensorsHealthy => "PREARM: sensors unhealthy", + CheckId::EstimatorConverged => "PREARM: estimator not converged", + CheckId::CalibrationPresent => "PREARM: no calibration", + CheckId::GeofenceLoaded => "PREARM: no geofence", + CheckId::BatteryOk => "PREARM: battery low/critical", + CheckId::FailsafeConfigured => "PREARM: failsafe unconfigured", + CheckId::EstimatorInnovation => "PREARM: innovation out of band", + CheckId::GnssAgreement => "PREARM: GNSS receivers disagree", + CheckId::ParamsFromNvm => "PREARM: params are defaults (no NVM)", + CheckId::GeofenceSane => "PREARM: geofence insane", + CheckId::ThresholdsOrdered => "PREARM: failsafe thresholds unordered", + CheckId::SensorsFresh => "PREARM: sensor data stale", + CheckId::EscTelemetry => "PREARM: ESC telemetry missing", + CheckId::BatteryTakeoffMargin => "PREARM: battery below takeoff margin", + CheckId::RangefinderPlausible => "PREARM: rangefinder implausible", + CheckId::CalibrationFresh => "PREARM: calibration stale", + CheckId::RcLink => "PREARM: RC link down", + CheckId::GcsLink => "PREARM: GCS link down", + CheckId::NoFailsafeLatched => "PREARM: failsafe latched", + } + } +} + +/// The table: per-row (required, passed). The legacy six are ALWAYS +/// required; the breadth rows become required when the integration first +/// sets them (declaring "this vehicle has this signal"). +#[derive(Clone, Copy, Debug)] +pub struct CheckTable { + required: [bool; CHECK_COUNT], + passed: [bool; CHECK_COUNT], +} + +impl Default for CheckTable { + fn default() -> Self { + Self::new() + } +} + +impl CheckTable { + /// Legacy six required (and failing — a fresh table must not arm); + /// breadth rows undeclared. + pub fn new() -> Self { + let mut required = [false; CHECK_COUNT]; + for r in required.iter_mut().take(6) { + *r = true; + } + CheckTable { required, passed: [false; CHECK_COUNT] } + } + + /// Set a row's pass state. Setting ANY row marks it required — an + /// integration that reports a signal is thereafter gated on it + /// (declaring is a one-way door; see PREFLIGHT-K04 monotonicity). + pub fn set(&mut self, id: CheckId, pass: bool) { + let i = id as usize; + self.required[i] = true; + self.passed[i] = pass; + } + + pub fn is_required(&self, id: CheckId) -> bool { + self.required[id as usize] + } + + pub fn passed(&self, id: CheckId) -> bool { + self.passed[id as usize] + } +} + +/// The table verdict. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum TableVerdict { + Allowed, + /// Blocked by the FIRST failing required row (gate order); carry its + /// id — `reason_text` turns it into the operator STATUSTEXT. + Blocked(CheckId), +} + +/// The table gate: `Allowed` iff every REQUIRED row passes; otherwise the +/// first failing required row in table order. Total; monotone (proven: +/// PREFLIGHT-K03/K04). +pub fn arm_check_table(t: &CheckTable) -> TableVerdict { + for id in CheckId::ALL { + if t.is_required(id) && !t.passed(id) { + return TableVerdict::Blocked(id); + } + } + TableVerdict::Allowed +} + #[cfg(kani)] mod kani_proofs; #[cfg(test)] mod tests { + extern crate std; + use super::*; fn all_ok() -> PreflightChecks { @@ -131,6 +290,55 @@ mod tests { assert_eq!(arm_check(c), ArmVerdict::Blocked(CheckFail::Sensors)); } + /// PREARM-P03 per-row pair tests, TABLE-DRIVEN: for every row, (a) an + /// all-pass table with that row failed blocks with EXACTLY that id and + /// a distinct reason text; (b) clearing it re-allows. Coverage equals + /// the table length by construction — a row added to CheckId::ALL is + /// automatically covered. + #[test] + fn every_row_blocks_alone_and_clears() { + use std::collections::HashSet; + let mut texts = HashSet::new(); + for id in CheckId::ALL { + let mut t = CheckTable::new(); + for other in CheckId::ALL { + t.set(other, true); + } + assert_eq!(arm_check_table(&t), TableVerdict::Allowed); + t.set(id, false); + assert_eq!( + arm_check_table(&t), + TableVerdict::Blocked(id), + "row {id:?} must block with its own id" + ); + assert!( + texts.insert(id.reason_text()), + "reason text for {id:?} must be DISTINCT" + ); + t.set(id, true); + assert_eq!(arm_check_table(&t), TableVerdict::Allowed, "{id:?} clears"); + } + assert_eq!(texts.len(), CHECK_COUNT); + } + + /// Undeclared breadth rows do not gate (a bench without RC must not be + /// hard-blocked by a link check it cannot satisfy) — but the legacy six + /// are ALWAYS required. + #[test] + fn undeclared_rows_do_not_gate_legacy_always_do() { + let mut t = CheckTable::new(); + for id in CheckId::ALL.iter().take(6) { + t.set(*id, true); + } + assert_eq!(arm_check_table(&t), TableVerdict::Allowed, "six pass, rest undeclared"); + let fresh = CheckTable::new(); + assert_eq!( + arm_check_table(&fresh), + TableVerdict::Blocked(CheckId::SensorsHealthy), + "a fresh table must not arm" + ); + } + #[test] fn default_is_all_failing_blocked() { // a fresh checks struct (all false) must NOT arm.