From 2a0349d3458cfba08b59fbde9ad068c89b721ab8 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Wed, 16 Sep 2026 12:55:52 +0200 Subject: [PATCH 1/2] fix(ci): the required Format gate has never been able to fail (#409) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `Format` is a required status check. Its only substantive step carried `continue-on-error: true` from d30a5e3 (2026-05-18, the v0.1.0 release), so a failing `cargo fmt --all -- --check` left the job `success` — for its entire life, about 137 releases. The mask was hiding a LIVE violation, not merely lost potency: on main 3ec4a2f the step logged 893 `Diff in` hunks and `exit code 1` while the job reported success. The jobs API reports such a step as `success` too; only the log shows it. An earlier draft of this change (and a months-old working note) blamed local-vs-CI rustfmt divergence and told readers NOT to run cargo fmt. There was no divergence — the locally generated reformat passes the unmasked check — and that advice is removed. The correction is kept in FV-RELAY-VGATE-004 because it is the same error the mask produces. Also: FV-RELAY-VGATE-004 `method: test` -> `automated-test` (schema value). Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01HvusAXYbHLyv3uTzfBcMbG --- .github/workflows/ci.yml | 15 ++++- .../verification/FV-RELAY-VGATE-004.yaml | 60 +++++++++++++++++++ 2 files changed, 72 insertions(+), 3 deletions(-) create mode 100644 artifacts/verification/FV-RELAY-VGATE-004.yaml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 435d7c57..40faca75 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -42,10 +42,19 @@ jobs: - uses: dtolnay/rust-toolchain@stable with: components: rustfmt - # Pre-existing cFS crates emit rustfmt deltas; v0.1 tolerates - # them while v0.2 cleans the workspace up. + # `Format` is a REQUIRED status check. It carried `continue-on-error: true` + # from d30a5e3 (2026-05-18, the v0.1.0 release) until v1.139 — so for its + # entire life it could not report failure, and it was promoted to required + # while in that state. The comment that stood here said "v0.1 tolerates + # them while v0.2 cleans the workspace up"; we were at v1.138. + # + # The mask WAS hiding a live violation. On main 3ec4a2f this step logged + # 893 `Diff in` hunks and `exit code 1` while the job reported success — + # and the jobs API reports a continue-on-error step as `success` too, so + # only the log tells. An earlier note here blamed local-vs-CI rustfmt + # divergence; there was none: a local `cargo fmt --all` passes this check. + # Run it. (#409) - run: cargo fmt --all -- --check - continue-on-error: true clippy: name: Clippy diff --git a/artifacts/verification/FV-RELAY-VGATE-004.yaml b/artifacts/verification/FV-RELAY-VGATE-004.yaml new file mode 100644 index 00000000..a9d1b021 --- /dev/null +++ b/artifacts/verification/FV-RELAY-VGATE-004.yaml @@ -0,0 +1,60 @@ +artifacts: + - id: FV-RELAY-VGATE-004 + type: sw-verification + title: "VGATE-P04 — the required Format gate can report failure again (v1.139)" + status: implemented + release: falcon-v1.139.0 + description: > + Partial verification of SWREQ-RELAY-VGATE-P04, for one instance of the + defect class it names: a gate that is wired, cited and required, and + cannot execute a failing verdict. + + WHAT WAS WRONG. `Format` is one of the required status checks on `main`. + Its only substantive step carried `continue-on-error: true`, so a failing + `cargo fmt --all -- --check` left the job conclusion `success`. It was + structurally incapable of going red, and it was promoted to a REQUIRED + context while in that state. + + `git log -S` dates the flag to d30a5e3 (2026-05-18) — the v0.1.0 release. + It was inert for its entire life, roughly 137 releases. The comment beside + it read "v0.1 tolerates them while v0.2 cleans the workspace up"; the repo + was at v1.138 when this was found. + + WHAT THE MASK WAS HIDING — a live violation, not merely lost potency. + CI's own rustfmt on main 3ec4a2f (job 104933185124) logged 893 `Diff in` + hunks and `Process completed with exit code 1`, and the job reported + `success`. The jobs API reported the step as `success` as well: + continue-on-error rewrites a failed step's conclusion, and only the log + shows the failure. This change's 106-file reformat is the debt the mask + hid. + + A CORRECTION, KEPT BECAUSE IT IS THE SAME ERROR. The first draft of this + artifact said the opposite: that CI's rustfmt reported success on every + recent run, and that ~891 local hunks were "the known local-vs-CI rustfmt + divergence". That draft read the masked JOB conclusion as the STEP + result — the exact confusion the mask produces. There was no divergence: + the reformat generated by a local `cargo fmt --all` passed this change's + unmasked check, and the months-old "divergence" belief had the same root. + + WHY IT BELONGS TO VGATE-P04. Branch protection asserts that a set of + checks gates `main`. An assessor reading that list would conclude + formatting is enforced. It was not, and had not been since the first + release. That is the same shape as a verification track that cannot run: + wired correctly, cited correctly, and incapable of saying no. + + NOT CLAIMED: this artifact covers the Format gate only. The other + instances VGATE-P04 names — the Verus track that cannot find core/std + (#405), the Verus PR trigger filtered on Lean paths (#418), and the + verification gate's missing main backstop (#410) — are open. + tags: [verification, relay, verification-gate, ci, v1.139] + fields: + method: automated-test + steps: + # The flag is gone. A grep is the right assertion here: the property IS + # the absence of a line in the workflow definition. + - run: "! grep -A1 'cargo fmt --all' .github/workflows/ci.yml | grep -q 'continue-on-error'" + # ...and the step still exists, so the fix cannot be 'delete the check'. + - run: "grep -q 'cargo fmt --all -- --check' .github/workflows/ci.yml" + links: + - type: verifies + target: SWREQ-RELAY-VGATE-P04 From fd4a18f7616605c4197dd9ad80d1600bf36b7d73 Mon Sep 17 00:00:00 2001 From: Ralf Anton Beier Date: Wed, 16 Sep 2026 23:23:41 +0200 Subject: [PATCH 2/2] =?UTF-8?q?style:=20rustfmt=20the=20tree=20=E2=80=94?= =?UTF-8?q?=20the=20debt=20the=20Format=20mask=20was=20hiding=20(#409)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `cargo fmt --all`, regenerated on main a1b0e17 (after #430 and #431) rather than hand-merged: the same 106 files as the first generation, plus the new lines #430/#431 added to four of them. `cargo fmt --all -- --check` exits 0. No hand edits. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01HvusAXYbHLyv3uTzfBcMbG --- .../benches/cascade_throughput.rs | 45 +- .../benches/engine_throughput.rs | 13 +- crates/falcon-baromag/src/lib.rs | 64 +- .../falcon-core/plain/src/blackbox_backend.rs | 13 +- crates/falcon-core/plain/src/lib.rs | 773 ++++++++++++++---- crates/falcon-core/plain/src/tuning.rs | 37 +- crates/falcon-esc-dshot/src/lib.rs | 43 +- crates/falcon-gnss-ubx/src/dual.rs | 95 ++- crates/falcon-gnss-ubx/src/lib.rs | 30 +- crates/falcon-hitl/src/lib.rs | 17 +- crates/falcon-imu-icm42688/src/lib.rs | 40 +- crates/falcon-param/plain/src/lib.rs | 38 +- crates/relay-adrc/plain/src/lib.rs | 157 +++- crates/relay-arm/plain/src/lib.rs | 71 +- crates/relay-att/plain/src/lib.rs | 72 +- crates/relay-avoid/plain/src/lib.rs | 24 +- crates/relay-batt/plain/src/kani_proofs.rs | 2 +- crates/relay-batt/plain/src/lib.rs | 45 +- crates/relay-calib/plain/src/flow.rs | 92 ++- crates/relay-calib/plain/src/lib.rs | 125 ++- crates/relay-ccsds/plain/src/engine.rs | 6 +- crates/relay-ccsds/plain/src/sensor_wire.rs | 48 +- crates/relay-cfdp/plain/src/engine.rs | 52 +- crates/relay-ci/plain/src/engine.rs | 23 +- crates/relay-cs/plain/src/engine.rs | 20 +- crates/relay-dronecan/plain/src/crc.rs | 6 +- crates/relay-dronecan/plain/src/dsdl.rs | 8 +- crates/relay-dronecan/plain/src/float16.rs | 6 +- crates/relay-dronecan/plain/src/id.rs | 19 +- .../relay-dronecan/plain/src/kani_proofs.rs | 17 +- crates/relay-dronecan/plain/src/msg.rs | 5 +- crates/relay-dronecan/plain/src/node.rs | 22 +- crates/relay-dronecan/plain/src/sensors.rs | 4 +- crates/relay-dronecan/plain/src/transfer.rs | 76 +- crates/relay-ds/plain/src/engine.rs | 18 +- crates/relay-ekf-stub/plain/src/lib.rs | 46 +- crates/relay-ekf/plain/src/lib.rs | 51 +- crates/relay-flowrange/plain/src/lib.rs | 13 +- crates/relay-flowrange/plain/src/tf02.rs | 46 +- crates/relay-fm/plain/src/engine.rs | 109 ++- crates/relay-fsafe/plain/src/lib.rs | 50 +- crates/relay-fsm/plain/src/lib.rs | 89 +- crates/relay-geo/plain/src/lib.rs | 259 ++++-- crates/relay-hk/plain/src/engine.rs | 52 +- crates/relay-hs/plain/src/engine.rs | 35 +- crates/relay-iekf/plain/src/lib.rs | 387 +++++++-- crates/relay-lc-diff/src/lib.rs | 2 +- crates/relay-lc/plain/src/c_api.rs | 5 +- crates/relay-lc/plain/src/engine.rs | 296 ++++++- crates/relay-log/plain/src/blackbox.rs | 13 +- crates/relay-log/plain/src/kani_proofs.rs | 11 +- crates/relay-log/plain/src/lib.rs | 50 +- crates/relay-math/src/lib.rs | 60 +- crates/relay-mavlink/plain/src/kani_proofs.rs | 6 +- crates/relay-mavlink/plain/src/param.rs | 10 +- crates/relay-mavlink/plain/src/telemetry.rs | 5 +- .../plain/src/telemetry_sched.rs | 55 +- crates/relay-md/plain/src/engine.rs | 12 +- crates/relay-mix-multi/plain/src/lib.rs | 42 +- crates/relay-mix-quad/plain/src/lib.rs | 171 ++-- crates/relay-mm/plain/src/engine.rs | 19 +- crates/relay-modextra/plain/src/lib.rs | 30 +- crates/relay-nid/plain/src/bitpack.rs | 22 +- crates/relay-nid/plain/src/lib.rs | 23 +- crates/relay-notch/plain/src/lib.rs | 10 +- crates/relay-offboard/plain/src/lib.rs | 18 +- crates/relay-param/plain/src/kani_proofs.rs | 14 +- crates/relay-param/plain/src/lib.rs | 115 ++- crates/relay-param/plain/src/persist.rs | 39 +- crates/relay-pos/plain/src/lib.rs | 89 +- crates/relay-preflight/plain/src/lib.rs | 16 +- crates/relay-primitives/plain/src/ccsds.rs | 8 +- crates/relay-primitives/plain/src/compare.rs | 2 +- crates/relay-primitives/plain/src/filter.rs | 6 +- crates/relay-primitives/plain/src/lib.rs | 8 +- crates/relay-primitives/plain/src/merge.rs | 12 +- .../relay-primitives/plain/src/persistence.rs | 6 +- .../relay-primitives/plain/src/rate_divide.rs | 6 +- .../relay-primitives/plain/src/time_gate.rs | 6 +- crates/relay-rate/plain/src/lib.rs | 49 +- crates/relay-rc/plain/src/lib.rs | 36 +- crates/relay-sc/plain/src/engine.rs | 117 ++- crates/relay-sch/plain/src/engine.rs | 141 +++- crates/relay-sec/plain/src/ascon.rs | 51 +- crates/relay-sec/plain/src/frame.rs | 15 +- crates/relay-sec/plain/src/header.rs | 33 +- crates/relay-sec/plain/src/kani_proofs.rs | 4 +- crates/relay-sensvote/plain/src/lib.rs | 6 +- crates/relay-to/plain/src/engine.rs | 12 +- crates/relay-traj/plain/src/lib.rs | 69 +- examples/falcon-ekf-bench/src/main.rs | 43 +- examples/falcon-hello/src/main.rs | 28 +- examples/falcon-hitl-rfspoof/src/hackrf.rs | 31 +- examples/falcon-hitl-rfspoof/src/harness.rs | 18 +- examples/falcon-hitl-rfspoof/src/main.rs | 44 +- examples/falcon-hitl-rfspoof/src/mavlink.rs | 71 +- examples/falcon-hitl-rfspoof/src/stub.rs | 62 +- examples/falcon-hold-bench/src/main.rs | 29 +- examples/falcon-iekf-bench/src/main.rs | 19 +- examples/falcon-sitl-gz/src/campaign.rs | 165 +++- examples/falcon-sitl-gz/src/main.rs | 572 ++++++++++--- examples/falcon-sitl-gz/src/physics.rs | 108 ++- examples/falcon-sitl-hover/src/main.rs | 243 ++++-- host/falcon-config/src/lib.rs | 45 +- host/relay-sb/examples/intercore.rs | 22 +- host/relay-sb/src/core.rs | 12 +- 106 files changed, 4989 insertions(+), 1506 deletions(-) diff --git a/benches/cascade-throughput/benches/cascade_throughput.rs b/benches/cascade-throughput/benches/cascade_throughput.rs index 19f0dd90..b9790006 100644 --- a/benches/cascade-throughput/benches/cascade_throughput.rs +++ b/benches/cascade-throughput/benches/cascade_throughput.rs @@ -34,13 +34,22 @@ const ACCEL: [f32; 3] = [0.15, -0.09, -9.79]; const DT: f32 = 0.001; // 1 kHz rate loop fn ts_rate(ms: u64) -> RateTime { - RateTime { seconds: ms / 1000, fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32 } + RateTime { + seconds: ms / 1000, + fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32, + } } fn ts_att(ms: u64) -> AttTime { - AttTime { seconds: ms / 1000, fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32 } + AttTime { + seconds: ms / 1000, + fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32, + } } fn ts_pos(ms: u64) -> PosTime { - PosTime { seconds: ms / 1000, fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32 } + PosTime { + seconds: ms / 1000, + fraction: ((ms % 1000) * (1u64 << 32) / 1000) as u32, + } } /// IEKF — one propagate step at the rate-loop dt. The estimator is the heaviest @@ -52,7 +61,13 @@ fn bench_iekf(c: &mut Criterion) { let mut t = 0u64; b.iter(|| { t = t.wrapping_add(1); - ekf.propagate(black_box(Imu { gyro: GYRO, accel: ACCEL }), black_box(DT)); + ekf.propagate( + black_box(Imu { + gyro: GYRO, + accel: ACCEL, + }), + black_box(DT), + ); black_box(ekf.state()) }) }); @@ -106,7 +121,11 @@ fn bench_rate(c: &mut Criterion) { let mut ms = 0u64; b.iter(|| { ms = ms.wrapping_add(1); // 1 kHz - black_box(pid.tick(black_box(ts_rate(ms)), black_box(GYRO), black_box([0.0, 0.0, 0.0]))) + black_box(pid.tick( + black_box(ts_rate(ms)), + black_box(GYRO), + black_box([0.0, 0.0, 0.0]), + )) }) }); } @@ -138,10 +157,22 @@ fn bench_full_cascade(c: &mut Criterion) { b.iter(|| { ms = ms.wrapping_add(1); // 1. estimator - ekf.propagate(black_box(Imu { gyro: GYRO, accel: ACCEL }), DT); + ekf.propagate( + black_box(Imu { + gyro: GYRO, + accel: ACCEL, + }), + DT, + ); let st = ekf.state(); // 2. position -> attitude setpoint - let att_sp = pos.tick(ts_pos(ms), [0.3, -0.2, -4.8], [0.05, -0.03, 0.01], [1.0, 0.0, 0.0, 0.0], sp); + let att_sp = pos.tick( + ts_pos(ms), + [0.3, -0.2, -4.8], + [0.05, -0.03, 0.01], + [1.0, 0.0, 0.0, 0.0], + sp, + ); // 3. attitude -> rate setpoint let rate_sp = att.tick(ts_att(ms), att_sp.quaternion, [1.0, 0.0, 0.0, 0.0]); // 4. rate -> torque diff --git a/benches/engine-throughput/benches/engine_throughput.rs b/benches/engine-throughput/benches/engine_throughput.rs index 1b0a1029..62ba1d89 100644 --- a/benches/engine-throughput/benches/engine_throughput.rs +++ b/benches/engine-throughput/benches/engine_throughput.rs @@ -124,7 +124,9 @@ fn bench_cfdp(c: &mut Criterion) { let mut table = TransactionTable::new(); let txn = table - .begin_send(/* file_size */ 65_536, /* max_retransmit */ 1_000_000) + .begin_send( + /* file_size */ 65_536, /* max_retransmit */ 1_000_000, + ) .expect("transaction slot available"); c.bench_function("cfdp/process_nak__retransmit_event", |b| { @@ -132,12 +134,5 @@ fn bench_cfdp(c: &mut Criterion) { }); } -criterion_group!( - engines, - bench_lc, - bench_sch, - bench_sc, - bench_hs, - bench_cfdp -); +criterion_group!(engines, bench_lc, bench_sch, bench_sc, bench_hs, bench_cfdp); criterion_main!(engines); diff --git a/crates/falcon-baromag/src/lib.rs b/crates/falcon-baromag/src/lib.rs index 69773aa8..f222243b 100644 --- a/crates/falcon-baromag/src/lib.rs +++ b/crates/falcon-baromag/src/lib.rs @@ -91,7 +91,10 @@ pub mod mag { pub fn init(&mut self) -> Result<(), DriverError> { let id = self.bus.read_reg(REG_WHO_AM_I); if id != WHO_AM_I_VALUE { - return Err(DriverError::WrongIdentity { got: id, want: WHO_AM_I_VALUE }); + return Err(DriverError::WrongIdentity { + got: id, + want: WHO_AM_I_VALUE, + }); } self.bus.write_reg(REG_CNTL1, MODE_CONTINUOUS); Ok(()) @@ -133,7 +136,10 @@ pub mod mag { /// Wrap an I2C bus at the default address. pub fn new(i2c: I) -> Self { - Self { i2c, addr: Self::DEFAULT_ADDR } + Self { + i2c, + addr: Self::DEFAULT_ADDR, + } } /// Wrap an I2C bus at an explicit address. @@ -202,7 +208,11 @@ pub mod baro { // Nominal scales: BMP388 raw is ~24-bit over the sensor range; these // map a mid-range raw to ~standard sea-level pressure. Replaced by // the per-chip NVM trim at calibration time. - Self { p_scale: 0.012_5, t_scale: 0.005, t_offset: 0.0 } + Self { + p_scale: 0.012_5, + t_scale: 0.005, + t_offset: 0.0, + } } } @@ -215,7 +225,10 @@ pub mod baro { impl Bmp388 { /// Wrap a bus with the default calibration. pub fn new(bus: B) -> Self { - Self { bus, cal: BaroCal::default() } + Self { + bus, + cal: BaroCal::default(), + } } /// Wrap a bus with an explicit calibration. @@ -227,7 +240,10 @@ pub mod baro { pub fn init(&mut self) -> Result<(), DriverError> { let id = self.bus.read_reg(REG_CHIP_ID); if id != CHIP_ID_VALUE { - return Err(DriverError::WrongIdentity { got: id, want: CHIP_ID_VALUE }); + return Err(DriverError::WrongIdentity { + got: id, + want: CHIP_ID_VALUE, + }); } self.bus.write_reg(REG_PWR_CTRL, PWR_PRESS_TEMP_NORMAL); Ok(()) @@ -270,7 +286,11 @@ pub mod baro { /// Wrap an I2C bus at the default address + default calibration. pub fn new(i2c: I) -> Self { - Self { i2c, addr: Self::DEFAULT_ADDR, cal: BaroCal::default() } + Self { + i2c, + addr: Self::DEFAULT_ADDR, + cal: BaroCal::default(), + } } /// Wrap an I2C bus at an explicit address + calibration. @@ -349,13 +369,23 @@ mod tests { let mut bus = MockBus::new(); bus.set(0x00, 0xAB); let mut m = Ist8310::new(bus); - assert_eq!(m.init(), Err(DriverError::WrongIdentity { got: 0xAB, want: 0x10 })); + assert_eq!( + m.init(), + Err(DriverError::WrongIdentity { + got: 0xAB, + want: 0x10 + }) + ); } #[test] fn flat_heading_cardinal() { // field pointing +Y (east) ⇒ heading +π/2. - let h = flat_heading(MagField { x_ut: 0.0, y_ut: 10.0, z_ut: 0.0 }); + let h = flat_heading(MagField { + x_ut: 0.0, + y_ut: 10.0, + z_ut: 0.0, + }); assert!((h - core::f32::consts::FRAC_PI_2).abs() < 1e-4); } @@ -383,7 +413,13 @@ mod tests { let mut bus = MockBus::new(); bus.set(0x00, 0x99); let mut b = Bmp388::new(bus); - assert_eq!(b.init(), Err(DriverError::WrongIdentity { got: 0x99, want: 0x50 })); + assert_eq!( + b.init(), + Err(DriverError::WrongIdentity { + got: 0x99, + want: 0x50 + }) + ); } // ── async hardware path (embedded-hal-async I2c) ───────────────────────── @@ -477,9 +513,15 @@ mod tests { /// The async paths are fallible: an I2C transport error propagates as Err. #[test] fn baromag_async_propagates_transport_error() { - let mut mag = Ist8310I2c::new(MockI2c { block: [0; 6], fail: true }); + let mut mag = Ist8310I2c::new(MockI2c { + block: [0; 6], + fail: true, + }); assert!(block_on(mag.read_field()).is_err()); - let mut baro = Bmp388I2c::new(MockI2c { block: [0; 6], fail: true }); + let mut baro = Bmp388I2c::new(MockI2c { + block: [0; 6], + fail: true, + }); assert!(block_on(baro.read()).is_err()); } diff --git a/crates/falcon-core/plain/src/blackbox_backend.rs b/crates/falcon-core/plain/src/blackbox_backend.rs index fe01c0af..b73444a0 100644 --- a/crates/falcon-core/plain/src/blackbox_backend.rs +++ b/crates/falcon-core/plain/src/blackbox_backend.rs @@ -13,7 +13,7 @@ use crate::{FlightBackend, ImuSample, NavState}; use relay_iekf::Vec3; -use relay_log::blackbox::{BlackboxWriter, BlockLog, TickRecord, TICK_MAX}; +use relay_log::blackbox::{BlackboxWriter, BlockLog, TICK_MAX, TickRecord}; /// Wraps a live backend; call [`LoggingBackend::finish_tick`] after each /// `FlightCore::step` with the estimator state to seal that tick's record. @@ -102,8 +102,10 @@ impl FlightBackend for LoggingBackend<'_, B, L> { } fn read_gnss_dual( &mut self, - ) -> Option<(Option, Option)> - { + ) -> Option<( + Option, + Option, + )> { // Forwarded, NOT logged: the TickRecord carries the SELECTED position // (what the estimator consumed); per-receiver raw lanes are part of // the schema-v2 slice (#277). @@ -160,7 +162,10 @@ impl<'a> ReplayBackend<'a> { impl FlightBackend for ReplayBackend<'_> { fn read_imu(&mut self) -> ImuSample { let t = &self.ticks[self.at.min(self.ticks.len() - 1)]; - ImuSample { accel: t.accel, gyro: t.gyro } + ImuSample { + accel: t.accel, + gyro: t.gyro, + } } fn read_position(&mut self) -> Option { self.ticks.get(self.at).and_then(|t| t.pos) diff --git a/crates/falcon-core/plain/src/lib.rs b/crates/falcon-core/plain/src/lib.rs index 729777f2..18e39120 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -20,9 +20,9 @@ #![allow(clippy::needless_range_loop)] use relay_adrc::{AdrcRate, GyroLpf}; -use relay_geo::{quat_to_rotmat, thrust_axis_ned, GeoAtt, GeoGains}; +use relay_geo::{GeoAtt, GeoGains, quat_to_rotmat, thrust_axis_ned}; use relay_iekf::{Iekf, Imu as IekfImu, NavState, RotorFaultDetector, Vec3}; -use relay_mix_quad::{motors_to_torque_signs, QuadMixer}; +use relay_mix_quad::{QuadMixer, motors_to_torque_signs}; /// One inertial-measurement sample in the body frame. #[derive(Clone, Copy, Debug)] @@ -91,8 +91,10 @@ pub trait FlightBackend { /// flag) feeds the estimator instead. fn read_gnss_dual( &mut self, - ) -> Option<(Option, Option)> - { + ) -> Option<( + Option, + Option, + )> { None } /// Downward rangefinder distance (m), already wire-decoded and @@ -132,7 +134,6 @@ pub trait FlightBackend { /// both cores (the raw IMU sample + heading), passed in by the caller so /// a sim backend's one sample is never drawn twice. pub struct EstimatorPartition { - iekf: Iekf, grav_var: f32, pos_var: f32, @@ -153,7 +154,8 @@ pub struct EstimatorPartition { /// (gyro/accel bias+scale, mag hard/soft-iron). Identity until /// `set_calibration` installs solved offsets — the explicit replacement for /// the prior identity-remap placeholder (raw samples flowed in uncorrected). - calib: relay_calib::CalParams, /// Control-step counter, for the FDI spin-up guard. At arm/spin-up the ESC + calib: relay_calib::CalParams, + /// Control-step counter, for the FDI spin-up guard. At arm/spin-up the ESC /// RPM lags the commanded throttle (real actuators, or a sim reporting the /// achieved rotor state), so the commanded-vs-achieved effectiveness /// residual spikes on EVERY rotor for the first fraction of a second — which @@ -168,7 +170,6 @@ pub struct EstimatorPartition { impl EstimatorPartition { fn new(warmup: u32) -> Self { EstimatorPartition { - iekf: Iekf::level(), grav_var: 0.5, pos_var: 0.01, @@ -261,7 +262,6 @@ impl EstimatorPartition { } } - /// One estimator tick: fanned-out IMU sample + heading, own reads of /// GNSS/mag/baro, publish the estimate. The partition's ONLY output. pub fn step( @@ -282,7 +282,8 @@ impl EstimatorPartition { self.iekf.update_gravity(accel, self.grav_var); self.fuse_gnss(b); if let Some(m) = b.read_mag() { - self.iekf.update_magnetometer(self.calib.apply_mag(m), 0.0, self.mag_var); + self.iekf + .update_magnetometer(self.calib.apply_mag(m), 0.0, self.mag_var); } // v1.113 — direct heading update: a backend that resolves a clean // absolute yaw (fused compass / GNSS heading / sim truth) feeds it @@ -298,7 +299,8 @@ impl EstimatorPartition { // estimation rather than a hand-rolled complementary filter. if let Some(bz) = b.read_baro() { let e = self.iekf.state(); - self.iekf.update_position([e.p[0], e.p[1], bz], self.baro_var); + self.iekf + .update_position([e.p[0], e.p[1], bz], self.baro_var); } self.step_count = self.step_count.saturating_add(1); self.iekf.state() @@ -415,8 +417,8 @@ impl CascadePartition { setpoint: [0.0; 3], kp_alt: 0.05, kd_alt: 0.30, - ki_alt: 0.0, // opt-in (set_altitude_integral_gain): the integral is - alt_int: 0.0, // for high-altitude thrust-lapse compensation; default + ki_alt: 0.0, // opt-in (set_altitude_integral_gain): the integral is + alt_int: 0.0, // for high-altitude thrust-lapse compensation; default alt_int_max: 0.4, // off as it interacts with aggressive alt transients. landing: false, landing_descent: 0.5, // m/s controlled descent rate (NED z, +down) @@ -471,12 +473,19 @@ impl CascadePartition { } /// Set the hover-thrust feedforward (per-airframe; clamped to [0,1]). pub fn set_hover_thrust_core(&mut self, t: f32) { - self.hover_thrust = if t.is_finite() { t.clamp(0.0, 1.0) } else { self.hover_thrust }; + self.hover_thrust = if t.is_finite() { + t.clamp(0.0, 1.0) + } else { + self.hover_thrust + }; } /// Set the landing descent rate (m/s, NED +down; clamped to [0.1, 2]). pub fn set_landing_descent(&mut self, vz: f32) { - self.landing_descent = - if vz.is_finite() { vz.clamp(0.1, 2.0) } else { self.landing_descent }; + self.landing_descent = if vz.is_finite() { + vz.clamp(0.1, 2.0) + } else { + self.landing_descent + }; } /// Current altitude P/D gains (tuning observability, v1.119). pub fn altitude_gains(&self) -> (f32, f32) { @@ -649,8 +658,7 @@ impl CascadePartition { } else { self.hover_thrust }; - (hover_eff - self.kvz_land * (self.landing_descent - est.v[2])) - .clamp(0.0, 1.0) + (hover_eff - self.kvz_land * (self.landing_descent - est.v[2])).clamp(0.0, 1.0) } } else { // ── Position altitude P-I-D (v1.2; v1.20 baro-anchored; v1.22 +I) ── @@ -690,7 +698,11 @@ impl CascadePartition { if !converging { self.alt_int += alt_err * dt; } - let cap = if self.ki_alt > 0.0 { self.alt_int_max / self.ki_alt } else { 0.0 }; + let cap = if self.ki_alt > 0.0 { + self.alt_int_max / self.ki_alt + } else { + 0.0 + }; self.alt_int = self.alt_int.clamp(-cap, cap); (self.hover_thrust - self.kp_alt * alt_err - self.ki_alt * self.alt_int + self.kd_alt * est.v[2]) @@ -706,7 +718,11 @@ impl CascadePartition { let perr = [self.setpoint[0] - est.p[0], self.setpoint[1] - est.p[1]]; for i in 0..2 { self.pos_int[i] += perr[i] * dt; - let cap = if self.ki_pos > 0.0 { self.pos_int_max / self.ki_pos } else { 0.0 }; + let cap = if self.ki_pos > 0.0 { + self.pos_int_max / self.ki_pos + } else { + 0.0 + }; self.pos_int[i] = self.pos_int[i].clamp(-cap, cap); } let mut a_cmd = [ @@ -739,7 +755,8 @@ impl CascadePartition { 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_ctrl, b3_d); - self.mixer.mix_rotor_out(failed, torque, thrust, ROTOR_OUT_FLOOR) + 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, self.yaw_setpoint); @@ -789,10 +806,7 @@ impl CascadePartition { // command-ALIGNED residual, not this gate. let fdi_steady = tilt_cos > 0.90 && rp_rate2 < 4.0; // ≲26° tilt, ≲2 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 - { + if self.failed_motor.is_none() && self.step_count >= self.fdi_warmup_steps && fdi_steady { if let Some(rpm) = rpm_now { let mut resid = [0.0f32; 4]; let mut i = 0; @@ -979,7 +993,6 @@ impl FlightCore { pub fn set_altitude_gains(&mut self, kp: f32, kd: f32) { self.casc.set_altitude_gains(kp, kd) } - } /// The PARTITIONED deployment shape (PART-P01, v1.124): the two halves of @@ -1004,7 +1017,13 @@ pub struct PartitionedCore { } impl PartitionedCore { - pub fn new(hover_thrust: f32, loop_hz: f32, delay_ticks: usize, jitter: bool, seed: u32) -> Self { + pub fn new( + hover_thrust: f32, + loop_hz: f32, + delay_ticks: usize, + jitter: bool, + seed: u32, + ) -> Self { let warmup = ((loop_hz * 0.2) as u32).max(10); PartitionedCore { est: EstimatorPartition::new(warmup), @@ -1227,7 +1246,10 @@ impl FlightSupervisor { waypoints: [home; MAX_WAYPOINTS], wp_count: 0, wp_index: 0, - zones: [KeepoutZone { center: home, radius: 0.0 }; MAX_KEEPOUT_ZONES], + zones: [KeepoutZone { + center: home, + radius: 0.0, + }; MAX_KEEPOUT_ZONES], zone_count: 0, rtl_latched: false, runaway_count: 0, @@ -1269,7 +1291,11 @@ impl FlightSupervisor { /// Set the hover-thrust feedforward on the wrapped core (per-plant: the /// analytic sim hovers at ~0.49, the real gz falcon-quad at ~0.585). pub fn set_hover_thrust(&mut self, t: f32) { - self.core.casc.hover_thrust = if t.is_finite() { t.clamp(0.0, 1.0) } else { 0.5 }; + self.core.casc.hover_thrust = if t.is_finite() { + t.clamp(0.0, 1.0) + } else { + 0.5 + }; } pub fn mode(&self) -> relay_fsm::Mode { @@ -1341,11 +1367,20 @@ impl FlightSupervisor { use relay_preflight::CheckId; let t = &mut self.check_table; t.set(CheckId::SensorsHealthy, self.preflight.sensors_healthy); - t.set(CheckId::EstimatorConverged, self.preflight.estimator_converged); - t.set(CheckId::CalibrationPresent, self.preflight.calibration_present); + t.set( + CheckId::EstimatorConverged, + self.preflight.estimator_converged, + ); + t.set( + CheckId::CalibrationPresent, + self.preflight.calibration_present, + ); t.set(CheckId::GeofenceLoaded, self.preflight.geofence_loaded); t.set(CheckId::BatteryOk, self.preflight.battery_ok); - t.set(CheckId::FailsafeConfigured, self.preflight.failsafe_configured); + t.set( + CheckId::FailsafeConfigured, + self.preflight.failsafe_configured, + ); t.set(CheckId::EstimatorInnovation, !self.core.nav_compromised()); t.set(CheckId::GnssAgreement, !self.core.gnss_diverged()); t.set( @@ -1418,7 +1453,12 @@ impl FlightSupervisor { relay_preflight::arm_check_table(&self.check_table), relay_preflight::TableVerdict::Allowed ); - let g = relay_fsm::Gates { level, throttle_low, have_position: true, prearm_ok }; + let g = relay_fsm::Gates { + level, + throttle_low, + have_position: true, + prearm_ok, + }; self.fsm.on(ev, g); } @@ -1491,8 +1531,7 @@ impl FlightSupervisor { // OBSTRUCTS only if it is ahead, before the goal, and laterally close. let along = -rx * ux + -ry * uy; let perp = relay_math::sqrtf(((rx * rx + ry * ry) - along * along).max(0.0)); - let obstructs = - along > 0.0 && along < glen + safe && perp < safe + KEEPOUT_INFLUENCE; + let obstructs = along > 0.0 && along < glen + safe && perp < safe + KEEPOUT_INFLUENCE; // Deflect when the zone blocks the path (and the vehicle is within // reach of it), OR as a hard guard whenever the vehicle is inside the // safe ring. Crucially NOT when the zone is merely near but off-path @@ -1523,7 +1562,12 @@ impl FlightSupervisor { let dy = est.p[1] - self.home[1]; let dist_home = relay_math::sqrtf(dx * dx + dy * dy); let alt_agl = -est.p[2]; // NED z negative = up - let g = Gates { level: true, throttle_low: true, have_position: true, prearm_ok: true }; + let g = Gates { + level: true, + throttle_low: true, + have_position: true, + prearm_ok: true, + }; // ── FAILSAFE actuation (the audit's gap): geofence breach OR low // battery from any flying state ⇒ Failsafe ⇒ the FSM commands RTL. ── @@ -1548,10 +1592,16 @@ impl FlightSupervisor { // shown eRPM (no bidir-DShot) is not gated on it, but once seen, // its LOSS blocks arming (one-way, like every declared row). match b.read_motor_rpm() { - Some(_) => self.check_table.set(relay_preflight::CheckId::EscTelemetry, true), + Some(_) => self + .check_table + .set(relay_preflight::CheckId::EscTelemetry, true), None => { - if self.check_table.is_required(relay_preflight::CheckId::EscTelemetry) { - self.check_table.set(relay_preflight::CheckId::EscTelemetry, false); + if self + .check_table + .is_required(relay_preflight::CheckId::EscTelemetry) + { + self.check_table + .set(relay_preflight::CheckId::EscTelemetry, false); } } } @@ -1649,8 +1699,7 @@ impl FlightSupervisor { // at an uncontrolled ~3.4 m/s. The velocity-landing law arrests that // and rides down at its commanded descent rate instead.) let landing = matches!(self.fsm.mode(), Mode::Land) - && (self.core.failed_motor().is_some() - || (horiz_speed < 0.4 && dist_home < 0.5)); + && (self.core.failed_motor().is_some() || (horiz_speed < 0.4 && dist_home < 0.5)); self.core.set_landing(landing); // ── mode → setpoint ── (horizontal hold target; while landing the core's @@ -1685,7 +1734,10 @@ impl FlightSupervisor { // Disarmed kept the altitude loop live and flew the vehicle away // (and "Terminate = motors cut" was only ever an FSM claim, never an // actuator command — the software half of PART-P02's backstop). - if matches!(self.fsm.mode(), Mode::Disarmed | Mode::Armed | Mode::Terminated) { + if matches!( + self.fsm.mode(), + Mode::Disarmed | Mode::Armed | Mode::Terminated + ) { self.core.step_estimate_only(b); // estimator warm, motors OFF return; } @@ -1701,7 +1753,12 @@ impl FlightSupervisor { && self.fsm.is_airborne() && self.fsm.mode() != Mode::Land { - let land = Gates { level: true, throttle_low: true, have_position: false, prearm_ok: true }; + let land = Gates { + level: true, + throttle_low: true, + have_position: false, + prearm_ok: true, + }; self.fsm.on(Event::Failsafe, land); self.rtl_latched = true; } @@ -2076,7 +2133,11 @@ impl SimBackend { } fn integrate(&mut self, torque: Vec3) { - let jo = [self.j[0] * self.omega[0], self.j[1] * self.omega[1], self.j[2] * self.omega[2]]; + let jo = [ + self.j[0] * self.omega[0], + self.j[1] * self.omega[1], + self.j[2] * self.omega[2], + ]; let gyro = [ self.omega[1] * jo[2] - self.omega[2] * jo[1], self.omega[2] * jo[0] - self.omega[0] * jo[2], @@ -2089,8 +2150,16 @@ impl SimBackend { self.omega[i] += self.dt * (torque[i] - gyro[i] - drag) / self.j[i]; } // first-order rotation integration (Rᵢ₊₁ = Rᵢ·(I + [ω]ₓdt)) - let wd = [self.omega[0] * self.dt, self.omega[1] * self.dt, self.omega[2] * self.dt]; - let incr = [[1.0, -wd[2], wd[1]], [wd[2], 1.0, -wd[0]], [-wd[1], wd[0], 1.0]]; + let wd = [ + self.omega[0] * self.dt, + self.omega[1] * self.dt, + self.omega[2] * self.dt, + ]; + let incr = [ + [1.0, -wd[2], wd[1]], + [wd[2], 1.0, -wd[0]], + [-wd[1], wd[0], 1.0], + ]; let mut m = [[0.0f32; 3]; 3]; for i in 0..3 { for jj in 0..3 { @@ -2270,7 +2339,11 @@ impl FlightBackend for SimBackend { } } const K_WIND: f32 = 0.15; - if self.wind[0] != 0.0 || self.wind[1] != 0.0 || self.gust_amp != 0.0 || self.path.turbulence > 0.0 { + if self.wind[0] != 0.0 + || self.wind[1] != 0.0 + || self.gust_amp != 0.0 + || self.path.turbulence > 0.0 + { for i in 0..2 { let gust = self.gust_amp * self.noise_unit(); accel[i] += K_WIND * (self.wind[i] + gust + self.turb_state[i] - self.vel[i]); @@ -2314,7 +2387,8 @@ impl FlightBackend for SimBackend { self.last_collective = collective; if self.battery_drain { let current = collective; // ∝ total motor power - self.battery_charge = (self.battery_charge - current * 5.0e-5 * self.dt / 0.002).max(0.0); + self.battery_charge = + (self.battery_charge - current * 5.0e-5 * self.dt / 0.002).max(0.0); self.battery_v = 12.6 + 4.2 * self.battery_charge - 0.3 * current; } } @@ -2339,8 +2413,10 @@ impl FlightBackend for SimBackend { } fn read_gnss_dual( &mut self, - ) -> Option<(Option, Option)> - { + ) -> Option<( + Option, + Option, + )> { use falcon_gnss_ubx::dual::NedFix; if !self.gnss_dual_enabled { return None; @@ -2352,7 +2428,12 @@ impl FlightBackend for SimBackend { let a = if self.gnss_a_down { None } else { - Some(NedFix { pos: base, acc_m: self.gnss_a_acc, sats: 14, fix_ok: true }) + Some(NedFix { + pos: base, + acc_m: self.gnss_a_acc, + sats: 14, + fix_ok: true, + }) }; let b = if self.gnss_b_down { None @@ -2360,7 +2441,12 @@ impl FlightBackend for SimBackend { let mut pb = base; pb[0] += self.gnss_b_offset[0]; pb[1] += self.gnss_b_offset[1]; - Some(NedFix { pos: pb, acc_m: self.gnss_b_acc, sats: 14, fix_ok: true }) + Some(NedFix { + pos: pb, + acc_m: self.gnss_b_acc, + sats: 14, + fix_ok: true, + }) }; Some((a, b)) } @@ -2405,7 +2491,7 @@ mod blackbox_replay_tests { extern crate std; use super::blackbox_backend::*; use super::*; - use relay_log::blackbox::{scan, BlockLog, TickRecord, REC_TICK, TICK_MAX}; + use relay_log::blackbox::{BlockLog, REC_TICK, TICK_MAX, TickRecord, scan}; use std::vec::Vec; struct VecLog(Vec); @@ -2463,9 +2549,21 @@ mod blackbox_replay_tests { replayed.step(&mut rb); let est = replayed.state(); let t = rb.current().unwrap(); - assert_eq!(est.q.map(f32::to_bits), t.est_q.map(f32::to_bits), "q tick {checked}"); - assert_eq!(est.v.map(f32::to_bits), t.est_v.map(f32::to_bits), "v tick {checked}"); - assert_eq!(est.p.map(f32::to_bits), t.est_p.map(f32::to_bits), "p tick {checked}"); + assert_eq!( + est.q.map(f32::to_bits), + t.est_q.map(f32::to_bits), + "q tick {checked}" + ); + assert_eq!( + est.v.map(f32::to_bits), + t.est_v.map(f32::to_bits), + "v tick {checked}" + ); + assert_eq!( + est.p.map(f32::to_bits), + t.est_p.map(f32::to_bits), + "p tick {checked}" + ); assert_eq!( replayed.cov_summary().map(f32::to_bits), t.cov.map(f32::to_bits), @@ -2498,7 +2596,9 @@ mod blackbox_replay_tests { Ok(b) => b, Err(_) => { // First-build bootstrap: golden not yet generated. - std::eprintln!("golden log missing — run `cargo test regen_golden_log -- --ignored` and commit it"); + std::eprintln!( + "golden log missing — run `cargo test regen_golden_log -- --ignored` and commit it" + ); return; } }; @@ -2532,7 +2632,7 @@ mod blackbox_replay_tests { #[test] fn supervised_flight_logs_boot_ticks_and_events() { use relay_fsm::Event; - use relay_log::blackbox::{encode_boot, encode_event, REC_BOOT, REC_EVENT}; + use relay_log::blackbox::{REC_BOOT, REC_EVENT, encode_boot, encode_event}; let dt = 0.002f32; let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; let mut plant = SimBackend::new(level, dt); @@ -2571,7 +2671,10 @@ mod blackbox_replay_tests { assert_eq!(used, bytes.len(), "no torn tail"); assert_eq!(boots, 1); assert_eq!(ticks, 8000); - assert!(events >= 1, "mode transitions must appear as events (got {events})"); + assert!( + events >= 1, + "mode transitions must appear as events (got {events})" + ); } /// Writes the golden fixture (deliberate, reviewed regeneration only). @@ -2616,7 +2719,6 @@ 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 + @@ -2637,7 +2739,10 @@ mod tests { core.step(&mut backend); } let tilt = backend.tilt(); - assert!(tilt < 0.1, "core must recover to level through the HAL: {tilt} rad (start {tilt0})"); + assert!( + tilt < 0.1, + "core must recover to level through the HAL: {tilt} rad (start {tilt0})" + ); } /// The backend is a SEAM, not a fixed simulator: a trivial stand-in @@ -2657,7 +2762,10 @@ mod tests { } impl FlightBackend for BiasedGyro { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0, 0.0, self.bias_z] } + ImuSample { + accel: [0.0, 0.0, -GRAVITY], + gyro: [0.0, 0.0, self.bias_z], + } } fn read_position(&mut self) -> Option { None @@ -2679,7 +2787,10 @@ mod tests { } let mut cal = FlightCore::new(0.5, 250.0); - cal.set_calibration(CalParams { gyro_bias: [0.0, 0.0, bias], ..CalParams::identity() }); + cal.set_calibration(CalParams { + gyro_bias: [0.0, 0.0, bias], + ..CalParams::identity() + }); let mut bc = BiasedGyro { bias_z: bias }; for _ in 0..250 { cal.step(&mut bc); @@ -2691,7 +2802,10 @@ mod tests { + (qu[1] - qc[1]).abs() + (qu[2] - qc[2]).abs() + (qu[3] - qc[3]).abs(); - assert!(diff > 0.05, "calibration must change the estimate (yaw drift suppressed): diff {diff}"); + assert!( + diff > 0.05, + "calibration must change the estimate (yaw drift suppressed): diff {diff}" + ); // the installed calibration is reported back. assert_eq!(cal.calibration().gyro_bias, [0.0, 0.0, bias]); } @@ -2708,7 +2822,10 @@ mod tests { } impl FlightBackend for RpmBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + 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 @@ -2739,8 +2856,14 @@ mod tests { } } 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 }; + 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); @@ -2761,7 +2884,11 @@ mod tests { for _ in 0..40 { sup.step(&mut b); } - assert_eq!(sup.mode(), relay_fsm::Mode::Land, "motor failure commands Land"); + 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 @@ -2779,7 +2906,10 @@ mod tests { } impl FlightBackend for RpmBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + 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 @@ -2809,20 +2939,32 @@ mod tests { } 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 }; + 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"); + 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"); + 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). @@ -2830,7 +2972,10 @@ mod tests { 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}"); + assert!( + (ROTOR_OUT_FLOOR - 1e-6..=1.0 + 1e-6).contains(&v), + "healthy rotor {i} = {v}" + ); } } } @@ -2863,8 +3008,15 @@ mod tests { core.step(&mut backend); } let hover_tilt = backend.tilt(); - assert!(hover_tilt < 0.1, "must reach a level hover first: {hover_tilt} rad"); - assert_eq!(core.failed_motor(), None, "no false isolation while healthy"); + assert!( + hover_tilt < 0.1, + "must reach a level hover first: {hover_tilt} rad" + ); + assert_eq!( + core.failed_motor(), + None, + "no false isolation while healthy" + ); // Rotor 0 dies. A 3-rotor quad cannot hover, so the honest recovery is // a controlled spin-DESCENT: hold the thrust axis near-level while the @@ -2886,7 +3038,11 @@ mod tests { } } - assert_eq!(core.failed_motor(), Some(0), "FDI must isolate the dead rotor"); + assert_eq!( + core.failed_motor(), + Some(0), + "FDI must isolate the dead rotor" + ); // The whole point: the thrust axis never tips anywhere near inverted. A // pre-v1.114 run blew through 90° to ~180° (parasitic-moment flip); the // rank-3 allocation holds it near-level throughout the descent (the @@ -2917,7 +3073,10 @@ mod tests { } impl FlightBackend for NullBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + ImuSample { + accel: [0.0, 0.0, -GRAVITY], + gyro: [0.0; 3], + } } fn read_position(&mut self) -> Option { None @@ -2958,8 +3117,16 @@ mod tests { for _ in 0..15000 { core.step(&mut backend); } - assert!((backend.pos[2] + 2.0).abs() < 0.25, "altitude must reach −2 m: {}", backend.pos[2]); - assert!(backend.tilt() < 0.1, "should stay level while holding altitude: {}", backend.tilt()); + assert!( + (backend.pos[2] + 2.0).abs() < 0.25, + "altitude must reach −2 m: {}", + backend.pos[2] + ); + assert!( + backend.tilt() < 0.1, + "should stay level while holding altitude: {}", + backend.tilt() + ); } /// v1.3 — full 6-DoF: the backend-agnostic core flies to and holds a @@ -2983,8 +3150,16 @@ mod tests { backend.pos[2] + 2.0, ]; let err = relay_math::sqrtf(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]); - assert!(err < 0.5, "must reach the position setpoint through the HAL: {err} m, pos {:?}", backend.pos); - assert!(backend.tilt() < 0.15, "settle near level: {} rad", backend.tilt()); + assert!( + err < 0.5, + "must reach the position setpoint through the HAL: {err} m, pos {:?}", + backend.pos + ); + assert!( + backend.tilt() < 0.15, + "settle near level: {} rad", + backend.tilt() + ); } /// The verified ADRC inner loop REJECTS a sustained body-torque @@ -3010,7 +3185,10 @@ mod tests { // after the ESO converges, the disturbance is cancelled and the body // holds near level (a plain proportional loop would sit at a steady // offset; ADRC drives it out). - assert!(peak_after < 0.12, "ESO must reject the disturbance: steady tilt {peak_after} rad"); + assert!( + peak_after < 0.12, + "ESO must reject the disturbance: steady tilt {peak_after} rad" + ); } // ── v1.8 supervisor: geofence→RTL actuation + battery failsafe ──────── @@ -3032,7 +3210,11 @@ mod tests { for _ in 0..8000 { sup.step(&mut backend); } - assert_eq!(sup.mode(), Mode::Loiter, "should reach Loiter after takeoff"); + assert_eq!( + sup.mode(), + Mode::Loiter, + "should reach Loiter after takeoff" + ); sup.set_mission([4.0, 0.0, -2.0]); // OUTSIDE the 1.5 m fence sup.command(Event::RequestMission, true, false); for _ in 0..40000 { @@ -3041,8 +3223,13 @@ mod tests { break; } } - let dh = relay_math::sqrtf(backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1]); - assert!(dh < 1.0, "RTL must bring it home, not to [4,0]: horiz {dh} m, pos {:?}", backend.pos); + let dh = + relay_math::sqrtf(backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1]); + assert!( + dh < 1.0, + "RTL must bring it home, not to [4,0]: horiz {dh} m, pos {:?}", + backend.pos + ); assert!( matches!(sup.mode(), Mode::Land | Mode::Disarmed), "RTL should be landing/landed, mode {:?}", @@ -3073,7 +3260,11 @@ mod tests { for _ in 0..8000 { sup.step(&mut b); } - assert_eq!(sup.mode(), Mode::Loiter, "should reach Loiter after takeoff"); + assert_eq!( + sup.mode(), + Mode::Loiter, + "should reach Loiter after takeoff" + ); sup.command(Event::RequestMission, true, false); // Fly the mission. Track the closest approach to each waypoint and the @@ -3106,9 +3297,15 @@ mod tests { } for (i, d) in min_d.iter().enumerate() { - assert!(*d < WAYPOINT_RADIUS + 0.2, "waypoint {i} not visited: min dist {d} m"); + assert!( + *d < WAYPOINT_RADIUS + 0.2, + "waypoint {i} not visited: min dist {d} m" + ); } - assert!(order_ok, "waypoints must be flown in order (leg index monotonic)"); + assert!( + order_ok, + "waypoints must be flown in order (leg index monotonic)" + ); assert!( disarmed, "mission must complete autonomously: return home + land + disarm (mode {:?})", @@ -3131,7 +3328,10 @@ mod tests { // A single far waypoint straight across a zone that sits on the path. sup.set_mission_waypoints(&[[10.0, 0.0, -2.0]]); - let zone = KeepoutZone { center: [5.0, 0.0, -2.0], radius: 2.0 }; + let zone = KeepoutZone { + center: [5.0, 0.0, -2.0], + radius: 2.0, + }; sup.set_keepout_zones(&[zone]); sup.command(Event::Arm, true, true); @@ -3162,7 +3362,10 @@ mod tests { } } // visited the far waypoint (so it really crossed the obstacle field) … - assert!(min_wp < WAYPOINT_RADIUS + 0.3, "waypoint not reached: min dist {min_wp} m"); + assert!( + min_wp < WAYPOINT_RADIUS + 0.3, + "waypoint not reached: min dist {min_wp} m" + ); // … but never entered the no-fly zone … assert!( min_zone > zone.radius, @@ -3170,7 +3373,11 @@ mod tests { zone.radius ); // … and still completed the sortie autonomously. - assert!(disarmed, "mission with avoidance must still complete (mode {:?})", sup.mode()); + assert!( + disarmed, + "mission with avoidance must still complete (mode {:?})", + sup.mode() + ); } /// A low battery actuates a failsafe (the audit's "no battery failsafe"): @@ -3209,8 +3416,8 @@ mod tests { /// over the ArrayNvm mock), not a hand-set flag. #[test] fn defaults_fallback_params_block_arming() { - use relay_param::persist::{load, save, ArrayNvm, Layout, LoadOutcome}; use relay_param::ParamStore; + use relay_param::persist::{ArrayNvm, Layout, LoadOutcome, load, save}; use relay_preflight::CheckId; let dt = 0.004f32; @@ -3237,7 +3444,11 @@ mod tests { sup.step(&mut b); } sup.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup.mode(), relay_fsm::Mode::Disarmed, "defaults-fallback must not arm"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Disarmed, + "defaults-fallback must not arm" + ); assert_eq!(sup.arm_blocked_check(), Some(CheckId::ParamsFromNvm)); assert_eq!( CheckId::ParamsFromNvm.reason_text(), @@ -3253,7 +3464,11 @@ mod tests { sup.set_check(CheckId::ParamsFromNvm, rep2.outcome == LoadOutcome::Loaded); sup.step(&mut b); sup.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup.mode(), relay_fsm::Mode::Armed, "arms after the NVM load"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Armed, + "arms after the NVM load" + ); } /// PART-P02 (a): the F100 pass-through conformance fixture for gale#65. @@ -3307,7 +3522,12 @@ mod tests { # phase,m0_bits,m1_bits,m2_bits,m3_bits — f32 bit patterns (hex);\n\ # EXPECTED F100 OUTPUT == INPUT, byte-exact (no re-mix, no floors).\n", ); - let cap = |core: &mut FlightCore, b: &mut SimBackend, phase: &str, n: usize, every: usize, out: &mut std::string::String| { + let cap = |core: &mut FlightCore, + b: &mut SimBackend, + phase: &str, + n: usize, + every: usize, + out: &mut std::string::String| { for k in 0..n { core.step(b); if k % every == 0 { @@ -3524,7 +3744,10 @@ mod tests { } let mean = sum / n as f64; let var = (sum2 / n as f64 - mean * mean).max(0.0); - assert!(b.tilt() < 0.2, "hover must hold (telemetry={rpm_telemetry})"); + assert!( + b.tilt() < 0.2, + "hover must hold (telemetry={rpm_telemetry})" + ); (var.sqrt()) as f32 } let with_notch = hover_thrash(true); @@ -3580,7 +3803,10 @@ mod tests { b.gnss_dual_enabled = true; b.ground_contact = true; let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 50.0, 2.0, 14.0); - sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); for _ in 0..2000 { sup.step(&mut b); } @@ -3589,8 +3815,14 @@ mod tests { for _ in 0..2000 { sup.step(&mut b); } - assert!(sup.core().gnss_diverged(), "sustained 12 m split must latch"); - assert!(sup.arm_blocked_reason().is_some(), "divergence blocks arming"); + assert!( + sup.core().gnss_diverged(), + "sustained 12 m split must latch" + ); + assert!( + sup.arm_blocked_reason().is_some(), + "divergence blocks arming" + ); let e = sup.core().state(); assert!( relay_math::fabsf(e.p[0]) < 2.0, @@ -3644,9 +3876,15 @@ mod tests { "trial {trial} fault {fault}: estimate stepped {max_step} m" ); if fault == 3 { - assert!(core.gnss_diverged(), "trial {trial}: walk must raise the flag"); + assert!( + core.gnss_diverged(), + "trial {trial}: walk must raise the flag" + ); } else { - assert!(!core.gnss_diverged(), "trial {trial} fault {fault}: false flag"); + assert!( + !core.gnss_diverged(), + "trial {trial} fault {fault}: false flag" + ); } } } @@ -3774,7 +4012,10 @@ mod tests { sup.step(&mut backend); } assert_eq!(sup.mode(), Mode::Loiter); - assert!(!sup.battery().degraded, "current sense present ⇒ compensated path"); + assert!( + !sup.battery().degraded, + "current sense present ⇒ compensated path" + ); // 4-second 300 A punch: terminal sags to 8.2 V — WAY below the 14 V // raw threshold that used to gate the failsafe directly. @@ -3783,7 +4024,11 @@ mod tests { for _ in 0..2000 { sup.step(&mut backend); } - assert_eq!(sup.mode(), Mode::Loiter, "sag must not false-trigger the failsafe"); + assert_eq!( + sup.mode(), + Mode::Loiter, + "sag must not false-trigger the failsafe" + ); // Same terminal voltage, near-zero current, sustained: nothing to // credit back — a pack genuinely THIS low at rest is an emergency. @@ -3812,8 +4057,10 @@ mod tests { fn holds_through_accelerometer_vibration() { let dt = 0.002f32; let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let mut backend = SimBackend::new(level, dt) - .with_pathology(Pathology { vibration: 1.5, ..Default::default() }); + let mut backend = SimBackend::new(level, dt).with_pathology(Pathology { + vibration: 1.5, + ..Default::default() + }); let mut core = FlightCore::new(0.5, 1.0 / dt); core.set_altitude(-2.0); let mut peak = 0.0f32; @@ -3823,8 +4070,15 @@ mod tests { peak = peak.max(backend.tilt()); } } - assert!(peak < 0.15, "IEKF must reject accel vibration: peak tilt {peak} rad"); - assert!((backend.pos[2] + 2.0).abs() < 0.4, "altitude held under vibration: {}", backend.pos[2]); + assert!( + peak < 0.15, + "IEKF must reject accel vibration: peak tilt {peak} rad" + ); + assert!( + (backend.pos[2] + 2.0).abs() < 0.4, + "altitude held under vibration: {}", + backend.pos[2] + ); } /// A slow gyro bias drift (0.004 rad/s², ≈0.5°/s after 12 s): the IEKF's @@ -3834,8 +4088,10 @@ mod tests { fn iekf_tracks_gyro_bias_drift() { let dt = 0.002f32; let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let mut backend = SimBackend::new(level, dt) - .with_pathology(Pathology { gyro_bias_drift: 0.004, ..Default::default() }); + let mut backend = SimBackend::new(level, dt).with_pathology(Pathology { + gyro_bias_drift: 0.004, + ..Default::default() + }); let mut core = FlightCore::new(0.5, 1.0 / dt); core.set_altitude(-2.0); let mut peak = 0.0f32; @@ -3847,7 +4103,10 @@ mod tests { } // injected bias reaches ≈0.004·30 = 0.12 rad/s; without bias estimation // the attitude would integrate that into a steady tilt. The IEKF holds. - assert!(peak < 0.15, "IEKF gyro-bias state must track the drift: peak tilt {peak} rad"); + assert!( + peak < 0.15, + "IEKF gyro-bias state must track the drift: peak tilt {peak} rad" + ); } /// A GPS dropout mid-flight (2 s, steps 6000–7000): while holding position, @@ -3871,13 +4130,22 @@ mod tests { for k in 0..15000 { core.step(&mut backend); if (6000..8000).contains(&k) { - let d = relay_math::sqrtf(backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1]); + let d = relay_math::sqrtf( + backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1], + ); peak_drift = peak_drift.max(d); } } - let final_d = relay_math::sqrtf(backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1]); - assert!(peak_drift < 2.0, "dropout drift must stay bounded: {peak_drift} m"); - assert!(final_d < 0.5, "position must re-converge after the fix returns: {final_d} m"); + let final_d = + relay_math::sqrtf(backend.pos[0] * backend.pos[0] + backend.pos[1] * backend.pos[1]); + assert!( + peak_drift < 2.0, + "dropout drift must stay bounded: {peak_drift} m" + ); + assert!( + final_d < 0.5, + "position must re-converge after the fix returns: {final_d} m" + ); } /// Magnetometer interference (0.3 of unit field per axis): the heading @@ -3887,8 +4155,10 @@ mod tests { fn tolerates_mag_interference() { let dt = 0.002f32; let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let mut backend = SimBackend::new(level, dt) - .with_pathology(Pathology { mag_interference: 0.3, ..Default::default() }); + let mut backend = SimBackend::new(level, dt).with_pathology(Pathology { + mag_interference: 0.3, + ..Default::default() + }); let mut core = FlightCore::new(0.5, 1.0 / dt); core.set_altitude(-2.0); let mut peak = 0.0f32; @@ -3898,8 +4168,15 @@ mod tests { peak = peak.max(backend.tilt()); } } - assert!(peak < 0.15, "mag interference must not destabilise attitude: peak tilt {peak} rad"); - assert!((backend.pos[2] + 2.0).abs() < 0.4, "altitude held under mag interference: {}", backend.pos[2]); + assert!( + peak < 0.15, + "mag interference must not destabilise attitude: peak tilt {peak} rad" + ); + assert!( + (backend.pos[2] + 2.0).abs() < 0.4, + "altitude held under mag interference: {}", + backend.pos[2] + ); } // ── v1.11 — the real-hardware backend SEAM, composed end-to-end ─────── @@ -4019,7 +4296,10 @@ mod tests { let d_pd = fly_under_wind([3.0, 4.0, 0.0], 0.0, Some(0.0), 30000); let d_pid = fly_under_wind([3.0, 4.0, 0.0], 0.0, None, 30000); assert!(d_pd > 1.5, "P-D alone should offset under wind: {d_pd} m"); - assert!(d_pid < d_pd * 0.5, "the integral must cut the offset: PID {d_pid} m vs PD {d_pd} m"); + assert!( + d_pid < d_pd * 0.5, + "the integral must cut the offset: PID {d_pid} m vs PD {d_pd} m" + ); } /// Gusts on top of the steady wind stay bounded — the integral tracks the @@ -4027,7 +4307,10 @@ mod tests { #[test] fn rejects_wind_gusts() { let d = fly_under_wind([3.0, 0.0, 0.0], 2.0, None, 30000); // 3 m/s + 2 m/s gusts - assert!(d < 1.0, "P-I-D must keep gusty wind bounded: {d} m from home"); + assert!( + d < 1.0, + "P-I-D must keep gusty wind bounded: {d} m from home" + ); } // ── v1.17 aerodynamic drag (quadratic, ∝ v²) ────────────────────────── @@ -4107,7 +4390,10 @@ mod tests { } // after ~40 s the injected bias has random-walked to ≈0.01·√40 ≈ 0.06 // rad/s; the IEKF tracks it, so the steady tilt stays bounded. - assert!(peak < 0.15, "IEKF must hold under random-walk gyro bias: peak tilt {peak} rad"); + assert!( + peak < 0.15, + "IEKF must hold under random-walk gyro bias: peak tilt {peak} rad" + ); } // ── v1.19 GNSS realism: continuous position noise + INTERMITTENT periodic @@ -4145,7 +4431,10 @@ mod tests { #[test] fn holds_under_noisy_intermittent_gps() { let peak = fly_noisy_gps(0.09); // var = (0.3 m)² — matched to the fix noise - assert!(peak < 1.5, "variance-matched filter must hold under noisy GNSS: peak {peak} m"); + assert!( + peak < 1.5, + "variance-matched filter must hold under noisy GNSS: peak {peak} m" + ); } /// FALSIFICATION: the optimistic default variance (0.01 = 1 cm²) over-trusts @@ -4156,8 +4445,14 @@ mod tests { fn optimistic_variance_diverges_under_noisy_gps() { let peak_optimistic = fly_noisy_gps(0.01); // default 1 cm² — over-trusting let peak_matched = fly_noisy_gps(0.09); // matched - assert!(peak_optimistic > 10.0, "over-trust should diverge: {peak_optimistic} m"); - assert!(peak_matched < peak_optimistic * 0.1, "matched must be far better: {peak_matched} vs {peak_optimistic} m"); + assert!( + peak_optimistic > 10.0, + "over-trust should diverge: {peak_optimistic} m" + ); + assert!( + peak_matched < peak_optimistic * 0.1, + "matched must be far better: {peak_matched} vs {peak_optimistic} m" + ); } // ── v1.20 barometer fusion: an independent vertical source so altitude @@ -4191,7 +4486,10 @@ mod tests { fn baro_holds_altitude_through_gps_loss() { let err_baro = alt_err_through_gps_loss(true); let err_nobaro = alt_err_through_gps_loss(false); - assert!(err_baro < 1.5, "baro must hold altitude through GPS loss: {err_baro} m"); + assert!( + err_baro < 1.5, + "baro must hold altitude through GPS loss: {err_baro} m" + ); assert!( err_baro < err_nobaro, "baro must beat GPS-only dead-reckoning: baro {err_baro} vs no-baro {err_nobaro} m" @@ -4226,7 +4524,10 @@ mod tests { } } assert!(fired, "draining battery must actuate the failsafe"); - assert!(min_v < 14.0, "voltage must have SAGGED below threshold from drain, not set: {min_v} V"); + assert!( + min_v < 14.0, + "voltage must have SAGGED below threshold from drain, not set: {min_v} V" + ); assert!( backend.battery_charge < 0.9, "charge must have genuinely depleted: {}", @@ -4269,8 +4570,15 @@ mod tests { fn read_imu(&mut self) -> ImuSample { // tilted: gravity measured along body-x ⇒ the estimate converges // to a ~90° tilt (well past the runaway limit); else level. - let accel = if self.tilted { [GRAVITY, 0.0, 0.0] } else { [0.0, 0.0, -GRAVITY] }; - ImuSample { accel, gyro: [0.0; 3] } + let accel = if self.tilted { + [GRAVITY, 0.0, 0.0] + } else { + [0.0, 0.0, -GRAVITY] + }; + ImuSample { + accel, + gyro: [0.0; 3], + } } fn read_position(&mut self) -> Option { Some([0.0, 0.0, 0.0]) @@ -4287,7 +4595,10 @@ mod tests { } } let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 50.0, 2.0, 14.0); - sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); let mut b = TumbleBackend { tilted: false }; // converge level, then arm + take off (airborne, still level). @@ -4302,7 +4613,11 @@ mod tests { for _ in 0..200 { sup.step(&mut b); } - assert_ne!(sup.mode(), relay_fsm::Mode::Terminated, "level flight must not terminate"); + assert_ne!( + sup.mode(), + relay_fsm::Mode::Terminated, + "level flight must not terminate" + ); // now a sustained tumble → flight termination. The tilt blows through the // high-wind band (~47 cycles, < the wind debounce) into the runaway range, @@ -4311,7 +4626,11 @@ mod tests { for _ in 0..3000 { sup.step(&mut b); } - assert_eq!(sup.mode(), relay_fsm::Mode::Terminated, "sustained attitude runaway cuts motors"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Terminated, + "sustained attitude runaway cuts motors" + ); } /// v1.101 expanded failsafe — HIGH WIND: control saturation while leaning hard @@ -4335,7 +4654,10 @@ mod tests { } else { [0.0, 0.0, -GRAVITY] }; - ImuSample { accel, gyro: [0.0; 3] } + ImuSample { + accel, + gyro: [0.0; 3], + } } fn read_position(&mut self) -> Option { Some([100.0, 0.0, 0.0]) // away from home: an RTL flies, never lands @@ -4354,7 +4676,10 @@ mod tests { // effectively-infinite fence so the tilt-corrupted position estimate can // never trip the GEOFENCE failsafe — isolating the high-wind path. let mut sup = FlightSupervisor::new([0.0, 0.0, 0.0], 1.0e9, 2.0, 14.0); - sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); let mut b = WindBackend { windy: false }; // converge level, arm, take off. for _ in 0..1500 { @@ -4377,15 +4702,26 @@ mod tests { sup.step(&mut b); // the FIRST failsafe to fire does so from Takeoff → Rtl (recovery). if sup.rtl_latched && before == relay_fsm::Mode::Takeoff { - fired_at_takeoff = sup.mode() == relay_fsm::Mode::Rtl || sup.mode() == relay_fsm::Mode::Land; + fired_at_takeoff = + sup.mode() == relay_fsm::Mode::Rtl || sup.mode() == relay_fsm::Mode::Land; } if sup.rtl_latched { break; } } - assert!(sup.rtl_latched, "sustained high-wind saturation must fire the RTL-class failsafe"); - assert!(fired_at_takeoff, "the failsafe fired from normal flight (Takeoff → RTL recovery)"); - assert_ne!(sup.mode(), relay_fsm::Mode::Terminated, "high wind recovers (RTL), it does NOT terminate"); + assert!( + sup.rtl_latched, + "sustained high-wind saturation must fire the RTL-class failsafe" + ); + assert!( + fired_at_takeoff, + "the failsafe fired from normal flight (Takeoff → RTL recovery)" + ); + assert_ne!( + sup.mode(), + relay_fsm::Mode::Terminated, + "high wind recovers (RTL), it does NOT terminate" + ); } /// #413 — ABSENCE IS NOT HEALTH. A backend with no battery sense must make @@ -4406,7 +4742,10 @@ mod tests { struct NoBatteryBackend; impl FlightBackend for NoBatteryBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + ImuSample { + accel: [0.0, 0.0, -GRAVITY], + gyro: [0.0; 3], + } } fn read_position(&mut self) -> Option { Some([0.0, 0.0, 0.0]) @@ -4425,7 +4764,10 @@ mod tests { let mut b = NoBatteryBackend; // A calibration is installed so that CALIBRATION is not what blocks us — // otherwise this test would pass for the wrong reason. - sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); for _ in 0..1500 { sup.step(&mut b); } @@ -4456,7 +4798,10 @@ mod tests { } impl FlightBackend for RestBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: [0.0, 0.0, -GRAVITY], gyro: [0.0; 3] } + ImuSample { + accel: [0.0, 0.0, -GRAVITY], + gyro: [0.0; 3], + } } fn read_position(&mut self) -> Option { Some([0.0, 0.0, 0.0]) @@ -4481,26 +4826,50 @@ mod tests { sup.step(&mut b); } sup.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup.mode(), relay_fsm::Mode::Disarmed, "must not arm without calibration"); - assert_eq!(sup.arm_blocked_reason(), Some(relay_preflight::CheckFail::Calibration)); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Disarmed, + "must not arm without calibration" + ); + assert_eq!( + sup.arm_blocked_reason(), + Some(relay_preflight::CheckFail::Calibration) + ); // install a (non-identity) calibration → step once to refresh → arms. - sup.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); sup.step(&mut b); assert_eq!(sup.arm_blocked_reason(), None, "all real checks pass"); sup.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup.mode(), relay_fsm::Mode::Armed, "arms once the real signals are good"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Armed, + "arms once the real signals are good" + ); // a low battery (read each step) blocks re-arming after a disarm. let mut low = RestBackend { batt: 13.0 }; let mut sup2 = FlightSupervisor::new([0.0, 0.0, 0.0], 50.0, 2.0, 14.0); - sup2.set_calibration(CalParams { gyro_bias: [0.001, 0.0, 0.0], ..CalParams::identity() }); + sup2.set_calibration(CalParams { + gyro_bias: [0.001, 0.0, 0.0], + ..CalParams::identity() + }); for _ in 0..1500 { sup2.step(&mut low); } sup2.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup2.mode(), relay_fsm::Mode::Disarmed, "low battery blocks arming"); - assert_eq!(sup2.arm_blocked_reason(), Some(relay_preflight::CheckFail::Battery)); + assert_eq!( + sup2.mode(), + relay_fsm::Mode::Disarmed, + "low battery blocks arming" + ); + assert_eq!( + sup2.arm_blocked_reason(), + Some(relay_preflight::CheckFail::Battery) + ); } /// v1.97 pre-arm gate (the seam): the FlightSupervisor refuses to arm unless @@ -4514,7 +4883,11 @@ mod tests { // all checks FAILING (Default = all false): arming refused, reason = first. sup.set_preflight(PreflightChecks::default()); sup.command(relay_fsm::Event::Arm, true, true); // level + throttle idle - assert_eq!(sup.mode(), relay_fsm::Mode::Disarmed, "no arm with failed pre-arm checks"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Disarmed, + "no arm with failed pre-arm checks" + ); assert_eq!(sup.arm_blocked_reason(), Some(CheckFail::Sensors)); // only the battery failing → blocked on Battery, still won't arm. @@ -4541,13 +4914,20 @@ mod tests { }); assert_eq!(sup.arm_blocked_reason(), None); sup.command(relay_fsm::Event::Arm, true, true); - assert_eq!(sup.mode(), relay_fsm::Mode::Armed, "arms once every pre-arm check passes"); + assert_eq!( + sup.mode(), + relay_fsm::Mode::Armed, + "arms once every pre-arm check passes" + ); } #[test] fn holds_altitude_under_thrust_lapse() { let alt = final_altitude_under_lapse(20.0, 0.01, Some(0.02)); // integral ON - assert!((alt - 20.0).abs() < 1.0, "must hold 20 m despite thrust lapse: {alt} m"); + assert!( + (alt - 20.0).abs() < 1.0, + "must hold 20 m despite thrust lapse: {alt} m" + ); } /// FALSIFICATION: with the altitude integral DISABLED (P-D only) the thrust @@ -4557,7 +4937,10 @@ mod tests { fn bare_altitude_pd_sags_under_lapse() { let alt_pd = final_altitude_under_lapse(20.0, 0.01, Some(0.0)); // integral OFF let alt_pid = final_altitude_under_lapse(20.0, 0.01, Some(0.02)); // integral ON - assert!(alt_pd < 19.0, "P-D alone should sag below target under lapse: {alt_pd} m"); + assert!( + alt_pd < 19.0, + "P-D alone should sag below target under lapse: {alt_pd} m" + ); assert!( (alt_pid - 20.0).abs() < 1.0 && alt_pid > alt_pd, "the integral must close the gap: pid {alt_pid} vs pd {alt_pd} m" @@ -4619,7 +5002,10 @@ mod tests { } let e = [b.pos[0] - 1.5, b.pos[1] + 1.0, b.pos[2] + 2.0]; let err = relay_math::sqrtf(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]); - assert!(err < 0.6, "position hold must stay bounded under motor lag: {err} m"); + assert!( + err < 0.6, + "position hold must stay bounded under motor lag: {err} m" + ); } // ── v1.24 ground effect: a thrust cushion near the surface (landing/takeoff). @@ -4639,7 +5025,10 @@ mod tests { core.step(&mut b); } let alt = -b.pos[2]; - assert!((alt - 2.0).abs() < 0.3, "ground effect must aid takeoff to altitude: {alt} m"); + assert!( + (alt - 2.0).abs() < 0.3, + "ground effect must aid takeoff to altitude: {alt} m" + ); } /// HONEST LIMITATION (documented, not faked): ground effect cushions the @@ -4683,8 +5072,10 @@ mod tests { fn holds_position_under_turbulence() { let dt = 0.002f32; let level = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let mut b = SimBackend::new(level, dt) - .with_pathology(Pathology { turbulence: 2.0, ..Default::default() }); + let mut b = SimBackend::new(level, dt).with_pathology(Pathology { + turbulence: 2.0, + ..Default::default() + }); let mut core = FlightCore::new(0.5, 1.0 / dt); core.set_position([0.0, 0.0, -2.0]); let mut peak = 0.0f32; @@ -4698,7 +5089,10 @@ mod tests { // bounded under continuous turbulence — it does not diverge (an // over-authority wind blew the vehicle to 100s of metres; this rides // out the persistent gusts within a few metres). - assert!(peak < 4.0, "turbulence must stay bounded (not diverge): peak {peak} m"); + assert!( + peak < 4.0, + "turbulence must stay bounded (not diverge): peak {peak} m" + ); } // ── v1.27 velocity-based touchdown: the clean landing the v1.24 ground- @@ -4725,8 +5119,15 @@ mod tests { core.step(&mut b); } let alt = -b.pos[2]; - assert!(alt < 0.15, "velocity touchdown must reach the surface through ground effect: {alt} m"); - assert!(b.vel[2].abs() < 0.3, "should settle on touchdown: vz {} m/s", b.vel[2]); + assert!( + alt < 0.15, + "velocity touchdown must reach the surface through ground effect: {alt} m" + ); + assert!( + b.vel[2].abs() < 0.3, + "should settle on touchdown: vz {} m/s", + b.vel[2] + ); } // ── v1.29: wire the v1.27 velocity-landing into the FlightSupervisor ── @@ -4757,7 +5158,11 @@ mod tests { for _ in 0..8000 { sup.step(&mut b); } - assert_eq!(sup.mode(), Mode::Loiter, "should reach Loiter after takeoff"); + assert_eq!( + sup.mode(), + Mode::Loiter, + "should reach Loiter after takeoff" + ); sup.command(Event::RequestLand, true, false); // → Land (velocity touchdown) let mut disarmed = false; @@ -4778,7 +5183,11 @@ mod tests { // Touchdown→Disarmed interrupts the descent at the 0.15 m trigger and the // disarmed position-hold settles just above it through ground effect — on // the surface (vs the ~1.3 m float without the velocity-landing). - assert!(-b.pos[2] < 0.25, "must settle on the surface (not float): {} m", -b.pos[2]); + assert!( + -b.pos[2] < 0.25, + "must settle on the surface (not float): {} m", + -b.pos[2] + ); } /// v1.117 (FAULT-P04) — the SUPERVISED rotor-out chain ends on the ground: @@ -4803,7 +5212,11 @@ mod tests { for _ in 0..8000 { sup.step(&mut b); } - assert_eq!(sup.mode(), Mode::Loiter, "should reach Loiter after takeoff"); + assert_eq!( + sup.mode(), + Mode::Loiter, + "should reach Loiter after takeoff" + ); // The failure must be injected from a SETTLED hover — otherwise the // sink metrics measure the pre-existing limit cycle, not the recovery // (this precondition caught exactly that on first write). @@ -4841,14 +5254,22 @@ mod tests { break; } } - assert!(land_cmded, "motor failsafe must command Land (mode {:?})", sup.mode()); + assert!( + land_cmded, + "motor failsafe must command Land (mode {:?})", + sup.mode() + ); assert!( landed, "supervised rotor-out must touch down + disarm: mode {:?}, alt {} m", sup.mode(), -b.pos[2] ); - assert!(-b.pos[2] < 0.25, "must settle on the surface: {} m", -b.pos[2]); + assert!( + -b.pos[2] < 0.25, + "must settle on the surface: {} m", + -b.pos[2] + ); // The lift-loss transient is bounded (arrested well short of freefall // from 2 m ≈ 6.3 m/s) and the FINAL APPROACH is gentle (the velocity // landing commands 0.5 m/s; allow margin for the rotor-out wobble). @@ -4856,10 +5277,20 @@ mod tests { peak_sink_transient < 4.0, "lift-loss transient must be arrested: peak {peak_sink_transient} m/s" ); - assert!(approach_sink < 1.0, "final approach must be gentle: {approach_sink} m/s"); - assert!(peak_tilt < 0.5, "must stay near-level during the descent: peak {peak_tilt} rad"); + assert!( + approach_sink < 1.0, + "final approach must be gentle: {approach_sink} m/s" + ); + assert!( + peak_tilt < 0.5, + "must stay near-level during the descent: peak {peak_tilt} rad" + ); // Upright on the ground. - assert!(b.tilt() < 0.35, "must be upright at touchdown: {} rad", b.tilt()); + assert!( + b.tilt() < 0.35, + "must be upright at touchdown: {} rad", + b.tilt() + ); } } @@ -4963,7 +5394,11 @@ mod failsafe_campaign { pos: [0.0, 0.0, 0.0], }; airborne(&mut sup, &mut b); - assert_ne!(sup.mode(), Mode::Terminated, "trial {i}: terminated on level takeoff"); + assert_ne!( + sup.mode(), + Mode::Terminated, + "trial {i}: terminated on level takeoff" + ); if runaway { b.tilt = rng.range(1.15, 1.50); // strictly > TILT_RUNAWAY_LIMIT for _ in 0..4000 { @@ -4980,7 +5415,11 @@ mod failsafe_campaign { for _ in 0..500 { sup.step(&mut b); } - assert_eq!(sup.mode(), Mode::Terminated, "trial {i}: terminate did not latch"); + assert_eq!( + sup.mode(), + Mode::Terminated, + "trial {i}: terminate did not latch" + ); term_fires += 1; } else { b.tilt = rng.range(0.0, 0.95); // strictly < TILT_RUNAWAY_LIMIT diff --git a/crates/falcon-core/plain/src/tuning.rs b/crates/falcon-core/plain/src/tuning.rs index bf7bb96d..acaeae54 100644 --- a/crates/falcon-core/plain/src/tuning.rs +++ b/crates/falcon-core/plain/src/tuning.rs @@ -10,7 +10,7 @@ //! K01-proven store write, then the setters' own clamps). use crate::FlightCore; -use relay_param::{param_id, ParamDef, ParamStore}; +use relay_param::{ParamDef, ParamStore, param_id}; /// Register falcon's tunable knobs (schema bounds + current-behavior /// defaults). Idempotent per store; returns false if the store lacks room. @@ -18,13 +18,38 @@ pub fn register_tuning(store: &mut ParamStore) -> bool { let defs = [ // Altitude P-I-D (the gz-reconciled defaults are per-plant; these // are the analytic-plant baseline the core constructs with). - ParamDef { id: param_id("MC_ALT_P"), min: 0.01, max: 1.0, default: 0.05 }, - ParamDef { id: param_id("MC_ALT_D"), min: 0.0, max: 3.0, default: 0.30 }, - ParamDef { id: param_id("MC_ALT_I"), min: 0.0, max: 0.2, default: 0.0 }, + ParamDef { + id: param_id("MC_ALT_P"), + min: 0.01, + max: 1.0, + default: 0.05, + }, + ParamDef { + id: param_id("MC_ALT_D"), + min: 0.0, + max: 3.0, + default: 0.30, + }, + ParamDef { + id: param_id("MC_ALT_I"), + min: 0.0, + max: 0.2, + default: 0.0, + }, // Hover-thrust feedforward (per-airframe). - ParamDef { id: param_id("MC_HOVER_THR"), min: 0.2, max: 0.8, default: 0.5 }, + ParamDef { + id: param_id("MC_HOVER_THR"), + min: 0.2, + max: 0.8, + default: 0.5, + }, // Landing descent rate (m/s, NED +down). - ParamDef { id: param_id("MC_LAND_VZ"), min: 0.2, max: 1.5, default: 0.5 }, + ParamDef { + id: param_id("MC_LAND_VZ"), + min: 0.2, + max: 1.5, + default: 0.5, + }, ]; for d in defs { if !store.register(d) { diff --git a/crates/falcon-esc-dshot/src/lib.rs b/crates/falcon-esc-dshot/src/lib.rs index ae746919..bda0cd4d 100644 --- a/crates/falcon-esc-dshot/src/lib.rs +++ b/crates/falcon-esc-dshot/src/lib.rs @@ -95,10 +95,22 @@ impl BatteryMonitor { /// `volts_per_count` calibrates the ADC + divider; `low_v`/`critical_v` are /// the failsafe thresholds (critical ≤ low). Degenerate args are sanitised. pub fn new(volts_per_count: f32, low_v: f32, critical_v: f32) -> Self { - let vpc = if volts_per_count.is_finite() && volts_per_count > 0.0 { volts_per_count } else { 0.0 }; + let vpc = if volts_per_count.is_finite() && volts_per_count > 0.0 { + volts_per_count + } else { + 0.0 + }; let low = if low_v.is_finite() { low_v } else { 0.0 }; - let crit = if critical_v.is_finite() { critical_v.min(low) } else { 0.0 }; - Self { volts_per_count: vpc, low_v: low, critical_v: crit } + let crit = if critical_v.is_finite() { + critical_v.min(low) + } else { + 0.0 + }; + Self { + volts_per_count: vpc, + low_v: low, + critical_v: crit, + } } /// Pack voltage from a raw ADC count. @@ -168,7 +180,11 @@ pub struct BatteryAdc { impl BatteryAdc { /// Wrap an `AdcIn` reading rail `channel`, with the given monitor calibration. pub fn new(adc: A, channel: u8, monitor: BatteryMonitor) -> Self { - Self { adc, channel, monitor } + Self { + adc, + channel, + monitor, + } } /// Async read of the battery state: pull the raw count via `AdcIn`, then @@ -292,7 +308,10 @@ mod tests { #[test] fn esc_send_emits_dshot_frames() { let motors = [0.0_f32, 0.25, 0.5, 1.0]; - let mut esc = DShotEsc::new(MockPwm { last: None, fail: false }); + let mut esc = DShotEsc::new(MockPwm { + last: None, + fail: false, + }); block_on(esc.send(&motors)).unwrap(); let sent = esc.release().last.expect("a frame batch was sent"); let expected = [ @@ -307,7 +326,10 @@ mod tests { /// The async path is fallible: a sink error propagates as Err. #[test] fn esc_send_propagates_sink_error() { - let mut esc = DShotEsc::new(MockPwm { last: None, fail: true }); + let mut esc = DShotEsc::new(MockPwm { + last: None, + fail: true, + }); assert_eq!(block_on(esc.send(&[0.0; 4])), Err(MockPwmError)); } @@ -348,7 +370,14 @@ mod tests { #[test] fn battery_adc_propagates_error() { let monitor = BatteryMonitor::new(0.016, 14.0, 13.2); - let mut bat = BatteryAdc::new(MockAdc { count: 0, fail: true }, 3, monitor); + let mut bat = BatteryAdc::new( + MockAdc { + count: 0, + fail: true, + }, + 3, + monitor, + ); assert_eq!(block_on(bat.read_state()), Err(MockPwmError)); } diff --git a/crates/falcon-gnss-ubx/src/dual.rs b/crates/falcon-gnss-ubx/src/dual.rs index ccec6544..53b20310 100644 --- a/crates/falcon-gnss-ubx/src/dual.rs +++ b/crates/falcon-gnss-ubx/src/dual.rs @@ -79,7 +79,8 @@ pub const DIVERGE_FLOOR_M: f32 = 5.0; pub const DIVERGE_DEBOUNCE: u32 = 10; fn sane3(p: &[f32; 3]) -> bool { - p.iter().all(|v| v.is_finite() && *v >= -MAX_POS_M && *v <= MAX_POS_M) + p.iter() + .all(|v| v.is_finite() && *v >= -MAX_POS_M && *v <= MAX_POS_M) } /// Squared 2D distance — every gate compares SQUARED quantities (order- @@ -101,7 +102,10 @@ fn healthy(fix: &Option, est: Option<[f32; 3]>) -> Option { if !sane3(&f.pos) || !f.acc_m.is_finite() || f.acc_m <= 0.0 || f.acc_m > MAX_ACC_M { return None; } - let f = NedFix { acc_m: f.acc_m.max(MIN_ACC_M), ..f }; + let f = NedFix { + acc_m: f.acc_m.max(MIN_ACC_M), + ..f + }; if let Some(e) = est { if sane3(&e) && dist2d_sq(&f.pos, &e) > INNOVATION_GATE_M * INNOVATION_GATE_M { return None; @@ -125,7 +129,10 @@ impl Default for DualGnss { impl DualGnss { pub fn new() -> Self { - DualGnss { diverge_count: 0, diverged: false } + DualGnss { + diverge_count: 0, + diverged: false, + } } /// Clear the latched divergence flag (ground reset only). @@ -183,8 +190,17 @@ impl DualGnss { // Blended accuracy: conservatively the better receiver's // (the true inverse-variance value is smaller; reporting // min keeps the field sqrt-free and never over-claims). - let acc = if fa.acc_m <= fb.acc_m { fa.acc_m } else { fb.acc_m }; - GnssDecision { pos: Some(pos), acc_m: acc, source: GnssSource::Blend, diverged: false } + let acc = if fa.acc_m <= fb.acc_m { + fa.acc_m + } else { + fb.acc_m + }; + GnssDecision { + pos: Some(pos), + acc_m: acc, + source: GnssSource::Blend, + diverged: false, + } } (Some(f), None) => GnssDecision { pos: Some(f.pos), @@ -217,7 +233,12 @@ mod tests { use super::*; fn fix(x: f32, y: f32, acc: f32) -> Option { - Some(NedFix { pos: [x, y, -10.0], acc_m: acc, sats: 12, fix_ok: true }) + Some(NedFix { + pos: [x, y, -10.0], + acc_m: acc, + sats: 12, + fix_ok: true, + }) } /// Both healthy: the blend lies between the fixes, weighted toward the @@ -228,8 +249,15 @@ mod tests { let dec = d.update(fix(0.0, 0.0, 1.0), fix(1.0, 0.0, 2.0), None); let p = dec.pos.unwrap(); assert_eq!(dec.source, GnssSource::Blend); - assert!(p[0] > 0.0 && p[0] < 0.5, "weighted toward A (acc 1 vs 2): {}", p[0]); - assert!(dec.acc_m <= 1.0, "blend accuracy never worse than the best receiver"); + assert!( + p[0] > 0.0 && p[0] < 0.5, + "weighted toward A (acc 1 vs 2): {}", + p[0] + ); + assert!( + dec.acc_m <= 1.0, + "blend accuracy never worse than the best receiver" + ); assert!(!dec.diverged); } @@ -245,7 +273,11 @@ mod tests { assert_eq!(after.source, GnssSource::B); let p1 = after.pos.unwrap(); let step = ((p1[0] - p0[0]).powi(2) + (p1[1] - p0[1]).powi(2)).sqrt(); - assert!(step <= 1.0, "failover step {} must stay within B's accuracy", step); + assert!( + step <= 1.0, + "failover step {} must stay within B's accuracy", + step + ); } /// Degradation failovers: accuracy collapse and a jump the innovation @@ -254,14 +286,27 @@ mod tests { fn accuracy_collapse_and_jump_disqualify() { let mut d = DualGnss::new(); let dec = d.update( - Some(NedFix { pos: [0.0; 3], acc_m: 50.0, sats: 12, fix_ok: true }), + Some(NedFix { + pos: [0.0; 3], + acc_m: 50.0, + sats: 12, + fix_ok: true, + }), fix(0.1, 0.0, 1.0), None, ); - assert_eq!(dec.source, GnssSource::B, "acc 50 m > ceiling disqualifies A"); + assert_eq!( + dec.source, + GnssSource::B, + "acc 50 m > ceiling disqualifies A" + ); // jump: estimator at origin, A reports 40 m away. let dec = d.update(fix(40.0, 0.0, 1.0), fix(0.1, 0.0, 1.0), Some([0.0; 3])); - assert_eq!(dec.source, GnssSource::B, "innovation gate rejects the jump"); + assert_eq!( + dec.source, + GnssSource::B, + "innovation gate rejects the jump" + ); } /// Divergence: sustained disagreement latches the flag; the selector @@ -273,11 +318,19 @@ mod tests { let mut d = DualGnss::new(); let mut last = None; for _ in 0..DIVERGE_DEBOUNCE { - last = Some(d.update(fix(0.0, 0.0, 1.0), fix(10.0, 0.0, 1.0), Some([2.0, 0.0, -10.0]))); + last = Some(d.update( + fix(0.0, 0.0, 1.0), + fix(10.0, 0.0, 1.0), + Some([2.0, 0.0, -10.0]), + )); } let dec = last.unwrap(); assert!(dec.diverged, "sustained 10 m split must latch divergence"); - assert_eq!(dec.source, GnssSource::A, "picks the estimator-consistent side"); + assert_eq!( + dec.source, + GnssSource::A, + "picks the estimator-consistent side" + ); // and it stays latched through re-agreement: let dec = d.update(fix(0.0, 0.0, 1.0), fix(0.1, 0.0, 1.0), None); assert!(dec.diverged, "divergence is latched"); @@ -303,8 +356,18 @@ mod tests { assert!(dec.pos.is_none()); assert_eq!(dec.source, GnssSource::None); let dec = d.update( - Some(NedFix { pos: [f32::NAN; 3], acc_m: 1.0, sats: 12, fix_ok: true }), - Some(NedFix { pos: [0.0; 3], acc_m: 1.0, sats: 3, fix_ok: true }), + Some(NedFix { + pos: [f32::NAN; 3], + acc_m: 1.0, + sats: 12, + fix_ok: true, + }), + Some(NedFix { + pos: [0.0; 3], + acc_m: 1.0, + sats: 3, + fix_ok: true, + }), None, ); assert!(dec.pos.is_none(), "NaN pos and 3 sats both unhealthy"); diff --git a/crates/falcon-gnss-ubx/src/lib.rs b/crates/falcon-gnss-ubx/src/lib.rs index 7069c829..54919ea1 100644 --- a/crates/falcon-gnss-ubx/src/lib.rs +++ b/crates/falcon-gnss-ubx/src/lib.rs @@ -120,7 +120,11 @@ impl UbxParser { } } State::Sync2 => { - self.state = if byte == SYNC2 { State::Header } else { State::Sync1 }; + self.state = if byte == SYNC2 { + State::Header + } else { + State::Sync1 + }; self.hdr_n = 0; self.ck_a = 0; self.ck_b = 0; @@ -230,7 +234,10 @@ pub struct UbxReader { impl UbxReader { /// Wrap an async byte source. pub fn new(rx: R) -> Self { - Self { rx, parser: UbxParser::new() } + Self { + rx, + parser: UbxParser::new(), + } } /// Read bytes until a complete, checksum-valid NAV-PVT frame decodes, then @@ -337,7 +344,10 @@ mod tests { got = Some(pvt); } } - assert!(got.is_some(), "the parser must resync and decode the real frame"); + assert!( + got.is_some(), + "the parser must resync and decode the real frame" + ); } // ── async stream path (embedded-io-async Read) + inter-core carrier ────── @@ -416,7 +426,12 @@ mod tests { } let sync_fix = sync_fix.unwrap(); // async Read path - let mut rdr = UbxReader::new(MockRx { data, len: n, pos: 0, fail: false }); + let mut rdr = UbxReader::new(MockRx { + data, + len: n, + pos: 0, + fail: false, + }); let async_fix = block_on(rdr.read_fix()).unwrap(); assert_eq!(sync_fix, async_fix); } @@ -424,7 +439,12 @@ mod tests { /// The async path is fallible: a read transport error propagates as Err. #[test] fn gnss_async_propagates_read_error() { - let mut rdr = UbxReader::new(MockRx { data: [0; 128], len: 0, pos: 0, fail: true }); + let mut rdr = UbxReader::new(MockRx { + data: [0; 128], + len: 0, + pos: 0, + fail: true, + }); assert!(block_on(rdr.read_fix()).is_err()); } diff --git a/crates/falcon-hitl/src/lib.rs b/crates/falcon-hitl/src/lib.rs index f024a824..91084894 100644 --- a/crates/falcon-hitl/src/lib.rs +++ b/crates/falcon-hitl/src/lib.rs @@ -134,7 +134,11 @@ impl LinkBackend { let zero = [0u8; ACTUATOR_FRAME_LEN]; let mut reply = [0u8; SENSOR_FRAME_LEN]; transport.exchange(&zero, &mut reply); - LinkBackend { transport, cache: decode_sensor(&reply), dt } + LinkBackend { + transport, + cache: decode_sensor(&reply), + dt, + } } /// The latest sensor frame (telemetry / tests). @@ -145,7 +149,10 @@ impl LinkBackend { impl FlightBackend for LinkBackend { fn read_imu(&mut self) -> ImuSample { - ImuSample { accel: self.cache.accel, gyro: self.cache.gyro } + ImuSample { + accel: self.cache.accel, + gyro: self.cache.gyro, + } } fn read_position(&mut self) -> Option { self.cache.pos_valid.then_some(self.cache.pos) @@ -228,11 +235,7 @@ mod tests { server: SimServer, } impl Transport for Loopback { - fn exchange( - &mut self, - out: &[u8; ACTUATOR_FRAME_LEN], - reply: &mut [u8; SENSOR_FRAME_LEN], - ) { + fn exchange(&mut self, out: &[u8; ACTUATOR_FRAME_LEN], reply: &mut [u8; SENSOR_FRAME_LEN]) { self.server.serve(out, reply); } } diff --git a/crates/falcon-imu-icm42688/src/lib.rs b/crates/falcon-imu-icm42688/src/lib.rs index 283959c4..00df1bac 100644 --- a/crates/falcon-imu-icm42688/src/lib.rs +++ b/crates/falcon-imu-icm42688/src/lib.rs @@ -207,16 +207,17 @@ mod tests { } impl MockBus { fn new(who: u8, data: [u8; 12]) -> Self { - MockBus { who, data, writes: [(0, 0); 8], n_writes: 0 } + MockBus { + who, + data, + writes: [(0, 0); 8], + n_writes: 0, + } } } impl RegBus for MockBus { fn read_reg(&mut self, reg: u8) -> u8 { - if reg == REG_WHO_AM_I { - self.who - } else { - 0 - } + if reg == REG_WHO_AM_I { self.who } else { 0 } } fn write_reg(&mut self, reg: u8, val: u8) { if self.n_writes < self.writes.len() { @@ -225,7 +226,10 @@ mod tests { } } fn read_burst(&mut self, reg: u8, buf: &mut [u8]) { - assert_eq!(reg, REG_ACCEL_DATA_X1, "driver must burst from the data block"); + assert_eq!( + reg, REG_ACCEL_DATA_X1, + "driver must burst from the data block" + ); buf.copy_from_slice(&self.data); } } @@ -262,8 +266,16 @@ mod tests { let s = imu.read(); assert!((s.accel[0]).abs() < 1e-4); assert!((s.accel[1]).abs() < 1e-4); - assert!((s.accel[2] - 9.80665).abs() < 1e-3, "AZ → 1 g: {}", s.accel[2]); - assert!((s.gyro[0] - 0.174533).abs() < 1e-4, "GX → 10 dps: {}", s.gyro[0]); + assert!( + (s.accel[2] - 9.80665).abs() < 1e-3, + "AZ → 1 g: {}", + s.accel[2] + ); + assert!( + (s.gyro[0] - 0.174533).abs() < 1e-4, + "GX → 10 dps: {}", + s.gyro[0] + ); assert!((s.gyro[1]).abs() < 1e-6 && (s.gyro[2]).abs() < 1e-6); } @@ -282,7 +294,11 @@ mod tests { let mut imu = Icm42688::new(MockBus::new(WHO_AM_I_VALUE, data)); imu.init().unwrap(); let s = imu.read(); - assert!((s.accel[2] + 9.80665).abs() < 1e-3, "AZ → −1 g: {}", s.accel[2]); + assert!( + (s.accel[2] + 9.80665).abs() < 1e-3, + "AZ → −1 g: {}", + s.accel[2] + ); } /// The driver satisfies the seam: it plugs into a `HardwareBackend` as the @@ -352,7 +368,9 @@ mod tests { #[test] fn async_spi_path_matches_sync_decode() { // −1 g on AZ, +2000-dps-ish on GX, arbitrary on the rest. - let data = [0x10, 0x00, 0xF0, 0x00, 0xF8, 0x00, 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB]; + let data = [ + 0x10, 0x00, 0xF0, 0x00, 0xF8, 0x00, 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, + ]; // sync RegBus path let mut sync_imu = Icm42688::new(MockBus::new(WHO_AM_I_VALUE, data)); diff --git a/crates/falcon-param/plain/src/lib.rs b/crates/falcon-param/plain/src/lib.rs index 64687d8e..701262e1 100644 --- a/crates/falcon-param/plain/src/lib.rs +++ b/crates/falcon-param/plain/src/lib.rs @@ -33,7 +33,7 @@ #![no_std] #![forbid(unsafe_code)] -use relay_mavlink::{ParamRequestRead, ParamSet, ParamValue, MAV_PARAM_TYPE_REAL32}; +use relay_mavlink::{MAV_PARAM_TYPE_REAL32, ParamRequestRead, ParamSet, ParamValue}; use relay_param::{ParamId, ParamStore, SetResult}; /// The (index, count) of a parameter id, by scanning the store — the PARAM_VALUE @@ -158,18 +158,24 @@ mod tuning_tests { assert_eq!(core.altitude_gains().0, 0.15, "live on the very next cycle"); // Out-of-range: rejected at the store; the core NEVER sees it. - let bad = ParamSet { param_value: 99.0, ..set }; + let bad = ParamSet { + param_value: 99.0, + ..set + }; let ack = on_param_set(&mut store, &bad).expect("known param"); assert_eq!(ack.param_value, 0.15, "ack reverts the GCS UI"); apply_tuning(&store, &mut core); assert_eq!(core.altitude_gains().0, 0.15, "core unchanged"); // The other knobs apply too. - on_param_set(&mut store, &ParamSet { - param_id: param_id("MC_LAND_VZ"), - param_value: 0.8, - ..set - }); + on_param_set( + &mut store, + &ParamSet { + param_id: param_id("MC_LAND_VZ"), + param_value: 0.8, + ..set + }, + ); apply_tuning(&store, &mut core); assert_eq!(core.landing_descent(), 0.8); @@ -191,8 +197,18 @@ mod tests { fn store() -> ParamStore<4> { let mut s = ParamStore::new(); - s.register(ParamDef { id: param_id("MC_ROLL_P"), min: 0.0, max: 12.0, default: 6.5 }); - s.register(ParamDef { id: param_id("BAT_LOW_V"), min: 10.0, max: 16.8, default: 14.0 }); + s.register(ParamDef { + id: param_id("MC_ROLL_P"), + min: 0.0, + max: 12.0, + default: 6.5, + }); + s.register(ParamDef { + id: param_id("BAT_LOW_V"), + min: 10.0, + max: 16.8, + default: 14.0, + }); s } @@ -292,8 +308,8 @@ mod tests { #[test] fn full_frame_param_set_drives_store() { use relay_mavlink::{ - encode_frame, parse_frame, FrameHeader, MAGIC_V2, PARAM_SET_CRC_EXTRA, - PARAM_SET_MSG_ID, PARAM_SET_PAYLOAD_LEN, PARAM_VALUE_CRC_EXTRA, PARAM_VALUE_MSG_ID, + FrameHeader, MAGIC_V2, PARAM_SET_CRC_EXTRA, PARAM_SET_MSG_ID, PARAM_SET_PAYLOAD_LEN, + PARAM_VALUE_CRC_EXTRA, PARAM_VALUE_MSG_ID, encode_frame, parse_frame, }; let mut s = store(); // GCS builds + frames a PARAM_SET for MC_ROLL_P = 9.0 (in range). diff --git a/crates/relay-adrc/plain/src/lib.rs b/crates/relay-adrc/plain/src/lib.rs index 5c5cc01c..6b682fb6 100644 --- a/crates/relay-adrc/plain/src/lib.rs +++ b/crates/relay-adrc/plain/src/lib.rs @@ -56,13 +56,23 @@ pub struct AdrcGains { impl AdrcGains { /// Build with no actuator-lag model (instant actuator). pub const fn new(omega_o: f32, omega_c: f32, b0: f32) -> Self { - AdrcGains { omega_o, omega_c, b0, tau: 0.0 } + AdrcGains { + omega_o, + omega_c, + b0, + tau: 0.0, + } } /// Build with an explicit actuator time constant τ (the recommended /// form for the lag-sensitive yaw axis). pub const fn with_tau(omega_o: f32, omega_c: f32, b0: f32, tau: f32) -> Self { - AdrcGains { omega_o, omega_c, b0, tau } + AdrcGains { + omega_o, + omega_c, + b0, + tau, + } } /// Bandwidth-parameterized construction (Gao 2003): pick the controller @@ -74,7 +84,12 @@ impl AdrcGains { /// phase margin. This constructor makes [`well_separated`] true by /// construction for any `min_ratio ≤ separation`. pub fn from_bandwidth(omega_c: f32, separation: f32, b0: f32, tau: f32) -> Self { - AdrcGains { omega_o: separation * omega_c, omega_c, b0, tau } + AdrcGains { + omega_o: separation * omega_c, + omega_c, + b0, + tau, + } } /// Observer/controller timescale-separation invariant: ω_o ≥ ratio·ω_c. @@ -97,10 +112,7 @@ impl AdrcGains { /// outer position→attitude cascade). A `margin` < 1 leaves headroom. #[inline] pub fn eso_dt_stable(&self, dt: f32, margin: f32) -> bool { - self.omega_o.is_finite() - && dt.is_finite() - && dt > 0.0 - && self.omega_o * dt < 2.0 * margin + self.omega_o.is_finite() && dt.is_finite() && dt > 0.0 && self.omega_o * dt < 2.0 * margin } } @@ -116,7 +128,13 @@ pub struct AdrcAxis { impl AdrcAxis { pub fn new(g: AdrcGains) -> Self { - AdrcAxis { z1: 0.0, z2: 0.0, u_prev: 0.0, u_act: 0.0, g } + AdrcAxis { + z1: 0.0, + z2: 0.0, + u_prev: 0.0, + u_act: 0.0, + g, + } } /// Disturbance estimate (rad/s²) — the lumped unmodeled torque/J the @@ -130,14 +148,34 @@ impl AdrcAxis { /// outer loop, `dt` (s). Returns the control output `u` (torque, /// normalised to the same units the mixer expects). pub fn tick(&mut self, omega_meas: f32, omega_d: f32, dt: f32) -> f32 { - let dt = if dt.is_finite() { dt.clamp(1e-4, 0.1) } else { 1e-3 }; - let om = if omega_meas.is_finite() { omega_meas } else { 0.0 }; + let dt = if dt.is_finite() { + dt.clamp(1e-4, 0.1) + } else { + 1e-3 + }; + let om = if omega_meas.is_finite() { + omega_meas + } else { + 0.0 + }; let od = if omega_d.is_finite() { omega_d } else { 0.0 }; // Guard the tuning (positive, finite) so the law is total. - let omega_o = if self.g.omega_o.is_finite() && self.g.omega_o > 0.0 { self.g.omega_o } else { 10.0 }; - let omega_c = if self.g.omega_c.is_finite() && self.g.omega_c > 0.0 { self.g.omega_c } else { 3.0 }; - let b0 = if self.g.b0.is_finite() && self.g.b0.abs() > 1e-3 { self.g.b0 } else { 1.0 }; + let omega_o = if self.g.omega_o.is_finite() && self.g.omega_o > 0.0 { + self.g.omega_o + } else { + 10.0 + }; + let omega_c = if self.g.omega_c.is_finite() && self.g.omega_c > 0.0 { + self.g.omega_c + } else { + 3.0 + }; + let b0 = if self.g.b0.is_finite() && self.g.b0.abs() > 1e-3 { + self.g.b0 + } else { + 1.0 + }; let beta1 = 2.0 * omega_o; let beta2 = omega_o * omega_o; let kp = omega_c; @@ -219,17 +257,32 @@ impl Biquad { b2: ((1.0 - cw) * 0.5) / a0, a1: (-2.0 * cw) / a0, a2: (1.0 - alpha) / a0, - x1: 0.0, x2: 0.0, y1: 0.0, y2: 0.0, + x1: 0.0, + x2: 0.0, + y1: 0.0, + y2: 0.0, } } fn passthrough() -> Self { - Biquad { b0: 1.0, b1: 0.0, b2: 0.0, a1: 0.0, a2: 0.0, x1: 0.0, x2: 0.0, y1: 0.0, y2: 0.0 } + Biquad { + b0: 1.0, + b1: 0.0, + b2: 0.0, + a1: 0.0, + a2: 0.0, + x1: 0.0, + x2: 0.0, + y1: 0.0, + y2: 0.0, + } } pub fn filter(&mut self, x: f32) -> f32 { let x = if x.is_finite() { x } else { 0.0 }; - let y = self.b0 * x + self.b1 * self.x1 + self.b2 * self.x2 - self.a1 * self.y1 - self.a2 * self.y2; + let y = self.b0 * x + self.b1 * self.x1 + self.b2 * self.x2 + - self.a1 * self.y1 + - self.a2 * self.y2; let y = if y.is_finite() { y } else { 0.0 }; self.x2 = self.x1; self.x1 = x; @@ -253,7 +306,11 @@ impl GyroLpf { } pub fn filter(&mut self, gyro: [f32; 3]) -> [f32; 3] { - [self.axes[0].filter(gyro[0]), self.axes[1].filter(gyro[1]), self.axes[2].filter(gyro[2])] + [ + self.axes[0].filter(gyro[0]), + self.axes[1].filter(gyro[1]), + self.axes[2].filter(gyro[2]), + ] } } @@ -264,7 +321,13 @@ pub struct AdrcRate { impl AdrcRate { pub fn new(gains: [AdrcGains; 3]) -> Self { - AdrcRate { axes: [AdrcAxis::new(gains[0]), AdrcAxis::new(gains[1]), AdrcAxis::new(gains[2])] } + AdrcRate { + axes: [ + AdrcAxis::new(gains[0]), + AdrcAxis::new(gains[1]), + AdrcAxis::new(gains[2]), + ], + } } /// Falcon-quad defaults: roll/pitch fast (high effectiveness), yaw @@ -305,7 +368,11 @@ impl AdrcRate { } pub fn disturbance(&self) -> [f32; 3] { - [self.axes[0].disturbance(), self.axes[1].disturbance(), self.axes[2].disturbance()] + [ + self.axes[0].disturbance(), + self.axes[1].disturbance(), + self.axes[2].disturbance(), + ] } } @@ -360,14 +427,28 @@ impl CommandFilter { /// bandwidth ω_c for separation. `max_mag`/`max_rate` saturate the /// command and its slew (use f32::INFINITY to disable a limit). pub fn new(omega_n: f32, max_mag: f32, max_rate: f32) -> Self { - let omega_n = if omega_n.is_finite() && omega_n > 0.0 { omega_n } else { 1.0 }; - CommandFilter { omega_n, max_mag, max_rate, y: 0.0, yd: 0.0 } + let omega_n = if omega_n.is_finite() && omega_n > 0.0 { + omega_n + } else { + 1.0 + }; + CommandFilter { + omega_n, + max_mag, + max_rate, + y: 0.0, + yd: 0.0, + } } /// Filter one sample of the raw command `u` over `dt` seconds; returns /// the smoothed, separation-bounded command. pub fn step(&mut self, u: f32, dt: f32) -> f32 { - let dt = if dt.is_finite() { dt.clamp(1e-4, 0.1) } else { 1e-3 }; + let dt = if dt.is_finite() { + dt.clamp(1e-4, 0.1) + } else { + 1e-3 + }; let u = if u.is_finite() { u } else { 0.0 }; // Sanitise state so the law is total even from a non-finite state. let y0 = if self.y.is_finite() { self.y } else { 0.0 }; @@ -411,7 +492,11 @@ impl CommandFilter3 { } pub fn step(&mut self, u: [f32; 3], dt: f32) -> [f32; 3] { - [self.axes[0].step(u[0], dt), self.axes[1].step(u[1], dt), self.axes[2].step(u[2], dt)] + [ + self.axes[0].step(u[0], dt), + self.axes[1].step(u[1], dt), + self.axes[2].step(u[2], dt), + ] } pub fn reset(&mut self) { @@ -463,7 +548,10 @@ mod tests { // need not equal 2 exactly — but it must be a substantial, finite, // same-sign estimate (not zero, not NaN). assert!(d_est.is_finite()); - assert!(d_est > 0.5, "ESO should identify a positive disturbance, got {d_est}"); + assert!( + d_est > 0.5, + "ESO should identify a positive disturbance, got {d_est}" + ); } /// The gyro LPF passes DC unchanged and strongly attenuates a @@ -485,7 +573,9 @@ mod tests { for k in 0..2000 { let x = relay_math::sinf(2.0 * core::f32::consts::PI * 300.0 * (k as f32) / fs); let y = lpf2.filter([x, 0.0, 0.0])[0].abs(); - if k > 200 && y > peak { peak = y; } + if k > 200 && y > peak { + peak = y; + } } assert!(peak < 0.15, "300 Hz should be attenuated, peak {peak}"); } @@ -496,7 +586,10 @@ mod tests { fn regulates_rate_to_zero_under_disturbance() { let g = AdrcGains::new(20.0, 5.0, 6.0); let (omega, _) = sim_axis(4.0, 0.05, 1.5, 0.0, g); - assert!(omega.abs() < 0.15, "rate should be held near 0, got {omega}"); + assert!( + omega.abs() < 0.15, + "rate should be held near 0, got {omega}" + ); } proptest::proptest! { @@ -562,7 +655,9 @@ mod tests { let mut last = 0.0f32; for _ in 0..2000 { let y = cf.step(1.0, dt); - if y > peak { peak = y; } + if y > peak { + peak = y; + } last = y; } // Critically damped ⇒ the step response never overshoots the target @@ -664,7 +759,13 @@ mod kani_harness { // the step preserves it gives the running guarantee for all time. kani::assume(y.is_finite() && y.abs() <= max_mag); kani::assume(yd.is_finite() && yd.abs() <= max_rate); - let mut cf = CommandFilter { omega_n, max_mag, max_rate, y, yd }; + let mut cf = CommandFilter { + omega_n, + max_mag, + max_rate, + y, + yd, + }; let out = cf.step(u, dt); assert!(out <= max_mag && out >= -max_mag); assert!(cf.rate() <= max_rate && cf.rate() >= -max_rate); diff --git a/crates/relay-arm/plain/src/lib.rs b/crates/relay-arm/plain/src/lib.rs index 8ec78b68..0c8a68c0 100644 --- a/crates/relay-arm/plain/src/lib.rs +++ b/crates/relay-arm/plain/src/lib.rs @@ -66,7 +66,11 @@ 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 } + ArmingConfig { + spinup_ticks: 30, + level_ticks_required: 10, + tilt_thresh_rad: 0.087, + } } } @@ -93,11 +97,20 @@ pub struct ArmingSequencer { impl ArmingSequencer { pub fn new(cfg: ArmingConfig) -> Self { - ArmingSequencer { phase: DISARMED, tick_in_phase: 0, level_count: 0, cfg } + 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 } + pub fn phase(&self) -> u8 { + self.phase + } + pub fn level_count(&self) -> u32 { + self.level_count + } /// Advance one control tick. /// @@ -188,8 +201,17 @@ mod kani_proofs { // 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 }; + 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()) } @@ -230,10 +252,19 @@ mod kani_proofs { 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 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 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()); @@ -246,7 +277,11 @@ mod tests { use super::*; fn cfg() -> ArmingConfig { - ArmingConfig { spinup_ticks: 5, level_ticks_required: 3, tilt_thresh_rad: 0.087 } + ArmingConfig { + spinup_ticks: 5, + level_ticks_required: 3, + tilt_thresh_rad: 0.087, + } } /// ARM-P01: torque authority is never granted before the level gate. @@ -274,7 +309,10 @@ mod tests { 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"); + assert!( + o.torque_authority, + "torque engages only after level confirmed" + ); } /// A tilt above threshold during LevelHold resets the counter, so the @@ -284,7 +322,9 @@ mod tests { 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 + for _ in 0..5 { + s.tick(0.0, true); + } // reach LevelHold assert_eq!(s.phase(), LEVEL_HOLD); s.tick(0.0, true); // count 1 @@ -302,7 +342,9 @@ mod tests { 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..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"); @@ -317,7 +359,10 @@ mod tests { 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!( + 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; } diff --git a/crates/relay-att/plain/src/lib.rs b/crates/relay-att/plain/src/lib.rs index 47d4f529..243c4281 100644 --- a/crates/relay-att/plain/src/lib.rs +++ b/crates/relay-att/plain/src/lib.rs @@ -60,7 +60,10 @@ pub struct Timestamp { } impl Timestamp { - pub const ZERO: Self = Self { seconds: 0, fraction: 0 }; + pub const ZERO: Self = Self { + seconds: 0, + fraction: 0, + }; } /// Attitude controller tuning gains. @@ -247,7 +250,10 @@ mod tests { fn ts(secs: f32) -> Timestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - Timestamp { seconds: secs as u64, fraction: frac } + Timestamp { + seconds: secs as u64, + fraction: frac, + } } #[test] @@ -264,7 +270,11 @@ mod tests { let half = (20.0_f32).to_radians() * 0.5; let q_sp = [half.cos(), half.sin(), 0.0, 0.0]; let r = a.tick(ts(0.001), [1.0, 0.0, 0.0, 0.0], q_sp); - assert!(r[0] > 0.0, "+roll setpoint must produce +x rate, got {}", r[0]); + assert!( + r[0] > 0.0, + "+roll setpoint must produce +x rate, got {}", + r[0] + ); assert!(r[1].abs() < 1.0e-6); assert!(r[2].abs() < 1.0e-6); } @@ -275,7 +285,11 @@ mod tests { let half = (-20.0_f32).to_radians() * 0.5; let q_sp = [half.cos(), half.sin(), 0.0, 0.0]; let r = a.tick(ts(0.001), [1.0, 0.0, 0.0, 0.0], q_sp); - assert!(r[0] < 0.0, "-roll setpoint must produce -x rate, got {}", r[0]); + assert!( + r[0] < 0.0, + "-roll setpoint must produce -x rate, got {}", + r[0] + ); } #[test] @@ -292,7 +306,10 @@ mod tests { assert!( r[i].abs() <= a.gains().rate_max[i] + 1.0e-6, "r[{}]={} exceeds rate_max[{}]={}", - i, r[i], i, a.gains().rate_max[i] + i, + r[i], + i, + a.gains().rate_max[i] ); } } @@ -306,8 +323,11 @@ mod tests { let q = [r2, 0.0, r2, 0.0]; // 90° pitch let q_sp = [0.9659_f32, 0.0, 0.2588, 0.0]; // 30° pitch a.tick(ts(0.001), q, q_sp); - assert!(is_unit_quaternion(a.last_q_err()), - "q_err not unit: {:?}", a.last_q_err()); + assert!( + is_unit_quaternion(a.last_q_err()), + "q_err not unit: {:?}", + a.last_q_err() + ); } #[test] @@ -329,14 +349,19 @@ mod tests { let q_sp = [h_sp.cos(), h_sp.sin(), 0.0, 0.0]; a.tick(ts(0.001), q, q_sp); let q_err = a.last_q_err(); - assert!(q_err[0] >= 0.0, + assert!( + q_err[0] >= 0.0, "q_err scalar must be non-negative after sign correction, got {:?}", - q_err); + q_err + ); // Shortest-arc magnitude is small (10° = 2*5°, vec ≈ sin(5°)). let vec_mag = (q_err[1].powi(2) + q_err[2].powi(2) + q_err[3].powi(2)).sqrt(); - assert!(vec_mag < 0.1, + assert!( + vec_mag < 0.1, "shortest-arc vec magnitude should be small (~sin(5°)≈0.087), got {} ({:?})", - vec_mag, q_err); + vec_mag, + q_err + ); } #[test] @@ -360,9 +385,14 @@ mod tests { // ω_lin / exact = 2*sin(θ/2) / θ. For θ=30° this is // 0.9886, i.e. 1.14% off. Loosen budget to 1.5% to cover // up to 30° edge. - assert!(err <= 0.015, + assert!( + err <= 0.015, "deg={}: linearised={} exact={} relerr={}", - deg, r[0], exact, err); + deg, + r[0], + exact, + err + ); } } @@ -385,20 +415,20 @@ mod tests { for &x in &[0.0_f32, 0.5, 1.0, 2.0, 9.0, 100.0, 1.0e6] { let mine = sqrt_f32(x); let exact = x.sqrt(); - assert!((mine - exact).abs() <= exact.abs() * 1.0e-5 + 1.0e-6, - "sqrt({}): mine={} exact={}", x, mine, exact); + assert!( + (mine - exact).abs() <= exact.abs() * 1.0e-5 + 1.0e-6, + "sqrt({}): mine={} exact={}", + x, + mine, + exact + ); } } use proptest::prelude::*; fn arb_unit_quaternion() -> impl Strategy { - ( - -1.0_f32..1.0, - -1.0_f32..1.0, - -1.0_f32..1.0, - -1.0_f32..1.0, - ) + (-1.0_f32..1.0, -1.0_f32..1.0, -1.0_f32..1.0, -1.0_f32..1.0) .prop_filter("non-zero", |(a, b, c, d)| { a * a + b * b + c * c + d * d > 1.0e-3 }) diff --git a/crates/relay-avoid/plain/src/lib.rs b/crates/relay-avoid/plain/src/lib.rs index 7a78d7e2..d4ff7850 100644 --- a/crates/relay-avoid/plain/src/lib.rs +++ b/crates/relay-avoid/plain/src/lib.rs @@ -27,11 +27,7 @@ #[inline] fn finite_or(x: f32, d: f32) -> f32 { - if x.is_finite() { - x - } else { - d - } + if x.is_finite() { x } else { d } } /// Cap an approach speed `v_cmd` (≥ 0, toward the obstacle) at `v_allowed`. Pure @@ -40,11 +36,7 @@ fn finite_or(x: f32, d: f32) -> f32 { pub fn cap_approach(v_cmd: f32, v_allowed: f32) -> f32 { let v = finite_or(v_cmd, 0.0).max(0.0); let a = finite_or(v_allowed, 0.0).max(0.0); - if a < v { - a - } else { - v - } + if a < v { a } else { v } } /// Braking-limited speed for a usable clear distance `usable` (m) and max @@ -55,11 +47,7 @@ pub fn braking_speed(usable: f32, a_max: f32) -> f32 { let u = finite_or(usable, 0.0).max(0.0); let a = finite_or(a_max, 0.0).max(0.0); let s = relay_math::sqrtf(2.0 * a * u); - if s.is_finite() && s >= 0.0 { - s - } else { - 0.0 - } + if s.is_finite() && s >= 0.0 { s } else { 0.0 } } /// The collision-prevention velocity cap: limit the commanded approach speed so @@ -132,7 +120,11 @@ mod tests { #[test] fn never_exceeds_command_or_goes_negative() { - for &(v, d, m, a) in &[(2.0, 5.0, 1.0, 3.0), (10.0, 0.5, 1.0, 4.0), (-1.0, 5.0, 1.0, 3.0)] { + for &(v, d, m, a) in &[ + (2.0, 5.0, 1.0, 3.0), + (10.0, 0.5, 1.0, 4.0), + (-1.0, 5.0, 1.0, 3.0), + ] { let r = limit_approach_speed(v, d, m, a); assert!(r >= 0.0 && r <= v.max(0.0)); } diff --git a/crates/relay-batt/plain/src/kani_proofs.rs b/crates/relay-batt/plain/src/kani_proofs.rs index e6b58f38..c5c566f0 100644 --- a/crates/relay-batt/plain/src/kani_proofs.rs +++ b/crates/relay-batt/plain/src/kani_proofs.rs @@ -7,7 +7,7 @@ //! multiplication is intractable. #![cfg(kani)] -use crate::{sanitize, TripLatch}; +use crate::{TripLatch, sanitize}; /// BATT-K01 — `sanitize` is total and in-range for ANY f32 input /// (incl. NaN/±∞), provided the bounds are ordered and the NaN default diff --git a/crates/relay-batt/plain/src/lib.rs b/crates/relay-batt/plain/src/lib.rs index ab7c5bbb..05da49ef 100644 --- a/crates/relay-batt/plain/src/lib.rs +++ b/crates/relay-batt/plain/src/lib.rs @@ -234,15 +234,23 @@ impl BatteryEstimator { pub fn update(&mut self, dt_s: f32, volts: f32, amps: Option) -> BattState { let dt = sanitize(dt_s, 0.0, DT_MAX, 0.0); let v = sanitize(volts, 0.0, V_MAX, 0.0); - let cells = if self.cfg.cells == 0 { 1 } else { self.cfg.cells } as f32; + let cells = if self.cfg.cells == 0 { + 1 + } else { + self.cfg.cells + } as f32; match amps { Some(a) => { let i = sanitize(a, 0.0, I_MAX, 0.0); // Coulomb count: A·s → mAh, drift-bounded. let cap = sanitize(self.cfg.capacity_mah, 1.0, 1.0e6, 5000.0); - self.consumed_mah = - sanitize(self.consumed_mah + i * dt * (1000.0 / 3600.0), 0.0, cap, cap); + self.consumed_mah = sanitize( + self.consumed_mah + i * dt * (1000.0 / 3600.0), + 0.0, + cap, + cap, + ); // Online R: accept only decorrelation-safe big current steps. if self.have_prev { @@ -266,11 +274,18 @@ impl BatteryEstimator { let soc_coulomb = sanitize(1.0 - self.consumed_mah / cap, 0.0, 1.0, 0.0); let soc_ocv = ocv_soc_cell(rest / cells); // Conservative fusion: either signal low pulls the SoC down. - let soc = if soc_coulomb < soc_ocv { soc_coulomb } else { soc_ocv }; - - let low = self.low.update(dt, soc < self.cfg.low_soc, self.cfg.debounce_s); + let soc = if soc_coulomb < soc_ocv { + soc_coulomb + } else { + soc_ocv + }; + + let low = self + .low + .update(dt, soc < self.cfg.low_soc, self.cfg.debounce_s); let critical = - self.critical.update(dt, soc < self.cfg.crit_soc, self.cfg.debounce_s); + self.critical + .update(dt, soc < self.cfg.crit_soc, self.cfg.debounce_s); BattState { volts: v, rest_volts: rest, @@ -386,7 +401,10 @@ mod tests { let s = est.update(0.02, 3.85 * 4.0, Some(2.0)); tripped = s.critical; } - assert!(tripped, "spent pack must trip critical despite healthy volts"); + assert!( + tripped, + "spent pack must trip critical despite healthy volts" + ); } /// mAh integration error < 2% over a simulated 20-minute flight with @@ -405,7 +423,11 @@ mod tests { for k in 0..steps { // Duty profile: hover 18 A with 40 A climbs every 2 min. let t = k as f32 * dt; - let i_true = if (t / 120.0).fract() < 0.1 { 40.0 } else { 18.0 }; + let i_true = if (t / 120.0).fract() < 0.1 { + 40.0 + } else { + 18.0 + }; lcg = lcg.wrapping_mul(1664525).wrapping_add(1013904223); // Zero-mean ±2 A uniform sensor noise. let noise = ((lcg >> 8) as f32 / 16777216.0 - 0.5) * 4.0; @@ -442,7 +464,10 @@ mod tests { } assert!(s_fb.degraded, "fallback must be flagged"); assert!(!s_comp.degraded); - assert!(s_fb.low, "wider fallback margin trips at 3.68 < 3.70 V/cell"); + assert!( + s_fb.low, + "wider fallback margin trips at 3.68 < 3.70 V/cell" + ); assert!( s_comp.rest_volts > s_fb.rest_volts, "compensation credits the sag the fallback cannot" diff --git a/crates/relay-calib/plain/src/flow.rs b/crates/relay-calib/plain/src/flow.rs index 1cd2adc3..0c6cf83f 100644 --- a/crates/relay-calib/plain/src/flow.rs +++ b/crates/relay-calib/plain/src/flow.rs @@ -13,7 +13,7 @@ //! aggregates that are algebraically identical to the solvers over the same //! samples — pinned by equivalence tests), `no_std`, no panics. -use crate::{accel_6point, CalParams, Vec3}; +use crate::{CalParams, Vec3, accel_6point}; // ── Gyro null flow ─────────────────────────────────────────────────────────── @@ -60,12 +60,16 @@ impl GyroNullFlow { if self.n >= self.needed { return WindowStatus::Done; } - let still = gyro.iter().all(|a| a.is_finite() && a.abs() <= self.motion_thresh); + let still = gyro + .iter() + .all(|a| a.is_finite() && a.abs() <= self.motion_thresh); if !still { self.sum = [0.0; 3]; self.n = 0; self.restarts = self.restarts.saturating_add(1); - return WindowStatus::Collecting { remaining: self.needed }; + return WindowStatus::Collecting { + remaining: self.needed, + }; } self.sum[0] += gyro[0]; self.sum[1] += gyro[1]; @@ -74,7 +78,9 @@ impl GyroNullFlow { if self.n >= self.needed { WindowStatus::Done } else { - WindowStatus::Collecting { remaining: self.needed - self.n } + WindowStatus::Collecting { + remaining: self.needed - self.n, + } } } @@ -166,12 +172,36 @@ impl Accel6PointFlow { let dom = self.dom_frac * self.g; let off = self.off_frac * self.g; let faces = [ - (Face::XPos, a[0] >= dom, a[1].abs() <= off && a[2].abs() <= off), - (Face::XNeg, a[0] <= -dom, a[1].abs() <= off && a[2].abs() <= off), - (Face::YPos, a[1] >= dom, a[0].abs() <= off && a[2].abs() <= off), - (Face::YNeg, a[1] <= -dom, a[0].abs() <= off && a[2].abs() <= off), - (Face::ZPos, a[2] >= dom, a[0].abs() <= off && a[1].abs() <= off), - (Face::ZNeg, a[2] <= -dom, a[0].abs() <= off && a[1].abs() <= off), + ( + Face::XPos, + a[0] >= dom, + a[1].abs() <= off && a[2].abs() <= off, + ), + ( + Face::XNeg, + a[0] <= -dom, + a[1].abs() <= off && a[2].abs() <= off, + ), + ( + Face::YPos, + a[1] >= dom, + a[0].abs() <= off && a[2].abs() <= off, + ), + ( + Face::YNeg, + a[1] <= -dom, + a[0].abs() <= off && a[2].abs() <= off, + ), + ( + Face::ZPos, + a[2] >= dom, + a[0].abs() <= off && a[1].abs() <= off, + ), + ( + Face::ZNeg, + a[2] <= -dom, + a[0].abs() <= off && a[1].abs() <= off, + ), ]; for (f, dom_ok, off_ok) in faces { if dom_ok && off_ok { @@ -212,7 +242,11 @@ impl Accel6PointFlow { self.n[i] += 1; if self.n[i] >= self.needed { let inv = 1.0 / self.n[i] as f32; - self.face_mean[i] = [self.sum[i][0] * inv, self.sum[i][1] * inv, self.sum[i][2] * inv]; + self.face_mean[i] = [ + self.sum[i][0] * inv, + self.sum[i][1] * inv, + self.sum[i][2] * inv, + ]; self.captured[i] = true; self.current = None; if self.captured.iter().all(|&c| c) { @@ -221,7 +255,10 @@ impl Accel6PointFlow { SixPointStatus::FaceCaptured { face } } } else { - SixPointStatus::Sampling { face, remaining: self.needed - self.n[i] } + SixPointStatus::Sampling { + face, + remaining: self.needed - self.n[i], + } } } @@ -276,7 +313,10 @@ pub enum MagSweepVerdict { Accepted, /// Sweep rejected: the smallest per-axis half-range (gauss) and the /// min/max half-range ratio that failed the gate — operator retries. - Rejected { min_half_range: f32, anisotropy: f32 }, + Rejected { + min_half_range: f32, + anisotropy: f32, + }, } impl MagSweepFlow { @@ -306,7 +346,9 @@ impl MagSweepFlow { if self.n >= self.needed { WindowStatus::Done } else { - WindowStatus::Collecting { remaining: self.needed - self.n } + WindowStatus::Collecting { + remaining: self.needed - self.n, + } } } @@ -321,7 +363,13 @@ impl MagSweepFlow { max_anisotropy: f32, ) -> (MagSweepVerdict, Option<(Vec3, Vec3)>) { if self.n < self.needed { - return (MagSweepVerdict::Rejected { min_half_range: 0.0, anisotropy: f32::INFINITY }, None); + return ( + MagSweepVerdict::Rejected { + min_half_range: 0.0, + anisotropy: f32::INFINITY, + }, + None, + ); } let half = [ (self.hi[0] - self.lo[0]) * 0.5, @@ -338,9 +386,19 @@ impl MagSweepFlow { max_h = h; } } - let anisotropy = if min_h > 0.0 { max_h / min_h } else { f32::INFINITY }; + let anisotropy = if min_h > 0.0 { + max_h / min_h + } else { + f32::INFINITY + }; if !(min_h.is_finite() && min_h >= min_span && anisotropy <= max_anisotropy) { - return (MagSweepVerdict::Rejected { min_half_range: min_h, anisotropy }, None); + return ( + MagSweepVerdict::Rejected { + min_half_range: min_h, + anisotropy, + }, + None, + ); } let offset = [ (self.lo[0] + self.hi[0]) * 0.5, diff --git a/crates/relay-calib/plain/src/lib.rs b/crates/relay-calib/plain/src/lib.rs index cae1dd70..fb98273d 100644 --- a/crates/relay-calib/plain/src/lib.rs +++ b/crates/relay-calib/plain/src/lib.rs @@ -180,7 +180,11 @@ fn min_max(samples: &[Vec3]) -> (Vec3, Vec3) { /// a full rotation. Total: empty → zero offset. Never panics. pub fn mag_hardiron(samples: &[Vec3]) -> Vec3 { let (lo, hi) = min_max(samples); - [(lo[0] + hi[0]) * 0.5, (lo[1] + hi[1]) * 0.5, (lo[2] + hi[2]) * 0.5] + [ + (lo[0] + hi[0]) * 0.5, + (lo[1] + hi[1]) * 0.5, + (lo[2] + hi[2]) * 0.5, + ] } /// Magnetometer soft-iron DIAGONAL scale: normalise each axis's half-range to the @@ -189,7 +193,11 @@ pub fn mag_hardiron(samples: &[Vec3]) -> Vec3 { /// fit is a documented follow-up. pub fn mag_softiron_diag(samples: &[Vec3]) -> Vec3 { let (lo, hi) = min_max(samples); - let half = [(hi[0] - lo[0]) * 0.5, (hi[1] - lo[1]) * 0.5, (hi[2] - lo[2]) * 0.5]; + let half = [ + (hi[0] - lo[0]) * 0.5, + (hi[1] - lo[1]) * 0.5, + (hi[2] - lo[2]) * 0.5, + ]; let avg = (half[0] + half[1] + half[2]) / 3.0; let mut scale = [1.0f32; 3]; let mut a = 0; @@ -202,7 +210,13 @@ pub fn mag_softiron_diag(samples: &[Vec3]) -> Vec3 { /// Solve a full [`CalParams`] from the raw calibration data: at-rest gyro /// samples, the accel ±g face endpoints, and the mag rotation sweep. -pub fn solve(gyro_rest: &[Vec3], accel_pos: Vec3, accel_neg: Vec3, g: f32, mag_sweep: &[Vec3]) -> CalParams { +pub fn solve( + gyro_rest: &[Vec3], + accel_pos: Vec3, + accel_neg: Vec3, + g: f32, + mag_sweep: &[Vec3], +) -> CalParams { let (accel_bias, accel_scale) = accel_6point(accel_pos, accel_neg, g); CalParams { gyro_bias: gyro_null(gyro_rest), @@ -252,12 +266,18 @@ mod flow_tests { f.step([0.01, 0.0, 0.0]); f.step([0.01, 0.0, 0.0]); // Bump: over threshold ⇒ restart, nothing from before survives. - assert!(matches!(f.step([0.5, 0.0, 0.0]), WindowStatus::Collecting { remaining: 3 })); + assert!(matches!( + f.step([0.5, 0.0, 0.0]), + WindowStatus::Collecting { remaining: 3 } + )); assert_eq!(f.restarts(), 1); assert_eq!(f.bias(), None); // NaN is also disqualifying. f.step([0.01, 0.0, 0.0]); - assert!(matches!(f.step([f32::NAN, 0.0, 0.0]), WindowStatus::Collecting { remaining: 3 })); + assert!(matches!( + f.step([f32::NAN, 0.0, 0.0]), + WindowStatus::Collecting { remaining: 3 } + )); assert_eq!(f.restarts(), 2); } @@ -277,11 +297,26 @@ mod flow_tests { fn accel_flow_captures_all_faces_and_matches_solver() { let mut f = Accel6PointFlow::new(3, G); // Slight bias on x (+0.1) so the solve is non-trivial. - assert!(matches!(feed_face(&mut f, [G + 0.1, 0.0, 0.0], 3), SixPointStatus::FaceCaptured { face: Face::XPos })); - assert!(matches!(feed_face(&mut f, [-G + 0.1, 0.0, 0.0], 3), SixPointStatus::FaceCaptured { face: Face::XNeg })); - assert!(matches!(feed_face(&mut f, [0.0, G, 0.0], 3), SixPointStatus::FaceCaptured { face: Face::YPos })); - assert!(matches!(feed_face(&mut f, [0.0, -G, 0.0], 3), SixPointStatus::FaceCaptured { face: Face::YNeg })); - assert!(matches!(feed_face(&mut f, [0.0, 0.0, G], 3), SixPointStatus::FaceCaptured { face: Face::ZPos })); + assert!(matches!( + feed_face(&mut f, [G + 0.1, 0.0, 0.0], 3), + SixPointStatus::FaceCaptured { face: Face::XPos } + )); + assert!(matches!( + feed_face(&mut f, [-G + 0.1, 0.0, 0.0], 3), + SixPointStatus::FaceCaptured { face: Face::XNeg } + )); + assert!(matches!( + feed_face(&mut f, [0.0, G, 0.0], 3), + SixPointStatus::FaceCaptured { face: Face::YPos } + )); + assert!(matches!( + feed_face(&mut f, [0.0, -G, 0.0], 3), + SixPointStatus::FaceCaptured { face: Face::YNeg } + )); + assert!(matches!( + feed_face(&mut f, [0.0, 0.0, G], 3), + SixPointStatus::FaceCaptured { face: Face::ZPos } + )); assert_eq!(feed_face(&mut f, [0.0, 0.0, -G], 3), SixPointStatus::Done); assert_eq!(f.captured_mask(), 0b11_1111); let (bias, scale) = f.solve().unwrap(); @@ -299,18 +334,39 @@ mod flow_tests { fn accel_flow_rejects_tilted_and_shaky_faces() { let mut f = Accel6PointFlow::new(3, G); // Tilted 45° — no dominant axis within gates ⇒ never recognised. - assert_eq!(f.step([G * 0.7, G * 0.7, 0.0]), SixPointStatus::WaitingForFace); + assert_eq!( + f.step([G * 0.7, G * 0.7, 0.0]), + SixPointStatus::WaitingForFace + ); // Start a valid face then shake out of it ⇒ that window restarts. - assert!(matches!(f.step([G, 0.0, 0.0]), SixPointStatus::Sampling { face: Face::XPos, remaining: 2 })); - assert_eq!(f.step([G * 0.5, G * 0.5, 0.0]), SixPointStatus::WaitingForFace); + assert!(matches!( + f.step([G, 0.0, 0.0]), + SixPointStatus::Sampling { + face: Face::XPos, + remaining: 2 + } + )); + assert_eq!( + f.step([G * 0.5, G * 0.5, 0.0]), + SixPointStatus::WaitingForFace + ); // Window restarted: needs the full count again. - assert!(matches!(f.step([G, 0.0, 0.0]), SixPointStatus::Sampling { face: Face::XPos, remaining: 2 })); + assert!(matches!( + f.step([G, 0.0, 0.0]), + SixPointStatus::Sampling { + face: Face::XPos, + remaining: 2 + } + )); } #[test] fn accel_flow_never_rerecords_a_face() { let mut f = Accel6PointFlow::new(2, G); - assert!(matches!(feed_face(&mut f, [G, 0.0, 0.0], 2), SixPointStatus::FaceCaptured { face: Face::XPos })); + assert!(matches!( + feed_face(&mut f, [G, 0.0, 0.0], 2), + SixPointStatus::FaceCaptured { face: Face::XPos } + )); // Presenting the same face again is ignored (WaitingForFace). assert_eq!(f.step([G, 0.0, 0.0]), SixPointStatus::WaitingForFace); assert_eq!(f.captured_mask(), 0b00_0001); @@ -372,8 +428,8 @@ mod flow_tests { /// the pre-arm `calibration_present` gate consumes). #[test] fn calibrate_persist_reboot_roundtrip() { - use relay_param::persist::{load, save, ArrayNvm, Layout, LoadOutcome}; - use relay_param::{param_id, ParamDef, ParamStore}; + use relay_param::persist::{ArrayNvm, Layout, LoadOutcome, load, save}; + use relay_param::{ParamDef, ParamStore, param_id}; // 1. Flows produce a calibration. let mut gy = GyroNullFlow::new(4, 0.1); @@ -411,14 +467,27 @@ mod flow_tests { // Generous physical bounds; defaults = identity calibration. let d = CalParams::identity().to_named(); let default = d.iter().find(|(n, _)| *n == name).unwrap().1; - s.register(ParamDef { id: param_id(name), min: -50.0, max: 50.0, default }); + s.register(ParamDef { + id: param_id(name), + min: -50.0, + max: 50.0, + default, + }); } - s.register(ParamDef { id: param_id("CAL_VALID"), min: 0.0, max: 1.0, default: 0.0 }); + s.register(ParamDef { + id: param_id("CAL_VALID"), + min: 0.0, + max: 1.0, + default: 0.0, + }); s } let mut store = schema(); for (name, v) in cal.to_named() { - assert_eq!(store.set(¶m_id(name), v), relay_param::SetResult::Applied); + assert_eq!( + store.set(¶m_id(name), v), + relay_param::SetResult::Applied + ); } store.set(¶m_id("CAL_VALID"), 1.0); let mut nvm: ArrayNvm = ArrayNvm::new(); @@ -434,13 +503,19 @@ mod flow_tests { vals[i] = store2.get(¶m_id(name)).unwrap(); } let back = CalParams::from_values(vals); - assert_eq!(back, cal, "reboot must reproduce the calibration bit-exactly"); + assert_eq!( + back, cal, + "reboot must reproduce the calibration bit-exactly" + ); // 4. A NEVER-CALIBRATED device: fresh NVM ⇒ defaults ⇒ CAL_VALID=0 — // the value that keeps pre-arm `calibration_present` false. let blank: ArrayNvm = ArrayNvm::new(); let mut store3 = schema(); - assert_eq!(load(&mut store3, &blank, LAYOUT, 1).outcome, LoadOutcome::FreshDefaults); + assert_eq!( + load(&mut store3, &blank, LAYOUT, 1).outcome, + LoadOutcome::FreshDefaults + ); assert_eq!(store3.get(¶m_id("CAL_VALID")), Some(0.0)); } @@ -506,7 +581,11 @@ mod tests { assert!(close(bias, b, 1e-4), "bias {bias:?} vs {b:?}"); assert!(close(scale, s, 1e-4), "scale {scale:?} vs {s:?}"); // and applying the solved cal to the +g reading yields +G per axis. - let cal = CalParams { accel_bias: bias, accel_scale: scale, ..CalParams::identity() }; + let cal = CalParams { + accel_bias: bias, + accel_scale: scale, + ..CalParams::identity() + }; let corrected = cal.apply_accel(pos); assert!(close(corrected, [G, G, G], 1e-3)); } diff --git a/crates/relay-ccsds/plain/src/engine.rs b/crates/relay-ccsds/plain/src/engine.rs index b2da221a..745bdae3 100644 --- a/crates/relay-ccsds/plain/src/engine.rs +++ b/crates/relay-ccsds/plain/src/engine.rs @@ -31,7 +31,11 @@ pub struct CcsdsHeader { } pub fn encode_header(header: &CcsdsHeader, buf: &mut [u8; 6]) { - let type_bit: u8 = if header.packet_type == PacketType::Command { 1 } else { 0 }; + let type_bit: u8 = if header.packet_type == PacketType::Command { + 1 + } else { + 0 + }; let sec_bit: u8 = if header.sec_header_flag { 1 } else { 0 }; let apid_masked: u16 = header.apid & 0x07FF; let apid_hi: u8 = ((apid_masked >> 8) & 0x07) as u8; diff --git a/crates/relay-ccsds/plain/src/sensor_wire.rs b/crates/relay-ccsds/plain/src/sensor_wire.rs index fad9af3b..00fc19dc 100644 --- a/crates/relay-ccsds/plain/src/sensor_wire.rs +++ b/crates/relay-ccsds/plain/src/sensor_wire.rs @@ -160,9 +160,13 @@ pub fn decode_packet(buf: &[u8]) -> Result { /// Convert a Loxone f64 Celsius temperature to Wohl i32 centidegrees pub fn celsius_to_centidegrees(celsius: f64) -> i32 { let scaled = celsius * 100.0; - if scaled >= i32::MAX as f64 { i32::MAX } - else if scaled <= i32::MIN as f64 { i32::MIN } - else { scaled as i32 } + if scaled >= i32::MAX as f64 { + i32::MAX + } else if scaled <= i32::MIN as f64 { + i32::MIN + } else { + scaled as i32 + } } /// Convert Wohl i32 centidegrees back to f64 Celsius @@ -173,9 +177,13 @@ pub fn centidegrees_to_celsius(cd: i32) -> f64 { /// Convert a Loxone f64 watts to Wohl i32 (watts × 10) pub fn watts_to_fixed(watts: f64) -> i32 { let scaled = watts * 10.0; - if scaled >= i32::MAX as f64 { i32::MAX } - else if scaled <= i32::MIN as f64 { i32::MIN } - else { scaled as i32 } + if scaled >= i32::MAX as f64 { + i32::MAX + } else if scaled <= i32::MIN as f64 { + i32::MIN + } else { + scaled as i32 + } } #[cfg(test)] @@ -281,13 +289,29 @@ mod tests { #[test] fn test_all_sensor_types() { - for st in [SENSOR_TEMP, SENSOR_HUMIDITY, SENSOR_CO2, SENSOR_PM25, SENSOR_VOC, - SENSOR_CONTACT, SENSOR_WATER, SENSOR_MOTION, - SENSOR_POWER, SENSOR_ENERGY, - SENSOR_LUX, SENSOR_PRESSURE, SENSOR_WIND, SENSOR_RAIN] { + for st in [ + SENSOR_TEMP, + SENSOR_HUMIDITY, + SENSOR_CO2, + SENSOR_PM25, + SENSOR_VOC, + SENSOR_CONTACT, + SENSOR_WATER, + SENSOR_MOTION, + SENSOR_POWER, + SENSOR_ENERGY, + SENSOR_LUX, + SENSOR_PRESSURE, + SENSOR_WIND, + SENSOR_RAIN, + ] { let packet = SensorPacket { - device_id: 1, sequence: 0, sensor_type: st, - quality: QUALITY_GOOD, zone_id: 1, value: 42, + device_id: 1, + sequence: 0, + sensor_type: st, + quality: QUALITY_GOOD, + zone_id: 1, + value: 42, }; let mut buf = [0u8; PACKET_SIZE]; encode_packet(&packet, &mut buf); diff --git a/crates/relay-cfdp/plain/src/engine.rs b/crates/relay-cfdp/plain/src/engine.rs index 7c08e4af..ba77865b 100644 --- a/crates/relay-cfdp/plain/src/engine.rs +++ b/crates/relay-cfdp/plain/src/engine.rs @@ -139,36 +139,34 @@ impl TransactionTable { TransactionState::Idle => { self.transactions[idx as usize].state = TransactionState::MetadataSent; result.add_action(CfdpAction::SendMetadata); - }, + } TransactionState::MetadataSent => { self.transactions[idx as usize].state = TransactionState::DataSending; let len = txn.file_size; if len > 0 { - result.add_action(CfdpAction::SendData { offset: 0, length: len }); + result.add_action(CfdpAction::SendData { + offset: 0, + length: len, + }); } - }, + } TransactionState::DataSending => { self.transactions[idx as usize].bytes_sent = txn.file_size; self.transactions[idx as usize].state = TransactionState::EofSent; result.add_action(CfdpAction::SendEof); - }, + } TransactionState::EofSent => { self.transactions[idx as usize].state = TransactionState::Finished; result.add_action(CfdpAction::Complete); - }, - TransactionState::Finished => {}, - TransactionState::Cancelled => {}, + } + TransactionState::Finished => {} + TransactionState::Cancelled => {} } result } - pub fn process_nak( - &mut self, - transaction_id: u32, - offset: u32, - length: u32, - ) -> CfdpResult { + pub fn process_nak(&mut self, transaction_id: u32, offset: u32, length: u32) -> CfdpResult { let mut result = CfdpResult::new(); let idx = self.find_transaction(transaction_id); if idx as usize >= MAX_TRANSACTIONS || idx >= self.count { @@ -191,13 +189,20 @@ impl TransactionTable { let clamped_length = if offset < txn.file_size { let remaining = txn.file_size - offset; - if length < remaining { length } else { remaining } + if length < remaining { + length + } else { + remaining + } } else { 0 }; if clamped_length > 0 { - result.add_action(CfdpAction::Retransmit { offset, length: clamped_length }); + result.add_action(CfdpAction::Retransmit { + offset, + length: clamped_length, + }); } result @@ -215,28 +220,31 @@ impl TransactionTable { TransactionState::Idle => { self.transactions[idx as usize].state = TransactionState::MetadataSent; result.add_action(CfdpAction::SendMetadata); - }, + } TransactionState::MetadataSent => { result.add_action(CfdpAction::SendMetadata); - }, + } TransactionState::DataSending => { let remaining = txn.file_size - txn.bytes_sent; if remaining > 0 { - result.add_action(CfdpAction::SendData { offset: txn.bytes_sent, length: remaining }); + result.add_action(CfdpAction::SendData { + offset: txn.bytes_sent, + length: remaining, + }); } else { self.transactions[idx as usize].state = TransactionState::EofSent; result.add_action(CfdpAction::SendEof); } - }, + } TransactionState::EofSent => { result.add_action(CfdpAction::SendAck); - }, + } TransactionState::Finished => { result.add_action(CfdpAction::Complete); - }, + } TransactionState::Cancelled => { result.add_action(CfdpAction::Cancel); - }, + } } result diff --git a/crates/relay-ci/plain/src/engine.rs b/crates/relay-ci/plain/src/engine.rs index 30b86192..2c3d3235 100644 --- a/crates/relay-ci/plain/src/engine.rs +++ b/crates/relay-ci/plain/src/engine.rs @@ -34,7 +34,13 @@ pub struct CiConfig { impl CommandHeader { pub const fn empty() -> Self { - CommandHeader { stream_id: 0, sequence: 0, length: 0, function_code: 0, checksum: 0 } + CommandHeader { + stream_id: 0, + sequence: 0, + length: 0, + function_code: 0, + checksum: 0, + } } } @@ -137,7 +143,10 @@ mod tests { function_code: 5, checksum: 0, }; - assert_eq!(validate_header(&config, &header), CiValidation::InvalidStreamId); + assert_eq!( + validate_header(&config, &header), + CiValidation::InvalidStreamId + ); } #[test] @@ -163,7 +172,10 @@ mod tests { function_code: 99, checksum: 0, }; - assert_eq!(validate_header(&config, &header), CiValidation::InvalidCmdCode); + assert_eq!( + validate_header(&config, &header), + CiValidation::InvalidCmdCode + ); } #[test] @@ -176,7 +188,10 @@ mod tests { function_code: 5, checksum: 0, }; - assert_eq!(validate_header(&config, &header), CiValidation::LengthMismatch); + assert_eq!( + validate_header(&config, &header), + CiValidation::LengthMismatch + ); } #[test] diff --git a/crates/relay-cs/plain/src/engine.rs b/crates/relay-cs/plain/src/engine.rs index 4dcb7313..41f7b019 100644 --- a/crates/relay-cs/plain/src/engine.rs +++ b/crates/relay-cs/plain/src/engine.rs @@ -71,13 +71,23 @@ pub struct ChecksumTable { impl Region { pub const fn empty() -> Self { - Region { region_id: 0, baseline_crc: 0, enabled: false, last_checked: 0 } + Region { + region_id: 0, + baseline_crc: 0, + enabled: false, + last_checked: 0, + } } } impl CheckResult { pub const fn empty() -> Self { - CheckResult { region_id: 0, computed_crc: 0, baseline_crc: 0, mismatch: false } + CheckResult { + region_id: 0, + computed_crc: 0, + baseline_crc: 0, + mismatch: false, + } } } @@ -141,11 +151,7 @@ impl ChecksumTable { /// Check a batch of regions. Input is an array of (region_id, data) pairs. /// Output bounded by MAX_CHECK_PER_CYCLE. - pub fn check_batch( - &mut self, - region_data: &[(u32, &[u8])], - current_time: u64, - ) -> CheckOutput { + pub fn check_batch(&mut self, region_data: &[(u32, &[u8])], current_time: u64) -> CheckOutput { let mut output = CheckOutput { results: [CheckResult::empty(); MAX_CHECK_PER_CYCLE], result_count: 0, diff --git a/crates/relay-dronecan/plain/src/crc.rs b/crates/relay-dronecan/plain/src/crc.rs index bd49b35c..932938e6 100644 --- a/crates/relay-dronecan/plain/src/crc.rs +++ b/crates/relay-dronecan/plain/src/crc.rs @@ -18,7 +18,11 @@ pub fn crc16_add(mut crc: u16, data: &[u8]) -> u16 { crc ^= (b as u16) << 8; let mut i = 0; while i < 8 { - crc = if crc & 0x8000 != 0 { (crc << 1) ^ 0x1021 } else { crc << 1 }; + crc = if crc & 0x8000 != 0 { + (crc << 1) ^ 0x1021 + } else { + crc << 1 + }; i += 1; } } diff --git a/crates/relay-dronecan/plain/src/dsdl.rs b/crates/relay-dronecan/plain/src/dsdl.rs index dde9c2a6..e893a2d6 100644 --- a/crates/relay-dronecan/plain/src/dsdl.rs +++ b/crates/relay-dronecan/plain/src/dsdl.rs @@ -129,7 +129,13 @@ mod tests { #[test] fn write_then_read_round_trips() { // the codec is self-inverse for any offset/width/value in range - for &(o, w, v) in &[(0usize, 14usize, 8191u64), (14, 14, 0x2000), (32, 2, 2), (34, 3, 5), (5, 18, 0x3ABCD)] { + for &(o, w, v) in &[ + (0usize, 14usize, 8191u64), + (14, 14, 0x2000), + (32, 2, 2), + (34, 3, 5), + (5, 18, 0x3ABCD), + ] { let mut buf = [0u8; 16]; write_uint(&mut buf, o, w, v); assert_eq!(read_uint(&buf, o, w), v & ((1u64 << w) - 1), "o={o} w={w}"); diff --git a/crates/relay-dronecan/plain/src/float16.rs b/crates/relay-dronecan/plain/src/float16.rs index 7059a50b..9678fbcf 100644 --- a/crates/relay-dronecan/plain/src/float16.rs +++ b/crates/relay-dronecan/plain/src/float16.rs @@ -17,11 +17,7 @@ pub fn f16_to_f32(bits: u16) -> f32 { } 0x1F => { // inf (frac == 0) or NaN - if frac == 0 { - f32::INFINITY - } else { - f32::NAN - } + if frac == 0 { f32::INFINITY } else { f32::NAN } } _ => { // normal: (1 + frac/1024) * 2^(exp-15) diff --git a/crates/relay-dronecan/plain/src/id.rs b/crates/relay-dronecan/plain/src/id.rs index 82dafee6..c01fcc67 100644 --- a/crates/relay-dronecan/plain/src/id.rs +++ b/crates/relay-dronecan/plain/src/id.rs @@ -53,22 +53,33 @@ mod tests { #[test] fn nodestatus_id_round_trips() { // priority 16, DTID 341 (NodeStatus), node 42 - let m = MessageId { priority: 16, data_type_id: 341, source_node_id: 42 }; + let m = MessageId { + priority: 16, + data_type_id: 341, + source_node_id: 42, + }; let raw = encode_message_id(&m); assert_eq!(decode_message_id(raw), Some(m)); } #[test] fn service_frame_is_rejected() { - let raw = encode_message_id(&MessageId { priority: 0, data_type_id: 1, source_node_id: 1 }) - | SERVICE_NOT_MESSAGE; + let raw = encode_message_id(&MessageId { + priority: 0, + data_type_id: 1, + source_node_id: 1, + }) | SERVICE_NOT_MESSAGE; assert_eq!(decode_message_id(raw), None); } #[test] fn fields_are_masked_to_width() { // node id is 7 bits: 0x7F is the max - let m = MessageId { priority: 31, data_type_id: 0xFFFF, source_node_id: 0x7F }; + let m = MessageId { + priority: 31, + data_type_id: 0xFFFF, + source_node_id: 0x7F, + }; assert_eq!(decode_message_id(encode_message_id(&m)), Some(m)); } } diff --git a/crates/relay-dronecan/plain/src/kani_proofs.rs b/crates/relay-dronecan/plain/src/kani_proofs.rs index 75d54e4f..4787e2d0 100644 --- a/crates/relay-dronecan/plain/src/kani_proofs.rs +++ b/crates/relay-dronecan/plain/src/kani_proofs.rs @@ -7,10 +7,10 @@ #![cfg(kani)] use crate::dsdl; -use crate::id::{decode_message_id, encode_message_id, MessageId}; -use crate::msg::{decode_node_status, encode_node_status, encode_raw_command, NodeStatus, MAX_ESC}; +use crate::id::{MessageId, decode_message_id, encode_message_id}; +use crate::msg::{MAX_ESC, NodeStatus, decode_node_status, encode_node_status, encode_raw_command}; use crate::tail::{decode_tail, encode_tail}; -use crate::transfer::{encode_single_frame, CanFrame, Reassembler, MAX_PAYLOAD}; +use crate::transfer::{CanFrame, MAX_PAYLOAD, Reassembler, encode_single_frame}; /// DC-K01 — the message-id round-trip is exact for in-range fields: encode then /// decode recovers priority/dtid/node for ANY field values (masked to width). @@ -162,8 +162,15 @@ fn verify_dsdl_read_bounded() { // NodeStatus health(2)/mode(3)/sub_mode(3); esc.Status power_rating(7)/ // esc_index(5)/rpm(18); esc.RawCommand int14; + an out-of-range case (120+18 // > 128) exercising the stream_bit guard. - let cases: [(usize, usize); 7] = - [(32, 2), (34, 3), (37, 3), (98, 7), (105, 5), (80, 18), (120, 18)]; + let cases: [(usize, usize); 7] = [ + (32, 2), + (34, 3), + (37, 3), + (98, 7), + (105, 5), + (80, 18), + (120, 18), + ]; let mut i = 0; while i < cases.len() { let (o, w) = cases[i]; diff --git a/crates/relay-dronecan/plain/src/msg.rs b/crates/relay-dronecan/plain/src/msg.rs index 06c97687..b33e2e3a 100644 --- a/crates/relay-dronecan/plain/src/msg.rs +++ b/crates/relay-dronecan/plain/src/msg.rs @@ -177,7 +177,10 @@ mod tests { sub_mode: 1, vendor_status: 0xBEEF, }; - assert_eq!(encode_node_status(&s), [0x04, 0x03, 0x02, 0x01, 0x99, 0xEF, 0xBE]); + assert_eq!( + encode_node_status(&s), + [0x04, 0x03, 0x02, 0x01, 0x99, 0xEF, 0xBE] + ); } #[test] diff --git a/crates/relay-dronecan/plain/src/node.rs b/crates/relay-dronecan/plain/src/node.rs index 65950a83..96bd4cf2 100644 --- a/crates/relay-dronecan/plain/src/node.rs +++ b/crates/relay-dronecan/plain/src/node.rs @@ -9,8 +9,8 @@ //! follow-on. The decode/dispatch step is the verified reassembler — the async //! path adds only the await on the bus. -use crate::msg::{encode_node_status, NodeStatus, DTID_NODE_STATUS}; -use crate::transfer::{encode_single_frame, Reassembler, Transfer}; +use crate::msg::{DTID_NODE_STATUS, NodeStatus, encode_node_status}; +use crate::transfer::{Reassembler, Transfer, encode_single_frame}; use relay_hal::CanBus; /// DroneCAN transfer priority used for the NodeStatus heartbeat (mid priority). @@ -89,8 +89,18 @@ mod tests { impl MockCanBus { fn new() -> Self { - let empty = CanFrame { id: 0, dlc: 0, data: [0; 8] }; - Self { rx: [empty; 4], rx_len: 0, rx_pos: 0, tx: [empty; 4], tx_len: 0 } + let empty = CanFrame { + id: 0, + dlc: 0, + data: [0; 8], + }; + Self { + rx: [empty; 4], + rx_len: 0, + rx_pos: 0, + tx: [empty; 4], + tx_len: 0, + } } } @@ -141,7 +151,9 @@ mod tests { bus.rx[0] = frame; bus.rx_len = 1; let mut node = DroneCanNode::new(bus, 9, 0); - let asyncd = block_on(node.poll()).unwrap().expect("poll yields the transfer"); + let asyncd = block_on(node.poll()) + .unwrap() + .expect("poll yields the transfer"); assert_eq!(asyncd.data_type_id, sync.data_type_id); assert_eq!(asyncd.len, sync.len); diff --git a/crates/relay-dronecan/plain/src/sensors.rs b/crates/relay-dronecan/plain/src/sensors.rs index 8445eb94..54d19e11 100644 --- a/crates/relay-dronecan/plain/src/sensors.rs +++ b/crates/relay-dronecan/plain/src/sensors.rs @@ -125,7 +125,9 @@ mod tests { /// use the DSDL codec (the LSB-first read_bits was wrong). #[test] fn esc_status_decodes_fields() { - let p = [0x07, 0, 0, 0, 0x00, 0x49, 0x00, 0x3c, 0x88, 0x5c, 0x88, 0x13, 0x19, 0x0c]; + let p = [ + 0x07, 0, 0, 0, 0x00, 0x49, 0x00, 0x3c, 0x88, 0x5c, 0x88, 0x13, 0x19, 0x0c, + ]; let s = decode_esc_status(&p).unwrap(); assert_eq!(s.error_count, 7); assert_eq!(s.voltage, 10.0); diff --git a/crates/relay-dronecan/plain/src/transfer.rs b/crates/relay-dronecan/plain/src/transfer.rs index a6000e8b..a317be50 100644 --- a/crates/relay-dronecan/plain/src/transfer.rs +++ b/crates/relay-dronecan/plain/src/transfer.rs @@ -17,7 +17,7 @@ //! state-machine core proven here is unchanged by it. use crate::crc::{crc16_add, crc16_signature}; -use crate::id::{encode_message_id, decode_message_id, MessageId}; +use crate::id::{MessageId, decode_message_id, encode_message_id}; use crate::tail::{decode_tail, single_frame_tail}; /// The CAN frame value type is owned by the relay-hal seam (jess binds FlexCAN @@ -205,7 +205,11 @@ pub fn encode_single_frame( data[..payload.len()].copy_from_slice(payload); data[payload.len()] = single_frame_tail(transfer_id); Some(CanFrame { - id: encode_message_id(&MessageId { priority, data_type_id, source_node_id }), + id: encode_message_id(&MessageId { + priority, + data_type_id, + source_node_id, + }), dlc: (payload.len() + 1) as u8, data, }) @@ -214,8 +218,8 @@ pub fn encode_single_frame( #[cfg(test)] mod tests { use super::*; - use crate::id::{encode_message_id, MessageId}; - use crate::tail::{encode_tail, single_frame_tail, Tail}; + use crate::id::{MessageId, encode_message_id}; + use crate::tail::{Tail, encode_tail, single_frame_tail}; const SIG: u64 = 0x0102_0304_0506_0708; @@ -225,7 +229,11 @@ mod tests { data[..n].copy_from_slice(&body[..n]); data[n] = tailb; CanFrame { - id: encode_message_id(&MessageId { priority: 16, data_type_id: dtid, source_node_id: node }), + id: encode_message_id(&MessageId { + priority: 16, + data_type_id: dtid, + source_node_id: node, + }), dlc: (n + 1) as u8, data, } @@ -234,7 +242,9 @@ mod tests { #[test] fn single_frame_transfer_emits_payload() { let mut r = Reassembler::new(SIG); - let t = r.push(&frame(341, 42, single_frame_tail(3), &[0xAA, 0xBB, 0xCC])).unwrap(); + let t = r + .push(&frame(341, 42, single_frame_tail(3), &[0xAA, 0xBB, 0xCC])) + .unwrap(); assert_eq!(t.data_type_id, 341); assert_eq!(t.source_node_id, 42); assert_eq!(t.transfer_id, 3); @@ -250,12 +260,26 @@ mod tests { let crc_b = crc.to_le_bytes(); let mut r = Reassembler::new(SIG); // frame 1: SOT, toggle 0 — [crc_lo, crc_hi, p0..p4] - let f1_body = [crc_b[0], crc_b[1], payload[0], payload[1], payload[2], payload[3], payload[4]]; - let t1 = encode_tail(&Tail { start_of_transfer: true, end_of_transfer: false, toggle: false, transfer_id: 5 }); + let f1_body = [ + crc_b[0], crc_b[1], payload[0], payload[1], payload[2], payload[3], payload[4], + ]; + let t1 = encode_tail(&Tail { + start_of_transfer: true, + end_of_transfer: false, + toggle: false, + transfer_id: 5, + }); assert!(r.push(&frame(1063, 7, t1, &f1_body)).is_none()); // frame 2: EOT, toggle 1 — [p5..p8] - let t2 = encode_tail(&Tail { start_of_transfer: false, end_of_transfer: true, toggle: true, transfer_id: 5 }); - let out = r.push(&frame(1063, 7, t2, &payload[5..])).expect("transfer completes"); + let t2 = encode_tail(&Tail { + start_of_transfer: false, + end_of_transfer: true, + toggle: true, + transfer_id: 5, + }); + let out = r + .push(&frame(1063, 7, t2, &payload[5..])) + .expect("transfer completes"); assert_eq!(&out.payload[..out.len], &payload); assert_eq!(out.data_type_id, 1063); } @@ -265,20 +289,42 @@ mod tests { let payload: [u8; 9] = [1, 2, 3, 4, 5, 6, 7, 8, 9]; let bad = 0x0000u16.to_le_bytes(); // wrong CRC let mut r = Reassembler::new(SIG); - let f1 = [bad[0], bad[1], payload[0], payload[1], payload[2], payload[3], payload[4]]; - let t1 = encode_tail(&Tail { start_of_transfer: true, end_of_transfer: false, toggle: false, transfer_id: 5 }); + let f1 = [ + bad[0], bad[1], payload[0], payload[1], payload[2], payload[3], payload[4], + ]; + let t1 = encode_tail(&Tail { + start_of_transfer: true, + end_of_transfer: false, + toggle: false, + transfer_id: 5, + }); r.push(&frame(1063, 7, t1, &f1)); - let t2 = encode_tail(&Tail { start_of_transfer: false, end_of_transfer: true, toggle: true, transfer_id: 5 }); + let t2 = encode_tail(&Tail { + start_of_transfer: false, + end_of_transfer: true, + toggle: true, + transfer_id: 5, + }); assert!(r.push(&frame(1063, 7, t2, &payload[5..])).is_none()); // CRC sink drops it } #[test] fn wrong_toggle_drops_the_transfer() { let mut r = Reassembler::new(SIG); - let t1 = encode_tail(&Tail { start_of_transfer: true, end_of_transfer: false, toggle: false, transfer_id: 5 }); + let t1 = encode_tail(&Tail { + start_of_transfer: true, + end_of_transfer: false, + toggle: false, + transfer_id: 5, + }); r.push(&frame(1063, 7, t1, &[0, 0, 1, 2, 3])); // continuation with the WRONG toggle (expected 1, send 0) - let bad = encode_tail(&Tail { start_of_transfer: false, end_of_transfer: true, toggle: false, transfer_id: 5 }); + let bad = encode_tail(&Tail { + start_of_transfer: false, + end_of_transfer: true, + toggle: false, + transfer_id: 5, + }); assert!(r.push(&frame(1063, 7, bad, &[4, 5])).is_none()); assert!(!r.active); // dropped } diff --git a/crates/relay-ds/plain/src/engine.rs b/crates/relay-ds/plain/src/engine.rs index ab965b8a..b19637e6 100644 --- a/crates/relay-ds/plain/src/engine.rs +++ b/crates/relay-ds/plain/src/engine.rs @@ -7,7 +7,11 @@ pub const MAX_DECISIONS_PER_CHECK: usize = 16; #[derive(Clone, Copy, PartialEq, Eq)] #[repr(u8)] -pub enum FileType { Sequence = 0, Time = 1, Count = 2 } +pub enum FileType { + Sequence = 0, + Time = 1, + Count = 2, +} #[derive(Clone, Copy)] pub struct FilterEntry { @@ -64,14 +68,18 @@ impl FilterTable { } pub fn add_filter(&mut self, entry: FilterEntry) -> bool { - if self.filter_count as usize >= MAX_FILTERS { return false; } + if self.filter_count as usize >= MAX_FILTERS { + return false; + } let idx = self.filter_count as usize; self.filters[idx] = entry; self.filter_count = self.filter_count + 1; true } - pub fn filter_count(&self) -> u32 { self.filter_count } + pub fn filter_count(&self) -> u32 { + self.filter_count + } pub fn evaluate(&self, data_id: u32) -> FilterResult { let mut result = FilterResult { @@ -82,7 +90,9 @@ impl FilterTable { let count = self.filter_count; let mut i: u32 = 0; while i < count { - if result.decision_count as usize >= MAX_DECISIONS_PER_CHECK { break; } + if result.decision_count as usize >= MAX_DECISIONS_PER_CHECK { + break; + } let idx = i as usize; let f = self.filters[idx]; diff --git a/crates/relay-ekf-stub/plain/src/lib.rs b/crates/relay-ekf-stub/plain/src/lib.rs index 7027bdf7..5443901b 100644 --- a/crates/relay-ekf-stub/plain/src/lib.rs +++ b/crates/relay-ekf-stub/plain/src/lib.rs @@ -48,7 +48,12 @@ pub struct EkfStub { impl EkfStub { pub const fn new() -> Self { - Self { last_time: Timestamp { seconds: 0, fraction: 0 } } + Self { + last_time: Timestamp { + seconds: 0, + fraction: 0, + }, + } } /// Advance the stub by one tick. The real EKF (v0.2+) consumes @@ -78,7 +83,11 @@ pub const QUATERNION_NORM_TOLERANCE: f32 = 1.0e-6; /// Mirrors the contract that the real EKF will satisfy under Verus. pub fn is_unit_quaternion(q: [f32; 4]) -> bool { let norm_sq = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; - let diff = if norm_sq >= 1.0 { norm_sq - 1.0 } else { 1.0 - norm_sq }; + let diff = if norm_sq >= 1.0 { + norm_sq - 1.0 + } else { + 1.0 - norm_sq + }; diff <= QUATERNION_NORM_TOLERANCE } @@ -89,13 +98,22 @@ mod tests { #[test] fn fresh_stub_has_zero_time() { let s = EkfStub::new(); - assert_eq!(s.last_time(), Timestamp { seconds: 0, fraction: 0 }); + assert_eq!( + s.last_time(), + Timestamp { + seconds: 0, + fraction: 0 + } + ); } #[test] fn tick_returns_identity_quaternion() { let mut s = EkfStub::new(); - let state = s.tick(Timestamp { seconds: 1, fraction: 0 }); + let state = s.tick(Timestamp { + seconds: 1, + fraction: 0, + }); assert_eq!(state.quaternion, [1.0, 0.0, 0.0, 0.0]); assert!(is_unit_quaternion(state.quaternion)); } @@ -103,7 +121,10 @@ mod tests { #[test] fn tick_returns_zero_position_and_velocity() { let mut s = EkfStub::new(); - let state = s.tick(Timestamp { seconds: 1, fraction: 0 }); + let state = s.tick(Timestamp { + seconds: 1, + fraction: 0, + }); assert_eq!(state.position_ned, [0.0, 0.0, 0.0]); assert_eq!(state.velocity_ned, [0.0, 0.0, 0.0]); } @@ -111,7 +132,10 @@ mod tests { #[test] fn tick_passes_time_through() { let mut s = EkfStub::new(); - let t = Timestamp { seconds: 42, fraction: 12345 }; + let t = Timestamp { + seconds: 42, + fraction: 12345, + }; let state = s.tick(t); assert_eq!(state.time, t); assert_eq!(s.last_time(), t); @@ -122,7 +146,10 @@ mod tests { // Real EKF reports innovation as residual magnitude; stub // never sees sensor disagreement, so it stays at 0.0. let mut s = EkfStub::new(); - let state = s.tick(Timestamp { seconds: 0, fraction: 0 }); + let state = s.tick(Timestamp { + seconds: 0, + fraction: 0, + }); assert_eq!(state.innovation, 0.0); } @@ -130,7 +157,10 @@ mod tests { fn deterministic_across_ticks_with_same_time() { let mut a = EkfStub::new(); let mut b = EkfStub::new(); - let t = Timestamp { seconds: 7, fraction: 0 }; + let t = Timestamp { + seconds: 7, + fraction: 0, + }; let state_a = a.tick(t); let state_b = b.tick(t); assert_eq!(state_a, state_b); diff --git a/crates/relay-ekf/plain/src/lib.rs b/crates/relay-ekf/plain/src/lib.rs index 65d1279b..4b414e8d 100644 --- a/crates/relay-ekf/plain/src/lib.rs +++ b/crates/relay-ekf/plain/src/lib.rs @@ -68,7 +68,10 @@ pub struct Timestamp { } impl Timestamp { - pub const ZERO: Self = Self { seconds: 0, fraction: 0 }; + pub const ZERO: Self = Self { + seconds: 0, + fraction: 0, + }; /// Seconds (f32) from the epoch. pub fn as_secs_f32(self) -> f32 { @@ -440,7 +443,10 @@ mod tests { fn imu_at(secs: f32, accel: [f32; 3], gyro: [f32; 3]) -> ImuSample { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; ImuSample { - time: Timestamp { seconds: secs as u64, fraction: frac }, + time: Timestamp { + seconds: secs as u64, + fraction: frac, + }, accel_body: accel, gyro_body: gyro, } @@ -505,11 +511,16 @@ mod tests { assert!(is_unit_quaternion(q)); // Verify body-z axis is aligned with NED-z within tolerance. let z_body_in_ned = rotate_body_to_ned_inverse(quat_conj(q), [0.0, 0.0, 1.0]); - assert!(z_body_in_ned[2] > 0.99, + assert!( + z_body_in_ned[2] > 0.99, "body-down axis should be aligned with NED-down after convergence; got z={}", - z_body_in_ned[2]); - assert!(e.last_innovation() < 0.01, - "innovation should converge near zero; got {}", e.last_innovation()); + z_body_in_ned[2] + ); + assert!( + e.last_innovation() < 0.01, + "innovation should converge near zero; got {}", + e.last_innovation() + ); } #[test] @@ -527,9 +538,11 @@ mod tests { // After convergence the body-z axis should still align with NED-z // (yaw doesn't tilt the gravity-aligned frame). let z_body_in_ned = rotate_body_to_ned_inverse(quat_conj(q), [0.0, 0.0, 1.0]); - assert!(z_body_in_ned[2] > 0.99, + assert!( + z_body_in_ned[2] > 0.99, "yaw rotation must not tilt the attitude estimate; z={}", - z_body_in_ned[2]); + z_body_in_ned[2] + ); } #[test] @@ -541,12 +554,20 @@ mod tests { for deg in [0.0_f32, 5.0, 10.0, 20.0, 40.0, 60.0] { let rad = deg.to_radians(); // Tilt accel about body-x: gravity now has body-y component. - let accel = [0.0, relay_math::sinf(rad) * 9.81, relay_math::cosf(rad) * 9.81]; + let accel = [ + 0.0, + relay_math::sinf(rad) * 9.81, + relay_math::cosf(rad) * 9.81, + ]; let mut e = Ekf::new(); e.tick(imu_at(0.01, accel, [0.0; 3])); - assert!(e.last_innovation() + 1.0e-6 >= prev, + assert!( + e.last_innovation() + 1.0e-6 >= prev, "innovation must be non-decreasing in tilt: deg={} inn={} prev={}", - deg, e.last_innovation(), prev); + deg, + e.last_innovation(), + prev + ); prev = e.last_innovation(); } } @@ -563,8 +584,12 @@ mod tests { e.tick(imu_at(t, bad_accel, [0.0; 3])); } for i in 0..3 { - assert!(e.bias()[i].abs() <= 0.5 + 1.0e-6, - "bias[{}] = {} out of bound", i, e.bias()[i]); + assert!( + e.bias()[i].abs() <= 0.5 + 1.0e-6, + "bias[{}] = {} out of bound", + i, + e.bias()[i] + ); } assert!(is_unit_quaternion(e.quaternion())); } diff --git a/crates/relay-flowrange/plain/src/lib.rs b/crates/relay-flowrange/plain/src/lib.rs index 9d6d0c8b..8621df15 100644 --- a/crates/relay-flowrange/plain/src/lib.rs +++ b/crates/relay-flowrange/plain/src/lib.rs @@ -28,7 +28,12 @@ /// Returns `None` (reading rejected) if the range is non-finite or outside the /// sensor's valid band `[min_valid, max_valid]`. The tilt is clamped to a sane /// range so an absurd attitude can't invert the sign. -pub fn range_to_altitude(range_m: f32, tilt_rad: f32, min_valid: f32, max_valid: f32) -> Option { +pub fn range_to_altitude( + range_m: f32, + tilt_rad: f32, + min_valid: f32, + max_valid: f32, +) -> Option { if !range_m.is_finite() || range_m < min_valid || range_m > max_valid { return None; } @@ -39,11 +44,7 @@ pub fn range_to_altitude(range_m: f32, tilt_rad: f32, min_valid: f32, max_valid: 0.0 }; let alt = range_m * relay_math::cosf(t); - if alt.is_finite() { - Some(alt) - } else { - None - } + if alt.is_finite() { Some(alt) } else { None } } /// Horizontal velocity from optical flow. diff --git a/crates/relay-flowrange/plain/src/tf02.rs b/crates/relay-flowrange/plain/src/tf02.rs index 3edbd335..1edea80f 100644 --- a/crates/relay-flowrange/plain/src/tf02.rs +++ b/crates/relay-flowrange/plain/src/tf02.rs @@ -76,7 +76,10 @@ pub fn decode_tf02_frame(frame: &[u8; TF02_FRAME_LEN]) -> Result Self { - FilePath { bytes: [0u8; MAX_PATH_LEN], len: 0 } + FilePath { + bytes: [0u8; MAX_PATH_LEN], + len: 0, + } } pub fn from_bytes(src: &[u8]) -> Self { let mut path = FilePath::empty(); - let copy_len = if src.len() <= MAX_PATH_LEN { src.len() } else { MAX_PATH_LEN }; + let copy_len = if src.len() <= MAX_PATH_LEN { + src.len() + } else { + MAX_PATH_LEN + }; let mut i = 0; while i < copy_len { path.bytes[i] = src[i]; @@ -93,7 +115,7 @@ pub fn validate_request(req: &FmRequest) -> FmValidation { match req.command { FmCommand::Delete | FmCommand::DeleteDir => { // These commands don't need a dest path - }, + } _ => { if !validate_path(&req.dest) { if req.dest.len as usize > MAX_PATH_LEN { @@ -101,7 +123,7 @@ pub fn validate_request(req: &FmRequest) -> FmValidation { } return FmValidation::InvalidPath; } - }, + } } // Check source == dest for Copy/Move/Rename match req.command { @@ -109,8 +131,8 @@ pub fn validate_request(req: &FmRequest) -> FmValidation { if paths_equal(&req.source, &req.dest) { return FmValidation::SourceEqDest; } - }, - _ => {}, + } + _ => {} } FmValidation::Valid } @@ -154,7 +176,11 @@ mod tests { fn test_source_eq_dest_rejected() { let src = make_path(b"/data/file.bin"); let dest = make_path(b"/data/file.bin"); - let req = FmRequest { command: FmCommand::Copy, source: src, dest }; + let req = FmRequest { + command: FmCommand::Copy, + source: src, + dest, + }; assert_eq!(validate_request(&req), FmValidation::SourceEqDest); } @@ -162,7 +188,11 @@ mod tests { fn test_valid_copy_command() { let src = make_path(b"/data/a.bin"); let dest = make_path(b"/data/b.bin"); - let req = FmRequest { command: FmCommand::Copy, source: src, dest }; + let req = FmRequest { + command: FmCommand::Copy, + source: src, + dest, + }; assert_eq!(validate_request(&req), FmValidation::Valid); } @@ -170,7 +200,11 @@ mod tests { fn test_delete_no_dest_needed() { let src = make_path(b"/data/old.bin"); let dest = FilePath::empty(); // empty dest is fine for delete - let req = FmRequest { command: FmCommand::Delete, source: src, dest }; + let req = FmRequest { + command: FmCommand::Delete, + source: src, + dest, + }; assert_eq!(validate_request(&req), FmValidation::Valid); } @@ -179,18 +213,46 @@ mod tests { // Valid let src = make_path(b"/a"); let dest = make_path(b"/b"); - assert_eq!(validate_request(&FmRequest { command: FmCommand::Move, source: src, dest }), FmValidation::Valid); + assert_eq!( + validate_request(&FmRequest { + command: FmCommand::Move, + source: src, + dest + }), + FmValidation::Valid + ); // InvalidPath (empty source) - assert_eq!(validate_request(&FmRequest { command: FmCommand::Copy, source: FilePath::empty(), dest }), FmValidation::InvalidPath); + assert_eq!( + validate_request(&FmRequest { + command: FmCommand::Copy, + source: FilePath::empty(), + dest + }), + FmValidation::InvalidPath + ); // PathTooLong let mut long = FilePath::empty(); long.len = (MAX_PATH_LEN as u32) + 1; - assert_eq!(validate_request(&FmRequest { command: FmCommand::Copy, source: long, dest }), FmValidation::PathTooLong); + assert_eq!( + validate_request(&FmRequest { + command: FmCommand::Copy, + source: long, + dest + }), + FmValidation::PathTooLong + ); // SourceEqDest - assert_eq!(validate_request(&FmRequest { command: FmCommand::Rename, source: src, dest: src }), FmValidation::SourceEqDest); + assert_eq!( + validate_request(&FmRequest { + command: FmCommand::Rename, + source: src, + dest: src + }), + FmValidation::SourceEqDest + ); } #[test] @@ -210,12 +272,21 @@ mod tests { let src = make_path(b"/src"); let dest = make_path(b"/dest"); let commands = [ - FmCommand::Copy, FmCommand::Move, FmCommand::Rename, - FmCommand::Delete, FmCommand::CreateDir, FmCommand::DeleteDir, - FmCommand::Decompress, FmCommand::Concat, + FmCommand::Copy, + FmCommand::Move, + FmCommand::Rename, + FmCommand::Delete, + FmCommand::CreateDir, + FmCommand::DeleteDir, + FmCommand::Decompress, + FmCommand::Concat, ]; for cmd in commands { - let req = FmRequest { command: cmd, source: src, dest }; + let req = FmRequest { + command: cmd, + source: src, + dest, + }; let v = validate_request(&req); assert_eq!(v, FmValidation::Valid); } diff --git a/crates/relay-fsafe/plain/src/lib.rs b/crates/relay-fsafe/plain/src/lib.rs index eb71b102..3de5016f 100644 --- a/crates/relay-fsafe/plain/src/lib.rs +++ b/crates/relay-fsafe/plain/src/lib.rs @@ -102,7 +102,9 @@ pub struct FailsafeArbiter { impl FailsafeArbiter { /// A fresh arbiter with no failsafe latched. pub fn new() -> Self { - Self { latched: FailsafeAction::None } + Self { + latched: FailsafeAction::None, + } } /// Evaluate this cycle's triggers and return the action to take. The result @@ -147,20 +149,36 @@ mod tests { #[test] fn rc_loss_returns_to_launch() { let mut a = FailsafeArbiter::new(); - assert_eq!(a.evaluate(Triggers { rc_loss: true, ..t() }), FailsafeAction::Rtl); + assert_eq!( + a.evaluate(Triggers { + rc_loss: true, + ..t() + }), + FailsafeAction::Rtl + ); } #[test] fn offboard_stale_holds() { let mut a = FailsafeArbiter::new(); - assert_eq!(a.evaluate(Triggers { offboard_stale: true, ..t() }), FailsafeAction::Hold); + assert_eq!( + a.evaluate(Triggers { + offboard_stale: true, + ..t() + }), + FailsafeAction::Hold + ); } #[test] fn most_severe_wins() { let mut a = FailsafeArbiter::new(); // RC loss (Rtl) + critical battery (Land) ⇒ Land. - let act = a.evaluate(Triggers { rc_loss: true, critical_battery: true, ..t() }); + let act = a.evaluate(Triggers { + rc_loss: true, + critical_battery: true, + ..t() + }); assert_eq!(act, FailsafeAction::Land); } @@ -168,7 +186,11 @@ mod tests { fn no_blind_rtl_when_position_lost() { let mut a = FailsafeArbiter::new(); // RC loss would be Rtl, but position is lost ⇒ must be Land, never Rtl. - let act = a.evaluate(Triggers { rc_loss: true, position_loss: true, ..t() }); + let act = a.evaluate(Triggers { + rc_loss: true, + position_loss: true, + ..t() + }); assert_eq!(act, FailsafeAction::Land); assert_ne!(act, FailsafeAction::Rtl); } @@ -176,17 +198,29 @@ mod tests { #[test] fn latch_does_not_downgrade() { let mut a = FailsafeArbiter::new(); - a.evaluate(Triggers { critical_battery: true, ..t() }); // Land + a.evaluate(Triggers { + critical_battery: true, + ..t() + }); // Land // triggers clear, but the latch holds the severe action. assert_eq!(a.evaluate(t()), FailsafeAction::Land); // a less-severe trigger cannot lower it. - assert_eq!(a.evaluate(Triggers { offboard_stale: true, ..t() }), FailsafeAction::Land); + assert_eq!( + a.evaluate(Triggers { + offboard_stale: true, + ..t() + }), + FailsafeAction::Land + ); } #[test] fn reset_clears_on_disarm() { let mut a = FailsafeArbiter::new(); - a.evaluate(Triggers { rc_loss: true, ..t() }); + a.evaluate(Triggers { + rc_loss: true, + ..t() + }); a.reset(); assert_eq!(a.action(), FailsafeAction::None); } diff --git a/crates/relay-fsm/plain/src/lib.rs b/crates/relay-fsm/plain/src/lib.rs index fd7f688a..583e24c5 100644 --- a/crates/relay-fsm/plain/src/lib.rs +++ b/crates/relay-fsm/plain/src/lib.rs @@ -50,7 +50,10 @@ impl Mode { /// is EXCLUDED: its motors are already cut by design (the termination is the /// intentional last-resort), so the never-cut-motors-airborne concern is moot. pub fn is_airborne(self) -> bool { - matches!(self, Mode::Takeoff | Mode::Loiter | Mode::Mission | Mode::Rtl | Mode::Land) + matches!( + self, + Mode::Takeoff | Mode::Loiter | Mode::Mission | Mode::Rtl | Mode::Land + ) } } @@ -106,7 +109,9 @@ impl Default for FlightFsm { impl FlightFsm { pub fn new() -> Self { - FlightFsm { mode: Mode::Disarmed } + FlightFsm { + mode: Mode::Disarmed, + } } pub fn mode(&self) -> Mode { @@ -171,7 +176,12 @@ mod tests { fn g(level: bool, throttle_low: bool, have_position: bool) -> Gates { // prearm_ok defaults true here so the existing physical-gate tests are // unchanged; the prearm-blocks-arm cases set it explicitly below. - Gates { level, throttle_low, have_position, prearm_ok: true } + Gates { + level, + throttle_low, + have_position, + prearm_ok: true, + } } #[test] @@ -179,9 +189,18 @@ mod tests { let mut f = FlightFsm::new(); assert_eq!(f.mode(), Mode::Disarmed); assert_eq!(f.on(Event::Arm, g(true, true, true)), Mode::Armed); - assert_eq!(f.on(Event::RequestTakeoff, g(true, true, true)), Mode::Takeoff); - assert_eq!(f.on(Event::ReachedAltitude, g(true, false, true)), Mode::Loiter); - assert_eq!(f.on(Event::RequestMission, g(true, false, true)), Mode::Mission); + assert_eq!( + f.on(Event::RequestTakeoff, g(true, true, true)), + Mode::Takeoff + ); + assert_eq!( + f.on(Event::ReachedAltitude, g(true, false, true)), + Mode::Loiter + ); + assert_eq!( + f.on(Event::RequestMission, g(true, false, true)), + Mode::Mission + ); assert_eq!(f.on(Event::ReachedHome, g(true, false, true)), Mode::Loiter); assert_eq!(f.on(Event::RequestLand, g(true, false, true)), Mode::Land); assert_eq!(f.on(Event::Touchdown, g(true, true, true)), Mode::Disarmed); @@ -200,7 +219,12 @@ mod tests { // Physically ready (level + throttle idle) but the commander pre-arm // verdict is false → arming is refused, the motors stay safe. let mut f = FlightFsm::new(); - let blocked = Gates { level: true, throttle_low: true, have_position: true, prearm_ok: false }; + let blocked = Gates { + level: true, + throttle_low: true, + have_position: true, + prearm_ok: false, + }; assert_eq!(f.on(Event::Arm, blocked), Mode::Disarmed); // and once the checks pass, the same physical state arms. assert_eq!(f.on(Event::Arm, g(true, true, true)), Mode::Armed); @@ -210,7 +234,11 @@ mod tests { fn cannot_disarm_airborne() { for start in [Mode::Takeoff, Mode::Loiter, Mode::Mission, Mode::Rtl] { let mut f = FlightFsm { mode: start }; - assert_eq!(f.on(Event::RequestDisarm, g(true, true, true)), start, "disarm must no-op in {start:?}"); + assert_eq!( + f.on(Event::RequestDisarm, g(true, true, true)), + start, + "disarm must no-op in {start:?}" + ); } } @@ -227,12 +255,28 @@ mod tests { Mode::Rtl, ] { let mut f = FlightFsm { mode: start }; - assert_eq!(f.on(Event::Terminate, g(true, true, true)), Mode::Terminated, "{start:?} → Terminated"); + assert_eq!( + f.on(Event::Terminate, g(true, true, true)), + Mode::Terminated, + "{start:?} → Terminated" + ); } // and Terminated is absorbing: no event leaves it. - let mut f = FlightFsm { mode: Mode::Terminated }; - for ev in [Event::Arm, Event::RequestTakeoff, Event::Failsafe, Event::Touchdown, Event::RequestDisarm] { - assert_eq!(f.on(ev, g(true, true, true)), Mode::Terminated, "Terminated absorbs {ev:?}"); + let mut f = FlightFsm { + mode: Mode::Terminated, + }; + for ev in [ + Event::Arm, + Event::RequestTakeoff, + Event::Failsafe, + Event::Touchdown, + Event::RequestDisarm, + ] { + assert_eq!( + f.on(ev, g(true, true, true)), + Mode::Terminated, + "Terminated absorbs {ev:?}" + ); } } @@ -240,9 +284,17 @@ mod tests { fn failsafe_recovers_from_flight() { for start in [Mode::Takeoff, Mode::Loiter, Mode::Mission] { let mut f = FlightFsm { mode: start }; - assert_eq!(f.on(Event::Failsafe, g(true, false, true)), Mode::Rtl, "with position → RTL"); + assert_eq!( + f.on(Event::Failsafe, g(true, false, true)), + Mode::Rtl, + "with position → RTL" + ); let mut f2 = FlightFsm { mode: start }; - assert_eq!(f2.on(Event::Failsafe, g(true, false, false)), Mode::Land, "no position → Land"); + assert_eq!( + f2.on(Event::Failsafe, g(true, false, false)), + Mode::Land, + "no position → Land" + ); } } } @@ -351,7 +403,9 @@ mod kani_harness { /// back to an armed/flying mode without an on-ground reset (a fresh FSM). #[kani::proof] fn verify_terminated_is_absorbing() { - let mut f = FlightFsm { mode: Mode::Terminated }; + let mut f = FlightFsm { + mode: Mode::Terminated, + }; let g = Gates { level: kani::any(), throttle_low: kani::any(), @@ -366,7 +420,10 @@ mod kani_harness { #[kani::proof] fn verify_failsafe_recovers() { let start = any_mode(); - kani::assume(matches!(start, Mode::Takeoff | Mode::Loiter | Mode::Mission | Mode::Rtl)); + kani::assume(matches!( + start, + Mode::Takeoff | Mode::Loiter | Mode::Mission | Mode::Rtl + )); let mut f = FlightFsm { mode: start }; let g = Gates { level: kani::any(), diff --git a/crates/relay-geo/plain/src/lib.rs b/crates/relay-geo/plain/src/lib.rs index 685ae859..a0060d77 100644 --- a/crates/relay-geo/plain/src/lib.rs +++ b/crates/relay-geo/plain/src/lib.rs @@ -38,7 +38,11 @@ pub const GRAVITY_NED: Vec3 = [0.0, 0.0, 9.81]; #[inline] fn cross(a: Vec3, b: Vec3) -> Vec3 { - [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]] + [ + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0], + ] } #[inline] fn dot(a: Vec3, b: Vec3) -> f32 { @@ -99,9 +103,21 @@ fn vee(m: &Mat3) -> Vec3 { pub fn quat_to_rotmat(q: [f32; 4]) -> Mat3 { let (w, x, y, z) = (q[0], q[1], q[2], q[3]); [ - [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), 2.0 * (x * z + w * y)], - [2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - w * x)], - [2.0 * (x * z - w * y), 2.0 * (y * z + w * x), 1.0 - 2.0 * (x * x + y * y)], + [ + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + ], + [ + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + ], + [ + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y), + ], ] } @@ -303,8 +319,7 @@ impl GeoAtt { /// the direct-torque yaw loop). pub fn desired_rate(&self, q_est: [f32; 4], a_cmd_ned: Vec3, yaw_d: f32) -> Vec3 { let r = quat_to_rotmat(q_est); - let r_d = thrust_axis_ned(sanitise3(a_cmd_ned)) - .and_then(|b3| desired_attitude(b3, yaw_d)); + let r_d = thrust_axis_ned(sanitise3(a_cmd_ned)).and_then(|b3| desired_attitude(b3, yaw_d)); match r_d { Some(r_d) => { let e = Self::attitude_error(&r, &r_d); @@ -323,8 +338,7 @@ impl GeoAtt { pub fn tick(&self, q_est: [f32; 4], omega_body: Vec3, a_cmd_ned: Vec3, yaw_d: f32) -> Vec3 { let r = quat_to_rotmat(q_est); let omega = sanitise3(omega_body); - let r_d = thrust_axis_ned(sanitise3(a_cmd_ned)) - .and_then(|b3| desired_attitude(b3, yaw_d)); + let r_d = thrust_axis_ned(sanitise3(a_cmd_ned)).and_then(|b3| desired_attitude(b3, yaw_d)); match r_d { Some(r_d) => sanitise3(self.moment(&r, omega, &r_d)), None => { @@ -368,20 +382,32 @@ pub struct RecoverableSet { impl RecoverableSet { pub const fn new(k_r: f32, lambda_j: f32, psi_max: f32) -> Self { - RecoverableSet { k_r, lambda_j, psi_max } + RecoverableSet { + k_r, + lambda_j, + psi_max, + } } /// Signed margin to the recoverable boundary: `≥ 0` ⇔ recoverable. /// `margin = min(ψ_max − Ψ, 2k_R(2−Ψ)/λ_M(J) − ‖e_Ω‖²)`. Returns /// `−∞` (definitely outside / fail-safe) on any non-finite input. pub fn margin(&self, psi: f32, e_omega_sq: f32) -> f32 { - if !psi.is_finite() || !e_omega_sq.is_finite() || !self.lambda_j.is_finite() || self.lambda_j <= 0.0 { + if !psi.is_finite() + || !e_omega_sq.is_finite() + || !self.lambda_j.is_finite() + || self.lambda_j <= 0.0 + { return f32::NEG_INFINITY; } let psi_margin = self.psi_max - psi; let rate_cap = 2.0 * self.k_r * (2.0 - psi) / self.lambda_j; let rate_margin = rate_cap - e_omega_sq; - let m = if psi_margin < rate_margin { psi_margin } else { rate_margin }; + let m = if psi_margin < rate_margin { + psi_margin + } else { + rate_margin + }; if m.is_finite() { m } else { f32::NEG_INFINITY } } @@ -417,9 +443,22 @@ impl SimplexShield { /// inside the set); `exit` = deeper margin at which to return to agile /// (`exit > enter`). Degenerate args clamped to a safe ordering. pub fn new(set: RecoverableSet, enter: f32, exit: f32) -> Self { - let enter = if enter.is_finite() && enter > 0.0 { enter } else { 0.1 }; - let exit = if exit.is_finite() && exit > enter { exit } else { enter * 2.0 }; - SimplexShield { set, engaged: true, enter, exit } + let enter = if enter.is_finite() && enter > 0.0 { + enter + } else { + 0.1 + }; + let exit = if exit.is_finite() && exit > enter { + exit + } else { + enter * 2.0 + }; + SimplexShield { + set, + engaged: true, + enter, + exit, + } } /// True while the certified fallback is in control. @@ -431,7 +470,13 @@ impl SimplexShield { /// Select the command: `agile` while safely inside the recoverable set, /// else the certified `fallback`. `psi`, `e_omega_sq` = current /// geometric error `(Ψ, ‖Ω‖²)`. Returns `(command, used_fallback)`. - pub fn filter(&mut self, psi: f32, e_omega_sq: f32, agile: Vec3, fallback: Vec3) -> (Vec3, bool) { + pub fn filter( + &mut self, + psi: f32, + e_omega_sq: f32, + agile: Vec3, + fallback: Vec3, + ) -> (Vec3, bool) { self.step(self.set.margin(psi, e_omega_sq), agile, fallback) } @@ -524,7 +569,11 @@ mod tests { for i in 0..3 { for j in 0..3 { let want = if i == j { 1.0 } else { 0.0 }; - assert!((r_d[i][j] - want).abs() < 1e-5, "R_d[{i}][{j}]={}", r_d[i][j]); + assert!( + (r_d[i][j] - want).abs() < 1e-5, + "R_d[{i}][{j}]={}", + r_d[i][j] + ); } } let e = GeoAtt::attitude_error(&identity3(), &r_d); @@ -550,14 +599,23 @@ mod tests { // Pure yaw error: R = I, R_d = Rz(20°). let r_d_yaw = rot_z(20f32.to_radians()); let e_yaw = GeoAtt::attitude_error(&identity3(), &r_d_yaw); - assert!(e_yaw[0].abs() < 1e-4 && e_yaw[1].abs() < 1e-4, "yaw err leaked to roll/pitch: {e_yaw:?}"); - assert!(e_yaw[2].abs() > 0.05, "yaw err should be present: {e_yaw:?}"); + assert!( + e_yaw[0].abs() < 1e-4 && e_yaw[1].abs() < 1e-4, + "yaw err leaked to roll/pitch: {e_yaw:?}" + ); + assert!( + e_yaw[2].abs() > 0.05, + "yaw err should be present: {e_yaw:?}" + ); // Pure roll error at a non-zero heading: R = Rz(90°), R_d = Rz(90°)Rx(15°). let r = rot_z(90f32.to_radians()); let r_d = matmul3(&r, &rot_x(15f32.to_radians())); let e = GeoAtt::attitude_error(&r, &r_d); - assert!(e[2].abs() < 1e-3, "tilt at heading=90° leaked into YAW (the Euler bug): {e:?}"); + assert!( + e[2].abs() < 1e-3, + "tilt at heading=90° leaked into YAW (the Euler bug): {e:?}" + ); assert!(e[0].abs() > 0.05, "roll error should be present: {e:?}"); } @@ -599,8 +657,20 @@ mod tests { fn flatness_feedforward_matches_attitude_derivative() { let yaw = 0.0f32; let dt = 1e-4f32; - let accel = |t: f32| [0.5 * relay_math::sinf(t), 0.3 * relay_math::cosf(0.7 * t), 0.2 * t]; - let jerk = |t: f32| [0.5 * relay_math::cosf(t), -0.21 * relay_math::sinf(0.7 * t), 0.2]; + let accel = |t: f32| { + [ + 0.5 * relay_math::sinf(t), + 0.3 * relay_math::cosf(0.7 * t), + 0.2 * t, + ] + }; + let jerk = |t: f32| { + [ + 0.5 * relay_math::cosf(t), + -0.21 * relay_math::sinf(0.7 * t), + 0.2, + ] + }; for k in 1..20 { let t = k as f32 * 0.15; let (a, jc) = (accel(t), jerk(t)); @@ -618,7 +688,8 @@ mod tests { assert!( (w_ff[i] - w_fd[i]).abs() < 0.05, "axis {i} at t={t}: ff {} vs fd {}", - w_ff[i], w_fd[i] + w_ff[i], + w_fd[i] ); } } @@ -629,11 +700,16 @@ mod tests { #[test] fn reduced_error_zero_at_alignment_no_yaw() { let e0 = GeoAtt::attitude_error_reduced(&identity3(), [0.0, 0.0, 1.0]); - assert!(e0[0].abs() < 1e-6 && e0[1].abs() < 1e-6 && e0[2] == 0.0, - "zero at alignment: {e0:?}"); + assert!( + e0[0].abs() < 1e-6 && e0[1].abs() < 1e-6 && e0[2] == 0.0, + "zero at alignment: {e0:?}" + ); let e1 = GeoAtt::attitude_error_reduced(&rot_x(0.3), [0.0, 0.0, 1.0]); assert_eq!(e1[2], 0.0, "no yaw component: {e1:?}"); - assert!(e1[0].abs() + e1[1].abs() > 0.1, "tilt produces error: {e1:?}"); + assert!( + e1[0].abs() + e1[1].abs() > 0.1, + "tilt produces error: {e1:?}" + ); } /// v0.26 reduced-attitude control: a tilted body under `moment_reduced` @@ -660,8 +736,14 @@ mod tests { r = integrate_rotation(&r, omega, dt); } let tilt1 = relay_math::acosf(r[2][2].clamp(-1.0, 1.0)); - assert!(tilt1 < tilt0, "thrust-axis tilt must decrease: {tilt0} -> {tilt1}"); - assert!(tilt1 < 0.05, "thrust axis should align to target: tilt={tilt1}"); + assert!( + tilt1 < tilt0, + "thrust-axis tilt must decrease: {tilt0} -> {tilt1}" + ); + assert!( + tilt1 < 0.05, + "thrust axis should align to target: tilt={tilt1}" + ); } /// Integrate R by the body rate over dt, with Gram-Schmidt @@ -683,10 +765,18 @@ mod tests { let c0 = [m[0][0], m[1][0], m[2][0]]; let c1 = [m[0][1], m[1][1], m[2][1]]; let e0 = normalize(c0).unwrap(); - let p1 = [c1[0] - dot(e0, c1) * e0[0], c1[1] - dot(e0, c1) * e0[1], c1[2] - dot(e0, c1) * e0[2]]; + let p1 = [ + c1[0] - dot(e0, c1) * e0[0], + c1[1] - dot(e0, c1) * e0[1], + c1[2] - dot(e0, c1) * e0[2], + ]; let e1 = normalize(p1).unwrap(); let e2 = cross(e0, e1); - [[e0[0], e1[0], e2[0]], [e0[1], e1[1], e2[1]], [e0[2], e1[2], e2[2]]] + [ + [e0[0], e1[0], e2[0]], + [e0[1], e1[1], e2[1]], + [e0[2], e1[2], e2[2]], + ] } /// v0.23 — RUNNABLE Lyapunov certificate (the oracle backing the Lean @@ -709,14 +799,20 @@ mod tests { let kr = 8.0f32; let kw = 2.0f32; let j = [0.0217f32, 0.0217, 0.04]; - let ctrl = GeoAtt::new(GeoGains { k_r: [kr; 3], k_omega: [kw; 3], j }); + let ctrl = GeoAtt::new(GeoGains { + k_r: [kr; 3], + k_omega: [kw; 3], + j, + }); let r_d = identity3(); let dt = 1e-4f32; let angles = [0.2f32, 0.8, 1.5, 2.4, 2.9]; // up to ~166° (Ψ<2) let omegas = [ - [1.0f32, 0.0, 0.0], [0.0, 2.0, -1.0], - [3.0, -2.0, 4.0], [-5.0, 1.0, 2.0], + [1.0f32, 0.0, 0.0], + [0.0, 2.0, -1.0], + [3.0, -2.0, 4.0], + [-5.0, 1.0, 2.0], ]; let mut checked = 0; for &ax in &angles { @@ -728,30 +824,36 @@ mod tests { for &omega in &omegas { // FACT 1: M − Ω×JΩ == −k_R e_R − k_Ω Ω (exact, f32). let m = ctrl.moment(&r, omega, &r_d); - let jo = [j[0]*omega[0], j[1]*omega[1], j[2]*omega[2]]; + let jo = [j[0] * omega[0], j[1] * omega[1], j[2] * omega[2]]; let j_omega_dot = { let g = cross(omega, jo); - [m[0]-g[0], m[1]-g[1], m[2]-g[2]] + [m[0] - g[0], m[1] - g[1], m[2] - g[2]] }; for i in 0..3 { let want = -kr * e_r[i] - kw * omega[i]; - assert!((j_omega_dot[i] - want).abs() < 1e-3, - "FACT1 axis {i}: JΩ̇={} vs {want}", j_omega_dot[i]); + assert!( + (j_omega_dot[i] - want).abs() < 1e-3, + "FACT1 axis {i}: JΩ̇={} vs {want}", + j_omega_dot[i] + ); } // FACT 2: Ψ̇ == ½ e_R·Ω (finite-diff of real psi). let r1 = integrate_rotation(&r, omega, dt); let psi_dot = (GeoAtt::psi(&r1, &r_d) - psi) / dt; let half_e_dot_w = 0.5 * dot(e_r, omega); let tol2 = 0.02 + 0.03 * half_e_dot_w.abs(); - assert!((psi_dot - half_e_dot_w).abs() <= tol2, - "FACT2: Ψ̇={psi_dot} vs ½e_R·Ω={half_e_dot_w}; Ψ={psi}"); + assert!( + (psi_dot - half_e_dot_w).abs() <= tol2, + "FACT2: Ψ̇={psi_dot} vs ½e_R·Ω={half_e_dot_w}; Ψ={psi}" + ); // Assembled V̇ from the REAL moment + e_R. let vdot = dot(omega, j_omega_dot) + kr * dot(e_r, omega); let expected = -kw * dot(omega, omega); - assert!(vdot <= 0.0, - "V̇ must be ≤ 0: {vdot} at Ψ={psi}, ω={omega:?}"); - assert!((vdot - expected).abs() < 1e-2, - "V̇ ({vdot}) must equal −k_Ω‖Ω‖² ({expected}); Ψ={psi}"); + assert!(vdot <= 0.0, "V̇ must be ≤ 0: {vdot} at Ψ={psi}, ω={omega:?}"); + assert!( + (vdot - expected).abs() < 1e-2, + "V̇ ({vdot}) must equal −k_Ω‖Ω‖² ({expected}); Ψ={psi}" + ); checked += 1; } } @@ -775,15 +877,21 @@ mod tests { let kr = 8.0f32; let kw = 2.0f32; let j = [0.0217f32, 0.0217, 0.04]; - let ctrl = GeoAtt::new(GeoGains { k_r: [kr; 3], k_omega: [kw; 3], j }); + let ctrl = GeoAtt::new(GeoGains { + k_r: [kr; 3], + k_omega: [kw; 3], + j, + }); let r_d = identity3(); let dt = 1e-5f32; let c = 0.02f32; // cross-term coupling (small ⇒ V stays PD, V̇ ND) let angles = [0.2f32, 0.8, 1.5, 2.4, 2.9]; let omegas = [ - [1.0f32, 0.0, 0.0], [0.0, 2.0, -1.0], - [3.0, -2.0, 4.0], [-5.0, 1.0, 2.0], + [1.0f32, 0.0, 0.0], + [0.0, 2.0, -1.0], + [3.0, -2.0, 4.0], + [-5.0, 1.0, 2.0], ]; let (mut c_lo, mut c_hi, mut c_d) = (f32::INFINITY, 0.0f32, f32::INFINITY); let mut checked = 0; @@ -795,21 +903,32 @@ mod tests { let e_r = GeoAtt::attitude_error(&r, &r_d); for &omega in &omegas { let m = ctrl.moment(&r, omega, &r_d); - let jo = [j[0]*omega[0], j[1]*omega[1], j[2]*omega[2]]; - let jwd = { let g = cross(omega, jo); [m[0]-g[0], m[1]-g[1], m[2]-g[2]] }; - let omega_dot = [jwd[0]/j[0], jwd[1]/j[1], jwd[2]/j[2]]; // Ω̇ + let jo = [j[0] * omega[0], j[1] * omega[1], j[2] * omega[2]]; + let jwd = { + let g = cross(omega, jo); + [m[0] - g[0], m[1] - g[1], m[2] - g[2]] + }; + let omega_dot = [jwd[0] / j[0], jwd[1] / j[1], jwd[2] / j[2]]; // Ω̇ // central-difference ė_R of the real attitude_error let rp = integrate_rotation(&r, omega, dt); let rm = integrate_rotation(&r, omega, -dt); let ep = GeoAtt::attitude_error(&rp, &r_d); let em = GeoAtt::attitude_error(&rm, &r_d); - let e_r_dot = [(ep[0]-em[0])/(2.0*dt), (ep[1]-em[1])/(2.0*dt), (ep[2]-em[2])/(2.0*dt)]; + let e_r_dot = [ + (ep[0] - em[0]) / (2.0 * dt), + (ep[1] - em[1]) / (2.0 * dt), + (ep[2] - em[2]) / (2.0 * dt), + ]; let vdot_base = dot(omega, jwd) + kr * dot(e_r, omega); // = −k_Ω‖Ω‖² let cross_dot = dot(e_r_dot, omega) + dot(e_r, omega_dot); let vdot = vdot_base + c * cross_dot; - let v = 0.5*(j[0]*omega[0]*omega[0] + j[1]*omega[1]*omega[1] + j[2]*omega[2]*omega[2]) - + 2.0*kr*psi + c*dot(e_r, omega); + let v = 0.5 + * (j[0] * omega[0] * omega[0] + + j[1] * omega[1] * omega[1] + + j[2] * omega[2] * omega[2]) + + 2.0 * kr * psi + + c * dot(e_r, omega); let rs = dot(e_r, e_r) + dot(omega, omega); // V is positive-DEFINITE (the cross term does not spoil it). @@ -817,11 +936,17 @@ mod tests { // V̇ is negative-DEFINITE — strictly dissipative everywhere // (the semidefinite base gives V̇=0 at Ω=0; the cross term // makes it < 0 even there). Small tol for finite-diff noise. - assert!(vdot < 1e-2, "V̇ must be < 0 (strict): {vdot} at Ψ={psi}, ω={omega:?}"); + assert!( + vdot < 1e-2, + "V̇ must be < 0 (strict): {vdot} at Ψ={psi}, ω={omega:?}" + ); // Exponential-decay inequality on the REAL controller: // V̇ ≤ −γ·V with γ = 0.05 (below the measured c_D/c_hi). - assert!(vdot <= -0.05 * v + 1e-2, - "V̇ ({vdot}) must be ≤ −0.05·V ({}) at Ψ={psi}", -0.05 * v); + assert!( + vdot <= -0.05 * v + 1e-2, + "V̇ ({vdot}) must be ≤ −0.05·V ({}) at Ψ={psi}", + -0.05 * v + ); if rs > 0.5 { c_lo = c_lo.min(v / rs); c_hi = c_hi.max(v / rs); @@ -837,8 +962,15 @@ mod tests { // c_hi≈31.9, c_D≈1.90 ⇒ conservative rate γ = c_D/c_hi ≈ 0.056.) assert!(c_lo > 0.025, "V positive-definite floor too small: {c_lo}"); assert!(c_hi < 40.0, "V radial cap unexpectedly large: {c_hi}"); - assert!(c_d > 1.5, "−V̇ dissipation floor must be strictly positive: {c_d}"); - assert!(c_d / c_hi > 0.05, "exponential rate γ must exceed 0.05: {}", c_d / c_hi); + assert!( + c_d > 1.5, + "−V̇ dissipation floor must be strictly positive: {c_d}" + ); + assert!( + c_d / c_hi > 0.05, + "exponential rate γ must exceed 0.05: {}", + c_d / c_hi + ); assert!(checked >= 80, "grid too small: {checked}"); } @@ -874,14 +1006,20 @@ mod tests { let ev_sq = e_v[0] * e_v[0] + e_v[1] * e_v[1] + e_v[2] * e_v[2]; let expected = -kv * ev_sq; // cross-terms cancel ⇒ V̇_pos == −k_v‖e_v‖² ≤ 0 - assert!((vdot - expected).abs() < 1e-3, "V̇_pos {vdot} ≠ −k_v‖e_v‖² {expected}"); + assert!( + (vdot - expected).abs() < 1e-3, + "V̇_pos {vdot} ≠ −k_v‖e_v‖² {expected}" + ); assert!(vdot <= 1e-4, "V̇_pos must be ≤ 0, got {vdot}"); // combined full-state with a sample attitude term let omega = [1.0f32, -2.0, 0.5]; let kw = 2.0f32; let w_sq = omega[0] * omega[0] + omega[1] * omega[1] + omega[2] * omega[2]; let vdot_full = -kw * w_sq + vdot; - assert!(vdot_full <= 1e-4, "full-state V̇ must be ≤ 0, got {vdot_full}"); + assert!( + vdot_full <= 1e-4, + "full-state V̇ must be ≤ 0, got {vdot_full}" + ); checked += 1; } } @@ -894,7 +1032,10 @@ mod tests { #[test] fn recoverable_set_matches_lyapunov_region() { let set = RecoverableSet::new(8.0, 0.04, 1.8); // k_R, λ_M(J), Ψ_max - assert!(set.recoverable(0.1, 1.0), "near-level low-rate is recoverable"); + assert!( + set.recoverable(0.1, 1.0), + "near-level low-rate is recoverable" + ); assert!(!set.recoverable(1.9, 0.0), "Ψ past the ceiling is not"); // Rate cap at Ψ=0: 2·8·2/0.04 = 800; just over ⇒ outside. assert!(set.recoverable(0.0, 700.0)); diff --git a/crates/relay-hk/plain/src/engine.rs b/crates/relay-hk/plain/src/engine.rs index f01a42f2..eee7d0e8 100644 --- a/crates/relay-hk/plain/src/engine.rs +++ b/crates/relay-hk/plain/src/engine.rs @@ -33,19 +33,31 @@ pub struct HkPacket { impl CopyEntry { pub const fn empty() -> Self { - CopyEntry { source_id: 0, source_offset: 0, length: 0, output_offset: 0 } + CopyEntry { + source_id: 0, + source_offset: 0, + length: 0, + output_offset: 0, + } } } impl SourceData { pub const fn empty() -> Self { - SourceData { source_id: 0, data: [0u8; SOURCE_DATA_SIZE] } + SourceData { + source_id: 0, + data: [0u8; SOURCE_DATA_SIZE], + } } } impl HkPacket { pub fn new() -> Self { - HkPacket { data: [0u8; MAX_OUTPUT_SIZE], length: 0, sequence: 0 } + HkPacket { + data: [0u8; MAX_OUTPUT_SIZE], + length: 0, + sequence: 0, + } } } @@ -58,13 +70,17 @@ impl CopyTable { } pub fn add_entry(&mut self, entry: CopyEntry) -> bool { - if self.entry_count as usize >= MAX_COPY_ENTRIES { return false; } + if self.entry_count as usize >= MAX_COPY_ENTRIES { + return false; + } self.entries[self.entry_count as usize] = entry; self.entry_count = self.entry_count + 1; true } - pub fn entry_count(&self) -> u32 { self.entry_count } + pub fn entry_count(&self) -> u32 { + self.entry_count + } pub fn collect(&self, sources: &[SourceData], packet: &mut HkPacket) -> bool { let count = self.entry_count; @@ -74,11 +90,15 @@ impl CopyTable { // Bounds check: output region must fit in packet let out_end = entry.output_offset as usize + entry.length as usize; - if out_end > MAX_OUTPUT_SIZE { return false; } + if out_end > MAX_OUTPUT_SIZE { + return false; + } // Bounds check: source region must fit in source data let src_end = entry.source_offset as usize + entry.length as usize; - if src_end > SOURCE_DATA_SIZE { return false; } + if src_end > SOURCE_DATA_SIZE { + return false; + } // Find matching source let mut found = false; @@ -98,7 +118,9 @@ impl CopyTable { s = s + 1; } - if !found { return false; } + if !found { + return false; + } // Track the high-water mark for packet length if out_end as u32 > packet.length { @@ -155,8 +177,18 @@ mod tests { #[test] fn test_multiple_copies() { let mut table = CopyTable::new(); - table.add_entry(CopyEntry { source_id: 1, source_offset: 0, length: 2, output_offset: 0 }); - table.add_entry(CopyEntry { source_id: 2, source_offset: 4, length: 2, output_offset: 2 }); + table.add_entry(CopyEntry { + source_id: 1, + source_offset: 0, + length: 2, + output_offset: 0, + }); + table.add_entry(CopyEntry { + source_id: 2, + source_offset: 4, + length: 2, + output_offset: 2, + }); let mut src1 = SourceData::empty(); src1.source_id = 1; diff --git a/crates/relay-hs/plain/src/engine.rs b/crates/relay-hs/plain/src/engine.rs index a55dff6d..c61756bc 100644 --- a/crates/relay-hs/plain/src/engine.rs +++ b/crates/relay-hs/plain/src/engine.rs @@ -7,7 +7,12 @@ pub const MAX_ALERTS_PER_CHECK: usize = 8; #[derive(Clone, Copy, PartialEq, Eq)] #[repr(u8)] -pub enum HsAction { NoAction = 0, Event = 1, RestartApp = 2, ProcessorReset = 3 } +pub enum HsAction { + NoAction = 0, + Event = 1, + RestartApp = 2, + ProcessorReset = 3, +} #[derive(Clone, Copy)] pub struct AppMonitor { @@ -54,7 +59,12 @@ impl AppMonitor { impl HsAlert { pub const fn empty() -> Self { - HsAlert { app_id: 0, action: HsAction::NoAction, miss_count: 0, time: 0 } + HsAlert { + app_id: 0, + action: HsAction::NoAction, + miss_count: 0, + time: 0, + } } } @@ -67,7 +77,9 @@ impl HealthTable { } pub fn register_app(&mut self, app_id: u32, max_miss: u32, action: HsAction) -> bool { - if self.app_count as usize >= MAX_APPS { return false; } + if self.app_count as usize >= MAX_APPS { + return false; + } let idx = self.app_count as usize; self.apps[idx] = AppMonitor { app_id, @@ -94,7 +106,9 @@ impl HealthTable { } } - pub fn app_count(&self) -> u32 { self.app_count } + pub fn app_count(&self) -> u32 { + self.app_count + } pub fn check_health(&mut self, time: u64) -> HsResult { let mut result = HsResult { @@ -105,14 +119,20 @@ impl HealthTable { let count = self.app_count; let mut i: u32 = 0; while i < count { - if result.alert_count as usize >= MAX_ALERTS_PER_CHECK { break; } + if result.alert_count as usize >= MAX_ALERTS_PER_CHECK { + break; + } let idx = i as usize; let app = self.apps[idx]; if app.enabled { if app.last_count == app.expected_count { // Counter hasn't changed — increment miss - let new_miss = if app.current_miss < u32::MAX { app.current_miss + 1 } else { u32::MAX }; + let new_miss = if app.current_miss < u32::MAX { + app.current_miss + 1 + } else { + u32::MAX + }; self.apps[idx].current_miss = new_miss; if new_miss >= app.max_miss { let aidx = result.alert_count as usize; @@ -182,8 +202,7 @@ impl EkfHealthMonitor { if self.rtl_latched { return false; } - let (new_hist, over_count) = - Self::step_window(self.history, self.window, over_limit); + let (new_hist, over_count) = Self::step_window(self.history, self.window, over_limit); self.history = new_hist; if over_count >= self.trip_threshold { self.rtl_latched = true; diff --git a/crates/relay-iekf/plain/src/lib.rs b/crates/relay-iekf/plain/src/lib.rs index f50281b9..c63b2d2f 100644 --- a/crates/relay-iekf/plain/src/lib.rs +++ b/crates/relay-iekf/plain/src/lib.rs @@ -133,7 +133,11 @@ fn sanitise3(v: Vec3) -> Vec3 { /// `1 + α·‖·‖²` arithmetic so the bound is a comparison-only Kani proof. #[inline] fn clamp_factor(x: f32, max: f32) -> f32 { - let max = if max.is_finite() && max >= 1.0 { max } else { 1.0 }; + let max = if max.is_finite() && max >= 1.0 { + max + } else { + 1.0 + }; if !x.is_finite() || x < 1.0 { 1.0 } else if x > max { @@ -149,9 +153,21 @@ fn clamp_factor(x: f32, max: f32) -> f32 { fn quat_to_rotmat(q: Quat) -> [[f32; 3]; 3] { let (w, x, y, z) = (q[0], q[1], q[2], q[3]); [ - [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), 2.0 * (x * z + w * y)], - [2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - w * x)], - [2.0 * (x * z - w * y), 2.0 * (y * z + w * x), 1.0 - 2.0 * (x * x + y * y)], + [ + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + ], + [ + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + ], + [ + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y), + ], ] } @@ -423,15 +439,25 @@ impl Iekf { pub fn with_config(state: NavState, cfg: IekfConfig) -> Self { let mut p = mat_zero(); let v = [ - cfg.p0[0] * cfg.p0[0], cfg.p0[1] * cfg.p0[1], cfg.p0[2] * cfg.p0[2], - cfg.p0[3] * cfg.p0[3], cfg.p0[4] * cfg.p0[4], + cfg.p0[0] * cfg.p0[0], + cfg.p0[1] * cfg.p0[1], + cfg.p0[2] * cfg.p0[2], + cfg.p0[3] * cfg.p0[3], + cfg.p0[4] * cfg.p0[4], ]; for blk in 0..5 { for i in 0..3 { p[blk * 3 + i][blk * 3 + i] = v[blk]; } } - Iekf { state, p, cfg, q_vel_extra: 0.0, q_pos_extra: 0.0, variance_floor_hits: 0 } + Iekf { + state, + p, + cfg, + q_vel_extra: 0.0, + q_pos_extra: 0.0, + variance_floor_hits: 0, + } } pub fn level() -> Self { @@ -482,13 +508,21 @@ impl Iekf { /// p⁺ = p + v dt + ½ (R·a + g) dt² /// ``` pub fn propagate(&mut self, imu: Imu, dt: f32) { - let dt = if dt.is_finite() { dt.clamp(1e-4, 0.1) } else { 1e-3 }; + let dt = if dt.is_finite() { + dt.clamp(1e-4, 0.1) + } else { + 1e-3 + }; let s = &mut self.state; let gyro = sanitise3(imu.gyro); let accel = sanitise3(imu.accel); let omega = [gyro[0] - s.b_g[0], gyro[1] - s.b_g[1], gyro[2] - s.b_g[2]]; - let acc_b = [accel[0] - s.b_a[0], accel[1] - s.b_a[1], accel[2] - s.b_a[2]]; + let acc_b = [ + accel[0] - s.b_a[0], + accel[1] - s.b_a[1], + accel[2] - s.b_a[2], + ]; // Specific force rotated into NED, plus gravity → inertial accel. let acc_n_body = q_rotate(s.q, acc_b); @@ -503,7 +537,10 @@ impl Iekf { let r_hat = quat_to_rotmat(s.q); // Attitude: right-multiply by the body-frame incremental rotation. - s.q = q_normalize(q_mul(s.q, so3_exp([omega[0] * dt, omega[1] * dt, omega[2] * dt]))); + s.q = q_normalize(q_mul( + s.q, + so3_exp([omega[0] * dt, omega[1] * dt, omega[2] * dt]), + )); // Position uses the pre-update velocity (semi-implicit is a later // refinement); velocity then integrates the inertial accel. @@ -1010,11 +1047,7 @@ fn nis3(r: Vec3, s_inv: &[[f32; 3]; 3]) -> f32 { } acc += r[i] * row; } - if acc.is_finite() { - acc - } else { - f32::INFINITY - } + if acc.is_finite() { acc } else { f32::INFINITY } } fn nees_block(p: &Mat, base: usize, e: Vec3) -> f32 { @@ -1043,11 +1076,7 @@ fn nees_block(p: &Mat, base: usize, e: Vec3) -> f32 { } None => { let e2 = e[0] * e[0] + e[1] * e[1] + e[2] * e[2]; - if e2 < 1e-20 { - 0.0 - } else { - f32::INFINITY - } + if e2 < 1e-20 { 0.0 } else { f32::INFINITY } } } } @@ -1111,11 +1140,7 @@ pub fn mag_heading(mag_body: Vec3, q: Quat, declination: f32) -> Option { 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), ); let yaw = yaw_est + (declination - field_ang); - if yaw.is_finite() { - Some(yaw) - } else { - None - } + if yaw.is_finite() { Some(yaw) } else { None } } /// Persistence-of-excitation gate for **magless yaw observability** @@ -1144,16 +1169,32 @@ impl YawObservability { pub fn new(tau: f32, threshold: f32) -> Self { YawObservability { excitation: 0.0, - tau: if tau.is_finite() && tau > 1e-3 { tau } else { 1.0 }, - threshold: if threshold.is_finite() && threshold > 0.0 { threshold } else { 1.0 }, + tau: if tau.is_finite() && tau > 1e-3 { + tau + } else { + 1.0 + }, + threshold: if threshold.is_finite() && threshold > 0.0 { + threshold + } else { + 1.0 + }, } } /// Feed the horizontal specific-force magnitude `a_horiz` (m/s²) over /// `dt` (s); returns the current observability verdict. pub fn update(&mut self, a_horiz: f32, dt: f32) -> bool { - let a = if a_horiz.is_finite() { a_horiz.abs() } else { 0.0 }; - let dt = if dt.is_finite() { dt.clamp(0.0, 0.1) } else { 0.0 }; + let a = if a_horiz.is_finite() { + a_horiz.abs() + } else { + 0.0 + }; + let dt = if dt.is_finite() { + dt.clamp(0.0, 0.1) + } else { + 0.0 + }; let alpha = (dt / self.tau).clamp(0.0, 1.0); self.excitation += alpha * (a - self.excitation); if !self.excitation.is_finite() { @@ -1200,8 +1241,16 @@ impl RotorFaultDetector { pub fn new(threshold: f32, drift: f32) -> Self { RotorFaultDetector { cusum: [0.0; 4], - threshold: if threshold.is_finite() && threshold > 0.0 { threshold } else { 1.0 }, - drift: if drift.is_finite() && drift >= 0.0 { drift } else { 0.0 }, + threshold: if threshold.is_finite() && threshold > 0.0 { + threshold + } else { + 1.0 + }, + drift: if drift.is_finite() && drift >= 0.0 { + drift + } else { + 0.0 + }, failed: None, } } @@ -1213,7 +1262,11 @@ impl RotorFaultDetector { return self.failed; } for i in 0..4 { - let r = if residual[i].is_finite() { residual[i].abs() } else { 0.0 }; + let r = if residual[i].is_finite() { + residual[i].abs() + } else { + 0.0 + }; let s = self.cusum[i] + r - self.drift; self.cusum[i] = if s.is_finite() && s > 0.0 { s } else { 0.0 }; if self.cusum[i] >= self.threshold { @@ -1256,8 +1309,16 @@ impl SpoofMonitor { SpoofMonitor { g_hi: [0.0; 3], g_lo: [0.0; 3], - threshold: if threshold.is_finite() && threshold > 0.0 { threshold } else { 1.0 }, - drift: if drift.is_finite() && drift >= 0.0 { drift } else { 0.0 }, + threshold: if threshold.is_finite() && threshold > 0.0 { + threshold + } else { + 1.0 + }, + drift: if drift.is_finite() && drift >= 0.0 { + drift + } else { + 0.0 + }, spoofed: false, } } @@ -1269,7 +1330,11 @@ impl SpoofMonitor { return true; } for i in 0..3 { - let r = if innovation[i].is_finite() { innovation[i] } else { 0.0 }; + let r = if innovation[i].is_finite() { + innovation[i] + } else { + 0.0 + }; let hi = self.g_hi[i] + r - self.drift; self.g_hi[i] = if hi.is_finite() && hi > 0.0 { hi } else { 0.0 }; let lo = self.g_lo[i] - r - self.drift; @@ -1304,7 +1369,10 @@ mod tests { #[test] fn level_at_rest_holds_still() { let mut f = Iekf::level(); - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..1000 { f.propagate(imu, 0.01); } @@ -1320,15 +1388,25 @@ mod tests { fn pure_yaw_rate_integrates_to_yaw_no_tilt() { let mut f = Iekf::level(); // 1 rad/s yaw for 1 s → ~57.3° yaw, zero tilt. - let imu = Imu { gyro: [0.0, 0.0, 1.0], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0, 0.0, 1.0], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..100 { f.propagate(imu, 0.01); } let s = f.state(); - assert!(s.tilt_rad().to_degrees() < 0.5, "yaw should not tilt: {}", s.tilt_rad()); + assert!( + s.tilt_rad().to_degrees() < 0.5, + "yaw should not tilt: {}", + s.tilt_rad() + ); // yaw ≈ atan2(2(wz+xy), 1−2(y²+z²)) ≈ 1 rad let q = s.q; - let yaw = relay_math::atan2f(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])); + let yaw = relay_math::atan2f( + 2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), + ); assert!((yaw - 1.0).abs() < 0.05, "yaw {yaw}"); } @@ -1339,7 +1417,10 @@ mod tests { fn forward_accel_moves_north() { let mut f = Iekf::level(); // Body level; accel reads gravity reaction (−g down) + 1 m/s² north. - let imu = Imu { gyro: [0.0; 3], accel: [1.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [1.0, 0.0, -9.81], + }; for _ in 0..100 { f.propagate(imu, 0.01); } @@ -1353,7 +1434,10 @@ mod tests { #[test] fn position_update_pulls_toward_measurement() { let mut f = Iekf::level(); - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..50 { f.propagate(imu, 0.01); } @@ -1361,7 +1445,10 @@ mod tests { assert!(f.update_position([5.0, 0.0, 0.0], 0.01)); let s = f.state(); assert!(s.p[0] > 0.5, "estimate should move north: {:?}", s.p); - assert!(f.covariance()[6][6] < var_before, "north-pos variance should shrink"); + assert!( + f.covariance()[6][6] < var_before, + "north-pos variance should shrink" + ); } /// Fed the gravity-reaction accel + repeated (noiseless) position @@ -1370,7 +1457,10 @@ mod tests { #[test] fn converges_to_true_static_position() { let mut f = Iekf::level(); - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; let truth = [3.0, -2.0, -10.0]; for _ in 0..800 { f.propagate(imu, 0.01); @@ -1378,9 +1468,18 @@ mod tests { } let s = f.state(); for i in 0..3 { - assert!((s.p[i] - truth[i]).abs() < 0.3, "p[{i}] = {} vs {}", s.p[i], truth[i]); + assert!( + (s.p[i] - truth[i]).abs() < 0.3, + "p[{i}] = {} vs {}", + s.p[i], + truth[i] + ); } - assert!(s.tilt_rad().to_degrees() < 5.0, "stays roughly level: {}", s.tilt_rad().to_degrees()); + assert!( + s.tilt_rad().to_degrees() < 5.0, + "stays roughly level: {}", + s.tilt_rad().to_degrees() + ); } /// Heading update observes yaw: drive the estimate to ~1 rad of yaw, @@ -1390,21 +1489,33 @@ mod tests { #[test] fn yaw_update_corrects_heading() { let mut f = Iekf::level(); - let spin = Imu { gyro: [0.0, 0.0, 1.0], accel: [0.0, 0.0, -9.81] }; + let spin = Imu { + gyro: [0.0, 0.0, 1.0], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..100 { f.propagate(spin, 0.01); // yaw → ~1 rad } let q0 = f.state().q; - let yaw0 = relay_math::atan2f(2.0 * (q0[0] * q0[3] + q0[1] * q0[2]), 1.0 - 2.0 * (q0[2] * q0[2] + q0[3] * q0[3])); + let yaw0 = relay_math::atan2f( + 2.0 * (q0[0] * q0[3] + q0[1] * q0[2]), + 1.0 - 2.0 * (q0[2] * q0[2] + q0[3] * q0[3]), + ); assert!(yaw0 > 0.5, "setup: estimate should be yawed, got {yaw0}"); - let still = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let still = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..300 { f.propagate(still, 0.01); f.update_yaw(0.0, 0.02); } let q = f.state().q; - let yaw = relay_math::atan2f(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])); + let yaw = relay_math::atan2f( + 2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), + ); assert!(yaw.abs() < 0.1, "heading should converge to 0, got {yaw}"); } @@ -1415,20 +1526,29 @@ mod tests { #[test] fn gravity_update_corrects_tilt() { let mut f = Iekf::level(); - let roll = Imu { gyro: [0.5, 0.0, 0.0], accel: [0.0, 0.0, -9.81] }; + let roll = Imu { + gyro: [0.5, 0.0, 0.0], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..40 { f.propagate(roll, 0.01); // ~0.2 rad ≈ 11° roll } let tilt0 = f.state().tilt_rad().to_degrees(); assert!(tilt0 > 5.0, "setup: estimate should be tilted, got {tilt0}"); - let still = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let still = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..400 { f.propagate(still, 0.01); f.update_gravity([0.0, 0.0, -9.81], 0.5); // body level per accel } let tilt1 = f.state().tilt_rad().to_degrees(); - assert!(tilt1 < 2.0, "gravity update should correct tilt, got {tilt1}"); + assert!( + tilt1 < 2.0, + "gravity update should correct tilt, got {tilt1}" + ); } proptest::proptest! { @@ -1473,7 +1593,10 @@ mod tests { fn covariance_grows_without_measurements() { let mut f = Iekf::level(); let tr0: f32 = (0..N).map(|i| f.covariance()[i][i]).sum(); - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..200 { f.propagate(imu, 0.01); } @@ -1507,12 +1630,18 @@ mod tests { let before = f.nees_position(truth); // Confident measurement AT the current estimate (0) shrinks P_pos // without moving the estimate much off `truth`. - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; f.propagate(imu, 0.01); f.update_position([0.0, 0.0, 0.0], 1e-3); // very tight ⇒ P_pos ↓ let after = f.nees_position(truth); assert!(f.covariance()[6][6] < 1.0, "P_pos should shrink"); - assert!(after > before, "tighter P ⇒ larger NEES for same error: {before} -> {after}"); + assert!( + after > before, + "tighter P ⇒ larger NEES for same error: {before} -> {after}" + ); assert!(after.is_finite()); } @@ -1531,10 +1660,18 @@ mod tests { let h0 = mag_heading(m_world, [1.0, 0.0, 0.0, 0.0], 0.0).unwrap(); assert!(h0.abs() < 1e-3, "yaw 0 expected, got {h0}"); // Body yawed +90° (east): q = Rz(π/2); the body reads R(−π/2)·m_world. - let qz90 = [relay_math::cosf(pi / 4.0), 0.0, 0.0, relay_math::sinf(pi / 4.0)]; + let qz90 = [ + relay_math::cosf(pi / 4.0), + 0.0, + 0.0, + relay_math::sinf(pi / 4.0), + ]; let mb = q_rotate([qz90[0], -qz90[1], -qz90[2], -qz90[3]], m_world); // R(q)⁻¹·m_world let h90 = mag_heading(mb, qz90, 0.0).unwrap(); - assert!((h90 - pi / 2.0).abs() < 1e-2, "yaw +90° expected, got {h90}"); + assert!( + (h90 - pi / 2.0).abs() < 1e-2, + "yaw +90° expected, got {h90}" + ); // Declination: field points at d=0.3 rad east of true north, body at // true heading 0 → must still report 0 (declination removed). let d = 0.3_f32; @@ -1553,22 +1690,37 @@ mod tests { let pi = core::f32::consts::PI; let mut f = Iekf::level(); // Drive the estimate to ~1 rad yaw error. - let spin = Imu { gyro: [0.0, 0.0, 1.0], accel: [0.0, 0.0, -9.81] }; + let spin = Imu { + gyro: [0.0, 0.0, 1.0], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..100 { f.propagate(spin, 0.01); } - assert!(f.state().q[3].abs() > 0.2, "setup: estimate should be yawed"); + assert!( + f.state().q[3].abs() > 0.2, + "setup: estimate should be yawed" + ); // TRUE heading is 0; a magnetometer on the true-level-north body // reads the world field directly (m_world in body frame == NED). let m_world = [0.6_f32, 0.0, 0.8]; - let still = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let still = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for _ in 0..400 { f.propagate(still, 0.01); assert!(f.update_magnetometer(m_world, 0.0, 0.02)); } let q = f.state().q; - let yaw = relay_math::atan2f(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])); - assert!(yaw.abs() < 0.1, "mag update should drive heading to 0, got {yaw}"); + let yaw = relay_math::atan2f( + 2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), + ); + assert!( + yaw.abs() < 0.1, + "mag update should drive heading to 0, got {yaw}" + ); let _ = pi; } @@ -1600,7 +1752,13 @@ mod tests { // |a_horiz| high (observable) without runaway velocity. let a_north = if (k / 50) % 2 == 0 { a_mag } else { -a_mag }; let sf_body = [a_north, 0.0, -9.81]; // specific force, body=NED yaw 0 - f.propagate(Imu { gyro: [0.0; 3], accel: sf_body }, dt); + f.propagate( + Imu { + gyro: [0.0; 3], + accel: sf_body, + }, + dt, + ); v_true[0] += a_north * dt; p_true[0] += v_true[0] * dt; if k % 2 == 0 { @@ -1608,14 +1766,20 @@ mod tests { } } let q = f.state().q; - let yaw = relay_math::atan2f(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])); + let yaw = relay_math::atan2f( + 2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), + ); // Magless recovery from position aiding is genuinely WEAK and slow // (why PX4 uses a velocity-GSF and we use a magnetometer as primary // — docs/research/v0.22-heading-yaw-sota.md). The provable, testable // claim is that the −[f]× coupling makes yaw move CORRECTLY toward // truth under excitation (vs the [g]×-only form, which left it dead // at 0.4→0.399). Assert clear correct-direction recovery. - assert!(yaw < yaw0 - 0.03 && yaw > -0.1, "magless yaw should recover toward 0, {yaw0}→{yaw}"); + assert!( + yaw < yaw0 - 0.03 && yaw > -0.1, + "magless yaw should recover toward 0, {yaw0}→{yaw}" + ); } /// v0.30 acceleration-compensated tilt: a LEVEL body accelerating north @@ -1640,8 +1804,14 @@ mod tests { } let tilt_comp = f_co.state().tilt_rad().to_degrees(); - assert!(tilt_uncomp > 5.0, "uncompensated should tilt toward the accel: {tilt_uncomp}°"); - assert!(tilt_comp < 1.0, "compensated should stay level: {tilt_comp}°"); + assert!( + tilt_uncomp > 5.0, + "uncompensated should tilt toward the accel: {tilt_uncomp}°" + ); + assert!( + tilt_comp < 1.0, + "compensated should stay level: {tilt_comp}°" + ); } /// Rotor-FDI contract (v0.26): (1) NO FALSE ALARM while residuals stay @@ -1653,7 +1823,10 @@ mod tests { // over a long run. let mut fdi = RotorFaultDetector::new(2.0, 0.5); for _ in 0..1000 { - assert!(fdi.update([0.4, 0.5, 0.3, 0.45]).is_none(), "false alarm below slack"); + assert!( + fdi.update([0.4, 0.5, 0.3, 0.45]).is_none(), + "false alarm below slack" + ); } assert!(fdi.failed().is_none()); @@ -1692,7 +1865,11 @@ mod tests { break; } } - assert_eq!(fired, Some(1), "sustained excess on rotor 1 should isolate it"); + assert_eq!( + fired, + Some(1), + "sustained excess on rotor 1 should isolate it" + ); } /// Magless yaw observability gate: at rest (no horizontal accel) yaw is @@ -1719,7 +1896,11 @@ mod tests { for _ in 0..400 { g.update(0.0, 0.01); } - assert!(!g.is_observable(), "should decay to unobservable at hover, exc={}", g.excitation()); + assert!( + !g.is_observable(), + "should decay to unobservable at hover, exc={}", + g.excitation() + ); } /// Total: NEES is finite (or a clean INFINITY sentinel) for any inputs, @@ -1727,7 +1908,10 @@ mod tests { #[test] fn nees_is_total() { let mut f = Iekf::level(); - let imu = Imu { gyro: [0.1, -0.2, 0.3], accel: [0.5, -0.5, -9.0] }; + let imu = Imu { + gyro: [0.1, -0.2, 0.3], + accel: [0.5, -0.5, -9.0], + }; for _ in 0..50 { f.propagate(imu, 0.01); } @@ -1756,15 +1940,29 @@ mod tests { for k in 0..4000 { let t = k as f32 * dt; // truth horizontal accel (no vertical motion → level attitude) - let a_true = [2.5 * relay_math::sinf(0.8 * t), 2.0 * relay_math::cosf(0.6 * t), 0.0]; + let a_true = [ + 2.5 * relay_math::sinf(0.8 * t), + 2.0 * relay_math::cosf(0.6 * t), + 0.0, + ]; for i in 0..3 { tp[i] += tv[i] * dt + 0.5 * a_true[i] * dt * dt; tv[i] += a_true[i] * dt; } // IMU specific force (level, body=NED): horizontal scaled by the // unmodeled error, vertical = the constant gravity reaction. - let accel = [(1.0 + scale) * a_true[0], (1.0 + scale) * a_true[1], -GRAVITY_NED[2]]; - f.propagate(Imu { gyro: [0.0; 3], accel }, dt); + let accel = [ + (1.0 + scale) * a_true[0], + (1.0 + scale) * a_true[1], + -GRAVITY_NED[2], + ]; + f.propagate( + Imu { + gyro: [0.0; 3], + accel, + }, + dt, + ); if k % 20 == 0 { let jit = 0.01 * relay_math::sinf(13.0 * t); // deterministic meas jitter f.update_position([tp[0] + jit, tp[1] - jit, tp[2]], 0.01); @@ -1798,7 +1996,10 @@ mod tests { adaptive < fixed, "adaptive Q must add conservatism under motion (fixed {fixed}, adaptive {adaptive})" ); - assert!(adaptive > 0.05, "but not collapse the estimate (NEES {adaptive})"); + assert!( + adaptive > 0.05, + "but not collapse the estimate (NEES {adaptive})" + ); } // ── v0.37 sensor-fault / spoof robustness ──────────────────────────── @@ -1816,7 +2017,10 @@ mod tests { assert!(!accepted, "a 50 m jump fix must be gated out"); let after = f.state().p; for i in 0..3 { - assert!((after[i] - before[i]).abs() < 1e-5, "rejected fix walked the state"); + assert!( + (after[i] - before[i]).abs() < 1e-5, + "rejected fix walked the state" + ); } } @@ -1831,7 +2035,10 @@ mod tests { rejected += 1; } } - assert_eq!(rejected, 0, "honest noise must pass the gate, {rejected} rejected"); + assert_eq!( + rejected, 0, + "honest noise must pass the gate, {rejected} rejected" + ); } /// The spoof monitor latches on a slow same-sign walk-off (each step too @@ -1852,7 +2059,10 @@ mod tests { break; } } - assert!(at.is_some() && at.unwrap() <= 12, "walk-off detected ~10 steps, got {at:?}"); + assert!( + at.is_some() && at.unwrap() <= 12, + "walk-off detected ~10 steps, got {at:?}" + ); assert!(mon.spoofed()); // latched assert!(mon.update([0.0; 3]), "stays latched"); } @@ -1865,7 +2075,10 @@ mod tests { for _ in 0..100 { mon.update([f32::NAN, f32::INFINITY, 0.0]); } - assert!(!mon.spoofed(), "non-finite innovation must not trip the alarm"); + assert!( + !mon.spoofed(), + "non-finite innovation must not trip the alarm" + ); } /// At rest (ω≈0, a≈0) the inflation factor is exactly 1 — the propagated @@ -1873,7 +2086,10 @@ mod tests { /// behaviour is provably unchanged. #[test] fn at_rest_inflation_is_identity() { - let imu_rest = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -GRAVITY_NED[2]] }; + let imu_rest = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -GRAVITY_NED[2]], + }; let mut fixed = Iekf::with_config(NavState::identity(), { let mut c = IekfConfig::DEFAULT; c.q_motion_gyro = 0.0; @@ -1947,7 +2163,10 @@ mod variance_floor_tests { #[test] fn floor_never_fires_on_healthy_long_run() { let mut f = Iekf::level(); - let imu = Imu { gyro: [0.0; 3], accel: [0.0, 0.0, -9.81] }; + let imu = Imu { + gyro: [0.0; 3], + accel: [0.0, 0.0, -9.81], + }; for k in 0..50_000 { f.propagate(imu, 0.004); f.update_gravity(imu.accel, 0.5); @@ -1990,7 +2209,11 @@ mod kani_harness { let x: f32 = kani::any(); let y: f32 = kani::any(); let z: f32 = kani::any(); - kani::assume(relay_math::fabsf(x) <= drift && relay_math::fabsf(y) <= drift && relay_math::fabsf(z) <= drift); + kani::assume( + relay_math::fabsf(x) <= drift + && relay_math::fabsf(y) <= drift + && relay_math::fabsf(z) <= drift, + ); let alarmed = mon.update([x, y, z]); assert!(!alarmed); assert!(!mon.spoofed()); diff --git a/crates/relay-lc-diff/src/lib.rs b/crates/relay-lc-diff/src/lib.rs index dcceae51..0c2434ad 100644 --- a/crates/relay-lc-diff/src/lib.rs +++ b/crates/relay-lc-diff/src/lib.rs @@ -56,7 +56,7 @@ pub fn reference_evaluate( mod tests { use super::*; use proptest::prelude::*; - use relay_lc::engine::{compare, MAX_VIOLATIONS_PER_CYCLE}; + use relay_lc::engine::{MAX_VIOLATIONS_PER_CYCLE, compare}; fn op_from_u8(v: u8) -> ComparisonOp { match v % 6 { diff --git a/crates/relay-lc/plain/src/c_api.rs b/crates/relay-lc/plain/src/c_api.rs index a39f2792..dd472900 100644 --- a/crates/relay-lc/plain/src/c_api.rs +++ b/crates/relay-lc/plain/src/c_api.rs @@ -17,8 +17,8 @@ //! is untouched. This file is ONLY glue. use crate::engine::{ - ComparisonOp, EvalResult, SensorReading, Violation, Watchpoint, - WatchpointTable, MAX_VIOLATIONS_PER_CYCLE, MAX_WATCHPOINTS, + ComparisonOp, EvalResult, MAX_VIOLATIONS_PER_CYCLE, MAX_WATCHPOINTS, SensorReading, Violation, + Watchpoint, WatchpointTable, }; /// Fixed-point scaling factor: multiply f64 by this to get i64. @@ -258,7 +258,6 @@ pub extern "C" fn relay_lc_max_watchpoints() -> u32 { // uint32_t relay_lc_max_watchpoints(void); // // #endif /* RELAY_LC_H */ - #[cfg(test)] mod tests { use super::*; diff --git a/crates/relay-lc/plain/src/engine.rs b/crates/relay-lc/plain/src/engine.rs index 68024152..2cf6e26a 100644 --- a/crates/relay-lc/plain/src/engine.rs +++ b/crates/relay-lc/plain/src/engine.rs @@ -4,56 +4,118 @@ //! + LC-specific glue (watchpoint table, sensor-id match, bounded output). //! Source of truth: ../src/engine.rs. -pub use crate::compare::{compare_i64 as compare, ComparisonOp}; +pub use crate::compare::{ComparisonOp, compare_i64 as compare}; pub const MAX_WATCHPOINTS: usize = 128; pub const MAX_VIOLATIONS_PER_CYCLE: usize = 32; #[derive(Clone, Copy, Debug)] -pub struct Watchpoint { pub sensor_id: u32, pub op: ComparisonOp, pub threshold: i64, pub enabled: bool, pub persistence: u32, pub current_count: u32 } +pub struct Watchpoint { + pub sensor_id: u32, + pub op: ComparisonOp, + pub threshold: i64, + pub enabled: bool, + pub persistence: u32, + pub current_count: u32, +} #[derive(Clone, Copy, Debug)] -pub struct Violation { pub watchpoint_id: u32, pub measured: i64, pub threshold: i64, pub op: ComparisonOp } +pub struct Violation { + pub watchpoint_id: u32, + pub measured: i64, + pub threshold: i64, + pub op: ComparisonOp, +} #[derive(Clone, Copy, Debug)] -pub struct SensorReading { pub sensor_id: u32, pub value: i64 } +pub struct SensorReading { + pub sensor_id: u32, + pub value: i64, +} -pub struct EvalResult { pub violations: [Violation; MAX_VIOLATIONS_PER_CYCLE], pub violation_count: u32 } +pub struct EvalResult { + pub violations: [Violation; MAX_VIOLATIONS_PER_CYCLE], + pub violation_count: u32, +} -pub struct WatchpointTable { entries: [Watchpoint; MAX_WATCHPOINTS], entry_count: u32 } +pub struct WatchpointTable { + entries: [Watchpoint; MAX_WATCHPOINTS], + entry_count: u32, +} -impl Watchpoint { pub const fn empty() -> Self { Watchpoint { sensor_id: 0, op: ComparisonOp::LessThan, threshold: 0, enabled: false, persistence: 1, current_count: 0 } } } -impl Violation { pub const fn empty() -> Self { Violation { watchpoint_id: 0, measured: 0, threshold: 0, op: ComparisonOp::LessThan } } } +impl Watchpoint { + pub const fn empty() -> Self { + Watchpoint { + sensor_id: 0, + op: ComparisonOp::LessThan, + threshold: 0, + enabled: false, + persistence: 1, + current_count: 0, + } + } +} +impl Violation { + pub const fn empty() -> Self { + Violation { + watchpoint_id: 0, + measured: 0, + threshold: 0, + op: ComparisonOp::LessThan, + } + } +} impl WatchpointTable { - pub const NEW: Self = WatchpointTable { entries: [Watchpoint::empty(); MAX_WATCHPOINTS], entry_count: 0 }; - pub fn new() -> Self { Self::NEW } + pub const NEW: Self = WatchpointTable { + entries: [Watchpoint::empty(); MAX_WATCHPOINTS], + entry_count: 0, + }; + pub fn new() -> Self { + Self::NEW + } pub fn add_watchpoint(&mut self, wp: Watchpoint) -> bool { - if self.entry_count as usize >= MAX_WATCHPOINTS { return false; } + if self.entry_count as usize >= MAX_WATCHPOINTS { + return false; + } self.entries[self.entry_count as usize] = wp; self.entry_count += 1; true } - pub fn count(&self) -> u32 { self.entry_count } + pub fn count(&self) -> u32 { + self.entry_count + } pub fn evaluate(&mut self, reading: SensorReading) -> EvalResult { - let mut result = EvalResult { violations: [Violation::empty(); MAX_VIOLATIONS_PER_CYCLE], violation_count: 0 }; + let mut result = EvalResult { + violations: [Violation::empty(); MAX_VIOLATIONS_PER_CYCLE], + violation_count: 0, + }; let count = self.entry_count; let mut i: u32 = 0; while i < count { - if result.violation_count as usize >= MAX_VIOLATIONS_PER_CYCLE { break; } + if result.violation_count as usize >= MAX_VIOLATIONS_PER_CYCLE { + break; + } let idx = i as usize; let wp = self.entries[idx]; if wp.enabled && wp.sensor_id == reading.sensor_id { // Composition of verified primitives: compare → persistence::decide → persistence::apply. let violated = compare(reading.value, wp.op, wp.threshold); - let decision = crate::persistence::decide(violated, wp.current_count, wp.persistence); - self.entries[idx].current_count = crate::persistence::apply(decision, wp.current_count); + let decision = + crate::persistence::decide(violated, wp.current_count, wp.persistence); + self.entries[idx].current_count = + crate::persistence::apply(decision, wp.current_count); if decision == crate::persistence::PersistenceDecision::Fire { let vidx = result.violation_count as usize; - result.violations[vidx] = Violation { watchpoint_id: i, measured: reading.value, threshold: wp.threshold, op: wp.op }; + result.violations[vidx] = Violation { + watchpoint_id: i, + measured: reading.value, + threshold: wp.threshold, + op: wp.op, + }; result.violation_count += 1; } } @@ -85,14 +147,7 @@ pub struct Geofence { } impl Geofence { - pub fn new( - min_n: i32, - max_n: i32, - min_e: i32, - max_e: i32, - min_d: i32, - max_d: i32, - ) -> Self { + pub fn new(min_n: i32, max_n: i32, min_e: i32, max_e: i32, min_d: i32, max_d: i32) -> Self { Geofence { min_n, max_n, @@ -132,14 +187,168 @@ impl Geofence { mod tests { use super::*; - #[test] fn test_empty() { let mut t = WatchpointTable::new(); assert_eq!(t.evaluate(SensorReading { sensor_id: 1, value: 100 }).violation_count, 0); } - #[test] fn test_gt_violation() { let mut t = WatchpointTable::new(); t.add_watchpoint(Watchpoint { sensor_id: 1, op: ComparisonOp::GreaterThan, threshold: 50, enabled: true, persistence: 1, current_count: 0 }); assert_eq!(t.evaluate(SensorReading { sensor_id: 1, value: 100 }).violation_count, 1); assert_eq!(t.evaluate(SensorReading { sensor_id: 1, value: 30 }).violation_count, 0); } - #[test] fn test_persistence() { let mut t = WatchpointTable::new(); t.add_watchpoint(Watchpoint { sensor_id: 1, op: ComparisonOp::GreaterThan, threshold: 50, enabled: true, persistence: 3, current_count: 0 }); let r = SensorReading { sensor_id: 1, value: 100 }; assert_eq!(t.evaluate(r).violation_count, 0); assert_eq!(t.evaluate(r).violation_count, 0); assert_eq!(t.evaluate(r).violation_count, 1); } - #[test] fn test_persistence_reset() { let mut t = WatchpointTable::new(); t.add_watchpoint(Watchpoint { sensor_id: 1, op: ComparisonOp::GreaterThan, threshold: 50, enabled: true, persistence: 3, current_count: 0 }); let bad = SensorReading { sensor_id: 1, value: 100 }; let good = SensorReading { sensor_id: 1, value: 10 }; t.evaluate(bad); t.evaluate(bad); t.evaluate(good); assert_eq!(t.evaluate(bad).violation_count, 0); assert_eq!(t.evaluate(bad).violation_count, 0); assert_eq!(t.evaluate(bad).violation_count, 1); } - #[test] fn test_sensor_filter() { let mut t = WatchpointTable::new(); t.add_watchpoint(Watchpoint { sensor_id: 42, op: ComparisonOp::LessThan, threshold: 10, enabled: true, persistence: 1, current_count: 0 }); assert_eq!(t.evaluate(SensorReading { sensor_id: 99, value: 0 }).violation_count, 0); assert_eq!(t.evaluate(SensorReading { sensor_id: 42, value: 5 }).violation_count, 1); } - #[test] fn test_disabled() { let mut t = WatchpointTable::new(); t.add_watchpoint(Watchpoint { sensor_id: 1, op: ComparisonOp::GreaterThan, threshold: 0, enabled: false, persistence: 1, current_count: 0 }); assert_eq!(t.evaluate(SensorReading { sensor_id: 1, value: 999 }).violation_count, 0); } - #[test] fn test_ops() { assert!(compare(5, ComparisonOp::LessThan, 10)); assert!(compare(10, ComparisonOp::GreaterThan, 5)); assert!(compare(5, ComparisonOp::Equal, 5)); assert!(compare(5, ComparisonOp::NotEqual, 6)); } - #[test] fn test_bounded() { let mut t = WatchpointTable::new(); for _ in 0..(MAX_VIOLATIONS_PER_CYCLE + 10) { t.add_watchpoint(Watchpoint { sensor_id: 1, op: ComparisonOp::GreaterThan, threshold: 0, enabled: true, persistence: 1, current_count: 0 }); } assert_eq!(t.evaluate(SensorReading { sensor_id: 1, value: 100 }).violation_count, MAX_VIOLATIONS_PER_CYCLE as u32); } + #[test] + fn test_empty() { + let mut t = WatchpointTable::new(); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 1, + value: 100 + }) + .violation_count, + 0 + ); + } + #[test] + fn test_gt_violation() { + let mut t = WatchpointTable::new(); + t.add_watchpoint(Watchpoint { + sensor_id: 1, + op: ComparisonOp::GreaterThan, + threshold: 50, + enabled: true, + persistence: 1, + current_count: 0, + }); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 1, + value: 100 + }) + .violation_count, + 1 + ); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 1, + value: 30 + }) + .violation_count, + 0 + ); + } + #[test] + fn test_persistence() { + let mut t = WatchpointTable::new(); + t.add_watchpoint(Watchpoint { + sensor_id: 1, + op: ComparisonOp::GreaterThan, + threshold: 50, + enabled: true, + persistence: 3, + current_count: 0, + }); + let r = SensorReading { + sensor_id: 1, + value: 100, + }; + assert_eq!(t.evaluate(r).violation_count, 0); + assert_eq!(t.evaluate(r).violation_count, 0); + assert_eq!(t.evaluate(r).violation_count, 1); + } + #[test] + fn test_persistence_reset() { + let mut t = WatchpointTable::new(); + t.add_watchpoint(Watchpoint { + sensor_id: 1, + op: ComparisonOp::GreaterThan, + threshold: 50, + enabled: true, + persistence: 3, + current_count: 0, + }); + let bad = SensorReading { + sensor_id: 1, + value: 100, + }; + let good = SensorReading { + sensor_id: 1, + value: 10, + }; + t.evaluate(bad); + t.evaluate(bad); + t.evaluate(good); + assert_eq!(t.evaluate(bad).violation_count, 0); + assert_eq!(t.evaluate(bad).violation_count, 0); + assert_eq!(t.evaluate(bad).violation_count, 1); + } + #[test] + fn test_sensor_filter() { + let mut t = WatchpointTable::new(); + t.add_watchpoint(Watchpoint { + sensor_id: 42, + op: ComparisonOp::LessThan, + threshold: 10, + enabled: true, + persistence: 1, + current_count: 0, + }); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 99, + value: 0 + }) + .violation_count, + 0 + ); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 42, + value: 5 + }) + .violation_count, + 1 + ); + } + #[test] + fn test_disabled() { + let mut t = WatchpointTable::new(); + t.add_watchpoint(Watchpoint { + sensor_id: 1, + op: ComparisonOp::GreaterThan, + threshold: 0, + enabled: false, + persistence: 1, + current_count: 0, + }); + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 1, + value: 999 + }) + .violation_count, + 0 + ); + } + #[test] + fn test_ops() { + assert!(compare(5, ComparisonOp::LessThan, 10)); + assert!(compare(10, ComparisonOp::GreaterThan, 5)); + assert!(compare(5, ComparisonOp::Equal, 5)); + assert!(compare(5, ComparisonOp::NotEqual, 6)); + } + #[test] + fn test_bounded() { + let mut t = WatchpointTable::new(); + for _ in 0..(MAX_VIOLATIONS_PER_CYCLE + 10) { + t.add_watchpoint(Watchpoint { + sensor_id: 1, + op: ComparisonOp::GreaterThan, + threshold: 0, + enabled: true, + persistence: 1, + current_count: 0, + }); + } + assert_eq!( + t.evaluate(SensorReading { + sensor_id: 1, + value: 100 + }) + .violation_count, + MAX_VIOLATIONS_PER_CYCLE as u32 + ); + } // --- Geofence unit tests (v0.12) — give miri something concrete // to interpret. The exhaustive arbitrary-input coverage lives in @@ -149,33 +358,38 @@ mod tests { Geofence::new(-1_000, 1_000, -1_000, 1_000, -1_000, 1_000) } - #[test] fn geofence_inside_does_not_trip() { + #[test] + fn geofence_inside_does_not_trip() { let mut g = fence(); assert!(!g.check(0, 0, 0)); assert!(!g.violation_active()); } - #[test] fn geofence_outside_n_trips_once() { + #[test] + fn geofence_outside_n_trips_once() { let mut g = fence(); - assert!(g.check(2_000, 0, 0)); // rising edge + assert!(g.check(2_000, 0, 0)); // rising edge assert!(g.violation_active()); - assert!(!g.check(3_000, 0, 0)); // already latched — silent - assert!(!g.check(0, 0, 0)); // even returning inside — still silent + assert!(!g.check(3_000, 0, 0)); // already latched — silent + assert!(!g.check(0, 0, 0)); // even returning inside — still silent } - #[test] fn geofence_outside_e_trips() { + #[test] + fn geofence_outside_e_trips() { let mut g = fence(); assert!(g.check(0, -2_000, 0)); assert!(g.violation_active()); } - #[test] fn geofence_outside_d_trips() { + #[test] + fn geofence_outside_d_trips() { let mut g = fence(); assert!(g.check(0, 0, 2_000)); assert!(g.violation_active()); } - #[test] fn geofence_boundary_inclusive() { + #[test] + fn geofence_boundary_inclusive() { // Exact boundary values are inside per >= / <= in check(). let mut g = fence(); assert!(!g.check(1_000, 1_000, 1_000)); diff --git a/crates/relay-log/plain/src/blackbox.rs b/crates/relay-log/plain/src/blackbox.rs index ab1effb3..662d3397 100644 --- a/crates/relay-log/plain/src/blackbox.rs +++ b/crates/relay-log/plain/src/blackbox.rs @@ -342,7 +342,10 @@ pub fn scan)>(bytes: &[u8], mut sink: F) -> (usize, u if crc32(body) != stored { return (n, at); } - sink(ScannedRecord { rec_type, payload: &bytes[at + HDR..at + HDR + len] }); + sink(ScannedRecord { + rec_type, + payload: &bytes[at + HDR..at + HDR + len], + }); n += 1; at += total; } @@ -362,7 +365,13 @@ pub struct BlackboxWriter { impl BlackboxWriter { pub fn new(log: L, budget_per_tick: usize) -> Self { - BlackboxWriter { log, budget_per_tick, spent_this_tick: 0, dropped: 0, written: 0 } + BlackboxWriter { + log, + budget_per_tick, + spent_this_tick: 0, + dropped: 0, + written: 0, + } } /// Start a new control tick (resets the budget window). diff --git a/crates/relay-log/plain/src/kani_proofs.rs b/crates/relay-log/plain/src/kani_proofs.rs index 375c7237..efed2641 100644 --- a/crates/relay-log/plain/src/kani_proofs.rs +++ b/crates/relay-log/plain/src/kani_proofs.rs @@ -14,7 +14,11 @@ fn verify_ring_total_and_bounded() { kani::assume(n <= 10); let mut i = 0; while i < n { - log.record(LogEntry { t_ms: i, kind: 0, data: [0.0; 2] }); + log.record(LogEntry { + t_ms: i, + kind: 0, + data: [0.0; 2], + }); assert!(log.len() <= 4); i += 1; } @@ -29,10 +33,7 @@ fn verify_encode_decode_identity() { let e = LogEntry { t_ms: kani::any(), kind: kani::any(), - data: [ - f32::from_bits(kani::any()), - f32::from_bits(kani::any()), - ], + data: [f32::from_bits(kani::any()), f32::from_bits(kani::any())], }; let back = LogEntry::decode(&e.encode()); assert!(back.t_ms == e.t_ms); diff --git a/crates/relay-log/plain/src/lib.rs b/crates/relay-log/plain/src/lib.rs index fd6c930b..314b6572 100644 --- a/crates/relay-log/plain/src/lib.rs +++ b/crates/relay-log/plain/src/lib.rs @@ -78,8 +78,17 @@ impl Default for FlightLog { impl FlightLog { /// An empty logger. pub fn new() -> Self { - let blank = LogEntry { t_ms: 0, kind: 0, data: [0.0; 2] }; - FlightLog { buf: [blank; N], start: 0, len: 0, dropped: 0 } + let blank = LogEntry { + t_ms: 0, + kind: 0, + data: [0.0; 2], + }; + FlightLog { + buf: [blank; N], + start: 0, + len: 0, + dropped: 0, + } } /// Record an entry. When the ring is full the OLDEST entry is overwritten and @@ -149,7 +158,11 @@ pub(crate) fn crc32(data: &[u8]) -> u32 { crc ^= b as u32; let mut i = 0; while i < 8 { - crc = if crc & 1 != 0 { (crc >> 1) ^ 0xEDB8_8320 } else { crc >> 1 }; + crc = if crc & 1 != 0 { + (crc >> 1) ^ 0xEDB8_8320 + } else { + crc >> 1 + }; i += 1; } } @@ -164,7 +177,11 @@ mod tests { use super::*; fn e(t: u32, k: u8) -> LogEntry { - LogEntry { t_ms: t, kind: k, data: [t as f32, -(t as f32)] } + LogEntry { + t_ms: t, + kind: k, + data: [t as f32, -(t as f32)], + } } #[test] @@ -195,14 +212,26 @@ mod tests { #[test] fn encode_decode_roundtrips_exactly() { - let original = LogEntry { t_ms: 123456, kind: 7, data: [1.5, -2.25] }; + let original = LogEntry { + t_ms: 123456, + kind: 7, + data: [1.5, -2.25], + }; assert_eq!(LogEntry::decode(&original.encode()), original); } #[test] fn replay_decodes_a_stream() { - let a = LogEntry { t_ms: 10, kind: 1, data: [1.0, 2.0] }; - let b = LogEntry { t_ms: 20, kind: 2, data: [3.0, 4.0] }; + let a = LogEntry { + t_ms: 10, + kind: 1, + data: [1.0, 2.0], + }; + let b = LogEntry { + t_ms: 20, + kind: 2, + data: [3.0, 4.0], + }; let mut stream = [0u8; 32]; stream[0..16].copy_from_slice(&a.encode()); stream[16..32].copy_from_slice(&b.encode()); @@ -297,7 +326,12 @@ mod blackbox_tests { let expect = boundaries.iter().filter(|&&b| b <= cut).count(); let (cnt, used) = scan(&stream[..cut], |_| {}); assert_eq!(cnt, expect, "cut at {cut}"); - let expect_used = boundaries.iter().filter(|&&b| b <= cut).max().copied().unwrap_or(0); + let expect_used = boundaries + .iter() + .filter(|&&b| b <= cut) + .max() + .copied() + .unwrap_or(0); assert_eq!(used, expect_used, "cut at {cut}"); } } diff --git a/crates/relay-math/src/lib.rs b/crates/relay-math/src/lib.rs index c32a5cd8..dfe354b4 100644 --- a/crates/relay-math/src/lib.rs +++ b/crates/relay-math/src/lib.rs @@ -89,7 +89,11 @@ fn reduce(x: f32) -> (i32, f32) { // round-half-away in pure core (no_std has no f32::round): exact for // the envelope's quadrant counts (|n| ≤ 82 at |x| ≤ 128). let t = x * FRAC_2_PI; - let n = if t >= 0.0 { (t + 0.5) as i64 } else { (t - 0.5) as i64 } as f32; + let n = if t >= 0.0 { + (t + 0.5) as i64 + } else { + (t - 0.5) as i64 + } as f32; let r = ((x - n * PIO2_HI) - n * PIO2_MID) - n * PIO2_LO; // Belt for far-out-of-envelope inputs where the reduction has // degraded: the polynomials are only evaluated on a bounded r, so the @@ -217,8 +221,16 @@ mod tests { for k in 0..n { let x = -128.0 + 256.0 * (k as f32 + 0.5) / n as f32; let (rs, rc) = ref_f32(x); - let ds = if rs.abs() >= 1e-3 { ulp_diff(sinf(x), rs) } else { 0 }; - let dc = if rc.abs() >= 1e-3 { ulp_diff(cosf(x), rc) } else { 0 }; + let ds = if rs.abs() >= 1e-3 { + ulp_diff(sinf(x), rs) + } else { + 0 + }; + let dc = if rc.abs() >= 1e-3 { + ulp_diff(cosf(x), rc) + } else { + 0 + }; let d = ds.max(dc); if x.abs() <= 4.0 * core::f32::consts::PI { worst_inner = worst_inner.max(d); @@ -237,10 +249,20 @@ mod tests { /// finite value in [-1, 1] (non-finite input → 0.0 by spec). #[test] fn f32_kernels_total_and_bounded() { - for bits in [0u32, 0x7F80_0000, 0xFF80_0000, 0x7FC0_0000, 0x0000_0001, 0x7F7F_FFFF] { + for bits in [ + 0u32, + 0x7F80_0000, + 0xFF80_0000, + 0x7FC0_0000, + 0x0000_0001, + 0x7F7F_FFFF, + ] { let x = f32::from_bits(bits); for v in [sinf(x), cosf(x)] { - assert!(v.is_finite() && (-1.0001..=1.0001).contains(&v), "x={x} -> {v}"); + assert!( + v.is_finite() && (-1.0001..=1.0001).contains(&v), + "x={x} -> {v}" + ); } } let mut lcg = 0x1357_9BDFu32; @@ -248,7 +270,10 @@ mod tests { lcg = lcg.wrapping_mul(1664525).wrapping_add(1013904223); let x = f32::from_bits(lcg); for v in [sinf(x), cosf(x)] { - assert!(v.is_finite() && (-1.0001..=1.0001).contains(&v), "x={x} -> {v}"); + assert!( + v.is_finite() && (-1.0001..=1.0001).contains(&v), + "x={x} -> {v}" + ); } } } @@ -284,8 +309,8 @@ mod tests { #[test] #[ignore = "exhaustive ~2.2e9-point sweep — run on demand / nightly qualification"] fn f32_kernels_exhaustive_worst_case_bound() { - use std::sync::atomic::{AtomicU32, Ordering}; use std::sync::Arc; + use std::sync::atomic::{AtomicU32, Ordering}; use std::thread; // worst abs error stored as raw bits of the f32 (monotone for +ve). @@ -306,8 +331,12 @@ mod tests { let (rs, rc) = ref_f32(x); let (ks, kc) = (sinf(x), cosf(x)); la = la.max((ks - rs).abs()).max((kc - rc).abs()); - if rs.abs() >= 1e-3 { lu = lu.max(ulp_diff(ks, rs)); } - if rc.abs() >= 1e-3 { lu = lu.max(ulp_diff(kc, rc)); } + if rs.abs() >= 1e-3 { + lu = lu.max(ulp_diff(ks, rs)); + } + if rc.abs() >= 1e-3 { + lu = lu.max(ulp_diff(kc, rc)); + } } bits += n_threads as u64; } @@ -315,11 +344,18 @@ mod tests { wu.fetch_max(lu, Ordering::Relaxed); })); } - for h in handles { h.join().unwrap(); } + for h in handles { + h.join().unwrap(); + } let worst_abs = f32::from_bits(worst_abs_bits.load(Ordering::Relaxed)); let worst_ulp = worst_ulp_off.load(Ordering::Relaxed); - std::eprintln!("EXHAUSTIVE — worst |abs err| = {worst_abs:e}, worst ulp (|value|≥1e-3) = {worst_ulp}"); - assert!(worst_abs <= 1.2e-7, "worst absolute error {worst_abs:e} exceeds 1 ulp-of-unity"); + std::eprintln!( + "EXHAUSTIVE — worst |abs err| = {worst_abs:e}, worst ulp (|value|≥1e-3) = {worst_ulp}" + ); + assert!( + worst_abs <= 1.2e-7, + "worst absolute error {worst_abs:e} exceeds 1 ulp-of-unity" + ); assert!(worst_ulp <= 2, "worst off-zero ulp {worst_ulp} exceeds 2"); } diff --git a/crates/relay-mavlink/plain/src/kani_proofs.rs b/crates/relay-mavlink/plain/src/kani_proofs.rs index 741de3e1..39367842 100644 --- a/crates/relay-mavlink/plain/src/kani_proofs.rs +++ b/crates/relay-mavlink/plain/src/kani_proofs.rs @@ -20,7 +20,11 @@ fn any_slot() -> StreamSlot { StreamSlot { interval_ticks: interval, frame_bytes: frame, - priority: if critical { Priority::Critical } else { Priority::Normal }, + priority: if critical { + Priority::Critical + } else { + Priority::Normal + }, } } diff --git a/crates/relay-mavlink/plain/src/param.rs b/crates/relay-mavlink/plain/src/param.rs index 2f10d35a..4df7a283 100644 --- a/crates/relay-mavlink/plain/src/param.rs +++ b/crates/relay-mavlink/plain/src/param.rs @@ -58,7 +58,10 @@ impl ParamRequestList { if payload.len() != PARAM_REQUEST_LIST_PAYLOAD_LEN { return None; } - Some(Self { target_system: payload[0], target_component: payload[1] }) + Some(Self { + target_system: payload[0], + target_component: payload[1], + }) } } @@ -241,7 +244,10 @@ mod tests { assert_eq!(PARAM_REQUEST_LIST_MSG_ID, 21); assert_eq!(PARAM_REQUEST_LIST_CRC_EXTRA, 159); let canonical = [0x01, 0x01]; - let m = ParamRequestList { target_system: 1, target_component: 1 }; + let m = ParamRequestList { + target_system: 1, + target_component: 1, + }; assert_eq!(m.encode_payload(), canonical); assert_eq!(ParamRequestList::decode_payload(&canonical), Some(m)); } diff --git a/crates/relay-mavlink/plain/src/telemetry.rs b/crates/relay-mavlink/plain/src/telemetry.rs index 5ce913f6..44b3dd10 100644 --- a/crates/relay-mavlink/plain/src/telemetry.rs +++ b/crates/relay-mavlink/plain/src/telemetry.rs @@ -354,7 +354,10 @@ mod conformance { heading_deg: 90, throttle_pct: 58, }; - assert_hex(&m.encode_payload(), "000000000000204000000040000000bf5a003a00"); + assert_hex( + &m.encode_payload(), + "000000000000204000000040000000bf5a003a00", + ); } #[test] diff --git a/crates/relay-mavlink/plain/src/telemetry_sched.rs b/crates/relay-mavlink/plain/src/telemetry_sched.rs index 9dd8a2b4..02dc41b2 100644 --- a/crates/relay-mavlink/plain/src/telemetry_sched.rs +++ b/crates/relay-mavlink/plain/src/telemetry_sched.rs @@ -56,7 +56,12 @@ impl TelemetryScheduler { for (c, s) in countdown.iter_mut().zip(slots.iter()) { *c = s.interval_ticks; // stagger-free start; first emit after one interval } - TelemetryScheduler { slots, countdown, age: [0; N], deferred: 0 } + TelemetryScheduler { + slots, + countdown, + age: [0; N], + deferred: 0, + } } /// The no-starvation precondition: every critical frame fits the budget @@ -157,14 +162,46 @@ mod tests { fn falcon_set() -> TelemetryScheduler<8> { TelemetryScheduler::new([ - StreamSlot { interval_ticks: 10, frame_bytes: 21, priority: Priority::Critical }, - StreamSlot { interval_ticks: 10, frame_bytes: 66, priority: Priority::Critical }, - StreamSlot { interval_ticks: 1, frame_bytes: 40, priority: Priority::Normal }, - StreamSlot { interval_ticks: 2, frame_bytes: 40, priority: Priority::Normal }, - StreamSlot { interval_ticks: 2, frame_bytes: 32, priority: Priority::Normal }, - StreamSlot { interval_ticks: 5, frame_bytes: 43, priority: Priority::Normal }, - StreamSlot { interval_ticks: 5, frame_bytes: 64, priority: Priority::Normal }, - StreamSlot { interval_ticks: 5, frame_bytes: 49, priority: Priority::Normal }, + StreamSlot { + interval_ticks: 10, + frame_bytes: 21, + priority: Priority::Critical, + }, + StreamSlot { + interval_ticks: 10, + frame_bytes: 66, + priority: Priority::Critical, + }, + StreamSlot { + interval_ticks: 1, + frame_bytes: 40, + priority: Priority::Normal, + }, + StreamSlot { + interval_ticks: 2, + frame_bytes: 40, + priority: Priority::Normal, + }, + StreamSlot { + interval_ticks: 2, + frame_bytes: 32, + priority: Priority::Normal, + }, + StreamSlot { + interval_ticks: 5, + frame_bytes: 43, + priority: Priority::Normal, + }, + StreamSlot { + interval_ticks: 5, + frame_bytes: 64, + priority: Priority::Normal, + }, + StreamSlot { + interval_ticks: 5, + frame_bytes: 49, + priority: Priority::Normal, + }, ]) } diff --git a/crates/relay-md/plain/src/engine.rs b/crates/relay-md/plain/src/engine.rs index 38f87afb..7cbb0ab0 100644 --- a/crates/relay-md/plain/src/engine.rs +++ b/crates/relay-md/plain/src/engine.rs @@ -25,13 +25,21 @@ pub struct DwellResult { impl DwellEntry { pub const fn empty() -> Self { - DwellEntry { address: 0, size: 0, rate_divisor: 1, enabled: false } + DwellEntry { + address: 0, + size: 0, + rate_divisor: 1, + enabled: false, + } } } impl DwellRequest { pub const fn empty() -> Self { - DwellRequest { address: 0, size: 0 } + DwellRequest { + address: 0, + size: 0, + } } } diff --git a/crates/relay-mix-multi/plain/src/lib.rs b/crates/relay-mix-multi/plain/src/lib.rs index c6754922..64345390 100644 --- a/crates/relay-mix-multi/plain/src/lib.rs +++ b/crates/relay-mix-multi/plain/src/lib.rs @@ -90,14 +90,14 @@ pub fn hex_x() -> Mixer<6> { /// 8 rotors; the top/bottom of an arm share roll/pitch, oppose in yaw. pub fn octo_coax() -> Mixer<8> { Mixer::new([ - [1.0, -0.707, 0.707, 1.0], // arm 1 top - [1.0, -0.707, 0.707, -1.0], // arm 1 bottom - [1.0, 0.707, -0.707, 1.0], // arm 2 top - [1.0, 0.707, -0.707, -1.0], // arm 2 bottom - [1.0, 0.707, 0.707, -1.0], // arm 3 top - [1.0, 0.707, 0.707, 1.0], // arm 3 bottom + [1.0, -0.707, 0.707, 1.0], // arm 1 top + [1.0, -0.707, 0.707, -1.0], // arm 1 bottom + [1.0, 0.707, -0.707, 1.0], // arm 2 top + [1.0, 0.707, -0.707, -1.0], // arm 2 bottom + [1.0, 0.707, 0.707, -1.0], // arm 3 top + [1.0, 0.707, 0.707, 1.0], // arm 3 bottom [1.0, -0.707, -0.707, -1.0], // arm 4 top - [1.0, -0.707, -0.707, 1.0], // arm 4 bottom + [1.0, -0.707, -0.707, 1.0], // arm 4 bottom ]) } @@ -114,7 +114,12 @@ mod tests { #[test] fn hover_wrench_spins_all_rotors() { - let w = Wrench { thrust: 0.5, roll: 0.0, pitch: 0.0, yaw: 0.0 }; + let w = Wrench { + thrust: 0.5, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + }; let out = hex_x().mix(w); assert!(all_in_range(&out)); assert!(out.iter().all(|&x| (x - 0.5).abs() < 1e-6)); // even hover @@ -123,21 +128,36 @@ mod tests { #[test] fn hex_outputs_bounded_under_saturating_command() { // a huge mixed demand must clamp, never exceed 1 or go negative. - let w = Wrench { thrust: 1.0, roll: 5.0, pitch: -5.0, yaw: 3.0 }; + let w = Wrench { + thrust: 1.0, + roll: 5.0, + pitch: -5.0, + yaw: 3.0, + }; let out = hex_x().mix(w); assert!(all_in_range(&out)); } #[test] fn coax_outputs_bounded() { - let w = Wrench { thrust: 0.6, roll: -2.0, pitch: 1.0, yaw: -4.0 }; + let w = Wrench { + thrust: 0.6, + roll: -2.0, + pitch: 1.0, + yaw: -4.0, + }; let out = octo_coax().mix(w); assert!(all_in_range(&out)); } #[test] fn nan_command_is_zeroed_not_propagated() { - let w = Wrench { thrust: f32::NAN, roll: 0.0, pitch: 0.0, yaw: 0.0 }; + let w = Wrench { + thrust: f32::NAN, + roll: 0.0, + pitch: 0.0, + yaw: 0.0, + }; let out = hex_x().mix(w); assert!(all_in_range(&out)); assert!(out.iter().all(|&x| x == 0.0)); diff --git a/crates/relay-mix-quad/plain/src/lib.rs b/crates/relay-mix-quad/plain/src/lib.rs index 28c6c261..d3a67d01 100644 --- a/crates/relay-mix-quad/plain/src/lib.rs +++ b/crates/relay-mix-quad/plain/src/lib.rs @@ -86,7 +86,9 @@ pub struct QuadMixer { impl QuadMixer { pub const fn new() -> Self { - Self { last_motors: [0.0; 4] } + Self { + last_motors: [0.0; 4], + } } pub fn last_motors(&self) -> [f32; 4] { @@ -168,12 +170,7 @@ impl QuadMixer { /// Invariant (MIX-P05, proved in the verus tree + Kani harness): /// for `thrust ∈ [floor, 1]` and `floor ∈ [0, 1]`, every output /// motor is in `[floor, 1]` ⊆ `[0, 1]` and finite. - pub fn mix_thrust_floor( - &mut self, - torque_body: [f32; 3], - thrust: f32, - floor: f32, - ) -> [f32; 4] { + pub fn mix_thrust_floor(&mut self, torque_body: [f32; 3], thrust: f32, floor: f32) -> [f32; 4] { let t = clamp01(sanitise(thrust)); let floor = clamp01(sanitise(floor)); // Collective base; if thrust is below the floor we can't @@ -205,13 +202,19 @@ impl QuadMixer { for &di in &d { if di > EPS { let lim = (1.0 - base) / di; - if lim < s { s = lim; } + if lim < s { + s = lim; + } } else if di < -EPS { let lim = (base - floor) / (-di); - if lim < s { s = lim; } + if lim < s { + s = lim; + } } } - if s < 0.0 || !s.is_finite() { s = 0.0; } + if s < 0.0 || !s.is_finite() { + s = 0.0; + } let mut m = [0.0_f32; 4]; for i in 0..4 { @@ -238,12 +241,7 @@ impl QuadMixer { /// Invariant (same as MIX-P05): for `floor ∈ [0,1]` and any torque, /// every output motor ∈ `[floor, 1]` and finite — the final /// `clamp_floor` makes it a hard guarantee. - pub fn mix_priority( - &mut self, - torque_body: [f32; 3], - thrust: f32, - floor: f32, - ) -> [f32; 4] { + pub fn mix_priority(&mut self, torque_body: [f32; 3], thrust: f32, floor: f32) -> [f32; 4] { let t = clamp01(sanitise(thrust)); let floor = clamp01(sanitise(floor)); let base = if t < floor { floor } else { t }; @@ -266,7 +264,12 @@ impl QuadMixer { let base_rp = [base + drp[0], base + drp[1], base + drp[2], base + drp[3]]; let sy = scale_to_fit(&base_rp, &dy, floor); // 2. ROLL/PITCH next: scale to fit with the reduced yaw applied. - let base_y = [base + sy * dy[0], base + sy * dy[1], base + sy * dy[2], base + sy * dy[3]]; + let base_y = [ + base + sy * dy[0], + base + sy * dy[1], + base + sy * dy[2], + base + sy * dy[3], + ]; let srp = scale_to_fit(&base_y, &drp, floor); let mut m = [0.0_f32; 4]; @@ -304,8 +307,12 @@ impl QuadMixer { } let (mut dmin, mut dmax) = (d[0], d[0]); for &di in &d[1..] { - if di < dmin { dmin = di; } - if di > dmax { dmax = di; } + if di < dmin { + dmin = di; + } + if di > dmax { + dmax = di; + } } // If the differential spread exceeds the available range, scale the @@ -329,8 +336,12 @@ impl QuadMixer { let mut m = [t + d[0], t + d[1], t + d[2], t + d[3]]; let (mut lo, mut hi) = (m[0], m[0]); for &v in &m[1..] { - if v < lo { lo = v; } - if v > hi { hi = v; } + if v < lo { + lo = v; + } + if v > hi { + hi = v; + } } let shift = if lo < idle { idle - lo @@ -503,11 +514,7 @@ fn clamp01(x: f32) -> f32 { #[inline] fn sanitise(x: f32) -> f32 { - if !x.is_finite() { - 0.0 - } else { - x - } + if !x.is_finite() { 0.0 } else { x } } /// Largest scale `s ∈ [0,1]` keeping `base[i] + s·delta[i] ∈ [floor,1]` @@ -521,10 +528,14 @@ fn scale_to_fit(base: &[f32; 4], delta: &[f32; 4], floor: f32) -> f32 { let b = base[i]; if di > EPS { let lim = (1.0 - b) / di; - if lim < s { s = lim; } + if lim < s { + s = lim; + } } else if di < -EPS { let lim = (b - floor) / (-di); - if lim < s { s = lim; } + if lim < s { + s = lim; + } } } if s < 0.0 || !s.is_finite() { 0.0 } else { s } @@ -609,7 +620,11 @@ impl MixerN { /// so the peak |·| over rotors is 1 — matching the hand-tuned `MIXER_X` /// scale, so `from_geometry(quad-X angles)` reproduces it. pub fn from_geometry(rotors: &[(f32, bool)]) -> Self { - let n = if rotors.len() > MAX_ROTORS { MAX_ROTORS } else { rotors.len() }; + let n = if rotors.len() > MAX_ROTORS { + MAX_ROTORS + } else { + rotors.len() + }; let mut mix = [[0.0_f32; 4]; MAX_ROTORS]; // First pass: raw roll/pitch, track peaks for normalization. let mut peak_r = 0.0_f32; @@ -878,8 +893,10 @@ mod tests { } let roll_prio = motors_to_torque_signs(prio)[0].abs(); let roll_uni = motors_to_torque_signs(uni)[0].abs(); - assert!(roll_prio >= roll_uni - 1e-6, - "priority should preserve >= roll than uniform: {roll_prio} vs {roll_uni}"); + assert!( + roll_prio >= roll_uni - 1e-6, + "priority should preserve >= roll than uniform: {roll_prio} vs {roll_uni}" + ); } /// Without saturation, the priority mix passes the full torque through @@ -890,7 +907,11 @@ mod tests { let out = QuadMixer::new().mix_priority(torque, 0.6, 0.3); let tq = motors_to_torque_signs(out); // all three axes retain their commanded sign (non-zero). - assert!(tq[2].abs() > 1e-3, "yaw preserved when unsaturated: {:?}", tq); + assert!( + tq[2].abs() > 1e-3, + "yaw preserved when unsaturated: {:?}", + tq + ); } /// MIX-P07: airmode preserves the YAW differential where the priority @@ -907,9 +928,14 @@ mod tests { } let yaw_air = motors_to_torque_signs(air)[2].abs(); let yaw_prio = motors_to_torque_signs(prio)[2].abs(); - assert!(yaw_air >= yaw_prio - 1e-6, - "airmode should preserve >= yaw than priority: {yaw_air} vs {yaw_prio}"); - assert!(yaw_air > 1e-3, "airmode keeps real yaw authority: {yaw_air}"); + assert!( + yaw_air >= yaw_prio - 1e-6, + "airmode should preserve >= yaw than priority: {yaw_air} vs {yaw_prio}" + ); + assert!( + yaw_air > 1e-3, + "airmode keeps real yaw authority: {yaw_air}" + ); } /// MIX-P08 (v0.26): single-rotor-out allocator pins the failed rotor to @@ -920,17 +946,27 @@ mod tests { let (thrust, floor) = (0.6_f32, 0.15_f32); for failed in 0..4 { let out = QuadMixer::new().mix_rotor_out(failed, [0.1, -0.1, 0.5], thrust, floor); - assert_eq!(out[failed], 0.0, "failed rotor {failed} must be OFF: {out:?}"); + assert_eq!( + out[failed], 0.0, + "failed rotor {failed} must be OFF: {out:?}" + ); for (i, &v) in out.iter().enumerate() { if i != failed { - assert!(v >= floor && v <= 1.0, "healthy rotor {i} out of [floor,1]: {v}"); + assert!( + v >= floor && v <= 1.0, + "healthy rotor {i} out of [floor,1]: {v}" + ); } } // Yaw is relinquished: the same command with a different yaw must // produce identical healthy outputs. - let out_noyaw = QuadMixer::new().mix_rotor_out(failed, [0.1, -0.1, -9.0], thrust, floor); + let out_noyaw = + QuadMixer::new().mix_rotor_out(failed, [0.1, -0.1, -9.0], thrust, floor); for i in 0..4 { - assert!((out[i] - out_noyaw[i]).abs() < 1e-6, "yaw should not affect rotor {i}"); + assert!( + (out[i] - out_noyaw[i]).abs() < 1e-6, + "yaw should not affect rotor {i}" + ); } } } @@ -940,8 +976,11 @@ mod tests { let mut m = QuadMixer::new(); let r = m.mix([0.0, 0.0, 0.0], 0.5); for v in r.iter() { - assert!((v - 0.5).abs() < 1.0e-6, - "all motors should equal thrust at zero torque, got {:?}", r); + assert!( + (v - 0.5).abs() < 1.0e-6, + "all motors should equal thrust at zero torque, got {:?}", + r + ); } } @@ -953,8 +992,13 @@ mod tests { for &r in &[-1.0_f32, -0.5, 0.0, 0.5, 1.0] { let m = mixer.mix([r, r, r], t); for v in m.iter() { - assert!((0.0..=1.0).contains(v), - "motor out of bounds: t={} r={} -> {:?}", t, r, m); + assert!( + (0.0..=1.0).contains(v), + "motor out of bounds: t={} r={} -> {:?}", + t, + r, + m + ); } } } @@ -966,8 +1010,16 @@ mod tests { // less thrust than left-side (M3, M4). let mut m = QuadMixer::new(); let r = m.mix([0.5, 0.0, 0.0], 0.5); - assert!(r[0] < r[2], "M1 (right) must be less than M3 (left): {:?}", r); - assert!(r[1] < r[3], "M2 (right) must be less than M4 (left): {:?}", r); + assert!( + r[0] < r[2], + "M1 (right) must be less than M3 (left): {:?}", + r + ); + assert!( + r[1] < r[3], + "M2 (right) must be less than M4 (left): {:?}", + r + ); } #[test] @@ -975,8 +1027,16 @@ mod tests { // +pitch (nose up) → front motors (M1, M4) more, back (M2, M3) less. let mut m = QuadMixer::new(); let r = m.mix([0.0, 0.5, 0.0], 0.5); - assert!(r[0] > r[1], "M1 (front) must be greater than M2 (back): {:?}", r); - assert!(r[3] > r[2], "M4 (front) must be greater than M3 (back): {:?}", r); + assert!( + r[0] > r[1], + "M1 (front) must be greater than M2 (back): {:?}", + r + ); + assert!( + r[3] > r[2], + "M4 (front) must be greater than M3 (back): {:?}", + r + ); } #[test] @@ -1123,8 +1183,10 @@ mod tests { for &tq in &[0.0_f32, 0.3, 1.0, 5.0] { let out = m.mix_thrust_floor([tq, 0.0, 0.0], thr, 0.3); let mean = (out[0] + out[1] + out[2] + out[3]) / 4.0; - assert!((mean - thr.max(0.3)).abs() < 1.0e-5, - "collective drifted: thr={thr} tq={tq} mean={mean} out={out:?}"); + assert!( + (mean - thr.max(0.3)).abs() < 1.0e-5, + "collective drifted: thr={thr} tq={tq} mean={mean} out={out:?}" + ); } } } @@ -1208,13 +1270,20 @@ mod tests { let out = h.mix([0.0, 0.0, 0.15], 0.5); // CCW rotors (yaw col +1) rise, CW (−1) fall; all bounded. for i in 0..6 { - assert!(out[i] >= 0.0 && out[i] <= 1.0, "rotor {i} out of range: {}", out[i]); + assert!( + out[i] >= 0.0 && out[i] <= 1.0, + "rotor {i} out of range: {}", + out[i] + ); } // net yaw sign preserved: hexa CCW rotors are 0,2,4 (yaw col +1), // CW are 1,3,5 (−1); a +yaw command biases the CCW group up. let ccw = (out[0] + out[2] + out[4]) / 3.0; let cw = (out[1] + out[3] + out[5]) / 3.0; - assert!(ccw > cw, "positive yaw should bias CCW rotors up: ccw {ccw} cw {cw}"); + assert!( + ccw > cw, + "positive yaw should bias CCW rotors up: ccw {ccw} cw {cw}" + ); } use proptest::prelude::*; diff --git a/crates/relay-mm/plain/src/engine.rs b/crates/relay-mm/plain/src/engine.rs index 53975cec..eb1cf051 100644 --- a/crates/relay-mm/plain/src/engine.rs +++ b/crates/relay-mm/plain/src/engine.rs @@ -85,8 +85,8 @@ pub fn validate_request(config: &MmConfig, req: &MmRequest) -> MmValidation { if !is_aligned(req.address, req.size) { return MmValidation::AlignmentError; } - }, - _ => {}, + } + _ => {} } MmValidation::Valid @@ -125,7 +125,10 @@ mod tests { size: 4, value: 0, }; - assert_eq!(validate_request(&config, &req), MmValidation::AddressOutOfRange); + assert_eq!( + validate_request(&config, &req), + MmValidation::AddressOutOfRange + ); } #[test] @@ -161,7 +164,10 @@ mod tests { size: 4, value: 0, }; - assert_eq!(validate_request(&config, &req), MmValidation::AlignmentError); + assert_eq!( + validate_request(&config, &req), + MmValidation::AlignmentError + ); } #[test] @@ -183,7 +189,10 @@ mod tests { size: 4, value: 0, }; - assert_eq!(validate_request(&config, &req2), MmValidation::AddressOutOfRange); + assert_eq!( + validate_request(&config, &req2), + MmValidation::AddressOutOfRange + ); } #[test] diff --git a/crates/relay-modextra/plain/src/lib.rs b/crates/relay-modextra/plain/src/lib.rs index 7212d3c3..5fcd8d95 100644 --- a/crates/relay-modextra/plain/src/lib.rs +++ b/crates/relay-modextra/plain/src/lib.rs @@ -25,11 +25,7 @@ pub type Ned = [f32; 3]; #[inline] fn fin(x: f32) -> f32 { - if x.is_finite() { - x - } else { - 0.0 - } + if x.is_finite() { x } else { 0.0 } } #[inline] @@ -65,8 +61,16 @@ pub fn follow_setpoint(target: Ned, offset: Ned) -> Ned { /// ABOVE the current altitude (z only increases / down is +z). Total / NaN-safe. pub fn land_in_place(current: Ned, descent_rate: f32, dt: f32) -> Ned { let c = fin3(current); - let rate = if descent_rate.is_finite() && descent_rate > 0.0 { descent_rate } else { 0.0 }; - let d = if dt.is_finite() && dt > 0.0 { dt.min(1.0) } else { 0.0 }; + let rate = if descent_rate.is_finite() && descent_rate > 0.0 { + descent_rate + } else { + 0.0 + }; + let d = if dt.is_finite() && dt > 0.0 { + dt.min(1.0) + } else { + 0.0 + }; [c[0], c[1], c[2] + rate * d] // down is +z → descend } @@ -76,7 +80,11 @@ pub fn land_in_place(current: Ned, descent_rate: f32, dt: f32) -> Ned { /// (return at home altitude). pub fn rtl_setpoint(home: Ned, safe_alt_m: f32) -> Ned { let h = fin3(home); - let alt = if safe_alt_m.is_finite() && safe_alt_m > 0.0 { safe_alt_m } else { 0.0 }; + let alt = if safe_alt_m.is_finite() && safe_alt_m > 0.0 { + safe_alt_m + } else { + 0.0 + }; [h[0], h[1], h[2] - alt] // up = -z in NED } @@ -126,7 +134,11 @@ mod tests { fn all_nan_safe() { let n = f32::NAN; assert!(poi_yaw([n, n, n], [n, n, n], n).is_finite()); - assert!(follow_setpoint([n, 1.0, 2.0], [n, n, n]).iter().all(|x| x.is_finite())); + assert!( + follow_setpoint([n, 1.0, 2.0], [n, n, n]) + .iter() + .all(|x| x.is_finite()) + ); assert!(land_in_place([n, n, n], n, n).iter().all(|x| x.is_finite())); assert!(rtl_setpoint([n, n, n], n).iter().all(|x| x.is_finite())); } diff --git a/crates/relay-nid/plain/src/bitpack.rs b/crates/relay-nid/plain/src/bitpack.rs index af5fa5ca..2d8e56e1 100644 --- a/crates/relay-nid/plain/src/bitpack.rs +++ b/crates/relay-nid/plain/src/bitpack.rs @@ -25,8 +25,8 @@ //! Decoders never panic on arbitrary 25-byte input (proptest-fuzzed). use super::{ - BasicId, IdType, Location, MessageType, OperationalStatus, UaType, - FRAME_BYTES, PROTOCOL_VERSION, UAS_ID_BYTES, + BasicId, FRAME_BYTES, IdType, Location, MessageType, OperationalStatus, PROTOCOL_VERSION, + UAS_ID_BYTES, UaType, }; // ─── BasicId — bit-packed (lossless) ─────────────────────────────── @@ -63,7 +63,11 @@ pub fn decode_basic_id_bitpacked(buf: &[u8; FRAME_BYTES]) -> Option { let ua_type = UaType::from_code(ua_type_code)?; let mut uas_id = [0u8; UAS_ID_BYTES]; uas_id.copy_from_slice(&buf[2..2 + UAS_ID_BYTES]); - Some(BasicId { id_type, ua_type, uas_id }) + Some(BasicId { + id_type, + ua_type, + uas_id, + }) } // ─── Location — bit-packed (lossy, F3411 resolution) ─────────────── @@ -215,11 +219,11 @@ mod tests { status: OperationalStatus::Airborne, latitude_e7: 475_023_456, longitude_e7: 190_401_234, - altitude_cm: 12_000, // 120 m, on F3411 0.5 m grid - ground_speed_cms: 800, // 8 m/s - vertical_speed_cms: -50, // -0.5 m/s - track_centideg: 18_000, // 180° even - timestamp_decisec: 18_000, // 30:00 past hour + altitude_cm: 12_000, // 120 m, on F3411 0.5 m grid + ground_speed_cms: 800, // 8 m/s + vertical_speed_cms: -50, // -0.5 m/s + track_centideg: 18_000, // 180° even + timestamp_decisec: 18_000, // 30:00 past hour } } @@ -256,7 +260,7 @@ mod tests { // of canonicalize → encode → decode equals the canonical input. let mut msg = sample_location(); msg.track_centideg = 18_073; // sub-degree precision - msg.altitude_cm = 12_071; // sub-0.5 m precision + msg.altitude_cm = 12_071; // sub-0.5 m precision let canon = canonicalize_location(&msg); let mut buf = [0u8; FRAME_BYTES]; encode_location_bitpacked(&canon, &mut buf); diff --git a/crates/relay-nid/plain/src/lib.rs b/crates/relay-nid/plain/src/lib.rs index c913464a..09579a2a 100644 --- a/crates/relay-nid/plain/src/lib.rs +++ b/crates/relay-nid/plain/src/lib.rs @@ -199,7 +199,11 @@ pub fn decode_basic_id(buf: &[u8; FRAME_BYTES]) -> Option { let ua_type = UaType::from_code(buf[2])?; let mut uas_id = [0u8; UAS_ID_BYTES]; uas_id.copy_from_slice(&buf[3..3 + UAS_ID_BYTES]); - Some(BasicId { id_type, ua_type, uas_id }) + Some(BasicId { + id_type, + ua_type, + uas_id, + }) } /// Encode a Location / Vector message into the 25-byte frame. @@ -221,7 +225,8 @@ pub fn encode_location(msg: &Location, buf: &mut [u8; FRAME_BYTES]) { /// (≥ 360°), or an out-of-range timestamp (≥ 86 400 s). pub fn decode_location(buf: &[u8; FRAME_BYTES]) -> Option { let (msg_type_code, version) = (buf[0] >> 4, buf[0] & 0x0F); - if version != PROTOCOL_VERSION || MessageType::from_code(msg_type_code)? != MessageType::Location + if version != PROTOCOL_VERSION + || MessageType::from_code(msg_type_code)? != MessageType::Location { return None; } @@ -267,13 +272,13 @@ mod tests { fn sample_location() -> Location { Location { status: OperationalStatus::Airborne, - latitude_e7: 47_502_345_6, // 47.5023456° - longitude_e7: 19_040_123_4, // 19.0401234° - altitude_cm: 12_000, // 120.00 m - ground_speed_cms: 850, // 8.5 m/s - vertical_speed_cms: -25, // -0.25 m/s - track_centideg: 18_000, // 180.00° (due south) - timestamp_decisec: 432_000, // 12:00:00.0 UTC + latitude_e7: 47_502_345_6, // 47.5023456° + longitude_e7: 19_040_123_4, // 19.0401234° + altitude_cm: 12_000, // 120.00 m + ground_speed_cms: 850, // 8.5 m/s + vertical_speed_cms: -25, // -0.25 m/s + track_centideg: 18_000, // 180.00° (due south) + timestamp_decisec: 432_000, // 12:00:00.0 UTC } } diff --git a/crates/relay-notch/plain/src/lib.rs b/crates/relay-notch/plain/src/lib.rs index e190d8e5..754769ea 100644 --- a/crates/relay-notch/plain/src/lib.rs +++ b/crates/relay-notch/plain/src/lib.rs @@ -290,7 +290,10 @@ mod tests { let max = atts.iter().cloned().fold(f32::MIN, f32::max); // deep-notch DFT floors can differ hugely in dB; the criterion is // about the WORST point staying within 3 dB of nominal (20 dB). - assert!(min >= 20.0 - 3.0, "sweep worst point {min:.1} dB (max {max:.1})"); + assert!( + min >= 20.0 - 3.0, + "sweep worst point {min:.1} dB (max {max:.1})" + ); } /// Added phase lag at the rate-loop crossover (≈ 5 Hz for the ADRC @@ -409,7 +412,10 @@ mod tests { } // residual (output minus true body signal) is far below the // injected vibration power — and the body signal survived. - assert!(out_pow < 0.05 * in_pow, "residual {out_pow:.1} vs in {in_pow:.1}"); + assert!( + out_pow < 0.05 * in_pow, + "residual {out_pow:.1} vs in {in_pow:.1}" + ); assert!(sig_pow > 0.0); } diff --git a/crates/relay-offboard/plain/src/lib.rs b/crates/relay-offboard/plain/src/lib.rs index 40a320ab..92b92591 100644 --- a/crates/relay-offboard/plain/src/lib.rs +++ b/crates/relay-offboard/plain/src/lib.rs @@ -38,7 +38,11 @@ pub struct OffboardSetpoint { impl OffboardSetpoint { /// A position hold-point with zero feed-forward velocity, hold-yaw. pub fn position(position_ned: [f32; 3]) -> Self { - Self { position_ned, velocity_ned: [0.0; 3], yaw: f32::NAN } + Self { + position_ned, + velocity_ned: [0.0; 3], + yaw: f32::NAN, + } } } @@ -71,7 +75,11 @@ impl OffboardReceiver { timeout_us, last_fresh_us: 0, last_counter: 0, - sp: OffboardSetpoint { position_ned: [0.0; 3], velocity_ned: [0.0; 3], yaw: f32::NAN }, + sp: OffboardSetpoint { + position_ned: [0.0; 3], + velocity_ned: [0.0; 3], + yaw: f32::NAN, + }, started: false, } } @@ -127,7 +135,11 @@ mod tests { // Concrete yaw (not NaN) so setpoint equality is well-defined in asserts. fn sp(n: f32) -> OffboardSetpoint { - OffboardSetpoint { position_ned: [n, 0.0, -5.0], velocity_ned: [0.0; 3], yaw: 0.0 } + OffboardSetpoint { + position_ned: [n, 0.0, -5.0], + velocity_ned: [0.0; 3], + yaw: 0.0, + } } #[test] diff --git a/crates/relay-param/plain/src/kani_proofs.rs b/crates/relay-param/plain/src/kani_proofs.rs index 9353c991..dfdf9204 100644 --- a/crates/relay-param/plain/src/kani_proofs.rs +++ b/crates/relay-param/plain/src/kani_proofs.rs @@ -14,7 +14,12 @@ fn verify_out_of_range_never_lands() { let max: f32 = kani::any(); kani::assume(min.is_finite() && max.is_finite() && min <= max); let id = param_id("P"); - s.register(ParamDef { id, min, max, default: min }); + s.register(ParamDef { + id, + min, + max, + default: min, + }); let before = s.get(&id).unwrap(); let v: f32 = kani::any(); @@ -31,7 +36,12 @@ fn verify_out_of_range_never_lands() { fn verify_set_total_and_in_range_applies() { let mut s: ParamStore<1> = ParamStore::new(); let id = param_id("P"); - s.register(ParamDef { id, min: 0.0, max: 10.0, default: 5.0 }); + s.register(ParamDef { + id, + min: 0.0, + max: 10.0, + default: 5.0, + }); let v: f32 = kani::any(); kani::assume(v.is_finite() && v >= 0.0 && v <= 10.0); assert!(s.set(&id, v) == SetResult::Applied); diff --git a/crates/relay-param/plain/src/lib.rs b/crates/relay-param/plain/src/lib.rs index 94f4a2bb..852a87c7 100644 --- a/crates/relay-param/plain/src/lib.rs +++ b/crates/relay-param/plain/src/lib.rs @@ -76,10 +76,18 @@ impl ParamStore { /// An empty store. pub fn new() -> Self { let blank = Param { - def: ParamDef { id: [0; 16], min: 0.0, max: 0.0, default: 0.0 }, + def: ParamDef { + id: [0; 16], + min: 0.0, + max: 0.0, + default: 0.0, + }, value: 0.0, }; - ParamStore { params: [blank; N], count: 0 } + ParamStore { + params: [blank; N], + count: 0, + } } /// Register a parameter (value initialised to its default). Returns false if @@ -105,7 +113,9 @@ impl ParamStore { } fn index_of(&self, id: &ParamId) -> Option { - self.params[..self.count].iter().position(|p| &p.def.id == id) + self.params[..self.count] + .iter() + .position(|p| &p.def.id == id) } /// The current value of a parameter (for PARAM_VALUE). @@ -167,8 +177,18 @@ mod tests { fn store() -> ParamStore<4> { let mut s = ParamStore::new(); - s.register(ParamDef { id: param_id("MC_ROLL_P"), min: 0.0, max: 12.0, default: 6.5 }); - s.register(ParamDef { id: param_id("BAT_LOW_V"), min: 10.0, max: 16.8, default: 14.0 }); + s.register(ParamDef { + id: param_id("MC_ROLL_P"), + min: 0.0, + max: 12.0, + default: 6.5, + }); + s.register(ParamDef { + id: param_id("BAT_LOW_V"), + min: 10.0, + max: 16.8, + default: 14.0, + }); s } @@ -191,7 +211,10 @@ mod tests { let mut s = store(); assert_eq!(s.set(¶m_id("MC_ROLL_P"), 99.0), SetResult::OutOfRange); assert_eq!(s.set(¶m_id("MC_ROLL_P"), -1.0), SetResult::OutOfRange); - assert_eq!(s.set(¶m_id("MC_ROLL_P"), f32::NAN), SetResult::OutOfRange); + assert_eq!( + s.set(¶m_id("MC_ROLL_P"), f32::NAN), + SetResult::OutOfRange + ); assert_eq!(s.get(¶m_id("MC_ROLL_P")), Some(6.5)); // unchanged } @@ -211,8 +234,18 @@ mod tests { #[test] fn full_store_register_fails() { let mut s: ParamStore<1> = ParamStore::new(); - assert!(s.register(ParamDef { id: param_id("A"), min: 0.0, max: 1.0, default: 0.5 })); - assert!(!s.register(ParamDef { id: param_id("B"), min: 0.0, max: 1.0, default: 0.5 })); + assert!(s.register(ParamDef { + id: param_id("A"), + min: 0.0, + max: 1.0, + default: 0.5 + })); + assert!(!s.register(ParamDef { + id: param_id("B"), + min: 0.0, + max: 1.0, + default: 0.5 + })); } } @@ -227,9 +260,24 @@ mod persist_tests { fn schema() -> ParamStore<4> { let mut s = ParamStore::new(); - s.register(ParamDef { id: param_id("MC_ROLL_P"), min: 0.0, max: 12.0, default: 6.5 }); - s.register(ParamDef { id: param_id("BAT_LOW_V"), min: 10.0, max: 16.8, default: 14.0 }); - s.register(ParamDef { id: param_id("GF_RADIUS"), min: 5.0, max: 500.0, default: 100.0 }); + s.register(ParamDef { + id: param_id("MC_ROLL_P"), + min: 0.0, + max: 12.0, + default: 6.5, + }); + s.register(ParamDef { + id: param_id("BAT_LOW_V"), + min: 10.0, + max: 16.8, + default: 14.0, + }); + s.register(ParamDef { + id: param_id("GF_RADIUS"), + min: 5.0, + max: 500.0, + default: 100.0, + }); s } @@ -268,7 +316,10 @@ mod persist_tests { s.set(¶m_id("MC_ROLL_P"), 9.0); save(&s, &mut nvm, LAYOUT, VER).unwrap(); let mut s2 = schema(); - assert_eq!(load(&mut s2, &nvm, LAYOUT, VER).outcome, LoadOutcome::Loaded); + assert_eq!( + load(&mut s2, &nvm, LAYOUT, VER).outcome, + LoadOutcome::Loaded + ); assert_eq!(s2.get(¶m_id("MC_ROLL_P")), Some(9.0)); } @@ -340,14 +391,29 @@ mod persist_tests { // orphan is skipped_unknown, the now-out-of-bounds value rejected — // and BOTH are visible in the report (loud, never silent). let mut wide = ParamStore::<4>::new(); - wide.register(ParamDef { id: param_id("MC_ROLL_P"), min: 0.0, max: 50.0, default: 6.5 }); - wide.register(ParamDef { id: param_id("OLD_PARAM"), min: 0.0, max: 1.0, default: 0.5 }); + wide.register(ParamDef { + id: param_id("MC_ROLL_P"), + min: 0.0, + max: 50.0, + default: 6.5, + }); + wide.register(ParamDef { + id: param_id("OLD_PARAM"), + min: 0.0, + max: 1.0, + default: 0.5, + }); wide.set(¶m_id("MC_ROLL_P"), 40.0); // legal then, illegal later let mut nvm: ArrayNvm = ArrayNvm::new(); save(&wide, &mut nvm, LAYOUT, VER).unwrap(); let mut tight = ParamStore::<4>::new(); - tight.register(ParamDef { id: param_id("MC_ROLL_P"), min: 0.0, max: 12.0, default: 6.5 }); + tight.register(ParamDef { + id: param_id("MC_ROLL_P"), + min: 0.0, + max: 12.0, + default: 6.5, + }); let r = load(&mut tight, &nvm, LAYOUT, VER); assert_eq!(r.outcome, LoadOutcome::Loaded); assert_eq!((r.applied, r.skipped_unknown, r.rejected), (0, 1, 1)); @@ -360,7 +426,10 @@ mod persist_tests { let s = schema(); assert_eq!(save(&s, &mut nvm, LAYOUT, VER), Err(SaveError::Capacity)); let mut s2 = schema(); - assert_eq!(load(&mut s2, &nvm, LAYOUT, VER).outcome, LoadOutcome::FreshDefaults); + assert_eq!( + load(&mut s2, &nvm, LAYOUT, VER).outcome, + LoadOutcome::FreshDefaults + ); } } @@ -375,8 +444,18 @@ mod persist_proptests { fn schema() -> ParamStore<2> { let mut s = ParamStore::new(); - s.register(ParamDef { id: param_id("P"), min: -3.0, max: 7.0, default: 1.0 }); - s.register(ParamDef { id: param_id("Q"), min: 0.0, max: 100.0, default: 50.0 }); + s.register(ParamDef { + id: param_id("P"), + min: -3.0, + max: 7.0, + default: 1.0, + }); + s.register(ParamDef { + id: param_id("Q"), + min: 0.0, + max: 100.0, + default: 50.0, + }); s } diff --git a/crates/relay-param/plain/src/persist.rs b/crates/relay-param/plain/src/persist.rs index 09ac76f2..b00368f9 100644 --- a/crates/relay-param/plain/src/persist.rs +++ b/crates/relay-param/plain/src/persist.rs @@ -141,7 +141,11 @@ fn crc32(data: &[u8]) -> u32 { crc ^= b as u32; let mut i = 0; while i < 8 { - crc = if crc & 1 != 0 { (crc >> 1) ^ 0xEDB8_8320 } else { crc >> 1 }; + crc = if crc & 1 != 0 { + (crc >> 1) ^ 0xEDB8_8320 + } else { + crc >> 1 + }; i += 1; } } @@ -165,8 +169,11 @@ pub fn save( // Which slot is currently active? Write the OTHER one. let mut sel = [0u8; 1]; nvm.read(0, &mut sel).map_err(|_| SaveError::Nvm)?; - let (target_slot, new_selector) = - if sel[0] == SELECTOR_A { (1u8, SELECTOR_B) } else { (0u8, SELECTOR_A) }; + let (target_slot, new_selector) = if sel[0] == SELECTOR_A { + (1u8, SELECTOR_B) + } else { + (0u8, SELECTOR_A) + }; let base = layout.slot_offset(target_slot); // Header: magic | schema_version | count | crc(over first 12 bytes). @@ -205,7 +212,12 @@ pub fn load( layout: Layout, schema_version: u32, ) -> LoadReport { - let report = |outcome| LoadReport { outcome, applied: 0, skipped_unknown: 0, rejected: 0 }; + let report = |outcome| LoadReport { + outcome, + applied: 0, + skipped_unknown: 0, + rejected: 0, + }; if nvm.capacity() < layout.required_capacity() { return report(LoadOutcome::FreshDefaults); } @@ -244,8 +256,16 @@ pub fn load( let mut i = 0usize; while i < count as usize { let mut rec = [0u8; RECORD_LEN]; - if nvm.read(base + HEADER_LEN + i * RECORD_LEN, &mut rec).is_err() { - return LoadReport { outcome: LoadOutcome::NvmFault, applied, skipped_unknown, rejected }; + if nvm + .read(base + HEADER_LEN + i * RECORD_LEN, &mut rec) + .is_err() + { + return LoadReport { + outcome: LoadOutcome::NvmFault, + applied, + skipped_unknown, + rejected, + }; } let rcrc = u32::from_le_bytes([rec[20], rec[21], rec[22], rec[23]]); if rcrc != crc32(&rec[0..20]) { @@ -266,7 +286,12 @@ pub fn load( } i += 1; } - LoadReport { outcome: LoadOutcome::Loaded, applied, skipped_unknown, rejected } + LoadReport { + outcome: LoadOutcome::Loaded, + applied, + skipped_unknown, + rejected, + } } /// A fixed-size in-memory NVM — the test/Kani mock AND the Renode-stage diff --git a/crates/relay-pos/plain/src/lib.rs b/crates/relay-pos/plain/src/lib.rs index 81cff036..819de4bb 100644 --- a/crates/relay-pos/plain/src/lib.rs +++ b/crates/relay-pos/plain/src/lib.rs @@ -79,7 +79,10 @@ pub struct Timestamp { } impl Timestamp { - pub const ZERO: Self = Self { seconds: 0, fraction: 0 }; + pub const ZERO: Self = Self { + seconds: 0, + fraction: 0, + }; pub fn as_secs_f32(self) -> f32 { self.seconds as f32 + (self.fraction as f32) / (1u64 << 32) as f32 @@ -330,9 +333,8 @@ impl PosController { self.gains.i_max, ); let derivative = (v_err - self.last_v_err[i]) / dt; - accel[i] = self.gains.kp_vel * v_err - + self.gains.ki_vel * cand_integral - + 0.0 * derivative; // kd reserved for v0.6 + accel[i] = + self.gains.kp_vel * v_err + self.gains.ki_vel * cand_integral + 0.0 * derivative; // kd reserved for v0.6 self.integral[i] = cand_integral; self.last_v_err[i] = v_err; } @@ -394,11 +396,7 @@ impl Default for PosController { #[inline] fn sanitise(x: f32) -> f32 { - if !x.is_finite() { - 0.0 - } else { - x - } + if !x.is_finite() { 0.0 } else { x } } #[inline] @@ -446,7 +444,10 @@ mod tests { fn ts(secs: f32) -> Timestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - Timestamp { seconds: secs as u64, fraction: frac } + Timestamp { + seconds: secs as u64, + fraction: frac, + } } #[test] @@ -461,8 +462,12 @@ mod tests { sp, ); // Tilt should be near zero; thrust should be near hover. - assert!((out.thrust - c.gains().hover_thrust).abs() < 0.05, - "thrust {} not near hover {}", out.thrust, c.gains().hover_thrust); + assert!( + (out.thrust - c.gains().hover_thrust).abs() < 0.05, + "thrust {} not near hover {}", + out.thrust, + c.gains().hover_thrust + ); } #[test] @@ -481,8 +486,11 @@ mod tests { // Pitch should be negative (nose down) → accelerates forward. // Extract pitch from quaternion (small-angle: q.y ≈ pitch/2) let pitch_est = 2.0 * out.quaternion[2]; - assert!(pitch_est < -0.01, - "forward setpoint must pitch nose-down (q.y < 0), got pitch_est={}", pitch_est); + assert!( + pitch_est < -0.01, + "forward setpoint must pitch nose-down (q.y < 0), got pitch_est={}", + pitch_est + ); } #[test] @@ -498,8 +506,11 @@ mod tests { ); // Positive roll → right wing down → accelerates east. let roll_est = 2.0 * out.quaternion[1]; - assert!(roll_est > 0.01, - "east setpoint must roll right (q.x > 0), got roll_est={}", roll_est); + assert!( + roll_est > 0.01, + "east setpoint must roll right (q.x > 0), got roll_est={}", + roll_est + ); } #[test] @@ -515,8 +526,11 @@ mod tests { [1.0, 0.0, 0.0, 0.0], sp, ); - assert!(out.thrust < c.gains().hover_thrust, - "above-setpoint must reduce thrust, got {}", out.thrust); + assert!( + out.thrust < c.gains().hover_thrust, + "above-setpoint must reduce thrust, got {}", + out.thrust + ); } #[test] @@ -532,8 +546,11 @@ mod tests { [1.0, 0.0, 0.0, 0.0], sp, ); - assert!(out.thrust > c.gains().hover_thrust, - "below-setpoint must increase thrust, got {}", out.thrust); + assert!( + out.thrust > c.gains().hover_thrust, + "below-setpoint must increase thrust, got {}", + out.thrust + ); } #[test] @@ -557,10 +574,18 @@ mod tests { let q = out.quaternion; let roll = 2.0 * q[1].atan2(q[0]); let pitch = 2.0 * q[2].atan2(q[0]); - assert!(roll.abs() <= c.gains().tilt_max + 1.0e-3, - "roll {} exceeds tilt_max {}", roll, c.gains().tilt_max); - assert!(pitch.abs() <= c.gains().tilt_max + 1.0e-3, - "pitch {} exceeds tilt_max {}", pitch, c.gains().tilt_max); + assert!( + roll.abs() <= c.gains().tilt_max + 1.0e-3, + "roll {} exceeds tilt_max {}", + roll, + c.gains().tilt_max + ); + assert!( + pitch.abs() <= c.gains().tilt_max + 1.0e-3, + "pitch {} exceeds tilt_max {}", + pitch, + c.gains().tilt_max + ); } } @@ -582,8 +607,12 @@ mod tests { sp, ); for i in 0..3 { - assert!(c.integral()[i].abs() <= c.gains().i_max + 1.0e-6, - "integral[{}] = {} exceeds i_max", i, c.integral()[i]); + assert!( + c.integral()[i].abs() <= c.gains().i_max + 1.0e-6, + "integral[{}] = {} exceeds i_max", + i, + c.integral()[i] + ); } } } @@ -643,8 +672,12 @@ mod tests { let out = c.tick(ts(0.02), [0.0; 3], [0.0; 3], q, sp); // Output quaternion should have approximately the same yaw. let yaw_out = quat_to_yaw(out.quaternion); - assert!((yaw_out - yaw_target).abs() < 0.05, - "yaw not held: expected {} got {}", yaw_target, yaw_out); + assert!( + (yaw_out - yaw_target).abs() < 0.05, + "yaw not held: expected {} got {}", + yaw_target, + yaw_out + ); } #[test] diff --git a/crates/relay-preflight/plain/src/lib.rs b/crates/relay-preflight/plain/src/lib.rs index 4e34407f..ea78de06 100644 --- a/crates/relay-preflight/plain/src/lib.rs +++ b/crates/relay-preflight/plain/src/lib.rs @@ -208,7 +208,10 @@ impl CheckTable { for r in required.iter_mut().take(6) { *r = true; } - CheckTable { required, passed: [false; CHECK_COUNT] } + CheckTable { + required, + passed: [false; CHECK_COUNT], + } } /// Set a row's pass state. Setting ANY row marks it required — an @@ -330,7 +333,11 @@ mod tests { for id in CheckId::ALL.iter().take(6) { t.set(*id, true); } - assert_eq!(arm_check_table(&t), TableVerdict::Allowed, "six pass, rest undeclared"); + assert_eq!( + arm_check_table(&t), + TableVerdict::Allowed, + "six pass, rest undeclared" + ); let fresh = CheckTable::new(); assert_eq!( arm_check_table(&fresh), @@ -342,6 +349,9 @@ mod tests { #[test] fn default_is_all_failing_blocked() { // a fresh checks struct (all false) must NOT arm. - assert_eq!(arm_check(PreflightChecks::default()), ArmVerdict::Blocked(CheckFail::Sensors)); + assert_eq!( + arm_check(PreflightChecks::default()), + ArmVerdict::Blocked(CheckFail::Sensors) + ); } } diff --git a/crates/relay-primitives/plain/src/ccsds.rs b/crates/relay-primitives/plain/src/ccsds.rs index aeaf1ea9..9d467d12 100644 --- a/crates/relay-primitives/plain/src/ccsds.rs +++ b/crates/relay-primitives/plain/src/ccsds.rs @@ -152,15 +152,15 @@ mod tests { #[test] fn decode_too_short() { let buf = [0u8; 3]; - assert!(matches!(decode_header(& buf), Err(ParseError::TooShort))); + assert!(matches!(decode_header(&buf), Err(ParseError::TooShort))); } #[test] fn checksum_empty_is_zero() { - assert_eq!(compute_checksum(& []), 0); + assert_eq!(compute_checksum(&[]), 0); } #[test] fn checksum_xor() { - assert_eq!(compute_checksum(& [0x01, 0x02, 0x04]), 0x07); + assert_eq!(compute_checksum(&[0x01, 0x02, 0x04]), 0x07); } #[test] fn checksum_self_inverse() { @@ -169,6 +169,6 @@ mod tests { let mut all = [0u8; 6]; all[..5].copy_from_slice(&data); all[5] = cs; - assert_eq!(compute_checksum(& all), 0); + assert_eq!(compute_checksum(&all), 0); } } diff --git a/crates/relay-primitives/plain/src/compare.rs b/crates/relay-primitives/plain/src/compare.rs index 15a9901e..541a84c0 100644 --- a/crates/relay-primitives/plain/src/compare.rs +++ b/crates/relay-primitives/plain/src/compare.rs @@ -53,7 +53,7 @@ mod tests { #[test] fn less_than() { assert!(compare_i64(1, ComparisonOp::LessThan, 2)); - assert!(! compare_i64(2, ComparisonOp::LessThan, 2)); + assert!(!compare_i64(2, ComparisonOp::LessThan, 2)); } #[test] fn all_ops_total_on_zero_zero() { diff --git a/crates/relay-primitives/plain/src/filter.rs b/crates/relay-primitives/plain/src/filter.rs index aa266dea..9f372087 100644 --- a/crates/relay-primitives/plain/src/filter.rs +++ b/crates/relay-primitives/plain/src/filter.rs @@ -20,7 +20,11 @@ pub enum FilterDecision { } /// Pure decision: pass the value through iff `predicate_holds`. pub fn filter_decide(predicate_holds: bool) -> FilterDecision { - if predicate_holds { FilterDecision::Keep } else { FilterDecision::Drop } + if predicate_holds { + FilterDecision::Keep + } else { + FilterDecision::Drop + } } #[cfg(test)] mod tests { diff --git a/crates/relay-primitives/plain/src/lib.rs b/crates/relay-primitives/plain/src/lib.rs index 99829e5c..a4dba0dc 100644 --- a/crates/relay-primitives/plain/src/lib.rs +++ b/crates/relay-primitives/plain/src/lib.rs @@ -18,11 +18,11 @@ //! Compositional proofs (WCET(A ∘ B) ≤ WCET(A) + WCET(B) + overhead, //! mem(A ∘ B) ≤ mem(A) + mem(B) + buffer) live in proofs/rocq and proofs/lean. #![no_std] -pub mod crc32; +pub mod ccsds; pub mod compare; +pub mod crc32; +pub mod filter; +pub mod merge; pub mod persistence; pub mod rate_divide; pub mod time_gate; -pub mod ccsds; -pub mod merge; -pub mod filter; diff --git a/crates/relay-primitives/plain/src/merge.rs b/crates/relay-primitives/plain/src/merge.rs index 74873fa5..6ff6711d 100644 --- a/crates/relay-primitives/plain/src/merge.rs +++ b/crates/relay-primitives/plain/src/merge.rs @@ -26,17 +26,17 @@ pub enum MergeChoice { /// Pure decision: round-robin merge arbitration. /// `last_was_left` should be initialized to `false` so the first tie /// favors Left (matches natural left-to-right reading order). -pub fn merge_choose( - left_has: bool, - right_has: bool, - last_was_left: bool, -) -> MergeChoice { +pub fn merge_choose(left_has: bool, right_has: bool, last_was_left: bool) -> MergeChoice { match (left_has, right_has) { (false, false) => MergeChoice::None, (true, false) => MergeChoice::Left, (false, true) => MergeChoice::Right, (true, true) => { - if last_was_left { MergeChoice::Right } else { MergeChoice::Left } + if last_was_left { + MergeChoice::Right + } else { + MergeChoice::Left + } } } } diff --git a/crates/relay-primitives/plain/src/persistence.rs b/crates/relay-primitives/plain/src/persistence.rs index e7bc2e96..df99e768 100644 --- a/crates/relay-primitives/plain/src/persistence.rs +++ b/crates/relay-primitives/plain/src/persistence.rs @@ -32,11 +32,7 @@ pub enum PersistenceDecision { /// /// This is the minimum kernel. No state is mutated here; the caller owns /// the counter and applies the decision. -pub fn decide( - event_fired: bool, - current_count: u32, - persistence: u32, -) -> PersistenceDecision { +pub fn decide(event_fired: bool, current_count: u32, persistence: u32) -> PersistenceDecision { if !event_fired { return PersistenceDecision::Pass; } diff --git a/crates/relay-primitives/plain/src/rate_divide.rs b/crates/relay-primitives/plain/src/rate_divide.rs index d1b7dfea..c2811a26 100644 --- a/crates/relay-primitives/plain/src/rate_divide.rs +++ b/crates/relay-primitives/plain/src/rate_divide.rs @@ -32,7 +32,7 @@ mod tests { #[test] fn divisor_zero_never_emits() { for c in 0..10 { - assert!(! should_emit(c, 0)); + assert!(!should_emit(c, 0)); } } #[test] @@ -44,8 +44,8 @@ mod tests { #[test] fn divisor_five_emits_every_fifth() { assert!(should_emit(0, 5)); - assert!(! should_emit(1, 5)); - assert!(! should_emit(4, 5)); + assert!(!should_emit(1, 5)); + assert!(!should_emit(4, 5)); assert!(should_emit(5, 5)); assert!(should_emit(10, 5)); } diff --git a/crates/relay-primitives/plain/src/time_gate.rs b/crates/relay-primitives/plain/src/time_gate.rs index bceb7880..56978139 100644 --- a/crates/relay-primitives/plain/src/time_gate.rs +++ b/crates/relay-primitives/plain/src/time_gate.rs @@ -27,7 +27,7 @@ mod tests { use super::*; #[test] fn absolute_not_due_before() { - assert!(! is_due_absolute(99, 100)); + assert!(!is_due_absolute(99, 100)); } #[test] fn absolute_due_at_exact() { @@ -42,8 +42,8 @@ mod tests { let start = 1_000u64; for (elapsed, delay) in [(0u64, 0u64), (5, 10), (10, 10), (15, 10)] { assert_eq!( - is_due_relative(elapsed, delay), is_due_absolute(start + elapsed, start + - delay), + is_due_relative(elapsed, delay), + is_due_absolute(start + elapsed, start + delay), ); } } diff --git a/crates/relay-rate/plain/src/lib.rs b/crates/relay-rate/plain/src/lib.rs index f7a5f1d4..7e242cb7 100644 --- a/crates/relay-rate/plain/src/lib.rs +++ b/crates/relay-rate/plain/src/lib.rs @@ -55,7 +55,10 @@ pub struct Timestamp { } impl Timestamp { - pub const ZERO: Self = Self { seconds: 0, fraction: 0 }; + pub const ZERO: Self = Self { + seconds: 0, + fraction: 0, + }; /// Seconds (f32) from the epoch. pub fn as_secs_f32(self) -> f32 { @@ -259,11 +262,7 @@ impl RatePid { // 2. Provisional integral update. let cand_integral = self.integral[i] + error * dt; - let cand_integral = clamp_f32( - cand_integral, - -self.gains.i_max[i], - self.gains.i_max[i], - ); + let cand_integral = clamp_f32(cand_integral, -self.gains.i_max[i], self.gains.i_max[i]); // 3. Provisional output. let cand_torque = self.gains.kp[i] * error @@ -319,7 +318,10 @@ mod tests { fn t_at(secs: f32) -> Timestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - Timestamp { seconds: secs as u64, fraction: frac } + Timestamp { + seconds: secs as u64, + fraction: frac, + } } #[test] @@ -341,7 +343,11 @@ mod tests { fn positive_error_drives_positive_torque() { let mut p = RatePid::new(); let out = p.tick(t_at(0.001), [0.0, 0.0, 0.0], [1.0, 0.0, 0.0]); - assert!(out[0] > 0.0, "+x error must produce +x torque, got {}", out[0]); + assert!( + out[0] > 0.0, + "+x error must produce +x torque, got {}", + out[0] + ); assert_eq!(out[1], 0.0); assert_eq!(out[2], 0.0); } @@ -350,7 +356,11 @@ mod tests { fn negative_error_drives_negative_torque() { let mut p = RatePid::new(); let out = p.tick(t_at(0.001), [0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]); - assert!(out[0] < 0.0, "-x error must produce -x torque, got {}", out[0]); + assert!( + out[0] < 0.0, + "-x error must produce -x torque, got {}", + out[0] + ); } #[test] @@ -365,7 +375,10 @@ mod tests { assert!( out[i].abs() <= p.gains().torque_max[i] + 1.0e-6, "out[{}]={} exceeds torque_max[{}]={}", - i, out[i], i, p.gains().torque_max[i] + i, + out[i], + i, + p.gains().torque_max[i] ); } } @@ -385,7 +398,10 @@ mod tests { assert!( p.integral()[i].abs() <= p.gains().i_max[i] + 1.0e-6, "integral[{}]={} exceeds i_max[{}]={}", - i, p.integral()[i], i, p.gains().i_max[i] + i, + p.integral()[i], + i, + p.gains().i_max[i] ); } } @@ -441,8 +457,15 @@ mod tests { converged_at = step as f32 * dt; } } - assert!(!converged_at.is_nan(), "rate did not converge to setpoint within 3 s"); - assert!(converged_at < 2.0, "convergence took {:.2}s > 2.0s budget", converged_at); + assert!( + !converged_at.is_nan(), + "rate did not converge to setpoint within 3 s" + ); + assert!( + converged_at < 2.0, + "convergence took {:.2}s > 2.0s budget", + converged_at + ); assert!( (omega[0] - 1.0).abs() < 0.01, "steady-state error {:.4} rad/s exceeds 0.01 budget", diff --git a/crates/relay-rc/plain/src/lib.rs b/crates/relay-rc/plain/src/lib.rs index 94c12240..2c9ea47d 100644 --- a/crates/relay-rc/plain/src/lib.rs +++ b/crates/relay-rc/plain/src/lib.rs @@ -91,11 +91,7 @@ pub(crate) fn throttle01(x: f32) -> f32 { /// Clamp a positive limit to a finite non-negative value (NaN/neg ⇒ 0). #[inline] fn limit(x: f32) -> f32 { - if x.is_finite() && x > 0.0 { - x - } else { - 0.0 - } + if x.is_finite() && x > 0.0 { x } else { 0.0 } } /// Stabilized mode: sticks → a bounded attitude setpoint. Centre roll/pitch sticks @@ -274,7 +270,11 @@ fn crsf_crc8(data: &[u8]) -> u8 { crc ^= b; let mut bit = 0; while bit < 8 { - crc = if crc & 0x80 != 0 { (crc << 1) ^ 0xD5 } else { crc << 1 }; + crc = if crc & 0x80 != 0 { + (crc << 1) ^ 0xD5 + } else { + crc << 1 + }; bit += 1; } } @@ -317,7 +317,12 @@ mod tests { use super::*; fn rc(roll: f32, pitch: f32, yaw: f32, throttle: f32) -> RcInput { - RcInput { roll, pitch, yaw, throttle } + RcInput { + roll, + pitch, + yaw, + throttle, + } } #[test] @@ -412,8 +417,10 @@ mod tests { #[test] fn sbus_endpoints_map_to_stick_extremes() { // centre count -> level; min/max -> stick extremes; throttle idle/full. - let ch = [992, 172, 1811, 172, /* rest */ 992, 992, 992, 992, 992, 992, - 992, 992, 992, 992, 992, 992]; + let ch = [ + 992, 172, 1811, 172, /* rest */ 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, + 992, 992, + ]; let rc = sbus_to_rc(&decode_sbus(&pack_sbus(&ch, 0)).unwrap()); assert!(rc.roll.abs() < 1e-3); // 992 -> centre assert!((rc.pitch + 1.0).abs() < 2e-2); // 172 -> -100% @@ -461,7 +468,11 @@ mod tests { for &b in &f[2..25] { crc ^= b; for _ in 0..8 { - crc = if crc & 0x80 != 0 { (crc << 1) ^ 0xD5 } else { crc << 1 }; + crc = if crc & 0x80 != 0 { + (crc << 1) ^ 0xD5 + } else { + crc << 1 + }; } } crc @@ -501,8 +512,9 @@ mod tests { #[test] fn crsf_endpoints_map_to_stick_extremes() { - let ch = [992, 172, 1811, 1811, 992, 992, 992, 992, 992, 992, 992, 992, - 992, 992, 992, 992]; + let ch = [ + 992, 172, 1811, 1811, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, 992, + ]; let rc = crsf_to_rc(&decode_crsf_rc(&pack_crsf(&ch)).unwrap()); assert!(rc.roll.abs() < 1e-3); // 992 -> centre assert!((rc.pitch + 1.0).abs() < 2e-2); // 172 -> -100% diff --git a/crates/relay-sc/plain/src/engine.rs b/crates/relay-sc/plain/src/engine.rs index 73ace0d6..ef142ffd 100644 --- a/crates/relay-sc/plain/src/engine.rs +++ b/crates/relay-sc/plain/src/engine.rs @@ -54,13 +54,24 @@ pub struct DispatchResult { impl AtsCommand { pub const fn empty() -> Self { - AtsCommand { execute_at_sec: 0, command_code: 0, payload_offset: 0, payload_len: 0, dispatched: false } + AtsCommand { + execute_at_sec: 0, + command_code: 0, + payload_offset: 0, + payload_len: 0, + dispatched: false, + } } } impl RtsCommand { pub const fn empty() -> Self { - RtsCommand { delay_sec: 0, command_code: 0, payload_offset: 0, payload_len: 0 } + RtsCommand { + delay_sec: 0, + command_code: 0, + payload_offset: 0, + payload_len: 0, + } } } @@ -78,7 +89,11 @@ impl RtsSequence { impl DispatchedCommand { pub const fn empty() -> Self { - DispatchedCommand { command_code: 0, payload_offset: 0, payload_len: 0 } + DispatchedCommand { + command_code: 0, + payload_offset: 0, + payload_len: 0, + } } } @@ -92,15 +107,21 @@ impl CommandStore { } pub fn load_ats_command(&mut self, cmd: AtsCommand) -> bool { - if self.ats_count as usize >= MAX_ATS_COMMANDS { return false; } + if self.ats_count as usize >= MAX_ATS_COMMANDS { + return false; + } self.ats_table[self.ats_count as usize] = cmd; self.ats_count = self.ats_count + 1; true } pub fn start_rts(&mut self, rts_id: u32, current_time_sec: u64) -> bool { - if rts_id as usize >= MAX_RTS_SEQUENCES { return false; } - if self.rts_sequences[rts_id as usize].command_count == 0 { return false; } + if rts_id as usize >= MAX_RTS_SEQUENCES { + return false; + } + if self.rts_sequences[rts_id as usize].command_count == 0 { + return false; + } self.rts_sequences[rts_id as usize].running = true; self.rts_sequences[rts_id as usize].start_time_sec = current_time_sec; self.rts_sequences[rts_id as usize].current_index = 0; @@ -108,21 +129,29 @@ impl CommandStore { } pub fn stop_rts(&mut self, rts_id: u32) -> bool { - if rts_id as usize >= MAX_RTS_SEQUENCES { return false; } + if rts_id as usize >= MAX_RTS_SEQUENCES { + return false; + } self.rts_sequences[rts_id as usize].running = false; true } pub fn load_rts_command(&mut self, rts_id: u32, cmd: RtsCommand) -> bool { - if rts_id as usize >= MAX_RTS_SEQUENCES { return false; } + if rts_id as usize >= MAX_RTS_SEQUENCES { + return false; + } let seq = &mut self.rts_sequences[rts_id as usize]; - if seq.command_count as usize >= MAX_RTS_COMMANDS { return false; } + if seq.command_count as usize >= MAX_RTS_COMMANDS { + return false; + } seq.commands[seq.command_count as usize] = cmd; seq.command_count = seq.command_count + 1; true } - pub fn ats_count(&self) -> u32 { self.ats_count } + pub fn ats_count(&self) -> u32 { + self.ats_count + } pub fn process_tick(&mut self, current_time_sec: u64) -> DispatchResult { let mut result = DispatchResult { @@ -134,7 +163,9 @@ impl CommandStore { let ats_count = self.ats_count; let mut i: u32 = 0; while i < ats_count { - if result.dispatch_count as usize >= MAX_DISPATCH_PER_TICK { break; } + if result.dispatch_count as usize >= MAX_DISPATCH_PER_TICK { + break; + } let cmd = self.ats_table[i as usize]; if !cmd.dispatched && cmd.execute_at_sec <= current_time_sec { let idx = result.dispatch_count as usize; @@ -152,7 +183,9 @@ impl CommandStore { // Check RTS sequences let mut r: u32 = 0; while r < MAX_RTS_SEQUENCES as u32 { - if result.dispatch_count as usize >= MAX_DISPATCH_PER_TICK { break; } + if result.dispatch_count as usize >= MAX_DISPATCH_PER_TICK { + break; + } let seq = self.rts_sequences[r as usize]; if seq.running && seq.current_index < seq.command_count { let cmd = seq.commands[seq.current_index as usize]; @@ -238,18 +271,24 @@ mod tests { #[test] fn test_rts_sequence_execution() { let mut store = CommandStore::new(); - store.load_rts_command(0, RtsCommand { - delay_sec: 0, - command_code: 0x10, - payload_offset: 0, - payload_len: 4, - }); - store.load_rts_command(0, RtsCommand { - delay_sec: 5, - command_code: 0x11, - payload_offset: 4, - payload_len: 4, - }); + store.load_rts_command( + 0, + RtsCommand { + delay_sec: 0, + command_code: 0x10, + payload_offset: 0, + payload_len: 4, + }, + ); + store.load_rts_command( + 0, + RtsCommand { + delay_sec: 5, + command_code: 0x11, + payload_offset: 4, + payload_len: 4, + }, + ); assert!(store.start_rts(0, 100)); // First command fires immediately (delay=0, elapsed=0) @@ -270,18 +309,24 @@ mod tests { #[test] fn test_rts_stop() { let mut store = CommandStore::new(); - store.load_rts_command(0, RtsCommand { - delay_sec: 0, - command_code: 0x20, - payload_offset: 0, - payload_len: 0, - }); - store.load_rts_command(0, RtsCommand { - delay_sec: 10, - command_code: 0x21, - payload_offset: 0, - payload_len: 0, - }); + store.load_rts_command( + 0, + RtsCommand { + delay_sec: 0, + command_code: 0x20, + payload_offset: 0, + payload_len: 0, + }, + ); + store.load_rts_command( + 0, + RtsCommand { + delay_sec: 10, + command_code: 0x21, + payload_offset: 0, + payload_len: 0, + }, + ); assert!(store.start_rts(0, 0)); let r1 = store.process_tick(0); assert_eq!(r1.dispatch_count, 1); diff --git a/crates/relay-sch/plain/src/engine.rs b/crates/relay-sch/plain/src/engine.rs index ac2c40f2..7d26755a 100644 --- a/crates/relay-sch/plain/src/engine.rs +++ b/crates/relay-sch/plain/src/engine.rs @@ -35,49 +35,78 @@ pub struct TickResult { impl ScheduleSlot { pub const fn empty() -> Self { - ScheduleSlot { minor_frame: 0, major_frame: 0, target_channel: 0, payload_offset: 0, payload_len: 0, enabled: false } + ScheduleSlot { + minor_frame: 0, + major_frame: 0, + target_channel: 0, + payload_offset: 0, + payload_len: 0, + enabled: false, + } } } impl ScheduledAction { pub const fn empty() -> Self { - ScheduledAction { target_channel: 0, payload_offset: 0, payload_len: 0 } + ScheduledAction { + target_channel: 0, + payload_offset: 0, + payload_len: 0, + } } } impl ScheduleTable { pub fn new() -> Self { - ScheduleTable { slots: [ScheduleSlot::empty(); MAX_SCHEDULE_SLOTS], slot_count: 0 } + ScheduleTable { + slots: [ScheduleSlot::empty(); MAX_SCHEDULE_SLOTS], + slot_count: 0, + } } pub fn add_slot(&mut self, slot: ScheduleSlot) -> bool { - if self.slot_count as usize >= MAX_SCHEDULE_SLOTS { return false; } + if self.slot_count as usize >= MAX_SCHEDULE_SLOTS { + return false; + } self.slots[self.slot_count as usize] = slot; self.slot_count = self.slot_count + 1; true } pub fn set_enabled(&mut self, index: u32, enabled: bool) -> bool { - if index >= self.slot_count { return false; } + if index >= self.slot_count { + return false; + } self.slots[index as usize].enabled = enabled; true } - pub fn slot_count(&self) -> u32 { self.slot_count } + pub fn slot_count(&self) -> u32 { + self.slot_count + } pub fn process_tick(&self, current_minor: u32, current_major: u32) -> TickResult { - let mut result = TickResult { actions: [ScheduledAction::empty(); MAX_ACTIONS_PER_TICK], action_count: 0 }; + let mut result = TickResult { + actions: [ScheduledAction::empty(); MAX_ACTIONS_PER_TICK], + action_count: 0, + }; let count = self.slot_count; let mut i: u32 = 0; while i < count { - if result.action_count as usize >= MAX_ACTIONS_PER_TICK { break; } + if result.action_count as usize >= MAX_ACTIONS_PER_TICK { + break; + } let slot = self.slots[i as usize]; if slot.enabled { let minor_match = slot.minor_frame == current_minor; let major_match = slot.major_frame == 0 || slot.major_frame == current_major; if minor_match && major_match { let idx = result.action_count as usize; - result.actions[idx] = ScheduledAction { target_channel: slot.target_channel, payload_offset: slot.payload_offset, payload_len: slot.payload_len }; + result.actions[idx] = ScheduledAction { + target_channel: slot.target_channel, + payload_offset: slot.payload_offset, + payload_len: slot.payload_len, + }; result.action_count = result.action_count + 1; } } @@ -91,13 +120,93 @@ impl ScheduleTable { mod tests { use super::*; - #[test] fn test_empty() { assert_eq!(ScheduleTable::new().process_tick(0, 0).action_count, 0); } - #[test] fn test_match() { let mut t = ScheduleTable::new(); t.add_slot(ScheduleSlot { minor_frame: 5, major_frame: 0, target_channel: 42, payload_offset: 0, payload_len: 8, enabled: true }); assert_eq!(t.process_tick(5, 1).action_count, 1); assert_eq!(t.process_tick(6, 1).action_count, 0); } - #[test] fn test_disabled() { let mut t = ScheduleTable::new(); t.add_slot(ScheduleSlot { minor_frame: 0, major_frame: 0, target_channel: 1, payload_offset: 0, payload_len: 0, enabled: false }); assert_eq!(t.process_tick(0, 0).action_count, 0); } - #[test] fn test_major() { let mut t = ScheduleTable::new(); t.add_slot(ScheduleSlot { minor_frame: 0, major_frame: 3, target_channel: 10, payload_offset: 0, payload_len: 4, enabled: true }); assert_eq!(t.process_tick(0, 3).action_count, 1); assert_eq!(t.process_tick(0, 2).action_count, 0); } - #[test] fn test_bounded() { let mut t = ScheduleTable::new(); for ch in 0..(MAX_ACTIONS_PER_TICK as u32 + 10) { t.add_slot(ScheduleSlot { minor_frame: 0, major_frame: 0, target_channel: ch, payload_offset: 0, payload_len: 0, enabled: true }); } assert_eq!(t.process_tick(0, 0).action_count, MAX_ACTIONS_PER_TICK as u32); } - #[test] fn test_full() { let mut t = ScheduleTable::new(); for _ in 0..MAX_SCHEDULE_SLOTS { assert!(t.add_slot(ScheduleSlot::empty())); } assert!(!t.add_slot(ScheduleSlot::empty())); } - #[test] fn test_enable() { let mut t = ScheduleTable::new(); t.add_slot(ScheduleSlot { minor_frame: 0, major_frame: 0, target_channel: 1, payload_offset: 0, payload_len: 0, enabled: true }); assert_eq!(t.process_tick(0, 0).action_count, 1); t.set_enabled(0, false); assert_eq!(t.process_tick(0, 0).action_count, 0); assert!(!t.set_enabled(99, true)); } + #[test] + fn test_empty() { + assert_eq!(ScheduleTable::new().process_tick(0, 0).action_count, 0); + } + #[test] + fn test_match() { + let mut t = ScheduleTable::new(); + t.add_slot(ScheduleSlot { + minor_frame: 5, + major_frame: 0, + target_channel: 42, + payload_offset: 0, + payload_len: 8, + enabled: true, + }); + assert_eq!(t.process_tick(5, 1).action_count, 1); + assert_eq!(t.process_tick(6, 1).action_count, 0); + } + #[test] + fn test_disabled() { + let mut t = ScheduleTable::new(); + t.add_slot(ScheduleSlot { + minor_frame: 0, + major_frame: 0, + target_channel: 1, + payload_offset: 0, + payload_len: 0, + enabled: false, + }); + assert_eq!(t.process_tick(0, 0).action_count, 0); + } + #[test] + fn test_major() { + let mut t = ScheduleTable::new(); + t.add_slot(ScheduleSlot { + minor_frame: 0, + major_frame: 3, + target_channel: 10, + payload_offset: 0, + payload_len: 4, + enabled: true, + }); + assert_eq!(t.process_tick(0, 3).action_count, 1); + assert_eq!(t.process_tick(0, 2).action_count, 0); + } + #[test] + fn test_bounded() { + let mut t = ScheduleTable::new(); + for ch in 0..(MAX_ACTIONS_PER_TICK as u32 + 10) { + t.add_slot(ScheduleSlot { + minor_frame: 0, + major_frame: 0, + target_channel: ch, + payload_offset: 0, + payload_len: 0, + enabled: true, + }); + } + assert_eq!( + t.process_tick(0, 0).action_count, + MAX_ACTIONS_PER_TICK as u32 + ); + } + #[test] + fn test_full() { + let mut t = ScheduleTable::new(); + for _ in 0..MAX_SCHEDULE_SLOTS { + assert!(t.add_slot(ScheduleSlot::empty())); + } + assert!(!t.add_slot(ScheduleSlot::empty())); + } + #[test] + fn test_enable() { + let mut t = ScheduleTable::new(); + t.add_slot(ScheduleSlot { + minor_frame: 0, + major_frame: 0, + target_channel: 1, + payload_offset: 0, + payload_len: 0, + enabled: true, + }); + assert_eq!(t.process_tick(0, 0).action_count, 1); + t.set_enabled(0, false); + assert_eq!(t.process_tick(0, 0).action_count, 0); + assert!(!t.set_enabled(99, true)); + } } #[cfg(test)] diff --git a/crates/relay-sec/plain/src/ascon.rs b/crates/relay-sec/plain/src/ascon.rs index 4adb0442..0f583f56 100644 --- a/crates/relay-sec/plain/src/ascon.rs +++ b/crates/relay-sec/plain/src/ascon.rs @@ -108,7 +108,13 @@ fn p8(s: &mut [u64; 5]) { /// `ct` and returns the 128-bit tag. `mac` and `seal` are thin wrappers. /// /// Precondition: `ct.len() == pt.len()`. -fn aead_encrypt(key: &[u8; KEY_LEN], nonce: &[u8; NONCE_LEN], ad: &[u8], pt: &[u8], ct: &mut [u8]) -> [u8; TAG_LEN] { +fn aead_encrypt( + key: &[u8; KEY_LEN], + nonce: &[u8; NONCE_LEN], + ad: &[u8], + pt: &[u8], + ct: &mut [u8], +) -> [u8; TAG_LEN] { let k0 = load64(key, 0); let k1 = load64(key, 8); @@ -207,7 +213,12 @@ pub fn ct_eq_tag(a: &[u8; TAG_LEN], b: &[u8; TAG_LEN]) -> bool { } /// Verify a MAC-floor tag in constant time. -pub fn mac_verify(key: &[u8; KEY_LEN], nonce: &[u8; NONCE_LEN], msg: &[u8], tag: &[u8; TAG_LEN]) -> bool { +pub fn mac_verify( + key: &[u8; KEY_LEN], + nonce: &[u8; NONCE_LEN], + msg: &[u8], + tag: &[u8; TAG_LEN], +) -> bool { ct_eq_tag(&mac(key, nonce, msg), tag) } @@ -402,7 +413,14 @@ mod tests { let ct35 = hx::<1>("96"); let tag35 = hx::<16>("2B8016836C75A7D86866588CA245D886"); let mut pt2 = [0u8; 1]; - assert!(open(&key(), &nonce(), &hx::<1>("30"), &ct35, &tag35, &mut pt2)); + assert!(open( + &key(), + &nonce(), + &hx::<1>("30"), + &ct35, + &tag35, + &mut pt2 + )); assert_eq!(pt2, hx::<1>("20")); } @@ -420,14 +438,35 @@ mod tests { let mut ct = [0u8; 40]; let tag = seal(&k, &n, &[0xAA, 0xBB], &pt[..len], &mut ct[..len]); let mut out = [0u8; 40]; - assert!(open(&k, &n, &[0xAA, 0xBB], &ct[..len], &tag, &mut out[..len])); + assert!(open( + &k, + &n, + &[0xAA, 0xBB], + &ct[..len], + &tag, + &mut out[..len] + )); assert_eq!(&out[..len], &pt[..len]); // tampered tag rejects let mut bad = tag; bad[0] ^= 1; - assert!(!open(&k, &n, &[0xAA, 0xBB], &ct[..len], &bad, &mut out[..len])); + assert!(!open( + &k, + &n, + &[0xAA, 0xBB], + &ct[..len], + &bad, + &mut out[..len] + )); // tampered AD rejects - assert!(!open(&k, &n, &[0xAA, 0xCC], &ct[..len], &tag, &mut out[..len])); + assert!(!open( + &k, + &n, + &[0xAA, 0xCC], + &ct[..len], + &tag, + &mut out[..len] + )); } } } diff --git a/crates/relay-sec/plain/src/frame.rs b/crates/relay-sec/plain/src/frame.rs index 44d8fd02..abfc0d37 100644 --- a/crates/relay-sec/plain/src/frame.rs +++ b/crates/relay-sec/plain/src/frame.rs @@ -21,7 +21,7 @@ //! tampered tag → BadMac, wrong SPI → UnknownSpi, replay → Replay. use crate::ascon::{self, KEY_LEN, TAG_LEN}; -use crate::header::{SecurityHeader, SEC_HEADER_LEN}; +use crate::header::{SEC_HEADER_LEN, SecurityHeader}; use crate::{ReplayVerdict, ReplayWindow}; /// Smallest possible frame: header + empty payload + tag. @@ -126,7 +126,13 @@ impl SecurityChannel { Confidentiality::Aead => { // encrypt payload in place; AD = header, PT = payload. let (header, rest) = out.split_at_mut(SEC_HEADER_LEN); - ascon::seal(&self.key, &nonce, header, payload, &mut rest[..payload.len()]) + ascon::seal( + &self.key, + &nonce, + header, + payload, + &mut rest[..payload.len()], + ) } }; out[body_end..frame_len].copy_from_slice(&tag); @@ -364,7 +370,10 @@ mod tests { let n = tx.wrap(b"unlock", &mut buf).unwrap(); buf[SEC_HEADER_LEN] ^= 0xFF; // flip a ciphertext byte let mut out = [0u8; 64]; - assert_eq!(rx.verify_into(&buf[..n], &mut out), Err(VerifyError::BadMac)); + assert_eq!( + rx.verify_into(&buf[..n], &mut out), + Err(VerifyError::BadMac) + ); } #[test] diff --git a/crates/relay-sec/plain/src/header.rs b/crates/relay-sec/plain/src/header.rs index c271934f..3e756321 100644 --- a/crates/relay-sec/plain/src/header.rs +++ b/crates/relay-sec/plain/src/header.rs @@ -96,14 +96,22 @@ mod tests { #[test] fn write_too_short_is_false() { - let h = SecurityHeader { spi: 1, channel_id: 1, counter: 1 }; + let h = SecurityHeader { + spi: 1, + channel_id: 1, + counter: 1, + }; let mut tiny = [0u8; SEC_HEADER_LEN - 1]; assert!(!h.write(&mut tiny)); } #[test] fn nonce_embeds_fields() { - let h = SecurityHeader { spi: 0x0201, channel_id: 3, counter: 0x0A09_0807_0605_0403 }; + let h = SecurityHeader { + spi: 0x0201, + channel_id: 3, + counter: 0x0A09_0807_0605_0403, + }; let n = h.nonce(); assert_eq!(&n[0..2], &0x0201u16.to_le_bytes()); assert_eq!(n[2], 3); @@ -113,9 +121,24 @@ mod tests { #[test] fn distinct_headers_distinct_nonces() { - let a = SecurityHeader { spi: 1, channel_id: 0, counter: 5 }.nonce(); - let b = SecurityHeader { spi: 1, channel_id: 1, counter: 5 }.nonce(); // channel differs - let c = SecurityHeader { spi: 1, channel_id: 0, counter: 6 }.nonce(); // counter differs + let a = SecurityHeader { + spi: 1, + channel_id: 0, + counter: 5, + } + .nonce(); + let b = SecurityHeader { + spi: 1, + channel_id: 1, + counter: 5, + } + .nonce(); // channel differs + let c = SecurityHeader { + spi: 1, + channel_id: 0, + counter: 6, + } + .nonce(); // counter differs assert_ne!(a, b); assert_ne!(a, c); } diff --git a/crates/relay-sec/plain/src/kani_proofs.rs b/crates/relay-sec/plain/src/kani_proofs.rs index d0342ff8..ab7b7e34 100644 --- a/crates/relay-sec/plain/src/kani_proofs.rs +++ b/crates/relay-sec/plain/src/kani_proofs.rs @@ -88,8 +88,8 @@ fn verify_ascon_mac_total() { let _ = ascon::mac(&key, &nonce, &buf[..len]); } -use crate::frame::{SecurityChannel, MIN_FRAME_LEN}; -use crate::header::{SecurityHeader, SEC_HEADER_LEN}; +use crate::frame::{MIN_FRAME_LEN, SecurityChannel}; +use crate::header::{SEC_HEADER_LEN, SecurityHeader}; /// SEC-K07 — the Security-Header parser is total: any buffer (any bytes, any /// length) yields Some/None, never a panic. diff --git a/crates/relay-sensvote/plain/src/lib.rs b/crates/relay-sensvote/plain/src/lib.rs index 08ee14d2..6d2ffbc0 100644 --- a/crates/relay-sensvote/plain/src/lib.rs +++ b/crates/relay-sensvote/plain/src/lib.rs @@ -108,7 +108,11 @@ pub struct GpsFreshness { impl GpsFreshness { /// Declares the fix stale after `timeout_us` without an update. pub fn new(timeout_us: u64) -> Self { - Self { timeout_us, last_fix_us: 0, started: false } + Self { + timeout_us, + last_fix_us: 0, + started: false, + } } /// Record a fresh GPS fix observed at `now_us`. diff --git a/crates/relay-to/plain/src/engine.rs b/crates/relay-to/plain/src/engine.rs index 29287dd2..75a5cf0c 100644 --- a/crates/relay-to/plain/src/engine.rs +++ b/crates/relay-to/plain/src/engine.rs @@ -25,7 +25,11 @@ pub struct SubscriptionTable { impl Subscription { pub const fn empty() -> Self { - Subscription { msg_id: 0, priority: 0, enabled: false } + Subscription { + msg_id: 0, + priority: 0, + enabled: false, + } } } @@ -43,7 +47,11 @@ impl SubscriptionTable { return false; } let idx = self.entry_count as usize; - self.entries[idx] = Subscription { msg_id, priority, enabled: true }; + self.entries[idx] = Subscription { + msg_id, + priority, + enabled: true, + }; self.entry_count = self.entry_count + 1; true } diff --git a/crates/relay-traj/plain/src/lib.rs b/crates/relay-traj/plain/src/lib.rs index 145f55d5..315d1064 100644 --- a/crates/relay-traj/plain/src/lib.rs +++ b/crates/relay-traj/plain/src/lib.rs @@ -43,11 +43,7 @@ pub struct Sample { #[inline] fn fin(x: f32, fallback: f32) -> f32 { - if x.is_finite() { - x - } else { - fallback - } + if x.is_finite() { x } else { fallback } } impl Quintic { @@ -88,7 +84,11 @@ impl Quintic { /// Evaluate position/velocity/acceleration/jerk at time `t` (clamped to /// `[0, T]` so a sample is always on the segment). pub fn eval(&self, t: f32) -> Sample { - let t = if t.is_finite() { t.clamp(0.0, self.t_end) } else { 0.0 }; + let t = if t.is_finite() { + t.clamp(0.0, self.t_end) + } else { + 0.0 + }; let (c5, c4, c3, c2, c1, c0) = (self.c5, self.c4, self.c3, self.c2, self.c1, self.c0); // Horner for p; analytic derivatives. let p = ((((c5 * t + c4) * t + c3) * t + c2) * t + c1) * t + c0; @@ -168,7 +168,11 @@ impl Segment3 { /// Sample the trajectory at time `t` (clamped to `[0, T]`). pub fn eval(&self, t: f32) -> Sample3 { - let s = [self.axes[0].eval(t), self.axes[1].eval(t), self.axes[2].eval(t)]; + let s = [ + self.axes[0].eval(t), + self.axes[1].eval(t), + self.axes[2].eval(t), + ]; Sample3 { pos: [s[0].p, s[1].p, s[2].p], vel: [s[0].v, s[1].v, s[2].v], @@ -244,7 +248,12 @@ impl RefGovernor { /// ramps 1 → `g_min`. `g_min` ∈ (0, 1]: the slowest advance fraction /// (> 0 so the mission never permanently deadlocks). pub fn new(err_lo: f32, err_hi: f32, g_min: f32) -> Self { - RefGovernor { s: 0.0, err_lo, err_hi, g_min } + RefGovernor { + s: 0.0, + err_lo, + err_hi, + g_min, + } } /// The error-gate factor g ∈ [g_min, 1] for a tracking error. @@ -311,7 +320,15 @@ mod kani_proofs { kani::assume(c4.is_finite() && relay_math::fabsf(c4) <= 1e3); kani::assume(c3.is_finite() && relay_math::fabsf(c3) <= 1e3); kani::assume(t_end.is_finite() && t_end > 1e-3 && t_end <= 100.0); - let q = Quintic { c5, c4, c3, c2: 0.0, c1: 0.0, c0: 0.0, t_end }; + let q = Quintic { + c5, + c4, + c3, + c2: 0.0, + c1: 0.0, + c0: 0.0, + t_end, + }; let peak = q.peak_abs_jerk(); let t: f32 = kani::any(); @@ -404,7 +421,11 @@ mod tests { for _ in 0..500 { on.advance(0.0, dt); } - assert!((on.time() - 10.0).abs() < 1e-3, "on-track ≈ wall time, {}", on.time()); + assert!( + (on.time() - 10.0).abs() < 1e-3, + "on-track ≈ wall time, {}", + on.time() + ); // Persistently behind (error past hi): advances at g_min rate only. let mut behind = RefGovernor::new(0.3, 1.5, 0.05); let mut prev = 0.0; @@ -414,8 +435,15 @@ mod tests { assert!(s - prev <= dt + 1e-6, "rate ≤ dt"); // never faster than nominal prev = s; } - assert!((behind.time() - 0.05 * 10.0).abs() < 1e-2, "behind ≈ g_min·wall, {}", behind.time()); - assert!(behind.time() < on.time(), "governed clock lags the wall clock when behind"); + assert!( + (behind.time() - 0.05 * 10.0).abs() < 1e-2, + "behind ≈ g_min·wall, {}", + behind.time() + ); + assert!( + behind.time() < on.time(), + "governed clock lags the wall clock when behind" + ); } proptest::proptest! { @@ -467,10 +495,21 @@ mod tests { let sT = seg.eval(4.0); for i in 0..3 { let want = [3.0, -2.0, 1.5][i]; - assert!((sT.pos[i] - want).abs() < 1e-2, "axis {i} pos {} vs {want}", sT.pos[i]); - assert!(sT.vel[i].abs() < 1e-2 && sT.acc[i].abs() < 1e-2, "axis {i} not at rest"); + assert!( + (sT.pos[i] - want).abs() < 1e-2, + "axis {i} pos {} vs {want}", + sT.pos[i] + ); + assert!( + sT.vel[i].abs() < 1e-2 && sT.acc[i].abs() < 1e-2, + "axis {i} not at rest" + ); } - assert!(seg.peak_abs_jerk().iter().all(|&j| j.is_finite() && j >= 0.0)); + assert!( + seg.peak_abs_jerk() + .iter() + .all(|&j| j.is_finite() && j >= 0.0) + ); } /// Total: finite samples + finite peak for adversarial inputs. diff --git a/examples/falcon-ekf-bench/src/main.rs b/examples/falcon-ekf-bench/src/main.rs index fde2f8b3..b4b8af38 100644 --- a/examples/falcon-ekf-bench/src/main.rs +++ b/examples/falcon-ekf-bench/src/main.rs @@ -45,7 +45,7 @@ use std::process::ExitCode; use std::time::Instant; use libm::{cosf, sinf, sqrtf}; -use relay_ekf::{quat_mul, Ekf, ImuSample, Timestamp}; +use relay_ekf::{Ekf, ImuSample, Timestamp, quat_mul}; const SAMPLE_RATE_HZ: f32 = 200.0; const TRAJECTORY_SECONDS: f32 = 25.0; @@ -53,11 +53,11 @@ const GRAVITY: f32 = 9.81; /// Phase boundaries and gyro signals for the test trajectory. const PHASES: &[(f32, f32, [f32; 3])] = &[ - (0.0, 5.0, [0.0, 0.0, 0.0]), // rest at 20° pitch - (5.0, 10.0, [0.3, 0.0, 0.0]), // roll right - (10.0, 15.0, [0.0, 0.0, 0.0]), // rest - (15.0, 20.0, [0.0, 0.0, 0.5]), // yaw left - (20.0, 25.0, [0.0, 0.0, 0.0]), // rest + (0.0, 5.0, [0.0, 0.0, 0.0]), // rest at 20° pitch + (5.0, 10.0, [0.3, 0.0, 0.0]), // roll right + (10.0, 15.0, [0.0, 0.0, 0.0]), // rest + (15.0, 20.0, [0.0, 0.0, 0.5]), // yaw left + (20.0, 25.0, [0.0, 0.0, 0.0]), // rest ]; /// Initial true attitude: 20° pitch (about body-y). @@ -96,8 +96,7 @@ fn integrate_truth(q: [f32; 4], omega_body: [f32; 3], dt: f32) -> [f32; 4] { q[3] + 0.5 * qdot[3] * dt, ]; let n = sqrtf( - q_new[0] * q_new[0] + q_new[1] * q_new[1] - + q_new[2] * q_new[2] + q_new[3] * q_new[3], + q_new[0] * q_new[0] + q_new[1] * q_new[1] + q_new[2] * q_new[2] + q_new[3] * q_new[3], ); if n < 1.0e-12 { q @@ -120,7 +119,7 @@ struct BenchResult { rms_error_deg_steady: f32, // last 2.5 s peak_error_deg: f32, final_error_deg: f32, - convergence_time_s: f32, // first time error drops below 5° and stays there + convergence_time_s: f32, // first time error drops below 5° and stays there elapsed_micros: u128, nan_seen: bool, } @@ -173,7 +172,10 @@ fn run_bench(noise_std: f32) -> BenchResult { ]; let frac = ((t.fract() as f64) * ((1u64 << 32) as f64)) as u32; let sample = ImuSample { - time: Timestamp { seconds: t as u64, fraction: frac }, + time: Timestamp { + seconds: t as u64, + fraction: frac, + }, accel_body: accel_meas, gyro_body: gyro_meas, }; @@ -218,13 +220,19 @@ fn print_result(label: &str, r: &BenchResult) { println!("--- {label} ---"); println!(" samples {}", r.samples); println!(" RMS error (full) {:.3}°", r.rms_error_deg_full); - println!(" RMS error (steady) {:.3}° (last 2.5 s)", r.rms_error_deg_steady); + println!( + " RMS error (steady) {:.3}° (last 2.5 s)", + r.rms_error_deg_steady + ); println!(" peak error {:.3}°", r.peak_error_deg); println!(" final error {:.3}°", r.final_error_deg); if r.convergence_time_s.is_nan() { println!(" convergence time never"); } else { - println!(" convergence time {:.2}s (first sustained <5°)", r.convergence_time_s); + println!( + " convergence time {:.2}s (first sustained <5°)", + r.convergence_time_s + ); } println!(" estimator wall time {} µs", r.elapsed_micros); println!(" NaN/∞ seen {}", r.nan_seen); @@ -292,9 +300,8 @@ fn main() -> ExitCode { // filter cannot observe heading from gravity, so residual yaw // drift after the trajectory's yaw phase is fundamental until // v0.4 wires magnetometer fusion. - let pass_clean = clean.rms_error_deg_steady <= 5.0 - && clean.final_error_deg <= 5.0 - && !clean.nan_seen; + let pass_clean = + clean.rms_error_deg_steady <= 5.0 && clean.final_error_deg <= 5.0 && !clean.nan_seen; let pass_noisy = noisy.as_ref().map_or(true, |r| { r.rms_error_deg_steady <= 8.0 // looser tolerance with noise && r.final_error_deg <= 8.0 @@ -325,11 +332,13 @@ mod tests { assert!(!r.nan_seen); assert!( r.rms_error_deg_steady <= 5.0, - "RMS steady error {}° exceeds 5° budget", r.rms_error_deg_steady + "RMS steady error {}° exceeds 5° budget", + r.rms_error_deg_steady ); assert!( r.final_error_deg <= 5.0, - "final error {}° exceeds 5° budget", r.final_error_deg + "final error {}° exceeds 5° budget", + r.final_error_deg ); } diff --git a/examples/falcon-hello/src/main.rs b/examples/falcon-hello/src/main.rs index 8eb3317b..bca50e11 100644 --- a/examples/falcon-hello/src/main.rs +++ b/examples/falcon-hello/src/main.rs @@ -22,8 +22,8 @@ use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use relay_ekf_stub::{EkfStub, Timestamp}; use relay_mavlink::{ - encode_frame, parse_frame, peek_message_id, CodecError, Frame, FrameHeader, Heartbeat, - HEARTBEAT_CRC_EXTRA, HEARTBEAT_MSG_ID, HEARTBEAT_PAYLOAD_LEN, MAGIC_V2, MAX_FRAME_SIZE, + CodecError, Frame, FrameHeader, HEARTBEAT_CRC_EXTRA, HEARTBEAT_MSG_ID, HEARTBEAT_PAYLOAD_LEN, + Heartbeat, MAGIC_V2, MAX_FRAME_SIZE, encode_frame, parse_frame, peek_message_id, }; const DEFAULT_PORT: u16 = 14550; @@ -170,7 +170,10 @@ fn run_vehicle(args: &Args) -> Result<(), String> { let mut next_send = Instant::now(); let mut buf = [0u8; MAX_FRAME_SIZE]; - eprintln!("vehicle: emitting heartbeats at {} Hz → {}", args.rate_hz, args.remote); + eprintln!( + "vehicle: emitting heartbeats at {} Hz → {}", + args.rate_hz, args.remote + ); loop { if let Some(d) = args.duration { @@ -294,7 +297,9 @@ mod tests { let vehicle_bind: SocketAddr = "127.0.0.1:0".parse().unwrap(); let gcs_sock = UdpSocket::bind(gcs_bind).expect("gcs bind"); let vehicle_sock = UdpSocket::bind(vehicle_bind).expect("vehicle bind"); - gcs_sock.set_read_timeout(Some(Duration::from_secs(2))).unwrap(); + gcs_sock + .set_read_timeout(Some(Duration::from_secs(2))) + .unwrap(); let gcs_addr = gcs_sock.local_addr().unwrap(); let vehicle_addr = vehicle_sock.local_addr().unwrap(); @@ -396,13 +401,14 @@ mod tests { thread::sleep(Duration::from_millis(2)); let t2 = current_timestamp(); // Either seconds incremented, or fraction did. - assert!(t2.seconds > t1.seconds - || (t2.seconds == t1.seconds && t2.fraction > t1.fraction)); + assert!(t2.seconds > t1.seconds || (t2.seconds == t1.seconds && t2.fraction > t1.fraction)); } #[test] fn args_default_ports_for_vehicle_mode() { - let argv = ["falcon-hello", "--mode", "vehicle"].iter().map(|s| s.to_string()); + let argv = ["falcon-hello", "--mode", "vehicle"] + .iter() + .map(|s| s.to_string()); let args = Args::parse(argv).expect("parse"); assert_eq!(args.mode, Mode::Vehicle); assert_eq!(args.bind.port(), DEFAULT_PORT + 1); @@ -411,7 +417,9 @@ mod tests { #[test] fn args_default_ports_for_gcs_mode() { - let argv = ["falcon-hello", "--mode", "gcs"].iter().map(|s| s.to_string()); + let argv = ["falcon-hello", "--mode", "gcs"] + .iter() + .map(|s| s.to_string()); let args = Args::parse(argv).expect("parse"); assert_eq!(args.mode, Mode::Gcs); assert_eq!(args.bind.port(), DEFAULT_PORT); @@ -420,7 +428,9 @@ mod tests { #[test] fn args_rejects_unknown_mode() { - let argv = ["falcon-hello", "--mode", "spy"].iter().map(|s| s.to_string()); + let argv = ["falcon-hello", "--mode", "spy"] + .iter() + .map(|s| s.to_string()); let err = Args::parse(argv).unwrap_err(); assert!(err.contains("unknown --mode")); } diff --git a/examples/falcon-hitl-rfspoof/src/hackrf.rs b/examples/falcon-hitl-rfspoof/src/hackrf.rs index e9cafbe9..9d0739eb 100644 --- a/examples/falcon-hitl-rfspoof/src/hackrf.rs +++ b/examples/falcon-hitl-rfspoof/src/hackrf.rs @@ -78,10 +78,14 @@ impl HackRfConfig { pub fn hackrf_transfer_argv(&self) -> [String; 9] { [ "hackrf_transfer".into(), - "-t".into(), self.iq_path.clone(), - "-f".into(), self.freq_hz.to_string(), - "-s".into(), self.sample_rate_hz.to_string(), - "-x".into(), self.gain_db.to_string(), + "-t".into(), + self.iq_path.clone(), + "-f".into(), + self.freq_hz.to_string(), + "-s".into(), + self.sample_rate_hz.to_string(), + "-x".into(), + self.gain_db.to_string(), ] } } @@ -105,10 +109,17 @@ impl GpsSdrSimConfig { pub fn argv(&self) -> [String; 9] { [ "gps-sdr-sim".into(), - "-e".into(), self.nav_path.clone(), - "-l".into(), format!("{},{},{}", self.spoof_lat_deg, self.spoof_lon_deg, self.spoof_alt_m), - "-o".into(), self.out_iq_path.clone(), - "-d".into(), self.duration_s.to_string(), + "-e".into(), + self.nav_path.clone(), + "-l".into(), + format!( + "{},{},{}", + self.spoof_lat_deg, self.spoof_lon_deg, self.spoof_alt_m + ), + "-o".into(), + self.out_iq_path.clone(), + "-d".into(), + self.duration_s.to_string(), ] } } @@ -156,7 +167,9 @@ impl HackRfBench { } impl HitlBench for HackRfBench { - fn name(&self) -> &'static str { self.label } + fn name(&self) -> &'static str { + self.label + } fn step(&mut self, dt: f32) { self.t += dt; diff --git a/examples/falcon-hitl-rfspoof/src/harness.rs b/examples/falcon-hitl-rfspoof/src/harness.rs index c1bd4c10..c306d3b6 100644 --- a/examples/falcon-hitl-rfspoof/src/harness.rs +++ b/examples/falcon-hitl-rfspoof/src/harness.rs @@ -52,7 +52,9 @@ pub trait HitlBench { /// 60-tick test in microseconds" contract for `StubBench` /// and `InMemoryFrameSource`-backed `MavlinkBench` tests. /// `UdpFrameSource`-backed `MavlinkBench` overrides to true. - fn real_time(&self) -> bool { false } + fn real_time(&self) -> bool { + false + } /// `true` iff the RF spoofer is transmitting for this step. /// The harness uses this only for diagnostic correlation — @@ -82,7 +84,9 @@ pub struct NullCommandSink { } impl NullCommandSink { - pub fn new() -> Self { Self { frames_sent: 0 } } + pub fn new() -> Self { + Self { frames_sent: 0 } + } } impl CommandSink for NullCommandSink { @@ -90,7 +94,9 @@ impl CommandSink for NullCommandSink { self.frames_sent += 1; Ok(()) } - fn name(&self) -> &'static str { "null" } + fn name(&self) -> &'static str { + "null" + } } /// Outcome of one HITL run — what an evidence reviewer reads. @@ -127,8 +133,8 @@ impl HitlVerdict { /// `target_component` are PX4 defaults (1/1). fn build_rtl_frame(seq: u8) -> Vec { use relay_mavlink::{ - encode_frame, FrameHeader, COMMAND_LONG_CRC_EXTRA, COMMAND_LONG_MSG_ID, - COMMAND_LONG_PAYLOAD_LEN, HEADER_LEN, MAGIC_V2, + COMMAND_LONG_CRC_EXTRA, COMMAND_LONG_MSG_ID, COMMAND_LONG_PAYLOAD_LEN, FrameHeader, + HEADER_LEN, MAGIC_V2, encode_frame, }; let cmd = CommandLong::rtl(1, 1); debug_assert_eq!(cmd.command, MAV_CMD_NAV_RETURN_TO_LAUNCH); @@ -139,7 +145,7 @@ fn build_rtl_frame(seq: u8) -> Vec { incompat_flags: 0, compat_flags: 0, sequence: seq, - system_id: 255, // GCS-style sender id + system_id: 255, // GCS-style sender id component_id: 190, message_id: COMMAND_LONG_MSG_ID, }; diff --git a/examples/falcon-hitl-rfspoof/src/main.rs b/examples/falcon-hitl-rfspoof/src/main.rs index 716d357e..2fae4078 100644 --- a/examples/falcon-hitl-rfspoof/src/main.rs +++ b/examples/falcon-hitl-rfspoof/src/main.rs @@ -19,10 +19,10 @@ mod hackrf; mod harness; -mod stub; pub mod mavlink; +mod stub; -use harness::{load_rtl_rts, run_scenario, CommandSink, NullCommandSink}; +use harness::{CommandSink, NullCommandSink, load_rtl_rts, run_scenario}; use relay_lc::engine::Geofence; use relay_sc::engine::CommandStore; @@ -70,13 +70,17 @@ fn main() { "stub" => { let mut b = stub::StubBench::new(0, 0, -500, 0, 20_000, -500, 2.0); let mut sink = NullCommandSink::new(); - run_scenario(&mut b, &mut fence, &mut sc, &mut sink, 0.01, duration_s, 0, 1.0) + run_scenario( + &mut b, &mut fence, &mut sc, &mut sink, 0.01, duration_s, 0, 1.0, + ) } "hackrf" => { // 200 m east of the fence boundary — well outside. let mut b = hackrf::HackRfBench::new(2.0, 0, 0, -500, 0, 20_000, -500); let mut sink = NullCommandSink::new(); - run_scenario(&mut b, &mut fence, &mut sc, &mut sink, 0.01, duration_s, 0, 1.0) + run_scenario( + &mut b, &mut fence, &mut sc, &mut sink, 0.01, duration_s, 0, 1.0, + ) } "mavlink" => { // Bind UDP to whatever port the FC sends to (PX4 default 14550). @@ -84,14 +88,19 @@ fn main() { .or_else(|| defaults.listen.map(String::from)) .unwrap_or_else(|| "0.0.0.0:14550".into()); let sock = std::net::UdpSocket::bind(&bind_addr).unwrap_or_else(|e| { - eprintln!("could not bind {bind_addr}: {e}"); std::process::exit(3); + eprintln!("could not bind {bind_addr}: {e}"); + std::process::exit(3); }); sock.set_nonblocking(true).expect("set_nonblocking"); println!(" mavlink: listening on {bind_addr}"); // Default home = Budapest centre — override with --home=lat,lon,alt_m. let home = match arg(&args, "--home").or_else(|| defaults.home.map(String::from)) { Some(s) => parse_home(&s).expect("--home=lat,lon,alt_m"), - None => mavlink::Home { lat_e7: 475_023_456, lon_e7: 190_401_234, alt_mm: 120_000 }, + None => mavlink::Home { + lat_e7: 475_023_456, + lon_e7: 190_401_234, + alt_mm: 120_000, + }, }; // v0.14.2 round-trip: when the harness latches RTL it // pushes a COMMAND_LONG back to the FC. --peer= picks @@ -103,12 +112,14 @@ fn main() { let mut sink: Box = match peer_str.parse() { Ok(peer) => { println!(" mavlink: COMMAND_LONG sink → {peer_str}"); - let send_sock = std::net::UdpSocket::bind("0.0.0.0:0") - .expect("bind sink socket"); + let send_sock = + std::net::UdpSocket::bind("0.0.0.0:0").expect("bind sink socket"); Box::new(mavlink::UdpCommandSink::new(send_sock, peer)) } Err(_) => { - eprintln!("warning: --peer={peer_str} is not a valid socket address; using null sink"); + eprintln!( + "warning: --peer={peer_str} is not a valid socket address; using null sink" + ); Box::new(NullCommandSink::new()) } }; @@ -137,7 +148,16 @@ fn main() { // Use the full duration as the budget so the heuristic // fail-stop is effectively disabled in live mode; the // verdict's pass() still drives the exit code. - let v = run_scenario(&mut b, &mut fence, &mut sc, sink.as_mut(), 0.01, duration_s, 0, duration_s); + let v = run_scenario( + &mut b, + &mut fence, + &mut sc, + sink.as_mut(), + 0.01, + duration_s, + 0, + duration_s, + ); // Diagnostic counters — let a bench operator distinguish // "PX4 isn't sending us anything" (frames_recv == 0) from // "PX4 sends MAVLink but no GLOBAL_POSITION_INT yet" @@ -196,7 +216,9 @@ fn arg(args: &[String], key: &str) -> Option { fn parse_home(s: &str) -> Option { let parts: Vec<&str> = s.split(',').collect(); - if parts.len() != 3 { return None; } + if parts.len() != 3 { + return None; + } let lat: f64 = parts[0].parse().ok()?; let lon: f64 = parts[1].parse().ok()?; let alt_m: f64 = parts[2].parse().ok()?; diff --git a/examples/falcon-hitl-rfspoof/src/mavlink.rs b/examples/falcon-hitl-rfspoof/src/mavlink.rs index fd0163a4..21bc01a7 100644 --- a/examples/falcon-hitl-rfspoof/src/mavlink.rs +++ b/examples/falcon-hitl-rfspoof/src/mavlink.rs @@ -35,9 +35,8 @@ use crate::harness::{CommandSink, HitlBench}; use relay_mavlink::{ - parse_frame, peek_message_id, GlobalPositionInt, - GLOBAL_POSITION_INT_CRC_EXTRA, GLOBAL_POSITION_INT_MSG_ID, - GLOBAL_POSITION_INT_PAYLOAD_LEN, + GLOBAL_POSITION_INT_CRC_EXTRA, GLOBAL_POSITION_INT_MSG_ID, GLOBAL_POSITION_INT_PAYLOAD_LEN, + GlobalPositionInt, parse_frame, peek_message_id, }; use std::net::{SocketAddr, UdpSocket}; @@ -92,7 +91,9 @@ pub trait FrameSource { /// actually deliver them). `UdpFrameSource` overrides to true; /// `InMemoryFrameSource` keeps the default false so unit tests /// stay fast. - fn is_realtime(&self) -> bool { false } + fn is_realtime(&self) -> bool { + false + } } /// In-memory `FrameSource` — for tests + the deterministic backend. @@ -109,7 +110,9 @@ impl InMemoryFrameSource { } impl FrameSource for InMemoryFrameSource { - fn name(&self) -> &'static str { "mem" } + fn name(&self) -> &'static str { + "mem" + } fn next_frame(&mut self) -> Option<&[u8]> { if self.cursor >= self.frames.len() { return None; @@ -202,10 +205,12 @@ impl UdpFrameSource { /// HEARTBEAT just means the next call retries. fn send_heartbeat(&mut self) { use relay_mavlink::{ - encode_frame, FrameHeader, Heartbeat, HEADER_LEN, - HEARTBEAT_CRC_EXTRA, HEARTBEAT_MSG_ID, HEARTBEAT_PAYLOAD_LEN, MAGIC_V2, + FrameHeader, HEADER_LEN, HEARTBEAT_CRC_EXTRA, HEARTBEAT_MSG_ID, HEARTBEAT_PAYLOAD_LEN, + Heartbeat, MAGIC_V2, encode_frame, + }; + let Some(peer) = self.register_peer else { + return; }; - let Some(peer) = self.register_peer else { return }; let payload = Heartbeat::gcs().encode_payload(); let header = FrameHeader { magic: MAGIC_V2, @@ -228,8 +233,12 @@ impl UdpFrameSource { } impl FrameSource for UdpFrameSource { - fn name(&self) -> &'static str { "udp" } - fn is_realtime(&self) -> bool { true } + fn name(&self) -> &'static str { + "udp" + } + fn is_realtime(&self) -> bool { + true + } fn next_frame(&mut self) -> Option<&[u8]> { // Keep the registration alive by sending a HEARTBEAT every // second when a peer is configured. PX4-SITL forgets peers @@ -269,7 +278,9 @@ impl UdpCommandSink { } impl CommandSink for UdpCommandSink { - fn name(&self) -> &'static str { "udp" } + fn name(&self) -> &'static str { + "udp" + } fn send_frame(&mut self, bytes: &[u8]) -> Result<(), &'static str> { self.sock .send_to(bytes, self.peer) @@ -370,7 +381,9 @@ impl MavlinkBench { } impl HitlBench for MavlinkBench { - fn name(&self) -> &'static str { "mavlink" } + fn name(&self) -> &'static str { + "mavlink" + } fn step(&mut self, dt: f32) { self.t += dt; self.drain_frames(); @@ -378,14 +391,18 @@ impl HitlBench for MavlinkBench { fn position_cm(&self) -> (i32, i32, i32) { (self.last_n_cm, self.last_e_cm, self.last_d_cm) } - fn real_time(&self) -> bool { self.source.is_realtime() } - fn spoof_active(&self) -> bool { self.spoof_active } + fn real_time(&self) -> bool { + self.source.is_realtime() + } + fn spoof_active(&self) -> bool { + self.spoof_active + } } /// Build a MAVLink v2 frame carrying a GLOBAL_POSITION_INT payload. /// Used by tests + by bench tooling to script trajectories. pub fn build_global_position_frame(seq: u8, msg: &GlobalPositionInt) -> Vec { - use relay_mavlink::{encode_frame, FrameHeader, GLOBAL_POSITION_INT_CRC_EXTRA, MAGIC_V2}; + use relay_mavlink::{FrameHeader, GLOBAL_POSITION_INT_CRC_EXTRA, MAGIC_V2, encode_frame}; let payload = msg.encode_payload(); let header = FrameHeader { magic: MAGIC_V2, @@ -407,13 +424,17 @@ pub fn build_global_position_frame(seq: u8, msg: &GlobalPositionInt) -> Vec #[cfg(test)] mod tests { use super::*; - use crate::harness::{load_rtl_rts, run_scenario, NullCommandSink}; + use crate::harness::{NullCommandSink, load_rtl_rts, run_scenario}; use relay_lc::engine::Geofence; use relay_sc::engine::CommandStore; fn budapest_home() -> Home { // ~Budapest centre. - Home { lat_e7: 475_023_456, lon_e7: 190_401_234, alt_mm: 120_000 } + Home { + lat_e7: 475_023_456, + lon_e7: 190_401_234, + alt_mm: 120_000, + } } fn pos_at(home: Home, n_cm: i32, e_cm: i32, d_cm: i32) -> GlobalPositionInt { @@ -429,9 +450,13 @@ mod tests { let alt_mm = home.alt_mm + ((-d_cm as f64) * 10.0) as i32; GlobalPositionInt { time_boot_ms: 0, - lat_e7, lon_e7, alt_mm, + lat_e7, + lon_e7, + alt_mm, relative_alt_mm: -d_cm * 10, - vx_cms: 0, vy_cms: 0, vz_cms: 0, + vx_cms: 0, + vy_cms: 0, + vz_cms: 0, hdg_cdeg: 0, } } @@ -470,8 +495,8 @@ mod tests { fn mavlink_bench_trips_and_dispatches_on_spoof() { let home = budapest_home(); let frames: Vec> = vec![ - build_global_position_frame(0, &pos_at(home, 0, 0, -500)), // t=0 in-fence - build_global_position_frame(1, &pos_at(home, 0, 0, -500)), // t=1 in-fence + build_global_position_frame(0, &pos_at(home, 0, 0, -500)), // t=0 in-fence + build_global_position_frame(1, &pos_at(home, 0, 0, -500)), // t=1 in-fence build_global_position_frame(2, &pos_at(home, 0, 20_000, -500)), // t=2+ outside ]; let src = InMemoryFrameSource::new(frames); @@ -508,7 +533,9 @@ mod tests { load_rtl_rts(&mut sc, 0, 0xA17C); let mut sink = NullCommandSink::new(); - let v = run_scenario(&mut bench, &mut fence, &mut sc, &mut sink, 1.0, 5.0, 0, 10.0); + let v = run_scenario( + &mut bench, &mut fence, &mut sc, &mut sink, 1.0, 5.0, 0, 10.0, + ); assert!(!v.latched); assert!(!v.rtl_dispatched); assert!(!v.rtl_frame_sent); diff --git a/examples/falcon-hitl-rfspoof/src/stub.rs b/examples/falcon-hitl-rfspoof/src/stub.rs index 9a7bca66..bed59bfb 100644 --- a/examples/falcon-hitl-rfspoof/src/stub.rs +++ b/examples/falcon-hitl-rfspoof/src/stub.rs @@ -45,8 +45,12 @@ impl StubBench { spoof_start_s: f32, ) -> Self { StubBench { - pre_n_cm, pre_e_cm, pre_d_cm, - spoof_n_cm, spoof_e_cm, spoof_d_cm, + pre_n_cm, + pre_e_cm, + pre_d_cm, + spoof_n_cm, + spoof_e_cm, + spoof_d_cm, spoof_start_s, t: 0.0, } @@ -54,9 +58,13 @@ impl StubBench { } impl HitlBench for StubBench { - fn name(&self) -> &'static str { "stub" } + fn name(&self) -> &'static str { + "stub" + } - fn step(&mut self, dt: f32) { self.t += dt; } + fn step(&mut self, dt: f32) { + self.t += dt; + } fn position_cm(&self) -> (i32, i32, i32) { if self.t < self.spoof_start_s { @@ -66,13 +74,15 @@ impl HitlBench for StubBench { } } - fn spoof_active(&self) -> bool { self.t >= self.spoof_start_s } + fn spoof_active(&self) -> bool { + self.t >= self.spoof_start_s + } } #[cfg(test)] mod tests { use super::*; - use crate::harness::{load_rtl_rts, run_scenario, NullCommandSink}; + use crate::harness::{NullCommandSink, load_rtl_rts, run_scenario}; use relay_lc::engine::Geofence; use relay_sc::engine::CommandStore; @@ -92,24 +102,28 @@ mod tests { let mut sink = NullCommandSink::new(); let v = run_scenario( - &mut bench, - &mut fence, - &mut sc, - &mut sink, - 0.01, // 100 Hz tick - 5.0, // 5-second scenario - 0, // RTL RTS id - 1.0, // must latch within 1 s of spoof going active + &mut bench, &mut fence, &mut sc, &mut sink, 0.01, // 100 Hz tick + 5.0, // 5-second scenario + 0, // RTL RTS id + 1.0, // must latch within 1 s of spoof going active ); assert!(v.pass(), "verdict = {:?}", v); assert!(v.latched); assert!(v.rtl_dispatched); - assert!(v.rtl_frame_sent, "RTL COMMAND_LONG frame should have been pushed to sink"); + assert!( + v.rtl_frame_sent, + "RTL COMMAND_LONG frame should have been pushed to sink" + ); assert_eq!(sink.frames_sent, 1, "exactly one RTL frame per latch trip"); let latched_at = v.latched_at_s.unwrap(); let spoof_at = v.spoof_first_seen_at_s.unwrap(); - assert!(latched_at >= spoof_at, "latch before spoof: {} < {}", latched_at, spoof_at); + assert!( + latched_at >= spoof_at, + "latch before spoof: {} < {}", + latched_at, + spoof_at + ); // One-tick latency since the spoof is a step-jump. assert!(latched_at - spoof_at < 0.05); } @@ -130,20 +144,18 @@ mod tests { let mut sink = NullCommandSink::new(); let v = run_scenario( - &mut bench, - &mut fence, - &mut sc, - &mut sink, - 0.01, - 5.0, - 0, - 10.0, // generous budget so we don't fail-stop on the missing latch + &mut bench, &mut fence, &mut sc, &mut sink, 0.01, 5.0, 0, + 10.0, // generous budget so we don't fail-stop on the missing latch ); assert!(!v.latched); assert!(!v.rtl_dispatched); assert!(!v.rtl_frame_sent, "no frame pushed when no latch trip"); assert_eq!(sink.frames_sent, 0); - assert!(v.failure.is_none(), "harness fail-stopped on a benign run: {:?}", v.failure); + assert!( + v.failure.is_none(), + "harness fail-stopped on a benign run: {:?}", + v.failure + ); } } diff --git a/examples/falcon-hold-bench/src/main.rs b/examples/falcon-hold-bench/src/main.rs index 709d20e5..0f87e3c0 100644 --- a/examples/falcon-hold-bench/src/main.rs +++ b/examples/falcon-hold-bench/src/main.rs @@ -50,8 +50,18 @@ fn q_rotate(q: [f32; 4], v: [f32; 3]) -> [f32; 3] { /// Touchdown) reaches Disarmed. fn fsm_sequence() -> (Mode, bool) { let mut fsm = FlightFsm::new(); - let ground = Gates { level: true, throttle_low: true, have_position: true, prearm_ok: true }; - let flying = Gates { level: true, throttle_low: false, have_position: true, prearm_ok: true }; + let ground = Gates { + level: true, + throttle_low: true, + have_position: true, + prearm_ok: true, + }; + let flying = Gates { + level: true, + throttle_low: false, + have_position: true, + prearm_ok: true, + }; fsm.on(Event::Arm, ground); fsm.on(Event::RequestTakeoff, flying); @@ -84,7 +94,10 @@ fn hold_under_wind() -> (f32, f32) { let steps = 50 * 30; // 30 s for k in 0..steps { - let t = Timestamp { seconds: 0, fraction: ((k as f32 * DT) * 1e9) as u32 }; + let t = Timestamp { + seconds: 0, + fraction: ((k as f32 * DT) * 1e9) as u32, + }; let att = ctrl.tick(t, pos, vel, q, sp); q = att.quaternion; // ideal inner loop: achieved = commanded @@ -141,7 +154,13 @@ mod tests { #[test] fn position_and_altitude_hold_station_against_wind() { let (horiz, alt_err) = hold_under_wind(); - assert!(horiz <= 1.0, "horizontal drift {horiz:.3} m exceeds 1.0 m budget"); - assert!(alt_err <= 0.5, "altitude error {alt_err:.3} m exceeds 0.5 m budget"); + assert!( + horiz <= 1.0, + "horizontal drift {horiz:.3} m exceeds 1.0 m budget" + ); + assert!( + alt_err <= 0.5, + "altitude error {alt_err:.3} m exceeds 0.5 m budget" + ); } } diff --git a/examples/falcon-iekf-bench/src/main.rs b/examples/falcon-iekf-bench/src/main.rs index d92031d3..f01c449b 100644 --- a/examples/falcon-iekf-bench/src/main.rs +++ b/examples/falcon-iekf-bench/src/main.rs @@ -58,7 +58,10 @@ const G: f32 = 9.81; struct Lcg(u64); impl Lcg { fn next_unit(&mut self) -> f32 { - self.0 = self.0.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407); + self.0 = self + .0 + .wrapping_mul(6364136223846793005) + .wrapping_add(1442695040888963407); let bits = (self.0 >> 40) as u32; // 24 high bits (bits as f32 / (1u32 << 23) as f32) - 1.0 } @@ -158,7 +161,9 @@ fn main() -> ExitCode { println!("falcon-iekf-bench — full-state IEKF (IMU+GNSS), {SECONDS:.0} s @ {IMU_HZ:.0} Hz"); println!(" position RMS (last 5 s): {pos_rms:.3} m (budget ≤ 2.0)"); println!(" velocity RMS (last 5 s): {vel_rms:.3} m/s (budget ≤ 1.5)"); - println!(" mean position NEES: {mean_nees:.3} (diagnostic — consistency gated by gz EKF-002)"); + println!( + " mean position NEES: {mean_nees:.3} (diagnostic — consistency gated by gz EKF-002)" + ); println!(" finite throughout: {}", !any_nan); let pass = !any_nan && pos_rms <= 2.0 && vel_rms <= 1.5; @@ -232,7 +237,13 @@ mod tests { let pos_rms = (sp / cnt as f64).sqrt(); let vel_rms = (sv / cnt as f64).sqrt(); let _mean_nees = sn / cnt as f64; // reported by main(); consistency gated by gz EKF-002 - assert!(pos_rms <= 2.0, "position RMS {pos_rms:.3} m exceeds 2.0 m budget"); - assert!(vel_rms <= 1.5, "velocity RMS {vel_rms:.3} m/s exceeds 1.5 budget"); + assert!( + pos_rms <= 2.0, + "position RMS {pos_rms:.3} m exceeds 2.0 m budget" + ); + assert!( + vel_rms <= 1.5, + "velocity RMS {vel_rms:.3} m/s exceeds 1.5 budget" + ); } } diff --git a/examples/falcon-sitl-gz/src/campaign.rs b/examples/falcon-sitl-gz/src/campaign.rs index fa4d8746..bfc0b8ac 100644 --- a/examples/falcon-sitl-gz/src/campaign.rs +++ b/examples/falcon-sitl-gz/src/campaign.rs @@ -19,7 +19,7 @@ use relay_geo::{GeoAtt, GeoGains}; use relay_iekf::RotorFaultDetector; -use relay_mix_quad::{motors_to_torque_signs, QuadMixer}; +use relay_mix_quad::{QuadMixer, motors_to_torque_signs}; // ── Deterministic, splittable RNG ─────────────────────────────────────────── @@ -66,7 +66,11 @@ const FLOOR: f32 = 0.15; fn integ_rot(r: &[[f32; 3]; 3], w: [f32; 3], dt: f32) -> [[f32; 3]; 3] { let wd = [w[0] * dt, w[1] * dt, w[2] * dt]; - let incr = [[1.0, -wd[2], wd[1]], [wd[2], 1.0, -wd[0]], [-wd[1], wd[0], 1.0]]; + let incr = [ + [1.0, -wd[2], wd[1]], + [wd[2], 1.0, -wd[0]], + [-wd[1], wd[0], 1.0], + ]; let mut m = [[0.0f32; 3]; 3]; for i in 0..3 { for jj in 0..3 { @@ -338,8 +342,12 @@ mod tests { let rep = run_motor_out_campaign(MOTOR_OUT_TRIALS, MOTOR_OUT_SEED); eprintln!( "motor-out campaign: {} trials, {} failures | worst peak tilt {:.3} rad, worst final tilt {:.3} rad, worst detect latency {} steps ({:.0} ms)", - rep.trials, rep.failures, rep.worst_peak_tilt, rep.worst_final_tilt, - rep.worst_detect_latency_steps, rep.worst_detect_latency_steps as f32 * DT * 1000.0 + rep.trials, + rep.failures, + rep.worst_peak_tilt, + rep.worst_final_tilt, + rep.worst_detect_latency_steps, + rep.worst_detect_latency_steps as f32 * DT * 1000.0 ); // Primary safety assertion: not one trial in the envelope fails. @@ -353,12 +361,14 @@ mod tests { assert!( rep.worst_peak_tilt < 1.4, "worst peak tilt across {} trials = {:.3} rad (tumble bound 1.4)", - rep.trials, rep.worst_peak_tilt + rep.trials, + rep.worst_peak_tilt ); assert!( rep.worst_final_tilt < 0.5, "worst final tilt across {} trials = {:.3} rad (settle bound 0.5)", - rep.trials, rep.worst_final_tilt + rep.trials, + rep.worst_final_tilt ); // Tighter REGRESSION bounds, set just above the measured worst case // (peak 0.832, final 0.097, detect 1 step at seed 0xFA1C_0DEAD_0001) — @@ -582,8 +592,12 @@ mod fullloop_tests { let rep = run_fullloop_motor_out_campaign(FL_TRIALS, FL_SEED); eprintln!( "full-loop motor-out campaign: {} trials, {} failures | worst peak tilt {:.3} rad, worst yaw {:.1} rad/s, worst detect {} steps | (reported, not gated: least net descent {:.2} m — altitude is the supervisor's LAND job)", - rep.trials, rep.failures, rep.worst_peak_tilt, rep.worst_yaw_rate, - rep.worst_detect_latency_steps, rep.least_descent + rep.trials, + rep.failures, + rep.worst_peak_tilt, + rep.worst_yaw_rate, + rep.worst_detect_latency_steps, + rep.least_descent ); // Primary safety assertion: not one trial in the envelope fails — the @@ -665,8 +679,10 @@ pub fn run_fdi_noise_campaign(n: u32, campaign_seed: u64) -> FdiReport { 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))); + rep.failing.push(( + i, + format!("{t:?}: isolated {f}, expected {}", t.failed_rotor), + )); } } Some(_) => { @@ -762,7 +778,10 @@ pub struct SupLandReport { pub fn run_supervised_rotorout_landing_campaign(n: u32, campaign_seed: u64) -> SupLandReport { const DT: f32 = 0.002; let level = [[1.0f32, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; - let mut rep = SupLandReport { trials: n, ..Default::default() }; + let mut rep = SupLandReport { + trials: n, + ..Default::default() + }; for i in 0..n { let mut rng = trial_rng(campaign_seed, i); let t = sample_supland(&mut rng, i); @@ -804,7 +823,11 @@ pub fn run_supervised_rotorout_landing_campaign(n: u32, campaign_seed: u64) -> S let mut reason = String::new(); if !landed { - reason = format!("never touched down (mode {:?}, alt {:.2})", sup.mode(), -b.pos[2]); + reason = format!( + "never touched down (mode {:?}, alt {:.2})", + sup.mode(), + -b.pos[2] + ); } else if approach_sink >= 1.2 { reason = format!("hard approach: {approach_sink:.2} m/s"); } else if peak_tilt >= 0.6 { @@ -859,7 +882,7 @@ mod supland_tests { /// tilt and the body rate back to level. #[derive(Clone, Copy, Debug)] struct AttStabTrial { - roll0: f32, // initial tilt components (rad) + roll0: f32, // initial tilt components (rad) pitch0: f32, omega0: [f32; 3], // initial body rate (rad/s) } @@ -1158,7 +1181,11 @@ mod campaign2_tests { let rep = run_att_stab_campaign(ATT_STAB_TRIALS, ATT_STAB_SEED); eprintln!( "att-stab campaign: {} trials, {} failures | worst peak tilt {:.4} rad, worst final tilt {:.4} rad, worst final |ω| {:.4} rad/s", - rep.trials, rep.failures, rep.worst_peak_tilt, rep.worst_final_tilt, rep.worst_final_rate + rep.trials, + rep.failures, + rep.worst_peak_tilt, + rep.worst_final_tilt, + rep.worst_final_rate ); // Primary safety assertion: not one trial in the envelope fails. @@ -1172,17 +1199,20 @@ mod campaign2_tests { assert!( rep.worst_peak_tilt < 1.4, "worst peak tilt across {} trials = {:.4} rad (divergence bound 1.4)", - rep.trials, rep.worst_peak_tilt + rep.trials, + rep.worst_peak_tilt ); assert!( rep.worst_final_tilt < 0.05, "worst final tilt across {} trials = {:.4} rad (settle bound 0.05)", - rep.trials, rep.worst_final_tilt + rep.trials, + rep.worst_final_tilt ); assert!( rep.worst_final_rate < 0.05, "worst final rate across {} trials = {:.4} rad/s (rate-null bound 0.05)", - rep.trials, rep.worst_final_rate + rep.trials, + rep.worst_final_rate ); // Tighter REGRESSION bounds, set just above the measured worst case // (peak 0.5540, final 0.0000, rate 0.0002 at seed 0x0FA1C0DEAD000002). @@ -1210,7 +1240,11 @@ mod campaign2_tests { let rep = run_hexa_campaign(HEXA_TRIALS, HEXA_SEED); eprintln!( "hexa campaign: {} trials, {} failures | worst peak tilt {:.4} rad, worst final tilt {:.4} rad, worst final |ω| {:.4} rad/s", - rep.trials, rep.failures, rep.worst_peak_tilt, rep.worst_final_tilt, rep.worst_final_rate + rep.trials, + rep.failures, + rep.worst_peak_tilt, + rep.worst_final_tilt, + rep.worst_final_rate ); assert_eq!( @@ -1222,17 +1256,20 @@ mod campaign2_tests { assert!( rep.worst_peak_tilt < 1.4, "worst peak tilt across {} trials = {:.4} rad (divergence bound 1.4)", - rep.trials, rep.worst_peak_tilt + rep.trials, + rep.worst_peak_tilt ); assert!( rep.worst_final_tilt < 0.05, "worst final tilt across {} trials = {:.4} rad (settle bound 0.05)", - rep.trials, rep.worst_final_tilt + rep.trials, + rep.worst_final_tilt ); assert!( rep.worst_final_rate < 0.05, "worst final rate across {} trials = {:.4} rad/s (rate-null bound 0.05)", - rep.trials, rep.worst_final_rate + rep.trials, + rep.worst_final_rate ); // Tighter REGRESSION bounds, set just above the measured worst case // (peak 0.5226, final 0.0000, rate 0.0001 at seed 0x0FA1C0DEAD000003). @@ -1424,8 +1461,12 @@ mod campaign3_tests { let rep = run_estimator_campaign(EST_TRIALS, EST_SEED); eprintln!( "estimator campaign: {} trials, {} failures | worst tilt-err {:.4} rad, worst dropout pos-err {:.3} m, worst reconverged pos-err {:.3} m, worst NEES {:.2}", - rep.trials, rep.failures, rep.worst_tilt_err, rep.worst_dropout_pos_err, - rep.worst_reconverged_pos_err, rep.worst_nees + rep.trials, + rep.failures, + rep.worst_tilt_err, + rep.worst_dropout_pos_err, + rep.worst_reconverged_pos_err, + rep.worst_nees ); assert_eq!( rep.failures, 0, @@ -1519,7 +1560,13 @@ fn run_maneuver_trial(t: ManeuverTrial, rng: &mut SplitMix64) -> ManeuverOutcome (1.0 + t.scale_err) * a_true[1] + gaussian(rng) * t.accel_sigma, -9.81 + gaussian(rng) * t.accel_sigma, ]; - f.propagate(IekfImu { gyro: [0.0; 3], accel }, dt); + f.propagate( + IekfImu { + gyro: [0.0; 3], + accel, + }, + dt, + ); // NO gravity aiding under motion: specific force != gravity (aiding here // would read the maneuver accel as a tilt). gyro=0 keeps attitude level. if step % 20 == 0 { @@ -1532,10 +1579,9 @@ fn run_maneuver_trial(t: ManeuverTrial, rng: &mut SplitMix64) -> ManeuverOutcome } let s = f.state(); if step > 800 { - let perr = ((s.p[0] - tp[0]).powi(2) - + (s.p[1] - tp[1]).powi(2) - + (s.p[2] - tp[2]).powi(2)) - .sqrt(); + let perr = + ((s.p[0] - tp[0]).powi(2) + (s.p[1] - tp[1]).powi(2) + (s.p[2] - tp[2]).powi(2)) + .sqrt(); o.peak_pos_err = o.peak_pos_err.max(perr); o.peak_tilt = o.peak_tilt.max(s.tilt_rad()); let nees = f.nees_velocity(tv); @@ -1613,9 +1659,21 @@ mod campaign4_tests { ); // Regression bounds just above the measured worst (seed 0x0FA1_C0DE_3A17_0001: // pos-err 0.91 m, vel-NEES 11.2, tilt 0.064 rad). - assert!(rep.worst_pos_err < 2.0, "REGRESSION: worst pos err {:.2} m (was ~0.91)", rep.worst_pos_err); - assert!(rep.worst_tilt < 0.12, "REGRESSION: worst tilt {:.3} rad (was ~0.064)", rep.worst_tilt); - assert!(rep.worst_vel_nees < 30.0, "REGRESSION: worst vel-NEES {:.1} (was ~11.2)", rep.worst_vel_nees); + assert!( + rep.worst_pos_err < 2.0, + "REGRESSION: worst pos err {:.2} m (was ~0.91)", + rep.worst_pos_err + ); + assert!( + rep.worst_tilt < 0.12, + "REGRESSION: worst tilt {:.3} rad (was ~0.064)", + rep.worst_tilt + ); + assert!( + rep.worst_vel_nees < 30.0, + "REGRESSION: worst vel-NEES {:.1} (was ~11.2)", + rep.worst_vel_nees + ); } } @@ -1740,7 +1798,10 @@ mod campaign5_tests { "spoof campaign: {} trials | {detected} detected (worst latency {worst_latency} fixes), {clean} no-false-alarm", SPOOF_TRIALS ); - assert!(detected > 150 && clean > 150, "both regimes well-sampled ({detected}/{clean})"); + assert!( + detected > 150 && clean > 150, + "both regimes well-sampled ({detected}/{clean})" + ); } } @@ -1755,9 +1816,9 @@ mod campaign5_tests { #[derive(Clone, Copy, Debug)] struct MotorOutDispTrial { base: MotorOutTrial, - act_sigma: f32, // per-rotor multiplicative thrust noise + act_sigma: f32, // per-rotor multiplicative thrust noise wind_torque: [f32; 3], // constant disturbance torque - gust_sigma: f32, // per-step gust torque + gust_sigma: f32, // per-step gust torque } const MO_ACT_SIGMA: (f32, f32) = (0.0, 0.05); // ≤5% actuator scatter @@ -1895,7 +1956,10 @@ mod campaign6_tests { "motor-out DISPERSED campaign: {} trials, {} failures | worst peak tilt {:.3} rad, worst final tilt {:.3} rad", MOD_TRIALS, fails, worst_peak, worst_final ); - assert_eq!(fails, 0, "dispersed motor-out recovery failed in {fails}/{MOD_TRIALS}"); + assert_eq!( + fails, 0, + "dispersed motor-out recovery failed in {fails}/{MOD_TRIALS}" + ); assert!(worst_peak < 1.4, "worst peak tilt {:.3}", worst_peak); assert!(worst_final < 0.5, "worst final tilt {:.3}", worst_final); } @@ -1958,7 +2022,10 @@ const MISSION_START_OFFSET: f32 = 1.0; fn sample_mission(rng: &mut SplitMix64) -> MissionTrial { MissionTrial { - wind: [rng.range(-MISSION_WIND, MISSION_WIND), rng.range(-MISSION_WIND, MISSION_WIND)], + wind: [ + rng.range(-MISSION_WIND, MISSION_WIND), + rng.range(-MISSION_WIND, MISSION_WIND), + ], thrust_scale: rng.range(MISSION_THRUST_SCALE.0, MISSION_THRUST_SCALE.1), start_offset: [ rng.range(-MISSION_START_OFFSET, MISSION_START_OFFSET), @@ -1981,7 +2048,11 @@ fn run_mission_trial(t: MissionTrial) -> (f32, bool, u32, f32, bool) { let dt = 0.02f32; let max_steps = 3000u32; // 60 s budget let arrival_radius = 0.6f32; - let mut p = [wps[0][0] + t.start_offset[0], wps[0][1] + t.start_offset[1], wps[0][2]]; + let mut p = [ + wps[0][0] + t.start_offset[0], + wps[0][1] + t.start_offset[1], + wps[0][2], + ]; let mut v = [0.0f32; 3]; let mut q = [1.0f32, 0.0, 0.0, 0.0]; let mut wp = 1usize; @@ -2011,7 +2082,10 @@ fn run_mission_trial(t: MissionTrial) -> (f32, bool, u32, f32, bool) { if step > 50 { max_cross = max_cross.max(cross_track(wps[wp - 1], wps[wp], p)); } - let d = ((p[0] - wps[wp][0]).powi(2) + (p[1] - wps[wp][1]).powi(2) + (p[2] - wps[wp][2]).powi(2)).sqrt(); + let d = ((p[0] - wps[wp][0]).powi(2) + + (p[1] - wps[wp][1]).powi(2) + + (p[2] - wps[wp][2]).powi(2)) + .sqrt(); if d < arrival_radius { wp += 1; } @@ -2035,7 +2109,9 @@ mod campaign7_tests { thrust_scale: 1.0, start_offset: [0.0, 0.0], }); - eprintln!("mission diag (nominal): reached={r0} steps={s0} max_cross={c0:.2} final_x={fx0:.2}"); + eprintln!( + "mission diag (nominal): reached={r0} steps={s0} max_cross={c0:.2} final_x={fx0:.2}" + ); let (mut reached, mut worst_cross, mut worst_steps) = (0u32, 0.0f32, 0u32); let mut fails = 0u32; @@ -2057,10 +2133,17 @@ mod campaign7_tests { MISSION_TRIALS, worst_cross ); assert_eq!(fails, 0, "mission failed in {fails}/{MISSION_TRIALS}"); - assert_eq!(reached, MISSION_TRIALS, "every trial must complete all waypoints"); + assert_eq!( + reached, MISSION_TRIALS, + "every trial must complete all waypoints" + ); // Physical corridor half-width 3 m; regression bound just above the // measured worst (1.02 m at seed 0x0FA1_C0DE_3155_0001). - assert!(worst_cross < 3.0, "worst cross-track {:.2} m (corridor 3)", worst_cross); + assert!( + worst_cross < 3.0, + "worst cross-track {:.2} m (corridor 3)", + worst_cross + ); assert!( worst_cross < 1.6, "REGRESSION: worst cross-track {:.2} m exceeded 1.6 (was ~1.02)", diff --git a/examples/falcon-sitl-gz/src/main.rs b/examples/falcon-sitl-gz/src/main.rs index 4f34bcc3..4ffcaa84 100644 --- a/examples/falcon-sitl-gz/src/main.rs +++ b/examples/falcon-sitl-gz/src/main.rs @@ -22,19 +22,19 @@ mod flightcore; mod pace; mod physics; +use falcon_config::YawMode; use falcon_core::FlightCore; use flightcore::SitlBackend; use physics::{GazeboPhysics, MockPhysics, Physics}; -use relay_arm::{ArmingConfig, ArmingSequencer, ARMED}; -use relay_iekf::{Iekf, Imu as IekfImu, NavState}; -use falcon_config::YawMode; -use relay_geo::GeoAtt; -use relay_traj::{RefGovernor, Segment3}; +use relay_arm::{ARMED, ArmingConfig, ArmingSequencer}; use relay_att::{AttController, Timestamp as AttTimestamp}; use relay_ekf::{Ekf, ImuSample, Timestamp as EkfTimestamp}; +use relay_geo::GeoAtt; +use relay_iekf::{Iekf, Imu as IekfImu, NavState}; use relay_mix_quad::QuadMixer; use relay_pos::{PosController, PosGains, PositionSetpoint, Timestamp as PosTimestamp}; use relay_rate::{RatePid, Timestamp as RateTimestamp}; +use relay_traj::{RefGovernor, Segment3}; use std::fs; use std::io::Write; use std::path::PathBuf; @@ -50,7 +50,9 @@ fn main() { } let backend = arg(&args, "--backend").unwrap_or_else(|| "mock".into()); let scenario = arg(&args, "--scenario").unwrap_or_else(|| "hover".into()); - let duration_s: f32 = arg(&args, "--duration").and_then(|s| s.parse().ok()).unwrap_or(5.0); + let duration_s: f32 = arg(&args, "--duration") + .and_then(|s| s.parse().ok()) + .unwrap_or(5.0); let evidence_dir = arg(&args, "--evidence-dir").map(PathBuf::from); println!("falcon-sitl-gz: backend={backend} scenario={scenario} duration={duration_s}s"); @@ -90,7 +92,9 @@ fn main() { } }; - if let Some(s) = evidence.as_mut() { s.finish(pass); } + if let Some(s) = evidence.as_mut() { + s.finish(pass); + } if pass { println!("PASS"); @@ -196,7 +200,13 @@ fn run_scenario( "flightcore-rotorout" => { // Hover, then lose rotor 0 at the midpoint; the production FDI must // isolate it (RPM residual) and the loop keeps the vehicle aloft. - run_flightcore(physics, 2.0, duration_s, Some((0, duration_s * 0.5)), evidence) + run_flightcore( + physics, + 2.0, + duration_s, + Some((0, duration_s * 0.5)), + evidence, + ) } "supervised-rotorout" => { // v1.117 (FAULT-P04): the FULL production FlightSupervisor flies @@ -206,9 +216,7 @@ fn run_scenario( run_supervised_rotorout(physics, 2.0, duration_s, duration_s * 0.4, evidence) } other => { - eprintln!( - " scenario {other} not yet wired; falling back to closed-loop hover", - ); + eprintln!(" scenario {other} not yet wired; falling back to closed-loop hover",); run_closed_loop_hover(physics, duration_s, evidence) } } @@ -263,15 +271,23 @@ fn run_frame_check(physics: &mut dyn Physics, axis: usize, duration_s: f32) -> b } if pace_real_time { let used = tick_start.elapsed(); - if used < tick_period { std::thread::sleep(tick_period - used); } + if used < tick_period { + std::thread::sleep(tick_period - used); + } } } - let mean_rate = if count > 0 { sum_rate / count as f32 } else { 0.0 }; + let mean_rate = if count > 0 { + sum_rate / count as f32 + } else { + 0.0 + }; // After correction, +cmd on this axis should yield +rate. let agrees = mean_rate > 0.0; println!( " frame-check axis={axis_name}: commanded +0.15 (corrected={:?}) → mean sensed rate={:.4} rad/s [{}]", - cmd_corrected, mean_rate, if agrees { "AGREE ✓" } else { "OPPOSE ✗" }, + cmd_corrected, + mean_rate, + if agrees { "AGREE ✓" } else { "OPPOSE ✗" }, ); agrees } @@ -310,7 +326,13 @@ fn run_yaw_probe(physics: &mut dyn Physics, duration_s: f32) -> bool { let t = step as f32 * dt; let (imu, pos) = physics.measure(0.0); // Estimator: propagate + gravity + direct heading (the FlightCore path). - iekf.propagate(IekfImu { gyro: imu.gyro_body, accel: imu.accel_body }, dt); + iekf.propagate( + IekfImu { + gyro: imu.gyro_body, + accel: imu.accel_body, + }, + dt, + ); iekf.update_gravity(imu.accel_body, 0.5); iekf.update_position(pos, 0.01); if let Some(h) = physics.heading_ned() { @@ -333,7 +355,9 @@ fn run_yaw_probe(physics: &mut dyn Physics, duration_s: f32) -> bool { } if pace_real_time { let used = tick_start.elapsed(); - if used < tick_period { std::thread::sleep(tick_period - used); } + if used < tick_period { + std::thread::sleep(tick_period - used); + } } } println!(" yaw-probe: compare signs of Δtruth_head, Δest_yaw, gyro_z vs cmd_yaw=+"); @@ -411,7 +435,9 @@ fn run_arming_check(physics: &mut dyn Physics, duration_s: f32, gated: bool) -> let tilt = body_tilt_rad(imu_sample.accel_body); last_tilt = tilt; - if tilt > peak_tilt { peak_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); @@ -433,8 +459,7 @@ fn run_arming_check(physics: &mut dyn Physics, duration_s: f32, gated: bool) -> [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); + let motors = mixer.mix_thrust_floor(torque, att_sp.thrust * scale, 0.5 * scale); physics.step(motors, dt); if pace_real_time { @@ -462,12 +487,18 @@ fn run_arming_check(physics: &mut dyn Physics, duration_s: f32, gated: bool) -> 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()), + 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 ✗" }, + if pass { + "PASS ✓ (handoff)" + } else { + "FAIL ✗" + }, wall.as_secs_f32(), ); if pass && peak_tilt >= TUMBLE_RAD { @@ -512,14 +543,20 @@ impl Mission { fn sample(&self, t: f32) -> ([f32; 3], [f32; 3], [f32; 3], [f32; 3]) { let n = self.waypoints.len(); if n < 2 { - return (self.waypoints.first().copied().unwrap_or([0.0; 3]), [0.0; 3], [0.0; 3], [0.0; 3]); + return ( + self.waypoints.first().copied().unwrap_or([0.0; 3]), + [0.0; 3], + [0.0; 3], + [0.0; 3], + ); } if t >= self.total_time() { return (self.waypoints[n - 1], [0.0; 3], [0.0; 3], [0.0; 3]); } let leg = ((t / self.leg_time) as usize).min(n - 2); let tl = t - leg as f32 * self.leg_time; - let s = Segment3::rest_to_rest(self.waypoints[leg], self.waypoints[leg + 1], self.leg_time).eval(tl); + let s = Segment3::rest_to_rest(self.waypoints[leg], self.waypoints[leg + 1], self.leg_time) + .eval(tl); (s.pos, s.vel, s.acc, s.jerk) } } @@ -670,7 +707,10 @@ fn run_geo_cascade( // spoofer cannot steer the vehicle. FDI_OFF disables (A/B); SPOOF_WALKOFF // = bias rate (m/s) injects a growing position-measurement bias after 15 s. let fdi_on = std::env::var("FDI_OFF").is_err(); - let spoof_rate: f32 = std::env::var("SPOOF_WALKOFF").ok().and_then(|s| s.parse().ok()).unwrap_or(0.0); + let spoof_rate: f32 = std::env::var("SPOOF_WALKOFF") + .ok() + .and_then(|s| s.parse().ok()) + .unwrap_or(0.0); // drift slack 0.03 m ≈ the noiseless-SITL innovation floor; detects a // walk-off whose per-fix innovation exceeds it (rate ≳ 1.5 m/s at the // ~50 Hz outer rate). A slower covert spoof that the filter follows keeps @@ -750,7 +790,13 @@ fn run_geo_cascade( let (imu_sample, pos_ned) = physics.measure(0.0); // ── INNER (every tick): IEKF predict + gyro low-pass ── - iekf.propagate(IekfImu { gyro: imu_sample.gyro_body, accel: imu_sample.accel_body }, dt); + iekf.propagate( + IekfImu { + gyro: imu_sample.gyro_body, + accel: imu_sample.accel_body, + }, + dt, + ); let gyro_f = gyro_lpf.filter(imu_sample.gyro_body); // ── OUTER (every outer_decim ticks): aiding updates + position → @@ -902,7 +948,11 @@ fn run_geo_cascade( } else { geo.tick(est.q, gyro_f, a_cmd_held, yaw_d) }; - let yaw_t = if cfg.pos.yaw_mode == YawMode::Off { 0.0 } else { m[2] * torque_scale }; + let yaw_t = if cfg.pos.yaw_mode == YawMode::Off { + 0.0 + } else { + m[2] * torque_scale + }; [m[0] * torque_scale, m[1] * torque_scale, yaw_t] } else { adrc.reset(); @@ -923,8 +973,12 @@ fn run_geo_cascade( let de = pos_ned[1] - setpoint_ned[1]; let dd = pos_ned[2] - setpoint_ned[2]; let dist = (dn * dn + de * de + dd * dd).sqrt(); - if dist > peak_dist { peak_dist = dist; } - if dist < min_dist { min_dist = dist; } + if dist > peak_dist { + peak_dist = dist; + } + if dist < min_dist { + min_dist = dist; + } if t >= steady_start_t { sum_sq_steady += dist * dist; steady_count += 1; @@ -934,18 +988,32 @@ fn run_geo_cascade( if std::env::var("POS_DEBUG").is_ok() && step % 50 == 0 { let iyaw = { let q = est.q; - libm::atan2f(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])) + libm::atan2f( + 2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), + ) }; let chdg = physics.heading_ned().unwrap_or(f32::NAN).to_degrees(); eprintln!( " [geo] t={t:.1} pos=[{:.1},{:.1},{:.1}] dist={dist:.2} tilt={:.1}° IEKFyaw={:.1}° compass={:.1}°", - pos_ned[0], pos_ned[1], pos_ned[2], tilt.to_degrees(), - iyaw.to_degrees(), chdg, + pos_ned[0], + pos_ned[1], + pos_ned[2], + tilt.to_degrees(), + iyaw.to_degrees(), + chdg, ); } if let Some(ref mut e) = evidence { - e.write_tick(step, t, pos_ned, imu_sample.accel_body, imu_sample.gyro_body, - motors, physics.counters()); + e.write_tick( + step, + t, + pos_ned, + imu_sample.accel_body, + imu_sample.gyro_body, + motors, + physics.counters(), + ); } if sim_lock { // Two-stage gyro-synced pacing (v0.32). @@ -993,10 +1061,19 @@ fn run_geo_cascade( } else { f32::NAN }; - let anees = if nees_n > 0 { (nees_sum / nees_n as f64) as f32 } else { f32::NAN }; + let anees = if nees_n > 0 { + (nees_sum / nees_n as f64) as f32 + } else { + f32::NAN + }; println!( " verdict: backend={} scenario=geo-hover steps={} final_dist={:.2}m peak_dist={:.2}m rms_steady={:.2}m wall={:.2}s", - physics.name(), n, final_dist, peak_dist, rms_steady, wall.as_secs_f32(), + physics.name(), + n, + final_dist, + peak_dist, + rms_steady, + wall.as_secs_f32(), ); // IEKF consistency report (3-DoF position, χ²₃: E=3, 95% single-sample // band [0.216, 9.35]). The SAFETY-relevant direction is OVER-CONFIDENT @@ -1043,10 +1120,17 @@ fn run_geo_cascade( // corrections) is the honest operating accuracy — fine for hold. if mag_n > 0 { let mag_err_mean = (mag_err_sum / mag_n as f64) as f32; - let mag_status = if mag_err_mean.to_degrees() < 12.0 { "OK" } else { "DEGRADED" }; + let mag_status = if mag_err_mean.to_degrees() < 12.0 { + "OK" + } else { + "DEGRADED" + }; println!( " mag-heading: closed-loop err vs truth mean={:.1}° max={:.1}° n={} [{}]", - mag_err_mean.to_degrees(), mag_err_max.to_degrees(), mag_n, mag_status, + mag_err_mean.to_degrees(), + mag_err_max.to_degrees(), + mag_n, + mag_status, ); } else { println!(" mag-heading: no magnetometer frames received"); @@ -1063,7 +1147,15 @@ fn run_geo_cascade( ); } if let Some(ref mut e) = evidence { - e.write_summary_hover(n, final_dist, peak_dist, rms_steady, min_dist, wall.as_secs_f32(), physics.counters()); + e.write_summary_hover( + n, + final_dist, + peak_dist, + rms_steady, + min_dist, + wall.as_secs_f32(), + physics.counters(), + ); } // PASS requires BOTH position-hold AND filter consistency: an // over-confident estimator is unsafe even if this run's position @@ -1150,15 +1242,26 @@ fn run_alt_rate_hover( physics.step(motors, dt); let dist = alt_err.abs(); - if dist > peak_dist_err { peak_dist_err = dist; } - if dist < min_dist_seen { min_dist_seen = dist; } + if dist > peak_dist_err { + peak_dist_err = dist; + } + if dist < min_dist_seen { + min_dist_seen = dist; + } if t >= steady_start_t { sum_sq_steady += dist * dist; steady_count += 1; } if let Some(ref mut e) = evidence { - e.write_tick(step, t, pos_ned, imu_sample.accel_body, imu_sample.gyro_body, - motors, physics.counters()); + e.write_tick( + step, + t, + pos_ned, + imu_sample.accel_body, + imu_sample.gyro_body, + motors, + physics.counters(), + ); } if pace_real_time { let used = tick_start.elapsed(); @@ -1177,7 +1280,12 @@ fn run_alt_rate_hover( let counters = physics.counters(); println!( " verdict: backend={} scenario=alt-rate steps={} final_dist={:.2}m peak_dist={:.2}m rms_steady={:.2}m wall={:.2}s", - physics.name(), n, final_dist, peak_dist_err, rms_steady, wall.as_secs_f32(), + physics.name(), + n, + final_dist, + peak_dist_err, + rms_steady, + wall.as_secs_f32(), ); if let Some((imu_recv, navsat_recv, motor_send)) = counters { println!( @@ -1185,8 +1293,15 @@ fn run_alt_rate_hover( ); } if let Some(ref mut e) = evidence { - e.write_summary_hover(n, final_dist, peak_dist_err, rms_steady, - min_dist_seen, wall.as_secs_f32(), counters); + e.write_summary_hover( + n, + final_dist, + peak_dist_err, + rms_steady, + min_dist_seen, + wall.as_secs_f32(), + counters, + ); } final_dist < 0.5 && rms_steady < 1.0 } @@ -1254,24 +1369,32 @@ fn run_alt_only_hover( // both raise thrust to climb.) let alt_err = setpoint_d - pos_ned[2]; alt_integral = (alt_integral + alt_err * dt).clamp(-i_max / ki_alt, i_max / ki_alt); - let thrust = (hover_thrust - - kp_alt * alt_err - - ki_alt * alt_integral - + kd_alt * v_d_filt) + let thrust = (hover_thrust - kp_alt * alt_err - ki_alt * alt_integral + kd_alt * v_d_filt) .clamp(0.0, 1.0); let motors = mixer.mix([0.0_f32; 3], thrust); physics.step(motors, dt); let dist = alt_err.abs(); - if dist > peak_dist_err { peak_dist_err = dist; } - if dist < min_dist_seen { min_dist_seen = dist; } + if dist > peak_dist_err { + peak_dist_err = dist; + } + if dist < min_dist_seen { + min_dist_seen = dist; + } if t >= steady_start_t { sum_sq_steady += dist * dist; steady_count += 1; } if let Some(ref mut e) = evidence { - e.write_tick(step, t, pos_ned, imu_sample.accel_body, imu_sample.gyro_body, - motors, physics.counters()); + e.write_tick( + step, + t, + pos_ned, + imu_sample.accel_body, + imu_sample.gyro_body, + motors, + physics.counters(), + ); } if pace_real_time { let used = tick_start.elapsed(); @@ -1290,7 +1413,12 @@ fn run_alt_only_hover( let counters = physics.counters(); println!( " verdict: backend={} scenario=alt-only steps={} final_dist={:.2}m peak_dist={:.2}m rms_steady={:.2}m wall={:.2}s", - physics.name(), n, final_dist, peak_dist_err, rms_steady, wall.as_secs_f32(), + physics.name(), + n, + final_dist, + peak_dist_err, + rms_steady, + wall.as_secs_f32(), ); if let Some((imu_recv, navsat_recv, motor_send)) = counters { println!( @@ -1298,8 +1426,15 @@ fn run_alt_only_hover( ); } if let Some(ref mut e) = evidence { - e.write_summary_hover(n, final_dist, peak_dist_err, rms_steady, - min_dist_seen, wall.as_secs_f32(), counters); + e.write_summary_hover( + n, + final_dist, + peak_dist_err, + rms_steady, + min_dist_seen, + wall.as_secs_f32(), + counters, + ); } final_dist < 0.5 && rms_steady < 1.0 } @@ -1398,7 +1533,6 @@ fn run_supervised_rotorout( pass } - /// v1.113 — **FlightCore-in-the-loop**. Flies the PRODUCTION /// [`falcon_core::FlightCore`] (the verified IEKF → geometric-SE(3) → ADRC → /// mixer cascade, with the single-rotor-out FDI + degraded-allocator recovery) @@ -1446,7 +1580,9 @@ fn run_flightcore( .ok() .and_then(|s| s.parse().ok()) .unwrap_or(hover_thrust); - let est_tuning = std::env::var("EST_TUNING").map(|v| v != "0").unwrap_or(true); + let est_tuning = std::env::var("EST_TUNING") + .map(|v| v != "0") + .unwrap_or(true); let seed_alt = std::env::var("SEED_ALT") .ok() .and_then(|s| s.parse::().ok()) @@ -1495,7 +1631,10 @@ fn run_flightcore( { // GNSS_DIV: aiding cadence in ticks (default 50 = 5 Hz at 250 Hz). A knob // for #403, to separate an update-RATE lag from a filter-GAIN lag. - let gnss_div: u32 = std::env::var("GNSS_DIV").ok().and_then(|s| s.parse().ok()).unwrap_or(50); + let gnss_div: u32 = std::env::var("GNSS_DIV") + .ok() + .and_then(|s| s.parse().ok()) + .unwrap_or(50); let mut backend = SitlBackend::new(physics, dt, 0.0, gnss_div); for step in 0..n { let tick_start = Instant::now(); @@ -1522,14 +1661,25 @@ fn run_flightcore( 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]), ); let m = backend.last_motors(); - let tilt_deg = libm::acosf((1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])).clamp(-1.0, 1.0)) - * 57.2958; + let tilt_deg = + libm::acosf((1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])).clamp(-1.0, 1.0)) + * 57.2958; eprintln!( "t={:.2} true_z={:.2} est_z={:.2} vz={:+.2} alt_int={:+.3} \ xy=[{:+.2},{:+.2}] tilt={:.0}deg yaw={:+.2} mot=[{:.2},{:.2},{:.2},{:.2}]", - t, last_true[2], e.p[2], e.v[2], core.altitude_integral(), - last_true[0], last_true[1], tilt_deg, yaw, - m[0], m[1], m[2], m[3], + t, + last_true[2], + e.p[2], + e.v[2], + core.altitude_integral(), + last_true[0], + last_true[1], + tilt_deg, + yaw, + m[0], + m[1], + m[2], + m[3], ); // #403 — the diagnostic that separates the two hypotheses. The // line above has TRUE horizontal position only, so it cannot @@ -1541,9 +1691,17 @@ xy=[{:+.2},{:+.2}] tilt={:.0}deg yaw={:+.2} mot=[{:.2},{:.2},{:.2},{:.2}]", eprintln!( "H t={:.2} true_n={:+.3} true_e={:+.3} est_n={:+.3} est_e={:+.3} \ err_n={:+.3} err_e={:+.3} est_vn={:+.3} est_ve={:+.3} gyro_z={:+.3} yaw={:+.4}", - t, last_true[0], last_true[1], e.p[0], e.p[1], - e.p[0] - last_true[0], e.p[1] - last_true[1], - e.v[0], e.v[1], g[2], yaw, + t, + last_true[0], + last_true[1], + e.p[0], + e.p[1], + e.p[0] - last_true[0], + e.p[1] - last_true[1], + e.v[0], + e.v[1], + g[2], + yaw, ); let _ = (a, g); } @@ -1585,10 +1743,23 @@ err_n={:+.3} err_e={:+.3} est_vn={:+.3} est_ve={:+.3} gyro_z={:+.3} yaw={:+.4}", // Rotor-out mode: the production FDI must have ISOLATED the injected rotor // (via the commanded-vs-achieved RPM residual carried across the SITL seam). let isolated = core.failed_motor(); - let scen = if fail.is_some() { "flightcore-rotorout" } else { "flightcore" }; + let scen = if fail.is_some() { + "flightcore-rotorout" + } else { + "flightcore" + }; println!( " verdict: backend={} scenario={} steps={} target={:.1}m final_dist={:.2}m peak_dist={:.2}m rms_steady={:.2}m est_z={:.2}m isolated={:?} wall={:.2}s", - name, scen, n, target_alt_m, final_dist, peak_dist_err, rms_steady, est.p[2], isolated, wall.as_secs_f32(), + name, + scen, + n, + target_alt_m, + final_dist, + peak_dist_err, + rms_steady, + est.p[2], + isolated, + wall.as_secs_f32(), ); if let Some((imu_recv, navsat_recv, motor_send)) = counters { println!( @@ -1596,8 +1767,15 @@ err_n={:+.3} err_e={:+.3} est_vn={:+.3} est_ve={:+.3} gyro_z={:+.3} yaw={:+.4}", ); } if let Some(ref mut e) = evidence { - e.write_summary_hover(n, final_dist, peak_dist_err, rms_steady, - min_dist_seen, wall.as_secs_f32(), counters); + e.write_summary_hover( + n, + final_dist, + peak_dist_err, + rms_steady, + min_dist_seen, + wall.as_secs_f32(), + counters, + ); } match fail { // Rotor-out: PASS = the production FDI ISOLATED the CORRECT (failed) @@ -1672,7 +1850,11 @@ fn run_closed_loop_hover( let setpoint_ned = [0.0_f32, 0.0, -2.0]; let setpoint = if std::env::var("USE_IEKF").is_ok() { - PositionSetpoint { position_ned: setpoint_ned, velocity_ned: [0.0; 3], yaw_setpoint: yaw_hold } + PositionSetpoint { + position_ned: setpoint_ned, + velocity_ned: [0.0; 3], + yaw_setpoint: yaw_hold, + } } else { PositionSetpoint::hover_at(setpoint_ned) }; @@ -1708,13 +1890,21 @@ fn run_closed_loop_hover( // 2. EKF — attitude estimate. let est = ekf.tick(imu_sample); - if !est.quaternion[0].is_finite() { nan_seen = true; } + if !est.quaternion[0].is_finite() { + nan_seen = true; + } // 2b. IEKF — propagate on IMU, correct on gz position (the // "GPS"), and on heading (v0.22 "compass") which makes yaw // observable (the v0.21 ±130° wander). The measured accel is // body-frame specific force, exactly the IEKF's dynamics input. - iekf.propagate(IekfImu { gyro: imu_sample.gyro_body, accel: imu_sample.accel_body }, dt); + iekf.propagate( + IekfImu { + gyro: imu_sample.gyro_body, + accel: imu_sample.accel_body, + }, + dt, + ); // Adaptive gravity/tilt fusion (variance inflates under accel) + // position. Gives the roll/pitch observability the IMU+GPS-only // filter lacked (3° error → tip-over). @@ -1747,15 +1937,13 @@ fn run_closed_loop_hover( // IEKF, also feed its SMOOTH velocity estimate (the finite-diff // v_fd from NavSat is noisy and was destabilising the vel loop). let use_iekf = std::env::var("USE_IEKF").is_ok(); - let est_q = if use_iekf { iekf.state().q } else { est.quaternion }; + let est_q = if use_iekf { + iekf.state().q + } else { + est.quaternion + }; let v_ned = if use_iekf { iekf.state().v } else { v_fd }; - let att_sp = pos.tick( - pos_ts_of(t), - pos_ned, - v_ned, - est_q, - setpoint, - ); + let att_sp = pos.tick(pos_ts_of(t), pos_ned, v_ned, est_q, setpoint); current_att_sp = att_sp.quaternion; current_thrust = att_sp.thrust; @@ -1774,7 +1962,9 @@ fn run_closed_loop_hover( }; let torque = frame_correct_torque(torque_raw); for k in 0..3 { - if !torque[k].is_finite() { nan_seen = true; } + if !torque[k].is_finite() { + nan_seen = true; + } } // 6. MIX — torque + thrust → 4× motor PWM. v0.19.9 carries the @@ -1785,7 +1975,9 @@ fn run_closed_loop_hover( current_thrust * arm.thrust_scale, 0.5 * arm.thrust_scale, ); - if motors.iter().any(|v| !v.is_finite()) { nan_seen = true; } + if motors.iter().any(|v| !v.is_finite()) { + nan_seen = true; + } // 7. Publish to the bridge. physics.step(motors, dt); @@ -1797,13 +1989,23 @@ fn run_closed_loop_hover( let est_tilt = libm::acosf(bz_d.clamp(-1.0, 1.0)); let is = iekf.state(); let iq = is.q; - let iyaw = libm::atan2f(2.0 * (iq[0] * iq[3] + iq[1] * iq[2]), 1.0 - 2.0 * (iq[2] * iq[2] + iq[3] * iq[3])); + let iyaw = libm::atan2f( + 2.0 * (iq[0] * iq[3] + iq[1] * iq[2]), + 1.0 - 2.0 * (iq[2] * iq[2] + iq[3] * iq[3]), + ); let chdg = physics.heading_ned().unwrap_or(f32::NAN).to_degrees(); eprintln!( " [dbg] t={t:.1} pos=[{:.1},{:.1},{:.1}] true_tilt={:.1}° IEKF_tilt={:.1}° IEKF_yaw={:.1}° compass={:.1}° ipos=[{:.1},{:.1},{:.1}]", - pos_ned[0], pos_ned[1], pos_ned[2], - tilt.to_degrees(), is.tilt_rad().to_degrees(), iyaw.to_degrees(), chdg, - is.p[0], is.p[1], is.p[2], + pos_ned[0], + pos_ned[1], + pos_ned[2], + tilt.to_degrees(), + is.tilt_rad().to_degrees(), + iyaw.to_degrees(), + chdg, + is.p[0], + is.p[1], + is.p[2], ); } @@ -1812,16 +2014,27 @@ fn run_closed_loop_hover( let de = pos_ned[1] - setpoint_ned[1]; let dd = pos_ned[2] - setpoint_ned[2]; let dist = (dn * dn + de * de + dd * dd).sqrt(); - if dist > peak_dist_err { peak_dist_err = dist; } - if dist < min_dist_seen { min_dist_seen = dist; } + if dist > peak_dist_err { + peak_dist_err = dist; + } + if dist < min_dist_seen { + min_dist_seen = dist; + } if t >= steady_start_t { sum_sq_steady += dist * dist; steady_count += 1; } if let Some(ref mut e) = evidence { - e.write_tick(step, t, pos_ned, imu_sample.accel_body, imu_sample.gyro_body, - motors, physics.counters()); + e.write_tick( + step, + t, + pos_ned, + imu_sample.accel_body, + imu_sample.gyro_body, + motors, + physics.counters(), + ); } if pace_real_time { @@ -1851,7 +2064,12 @@ fn run_closed_loop_hover( println!( " verdict: backend={} scenario=hover steps={} final_dist={:.2}m peak_dist={:.2}m rms_steady={:.2}m wall={:.2}s", - physics.name(), n, final_dist, peak_dist_err, rms_steady, wall.as_secs_f32(), + physics.name(), + n, + final_dist, + peak_dist_err, + rms_steady, + wall.as_secs_f32(), ); if let Some((imu_recv, navsat_recv, motor_send)) = counters { println!( @@ -1859,8 +2077,15 @@ fn run_closed_loop_hover( ); } if let Some(ref mut e) = evidence { - e.write_summary_hover(n, final_dist, peak_dist_err, rms_steady, - min_dist_seen, wall.as_secs_f32(), counters); + e.write_summary_hover( + n, + final_dist, + peak_dist_err, + rms_steady, + min_dist_seen, + wall.as_secs_f32(), + counters, + ); } // PASS = within 0.5 m at end + RMS over last 5 s under 1.0 m + no NaN. @@ -1869,19 +2094,31 @@ fn run_closed_loop_hover( fn ekf_ts_of(secs: f32) -> EkfTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - EkfTimestamp { seconds: secs as u64, fraction: frac } + EkfTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn rate_ts_of(secs: f32) -> RateTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - RateTimestamp { seconds: secs as u64, fraction: frac } + RateTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn att_ts_of(secs: f32) -> AttTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - AttTimestamp { seconds: secs as u64, fraction: frac } + AttTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn pos_ts_of(secs: f32) -> PosTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - PosTimestamp { seconds: secs as u64, fraction: frac } + PosTimestamp { + seconds: secs as u64, + fraction: frac, + } } /// v0.19.3 open-loop smoke: command 70 % PWM constant, watch for @@ -1919,13 +2156,22 @@ fn run_open_loop_climb( physics.step(motor_pwm, dt); let (imu, pos) = physics.measure(0.01); let alt_m = -pos[2]; // NED down → altitude is -z - if initial_alt.is_none() { initial_alt = Some(alt_m); } + if initial_alt.is_none() { + initial_alt = Some(alt_m); + } min_alt = min_alt.min(alt_m); max_alt = max_alt.max(alt_m); if let Some(ref mut e) = evidence { - e.write_tick(step, t, pos, imu.accel_body, imu.gyro_body, motor_pwm, - physics.counters()); + e.write_tick( + step, + t, + pos, + imu.accel_body, + imu.gyro_body, + motor_pwm, + physics.counters(), + ); } t += dt; @@ -1944,7 +2190,12 @@ fn run_open_loop_climb( let counters = physics.counters(); println!( " verdict: backend={} steps={} climb={:.2} m (min={:.2} max={:.2}) wall={:.2}s", - physics.name(), n, net_climb, min_alt, max_alt, wall.as_secs_f32(), + physics.name(), + n, + net_climb, + min_alt, + max_alt, + wall.as_secs_f32(), ); if let Some((imu_recv, navsat_recv, motor_send)) = counters { println!( @@ -1991,7 +2242,11 @@ impl EvidenceSink { ticks, "step,t_s,n_m,e_m,d_m,ax_body,ay_body,az_body,gx_body,gy_body,gz_body,m0,m1,m2,m3,imu_recv,navsat_recv,motor_send" )?; - Ok(Self { harness, ticks, timestamp: format!("{ts}") }) + Ok(Self { + harness, + ticks, + timestamp: format!("{ts}"), + }) } #[allow(clippy::too_many_arguments)] @@ -2009,9 +2264,24 @@ impl EvidenceSink { let _ = writeln!( self.ticks, "{},{:.3},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.4},{:.3},{:.3},{:.3},{:.3},{},{},{}", - step, t, pos[0], pos[1], pos[2], - accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], - pwm[0], pwm[1], pwm[2], pwm[3], i, n, m, + step, + t, + pos[0], + pos[1], + pos[2], + accel[0], + accel[1], + accel[2], + gyro[0], + gyro[1], + gyro[2], + pwm[0], + pwm[1], + pwm[2], + pwm[3], + i, + n, + m, ); } @@ -2065,7 +2335,11 @@ impl EvidenceSink { } fn finish(&mut self, pass: bool) { - let _ = writeln!(self.harness, "verdict: {}", if pass { "PASS" } else { "FAIL" }); + let _ = writeln!( + self.harness, + "verdict: {}", + if pass { "PASS" } else { "FAIL" } + ); let _ = self.harness.flush(); let _ = self.ticks.flush(); let _ = &self.timestamp; @@ -2082,7 +2356,10 @@ fn build_gazebo(args: &[String], world: String, model: String) -> GazeboPhysics Some(s) => parse_home(&s).expect("--home=lat,lon,alt_m"), None => physics::Home::ORIGIN, }; - println!(" gazebo home: lat={} lon={} alt={} m", home.lat_deg, home.lon_deg, home.alt_m); + println!( + " gazebo home: lat={} lon={} alt={} m", + home.lat_deg, home.lon_deg, home.alt_m + ); GazeboPhysics::connect_with_home(world, model, home) .expect("connect_with_home: gz-transport connect failed; is `gz sim` running?") } @@ -2097,11 +2374,17 @@ fn build_gazebo(_args: &[String], world: String, model: String) -> GazeboPhysics #[cfg(feature = "gazebo")] fn parse_home(s: &str) -> Option { let parts: Vec<&str> = s.split(',').collect(); - if parts.len() != 3 { return None; } + if parts.len() != 3 { + return None; + } let lat_deg: f64 = parts[0].parse().ok()?; let lon_deg: f64 = parts[1].parse().ok()?; let alt_m: f64 = parts[2].parse().ok()?; - Some(physics::Home { lat_deg, lon_deg, alt_m }) + Some(physics::Home { + lat_deg, + lon_deg, + alt_m, + }) } fn arg(args: &[String], key: &str) -> Option { @@ -2188,7 +2471,10 @@ mod tests { fn production_flightcore_holds_altitude_through_sitl_plant() { let mut p = MockPhysics::at_rest(); let ok = run_flightcore(&mut p, 2.0, 25.0, None, None); - assert!(ok, "production FlightCore failed to hold 2 m altitude on the SITL plant"); + assert!( + ok, + "production FlightCore failed to hold 2 m altitude on the SITL plant" + ); // The true state stayed finite (no divergence / NaN escape). for i in 0..3 { assert!(p.p_ned[i].is_finite(), "p_ned[{i}] = {}", p.p_ned[i]); @@ -2237,12 +2523,25 @@ mod tests { let tmp = std::env::temp_dir().join(format!("fsg-bench-{}", std::process::id())); let _ = fs::remove_dir_all(&tmp); let mut sink = EvidenceSink::open(&tmp, "mock", "hover").expect("open"); - sink.write_tick(0, 0.0, [0.0; 3], [0.0; 3], [0.0; 3], [0.7; 4], Some((1, 2, 3))); + sink.write_tick( + 0, + 0.0, + [0.0; 3], + [0.0; 3], + [0.0; 3], + [0.7; 4], + Some((1, 2, 3)), + ); sink.write_summary(1, 0.5, 0.0, 0.5, 0.01, Some((1, 2, 3))); sink.finish(true); // Both files exist; the CSV has the header line + one data row. let entries: Vec<_> = fs::read_dir(&tmp).unwrap().collect(); - assert_eq!(entries.len(), 2, "expected harness.log + ticks.csv in {:?}", tmp); + assert_eq!( + entries.len(), + 2, + "expected harness.log + ticks.csv in {:?}", + tmp + ); let _ = fs::remove_dir_all(&tmp); } @@ -2272,7 +2571,7 @@ mod tests { fn fault_tolerance_chain_recovers_from_rotor_loss() { use relay_geo::{GeoAtt, GeoGains}; use relay_iekf::RotorFaultDetector; - use relay_mix_quad::{motors_to_torque_signs, QuadMixer}; + use relay_mix_quad::{QuadMixer, motors_to_torque_signs}; let ctrl = GeoAtt::new(GeoGains::FALCON_QUAD); let j = GeoGains::FALCON_QUAD.j; @@ -2353,7 +2652,11 @@ mod tests { } } let final_tilt = (r[2][2].clamp(-1.0, 1.0)).acos(); - assert_eq!(isolated, Some(real_failed), "FDI must isolate the failed rotor"); + assert_eq!( + isolated, + Some(real_failed), + "FDI must isolate the failed rotor" + ); // The body does NOT tumble (never inverts past ~80°) and SETTLES // back toward upright — the reduced-attitude law recovers the // thrust axis. (Full hover incl. the periodic spin solution is the @@ -2451,7 +2754,7 @@ mod tests { /// trajectory generator + flatness feedforward + controller compose. #[test] fn mission_follows_waypoint_trajectory() { - use relay_geo::{desired_attitude, thrust_axis_ned, flatness_omega_ff, GeoAtt, GeoGains}; + use relay_geo::{GeoAtt, GeoGains, desired_attitude, flatness_omega_ff, thrust_axis_ned}; use relay_traj::Segment3; let j = [0.0217f32, 0.0217, 0.04]; @@ -2492,7 +2795,11 @@ mod tests { let b3_d = thrust_axis_ned(a_cmd).unwrap(); let r_d = desired_attitude(b3_d, 0.0).unwrap(); let c = { - let d = [g_ned[0] - a_cmd[0], g_ned[1] - a_cmd[1], g_ned[2] - a_cmd[2]]; + let d = [ + g_ned[0] - a_cmd[0], + g_ned[1] - a_cmd[1], + g_ned[2] - a_cmd[2], + ]; (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt() }; let off = flatness_omega_ff(a_cmd, s.jerk, 0.0, 0.0).unwrap_or([0.0; 3]); @@ -2523,7 +2830,11 @@ mod tests { } // Reached the waypoint at the end of the leg. let err = { - let d = [p[0] - wps[leg + 1][0], p[1] - wps[leg + 1][1], p[2] - wps[leg + 1][2]]; + let d = [ + p[0] - wps[leg + 1][0], + p[1] - wps[leg + 1][1], + p[2] - wps[leg + 1][2], + ]; (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]).sqrt() }; assert!(err < 0.4, "leg {leg}: missed waypoint by {err} m (p={p:?})"); @@ -2552,7 +2863,11 @@ mod tests { let set = RecoverableSet::new(10.0, 0.04, 1.6); // k_R, λ_M(J), Ψ_max let dt = 0.001f32; // Tilt 0.4 rad in roll. - let r0 = [[1.0f32, 0.0, 0.0], [0.0, 0.92106, -0.38942], [0.0, 0.38942, 0.92106]]; + let r0 = [ + [1.0f32, 0.0, 0.0], + [0.0, 0.92106, -0.38942], + [0.0, 0.38942, 0.92106], + ]; // Destabilizing policy: POSITIVE attitude feedback (unstable). let bad = |r: &[[f32; 3]; 3]| { let e = GeoAtt::attitude_error(r, &r_d); @@ -2584,7 +2899,10 @@ mod tests { max_psi_agile = psi; } } - assert!(max_psi_agile > 1.9, "bad policy alone should leave {{Ψ<2}}: max {max_psi_agile}"); + assert!( + max_psi_agile > 1.9, + "bad policy alone should leave {{Ψ<2}}: max {max_psi_agile}" + ); // Run 2 — SHIELDED: stays in {Ψ<2} and recovers. let mut sh = SimplexShield::new(set, 0.15, 0.5); @@ -2621,7 +2939,11 @@ mod tests { #[cfg(test)] fn integ_rot(r: &[[f32; 3]; 3], w: [f32; 3], dt: f32) -> [[f32; 3]; 3] { let wd = [w[0] * dt, w[1] * dt, w[2] * dt]; - let incr = [[1.0, -wd[2], wd[1]], [wd[2], 1.0, -wd[0]], [-wd[1], wd[0], 1.0]]; + let incr = [ + [1.0, -wd[2], wd[1]], + [wd[2], 1.0, -wd[0]], + [-wd[1], wd[0], 1.0], + ]; let mut m = [[0.0f32; 3]; 3]; for i in 0..3 { for jj in 0..3 { @@ -2648,6 +2970,10 @@ mod tests { e0[2] * e1[0] - e0[0] * e1[2], e0[0] * e1[1] - e0[1] * e1[0], ]; - [[e0[0], e1[0], e2[0]], [e0[1], e1[1], e2[1]], [e0[2], e1[2], e2[2]]] + [ + [e0[0], e1[0], e2[0]], + [e0[1], e1[1], e2[1]], + [e0[2], e1[2], e2[2]], + ] } } diff --git a/examples/falcon-sitl-gz/src/physics.rs b/examples/falcon-sitl-gz/src/physics.rs index 4b39c59b..a77841ff 100644 --- a/examples/falcon-sitl-gz/src/physics.rs +++ b/examples/falcon-sitl-gz/src/physics.rs @@ -12,7 +12,7 @@ //! the stub for the real bridge. use libm::sqrtf; -use relay_ekf::{quat_mul, ImuSample}; +use relay_ekf::{ImuSample, quat_mul}; // Same physical constants the falcon-sitl-hover SITL uses. pub const INERTIA: f32 = 0.0125; // kg·m² @@ -49,25 +49,33 @@ pub trait Physics { /// "gz isn't publishing" (`imu_recv == 0`) from "gz publishes /// but our subscriber dropped frames" — same diagnostic shape /// as `MavlinkBench`'s `frames_recv` / `gpi_recv` from v0.18.2. - fn counters(&self) -> Option<(u64, u64, u64)> { None } + fn counters(&self) -> Option<(u64, u64, u64)> { + None + } /// v0.19.7 — true NED body velocity (m/s), if the backend supplies /// one. `None` means "no true velocity source; finite-difference /// position yourself". The real gz bridge overrides this with the /// OdometryPublisher twist (deterministic, unlike finite-diff /// NavSat which left the altitude velocity-cascade marginal). - fn velocity_ned(&self) -> Option<[f32; 3]> { None } + fn velocity_ned(&self) -> Option<[f32; 3]> { + None + } /// v0.22 — true NED heading (yaw, rad), if the backend supplies one. /// `None` means "no heading reference". The real gz bridge overrides /// this from the OdometryPublisher pose orientation — the "compass" /// that makes yaw observable for the IEKF (yaw is unobservable from /// IMU+GPS alone, the v0.21 ±130° wander). - fn heading_ned(&self) -> Option { None } + fn heading_ned(&self) -> Option { + None + } /// v0.22 — latest body-frame magnetometer reading (Tesla, NED body /// frame), or `None` if no magnetometer. The real heading source. - fn mag_body_ned(&self) -> Option<[f32; 3]> { None } + fn mag_body_ned(&self) -> Option<[f32; 3]> { + None + } /// v1.113 — per-rotor reported RPM (ESC telemetry), or `None` if the plant /// has no rotor feedback. This is the ACHIEVED-per-rotor source the @@ -77,7 +85,9 @@ pub trait Physics { /// Without it the FDI is inert, so a SITL rotor-out flight cannot exercise /// the recovery. Default `None` (backends without ESC feedback fly without /// rotor-fault detection, exactly as the `FlightBackend` default intends). - fn motor_rpm(&self) -> Option<[i32; 4]> { None } + fn motor_rpm(&self) -> Option<[i32; 4]> { + None + } /// v1.113 — inject a single-rotor failure: rotor `rotor`'s thrust (and its /// reported RPM) drop to zero from now on. The hook the rotor-out scenario @@ -147,7 +157,9 @@ impl MockPhysics { } impl Physics for MockPhysics { - fn name(&self) -> &'static str { "mock" } + fn name(&self) -> &'static str { + "mock" + } fn step(&mut self, motor_pwm: [f32; 4], dt: f32) { // Sum the four motor PWMs into a normalised collective thrust; @@ -182,8 +194,7 @@ impl Physics for MockPhysics { self.q[3] + 0.5 * qdot[3] * dt, ]; let n = sqrtf( - q_new[0] * q_new[0] + q_new[1] * q_new[1] - + q_new[2] * q_new[2] + q_new[3] * q_new[3], + q_new[0] * q_new[0] + q_new[1] * q_new[1] + q_new[2] * q_new[2] + q_new[3] * q_new[3], ); if n > 1.0e-12 { q_new = [q_new[0] / n, q_new[1] / n, q_new[2] / n, q_new[3] / n]; @@ -231,7 +242,10 @@ impl Physics for MockPhysics { fb[3] + noise_std * self.next_unit_normal(), ]; let sample = ImuSample { - time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, + time: relay_ekf::Timestamp { + seconds: 0, + fraction: 0, + }, accel_body, gyro_body, }; @@ -291,7 +305,9 @@ impl GazeboPhysics { #[cfg(not(feature = "gazebo"))] impl Physics for GazeboPhysics { - fn name(&self) -> &'static str { "gazebo (stub)" } + fn name(&self) -> &'static str { + "gazebo (stub)" + } fn step(&mut self, _motor_pwm: [f32; 4], _dt: f32) { eprintln!( @@ -303,7 +319,10 @@ impl Physics for GazeboPhysics { fn measure(&mut self, _noise_std: f32) -> (ImuSample, [f32; 3]) { ( ImuSample { - time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, + time: relay_ekf::Timestamp { + seconds: 0, + fraction: 0, + }, accel_body: [0.0; 3], gyro_body: [0.0; 3], }, @@ -325,9 +344,7 @@ impl Physics for GazeboPhysics { #[cfg(feature = "gazebo")] mod gz_real { - use super::{Physics, ImuSample}; - use std::sync::Mutex; - use std::sync::atomic::{AtomicI32, AtomicU64, Ordering}; + use super::{ImuSample, Physics}; use crossbeam_channel::Receiver; use gz_msgs::actuators::Actuators; use gz_msgs::imu::IMU; @@ -336,6 +353,8 @@ mod gz_real { use gz_msgs::odometry::Odometry; use gz_msgs::pose_v::Pose_V; use gz_transport::{Node, Publisher}; + use std::sync::Mutex; + use std::sync::atomic::{AtomicI32, AtomicU64, Ordering}; // The gz.msgs types (Actuators / IMU / NavSat / Pose_V / Magnetometer / // Odometry) now come from the `gz-msgs` crate (protobuf), imported above — @@ -374,7 +393,11 @@ mod gz_real { /// World-origin default — useful for SDF worlds whose vehicle /// spawns at lat/lon (0, 0) and want raw deltas without an /// anchor. - pub const ORIGIN: Self = Self { lat_deg: 0.0, lon_deg: 0.0, alt_m: 0.0 }; + pub const ORIGIN: Self = Self { + lat_deg: 0.0, + lon_deg: 0.0, + alt_m: 0.0, + }; /// Equirectangular projection of (lat_deg, lon_deg, alt_m) /// to local NED in metres. Down is positive — alt above @@ -455,12 +478,10 @@ mod gz_real { let model = model.into(); let mut node = Node::new()?; - let imu_topic = format!( - "/world/{world}/model/{model}/link/base_link/sensor/imu_sensor/imu" - ); - let navsat_topic = format!( - "/world/{world}/model/{model}/link/base_link/sensor/navsat_sensor/navsat" - ); + let imu_topic = + format!("/world/{world}/model/{model}/link/base_link/sensor/imu_sensor/imu"); + let navsat_topic = + format!("/world/{world}/model/{model}/link/base_link/sensor/navsat_sensor/navsat"); let odom_topic = format!("/model/{model}/odometry"); let pose_topic = format!("/model/{model}/pose"); let mag_topic = format!( @@ -609,7 +630,10 @@ mod gz_real { .map(|v| [v.x as f32, v.y as f32, v.z as f32]) .unwrap_or([0.0; 3]); last_imu = Some(ImuSample { - time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, + time: relay_ekf::Timestamp { + seconds: 0, + fraction: 0, + }, accel_body: enu_to_ned(accel), gyro_body: enu_to_ned(gyro), }); @@ -638,8 +662,7 @@ mod gz_real { for msg in self.odom_rx.try_iter() { if let Some(tw) = msg.twist.as_ref() { if let Some(lin) = tw.linear.as_ref() { - last_vel = - Some(enu_to_ned([lin.x as f32, lin.y as f32, lin.z as f32])); + last_vel = Some(enu_to_ned([lin.x as f32, lin.y as f32, lin.z as f32])); } } } @@ -721,11 +744,19 @@ mod gz_real { fn measure(&mut self, _noise_std: f32) -> (ImuSample, [f32; 3]) { self.pump(); - let sample = self.latest_imu.lock().unwrap().clone().unwrap_or(ImuSample { - time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, - accel_body: [0.0; 3], - gyro_body: [0.0; 3], - }); + let sample = self + .latest_imu + .lock() + .unwrap() + .clone() + .unwrap_or(ImuSample { + time: relay_ekf::Timestamp { + seconds: 0, + fraction: 0, + }, + accel_body: [0.0; 3], + gyro_body: [0.0; 3], + }); let pos = *self.latest_position_ned_m.lock().unwrap(); (sample, pos) } @@ -819,7 +850,11 @@ mod gz_real { // `MavlinkBench::Home::project_ned_cm` tests in // examples/falcon-hitl-rfspoof/src/mavlink.rs. fn budapest_home() -> Home { - Home { lat_deg: 47.5023456, lon_deg: 19.0401234, alt_m: 120.0 } + Home { + lat_deg: 47.5023456, + lon_deg: 19.0401234, + alt_m: 120.0, + } } #[test] @@ -841,7 +876,11 @@ mod gz_real { #[test] fn lat_step_translates_to_north() { - let h = Home { lat_deg: 0.0, lon_deg: 0.0, alt_m: 0.0 }; + let h = Home { + lat_deg: 0.0, + lon_deg: 0.0, + alt_m: 0.0, + }; // 1° of latitude ≈ 111_195 m on a 6_371_000 m-radius sphere. let p = h.project_to_ned_m(1.0, 0.0, 0.0); assert!((p[0] - 111_195.0).abs() < 1.0, "north = {}", p[0]); @@ -871,7 +910,10 @@ mod tests { // → zero thrust → falls under gravity; check the fall is // physically reasonable). p.step([0.0; 4], 0.01); - assert!(p.v_ned[2] > 0.0, "no thrust → should accelerate down (+z NED)"); + assert!( + p.v_ned[2] > 0.0, + "no thrust → should accelerate down (+z NED)" + ); assert!(p.v_ned[2] < 1.0, "1 step at dt=0.01 → v ≈ g*dt = 0.098 m/s"); } diff --git a/examples/falcon-sitl-hover/src/main.rs b/examples/falcon-sitl-hover/src/main.rs index 10b82615..dda619ba 100644 --- a/examples/falcon-sitl-hover/src/main.rs +++ b/examples/falcon-sitl-hover/src/main.rs @@ -46,18 +46,18 @@ use std::time::Instant; use libm::sqrtf; use relay_att::{AttController, Timestamp as AttTimestamp}; -use relay_ekf::{quat_mul, Ekf, ImuSample, Timestamp as EkfTimestamp}; +use relay_ekf::{Ekf, ImuSample, Timestamp as EkfTimestamp, quat_mul}; +use relay_lc::engine::Geofence; use relay_mix_quad::QuadMixer; use relay_pos::{PosController, PositionSetpoint, Timestamp as PosTimestamp}; -use relay_lc::engine::Geofence; use relay_rate::{RatePid, Timestamp as RateTimestamp}; use relay_sc::engine::{CommandStore, RtsCommand}; const SAMPLE_RATE_HZ: f32 = 1000.0; const TRAJECTORY_SECONDS: f32 = 5.0; const GRAVITY: f32 = 9.81; -const INERTIA: f32 = 0.005; // kg·m², 500 g, 10-inch quad -const FRICTION: f32 = 0.001; // rad/s damping coefficient +const INERTIA: f32 = 0.005; // kg·m², 500 g, 10-inch quad +const FRICTION: f32 = 0.001; // rad/s damping coefficient /// Thrust scale: normalised thrust 0.5 produces 1g acceleration at hover. const THRUST_SCALE: f32 = 19.62; const DRAG_COEFFICIENT: f32 = 0.4; @@ -130,8 +130,7 @@ impl Plant { fn step(&mut self, torque: [f32; 3], dt: f32) { // omega_dot = (torque - friction*omega) / inertia for i in 0..3 { - self.omega[i] += - ((torque[i] - FRICTION * self.omega[i]) / INERTIA) * dt; + self.omega[i] += ((torque[i] - FRICTION * self.omega[i]) / INERTIA) * dt; } self.integrate_quaternion(dt); } @@ -142,8 +141,7 @@ impl Plant { fn step_full(&mut self, torque: [f32; 3], thrust_normalised: f32, dt: f32) { // Rotational. for i in 0..3 { - self.omega[i] += - ((torque[i] - FRICTION * self.omega[i]) / INERTIA) * dt; + self.omega[i] += ((torque[i] - FRICTION * self.omega[i]) / INERTIA) * dt; } self.integrate_quaternion(dt); @@ -175,8 +173,7 @@ impl Plant { self.q[3] + 0.5 * qdot[3] * dt, ]; let n = sqrtf( - q_new[0] * q_new[0] + q_new[1] * q_new[1] - + q_new[2] * q_new[2] + q_new[3] * q_new[3], + q_new[0] * q_new[0] + q_new[1] * q_new[1] + q_new[2] * q_new[2] + q_new[3] * q_new[3], ); if n > 1.0e-12 { q_new = [q_new[0] / n, q_new[1] / n, q_new[2] / n, q_new[3] / n]; @@ -249,22 +246,34 @@ struct ScenarioResult { fn ekf_ts_of(secs: f32) -> EkfTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - EkfTimestamp { seconds: secs as u64, fraction: frac } + EkfTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn rate_ts_of(secs: f32) -> RateTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - RateTimestamp { seconds: secs as u64, fraction: frac } + RateTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn att_ts_of(secs: f32) -> AttTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - AttTimestamp { seconds: secs as u64, fraction: frac } + AttTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn pos_ts_of(secs: f32) -> PosTimestamp { let frac = ((secs.fract() as f64) * ((1u64 << 32) as f64)) as u32; - PosTimestamp { seconds: secs as u64, fraction: frac } + PosTimestamp { + seconds: secs as u64, + fraction: frac, + } } fn quat_error_deg(a: [f32; 4], b: [f32; 4]) -> f32 { @@ -402,9 +411,8 @@ fn run_disturbance(noise_std: f32) -> ScenarioResult { plant.step(torque, dt); if i > impulse_step && i <= impulse_step + recovery_window { - let mag = sqrtf( - plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2), - ); + let mag = + sqrtf(plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2)); if mag > peak_after_impulse { peak_after_impulse = mag; } @@ -461,9 +469,7 @@ fn run_hover(noise_std: f32) -> ScenarioResult { } } plant.step(torque, dt); - let mag = sqrtf( - plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2), - ); + let mag = sqrtf(plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2)); if mag > peak { peak = mag; } @@ -477,13 +483,8 @@ fn run_hover(noise_std: f32) -> ScenarioResult { } let elapsed = t0.elapsed().as_micros(); - let final_mag = sqrtf( - plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2), - ); - let pass = !nan_seen - && !convergence.is_nan() - && convergence <= 1.0 - && final_mag <= 0.02; + let final_mag = sqrtf(plant.omega[0].powi(2) + plant.omega[1].powi(2) + plant.omega[2].powi(2)); + let pass = !nan_seen && !convergence.is_nan() && convergence <= 1.0 && final_mag <= 0.02; ScenarioResult { label: "hover", @@ -519,7 +520,7 @@ fn run_mission(noise_std: f32) -> ScenarioResult { let setpoint = PositionSetpoint::hover_at(waypoint); let pos_decimation = 20_usize; // 1 kHz / 20 = 50 Hz pos rate - let att_decimation = 4_usize; // 1 kHz / 4 = 250 Hz att rate + let att_decimation = 4_usize; // 1 kHz / 4 = 250 Hz att rate let mut current_attitude_setpoint = [1.0_f32, 0.0, 0.0, 0.0]; let mut current_thrust = 0.5_f32; // start at hover let mut current_rate_setpoint = [0.0_f32; 3]; @@ -559,11 +560,8 @@ fn run_mission(noise_std: f32) -> ScenarioResult { } // 3. Attitude loop (250 Hz). if i % att_decimation == 0 { - current_rate_setpoint = att.tick( - att_ts_of(t), - st.quaternion, - current_attitude_setpoint, - ); + current_rate_setpoint = + att.tick(att_ts_of(t), st.quaternion, current_attitude_setpoint); } // 4. Rate loop (1 kHz). let torque = rate_pid.tick(rate_ts_of(t), gyro, current_rate_setpoint); @@ -617,11 +615,11 @@ fn run_mission(noise_std: f32) -> ScenarioResult { ScenarioResult { label: "mission", samples: n, - final_omega: plant.v_ned, // repurpose final-ω slot for final velocity + final_omega: plant.v_ned, // repurpose final-ω slot for final velocity peak_omega_after_setup: peak_dist_err, // peak distance error (m) rms_error_steady: rms_steady, convergence_time_s: convergence, - overshoot_pct: final_dist, // repurpose overshoot slot for final distance (m) + overshoot_pct: final_dist, // repurpose overshoot slot for final distance (m) nan_seen, elapsed_micros: elapsed, pass, @@ -671,8 +669,7 @@ fn run_attitude(noise_std: f32) -> ScenarioResult { } // Outer loop (250 Hz): refresh rate setpoint. if i % att_decimation == 0 { - current_rate_setpoint = - att.tick(att_ts_of(t), st.quaternion, attitude_setpoint); + current_rate_setpoint = att.tick(att_ts_of(t), st.quaternion, attitude_setpoint); } // Inner loop (1 kHz): rate PID -> torque. let torque = pid.tick(rate_ts_of(t), gyro, current_rate_setpoint); @@ -952,10 +949,10 @@ fn run_untethered(noise_std: f32) -> ScenarioResult { // Waypoints table — command payload_offset indexes here. let waypoints: [[f32; 3]; 4] = [ - [10.0, 0.0, 0.0], // 0: north 10 m - [0.0, 10.0, 0.0], // 1: east 10 m - [-10.0, 0.0, 0.0], // 2: south 10 m - [0.0, 0.0, 0.0], // 3: home + [10.0, 0.0, 0.0], // 0: north 10 m + [0.0, 10.0, 0.0], // 1: east 10 m + [-10.0, 0.0, 0.0], // 2: south 10 m + [0.0, 0.0, 0.0], // 3: home ]; let leg_secs: u32 = 8; for (k, _wp) in waypoints.iter().enumerate() { @@ -1055,9 +1052,8 @@ fn run_untethered(noise_std: f32) -> ScenarioResult { let visit_threshold = 2.5_f32; let visited = min_dist_per_wp.iter().all(|&d| d <= visit_threshold); - let final_dist_home = sqrtf( - plant.p_ned[0].powi(2) + plant.p_ned[1].powi(2) + plant.p_ned[2].powi(2), - ); + let final_dist_home = + sqrtf(plant.p_ned[0].powi(2) + plant.p_ned[1].powi(2) + plant.p_ned[2].powi(2)); let peak_visit_err = min_dist_per_wp.iter().cloned().fold(0.0_f32, f32::max); let pass = !nan_seen && visited && final_dist_home <= visit_threshold; @@ -1065,15 +1061,15 @@ fn run_untethered(noise_std: f32) -> ScenarioResult { ScenarioResult { label: "untethered", samples: n, - final_omega: plant.p_ned, // final NED position - peak_omega_after_setup: peak_visit_err, // worst-case min approach + final_omega: plant.p_ned, // final NED position + peak_omega_after_setup: peak_visit_err, // worst-case min approach rms_error_steady: 0.0, convergence_time_s: if visited { (waypoints.len() as f32) * (leg_secs as f32) } else { f32::NAN }, - overshoot_pct: final_dist_home, // final distance home (m) + overshoot_pct: final_dist_home, // final distance home (m) nan_seen, elapsed_micros: elapsed, pass, @@ -1242,32 +1238,54 @@ fn print_result(r: &ScenarioResult) { println!("--- scenario: {} ---", r.label); println!(" samples {}", r.samples); if r.label == "mission" { - println!(" final v (m/s NED) [{:+.3}, {:+.3}, {:+.3}]", - r.final_omega[0], r.final_omega[1], r.final_omega[2]); + println!( + " final v (m/s NED) [{:+.3}, {:+.3}, {:+.3}]", + r.final_omega[0], r.final_omega[1], r.final_omega[2] + ); println!(" peak distance error {:.3} m", r.peak_omega_after_setup); println!(" final distance {:.3} m", r.overshoot_pct); - println!(" RMS distance (steady){:.3} m (last 2s)", r.rms_error_steady); + println!( + " RMS distance (steady){:.3} m (last 2s)", + r.rms_error_steady + ); } else if r.label == "fault" { - println!(" final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", - r.final_omega[0], r.final_omega[1], r.final_omega[2]); + println!( + " final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", + r.final_omega[0], r.final_omega[1], r.final_omega[2] + ); println!(" peak EKF innovation {:.4}", r.peak_omega_after_setup); } else if r.label == "untethered" { - println!(" final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", - r.final_omega[0], r.final_omega[1], r.final_omega[2]); + println!( + " final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", + r.final_omega[0], r.final_omega[1], r.final_omega[2] + ); println!(" worst waypoint min {:.3} m", r.peak_omega_after_setup); println!(" final distance home {:.3} m", r.overshoot_pct); } else if r.label == "geofence" { - println!(" final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", - r.final_omega[0], r.final_omega[1], r.final_omega[2]); - println!(" peak true-N (truth) {:+.2} m (fence at +15.00 m)", r.peak_omega_after_setup); + println!( + " final position (NED) [{:+.2}, {:+.2}, {:+.2}] m", + r.final_omega[0], r.final_omega[1], r.final_omega[2] + ); + println!( + " peak true-N (truth) {:+.2} m (fence at +15.00 m)", + r.peak_omega_after_setup + ); } else { - println!(" final ω (rad/s) [{:+.4}, {:+.4}, {:+.4}]", - r.final_omega[0], r.final_omega[1], r.final_omega[2]); + println!( + " final ω (rad/s) [{:+.4}, {:+.4}, {:+.4}]", + r.final_omega[0], r.final_omega[1], r.final_omega[2] + ); if r.label == "attitude" { println!(" peak attitude err {:.3}°", r.peak_omega_after_setup); - println!(" RMS error (steady) {:.3}° (last 1s)", r.rms_error_steady); + println!( + " RMS error (steady) {:.3}° (last 1s)", + r.rms_error_steady + ); } else { - println!(" peak ω above sp {:.4} rad/s", r.peak_omega_after_setup); + println!( + " peak ω above sp {:.4} rad/s", + r.peak_omega_after_setup + ); } } if r.label == "step" { @@ -1281,9 +1299,15 @@ fn print_result(r: &ScenarioResult) { println!(" convergence/recovery never"); } } else if r.label == "disturbance" { - println!(" recovery time {:.3}s after impulse", r.convergence_time_s); + println!( + " recovery time {:.3}s after impulse", + r.convergence_time_s + ); } else if r.label == "fault" { - println!(" RTL detection {:.3}s after fault injection", r.convergence_time_s); + println!( + " RTL detection {:.3}s after fault injection", + r.convergence_time_s + ); } else if r.label == "untethered" { println!(" mission completed in {:.1}s", r.convergence_time_s); } else if r.label == "geofence" { @@ -1293,7 +1317,10 @@ fn print_result(r: &ScenarioResult) { } println!(" loop wall time {} µs", r.elapsed_micros); println!(" NaN/∞ seen {}", r.nan_seen); - println!(" outcome {}", if r.pass { "PASS" } else { "FAIL" }); + println!( + " outcome {}", + if r.pass { "PASS" } else { "FAIL" } + ); } fn print_help() { @@ -1337,7 +1364,10 @@ fn main() -> ExitCode { Some("geofence") => scenario = Scenario::Geofence, Some("all") => scenario = Scenario::All, other => { - eprintln!("error: --scenario expects step|disturbance|hover|attitude|mission|fault|untethered|geofence|all, got {:?}", other); + eprintln!( + "error: --scenario expects step|disturbance|hover|attitude|mission|fault|untethered|geofence|all, got {:?}", + other + ); return ExitCode::from(2); } }, @@ -1402,7 +1432,11 @@ fn main() -> ExitCode { println!("falcon-sitl-hover: PASS"); ExitCode::SUCCESS } else { - let failed: Vec<&str> = results.iter().filter(|r| !r.pass).map(|r| r.label).collect(); + let failed: Vec<&str> = results + .iter() + .filter(|r| !r.pass) + .map(|r| r.label) + .collect(); println!("falcon-sitl-hover: FAIL ({})", failed.join(", ")); ExitCode::from(1) } @@ -1451,8 +1485,11 @@ mod tests { assert!(r.pass, "attitude result: {:?}", r); assert!(!r.nan_seen); assert!(r.convergence_time_s <= 1.5); - assert!(r.rms_error_steady <= 2.0, - "attitude RMS-steady {:.3}° exceeds 2° budget", r.rms_error_steady); + assert!( + r.rms_error_steady <= 2.0, + "attitude RMS-steady {:.3}° exceeds 2° budget", + r.rms_error_steady + ); } #[test] @@ -1469,10 +1506,16 @@ mod tests { let r = run_mission(0.0); assert!(r.pass, "mission result: {:?}", r); assert!(!r.nan_seen); - assert!(r.convergence_time_s <= 10.0, - "mission convergence {} exceeds 10 s", r.convergence_time_s); - assert!(r.overshoot_pct <= 0.5, - "final distance {} m exceeds 0.5 m", r.overshoot_pct); + assert!( + r.convergence_time_s <= 10.0, + "mission convergence {} exceeds 10 s", + r.convergence_time_s + ); + assert!( + r.overshoot_pct <= 0.5, + "final distance {} m exceeds 0.5 m", + r.overshoot_pct + ); } #[test] @@ -1480,8 +1523,11 @@ mod tests { let r = run_mission(0.05); assert!(!r.nan_seen); // Looser budget with noise. - assert!(r.overshoot_pct <= 1.5, - "noisy mission final distance {} m exceeds 1.5 m", r.overshoot_pct); + assert!( + r.overshoot_pct <= 1.5, + "noisy mission final distance {} m exceeds 1.5 m", + r.overshoot_pct + ); } #[test] @@ -1490,11 +1536,16 @@ mod tests { assert!(r.pass, "fault result: {:?}", r); assert!(!r.nan_seen); assert!(!r.convergence_time_s.is_nan(), "RTL never triggered"); - assert!(r.convergence_time_s <= 0.5, - "RTL detection latency {:.3}s exceeds 0.5s budget", r.convergence_time_s); - assert!(r.peak_omega_after_setup > 0.4, + assert!( + r.convergence_time_s <= 0.5, + "RTL detection latency {:.3}s exceeds 0.5s budget", + r.convergence_time_s + ); + assert!( + r.peak_omega_after_setup > 0.4, "fault should drive EKF innovation past the 0.4 limit, got {:.3}", - r.peak_omega_after_setup); + r.peak_omega_after_setup + ); } #[test] @@ -1503,7 +1554,10 @@ mod tests { assert!(!r.nan_seen); // A hard accelerometer bias dwarfs the IMU noise floor — RTL // must still latch, just with a looser latency budget. - assert!(!r.convergence_time_s.is_nan(), "RTL never triggered under noise"); + assert!( + !r.convergence_time_s.is_nan(), + "RTL never triggered under noise" + ); assert!(r.convergence_time_s <= 1.0); } @@ -1529,7 +1583,10 @@ mod tests { wd.observe(0.02); } } - assert!(!wd.rtl_active(), "watchdog tripped on isolated noise spikes"); + assert!( + !wd.rtl_active(), + "watchdog tripped on isolated noise spikes" + ); } #[test] @@ -1559,9 +1616,11 @@ mod tests { assert!(!r.nan_seen); // Truth genuinely drove past the +15 m fence — proves the // spoof was effective (the controller can't see this). - assert!(r.peak_omega_after_setup > 15.0, + assert!( + r.peak_omega_after_setup > 15.0, "peak true-N {:.3} m did not exceed +15 m fence", - r.peak_omega_after_setup); + r.peak_omega_after_setup + ); assert!(!r.convergence_time_s.is_nan(), "geofence never latched"); } @@ -1572,11 +1631,17 @@ mod tests { assert!(!r.nan_seen); assert!(!r.convergence_time_s.is_nan(), "mission did not complete"); // peak_omega_after_setup holds the worst-case min approach (m). - assert!(r.peak_omega_after_setup <= 2.5, - "worst waypoint min {:.3} m exceeded 2.5 m budget", r.peak_omega_after_setup); + assert!( + r.peak_omega_after_setup <= 2.5, + "worst waypoint min {:.3} m exceeded 2.5 m budget", + r.peak_omega_after_setup + ); // overshoot_pct holds the final distance home (m). - assert!(r.overshoot_pct <= 2.5, - "final distance home {:.3} m exceeded 2.5 m budget", r.overshoot_pct); + assert!( + r.overshoot_pct <= 2.5, + "final distance home {:.3} m exceeded 2.5 m budget", + r.overshoot_pct + ); } #[test] @@ -1585,8 +1650,7 @@ mod tests { for _ in 0..1000 { plant.step_full([0.001, -0.001, 0.0005], 0.5, 1.0 / 1000.0); let n = sqrtf( - plant.q[0].powi(2) + plant.q[1].powi(2) - + plant.q[2].powi(2) + plant.q[3].powi(2), + plant.q[0].powi(2) + plant.q[1].powi(2) + plant.q[2].powi(2) + plant.q[3].powi(2), ); assert!((n - 1.0).abs() < 1.0e-3); for k in 0..3 { @@ -1604,8 +1668,7 @@ mod tests { let n = sqrtf( plant.q[0].powi(2) + plant.q[1].powi(2) + plant.q[2].powi(2) + plant.q[3].powi(2), ); - assert!((n - 1.0).abs() < 1.0e-3, - "plant quaternion non-unit: {}", n); + assert!((n - 1.0).abs() < 1.0e-3, "plant quaternion non-unit: {}", n); } } } diff --git a/host/falcon-config/src/lib.rs b/host/falcon-config/src/lib.rs index 4d4404a8..a3df4c88 100644 --- a/host/falcon-config/src/lib.rs +++ b/host/falcon-config/src/lib.rs @@ -121,11 +121,19 @@ pub struct PosCfg { // Priority for now: airmode gives the yaw loop FULL authority, which the // negative-b0 yaw WORKAROUND can't yet handle (it diverges). Airmode // becomes the default once the yaw sign is properly fixed (Track A #4). -fn default_mixer_mode() -> MixerMode { MixerMode::Priority } +fn default_mixer_mode() -> MixerMode { + MixerMode::Priority +} -fn default_loop_dt() -> f32 { 0.001 } -fn default_outer_decim() -> u32 { 10 } -fn default_gyro_lpf_hz() -> f32 { 60.0 } +fn default_loop_dt() -> f32 { + 0.001 +} +fn default_outer_decim() -> u32 { + 10 +} +fn default_gyro_lpf_hz() -> f32 { + 60.0 +} /// The whole falcon-quad tuning set. #[derive(Clone, Copy, Debug, Serialize, Deserialize)] @@ -165,7 +173,11 @@ impl FalconConfig { } pub fn to_geo_gains(&self) -> relay_geo::GeoGains { - relay_geo::GeoGains { k_r: self.geo.k_r, k_omega: self.geo.k_omega, j: self.geo.j } + relay_geo::GeoGains { + k_r: self.geo.k_r, + k_omega: self.geo.k_omega, + j: self.geo.j, + } } pub fn to_adrc(&self) -> relay_adrc::AdrcRate { @@ -204,8 +216,18 @@ impl Default for FalconConfig { j: [0.0217, 0.0217, 0.04], }, adrc: [ - AdrcCfg { omega_o: 40.0, omega_c: 12.0, b0: 30.0, tau: 0.0125 }, - AdrcCfg { omega_o: 40.0, omega_c: 12.0, b0: 30.0, tau: 0.0125 }, + AdrcCfg { + omega_o: 40.0, + omega_c: 12.0, + b0: 30.0, + tau: 0.0125, + }, + AdrcCfg { + omega_o: 40.0, + omega_c: 12.0, + b0: 30.0, + tau: 0.0125, + }, // Yaw: ω_o high / ω_c low (control bw below the motor pole) // + actuator lag τ=0.025 in the ESO. b0 is NEGATIVE (−6), // and that is now *validated*, not a fudge: the frame-yaw @@ -219,7 +241,12 @@ impl Default for FalconConfig { // body torque unchanged) but showed WORSE in limited gz runs // (0/6 vs 3/4) — unresolved (gz startup nondeterminism vs a // hidden asymmetry), so we keep the verified-good b0=−6. - AdrcCfg { omega_o: 30.0, omega_c: 3.0, b0: -6.0, tau: 0.025 }, + AdrcCfg { + omega_o: 30.0, + omega_c: 3.0, + b0: -6.0, + tau: 0.025, + }, ], pos: PosCfg { // Gentle horizontal gains: high gains excited the limit @@ -236,7 +263,7 @@ impl Default for FalconConfig { mixer_floor: 0.2, use_adrc: true, yaw_mode: YawMode::RateHold, - loop_dt: default_loop_dt(), // 1 kHz inner loop + loop_dt: default_loop_dt(), // 1 kHz inner loop outer_decim: default_outer_decim(), // 100 Hz outer loop gyro_lpf_hz: default_gyro_lpf_hz(), // 60 Hz gyro LPF mixer_mode: default_mixer_mode(), // priority diff --git a/host/relay-sb/examples/intercore.rs b/host/relay-sb/examples/intercore.rs index 4ea975d6..1ee240cc 100644 --- a/host/relay-sb/examples/intercore.rs +++ b/host/relay-sb/examples/intercore.rs @@ -58,7 +58,11 @@ fn ring_view(used: usize) -> String { fn hex(bytes: &[u8], max: usize) -> String { let n = bytes.len().min(max); - let mut s = bytes[..n].iter().map(|b| format!("{b:02x}")).collect::>().join(""); + let mut s = bytes[..n] + .iter() + .map(|b| format!("{b:02x}")) + .collect::>() + .join(""); if bytes.len() > max { s.push('…'); } @@ -116,7 +120,15 @@ fn main() { let mut buf = [0u8; FRAME_CAP]; let n = m7.wrap(format!("MSG{i}").as_bytes(), &mut buf).unwrap(); let ok = mailbox.push(Frame { buf, len: n }); - println!(" push MSG{i} → {} {}", if ok { format!("{G}accepted{X}") } else { format!("{R}refused{X}") }, ring_view(mailbox.len())); + println!( + " push MSG{i} → {} {}", + if ok { + format!("{G}accepted{X}") + } else { + format!("{R}refused{X}") + }, + ring_view(mailbox.len()) + ); beat(450); } let mut buf = [0u8; FRAME_CAP]; @@ -124,7 +136,11 @@ fn main() { let refused = !mailbox.push(Frame { buf, len: n }); println!( " push OVERFLOW → {Y}⊘ {}{X} {} {DIM}(MessageTransport::push → false){X}", - if refused { "BACKPRESSURE: refused, head intact" } else { "??" }, + if refused { + "BACKPRESSURE: refused, head intact" + } else { + "??" + }, ring_view(mailbox.len()) ); while mailbox.pop().is_some() {} // drain diff --git a/host/relay-sb/src/core.rs b/host/relay-sb/src/core.rs index 19be5f0a..b6002f19 100644 --- a/host/relay-sb/src/core.rs +++ b/host/relay-sb/src/core.rs @@ -217,7 +217,11 @@ impl SoftwareBus { } /// Subscribe a component to a channel. - pub fn subscribe(&mut self, channel: ChannelId, subscriber: SubscriberId) -> Result<(), SbError> { + pub fn subscribe( + &mut self, + channel: ChannelId, + subscriber: SubscriberId, + ) -> Result<(), SbError> { let result = self.subscriptions.subscribe(channel, subscriber); if result.is_ok() { self.stats.subscriptions_active = self.subscriptions.subscriber_count(); @@ -226,7 +230,11 @@ impl SoftwareBus { } /// Unsubscribe a component from a channel. - pub fn unsubscribe(&mut self, channel: ChannelId, subscriber: SubscriberId) -> Result<(), SbError> { + pub fn unsubscribe( + &mut self, + channel: ChannelId, + subscriber: SubscriberId, + ) -> Result<(), SbError> { let result = self.subscriptions.unsubscribe(channel, subscriber); if result.is_ok() { self.stats.subscriptions_active = self.subscriptions.subscriber_count();