From 0f71821d6e281e41bd35b0f0a7d5949d3fdfe2d2 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Sat, 27 Jun 2026 07:34:55 +0200 Subject: [PATCH] =?UTF-8?q?feat(failsafe):=20v1.103=20=E2=80=94=20promote?= =?UTF-8?q?=20single-rotor-out=20reconfiguration=20into=20production=20Fli?= =?UTF-8?q?ghtCore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The rotor-loss recovery (FAULT-P02) was verified end-to-end only in the falcon-sitl-gz example; the production FlightCore — the controller the real WASM component runs — always used the full-attitude mix(). This wires the detect→ reconfigure→land chain into FlightCore, REUSING the existing verified pieces (no new detector — verify-first caught that a planned one would have duplicated the relay-iekf RotorFaultDetector). - FlightBackend::read_motor_rpm() -> Option<[i32;4]> (default None): the achieved-per-rotor source (ESC telemetry, e.g. DroneCAN esc.Status). A backend without ESC feedback runs unchanged (no detection). - FlightCore::step: forms the per-rotor effectiveness residual |commanded − achieved| from the RPM, feeds the relay-iekf RotorFaultDetector (CUSUM, the same thresholds the SITL chain proved), and on isolation latches failed_motor and switches the control law to relay-geo moment_reduced (reduced-attitude S², yaw relinquished) + relay-mix-quad mix_rotor_out (failed rotor → 0, MIX-P08) — the exact composition FV-FALCON-FAULT-002 verifies. - FlightSupervisor: on isolation, commands LAND (Failsafe with have_position=false) — a 3-rotor quad has no yaw authority to navigate home, so RTL is unsafe. Tests (deterministic — RPM is a direct backend input, not estimator-derived, so none of the wind/runaway estimator-corruption fragility): - fdi_isolates_dead_rotor_and_reconfigures: healthy never isolated; a rotor at 0 RPM under command is isolated in a few cycles → commanded 0, healthy in [floor,1]. - motor_failure_commands_land: an airborne vehicle that loses a rotor → Land. 41 falcon-core tests; the SITL fault_tolerance chain still passes; falcon-mavlink/ hitl unaffected. rivet FV-FALCON-FAULT-002 extended with the production-wiring evidence. validate PASS, 0 error-gaps. Co-Authored-By: Claude Opus 4.8 --- .../verification/FV-FALCON-FAULT-002.yaml | 20 ++ crates/falcon-core/plain/src/lib.rs | 226 +++++++++++++++++- 2 files changed, 238 insertions(+), 8 deletions(-) diff --git a/artifacts/verification/FV-FALCON-FAULT-002.yaml b/artifacts/verification/FV-FALCON-FAULT-002.yaml index d216aba..b0f6d52 100644 --- a/artifacts/verification/FV-FALCON-FAULT-002.yaml +++ b/artifacts/verification/FV-FALCON-FAULT-002.yaml @@ -38,6 +38,24 @@ artifacts: rotor-out allocator above) ran only via this artifact's step before; now the dedicated Kani gate runs all six on every PR (belt-and-suspenders), closing the orphaned-proof gap. + + v1.103 PRODUCTION WIRING: the detect→reconfigure chain — composed + only in the falcon-sitl-gz example (4) before — is now in the + PRODUCTION FlightCore::step. The core runs the RotorFaultDetector + on the per-rotor commanded-vs-achieved residual (achieved from ESC + RPM via the new FlightBackend::read_motor_rpm seam), and on + isolation switches the control law to relay-geo moment_reduced + + relay-mix-quad mix_rotor_out (the same composition proven above); + the supervisor then commands LAND (a 3-rotor quad has no yaw + authority to navigate home). Deterministic falcon-core tests + (RPM is a direct backend input, not estimator-derived): + - fdi_isolates_dead_rotor_and_reconfigures: a healthy quad is + never isolated; a rotor whose RPM drops to 0 under a nonzero + command is isolated within a few cycles, after which the + reconfigured allocator commands it 0 and the healthy three + stay in [floor,1]. + - motor_failure_commands_land: an airborne vehicle that loses a + rotor is commanded to Land (not RTL). tags: [verification, falcon, fault-tolerance, rotor-loss, kani, v0.26] fields: method: formal-verification @@ -49,6 +67,8 @@ artifacts: - run: cargo test -p relay-iekf - run: cargo test -p relay-geo - run: cargo test -p falcon-sitl-gz fault_tolerance + - run: cargo test -p falcon-core fdi_isolates_dead_rotor_and_reconfigures + - run: cargo test -p falcon-core motor_failure_commands_land links: - type: verifies target: SWREQ-FALCON-FAULT-P02 diff --git a/crates/falcon-core/plain/src/lib.rs b/crates/falcon-core/plain/src/lib.rs index fe084d0..fc681d8 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -20,8 +20,8 @@ #![allow(clippy::needless_range_loop)] use relay_adrc::{AdrcRate, GyroLpf}; -use relay_geo::{GeoAtt, GeoGains}; -use relay_iekf::{Iekf, Imu as IekfImu, NavState, Vec3}; +use relay_geo::{quat_to_rotmat, thrust_axis_ned, GeoAtt, GeoGains}; +use relay_iekf::{Iekf, Imu as IekfImu, NavState, RotorFaultDetector, Vec3}; use relay_mix_quad::{motors_to_torque_signs, QuadMixer}; /// One inertial-measurement sample in the body frame. @@ -61,6 +61,14 @@ pub trait FlightBackend { fn read_baro(&mut self) -> Option { None } + /// Per-rotor reported RPM (from ESC telemetry, e.g. DroneCAN esc.Status), or + /// `None` if the ESCs report no RPM. The ACHIEVED-per-rotor source for the + /// single-rotor-out FDI: the core compares it against the commanded throttle + /// to form the effectiveness residual (v1.103). Default `None` = no telemetry, + /// so a backend without ESC feedback simply runs without rotor-fault detection. + fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + None + } } /// The verified flight core, generic over the backend. Holds the estimator + @@ -108,6 +116,14 @@ pub struct FlightCore { /// into the verified IEKF as a vertical anchor (a position update whose z is /// the baro), so altitude AND its rate survive GPS-vertical loss. baro_var: f32, + /// 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 + + /// the reconfigured allocator (v1.103 — the SITL-verified recovery, promoted + /// into the production loop). + fdi: RotorFaultDetector, + /// The isolated failed rotor (latched), or `None`. Drives the degraded path. + failed_motor: Option, /// Sensor calibration applied to raw IMU/mag samples before the estimator /// (gyro/accel bias+scale, mag hard/soft-iron). Identity until /// `set_calibration` installs solved offsets — the explicit replacement for @@ -147,6 +163,9 @@ impl FlightCore { pos_int: [0.0; 2], pos_int_max: 1.5, baro_var: 0.05, // ≈ (0.2 m)² baro noise; trusted less than a clean GPS-z + // CUSUM thresholds matching the SITL-verified fault-tolerance chain. + fdi: RotorFaultDetector::new(0.5, 0.1), + failed_motor: None, calib: relay_calib::CalParams::identity(), } } @@ -183,6 +202,12 @@ impl FlightCore { m[0].max(m[1]).max(m[2]).max(m[3]) } + /// The isolated failed rotor (latched), or `None` — the supervisor lands the + /// vehicle when this is set (a 3-rotor quad cannot navigate; v1.103). + pub fn failed_motor(&self) -> Option { + self.failed_motor + } + /// Command a target altitude (NED z, metres; negative = up). v1.2. pub fn set_altitude(&mut self, ned_z: f32) { self.setpoint[2] = ned_z; @@ -311,14 +336,42 @@ impl FlightCore { a_cmd[1] *= s; } - // ── Attitude ── geometric desired-rate from a_cmd (hold heading 0) → - // ADRC torque on filtered (calibrated) gyro. + // advance the (stateful) gyro low-pass every cycle so it stays warm. let gyro_f = self.gyro_lpf.filter(gyro); - let omega_d = self.geo.desired_rate(est.q, a_cmd, 0.0); - let torque = self.adrc.tick(gyro_f, omega_d, dt); - // ── Allocate + actuate ── - let motors = self.mixer.mix(torque, thrust); + // ── Attitude + allocate ── + let motors = if let Some(failed) = self.failed_motor { + // DEGRADED (single-rotor-out): reduced-attitude S² drives the thrust + // axis and RELINQUISHES yaw, and the reconfigured allocator pins the + // failed rotor to 0 (MIX-P08) — the SITL-verified recovery path, + // 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); + self.mixer.mix_rotor_out(failed, torque, thrust, ROTOR_OUT_FLOOR) + } else { + // NORMAL: full-attitude geometric desired-rate → ADRC torque → mix. + let omega_d = self.geo.desired_rate(est.q, a_cmd, 0.0); + let torque = self.adrc.tick(gyro_f, omega_d, dt); + self.mixer.mix(torque, thrust) + }; + + // ── Single-rotor-out FDI ── form the per-rotor effectiveness residual + // |commanded − achieved| from ESC RPM telemetry and feed the CUSUM; on + // isolation, latch the failed rotor (next step runs the degraded path). + if self.failed_motor.is_none() { + if let Some(rpm) = b.read_motor_rpm() { + let mut resid = [0.0f32; 4]; + 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(); + i += 1; + } + self.failed_motor = self.fdi.update(resid); + } + } + b.write_motors(&motors); } } @@ -340,6 +393,16 @@ pub const MAX_WAYPOINTS: usize = 16; /// fully settle on each leg. const WAYPOINT_RADIUS: f32 = 1.2; +/// Nominal ESC RPM at full throttle — normalises reported RPM to a per-rotor +/// EFFECTIVENESS (≈ commanded throttle when healthy) for the single-rotor-out FDI +/// residual (v1.103). Only the ratio matters; a per-airframe value scales it. +const ESC_RPM_FULL: f32 = 8000.0; + +/// Thrust floor for the reconfigured (single-rotor-out) allocator — the three +/// healthy rotors are kept at/above this so the body retains tilt authority while +/// it stabilises on S². Matches the SITL-verified fault-tolerance composition. +const ROTOR_OUT_FLOOR: f32 = 0.15; + /// Pre-arm estimator-convergence ceiling (v1.99): the IEKF tilt-uncertainty /// (roll²+pitch², rad²) must be at/below this for `estimator_converged` to pass. /// The filter starts well above it and settles below as gravity updates arrive, @@ -751,6 +814,20 @@ impl FlightSupervisor { }; self.core.set_position(sp); self.core.step(b); + + // ── MOTOR-FAILURE failsafe (v1.103): once the core's FDI isolates a + // rotor, LAND ASAP — a 3-rotor quad has no yaw authority to navigate home, + // so RTL is unsafe. Fire Failsafe with have_position=false, which the FSM + // maps to Land (not Rtl) from any flying state. The core has already + // switched to the reduced-attitude + reconfigured allocator. ── + if self.core.failed_motor().is_some() + && self.fsm.is_airborne() + && self.fsm.mode() != Mode::Land + { + let land = Gates { level: true, throttle_low: true, have_position: false, prearm_ok: true }; + self.fsm.on(Event::Failsafe, land); + self.rtl_latched = true; + } } } @@ -1335,6 +1412,139 @@ mod tests { assert_eq!(cal.calibration().gyro_bias, [0.0, 0.0, bias]); } + /// v1.103 — the supervisor LANDS on a motor failure: an airborne vehicle that + /// loses a rotor (detected by the core's FDI) is commanded to Land (not RTL — + /// a 3-rotor quad has no yaw authority to navigate home). + #[test] + fn motor_failure_commands_land() { + use relay_calib::CalParams; + struct RpmBackend { + last: [f32; 4], + inject: bool, + } + impl FlightBackend for RpmBackend { + fn read_imu(&mut self) -> ImuSample { + ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + } + fn read_position(&mut self) -> Option { + Some([0.0, 0.0, -2.0]) // at the 2 m cruise altitude + } + fn read_mag(&mut self) -> Option { + None + } + fn read_battery_v(&mut self) -> f32 { + 16.0 + } + fn write_motors(&mut self, m: &[f32]) { + self.last = [m[0], m[1], m[2], m[3]]; + } + fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + let mut rpm = [0i32; 4]; + let mut i = 0; + while i < 4 { + rpm[i] = (self.last[i] * ESC_RPM_FULL) as i32; + i += 1; + } + if self.inject { + rpm[1] = 0; // rotor 1 dies + } + Some(rpm) + } + fn dt(&self) -> f32 { + 0.004 + } + } + let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 200.0, 2.0, 14.0); + sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + let mut b = RpmBackend { last: [0.5; 4], inject: false }; + + for _ in 0..1500 { + sup.step(&mut b); + } + sup.command(relay_fsm::Event::Arm, true, true); + assert_eq!(sup.mode(), relay_fsm::Mode::Armed); + sup.command(relay_fsm::Event::RequestTakeoff, true, false); + assert!(sup.mode().is_airborne(), "airborne after takeoff"); + + // a rotor dies → the core's FDI isolates it → the supervisor lands. + b.inject = true; + for _ in 0..40 { + sup.step(&mut b); + } + assert_eq!(sup.mode(), relay_fsm::Mode::Land, "motor failure commands Land"); + } + + /// v1.103 — the production FlightCore detects a dead rotor from ESC RPM and + /// reconfigures: healthy rotors are never isolated, but a rotor whose RPM + /// drops to 0 under a nonzero command is isolated by the CUSUM FDI within a + /// few cycles, after which the reconfigured allocator commands that rotor 0 + /// and keeps the healthy three in [floor, 1] (MIX-P08). Deterministic — the + /// RPM is a direct backend input, not estimator-derived. + #[test] + fn fdi_isolates_dead_rotor_and_reconfigures() { + struct RpmBackend { + last: [f32; 4], + failed: usize, + inject: bool, + } + impl FlightBackend for RpmBackend { + fn read_imu(&mut self) -> ImuSample { + ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + } + fn read_position(&mut self) -> Option { + Some([0.0, 0.0, -5.0]) // holding 5 m up + } + fn read_mag(&mut self) -> Option { + None + } + fn write_motors(&mut self, m: &[f32]) { + self.last = [m[0], m[1], m[2], m[3]]; + } + fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + // reported RPM tracks the last command, except a dead rotor reads 0. + let mut rpm = [0i32; 4]; + let mut i = 0; + while i < 4 { + rpm[i] = (self.last[i] * ESC_RPM_FULL) as i32; + i += 1; + } + if self.inject { + rpm[self.failed] = 0; + } + Some(rpm) + } + fn dt(&self) -> f32 { + 0.004 + } + } + let mut core = FlightCore::new(0.5, 250.0); + core.set_altitude(-5.0); // hold 5 m → commands hover thrust to all rotors + let mut b = RpmBackend { last: [0.5; 4], failed: 2, inject: false }; + + // healthy: the FDI never isolates a rotor. + for _ in 0..100 { + core.step(&mut b); + } + assert_eq!(core.failed_motor(), None, "healthy rotors must not be isolated"); + + // rotor 2 dies (RPM → 0 under a nonzero command) → isolated quickly. + b.inject = true; + for _ in 0..30 { + core.step(&mut b); + } + assert_eq!(core.failed_motor(), Some(2), "the CUSUM FDI isolates the dead rotor"); + + // the reconfigured allocator commands the failed rotor 0 and the three + // healthy rotors within [floor, 1] (MIX-P08 in the production loop). + core.step(&mut b); + assert_eq!(b.last[2], 0.0, "failed rotor commanded 0"); + for (i, &v) in b.last.iter().enumerate() { + if i != 2 { + assert!((ROTOR_OUT_FLOOR - 1e-6..=1.0 + 1e-6).contains(&v), "healthy rotor {i} = {v}"); + } + } + } + #[test] fn arbitrary_backend_drives_the_core() { struct NullBackend {