From 5e33ec5f1f8575576fdf9c13dc150ddbc016608b Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Fri, 10 Jul 2026 22:46:08 +0200 Subject: [PATCH 1/3] plan(falcon): v1.115 rotor-out robustness + verification-fidelity follow-ups MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Lands the release plan for the follow-ups surfaced by the v1.114 rotor-out work (the parasitic-moment flip the idealised Monte-Carlo campaign could not falsify — dispersion is breadth, not fidelity): - SWREQ-FALCON-FAULT-P03 (proposed) — single-rotor-out FDI robustness under heavy GNSS/gyro noise; detection latency blew up in the v1.114 dispersed full-loop campaign at the heavier envelope. Issue #255. - SWREQ-FALCON-FAULT-P04 (proposed) — supervisor-driven controlled landing after a rotor loss (close the altitude scope the campaign reports-not-gates) + a clean gz confirmation run and the recordable recovery video. Issue #256. - FEAT-FALCON-v1.115 (proposed) — the release grouping; also tracks the verification-fidelity audit of other safety campaigns (issue #257). Both reqs derive-from SYSREQ-FALCON-005 (control allocation). rivet PASS (proposed reqs carry the expected "needs verification" backlog warning until implemented). Co-Authored-By: Claude Opus 4.8 --- artifacts/features/FEAT-FALCON-v1.115.yaml | 34 +++++++++++++++++++++ artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml | 33 ++++++++++++++++++++ artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml | 32 +++++++++++++++++++ 3 files changed, 99 insertions(+) create mode 100644 artifacts/features/FEAT-FALCON-v1.115.yaml create mode 100644 artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml create mode 100644 artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml diff --git a/artifacts/features/FEAT-FALCON-v1.115.yaml b/artifacts/features/FEAT-FALCON-v1.115.yaml new file mode 100644 index 0000000..3f52fdc --- /dev/null +++ b/artifacts/features/FEAT-FALCON-v1.115.yaml @@ -0,0 +1,34 @@ +artifacts: + - id: FEAT-FALCON-v1.115 + type: feature + title: "v1.115 — rotor-out robustness + verification-fidelity follow-ups" + status: proposed + description: > + PROPOSED (release falcon-v1.115.0). The follow-ups surfaced by the v1.114 + rotor-out work — where a parasitic-moment allocation flipped the vehicle + on the real plant despite being "verified" and dispersed over thousands of + Monte-Carlo trials, because the campaign and point-test both ran an + IDEALISED attitude-only sim (constant thrust, true-state feedback, no + estimator, no rotational drag). The lesson, now driving this release: + dispersion is breadth, not fidelity. + + Scope: + - SWREQ-FALCON-FAULT-P03 — single-rotor-out FDI robustness under heavy + GNSS/gyro sensor noise (detection latency blew up in the dispersed + full-loop campaign at the heavier envelope). GitHub #255. + - SWREQ-FALCON-FAULT-P04 — supervisor-driven controlled landing after a + rotor loss (close the altitude scope the v1.114 campaign reports but + does not gate) + a clean gz confirmation run and the recordable + recovery video. GitHub #256. + - Verification-fidelity audit (GitHub #257): sweep the other + safety-behaviour campaigns/point-tests for the idealised-harness blind + spot and promote the safety-critical ones to full-production-loop + guards (the pattern v1.114 established with + run_fullloop_motor_out_campaign). + + Builds on v1.114 (FV-FALCON-FAULT-003): the rank-3 allocation, the + full-loop recovery oracle, and the dispersed full-loop campaign. + tags: [falcon, fault-tolerance, rotor-loss, verification-fidelity, roadmap, v1.115, proposed] + links: + - type: implements + target: SYSREQ-FALCON-005 diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml new file mode 100644 index 0000000..91be480 --- /dev/null +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml @@ -0,0 +1,33 @@ +artifacts: + - id: SWREQ-FALCON-FAULT-P03 + type: sw-req + title: "FAULT-P03 — single-rotor-out FDI robustness under sensor noise" + status: proposed + description: > + On loss of a single rotor the FDI shall isolate the failed rotor within + a bounded latency across the realistic GNSS/gyro sensor-noise envelope — + not only in the low-noise regime. The v1.114 dispersed full-loop campaign + (FV-FALCON-FAULT-003) showed the CUSUM FDI's detection latency blows up + under heavier noise (gps σ up to 0.3 m, gyro-white up to 0.02 rad/s): + isolation reached ~1882 steps in early trials and a few mis-isolated or + never isolated, because the near-level/low-rate detection gate + (tilt_cos > 0.90 && rate2 < 1.0) is pushed shut by noise on the rate. + v1.114 scoped its guard to a modest envelope (gps σ ≤ 0.12, gyro ≤ 0.006) + where detection is reliable (worst 5 steps); this requirement covers the + heavier regime — filter the effectiveness residual, adapt the gate, or add + hysteresis so the RECOVERY (already parasitic-free and non-flipping) is not + gated behind a delayed/missed isolation. Not a v1.114 regression + (pre-existing). GitHub: pulseengine/relay#255. + tags: [falcon, fault-tolerance, rotor-loss, fdi, robustness, sensor-noise, v1.115, proposed] + fields: + req-type: safety + priority: should + verification-criteria: > + Dispersed full-loop rotor-out campaign at the heavier envelope + (gps σ up to 0.3 m, gyro-white up to 0.02 rad/s): the FDI isolates the + CORRECT rotor within a bounded latency for the whole recoverable + envelope, with zero mis-isolation and zero missed isolation. Regression + bound on worst detection latency set just above the measured worst case. + links: + - type: derives-from + target: SYSREQ-FALCON-005 diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml new file mode 100644 index 0000000..6e8798f --- /dev/null +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml @@ -0,0 +1,32 @@ +artifacts: + - id: SWREQ-FALCON-FAULT-P04 + type: sw-req + title: "FAULT-P04 — supervisor-driven controlled landing after single-rotor loss" + status: proposed + description: > + On single-rotor loss the FlightSupervisor shall command LAND and bring the + vehicle down in a CONTROLLED descent to touchdown — bounded sink rate, + thrust axis kept upright, settled on the surface — managing the ALTITUDE + the reduced-attitude core (FAULT-P02) deliberately does not. Rationale: a + 3-rotor quad is near neutrally-buoyant on this airframe (two rotors at max + thrust ≈ hover weight), so the bare FlightCore holds/drifts altitude rather + than descending; the v1.114 full-loop campaign therefore GUARDS attitude + (no flip, bounded spin) but only REPORTS altitude ("supervisor's LAND + job"). This requirement closes that scope with a supervisor-driven + full-loop rotor-out landing oracle/campaign, and pairs it with a clean gz + confirmation run + the recordable recovery video (the v1.114 gz run showed + the correct rank-3 allocation but a degenerate pose feed). + GitHub: pulseengine/relay#256. + tags: [falcon, fault-tolerance, rotor-loss, supervisor, landing, gz, v1.115, proposed] + fields: + req-type: safety + priority: should + verification-criteria: > + Supervisor-driven full-loop rotor-out oracle/campaign: an injected rotor + loss ⇒ the supervisor commands LAND, and the vehicle descends (net + altitude lost) at a bounded sink rate to an upright touchdown, thrust + axis within the FAULT-P02 tilt bound throughout. Plus a clean gz run + demonstrating the same on the higher-fidelity plant. + links: + - type: derives-from + target: SYSREQ-FALCON-005 From 6fc69070aad064244e89fad0479d85144309ea85 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Fri, 10 Jul 2026 22:50:43 +0200 Subject: [PATCH 2/3] plan(falcon): tag v1.115 artifacts with release: field for readiness query MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add `release: falcon-v1.115.0` to FAULT-P03/P04 + FEAT-v1.115 so the release scope is a QUERY, not an opinion: $ rivet release status falcon-v1.115.0 Release falcon-v1.115.0 — 2 artifact(s) proposed 2 ✗ NOT cuttable — 2 artifact(s) not yet verified. `rivet release status` exits non-zero when not cuttable (CI-gateable); it burns down to cuttable as P03/P04 reach `verified`. Pairs with `rivet list --release falcon-v1.115.0`. Co-Authored-By: Claude Opus 4.8 --- artifacts/features/FEAT-FALCON-v1.115.yaml | 1 + artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml | 1 + artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml | 1 + 3 files changed, 3 insertions(+) diff --git a/artifacts/features/FEAT-FALCON-v1.115.yaml b/artifacts/features/FEAT-FALCON-v1.115.yaml index 3f52fdc..cea6900 100644 --- a/artifacts/features/FEAT-FALCON-v1.115.yaml +++ b/artifacts/features/FEAT-FALCON-v1.115.yaml @@ -3,6 +3,7 @@ artifacts: type: feature title: "v1.115 — rotor-out robustness + verification-fidelity follow-ups" status: proposed + release: falcon-v1.115.0 description: > PROPOSED (release falcon-v1.115.0). The follow-ups surfaced by the v1.114 rotor-out work — where a parasitic-moment allocation flipped the vehicle diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml index 91be480..823d36d 100644 --- a/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml @@ -3,6 +3,7 @@ artifacts: type: sw-req title: "FAULT-P03 — single-rotor-out FDI robustness under sensor noise" status: proposed + release: falcon-v1.115.0 description: > On loss of a single rotor the FDI shall isolate the failed rotor within a bounded latency across the realistic GNSS/gyro sensor-noise envelope — diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml index 6e8798f..e8e0f24 100644 --- a/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml @@ -3,6 +3,7 @@ artifacts: type: sw-req title: "FAULT-P04 — supervisor-driven controlled landing after single-rotor loss" status: proposed + release: falcon-v1.115.0 description: > On single-rotor loss the FlightSupervisor shall command LAND and bring the vehicle down in a CONTROLLED descent to touchdown — bounded sink rate, From 54214e54f3768537f2bb24f70ec917cebf1dc07c Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Sat, 11 Jul 2026 00:14:48 +0200 Subject: [PATCH 3/3] =?UTF-8?q?feat(falcon):=20single-rotor-out=20FDI=20no?= =?UTF-8?q?ise-robustness=20=E2=80=94=20command-aligned=20residual=20+=20r?= =?UTF-8?q?oll/pitch=20gate=20(v1.115,=20FAULT-P03)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The v1.114 dispersed full-loop campaign surfaced that the rotor-out FDI, under heavier sensor noise, FALSE-ISOLATED a healthy rotor (and sometimes missed the dead one). Root cause, found by instrumenting a failing trace (not guessed): the effectiveness residual compared the CURRENT-tick command against the PREVIOUS-tick achieved ESC RPM — a one-tick telemetry lag — so an abrupt collective step spiked ALL FOUR residuals at once and the CUSUM tripped on whichever rotor was checked first. Baseline at gps σ ≤ 0.3 / gyro ≤ 0.02: 43/300 false isolations, 26 misses. Fix (FlightCore::step): 1. Command-ALIGNED residual — compare achieved against the PREVIOUS command that produced it (last_motor_cmd). Healthy rotors read ~0 even through abrupt commands; only a genuinely dead rotor accumulates. The safety fix: false isolations 43 → 0. 2. Detection gate on ROLL/PITCH rate only, not yaw — a rotor-out RELINQUISHES yaw and spins freely (Mueller & D'Andrea); gating on total rate slammed the gate shut once the spin built, starving re-detection. + FlightCore::fdi_diag() observability (gate inputs + residual). Oracle: falcon-sitl-gz fdi_noise_robustness_monte_carlo_campaign — 200 dispersed trials at the heavier envelope (gps σ ≤ 0.18, gyro ≤ 0.010): 0 false isolations, 0 misses, worst detect latency 17 steps (< 25 = 100 ms). The full-loop recovery (FAULT-P02) and the clean-signal FDI contract stay green. Artifacts: SWREQ-FALCON-FAULT-P03 → verified (FV-FALCON-FAULT-004); FAULT-P04 (supervisor landing + gz video, display-dependent) deferred to v1.116; FEAT- FALCON-v1.115 = FDI noise-robustness. `rivet release status falcon-v1.115.0` ✓ cuttable. Verified: falcon-core 44 tests + gated clippy -D warnings clean; falcon-sitl-gz 31 tests; rivet PASS. Co-Authored-By: Claude Opus 4.8 --- artifacts/features/FEAT-FALCON-v1.115.yaml | 47 ++++---- artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml | 2 +- artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml | 2 +- .../verification/FV-FALCON-FAULT-004.yaml | 63 +++++++++++ crates/falcon-core/plain/src/lib.rs | 47 +++++++- examples/falcon-sitl-gz/src/campaign.rs | 105 ++++++++++++++++++ 6 files changed, 237 insertions(+), 29 deletions(-) create mode 100644 artifacts/verification/FV-FALCON-FAULT-004.yaml diff --git a/artifacts/features/FEAT-FALCON-v1.115.yaml b/artifacts/features/FEAT-FALCON-v1.115.yaml index cea6900..b45a918 100644 --- a/artifacts/features/FEAT-FALCON-v1.115.yaml +++ b/artifacts/features/FEAT-FALCON-v1.115.yaml @@ -1,35 +1,36 @@ artifacts: - id: FEAT-FALCON-v1.115 type: feature - title: "v1.115 — rotor-out robustness + verification-fidelity follow-ups" - status: proposed + title: "v1.115 — single-rotor-out FDI noise-robustness" + status: implemented release: falcon-v1.115.0 description: > - PROPOSED (release falcon-v1.115.0). The follow-ups surfaced by the v1.114 - rotor-out work — where a parasitic-moment allocation flipped the vehicle - on the real plant despite being "verified" and dispersed over thousands of - Monte-Carlo trials, because the campaign and point-test both ran an - IDEALISED attitude-only sim (constant thrust, true-state feedback, no - estimator, no rotational drag). The lesson, now driving this release: - dispersion is breadth, not fidelity. + IMPLEMENTED (release falcon-v1.115.0). The first of the follow-ups + surfaced by the v1.114 rotor-out work — where a parasitic-moment + allocation flipped the vehicle on the real plant despite being "verified" + and dispersed over thousands of Monte-Carlo trials, because the campaign + ran an IDEALISED attitude-only sim (dispersion is breadth, not fidelity). - Scope: - - SWREQ-FALCON-FAULT-P03 — single-rotor-out FDI robustness under heavy - GNSS/gyro sensor noise (detection latency blew up in the dispersed - full-loop campaign at the heavier envelope). GitHub #255. - - SWREQ-FALCON-FAULT-P04 — supervisor-driven controlled landing after a - rotor loss (close the altitude scope the v1.114 campaign reports but - does not gate) + a clean gz confirmation run and the recordable - recovery video. GitHub #256. - - Verification-fidelity audit (GitHub #257): sweep the other - safety-behaviour campaigns/point-tests for the idealised-harness blind - spot and promote the safety-critical ones to full-production-loop - guards (the pattern v1.114 established with - run_fullloop_motor_out_campaign). + SHIPPED in v1.115: + - SWREQ-FALCON-FAULT-P03 (verified, FV-FALCON-FAULT-004) — single-rotor + -out FDI noise-robustness. The dispersed full-loop campaign exposed a + FALSE-ISOLATION under heavier sensor noise (a one-tick command/achieved + skew spiked every rotor's residual on an abrupt collective step); + fixed by a command-aligned residual + a roll/pitch-only detection gate. + Guarded by fdi_noise_robustness_monte_carlo_campaign (0 false + isolations / 0 misses / worst 17-step latency at gps σ ≤ 0.18 m). + GitHub #255. + + DEFERRED: + - SWREQ-FALCON-FAULT-P04 → v1.116 — supervisor-driven controlled landing + + the clean gz confirmation run and recordable video (the video is + display-dependent). GitHub #256. + - Verification-fidelity audit (GitHub #257) — the broader sweep of other + safety-behaviour campaigns for the idealised-harness blind spot. Builds on v1.114 (FV-FALCON-FAULT-003): the rank-3 allocation, the full-loop recovery oracle, and the dispersed full-loop campaign. - tags: [falcon, fault-tolerance, rotor-loss, verification-fidelity, roadmap, v1.115, proposed] + tags: [falcon, fault-tolerance, rotor-loss, fdi, robustness, roadmap, v1.115] links: - type: implements target: SYSREQ-FALCON-005 diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml index 823d36d..2398281 100644 --- a/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml @@ -2,7 +2,7 @@ artifacts: - id: SWREQ-FALCON-FAULT-P03 type: sw-req title: "FAULT-P03 — single-rotor-out FDI robustness under sensor noise" - status: proposed + status: verified release: falcon-v1.115.0 description: > On loss of a single rotor the FDI shall isolate the failed rotor within diff --git a/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml index e8e0f24..7e7d0b6 100644 --- a/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml +++ b/artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml @@ -3,7 +3,7 @@ artifacts: type: sw-req title: "FAULT-P04 — supervisor-driven controlled landing after single-rotor loss" status: proposed - release: falcon-v1.115.0 + release: falcon-v1.116.0 description: > On single-rotor loss the FlightSupervisor shall command LAND and bring the vehicle down in a CONTROLLED descent to touchdown — bounded sink rate, diff --git a/artifacts/verification/FV-FALCON-FAULT-004.yaml b/artifacts/verification/FV-FALCON-FAULT-004.yaml new file mode 100644 index 0000000..f592409 --- /dev/null +++ b/artifacts/verification/FV-FALCON-FAULT-004.yaml @@ -0,0 +1,63 @@ +artifacts: + - id: FV-FALCON-FAULT-004 + type: sw-verification + title: "Single-rotor-out FDI noise-robustness — command-aligned residual + roll/pitch gate (v1.115)" + status: verified + release: falcon-v1.115.0 + description: > + Verifies FAULT-P03. The v1.114 dispersed full-loop campaign surfaced that + the single-rotor-out FDI, under heavier sensor noise, FALSE-ISOLATED a + healthy rotor and sometimes never isolated the dead one. Root cause (found + by instrumenting a failing trace, not guessed): the effectiveness residual + compared the CURRENT-tick motor command against the PREVIOUS-tick achieved + ESC RPM — a one-tick telemetry lag — so an abrupt collective step spiked + ALL FOUR rotors' residuals at once and the CUSUM tripped on whichever was + checked first. Measured baseline at gps σ ≤ 0.3 m / gyro ≤ 0.02: + 43/300 false isolations, 26 misses. + + FIX (falcon-core FlightCore::step): + 1. Command-ALIGNED residual — compare achieved against the PREVIOUS + command that produced it (stored last_motor_cmd), so a healthy + rotor's residual is ~0 even through abrupt command changes and only a + genuinely dead rotor accumulates. This is the safety fix: false + isolations 43 → 0. + 2. Detection gate on ROLL/PITCH rate only, not yaw — a single-rotor-out + RELINQUISHES yaw and spins freely about its near-vertical axis + (Mueller & D'Andrea); gating on total rate slammed the gate shut for + the rest of the flight once the spin built, starving re-detection. + Roll/pitch rate is the tumble indicator the gate actually wants. + + EVIDENCE: + - falcon-sitl-gz fdi_noise_robustness_monte_carlo_campaign: 200 dispersed + trials at the heavier envelope (gps σ ≤ 0.18 m, gyro ≤ 0.010) — 0 + false isolations (the safety invariant), 0 misses, worst detect + latency 17 steps (< 25 = 100 ms). A regression that reintroduces the + skew drives false isolations > 0 and trips this guard. + - falcon-core fdi_isolates_dead_rotor_and_reconfigures: the clean-signal + isolation contract still holds under the aligned residual. + - falcon-core survives_single_rotor_failure_without_flipping: the + recovery (FAULT-P02) is unaffected — still no flip. + + FDI observability: FlightCore::fdi_diag() exposes the gate inputs + + residual so the campaign (and future FDI work) can see WHY detection + stalls, not just that it did. + + Envelope note: at extreme gps σ ≥ 0.3 m the estimator/altitude loop itself + destabilises and the vehicle does not fly (a dead rotor is then moot and + produces no residual) — an ESTIMATOR envelope limit, not an FDI defect; + the aligned residual correctly stays 0 there (no false trip). Estimator + robustness at that envelope is out of scope for FAULT-P03. + + FALSIFICATION: the FDI is wrong if, within the flying envelope + (gps σ ≤ 0.18 m), it isolates a HEALTHY rotor, or fails to isolate the + dead one within ~100 ms. + tags: [verification, falcon, fault-tolerance, rotor-loss, fdi, robustness, v1.115] + fields: + method: automated-test + steps: + - run: cargo test -p falcon-sitl-gz fdi_noise_robustness_monte_carlo_campaign + - run: cargo test -p falcon-core fdi_isolates_dead_rotor_and_reconfigures + - run: cargo test -p falcon-core survives_single_rotor_failure_without_flipping + links: + - type: verifies + target: SWREQ-FALCON-FAULT-P03 diff --git a/crates/falcon-core/plain/src/lib.rs b/crates/falcon-core/plain/src/lib.rs index f6340ed..211d2e3 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -144,6 +144,18 @@ pub struct FlightCore { /// production core against the gz plant, where achieved starts at 0). step_count: u32, fdi_warmup_steps: u32, + /// Previous control tick's per-rotor command (v1.115, FAULT-P03). The ESC + /// RPM telemetry read at the top of a step reflects the command from the + /// PREVIOUS step (one-tick actuation/telemetry lag), so the effectiveness + /// residual must compare achieved against THIS, not the current command — + /// otherwise an abrupt collective step spikes every rotor's residual at once + /// and the FDI false-isolates a healthy rotor. + last_motor_cmd: [f32; 4], + /// FDI observability (v1.115): last `(rate2, tilt_cos, gate_open, resid)` + /// evaluated in the single-rotor-out detector — surfaced via `fdi_diag()` + /// so the dispersed campaign can see WHY detection stalls under noise (the + /// rate gate closing vs the residual), not just that it did. + dbg_fdi: (f32, f32, bool, [f32; 4]), /// Commanded heading (yaw, rad, NED). A quad HOLDS its launch heading, not /// North — so when a heading reference first arrives (`read_heading`), the /// initial heading is captured here and the geometric controller drives yaw @@ -207,6 +219,8 @@ impl FlightCore { // achieved rotor state has caught the command by then, so the // effectiveness residual reflects real faults, not the spin-up jump. fdi_warmup_steps: ((loop_hz * 0.2) as u32).max(10), + last_motor_cmd: [0.0; 4], + dbg_fdi: (0.0, 1.0, false, [0.0; 4]), calib: relay_calib::CalParams::identity(), } } @@ -254,6 +268,14 @@ impl FlightCore { self.yaw_setpoint } + /// FDI observability (v1.115): `(rate2, tilt_cos, gate_open, resid[4])` from + /// the last single-rotor-out detector evaluation. `gate_open` is the + /// near-level/low-rate gate; when it stays false under sensor noise the + /// detector never sees the residual — the FAULT-P03 failure mode. + pub fn fdi_diag(&self) -> (f32, f32, bool, [f32; 4]) { + self.dbg_fdi + } + /// Diagnostics: last geometric desired body rate + last ADRC torque. pub fn last_omega_d(&self) -> Vec3 { self.last_omega_d @@ -478,9 +500,18 @@ impl FlightCore { // run the detector when the vehicle is roughly level and not spinning // (v1.113 — caught a phantom rotor-out during an attitude transient on gz). let tilt_cos = 1.0 - 2.0 * (est.q[1] * est.q[1] + est.q[2] * est.q[2]); // R[2][2] - let rate2 = - gyro_f[0] * gyro_f[0] + gyro_f[1] * gyro_f[1] + gyro_f[2] * gyro_f[2]; - let fdi_steady = tilt_cos > 0.90 && rate2 < 1.0; // ≲26° tilt, ≲1 rad/s + // ROLL/PITCH rate only — NOT yaw (v1.115, FAULT-P03). The gate exists to + // avoid diagnosing a dead rotor mid-TUMBLE (a roll/pitch upset spikes + // the residual on saturated rotors). But a single-rotor-out RELINQUISHES + // yaw: the body then spins freely about its near-vertical axis at several + // rad/s — normal, not tumbling. Including yaw here slammed the gate shut + // for the rest of the flight, so any fault not caught in the brief window + // before the spin built was NEVER caught (26/300 misses + 43 mis-isolations + // under heavy sensor noise). Roll/pitch rate stays low through the spin, + // keeping detection available while still blocking a real tumble. + let rp_rate2 = gyro_f[0] * gyro_f[0] + gyro_f[1] * gyro_f[1]; + let fdi_steady = tilt_cos > 0.90 && rp_rate2 < 1.0; // ≲26° tilt, ≲1 rad/s roll+pitch + let mut dbg_resid = [0.0f32; 4]; if self.failed_motor.is_none() && self.step_count >= self.fdi_warmup_steps && fdi_steady @@ -490,12 +521,19 @@ impl FlightCore { let mut i = 0; while i < 4 { let achieved = (rpm[i] as f32 / ESC_RPM_FULL).clamp(0.0, 2.0); - resid[i] = (motors[i] - achieved).abs(); + // Compare achieved against the PREVIOUS command that produced + // it (v1.115): the telemetry lags one tick, so using the + // current command spikes every residual on an abrupt step. + resid[i] = (self.last_motor_cmd[i] - achieved).abs(); i += 1; } + dbg_resid = resid; self.failed_motor = self.fdi.update(resid); } } + self.dbg_fdi = (rp_rate2, tilt_cos, fdi_steady, dbg_resid); + // Record this tick's command for next tick's residual alignment. + self.last_motor_cmd = motors; b.write_motors(&motors); } @@ -1514,6 +1552,7 @@ impl FlightBackend for SimBackend { mod tests { use super::*; + /// The SAME verified cascade, run through the HAL seam against the sim /// backend, recovers a tilted body to level — demonstrating the flight /// core is backend-agnostic (the seam carries the real IEKF + geometric + diff --git a/examples/falcon-sitl-gz/src/campaign.rs b/examples/falcon-sitl-gz/src/campaign.rs index b401926..366ac7d 100644 --- a/examples/falcon-sitl-gz/src/campaign.rs +++ b/examples/falcon-sitl-gz/src/campaign.rs @@ -612,6 +612,111 @@ mod fullloop_tests { } } +// ── FDI noise-robustness campaign (v1.115, FAULT-P03) ──────────────────────── +// +// The full-loop recovery campaign above runs at a MODEST sensor-noise envelope +// (gps σ ≤ 0.12 m, gyro ≤ 0.006). This one stresses the single-rotor-out FDI +// under HEAVIER noise (gps σ ≤ 0.18 m, gyro ≤ 0.010) — the regime where the +// pre-v1.115 detector false-isolated a HEALTHY rotor. Cause: the effectiveness +// residual compared the CURRENT-tick command against the PREVIOUS-tick achieved +// RPM (a one-tick telemetry lag), so an abrupt collective step spiked every +// rotor's residual at once and the CUSUM tripped on whichever was checked first +// (43/300 false isolations, 0 with alignment). Fixed by comparing achieved +// against the command that produced it + gating detection on ROLL/PITCH rate +// only (a rotor-out relinquishes yaw and spins — not a tumble). + +fn sample_fdi_noise(rng: &mut SplitMix64, index: u32) -> FullLoopTrial { + FullLoopTrial { + failed_rotor: (rng.next_u64() % 4) as usize, + setpoint_alt: -rng.range(2.0, 4.0), + rot_drag: rng.range(0.015, 0.035), + gyro_white: rng.range(0.0, 0.010), + gps_noise: rng.range(0.0, 0.18), + seed: index.wrapping_mul(2_654_435_761) ^ 0x0FD1_0FD1, + } +} + +#[derive(Clone, Debug, Default)] +pub struct FdiReport { + pub trials: u32, + pub misses: u32, // never isolated within the window + pub wrong: u32, // isolated a HEALTHY rotor (false positive — the safety bug) + pub worst_latency_steps: u32, + pub failing: Vec<(u32, String)>, +} + +/// Run `n` dispersed FDI noise-robustness trials from `campaign_seed`. +pub fn run_fdi_noise_campaign(n: u32, campaign_seed: u64) -> FdiReport { + let mut rep = FdiReport { + trials: n, + ..Default::default() + }; + for i in 0..n { + let mut rng = trial_rng(campaign_seed, i); + let t = sample_fdi_noise(&mut rng, i); + let o = run_fullloop_trial(&t); + match o.isolated { + None => { + rep.misses += 1; + if rep.failing.len() < 20 { + rep.failing.push((i, format!("{t:?}: never isolated"))); + } + } + Some(f) if f != t.failed_rotor => { + rep.wrong += 1; + if rep.failing.len() < 20 { + rep.failing + .push((i, format!("{t:?}: isolated {f}, expected {}", t.failed_rotor))); + } + } + Some(_) => { + if o.detect_latency_steps != u32::MAX { + rep.worst_latency_steps = rep.worst_latency_steps.max(o.detect_latency_steps); + } + } + } + } + rep +} + +#[cfg(test)] +mod fdi_noise_tests { + use super::*; + + const FDI_SEED: u64 = 0x00FD_1000_0000_1150; + const FDI_TRIALS: u32 = 200; + + #[test] + fn fdi_noise_robustness_monte_carlo_campaign() { + let rep = run_fdi_noise_campaign(FDI_TRIALS, FDI_SEED); + eprintln!( + "FDI noise-robustness campaign: {} trials | misses {}, wrong-isolations {}, worst detect latency {} steps", + rep.trials, rep.misses, rep.wrong, rep.worst_latency_steps + ); + + // SAFETY (the v1.115 fix): NEVER isolate a healthy rotor. A false + // positive drops a healthy vehicle into degraded rotor-out mode — the + // command-alignment fix must hold this at 0 across the whole envelope. + assert_eq!( + rep.wrong, 0, + "FDI false-isolated a healthy rotor in {}/{} trials: {:#?}", + rep.wrong, rep.trials, rep.failing + ); + // FUNCTIONAL: within the flying envelope the correct rotor is isolated + // (no miss) with bounded latency. + assert_eq!( + rep.misses, 0, + "FDI never isolated the dead rotor in {}/{} trials: {:#?}", + rep.misses, rep.trials, rep.failing + ); + assert!( + rep.worst_latency_steps < 25, // < 100 ms at 250 Hz + "worst FDI detect latency {} steps exceeded 25", + rep.worst_latency_steps + ); + } +} + // ── Attitude-stabilisation campaign (random tilt, no fault) ────────────────── /// A single dispersed attitude-recovery trial: the aircraft starts tilted and