From ffbc1d70189d9aa6834d3ae05bca645c995e7e3f Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Fri, 29 May 2026 21:36:08 +0200 Subject: [PATCH] =?UTF-8?q?feat(falcon):=20v0.19.9=20=E2=80=94=20verified?= =?UTF-8?q?=20arming=20sequencer=20(ARM-P01)=20+=20tip-over=20localization?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ships relay-arm, a Kani-verified startup state machine (Disarmed → SpinUp → LevelHold → Armed) that gates attitude-torque authority: the rate loop's torque reaches the mixer ONLY in Armed, and Armed is reachable ONLY through a confirmed-level window. Replaces the bench's ad-hoc time-only `t < 0.5 s` spawn-hold with verified time+level gating — the "deterministic startup sequencing" called for as fix (1) in the v0.19.7 bistability diagnosis. Verification — Kani, not Verus (the tilt compare is f32; like relay-mix-quad this is off the integer Verus track). The gating invariant is INDUCTIVE over one tick(), so the bounded check covers all input sequences: cargo kani verify_arming_torque_gated → 143 checks SUCCESSFUL (torque_authority ⟺ Armed; a new entry into Armed comes only from LevelHold with the level gate satisfied; phase ∈ [0,3]; thrust_scale ∈ [0,1] finite) cargo kani verify_arming_total → 137 checks SUCCESSFUL (panic-free + thrust bounded for NaN/∞ tilt, zero spin-up) Plus proptest + 5 unit tests. FALSIFICATION (the empirical value): I set out to "kill RC#1 startup tip-over" — the gz data falsified that framing. With the correct NavSat home, the gated AND ungated startups both hand off a LEVEL body (0° through arming at 0.40 s); the body then holds level ~3.5 s and only tips (~88°) once the position loop dominates. The tip-over is a DOWNSTREAM residual (relay-pos thrust normalisation + RC#3 EKF-attitude-during-accel), NOT a spawn transient. The arming sequencer is verified, correct, and a prerequisite building block — but it is NOT what unblocks position-hold. Publishing this localizes the v0.19.10+ target precisely. Regression gate: gz alt-rate reliable hover is unaffected — 3/3 consecutive PASS at ~0.20 m, identical to v0.19.8's 4/4. Ships: - crates/relay-arm: ArmingSequencer + ArmingConfig + Kani harnesses + tests (new workspace member). - examples/falcon-sitl-gz: sequencer wired into alt-rate AND the full closed-loop cascade (which also adopts mix_thrust_floor); body_tilt_rad helper; `arming` / `arming-ungated` diagnostic scenarios. - SWREQ-FALCON-ARM-P01 + FV-FALCON-ARM-001 + rollout v0.19.9 entry. - bench-evidence/gz-sim/2026-05-29-v0.19.9-*.md (incl. the -488 m NavSat-home methodology trap). Honestly NOT claimed: that arming makes full position-hold hover (it does not — run_peak ~88°), nor that a gz "arming-check PASS" means the quad hovers (PASS is scoped to the startup handoff). Full position-hold remains the residual, now localized to relay-pos thrust + RC#3 for v0.19.10+. Co-Authored-By: Claude Opus 4.8 (1M context) --- Cargo.toml | 1 + artifacts/features/FEAT-FALCON-rollout.yaml | 74 +++- artifacts/swreq/SWREQ-FALCON-ARM-P01.yaml | 60 +++ artifacts/verification/FV-FALCON-ARM-001.yaml | 87 +++++ ...ming-sequencer-and-tipover-localization.md | 101 +++++ crates/relay-arm/Cargo.toml | 23 ++ crates/relay-arm/plain/src/lib.rs | 345 ++++++++++++++++++ examples/falcon-sitl-gz/Cargo.toml | 1 + examples/falcon-sitl-gz/src/main.rs | 226 +++++++++++- 9 files changed, 902 insertions(+), 16 deletions(-) create mode 100644 artifacts/swreq/SWREQ-FALCON-ARM-P01.yaml create mode 100644 artifacts/verification/FV-FALCON-ARM-001.yaml create mode 100644 bench-evidence/gz-sim/2026-05-29-v0.19.9-arming-sequencer-and-tipover-localization.md create mode 100644 crates/relay-arm/Cargo.toml create mode 100644 crates/relay-arm/plain/src/lib.rs diff --git a/Cargo.toml b/Cargo.toml index 492574d..d0e4b40 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -26,6 +26,7 @@ members = [ "crates/relay-rate", "crates/relay-att", "crates/relay-mix-quad", + "crates/relay-arm", "crates/relay-pos", "examples/falcon-hello", "examples/falcon-ekf-bench", diff --git a/artifacts/features/FEAT-FALCON-rollout.yaml b/artifacts/features/FEAT-FALCON-rollout.yaml index fa8007a..f39f280 100644 --- a/artifacts/features/FEAT-FALCON-rollout.yaml +++ b/artifacts/features/FEAT-FALCON-rollout.yaml @@ -2132,6 +2132,78 @@ artifacts: - type: depends-on target: FEAT-FALCON-v0.19.7 + - id: FEAT-FALCON-v0.19.9 + type: feature + title: "v0.19.9 — verified arming sequencer (ARM-P01) + tip-over localization" + status: approved + description: > + LANDED. Ships relay-arm, a Kani-verified startup state machine + (Disarmed → SpinUp → LevelHold → Armed) that gates attitude-torque + authority: the rate loop's torque reaches the mixer ONLY in Armed, + and Armed is reachable ONLY through a confirmed-level window. It + replaces the bench's ad-hoc time-only `t < 0.5 s` spawn-hold with + verified time+level gating — the "deterministic startup sequencing" + called for as fix (1) in the v0.19.7 bistability diagnosis. + + Verification — Kani, not Verus (the tilt compare is f32; like + relay-mix-quad this is off the integer Verus track). The gating + invariant is INDUCTIVE over one tick(), so the bounded check covers + all input sequences: + cargo kani verify_arming_torque_gated → 143 checks SUCCESSFUL + (torque_authority ⟺ Armed; a new entry into Armed comes only + from LevelHold with the level gate satisfied; phase ∈ [0,3]; + thrust_scale ∈ [0,1] finite) + cargo kani verify_arming_total → 137 checks SUCCESSFUL + (panic-free + thrust bounded for NaN/∞ tilt, zero spin-up) + Plus proptest + unit tests (5/5): torque-gated-until-level, + tilt-resets-count, NaN-never-arms, monotonic spin-up ramp. + + FALSIFICATION (the empirical value): I set out to "kill RC#1 + startup tip-over" — the gz data falsified that framing. With the + correct NavSat home, the gated AND ungated startups both hand off a + LEVEL body (0° through arming at 0.40 s); the body then holds level + ~3.5 s and only tips (~88°) once the position loop dominates. The + tip-over is a DOWNSTREAM residual (relay-pos thrust normalisation + + RC#3 EKF-attitude-during-accel), not a spawn transient. The arming + sequencer is verified, correct, and a prerequisite building block — + but it is NOT what unblocks position-hold. Publishing this localizes + the v0.19.10+ target precisely. + + Ships: + - crates/relay-arm: ArmingSequencer + ArmingConfig + Kani + harnesses + tests (new workspace member). + - examples/falcon-sitl-gz: sequencer wired into alt-rate AND the + full closed-loop cascade (which also adopts mix_thrust_floor); + body_tilt_rad helper; `arming` / `arming-ungated` diagnostic + scenarios. + - SWREQ-FALCON-ARM-P01 + FV-FALCON-ARM-001. + - bench-evidence/gz-sim/2026-05-29-v0.19.9-*.md. + + Verification: + - cargo kani (both harnesses) → SUCCESSFUL. + - cargo test --workspace → green; relay-arm 5/5; falcon-sitl-gz + 10/10. + - gz bench REGRESSION GATE → alt-rate 3/3 PASS at ≈0.20 m (the + v0.19.8 reliable hover is unaffected by wiring in the sequencer). + - gz bench → arming clean handoff (0° through arming). + - rivet validate → PASS. + + Honestly NOT claimed: that arming makes full position-hold hover + (it does not — run_peak ~88°), nor that a gz "arming-check PASS" + means the quad hovers (the PASS is scoped to the startup handoff). + Full position-hold remains the residual, now localized to relay-pos + thrust + RC#3 for v0.19.10+. + tags: [falcon, milestone, v0.19.9, arming, sequencer, kani, falsification, landed] + fields: + release-target: "verified arming sequencer + position-hold tip-over localization" + bench-date: "2026-05-29" + verification: "Kani BMC (143 + 137 checks), inductive per tick — appropriate for the f32 tilt compare" + bench-result: "alt-rate 3/3 PASS ≈0.20 m (no regression); arming clean handoff; tip localized to RC#3" + falsification: "startup tip-over does not reproduce at spawn; tip is downstream (relay-pos thrust + RC#3)" + links: + - type: depends-on + target: FEAT-FALCON-v0.19.8 + - id: FEAT-FALCON-v1.0 type: feature title: "v1.0 — six-domain credit dossier + airframe variants" @@ -2168,4 +2240,4 @@ artifacts: - type: implements target: SYSREQ-FALCON-010 - type: depends-on - target: FEAT-FALCON-v0.19.8 + target: FEAT-FALCON-v0.19.9 diff --git a/artifacts/swreq/SWREQ-FALCON-ARM-P01.yaml b/artifacts/swreq/SWREQ-FALCON-ARM-P01.yaml new file mode 100644 index 0000000..6526fe5 --- /dev/null +++ b/artifacts/swreq/SWREQ-FALCON-ARM-P01.yaml @@ -0,0 +1,60 @@ +artifacts: + - id: SWREQ-FALCON-ARM-P01 + type: sw-req + title: "ARM-P01: Verified arming sequencer gates attitude-torque authority" + status: approved + description: > + The flight stack shall provide an arming sequencer + (`relay-arm::ArmingSequencer`) that gates attitude-torque + authority through an explicit startup state machine: + + Disarmed → SpinUp → LevelHold → Armed + + Attitude-torque authority (the rate loop's torque applied to the + mixer) shall be enabled ONLY in the Armed phase, and the Armed + phase shall be reachable ONLY through LevelHold after the measured + body tilt has stayed below a configured threshold for a configured + number of consecutive ticks (the level gate). Equivalently: + torque authority can never be granted unless the body was first + confirmed level. A NaN/non-finite tilt estimate shall never count + as level (it can never arm the vehicle). The collective-thrust + scale output shall be in [0, 1] and finite in every phase. + + Rationale: this replaces the pre-v0.19.9 bench's ad-hoc, time-only + `t < 0.5 s` spawn-hold, which engaged the rate loop at a fixed time + regardless of body state (a quad that spawned tilted, or was + bumped during spin-up, would have torque engaged into a bad + attitude). The sequencer gates on time AND confirmed-level, and + its gating is mechanically proven. It is the "deterministic + startup sequencing (arm → spin-up → release)" called for as fix + (1) in the v0.19.7 bistability diagnosis. + + Scope honesty: ARM-P01 is a startup-gating safety property. It is + NOT a position-hold guarantee. The v0.19.9 gz bench confirms the + gated startup hands off a level body (0° tilt through arming and + well beyond), but the full position-hold cascade still tips later + (~88° peak) due to a downstream residual (relay-pos thrust + normalisation + RC#3 EKF-attitude-during-accel). The arming + sequencer cannot fix a downstream cascade instability and does not + claim to; that residual is tracked for v0.19.10+. + tags: [falcon, arming, sequencer, safety, kani, startup] + fields: + req-type: safety + priority: must + verification-criteria: > + Kani BMC proof (the tilt compare is f32; like relay-mix-quad the + sequencer is off the integer-only Verus track): the torque-gating + invariant is inductive over one tick(), so the bounded check + covers all input sequences. Harnesses verify_arming_torque_gated + (torque_authority ⟺ phase==Armed; a new entry into Armed comes + only from LevelHold with the level gate satisfied; phase ∈ [0,3]; + thrust_scale ∈ [0,1] finite) and verify_arming_total (panic-free + + thrust bounded for NaN/∞ tilt and zero spin-up). Plus proptest + arm_p01_property_torque_implies_armed and unit tests + (torque-gated-until-level, tilt-resets-count, NaN-never-arms, + monotonic spin-up ramp). gz: alt-rate reliable hover unaffected + (3/3 PASS at ~0.20 m); arming-check clean handoff (0° through + arming). + links: + - type: derives-from + target: SYSREQ-FALCON-002 diff --git a/artifacts/verification/FV-FALCON-ARM-001.yaml b/artifacts/verification/FV-FALCON-ARM-001.yaml new file mode 100644 index 0000000..436f8d9 --- /dev/null +++ b/artifacts/verification/FV-FALCON-ARM-001.yaml @@ -0,0 +1,87 @@ +artifacts: + - id: FV-FALCON-ARM-001 + type: sw-verification + title: "ARM-P01 arming-sequencer gating — Kani BMC proof + gz handoff evidence + tip-over localization (v0.19.9)" + status: approved + description: > + Verifies SWREQ-FALCON-ARM-P01: the `relay-arm::ArmingSequencer` + gates attitude-torque authority so the rate loop's torque is + applied ONLY after a confirmed-level startup, never before. + + Why Kani, not Verus: the only float in the machine is the tilt + threshold compare; the rest is integer phase/counter logic. Like + relay-mix-quad (f32) this is plain Rust + Kani (CBMC) + proptest, + not the integer-only Verus track. The gating invariant is + INDUCTIVE over a single tick(), so Kani's bounded check (arbitrary + in-invariant state + arbitrary input, prove the invariant is + preserved) covers all input sequences of any length. + + Kani proof (cargo kani, crates/relay-arm/plain): + - verify_arming_torque_gated (143 checks, SUCCESSFUL): + torque_authority ⟺ phase==Armed; a NEW entry into Armed + comes only from LevelHold with level_count+1 ≥ required (the + RC#1-gate — torque can never engage before confirmed level); + phase ∈ [0,3]; thrust_scale ∈ [0,1] and finite. + - verify_arming_total (137 checks, SUCCESSFUL): tick() is + panic-free and thrust_scale stays in [0,1] finite even for + NaN/∞ tilt and a zero spin-up duration. + + Supporting evidence: + - proptest arm_p01_property_torque_implies_armed (across + arbitrary tick/arm sequences) + unit tests: torque-gated- + until-level-confirmed, tilt-resets-level-count, NaN-never-arms, + monotonic spin-up ramp. cargo test -p relay-arm → 5/5. + - body_tilt_rad geometry unit test (falcon-sitl-gz) → matches + atan2(|horizontal|,|vertical|) at 0/45/90°. + - gz bench (bench-evidence/gz-sim/2026-05-29-v0.19.9-*.md): + * REGRESSION GATE: --scenario=alt-rate, 3/3 consecutive PASS + at final_dist≈0.20 m, rms≈0.21 m / 30 s — the v0.19.8 + reliable hover is unaffected by wiring in the sequencer. + * --scenario=arming: clean gated handoff (handoff_peak=0.0° + through arming at 0.40 s); whole-run peak ~88° is the + DIAGNOSTIC of the downstream residual. + + Falsification finding (the empirical value of this bench): + The v0.19.7 "startup tip-over" (RC#1) does NOT reproduce as a + startup transient. The gated AND ungated startups both hand off a + level body (0° through arming); the body then holds level for + ~3.5 s and only tips once the position loop dominates. The + tip-over is therefore a DOWNSTREAM position-hold residual + (relay-pos thrust normalisation + RC#3 EKF-attitude-during-accel), + not a spawn transient. The arming sequencer is verified, correct, + and a prerequisite safety building block — but it is NOT what + unblocks full position-hold. This sharpens the v0.19.10+ target. + + Verification: + - cargo kani --harness verify_arming_torque_gated → SUCCESSFUL. + - cargo kani --harness verify_arming_total → SUCCESSFUL. + - cargo test -p relay-arm → 5/5. + - cargo test -p falcon-sitl-gz → 10/10. + - cargo test --workspace → green. + - gz bench → alt-rate 3/3 PASS; arming clean handoff. + - rivet validate → PASS. + + Honestly NOT claimed: + - That the arming sequencer makes full position-hold hover. It + does not (run_peak ~88°); the residual is downstream and + deferred to v0.19.10+. + - That a gz "arming-check PASS" means the quad hovers. The PASS + is explicitly scoped to the startup HANDOFF (label + "PASS ✓ (handoff)" + a NOTE reporting the whole-run tip). + - That Verus proves this. It can't (float tilt compare); Kani + does, BMC-complete over the bounded domain, inductive per tick. + tags: [falcon, arming, sequencer, kani, bmc, startup, falsification, v0.19.9] + fields: + runs: + - "cargo kani --harness verify_arming_torque_gated (143 checks, SUCCESSFUL)" + - "cargo kani --harness verify_arming_total (137 checks, SUCCESSFUL)" + - "cargo test -p relay-arm (5/5)" + - "cargo test -p falcon-sitl-gz (10/10)" + - "gz bench --scenario=alt-rate (3/3 PASS, final≈0.20 m — regression gate)" + - "gz bench --scenario=arming (clean handoff 0°; run_peak ~88° = downstream residual) # bench-only" + tool: "Kani (CBMC) — inductive single-tick proof; appropriate for the f32 tilt compare; Verus is integer-only here" + bench-result: "alt-rate 3/3 PASS final≈0.20 m (no regression); arming clean handoff, position-hold tip-over localized to RC#3" + falsification: "startup tip-over does not reproduce at spawn; tip is downstream (relay-pos thrust + RC#3)" + links: + - type: verifies + target: SWREQ-FALCON-ARM-P01 diff --git a/bench-evidence/gz-sim/2026-05-29-v0.19.9-arming-sequencer-and-tipover-localization.md b/bench-evidence/gz-sim/2026-05-29-v0.19.9-arming-sequencer-and-tipover-localization.md new file mode 100644 index 0000000..3fb719e --- /dev/null +++ b/bench-evidence/gz-sim/2026-05-29-v0.19.9-arming-sequencer-and-tipover-localization.md @@ -0,0 +1,101 @@ +# v0.19.9 — verified arming sequencer + tip-over localization + +Date: 2026-05-29 +Backend: Gazebo Harmonic (gz sim 8.11.0), headless `gz sim -s -r` +World: `examples/falcon-sitl-gz/worlds/falcon-quad.sdf` +Home: `--home=47.3977,8.5456,488` (matches the SDF `` +elevation — see the methodology note below; omitting it is a -488 m trap). + +## What shipped + +`relay-arm` — a Kani-verified arming state machine that gates +attitude-torque authority: + +``` +Disarmed ──arm──▶ SpinUp ──spinup_ticks──▶ LevelHold ──level gate──▶ Armed +(torque off, (thrust ramp, (thrust 1, (torque ON) + thrust 0) torque off) torque off, + count consecutive + below-θ-tilt ticks) +``` + +It replaces the bench's ad-hoc time-only `t < 0.5 s` spawn-hold (which +engaged the rate loop at a fixed time regardless of body state) with +time AND confirmed-level gating. Wired into the alt-rate path and the +full closed-loop cascade. + +## Verification (Kani — the real gate) + +The torque-gating invariant is **inductive over one `tick()`**, so a +bounded check covers all (unbounded-length) input sequences: + +| Harness | Checks | Result | +|---|---|---| +| `verify_arming_torque_gated` | 143 | SUCCESSFUL | +| `verify_arming_total` | 137 | SUCCESSFUL | + +`verify_arming_torque_gated` proves: `torque_authority ⟺ phase==Armed`; +a *new* entry into Armed comes only from LevelHold with the level gate +satisfied (torque can never engage before confirmed level); `phase ∈ +[0,3]`; `thrust_scale ∈ [0,1]` finite. `verify_arming_total` proves +panic-freedom + thrust bounds even for NaN/∞ tilt and zero spin-up. +Plus proptest + 5 unit tests. + +## gz bench + +### Regression gate — alt-rate reliable hover (must hold) + +| Run | final_dist | peak_dist | rms_steady (30 s) | +|---|---|---|---| +| 1 | 0.20 m | 1.98 m | 0.21 m | +| 2 | 0.20 m | 1.98 m | 0.21 m | +| 3 | 0.21 m | 1.98 m | 0.22 m | + +**3/3 PASS** — identical to v0.19.8's 4/4. Wiring in the sequencer does +not regress the reliable attitude+altitude hover. + +### Diagnostic — full-cascade arming + +``` +arming-check[gated]: armed_at=0.40s handoff_peak=0.0° run_peak=88.7° final=25.3° [PASS ✓ (handoff)] +arming-check[UNGATED]: armed_at=0.40s handoff_peak=0.0° run_peak=83.3° final=65.2° +``` + +Tilt trajectory (gated): `0.0°` from t=0 through t≈3.5 s, then +`1.3°→3.7°→7.2°→13.6°→21.6°→…→88°` while drifting horizontally to +[2.6, −2.9] m. + +## The falsification finding + +I set out to "kill RC#1 (startup tip-over)." **The data falsified that +framing.** The gated and ungated startups *both* hand off a perfectly +level body (0° through arming and for ~3.5 s after). The body tips only +once the position loop dominates. Therefore: + +- The tip-over is **not a startup transient** — it is a **downstream + position-hold residual**: `relay-pos` does not command enough thrust + to hold altitude on this 2 kg body (it sits, not climbs), and RC#3 + (EKF attitude during accel) corrupts the "level" reference once it + does move. +- The arming sequencer is **verified, correct, and a prerequisite + building block** (the v0.19.7 doc's recommended fix (1)), but it is + **not what unblocks position-hold**. It cannot fix a downstream + cascade instability and v0.19.9 does not claim it does. + +This is the methodology working as intended: publishing the +wrong-prediction data localizes the v0.19.10+ target far more precisely +than a fabricated "RC#1 fixed" would have. The next blocker is +**relay-pos thrust normalisation + RC#3 (EKF innovation gate / accel +down-weighting)** — the genuinely research-shaped work. + +## Methodology note — the −488 m NavSat trap + +First runs (mine) omitted `--home` and read `pos_d = −488 m` from t=0 +(the body appeared 488 m underground and inert). Falsified as +*environmental, not a code regression* by running the committed v0.19.8 +binary through the identical launch — it produced the same −488 m. Root +cause: the SDF world origin is at `488.0` (WGS84); +without `--home=lat,lon,488` the bridge anchors NavSat→NED at +`Home::ORIGIN` (alt 0), offsetting altitude by the full elevation. Every +prior PASS supplied `--home=47.3977,8.5456,488`; so must every future +run. Captured here so the trap is not re-stepped. diff --git a/crates/relay-arm/Cargo.toml b/crates/relay-arm/Cargo.toml new file mode 100644 index 0000000..b4334fd --- /dev/null +++ b/crates/relay-arm/Cargo.toml @@ -0,0 +1,23 @@ +[package] +name = "relay-arm" +description = "Relay arming sequencer — verified startup state machine that gates attitude-torque authority until the body is confirmed level + airborne (v0.19.9)" +version.workspace = true +edition.workspace = true +license.workspace = true +repository.workspace = true +rust-version.workspace = true + +[lib] +# Cargo tests + Kani use plain/ (no Verus tree: the sequencer's only +# float touch is the tilt-threshold compare, so it follows relay-mix-quad's +# plain+Kani+proptest posture rather than the integer-only Verus track). +path = "plain/src/lib.rs" +crate-type = ["rlib"] + +[dependencies] + +[dev-dependencies] +proptest.workspace = true + +[lints] +workspace = true diff --git a/crates/relay-arm/plain/src/lib.rs b/crates/relay-arm/plain/src/lib.rs new file mode 100644 index 0000000..c4b9625 --- /dev/null +++ b/crates/relay-arm/plain/src/lib.rs @@ -0,0 +1,345 @@ +//! Relay arming sequencer — verified startup state machine. +//! +//! Kills falcon root-cause #1 ("startup tip-over"): at spawn the body +//! is subject to rotor-imbalance + IMU transients. If the rate-PID is +//! allowed to demand attitude torque immediately, the mixer converts +//! those transients into bang-bang motor commands and the body tumbles +//! off-axis before steady state can establish. +//! +//! The pre-v0.19.9 bench worked around this with an ad-hoc `t < 0.5 s` +//! time gate that zeroed torque. That gate is *time-only*: it engages +//! the rate loop at t=0.5 s regardless of whether the body is actually +//! level. If the quad spawned tilted, or was bumped during spin-up, the +//! time gate engages torque into a bad attitude anyway. +//! +//! This sequencer replaces it with a state machine that gates torque +//! authority on **time AND confirmed-level**: +//! +//! ```text +//! Disarmed ──arm()──▶ SpinUp ──spinup_ticks──▶ LevelHold ──┐ +//! (thrust 0, (thrust ramp 0→1, (thrust 1, │ level_count +//! torque off) torque off) torque off) │ ≥ required +//! ▼ +//! Armed +//! (torque ON) +//! ``` +//! +//! ## Verified property (the RC#1 killer) +//! +//! **ARM-P01 (torque gating):** attitude-torque authority is enabled +//! *only* in `Armed`, and `Armed` is reachable *only* through `LevelHold` +//! after `level_count` reaches `level_ticks_required` consecutive +//! below-threshold-tilt ticks. Equivalently: `torque_authority` can never +//! be true unless the body was confirmed level. This is an **inductive** +//! invariant over a single `tick()`, so Kani's bounded check covers all +//! (unbounded-length) input sequences — see `kani_proofs`. +//! +//! Why Kani, not Verus: the only float in the machine is the tilt +//! threshold compare; the rest is integer phase/counter logic. Like +//! `relay-mix-quad`, this is plain Rust + Kani (CBMC bit-blasts the f32 +//! compare, NaN included) + proptest, not the integer-only Verus track. + +#![no_std] + +/// Phase encoded as a `u8` so the Kani invariant can reason about it as +/// a bounded integer (`phase <= ARMED`). Values are stable wire codes. +pub const DISARMED: u8 = 0; +pub const SPINUP: u8 = 1; +pub const LEVEL_HOLD: u8 = 2; +pub const ARMED: u8 = 3; + +/// Static configuration. The bench derives tick counts from its loop dt +/// (e.g. `spinup_ticks = round(0.3 s / dt)`). +#[derive(Clone, Copy)] +pub struct ArmingConfig { + /// Ticks spent ramping thrust 0→1 with torque OFF before the level + /// check begins. Lets the props reach idle without a jerk. + pub spinup_ticks: u32, + /// Consecutive below-threshold-tilt ticks required to confirm level + /// and transition LevelHold → Armed. + pub level_ticks_required: u32, + /// |tilt| (rad) must be strictly below this to count as a level tick. + pub tilt_thresh_rad: f32, +} + +impl ArmingConfig { + /// Defaults tuned for the falcon-quad bench at 100 Hz: + /// 0.3 s spin-up, 0.1 s (10 ticks) of confirmed level, 5° threshold. + pub const fn falcon_quad_100hz() -> Self { + ArmingConfig { spinup_ticks: 30, level_ticks_required: 10, tilt_thresh_rad: 0.087 } + } +} + +/// Per-tick output handed to the bench / cascade glue. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct ArmingOutput { + pub phase: u8, + /// `true` iff the rate loop's attitude torque may be applied. False + /// in every phase except `Armed`. + pub torque_authority: bool, + /// Collective-thrust scale in [0, 1]. Ramps during SpinUp, 1.0 after. + pub thrust_scale: f32, +} + +/// The arming state machine. `tick()` advances it one control step. +pub struct ArmingSequencer { + phase: u8, + /// Ticks elapsed in the current phase (drives the SpinUp ramp). + tick_in_phase: u32, + /// Consecutive below-threshold-tilt ticks seen in LevelHold. + level_count: u32, + cfg: ArmingConfig, +} + +impl ArmingSequencer { + pub fn new(cfg: ArmingConfig) -> Self { + ArmingSequencer { phase: DISARMED, tick_in_phase: 0, level_count: 0, cfg } + } + + pub fn phase(&self) -> u8 { self.phase } + pub fn level_count(&self) -> u32 { self.level_count } + + /// Advance one control tick. + /// + /// * `tilt_rad` — magnitude of the body tilt from vertical (rad). A + /// NaN/∞ tilt never counts as level (the `< thresh` compare is + /// false), so a degenerate estimate can never arm the vehicle. + /// * `arm_request` — operator/bench arm command. Moves Disarmed → + /// SpinUp; ignored in later phases (the machine latches forward). + pub fn tick(&mut self, tilt_rad: f32, arm_request: bool) -> ArmingOutput { + match self.phase { + DISARMED => { + if arm_request { + self.phase = SPINUP; + self.tick_in_phase = 0; + } + } + SPINUP => { + self.tick_in_phase = self.tick_in_phase.saturating_add(1); + if self.tick_in_phase >= self.cfg.spinup_ticks { + self.phase = LEVEL_HOLD; + self.tick_in_phase = 0; + self.level_count = 0; + } + } + LEVEL_HOLD => { + // NaN-safe: `< thresh` is false for NaN/∞ → resets count. + if tilt_rad.abs() < self.cfg.tilt_thresh_rad { + self.level_count = self.level_count.saturating_add(1); + } else { + self.level_count = 0; + } + if self.level_count >= self.cfg.level_ticks_required { + // The ONLY edge into Armed — guarded by the level gate. + self.phase = ARMED; + } + } + _ => { /* ARMED (and any out-of-range code) latches. */ } + } + + ArmingOutput { + phase: self.phase, + torque_authority: self.phase == ARMED, + thrust_scale: self.thrust_scale(), + } + } + + /// Collective-thrust scale for the current phase, clamped to [0, 1] + /// and total (never NaN/∞, even if `spinup_ticks == 0`). + fn thrust_scale(&self) -> f32 { + match self.phase { + DISARMED => 0.0, + SPINUP => { + if self.cfg.spinup_ticks == 0 { + 1.0 + } else { + let r = self.tick_in_phase as f32 / self.cfg.spinup_ticks as f32; + // clamp also maps a non-finite r (impossible here, but + // keeps the function total for the Kani total harness). + if r.is_nan() { 0.0 } else { r.clamp(0.0, 1.0) } + } + } + _ => 1.0, // LevelHold, Armed (and latched codes): full thrust. + } + } +} + +#[cfg(kani)] +mod kani_proofs { + use super::*; + + /// Build an arbitrary *reachable-shaped* sequencer state and config. + /// We bound the config to sane ranges (Kani needs finite domains) and + /// constrain the state to satisfy the inductive invariant on entry — + /// then prove `tick()` preserves it. Induction over ticks then covers + /// all input sequences of any length. + fn arbitrary() -> (ArmingSequencer, f32, bool) { + let phase: u8 = kani::any(); + kani::assume(phase <= ARMED); + let spinup_ticks: u32 = kani::any(); + kani::assume(spinup_ticks <= 1000); + let level_ticks_required: u32 = kani::any(); + kani::assume(level_ticks_required >= 1 && level_ticks_required <= 1000); + let tilt_thresh_rad: f32 = kani::any(); + kani::assume(tilt_thresh_rad.is_finite() && tilt_thresh_rad > 0.0); + + let tick_in_phase: u32 = kani::any(); + kani::assume(tick_in_phase <= 2000); + let level_count: u32 = kani::any(); + // Inductive precondition: not already past the gate while in + // LevelHold (else we'd already be Armed). + kani::assume(level_count < level_ticks_required); + + let cfg = ArmingConfig { spinup_ticks, level_ticks_required, tilt_thresh_rad }; + let seq = ArmingSequencer { phase, tick_in_phase, level_count, cfg }; + (seq, kani::any(), kani::any()) + } + + /// ARM-P01 — torque gating + phase bounds + thrust totality, proved + /// inductive over one tick. + #[kani::proof] + fn verify_arming_torque_gated() { + let (mut seq, tilt, arm_req) = arbitrary(); + let prev_phase = seq.phase; + let prev_level = seq.level_count; + let req = seq.cfg.level_ticks_required; + + let out = seq.tick(tilt, arm_req); + + // Output torque authority is exactly "we are Armed". + assert!(out.torque_authority == (seq.phase == ARMED)); + assert!(out.phase == seq.phase); + // Phase stays in range. + assert!(seq.phase <= ARMED); + // RC#1 killer: a *new* entry into Armed can come ONLY from + // LevelHold, and only once the level gate is satisfied. + if prev_phase != ARMED && seq.phase == ARMED { + assert!(prev_phase == LEVEL_HOLD); + assert!(prev_level + 1 >= req); + } + // Thrust scale is always a valid, finite collective in [0,1]. + assert!(out.thrust_scale.is_finite()); + assert!(out.thrust_scale >= 0.0 && out.thrust_scale <= 1.0); + } + + /// Totality — `tick()` is panic-free and thrust stays bounded even + /// for NaN/∞ tilt and a zero spin-up duration. + #[kani::proof] + fn verify_arming_total() { + let phase: u8 = kani::any(); + let spinup_ticks: u32 = kani::any(); + kani::assume(spinup_ticks <= 1000); + let level_ticks_required: u32 = kani::any(); + kani::assume(level_ticks_required <= 1000); + let tilt_thresh_rad: f32 = kani::any(); + let cfg = ArmingConfig { spinup_ticks, level_ticks_required, tilt_thresh_rad }; + let tick_in_phase: u32 = kani::any(); + let level_count: u32 = kani::any(); + let mut seq = ArmingSequencer { phase, tick_in_phase, level_count, cfg }; + + let out = seq.tick(kani::any(), kani::any()); // tilt may be NaN/∞ + assert!(out.thrust_scale.is_finite()); + assert!(out.thrust_scale >= 0.0 && out.thrust_scale <= 1.0); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn cfg() -> ArmingConfig { + ArmingConfig { spinup_ticks: 5, level_ticks_required: 3, tilt_thresh_rad: 0.087 } + } + + /// ARM-P01: torque authority is never granted before the level gate. + #[test] + fn arm_p01_torque_gated_until_level_confirmed() { + let mut s = ArmingSequencer::new(cfg()); + // Disarmed: torque off no matter what. + let o = s.tick(0.0, false); + assert_eq!(o.phase, DISARMED); + assert!(!o.torque_authority); + + // Arm → SpinUp; torque stays off through the whole ramp. + for _ in 0..5 { + let o = s.tick(0.0, true); + assert!(!o.torque_authority, "torque must stay off during spin-up"); + assert_eq!(o.phase, SPINUP); + } + // Next tick enters LevelHold (still no torque). + let o = s.tick(0.0, true); + assert_eq!(o.phase, LEVEL_HOLD); + assert!(!o.torque_authority); + + // Three consecutive level ticks confirm → Armed on the 3rd. + s.tick(0.0, true); // level_count 1 + s.tick(0.0, true); // level_count 2 + let o = s.tick(0.0, true); // level_count 3 == required → Armed + assert_eq!(o.phase, ARMED); + assert!(o.torque_authority, "torque engages only after level confirmed"); + } + + /// A tilt above threshold during LevelHold resets the counter, so the + /// vehicle cannot arm while it is not level — the property the + /// time-only `t < 0.5` hack could not provide. + #[test] + fn arm_p01_tilt_resets_level_count_prevents_arming() { + let mut s = ArmingSequencer::new(cfg()); + s.tick(0.0, true); + for _ in 0..5 { s.tick(0.0, true); } // reach LevelHold + assert_eq!(s.phase(), LEVEL_HOLD); + + s.tick(0.0, true); // count 1 + s.tick(0.0, true); // count 2 + // A tilt spike (10°, above the 5° threshold) wipes the progress. + let o = s.tick(0.2, true); + assert_eq!(s.level_count(), 0, "tilt must reset the level counter"); + assert!(!o.torque_authority); + assert_eq!(o.phase, LEVEL_HOLD); + } + + /// NaN tilt can never confirm level — a degenerate estimate must not + /// arm the vehicle. + #[test] + fn arm_p01_nan_tilt_never_arms() { + let mut s = ArmingSequencer::new(cfg()); + s.tick(0.0, true); + for _ in 0..5 { s.tick(0.0, true); } + for _ in 0..100 { + let o = s.tick(f32::NAN, true); + assert!(!o.torque_authority, "NaN tilt must never arm"); + } + } + + /// Thrust ramps monotonically 0→1 during spin-up and holds at 1. + #[test] + fn spinup_thrust_ramps_monotonic() { + let mut s = ArmingSequencer::new(cfg()); + let mut last = -1.0_f32; + s.tick(0.0, true); // enter SpinUp (tick_in_phase becomes 1) + for _ in 0..5 { + let o = s.tick(0.0, true); + assert!(o.thrust_scale >= last, "thrust scale must not decrease in spin-up"); + assert!((0.0..=1.0).contains(&o.thrust_scale)); + last = o.thrust_scale; + } + } + + proptest::proptest! { + /// Across arbitrary tick sequences, torque authority implies the + /// machine is in Armed, and thrust_scale stays a finite [0,1]. + #[test] + fn arm_p01_property_torque_implies_armed( + tilts in proptest::collection::vec(-1.0_f32..1.0, 0..400), + arm_at in 0usize..50, + ) { + let mut s = ArmingSequencer::new(cfg()); + for (i, &tilt) in tilts.iter().enumerate() { + let o = s.tick(tilt, i >= arm_at); + proptest::prop_assert!(o.thrust_scale.is_finite()); + proptest::prop_assert!((0.0..=1.0).contains(&o.thrust_scale)); + proptest::prop_assert_eq!(o.torque_authority, o.phase == ARMED); + } + } + } +} diff --git a/examples/falcon-sitl-gz/Cargo.toml b/examples/falcon-sitl-gz/Cargo.toml index b8a9ed8..b608965 100644 --- a/examples/falcon-sitl-gz/Cargo.toml +++ b/examples/falcon-sitl-gz/Cargo.toml @@ -27,6 +27,7 @@ relay-ekf = { path = "../../crates/relay-ekf" } relay-rate = { path = "../../crates/relay-rate" } relay-att = { path = "../../crates/relay-att" } relay-mix-quad = { path = "../../crates/relay-mix-quad" } +relay-arm = { path = "../../crates/relay-arm" } relay-pos = { path = "../../crates/relay-pos" } relay-lc = { path = "../../crates/relay-lc" } relay-sc = { path = "../../crates/relay-sc" } diff --git a/examples/falcon-sitl-gz/src/main.rs b/examples/falcon-sitl-gz/src/main.rs index c728f2c..927ea31 100644 --- a/examples/falcon-sitl-gz/src/main.rs +++ b/examples/falcon-sitl-gz/src/main.rs @@ -20,6 +20,7 @@ mod physics; use physics::{GazeboPhysics, MockPhysics, Physics}; +use relay_arm::{ArmingConfig, ArmingSequencer, ARMED}; use relay_att::{AttController, Timestamp as AttTimestamp}; use relay_ekf::{Ekf, ImuSample, Timestamp as EkfTimestamp}; use relay_mix_quad::QuadMixer; @@ -133,6 +134,18 @@ fn frame_correct_torque(torque_ned: [f32; 3]) -> [f32; 3] { ] } +/// Body tilt from vertical (rad), estimated from the specific-force +/// (accelerometer) vector. At rest / steady hover the accel measures the +/// reaction to gravity along body-down; the tilt is the angle between +/// that vector and the body-z axis: `atan2(|horizontal|, |vertical|)`. +/// Convention-agnostic (works for either sign of az) and total — a +/// degenerate all-zero sample yields 0, which the sequencer treats as +/// level only if it persists (the rate loop is still gated by spin-up). +fn body_tilt_rad(accel_body: [f32; 3]) -> f32 { + let horiz = libm::sqrtf(accel_body[0] * accel_body[0] + accel_body[1] * accel_body[1]); + libm::atan2f(horiz, accel_body[2].abs()) +} + fn run_scenario( physics: &mut dyn Physics, scenario: &str, @@ -147,6 +160,8 @@ fn run_scenario( "frame-roll" => run_frame_check(physics, 0, duration_s), "frame-pitch" => run_frame_check(physics, 1, duration_s), "frame-yaw" => run_frame_check(physics, 2, duration_s), + "arming" => run_arming_check(physics, duration_s, true), + "arming-ungated" => run_arming_check(physics, duration_s, false), other => { eprintln!( " scenario {other} not yet wired; falling back to closed-loop hover", @@ -218,6 +233,146 @@ fn run_frame_check(physics: &mut dyn Physics, axis: usize, duration_s: f32) -> b agrees } +/// v0.19.9 — arming-sequencer ORACLE + position-hold DIAGNOSTIC. +/// +/// Runs the verified `ArmingSequencer` ahead of the full cascade against +/// gz physics. Two distinct questions, deliberately separated: +/// +/// 1. **Does the sequencer deliver a clean handoff?** (its job, gated on) +/// PASS = it armed (level gate cleared) and tilt stayed below the +/// tumble threshold (30°) through the handoff window (spawn → 1.5 s +/// past arming). Empirically the gated startup holds 0° well past +/// arming — the sequencer does its job. +/// +/// 2. **Does full position-hold then hover?** (diagnostic, NOT gated on) +/// `run_peak` (whole-run peak tilt) is reported as a diagnostic. It +/// is large (~88°): the body holds level for ~3.5 s then tips and +/// drifts once the position loop dominates. That is the documented +/// downstream residual — `relay-pos` thrust normalisation + RC#3 +/// (EKF attitude during accel) — NOT a startup-gating failure. The +/// arming sequencer cannot fix a downstream cascade instability, and +/// v0.19.9 does not claim it does. This is the falsification finding +/// that localises the v0.19.10+ target. +/// +/// `gated`: when false, torque authority is forced on from t=0 (the +/// pre-v0.19.9 behaviour). The `arming-ungated` baseline confirms the +/// startup transient itself is benign in the nominal (level) gz spawn — +/// i.e. the tip-over is genuinely downstream, not at the spawn. +fn run_arming_check(physics: &mut dyn Physics, duration_s: f32, gated: bool) -> bool { + const TUMBLE_RAD: f32 = 0.52; // ~30° — past this the body has tipped + // Full cascade — this is where RC#1 lives: relay-pos sees the large + // initial position error at spawn and commands an aggressive attitude + // setpoint that ATT+RATE chase before the body is stable, tumbling it. + let mut ekf = Ekf::new(); + let mut rate_pid = RatePid::new(); + let mut att = AttController::new(); + let mut pos = PosController::new(); + let mut mixer = QuadMixer::new(); + let mut seq = ArmingSequencer::new(ArmingConfig::falcon_quad_100hz()); + let setpoint_ned = [0.0_f32, 0.0, -2.0]; + let setpoint = PositionSetpoint::hover_at(setpoint_ned); + let dt = 0.01_f32; + let n = (duration_s / dt) as u32; + let tick_period = Duration::from_secs_f32(dt); + let pace_real_time = physics.counters().is_some(); + + let mut peak_tilt = 0.0_f32; + let mut peak_tilt_handoff = 0.0_f32; + let mut armed_at: Option = None; + let mut last_tilt = 0.0_f32; + let mut last_pos_ned: Option<[f32; 3]> = None; + + let started_at = Instant::now(); + for step in 0..n { + let tick_start = Instant::now(); + let t = step as f32 * dt; + let (mut imu_sample, pos_ned) = physics.measure(0.0); + imu_sample.time = ekf_ts_of(t); + + let est = ekf.tick(imu_sample); + let v_ned = match last_pos_ned { + Some(p) => [ + (pos_ned[0] - p[0]) / dt, + (pos_ned[1] - p[1]) / dt, + (pos_ned[2] - p[2]) / dt, + ], + None => [0.0; 3], + }; + last_pos_ned = Some(pos_ned); + let att_sp = pos.tick(pos_ts_of(t), pos_ned, v_ned, est.quaternion, setpoint); + let rate_sp = att.tick(att_ts_of(t), est.quaternion, att_sp.quaternion); + + let tilt = body_tilt_rad(imu_sample.accel_body); + last_tilt = tilt; + if tilt > peak_tilt { peak_tilt = tilt; } + let arm = seq.tick(tilt, true); + if arm.phase == ARMED && armed_at.is_none() { + armed_at = Some(t); + } + // Handoff window = spawn → 1.5 s past arming: the interval the + // sequencer is responsible for (get airborne + hand off level). + // Tilt AFTER this is the downstream position-loop residual, NOT + // a startup-gating failure, so it is reported but not gated on. + if armed_at.map(|a| t <= a + 1.5).unwrap_or(true) && tilt > peak_tilt_handoff { + peak_tilt_handoff = tilt; + } + // `gated`: the sequencer decides torque authority. Ungated + // baseline: torque from t=0 (pre-v0.19.9), thrust_scale forced 1. + let authority = if gated { arm.torque_authority } else { true }; + let scale = if gated { arm.thrust_scale } else { 1.0 }; + let torque_raw = if authority { + rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, rate_sp) + } else { + [0.0_f32; 3] + }; + let torque = frame_correct_torque(torque_raw); + let motors = + mixer.mix_thrust_floor(torque, att_sp.thrust * scale, 0.5 * scale); + physics.step(motors, dt); + + if pace_real_time { + let used = tick_start.elapsed(); + if used < tick_period { + std::thread::sleep(tick_period - used); + } + } + } + let wall = started_at.elapsed(); + let armed = armed_at.is_some(); + // The sequencer's responsibility is the HANDOFF: arm only after a + // confirmed-level window and deliver a level body to the cascade. + // PASS gates on the handoff window only. `peak_tilt` (whole run) is + // reported as a DIAGNOSTIC of the downstream position-hold residual + // (relay-pos thrust + RC#3 EKF-during-accel) — out of v0.19.9 scope. + let clean_handoff = peak_tilt_handoff < TUMBLE_RAD; + // Honest scope: PASS means the sequencer delivered a clean handoff. + // The gated and ungated handoffs are BOTH clean (the nominal level + // spawn is benign with or without torque gating) — so this is not a + // gated-vs-ungated contrast; it is the finding that the tip-over is + // downstream, not at startup. The label says "(handoff)" and the + // NOTE reports the whole-run tip so PASS is never read as "hovers". + let pass = armed && clean_handoff; + println!( + " arming-check[{}]: armed_at={} handoff_peak={:.1}° run_peak={:.1}° final={:.1}° (tumble>{:.0}°) [{}] wall={:.2}s", + if gated { "gated" } else { "UNGATED-baseline" }, + armed_at.map(|t| format!("{t:.2}s")).unwrap_or_else(|| "NEVER".into()), + peak_tilt_handoff.to_degrees(), + peak_tilt.to_degrees(), + last_tilt.to_degrees(), + TUMBLE_RAD.to_degrees(), + if pass { "PASS ✓ (handoff)" } else { "FAIL ✗" }, + wall.as_secs_f32(), + ); + if pass && peak_tilt >= TUMBLE_RAD { + println!( + " NOTE: clean gated handoff, but the body tips later (run_peak={:.0}°) — \ + downstream position-hold residual (relay-pos thrust + RC#3), tracked for v0.19.10+.", + peak_tilt.to_degrees(), + ); + } + pass +} + /// v0.19.5 — alt-only thrust + rate-pid attitude damping (zero rate /// setpoint = "stay level"). The minimal closed-loop hover: P+D /// altitude controller for thrust + relay-rate's verified PID for @@ -232,6 +387,10 @@ fn run_alt_rate_hover( ) -> bool { let mut rate_pid = RatePid::new(); let mut mixer = QuadMixer::new(); + // v0.19.9 — verified arming sequencer replaces the old `t < 0.5` + // spawn-hold. Torque authority engages ONLY after spin-up + a + // confirmed-level window (Kani-proven gating, relay-arm ARM-P01). + let mut seq = ArmingSequencer::new(ArmingConfig::falcon_quad_100hz()); let setpoint_d = -2.0_f32; let hover_thrust = 0.72_f32; let kp_alt = 0.05_f32; @@ -268,17 +427,18 @@ fn run_alt_rate_hover( let alt_err = setpoint_d - pos_ned[2]; let thrust = (hover_thrust - kp_alt * alt_err + kd_alt * v_d_filt).clamp(0.0, 1.0); - // v0.19.5 — first 0.5 s is a "spawn hold": uniform thrust, - // no torque. Without this the rate-pid responds to spawn - // transients (rotor imbalance + IMU noise) by demanding - // torque, which the mixer's priority-preserving saturation - // then converts into bang-bang motors → body tumbles off - // axis before steady-state can establish. After the hold, - // rate-pid takes over with the body already airborne + level. - let torque_raw = if t < 0.5 { - [0.0_f32; 3] - } else { + // v0.19.9 — verified arming sequencer (relay-arm). Replaces the + // old time-only `t < 0.5` spawn-hold: torque authority engages + // ONLY after spin-up AND a confirmed-level window, so spawn + // transients (rotor imbalance + IMU noise) can never be converted + // into bang-bang torque before steady state establishes (RC#1). + // Gating is Kani-proven (ARM-P01); here it runs against gz physics. + let tilt = body_tilt_rad(imu_sample.accel_body); + let arm = seq.tick(tilt, true); + let torque_raw = if arm.torque_authority { rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, [0.0_f32; 3]) + } else { + [0.0_f32; 3] }; // v0.19.6 — frame-correction is identity now that the SDF // rotor index map is aligned to MIXER_X. The v0.19.5 negation @@ -291,7 +451,11 @@ fn run_alt_rate_hover( // so the rate loop's attitude torque can no longer steal lift // — the v0.19.7 altitude limit-cycle root cause. The 0.88 // thrust clamp above is now redundant but harmless. - let motors = mixer.mix_thrust_floor(torque, thrust, 0.5); + // v0.19.9 — the arming sequencer ramps collective (and the floor) + // during spin-up so the props reach idle smoothly; thrust_scale + // is 1.0 once armed, recovering the v0.19.8 behaviour exactly. + let motors = + mixer.mix_thrust_floor(torque, thrust * arm.thrust_scale, 0.5 * arm.thrust_scale); physics.step(motors, dt); let dist = alt_err.abs(); @@ -472,6 +636,10 @@ fn run_closed_loop_hover( let mut att = AttController::new(); let mut pos = PosController::new(); let mut mixer = QuadMixer::new(); + // v0.19.9 — arming sequencer gates the rate loop here too. The full + // cascade had no spawn-hold at all (torque from t=0), so it was the + // clearest RC#1 victim. + let mut seq = ArmingSequencer::new(ArmingConfig::falcon_quad_100hz()); let setpoint_ned = [0.0_f32, 0.0, -2.0]; let setpoint = PositionSetpoint::hover_at(setpoint_ned); @@ -532,15 +700,29 @@ fn run_closed_loop_hover( // 4. ATT — quaternion error → rate setpoint. let current_rate_sp = att.tick(att_ts_of(t), est.quaternion, current_att_sp); - // 5. RATE — gyro + rate setpoint → torque (frame-corrected). - let torque_raw = rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, current_rate_sp); + // 5. RATE — gyro + rate setpoint → torque (frame-corrected), + // gated by the arming sequencer (RC#1). Torque authority + // engages only after spin-up + confirmed level. + let tilt = body_tilt_rad(imu_sample.accel_body); + let arm = seq.tick(tilt, true); + let torque_raw = if arm.torque_authority { + rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, current_rate_sp) + } else { + [0.0_f32; 3] + }; let torque = frame_correct_torque(torque_raw); for k in 0..3 { if !torque[k].is_finite() { nan_seen = true; } } - // 6. MIX — torque + thrust → 4× motor PWM. - let motors = mixer.mix(torque, current_thrust); + // 6. MIX — torque + thrust → 4× motor PWM. v0.19.9 carries the + // v0.19.8 verified thrust-floor into the full cascade and + // ramps collective + floor during spin-up. + let motors = mixer.mix_thrust_floor( + torque, + current_thrust * arm.thrust_scale, + 0.5 * arm.thrust_scale, + ); if motors.iter().any(|v| !v.is_finite()) { nan_seen = true; } // 7. Publish to the bridge. @@ -865,6 +1047,20 @@ mod tests { assert!(pass, "70 % PWM × THRUST_SCALE should easily beat gravity"); } + /// v0.19.9 — `body_tilt_rad` reads 0 when the specific-force vector + /// is purely vertical (level) and grows toward 90° as it tilts into + /// the horizontal plane. Convention-agnostic in the sign of az. + #[test] + fn body_tilt_rad_matches_geometry() { + // Level: gravity reaction straight along body-z (either sign). + assert!(body_tilt_rad([0.0, 0.0, -9.81]).abs() < 1e-4); + assert!(body_tilt_rad([0.0, 0.0, 9.81]).abs() < 1e-4); + // 45°: equal horizontal and vertical components. + assert!((body_tilt_rad([9.81, 0.0, 9.81]) - std::f32::consts::FRAC_PI_4).abs() < 1e-4); + // Fully horizontal specific force → 90° tilt. + assert!((body_tilt_rad([9.81, 0.0, 0.0]) - std::f32::consts::FRAC_PI_2).abs() < 1e-4); + } + /// v0.19.6 — the frame-correction adapter is identity. Pins the /// oracle finding: once the SDF rotor index→position+spin map is /// aligned to relay-mix-quad's MIXER_X convention, +torque produces