diff --git a/tests/cascade-sitl-wasm/src/main.rs b/tests/cascade-sitl-wasm/src/main.rs index 57f4da1a..b42cd438 100644 --- a/tests/cascade-sitl-wasm/src/main.rs +++ b/tests/cascade-sitl-wasm/src/main.rs @@ -45,13 +45,13 @@ wasmtime::component::bindgen!({ inline: r#" package host:sitl; world composed-cascade { - export pulseengine:falcon-cascade/controller@0.7.0; + export pulseengine:falcon-cascade/controller@0.8.0; } "#, path: "../../wit/falcon-cascade", }); -use pulseengine::falcon_cascade::types::{ImuSample as WitImu, Waypoint}; +use pulseengine::falcon_cascade::types::{ImuSample as WitImu, SensorFrame, Vec3, Waypoint}; fn main() -> Result<()> { let mut args = std::env::args().skip(1); @@ -114,16 +114,35 @@ fn main() -> Result<()> { println!("schedule : {ticks} ticks @ dt={dt}s ({:.1}s, {:.0} Hz)", ticks as f32 * dt, 1.0 / dt); println!(); + let gnss_div: u32 = std::env::var("GNSS_DIV").ok().and_then(|v| v.parse().ok()) + .unwrap_or_else(|| ((1.0 / dt) / 5.0).round().max(1.0) as u32); + let mut fixes = 0u32; + let mut peak_tilt = 0.0f32; - for _ in 0..ticks { - let (s, _true_pos) = plant.measure(noise); + for tick in 0..ticks { + let (s, true_pos) = plant.measure(noise); let imu = WitImu { ax: s.accel_body[0], ay: s.accel_body[1], az: s.accel_body[2], gx: s.gyro_body[0], gy: s.gyro_body[1], gz: s.gyro_body[2], }; // The tick that matters: one full estimate -> position -> attitude -> // rate -> mixer pass, executed inside the wasm component. - let m = controller.call_step(&mut store, imu, target)?; + // v0.8: the host states its own period and offers what it has. A 5 Hz + // position fix matches the native bench's gnss_div=50 @250 Hz. + let position_ned = if gnss_div > 0 && tick % gnss_div == 0 { + fixes += 1; + Some(Vec3 { x: true_pos[0], y: true_pos[1], z: true_pos[2] }) + } else { + None + }; + let frame = SensorFrame { + imu, + dt_s: dt, + position_ned, + mag_body: None, + heading_rad: None, + }; + let m = controller.call_step(&mut store, frame, target)?; let tilt = (s.accel_body[0].powi(2) + s.accel_body[1].powi(2)).sqrt(); peak_tilt = peak_tilt.max(tilt); plant.step([m.m1, m.m2, m.m3, m.m4], dt); diff --git a/wasm/cm/cascade/Cargo.toml b/wasm/cm/cascade/Cargo.toml index 7a243865..bb301729 100644 --- a/wasm/cm/cascade/Cargo.toml +++ b/wasm/cm/cascade/Cargo.toml @@ -1,7 +1,7 @@ # Standalone cargo-component crate — the cascade orchestrator. [package] name = "falcon-cascade-cm" -description = "Falcon v0.7 — cascade orchestrator (imports 5 controllers, exports step)" +description = "Falcon v0.8 — the flight core as one component (falcon-core::FlightCore)" version = "0.1.0" edition = "2024" license = "Apache-2.0" @@ -15,6 +15,13 @@ crate-type = ["cdylib"] wit-bindgen-rt = { version = "0.41.0", features = ["bitflags"] } falcon-cm-rt = { path = "../../../crates/falcon-cm-rt" } +# THE FLIGHT CORE ITSELF. v0.7 imported five stage interfaces and wrapped +# relay-pos/att/rate — controllers falcon-core dropped on 2026-06-03 when it +# moved to geometric SE(3) + ADRC (#388). Depending on falcon-core directly is +# what makes the published component the vehicle we verify, and it is what the +# scripts/audit-component-deps.rs check exists to keep true. +falcon-core = { path = "../../../crates/falcon-core" } + [package.metadata.component] package = "pulseengine:falcon-cascade" diff --git a/wasm/cm/cascade/src/lib.rs b/wasm/cm/cascade/src/lib.rs index 28738c8c..2b391f0a 100644 --- a/wasm/cm/cascade/src/lib.rs +++ b/wasm/cm/cascade/src/lib.rs @@ -1,22 +1,26 @@ -//! falcon-cascade — the control-cascade orchestrator component. +//! falcon-cascade — THE FLIGHT CORE, as one component. //! -//! Imports the five controller interfaces (`ekf`, `position`, -//! `attitude`, `rate`, `mixer`) and exports the top-level `step`. -//! `step` runs the cascade in order: +//! v0.8 rework. v0.7 imported five stage interfaces (`ekf`, `position`, +//! `attitude`, `rate`, `mixer`) and composed them with `wac plug`. That +//! decomposition was a wasm-side invention which never corresponded to the +//! flight architecture, and it drifted: the stages wrapped relay-pos/att/rate, +//! which falcon-core DROPPED on 2026-06-03 when it moved to geometric SE(3) + +//! ADRC. For three months and ~40 releases the published components implemented +//! a control stack the vehicle does not fly, and nothing noticed because nothing +//! compared the two dependency sets (#388). //! -//! ```text -//! imu ─► ekf.estimate ─► state -//! state, waypoint ─► position.tick ─► attitude-setpoint -//! state, att-sp ─► attitude.tick ─► rate-setpoint -//! state, rate-sp ─► rate.tick ─► torque-setpoint -//! torque-sp ─► mixer.mix ─► motor-pwm -//! ``` +//! This component wraps `falcon_core::FlightCore` directly, so what is published +//! IS what is verified and flown: IEKF estimator, geometric SE(3) attitude, ADRC +//! inner loop, mixer with rotor-out FDI, and the altitude/position loops. //! -//! The imports are satisfied by the five leaf components at -//! composition time — `wac plug` (or falcon-cascade.wac) wires the -//! graph. This component contains no control logic itself: it is -//! pure orchestration, which is the point of the Component Model -//! split — the cascade topology lives in one small, auditable place. +//! It imports nothing. One component, one tick, one clock — which also removes +//! the other half of the defect: the five stages each advanced their own +//! hardcoded clock (20 ms, 4 ms, 1 ms) while the cascade called all five once +//! per tick, so they disagreed with each other by up to 20x. +//! +//! `FlightCore` drives a `FlightBackend` rather than returning motors, so the +//! component supplies a capture backend: the sensor frame goes in as the `read_*` +//! answers, and `write_motors` is caught on the way out. #![cfg_attr(not(feature = "std"), no_std)] @@ -36,23 +40,112 @@ mod bindings; // trait lives under `exports::`. The five imported controller // interfaces live at the bindings root. use bindings::exports::pulseengine::falcon_cascade::controller::Guest; -use bindings::pulseengine::falcon_cascade::{attitude, ekf, mixer, position, rate}; -use bindings::pulseengine::falcon_cascade::types::{ImuSample, MotorPwm, Waypoint}; +use bindings::pulseengine::falcon_cascade::types::{MotorPwm, SensorFrame, Waypoint}; + +use core::cell::RefCell; +use falcon_core::{FlightBackend, FlightCore, ImuSample as CoreImu}; + +/// falcon-core's `Vec3` alias is private, so it is restated here. Same shape +/// (`[f32; 3]`, NED); if that ever diverges this stops compiling, which is the +/// right failure. +type Vec3 = [f32; 3]; + +struct SingleThreaded(RefCell); +// SAFETY: the component model guarantees single-threaded, non-reentrant access. +unsafe impl Sync for SingleThreaded {} + +/// Lazily built: `FlightCore::new` needs the host's loop rate, which only +/// arrives with the first frame. Constructing it on a guessed rate is exactly +/// the defect v0.8 exists to remove. +static CORE: SingleThreaded> = SingleThreaded(RefCell::new(None)); + +/// One tick's sensors in, motors out. +/// +/// `FlightCore` pulls from a backend and pushes motors into it, which is the +/// seam that lets the SAME code run against SITL, Gazebo or real hardware. A +/// component has to answer with a value instead, so this stands in for one tick: +/// every `read_*` returns what the frame carried, and `write_motors` is caught. +/// +/// Returning `None` where the frame carried nothing is the point — the core +/// then skips that fusion step rather than being handed a fabricated zero, +/// which would be silently wrong rather than merely absent. +struct FrameBackend { + imu: CoreImu, + position: Option, + mag: Option, + heading: Option, + dt: f32, + motors: [f32; 4], +} + +impl FlightBackend for FrameBackend { + fn read_imu(&mut self) -> CoreImu { + self.imu + } + fn read_position(&mut self) -> Option { + self.position + } + fn read_mag(&mut self) -> Option { + self.mag + } + fn read_heading(&mut self) -> Option { + self.heading + } + fn write_motors(&mut self, motors: &[f32]) { + for (i, m) in self.motors.iter_mut().enumerate() { + *m = motors.get(i).copied().unwrap_or(0.0); + } + } + fn dt(&self) -> f32 { + self.dt + } +} struct Component; impl Guest for Component { - fn step(imu: ImuSample, target: Waypoint) -> MotorPwm { - // 1. State estimation. - let state = ekf::estimate(imu); - // 2. Outer position loop → attitude setpoint. - let att_sp = position::tick(state, target); - // 3. Attitude loop → rate setpoint. - let rate_sp = attitude::tick(state, att_sp); - // 4. Rate loop → torque setpoint. - let torque = rate::tick(state, rate_sp); - // 5. Control allocation → per-motor PWM. - mixer::mix(torque) + fn step(sensors: SensorFrame, target: Waypoint) -> MotorPwm { + #[cfg(not(feature = "std"))] + falcon_cm_rt::BumpArena::reset(); + + // The host's ACTUAL period. Clamped to [0.1 ms, 100 ms] so a garbage + // frame cannot wind the filters; a non-finite value falls back to the + // v0.7 constant, the only rate that was ever safe to assume. + let dt = if sensors.dt_s.is_finite() { + sensors.dt_s.clamp(0.0001, 0.1) + } else { + 0.001 + }; + + let imu = sensors.imu; + let mut backend = FrameBackend { + imu: CoreImu { + accel: [imu.ax, imu.ay, imu.az], + gyro: [imu.gx, imu.gy, imu.gz], + }, + position: sensors.position_ned.map(|p| [p.x, p.y, p.z]), + mag: sensors.mag_body.map(|m| [m.x, m.y, m.z]), + heading: sensors.heading_rad, + dt, + motors: [0.0; 4], + }; + + { + let mut guard = CORE.0.borrow_mut(); + // hover_thrust 0.5 matches falcon-core's own default and the value + // both native SITL scenarios construct with. The loop rate comes + // from the frame, not from a constant. + let core = guard.get_or_insert_with(|| FlightCore::new(0.5, 1.0 / dt)); + core.set_position([target.north, target.east, target.down]); + core.step(&mut backend); + } + + MotorPwm { + m1: backend.motors[0], + m2: backend.motors[1], + m3: backend.motors[2], + m4: backend.motors[3], + } } } diff --git a/wit/falcon-cascade/cascade.wit b/wit/falcon-cascade/cascade.wit index d3ea7317..69409661 100644 --- a/wit/falcon-cascade/cascade.wit +++ b/wit/falcon-cascade/cascade.wit @@ -1,4 +1,4 @@ -package pulseengine:falcon-cascade@0.7.0; +package pulseengine:falcon-cascade@0.8.0; /// Falcon control cascade — Component Model edition. /// @@ -58,6 +58,41 @@ interface types { record waypoint { north: f32, east: f32, down: f32, yaw: f32, } + + /// A 3-vector; the frame is named by the field that uses it. + record vec3 { + x: f32, y: f32, z: f32, + } + + /// Everything the flight core needs for ONE tick. + /// + /// v0.8 replaces the bare `imu-sample`, because v0.7 could not express two + /// things a host must be able to say — both found by actually driving the + /// published components rather than by reading the interface. + /// + /// `dt-s` — the host's ACTUAL control period. v0.7 had no way to state it, + /// so every stage hardcoded its own and they disagreed with each other by up + /// to 20x (estimator 1 ms, position 20 ms, attitude 4 ms, rate 1 ms) while + /// the cascade called all five once per tick. Measured on the SITL plant, + /// holding 2.0 m: 1000 Hz -> 0.00 m error, 400 Hz -> 10.43 m, 250 Hz -> 27.17 m, + /// with no error signal of any kind. + /// + /// The aiding measurements — v0.7 took the IMU alone, so position was + /// observable only by dead reckoning. The flight core fuses GNSS, the + /// magnetometer and an absolute heading; a seam that cannot carry them + /// cannot carry the flight core. Every field is `option<>`, and all-`none` + /// is a valid IMU-only host that simply inherits the drift. + record sensor-frame { + imu: imu-sample, + dt-s: f32, + /// NED position fix (m) — GNSS or equivalent. + position-ned: option, + /// Body-frame magnetometer. Any consistent unit; normalised internally. + mag-body: option, + /// Absolute heading (rad, NED). An alternative to `mag-body` for hosts + /// that already resolve yaw; both correct the same unobservable state. + heading-rad: option, + } } /// State estimator — wraps relay-ekf (Mahony complementary filter). @@ -104,21 +139,24 @@ world mixer-component { export mixer; } /// wit-bindgen export macro is generated the same, interface-shaped /// way the leaf components rely on. interface controller { - use types.{imu-sample, waypoint, motor-pwm}; - /// One full cascade pass: estimate → position → attitude → rate - /// → mixer. - step: func(imu: imu-sample, target: waypoint) -> motor-pwm; + use types.{sensor-frame, waypoint, motor-pwm}; + /// One full flight-core tick: estimate, fuse whatever aiding the frame + /// carries, then geometric SE(3) attitude → ADRC rate → mixer, with the + /// rotor-out FDI and the altitude/position loops the vehicle actually flies. + step: func(sensors: sensor-frame, target: waypoint) -> motor-pwm; } /// Imports the five controller interfaces, exports `controller`. /// The implementation calls estimate → position → attitude → rate /// → mixer in order. wac satisfies the imports from the leaf /// components (BUILD.bazel `wac_plug`). +/// v0.8: the cascade component wraps falcon-core's FlightCore directly, so it +/// imports NOTHING. The five-stage decomposition it used to compose was a +/// wasm-side invention that never corresponded to the flight architecture — +/// which is exactly how it drifted onto the legacy PID controllers while +/// falcon-core moved to geometric SE(3) + ADRC, unnoticed for three months +/// (#388). One component, one tick, one clock, and the same control law the +/// vehicle flies. world cascade { - import ekf; - import position; - import attitude; - import rate; - import mixer; export controller; }