feat(falcon): single-rotor-out FDI noise-robustness (v1.115, FAULT-P03)

artifacts/features/FEAT-FALCON-v1.115.yaml

@@ -0,0 +1,36 @@
+artifacts:
+  - id: FEAT-FALCON-v1.115
+    type: feature
+    title: "v1.115 — single-rotor-out FDI noise-robustness"
+    status: implemented
+    release: falcon-v1.115.0
+    description: >
+      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).
+
+      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, fdi, robustness, roadmap, v1.115]
+    links:
+      - type: implements
+        target: SYSREQ-FALCON-005

artifacts/swreq/SWREQ-FALCON-FAULT-P03.yaml

@@ -0,0 +1,34 @@
+artifacts:
+  - id: SWREQ-FALCON-FAULT-P03
+    type: sw-req
+    title: "FAULT-P03 — single-rotor-out FDI robustness under sensor noise"
+    status: verified
+    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 —
+      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

artifacts/swreq/SWREQ-FALCON-FAULT-P04.yaml

@@ -0,0 +1,33 @@
+artifacts:
+  - id: SWREQ-FALCON-FAULT-P04
+    type: sw-req
+    title: "FAULT-P04 — supervisor-driven controlled landing after single-rotor loss"
+    status: proposed
+    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,
+      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

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

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 +

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