diff --git a/artifacts/features/FEAT-FALCON-rollout.yaml b/artifacts/features/FEAT-FALCON-rollout.yaml index cbd6597..c660eee 100644 --- a/artifacts/features/FEAT-FALCON-rollout.yaml +++ b/artifacts/features/FEAT-FALCON-rollout.yaml @@ -1930,6 +1930,72 @@ artifacts: - type: depends-on target: FEAT-FALCON-v0.19.4 + - id: FEAT-FALCON-v0.19.6 + type: feature + title: "v0.19.6 — frame-correctness oracle + SDF/MIXER_X alignment" + status: approved + description: > + LANDED. The NED↔ENU frame unlock for the full verified cascade. + A frame-check oracle established the true actuator-path + convention; the fix was an SDF rotor index→position+spin + alignment to relay-mix-quad's MIXER_X convention — not a code + sign-flip. Roll/pitch torque now produce correctly-signed body + rate; the verified rate-PID damping holds the body level. + Horizontal drift collapsed from −13 m (v0.19.5) to ~±0.15 m. + + Oracle (--scenario=frame-{roll,pitch,yaw}): commands unit torque + on one axis + hover thrust, reports sensed body-rate sign. + AGREE = +torque→+rate (needed for rate-PID negative feedback to + stabilise). + + Before (scrambled SDF): roll ≈ 0 (no authority!), pitch OPPOSE, + yaw AGREE. Root cause: SDF rotor index→position map scrambled vs + MIXER_X (0=front-right CW, 1=back-right CCW, 2=back-left CW, + 3=front-left CCW). The v0.19.4/.5 negate-torque hack was + compensating for the scramble. + + After (aligned SDF, identity correction): roll +4.77 AGREE, + pitch +4.14 AGREE, yaw -0.0001 (weak authority, deferred). + frame_correct_torque is identity; unit test + frame_correction_is_identity pins it. + + Horizontal-stability proof: v0.19.5 drifted to n=−13/e=−7 → + crash; v0.19.6 holds n,e within ±0.15 m (alt-rate) / ±1.4 m + (full cascade, no runaway). The frame fix removed the runaway. + + Ships: + - --scenario=frame-{roll,pitch,yaw} oracle. + - SDF rotor index→position+spin aligned to MIXER_X. + - frame_correct_torque adapter (identity) on rate→mixer path. + - unit test frame_correction_is_identity. + - bench-evidence/gz-sim/2026-05-28-v0.19.6-frame-correctness.md. + - FV-FALCON-SIM-012. + + Verification: + - cargo test --workspace --all-targets → 407 passing. + - cargo test -p falcon-sitl-gz --features gazebo → 9/9. + - frame-check oracle: roll + pitch AGREE under gz. + - rivet validate → PASS. + + Honestly NOT claimed: + - Full cascade hovers. Doesn't lift yet — relay-pos default + hover_thrust (0.5) insufficient for the 2 kg body (~0.72 + needed). v0.19.7. Frame fix removed horizontal runaway; + lift + sustained hover is next. + - Yaw controlled. Authority ≈ 0 (momentConstant); separate + tuning, non-critical for hover. + tags: [falcon, milestone, v0.19.6, gazebo, frame-correctness, oracle, landed] + fields: + release-target: "frame-correctness oracle + SDF/MIXER_X alignment" + bench-date: "2026-05-28" + oracle: "--scenario=frame-{roll,pitch,yaw}" + frame-correction: "identity (SDF alignment was the fix)" + horizontal-drift: "±0.15 m (was −13 m at v0.19.5)" + deferred-to-v0.19.7: "relay-pos thrust tuning for 2 kg → lift + sustained hover" + links: + - type: depends-on + target: FEAT-FALCON-v0.19.5 + - id: FEAT-FALCON-v1.0 type: feature title: "v1.0 — six-domain credit dossier + airframe variants" @@ -1966,4 +2032,4 @@ artifacts: - type: implements target: SYSREQ-FALCON-010 - type: depends-on - target: FEAT-FALCON-v0.19.5 + target: FEAT-FALCON-v0.19.6 diff --git a/artifacts/verification/FV-FALCON-SIM-012.yaml b/artifacts/verification/FV-FALCON-SIM-012.yaml new file mode 100644 index 0000000..0efe807 --- /dev/null +++ b/artifacts/verification/FV-FALCON-SIM-012.yaml @@ -0,0 +1,85 @@ +artifacts: + - id: FV-FALCON-SIM-012 + type: sw-verification + title: "v0.19.6 — frame-correctness oracle + SDF/MIXER_X alignment (horizontal drift eliminated)" + status: approved + description: > + The NED↔ENU frame unlock for the full verified cascade. A + frame-check oracle established the true actuator-path convention; + the fix was an SDF rotor index→position+spin alignment to the + verified relay-mix-quad MIXER_X convention — not a code sign-flip. + Roll/pitch torque now produce correctly-signed body rate and the + verified rate-PID damping holds the body level: horizontal drift + collapsed from −13 m (v0.19.5) to ~±0.15 m. + + The oracle (--scenario=frame-{roll,pitch,yaw}): commands a unit + torque on one axis (post frame-correction) + hover thrust, reports + the sensed body-rate sign. AGREE = +torque → +rate (required for + the rate-PID negative feedback to be stabilising). + + Before (v0.19.0–.5 SDF, identity correction): + roll: +0.15 → -0.0002 rad/s OPPOSE (≈0 — no roll authority) + pitch: +0.15 → -0.0087 rad/s OPPOSE + yaw: +0.15 → +0.1246 rad/s AGREE + + Root cause: the SDF rotor index→position map was scrambled vs + MIXER_X (0=front-right CW, 1=back-right CCW, 2=back-left CW, + 3=front-left CCW). The v0.19.4/.5 "negate roll+pitch torque" + hack was compensating for the scramble, not a real frame flip. + + Fix: align SDF rotor index→position+spin to MIXER_X in gz ENU + coords (NED right = gz −Y; CW-about-NED-down = ccw-about-gz-up). + + After (aligned SDF, identity correction): + roll: +0.15 → +4.77 rad/s AGREE ✓ + pitch: +0.15 → +4.14 rad/s AGREE ✓ + yaw: +0.15 → -0.0001 rad/s OPPOSE (weak authority, deferred) + + frame_correct_torque is identity; unit test + frame_correction_is_identity pins it. If a future SDF/frame + change reintroduces a flip, the frame-check oracle catches it. + + Horizontal-stability proof: + v0.19.5 (scrambled + negate): drifts to n=−13, e=−7 → crash. + v0.19.6 (aligned, identity): n,e within ±0.15 m (alt-rate) / + ±1.4 m (full cascade, no runaway). + + Ships: + - --scenario=frame-{roll,pitch,yaw} oracle. + - SDF rotor index→position+spin aligned to MIXER_X. + - frame_correct_torque adapter (identity) on the rate→mixer + path in alt-rate + the full hover cascade. + - unit test frame_correction_is_identity. + - bench-evidence/gz-sim/2026-05-28-v0.19.6-frame-correctness.md. + + Verification: + - cargo test --workspace --all-targets → 407 passing (+1 + frame_correction_is_identity). + - cargo test -p falcon-sitl-gz --features gazebo → 9/9. + - frame-check oracle: roll + pitch AGREE under gz. + - rivet validate → PASS. + + Honestly NOT claimed: + - That the full cascade hovers. It doesn't lift yet — + relay-pos's default hover_thrust (0.5) is insufficient for + the 2 kg body (needs ~0.72). v0.19.7. The frame fix removed + the horizontal runaway; lift + sustained hover is the next + release. + - That yaw is controlled. Authority ≈ 0 (momentConstant); + separate tuning item, non-critical for hover. + - That the oracle covers attitude angle, only rate sign. + tags: [falcon, sim, gazebo, frame-correctness, oracle, bench-evidence, v0.19.6] + fields: + bench-evidence-dir: bench-evidence/gz-sim/ + bench-date: "2026-05-28" + gz-version: "8.11.0" + oracle: "--scenario=frame-{roll,pitch,yaw}" + frame-check-roll: "+4.77 rad/s AGREE" + frame-check-pitch: "+4.14 rad/s AGREE" + frame-check-yaw: "-0.0001 rad/s OPPOSE (weak authority, deferred)" + frame-correction: "identity (SDF alignment was the fix)" + root-cause: "SDF rotor index→position map scrambled vs relay-mix-quad MIXER_X" + deferred-to-v0.19.7: "relay-pos thrust tuning for 2 kg body → lift + sustained hover" + links: + - type: verifies + target: SWREQ-FALCON-SIM-P04 diff --git a/bench-evidence/gz-sim/2026-05-28-v0.19.6-frame-correctness.md b/bench-evidence/gz-sim/2026-05-28-v0.19.6-frame-correctness.md new file mode 100644 index 0000000..e1551c6 --- /dev/null +++ b/bench-evidence/gz-sim/2026-05-28-v0.19.6-frame-correctness.md @@ -0,0 +1,113 @@ +# v0.19.6 — frame-correctness oracle + fix — 2026-05-28 + +The unlock for the full verified cascade. A frame-check oracle +established the true NED↔ENU actuator-path convention; the fix was an +SDF rotor-index alignment (not a code sign-flip). Roll/pitch torque +now produces correctly-signed body rate, and the cascade's rate +damping holds the body level — horizontal drift collapsed from −13 m +(v0.19.5) to ~±0.15 m. + +## The oracle + +`--scenario=frame-{roll,pitch,yaw}` commands a unit torque on one axis +(post frame-correction) with hover thrust and reports the sign of the +sensed body rate. For the verified rate-PID's negative feedback to be +*stabilising*, +torque[axis] must yield +rate[axis] (AGREE). + +### Before — raw signs with the v0.19.0–.5 SDF (identity correction) + +``` +roll(X): +0.15 → -0.0002 rad/s OPPOSE (≈ 0 — no roll authority!) +pitch(Y): +0.15 → -0.0087 rad/s OPPOSE +yaw(Z): +0.15 → +0.1246 rad/s AGREE +``` + +Roll authority ≈ 0 was the smoking gun: the SDF's rotor index→position +map was **scrambled** relative to the verified mixer, so the mixer's +roll column applied to the wrong physical rotors nearly cancelled. + +### Root cause — SDF vs MIXER_X mismatch + +`crates/relay-mix-quad`'s actual `MIXER_X` convention: + +``` +index 0 = front-right, CW (NED) +index 1 = back-right, CCW +index 2 = back-left, CW +index 3 = front-left, CCW +``` + +The v0.19.0–.5 SDF assigned a different index→position map (0=FR, +1=BL, 2=FL, 3=BR with mismatched spins). The v0.19.4/.5 "negate +roll+pitch torque" hack was compensating for *this scramble*, not a +real frame inversion. + +### Fix — align SDF to MIXER_X (gz ENU coords) + +NED right = gz −Y; a CW-about-NED-down rotor is ccw-about-gz-up: + +``` +index 0 → gz (+X, −Y), ccw (was +X,−Y ccw — already right) +index 1 → gz (−X, −Y), cw (was −X,+Y ccw) +index 2 → gz (−X, +Y), ccw (was +X,+Y cw) +index 3 → gz (+X, +Y), cw (was −X,−Y cw) +``` + +### After — raw signs with the aligned SDF (identity correction) + +``` +roll(X): +0.15 → +4.77 rad/s AGREE ✓ +pitch(Y): +0.15 → +4.14 rad/s AGREE ✓ +yaw(Z): +0.15 → -0.0001 rad/s OPPOSE (weak authority — see below) +``` + +Roll + pitch now respond strongly and correctly with **identity** +frame-correction — confirming the SDF alignment was the real fix. +`frame_correct_torque` is identity; the unit test +`frame_correction_is_identity` pins it. + +## Horizontal-stability proof + +The decisive end-to-end check: with the verified rate-PID damping +attitude, does the body stay put horizontally? + +| Scenario | Horizontal excursion over 20 s | +|---|---| +| v0.19.5 (scrambled SDF + negate hack) | drifts to n=−13 m, e=−7 m → crash | +| **v0.19.6 (aligned SDF, identity)** | **n, e within ±0.15 m (alt-rate) / ±1.4 m (full cascade)** | + +The frame fix eliminated the horizontal runaway. The verified rate +loop is now genuinely stabilising. + +## Known-open: yaw authority + lift + +Two items deferred to v0.19.7, both *not* frame-sign problems: + +1. **Yaw authority ≈ 0.** Yaw torque comes from rotor drag-reaction + (`momentConstant`), far weaker than thrust differential. For hover + it's non-critical (heading holds roughly); proper yaw control is a + `momentConstant` / mixer-gain tuning item. +2. **Full cascade doesn't lift yet.** `--scenario=hover` now stays + near origin (n,e oscillate ±1.4 m, no runaway — the frame fix + works) but altitude stays at 0: `relay-pos`'s default + `hover_thrust` gain (0.5) gives insufficient thrust for the 2 kg + body (needs ~0.72). That's the v0.19.7 tuning — wire PosGains for + this airframe → the body lifts → sustained hover. + +## What v0.19.6 ships + +- `--scenario=frame-{roll,pitch,yaw}` frame-check oracle. +- SDF rotor index→position+spin aligned to MIXER_X. +- `frame_correct_torque` boundary adapter (identity) applied on the + rate→mixer path in both `alt-rate` and the full `hover` cascade. +- Unit test `frame_correction_is_identity`. +- This evidence doc. + +## Honestly NOT claimed + +- That the full cascade hovers. It doesn't lift yet (v0.19.7 thrust + tuning). v0.19.6 is the *frame* unlock: roll/pitch correct, no + horizontal runaway. +- That yaw is controlled. Authority is ~0; a separate tuning item. +- That the oracle covers attitude *angle*, only rate sign. Angle hold + (att controller) lands with the lift fix in v0.19.7. diff --git a/examples/falcon-sitl-gz/src/main.rs b/examples/falcon-sitl-gz/src/main.rs index 1e38e74..edc5bfd 100644 --- a/examples/falcon-sitl-gz/src/main.rs +++ b/examples/falcon-sitl-gz/src/main.rs @@ -98,6 +98,41 @@ fn main() { /// "is the bridge publish path alive at all". /// /// `step`, `mission`, `disturbance` reserved per docs/SIMULATOR.md. +/// v0.19.6 — NED↔ENU torque frame-correction. +/// +/// relay's verified controllers emit torque in NED body frame +/// (X-fwd, Y-right, Z-down). gz's MulticopterMotorModel applies the +/// mixer's per-motor thrust in its ENU body frame (X-fwd, Y-left, +/// Z-up), related to NED by a 180° rotation about X (Y and Z flip). +/// The verified relay-mix-quad maps NED torque → motor PWM assuming +/// NED rotor geometry; against gz's ENU physical layout that inverts +/// the roll, pitch AND yaw torque the cascade actually achieves. +/// +/// `frame_correct_torque` is the single boundary adapter for the +/// actuator path's frame sign. The frame-check oracle +/// (`--scenario=frame-{roll,pitch,yaw}`) established it empirically. +/// +/// Result (see bench-evidence/gz-sim/2026-05-28-v0.19.6-frame- +/// correctness.md): once the SDF rotor index→position+spin map is +/// aligned to the verified relay-mix-quad MIXER_X convention +/// (0=front-right CW, 1=back-right CCW, 2=back-left CW, +/// 3=front-left CCW), commanding +torque on roll/pitch produces +/// +rate on that axis as sensed (AGREE). So the correction is +/// **identity** — the v0.19.4/.5 "negate roll/pitch" hack was +/// compensating for the *scrambled* SDF index map, not a real frame +/// flip. Keeping this adapter (as identity) documents the boundary +/// and is where a correction would live if the SDF/frame convention +/// ever changes; the unit test `frame_correction_is_identity` pins it. +#[inline] +fn frame_correct_torque(torque_ned: [f32; 3]) -> [f32; 3] { + const SIGN: [f32; 3] = [1.0, 1.0, 1.0]; + [ + SIGN[0] * torque_ned[0], + SIGN[1] * torque_ned[1], + SIGN[2] * torque_ned[2], + ] +} + fn run_scenario( physics: &mut dyn Physics, scenario: &str, @@ -109,6 +144,9 @@ fn run_scenario( "open-loop-climb" => run_open_loop_climb(physics, duration_s, evidence), "alt-only" => run_alt_only_hover(physics, duration_s, evidence), "alt-rate" => run_alt_rate_hover(physics, duration_s, evidence), + "frame-roll" => run_frame_check(physics, 0, duration_s), + "frame-pitch" => run_frame_check(physics, 1, duration_s), + "frame-yaw" => run_frame_check(physics, 2, duration_s), other => { eprintln!( " scenario {other} not yet wired; falling back to closed-loop hover", @@ -118,6 +156,68 @@ fn run_scenario( } } +/// v0.19.6 — frame-correctness ORACLE. Commands a small constant +/// torque on ONE axis (roll=0, pitch=1, yaw=2) with hover thrust, +/// for a short window, and reports the sign of the sensed body rate +/// on that axis. +/// +/// The cascade computes torque in NED (X-fwd, Y-right, Z-down). gz's +/// MulticopterMotorModel applies it in ENU (X-fwd, Y-left, Z-up). For +/// the verified rate-PID (negative feedback: torque drives rate→ +/// setpoint) to be *stabilising*, a commanded +torque[axis] must +/// produce a +rate[axis] as the cascade senses it (post enu_to_ned). +/// If the response is NEGATIVE, that axis needs a sign flip in the +/// bridge's frame-correction (see `frame_correct_torque`). +/// +/// PASS = the post-correction response is positive on the commanded +/// axis (torque and sensed rate agree in sign). Printed verdict feeds +/// the unit test `frame_correction_signs_match_gz_enu`. +fn run_frame_check(physics: &mut dyn Physics, axis: usize, duration_s: f32) -> bool { + let mut mixer = QuadMixer::new(); + let hover_thrust = 0.72_f32; + let dt = 0.01_f32; + let n = (duration_s / dt) as u32; + let tick_period = Duration::from_secs_f32(dt); + let pace_real_time = physics.counters().is_some(); + + // Commanded torque on the test axis (post frame-correction, so the + // oracle measures the *corrected* path the cascade will use). + let mut cmd = [0.0_f32; 3]; + cmd[axis] = 0.15; + let cmd_corrected = frame_correct_torque(cmd); + + // Measure mean sensed rate on the test axis over the settle window + // [0.3, 0.8] s — long enough past the spawn transient, short enough + // that the body hasn't tumbled past small-angle. + let mut sum_rate = 0.0_f32; + let mut count = 0u32; + let axis_name = ["roll(X)", "pitch(Y)", "yaw(Z)"][axis]; + + for step in 0..n { + let tick_start = Instant::now(); + let t = step as f32 * dt; + let (imu_sample, _pos) = physics.measure(0.0); + let motors = mixer.mix(cmd_corrected, hover_thrust); + physics.step(motors, dt); + if (0.3..0.8).contains(&t) { + sum_rate += imu_sample.gyro_body[axis]; + count += 1; + } + if pace_real_time { + let used = tick_start.elapsed(); + if used < tick_period { std::thread::sleep(tick_period - used); } + } + } + 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 ✗" }, + ); + agrees +} + /// v0.19.5 — alt-only thrust + rate-pid attitude damping (zero rate /// setpoint = "stay level"). The minimal closed-loop hover: P+D /// altitude controller for thrust + relay-rate's verified PID for @@ -177,14 +277,11 @@ fn run_alt_rate_hover( } else { rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, [0.0_f32; 3]) }; - // v0.19.5 — NEGATE torque. The bridge's enu_to_ned gyro - // conversion + the mixer's NED torque convention + gz's ENU - // plugin frame compose to an overall sign flip on the roll/ - // pitch torque path: feeding rate-pid output straight to the - // mixer drove POSITIVE feedback (body spun up). Negating - // closes the loop with the correct damping sign. (Yaw axis - // unaffected in practice — diagonal pairs cancel.) - let torque = [-torque_raw[0], -torque_raw[1], -torque_raw[2]]; + // v0.19.6 — frame-correction is identity now that the SDF + // rotor index map is aligned to MIXER_X. The v0.19.5 negation + // hack is gone; the frame-check oracle confirms +torque → + // +rate (AGREE) on roll + pitch with no sign flip. + let torque = frame_correct_torque(torque_raw); let motors = mixer.mix(torque, thrust); physics.step(motors, dt); @@ -427,8 +524,9 @@ fn run_closed_loop_hover( // 4. ATT — quaternion error → rate setpoint. let current_rate_sp = att.tick(att_ts_of(t), est.quaternion, current_att_sp); - // 5. RATE — gyro + rate setpoint → torque. - let torque = rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, current_rate_sp); + // 5. RATE — gyro + rate setpoint → torque (frame-corrected). + let torque_raw = rate_pid.tick(rate_ts_of(t), imu_sample.gyro_body, current_rate_sp); + let torque = frame_correct_torque(torque_raw); for k in 0..3 { if !torque[k].is_finite() { nan_seen = true; } } @@ -759,6 +857,25 @@ mod tests { assert!(pass, "70 % PWM × THRUST_SCALE should easily beat gravity"); } + /// v0.19.6 — the frame-correction adapter is identity. Pins the + /// oracle finding: once the SDF rotor index→position+spin map is + /// aligned to relay-mix-quad's MIXER_X convention, +torque produces + /// +rate (AGREE) on roll + pitch with no sign flip — the v0.19.4/.5 + /// "negate" hack was compensating for a scrambled SDF, not a real + /// frame inversion. If a future SDF/frame change reintroduces a + /// flip, the frame-check oracle catches it and this test changes + /// deliberately. See bench-evidence/gz-sim/2026-05-28-v0.19.6-*.md. + #[test] + fn frame_correction_is_identity() { + let t = [0.3_f32, -0.7, 0.2]; + assert_eq!(frame_correct_torque(t), t, + "frame-correction must be identity after the v0.19.6 SDF index alignment"); + // Spot-check each axis independently. + assert_eq!(frame_correct_torque([1.0, 0.0, 0.0]), [1.0, 0.0, 0.0]); + assert_eq!(frame_correct_torque([0.0, 1.0, 0.0]), [0.0, 1.0, 0.0]); + assert_eq!(frame_correct_torque([0.0, 0.0, 1.0]), [0.0, 0.0, 1.0]); + } + /// v0.19.4 — closed-loop cascade against MockPhysics. Smoke test /// that the cascade wires + ticks without panic, no NaN escape. /// MockPhysics ignores per-rotor differential (applies only diff --git a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf index 82e45c2..ce20c73 100644 --- a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf +++ b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf @@ -153,12 +153,17 @@ - + > true @@ -189,7 +194,7 @@ true false - -0.0884 0.0884 0.025 0 0 0 + -0.0884 -0.0884 0.025 0 0 0 0.010 5e-75e-71e-6 @@ -215,7 +220,7 @@ true false - 0.0884 0.0884 0.025 0 0 0 + -0.0884 0.0884 0.025 0 0 0 0.010 5e-75e-71e-6 @@ -241,7 +246,7 @@ true false - -0.0884 -0.0884 0.025 0 0 0 + 0.0884 0.0884 0.025 0 0 0 0.010 5e-75e-71e-6 @@ -293,7 +298,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_1_joint rotor_1 - ccw + cw 0.0125 0.025 1000.0 @@ -310,7 +315,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_2_joint rotor_2 - cw + ccw 0.0125 0.025 1000.0