diff --git a/Cargo.lock b/Cargo.lock index 895f9c5..0cc602d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -50,30 +50,12 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5e764a1d40d510daf35e07be9eb06e75770908c27d411ee6c92109c9840eaaf7" -[[package]] -name = "bitflags" -version = "1.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" - [[package]] name = "bitflags" version = "2.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c4512299f36f043ab09a583e57bceb5a5aab7a73db1805848e8fef3c9e8c78b3" -[[package]] -name = "bumpalo" -version = "3.20.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72f5acc6cb2ba439de613abc23857ec3d78374d8ed5ac84e9d11336e87da8649" - -[[package]] -name = "bytes" -version = "1.11.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" - [[package]] name = "cast" version = "0.3.0" @@ -87,21 +69,9 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1dce859f0832a7d088c4f1119888ab94ef4b5d6795d1ce05afb7fe159d79f98" dependencies = [ "find-msvc-tools", - "jobserver", - "libc", "shlex", ] -[[package]] -name = "cfg-expr" -version = "0.15.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d067ad48b8650848b989a59a86c6c36a995d02d2bf778d45c3c5d57bc2718f02" -dependencies = [ - "smallvec", - "target-lexicon", -] - [[package]] name = "cfg-if" version = "1.0.4" @@ -171,7 +141,7 @@ dependencies = [ "ciborium", "clap", "criterion-plot", - "itertools 0.13.0", + "itertools", "num-traits", "oorandom", "regex", @@ -188,20 +158,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9b1bcc0dc7dfae599d84ad0b1a55f80cde8af3725da8313b528da95ef783e338" dependencies = [ "cast", - "itertools 0.13.0", -] - -[[package]] -name = "crossbeam" -version = "0.8.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8" -dependencies = [ - "crossbeam-channel", - "crossbeam-deque", - "crossbeam-epoch", - "crossbeam-queue", - "crossbeam-utils", + "itertools", ] [[package]] @@ -213,34 +170,6 @@ dependencies = [ "crossbeam-utils", ] -[[package]] -name = "crossbeam-deque" -version = "0.8.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9dd111b7b7f7d55b72c0a6ae361660ee5853c9af73f70c3c2ef6858b950e2e51" -dependencies = [ - "crossbeam-epoch", - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-epoch" -version = "0.9.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e" -dependencies = [ - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-queue" -version = "0.3.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f58bbc28f91df819d0aa2a2c00cd19754769c2fad90579b3592b1c9ba7a3115" -dependencies = [ - "crossbeam-utils", -] - [[package]] name = "crossbeam-utils" version = "0.8.21" @@ -253,17 +182,6 @@ version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "460fbee9c2c2f33933d720630a6a0bac33ba7053db5344fac858d4b8952d77d5" -[[package]] -name = "dircpy" -version = "0.3.20" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ebcbec2b9a580ddee352ac38523d2ecd4dcaad53532957034394556909e27f4b" -dependencies = [ - "jwalk", - "log", - "walkdir", -] - [[package]] name = "either" version = "1.16.0" @@ -325,7 +243,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" dependencies = [ "libc", - "windows-sys 0.61.2", + "windows-sys", ] [[package]] @@ -471,10 +389,12 @@ dependencies = [ name = "falcon-sitl-gz" version = "0.1.0" dependencies = [ + "crossbeam-channel", "falcon-config", - "gz-transport-rs", + "falcon-core", + "gz-msgs", + "gz-transport", "libm", - "prost 0.14.4", "relay-adrc", "relay-arm", "relay-att", @@ -487,7 +407,6 @@ dependencies = [ "relay-rate", "relay-sc", "relay-traj", - "tokio", ] [[package]] @@ -517,12 +436,6 @@ version = "0.1.9" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582" -[[package]] -name = "fixedbitset" -version = "0.5.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d674e81391d1e1ab681a28d99df07927c6d4aa5b027d7da16ba32d1d21ecd99" - [[package]] name = "fnv" version = "1.0.7" @@ -535,30 +448,6 @@ version = "0.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d9c4f5dac5e15c24eb999c26181a6ca40b39fe946cbe4c263c7209467bc83af2" -[[package]] -name = "futures-core" -version = "0.3.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e3450815272ef58cec6d564423f6e755e25379b217b0bc688e295ba24df6b1d" - -[[package]] -name = "futures-task" -version = "0.3.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "037711b3d59c33004d3856fbdc83b99d4ff37a24768fa1be9ce3538a1cde4393" - -[[package]] -name = "futures-util" -version = "0.3.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "389ca41296e6190b48053de0321d02a77f32f8a5d2461dd38762c0593805c6d6" -dependencies = [ - "futures-core", - "futures-task", - "pin-project-lite", - "slab", -] - [[package]] name = "geofence-subject-rs" version = "0.1.0" @@ -592,24 +481,66 @@ dependencies = [ ] [[package]] -name = "gz-transport-rs" -version = "0.1.0" +name = "gz-build" +version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "229ffed01db02cf69765715cba0526380de3a0c61fbad56936ef362f306d5e79" +checksum = "cb1dcb184ea5354e9e63edac4c1ad313f9580823e166d5f483d7f7a1f2bf79d5" dependencies = [ - "bytes", - "hostname", - "parking_lot", - "prost 0.13.5", - "prost-build", - "prost-types", - "socket2 0.5.10", - "thiserror", - "tokio", - "tracing", - "uuid", - "whoami", - "zmq", + "pkg-config", +] + +[[package]] +name = "gz-msgs" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be49b1a7e03d88e9d3157ede6c17fc9099af257c31536dbfe66c52a117d95fa6" +dependencies = [ + "gz-build", + "gz-msgs-common", + "protobuf", +] + +[[package]] +name = "gz-msgs-common" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "772dcb43c78d1e5f3f59d2bac8bd0a311015929d5c593963f6d076ea29e6dd98" +dependencies = [ + "gz-msgs-derive", + "protobuf", +] + +[[package]] +name = "gz-msgs-derive" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30df695f75dfad35495e8c92cdd5cd6f543f3563b464021d503acf12abd60722" +dependencies = [ + "quote", + "syn", +] + +[[package]] +name = "gz-transport" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7332b6e8d98b6760bd63965499a24b1f02e77e93030b3b0d8f8989ef1930af7a" +dependencies = [ + "crossbeam-channel", + "gz-build", + "gz-msgs-common", + "gz-transport-sys", + "log", +] + +[[package]] +name = "gz-transport-sys" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b2501893885070692a8cdedd62691cb5a382f7259b84863910383e7f7df1d1c" +dependencies = [ + "cc", + "gz-build", ] [[package]] @@ -644,17 +575,6 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" -[[package]] -name = "hostname" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "617aaa3557aef3810a6369d0a99fac8a080891b68bd9f9812a1eeda0c0730cbd" -dependencies = [ - "cfg-if", - "libc", - "windows-link", -] - [[package]] name = "id-arena" version = "2.3.0" @@ -682,53 +602,12 @@ dependencies = [ "either", ] -[[package]] -name = "itertools" -version = "0.14.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b192c782037fadd9cfa75548310488aabdbf3d2da73885b31bd0abd03351285" -dependencies = [ - "either", -] - [[package]] name = "itoa" version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" -[[package]] -name = "jobserver" -version = "0.1.34" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9afb3de4395d6b3e67a780b6de64b51c978ecf11cb9a462c66be7d4ca9039d33" -dependencies = [ - "getrandom 0.3.4", - "libc", -] - -[[package]] -name = "js-sys" -version = "0.3.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "142bc4740e452c1e57ade0cbc129f139c9093e354346f0872ef985f4f5cf5f11" -dependencies = [ - "cfg-if", - "futures-util", - "once_cell", - "wasm-bindgen", -] - -[[package]] -name = "jwalk" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2735847566356cd2179a2a38264839308f7079fa96e6bd5a42d740460e003c56" -dependencies = [ - "crossbeam", - "rayon", -] - [[package]] name = "leb128fmt" version = "0.1.0" @@ -747,33 +626,12 @@ version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" -[[package]] -name = "libredox" -version = "0.1.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e02f3bb43d335493c96bf3fd3a321600bf6bd07ed34bc64118e9293bdffea46c" -dependencies = [ - "bitflags 2.11.1", - "libc", - "plain", - "redox_syscall 0.7.5", -] - [[package]] name = "linux-raw-sys" version = "0.12.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "32a66949e030da00e8c7d4434b251670a91556f4144941d37452769c25d58a53" -[[package]] -name = "lock_api" -version = "0.4.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "224399e74b87b5f3557511d98dff8b14089b3dadafcab6bb93eab67d3aace965" -dependencies = [ - "scopeguard", -] - [[package]] name = "log" version = "0.4.30" @@ -786,23 +644,6 @@ version = "2.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6b947ae49db0d222b1dbc6b113ce7248a3fc3a6ca21b696717bfc000ba4484d8" -[[package]] -name = "mio" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "50b7e5b27aa02a74bac8c3f23f448f8d87ff11f92d3aac1a6ed369ee08cc56c1" -dependencies = [ - "libc", - "wasi", - "windows-sys 0.61.2", -] - -[[package]] -name = "multimap" -version = "0.10.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d87ecb2933e8aeadb3e3a02b828fed80a7528047e68b4f424523a0981a3a084" - [[package]] name = "num-traits" version = "0.2.19" @@ -824,57 +665,12 @@ version = "11.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d6790f58c7ff633d8771f42965289203411a5e5c68388703c06e14f24770b41e" -[[package]] -name = "parking_lot" -version = "0.12.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93857453250e3077bd71ff98b6a65ea6621a19bb0f559a85248955ac12c45a1a" -dependencies = [ - "lock_api", - "parking_lot_core", -] - -[[package]] -name = "parking_lot_core" -version = "0.9.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2621685985a2ebf1c516881c026032ac7deafcda1a2c9b7850dc81e3dfcb64c1" -dependencies = [ - "cfg-if", - "libc", - "redox_syscall 0.5.18", - "smallvec", - "windows-link", -] - -[[package]] -name = "petgraph" -version = "0.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3672b37090dbd86368a4145bc067582552b29c27377cad4e0a306c97f9bd7772" -dependencies = [ - "fixedbitset", - "indexmap", -] - -[[package]] -name = "pin-project-lite" -version = "0.2.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" - [[package]] name = "pkg-config" version = "0.3.33" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "19f132c84eca552bf34cab8ec81f1c1dcc229b811638f9d283dceabe58c5569e" -[[package]] -name = "plain" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4596b6d070b27117e987119b4dac604f3c58cfb0b191112e24771b2faeac1a6" - [[package]] name = "ppv-lite86" version = "0.2.21" @@ -911,7 +707,7 @@ checksum = "4b45fcc2344c680f5025fe57779faef368840d0bd1f42f216291f0dc4ace4744" dependencies = [ "bit-set", "bit-vec", - "bitflags 2.11.1", + "bitflags", "num-traits", "rand", "rand_chacha", @@ -923,78 +719,23 @@ dependencies = [ ] [[package]] -name = "prost" -version = "0.13.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2796faa41db3ec313a31f7624d9286acf277b52de526150b7e69f3debf891ee5" -dependencies = [ - "bytes", - "prost-derive 0.13.5", -] - -[[package]] -name = "prost" -version = "0.14.4" +name = "protobuf" +version = "3.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "528ac67416ff8646872a3c02cad9cc4ee5dc9f9540c9b10771855c95cb2e5ae1" +checksum = "d65a1d4ddae7d8b5de68153b48f6aa3bba8cb002b243dbdbc55a5afbc98f99f4" dependencies = [ - "bytes", - "prost-derive 0.14.4", -] - -[[package]] -name = "prost-build" -version = "0.13.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "be769465445e8c1474e9c5dac2018218498557af32d9ed057325ec9a41ae81bf" -dependencies = [ - "heck", - "itertools 0.14.0", - "log", - "multimap", "once_cell", - "petgraph", - "prettyplease", - "prost 0.13.5", - "prost-types", - "regex", - "syn", - "tempfile", -] - -[[package]] -name = "prost-derive" -version = "0.13.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a56d757972c98b346a9b766e3f02746cde6dd1cd1d1d563472929fdd74bec4d" -dependencies = [ - "anyhow", - "itertools 0.14.0", - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "prost-derive" -version = "0.14.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b570b25f7617e43d59005d0990ccb79e950a423952cea19671b7a876da390adf" -dependencies = [ - "anyhow", - "itertools 0.14.0", - "proc-macro2", - "quote", - "syn", + "protobuf-support", + "thiserror", ] [[package]] -name = "prost-types" -version = "0.13.5" +name = "protobuf-support" +version = "3.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "52c2c1bf36ddb1a1c396b3601a3cec27c2462e45f07c386894ec3ccf5332bd16" +checksum = "3e36c2f31e0a47f9280fb347ef5e461ffcd2c52dd520d8e216b52f93b0b0d7d6" dependencies = [ - "prost 0.13.5", + "thiserror", ] [[package]] @@ -1062,44 +803,6 @@ dependencies = [ "rand_core", ] -[[package]] -name = "rayon" -version = "1.12.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fb39b166781f92d482534ef4b4b1b2568f42613b53e5b6c160e24cfbfa30926d" -dependencies = [ - "either", - "rayon-core", -] - -[[package]] -name = "rayon-core" -version = "1.13.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91" -dependencies = [ - "crossbeam-deque", - "crossbeam-utils", -] - -[[package]] -name = "redox_syscall" -version = "0.5.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed2bf2547551a7053d6fdfafda3f938979645c44812fbfcda098faae3f1a362d" -dependencies = [ - "bitflags 2.11.1", -] - -[[package]] -name = "redox_syscall" -version = "0.7.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4666a1a60d8412eab19d94f6d13dcc9cea0a5ef4fdf6a5db306537413c661b1b" -dependencies = [ - "bitflags 2.11.1", -] - [[package]] name = "regex" version = "1.12.3" @@ -1505,19 +1208,13 @@ version = "1.1.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b6fe4565b9518b83ef4f91bb47ce29620ca828bd32cb7e408f0062e9930ba190" dependencies = [ - "bitflags 2.11.1", + "bitflags", "errno", "libc", "linux-raw-sys", - "windows-sys 0.61.2", + "windows-sys", ] -[[package]] -name = "rustversion" -version = "1.0.22" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" - [[package]] name = "rusty-fork" version = "0.3.1" @@ -1539,12 +1236,6 @@ dependencies = [ "winapi-util", ] -[[package]] -name = "scopeguard" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" - [[package]] name = "semver" version = "1.0.28" @@ -1594,53 +1285,12 @@ dependencies = [ "zmij", ] -[[package]] -name = "serde_spanned" -version = "0.6.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf41e0cfaf7226dca15e8197172c295a782857fcb97fad1808a166870dee75a3" -dependencies = [ - "serde", -] - [[package]] name = "shlex" version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" -[[package]] -name = "slab" -version = "0.4.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" - -[[package]] -name = "smallvec" -version = "1.15.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03" - -[[package]] -name = "socket2" -version = "0.5.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e22376abed350d73dd1cd119b57ffccad95b4e585a7cda43e286245ce23c0678" -dependencies = [ - "libc", - "windows-sys 0.52.0", -] - -[[package]] -name = "socket2" -version = "0.6.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3a766e1110788c36f4fa1c2b71b387a7815aa65f88ce0229841826633d93723e" -dependencies = [ - "libc", - "windows-sys 0.61.2", -] - [[package]] name = "syn" version = "2.0.117" @@ -1652,25 +1302,6 @@ dependencies = [ "unicode-ident", ] -[[package]] -name = "system-deps" -version = "6.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a3e535eb8dded36d55ec13eddacd30dec501792ff23a0b1682c38601b8cf2349" -dependencies = [ - "cfg-expr", - "heck", - "pkg-config", - "toml", - "version-compare", -] - -[[package]] -name = "target-lexicon" -version = "0.12.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61c41af27dd6d1e27b1b16b489db798443478cef1f06a660c96db617ba5de3b1" - [[package]] name = "tempfile" version = "3.27.0" @@ -1681,7 +1312,7 @@ dependencies = [ "getrandom 0.4.2", "once_cell", "rustix", - "windows-sys 0.61.2", + "windows-sys", ] [[package]] @@ -1714,96 +1345,6 @@ dependencies = [ "serde_json", ] -[[package]] -name = "tokio" -version = "1.52.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8fc7f01b389ac15039e4dc9531aa973a135d7a4135281b12d7c1bc79fd57fffe" -dependencies = [ - "libc", - "mio", - "pin-project-lite", - "socket2 0.6.3", - "tokio-macros", - "windows-sys 0.61.2", -] - -[[package]] -name = "tokio-macros" -version = "2.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "385a6cb71ab9ab790c5fe8d67f1645e6c450a7ce006a33de03daa956cf70a496" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "toml" -version = "0.8.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc1beb996b9d83529a9e75c17a1686767d148d70663143c7854d8b4a09ced362" -dependencies = [ - "serde", - "serde_spanned", - "toml_datetime", - "toml_edit", -] - -[[package]] -name = "toml_datetime" -version = "0.6.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "22cddaf88f4fbc13c51aebbf5f8eceb5c7c5a9da2ac40a13519eb5b0a0e8f11c" -dependencies = [ - "serde", -] - -[[package]] -name = "toml_edit" -version = "0.22.27" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "41fe8c660ae4257887cf66394862d21dbca4a6ddd26f04a3560410406a2f819a" -dependencies = [ - "indexmap", - "serde", - "serde_spanned", - "toml_datetime", - "winnow", -] - -[[package]] -name = "tracing" -version = "0.1.44" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" -dependencies = [ - "pin-project-lite", - "tracing-attributes", - "tracing-core", -] - -[[package]] -name = "tracing-attributes" -version = "0.1.31" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "tracing-core" -version = "0.1.36" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" -dependencies = [ - "once_cell", -] - [[package]] name = "unarray" version = "0.1.4" @@ -1822,23 +1363,6 @@ version = "0.2.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" -[[package]] -name = "uuid" -version = "1.23.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ddd74a9687298c6858e9b88ec8935ec45d22e8fd5e6394fa1bd4e99a87789c76" -dependencies = [ - "getrandom 0.4.2", - "js-sys", - "wasm-bindgen", -] - -[[package]] -name = "version-compare" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "03c2856837ef78f57382f06b2b8563a2f512f7185d732608fd9176cb3b8edf0e" - [[package]] name = "wait-timeout" version = "0.2.1" @@ -1858,12 +1382,6 @@ dependencies = [ "winapi-util", ] -[[package]] -name = "wasi" -version = "0.11.1+wasi-snapshot-preview1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" - [[package]] name = "wasip2" version = "1.0.1+wasi-0.2.4" @@ -1882,57 +1400,6 @@ dependencies = [ "wit-bindgen 0.51.0", ] -[[package]] -name = "wasite" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b8dad83b4f25e74f184f64c43b150b91efe7647395b42289f38e50566d82855b" - -[[package]] -name = "wasm-bindgen" -version = "0.2.122" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ed04576f974d2b2fba0f38c51dbc5518011e38c36bf1143164be765528fd409" -dependencies = [ - "cfg-if", - "once_cell", - "rustversion", - "wasm-bindgen-macro", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-macro" -version = "0.2.122" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "916151b09da36bd82f6615cbf3a419e2f0ba23a03c6160e8e92eb6bd4aa1dec6" -dependencies = [ - "quote", - "wasm-bindgen-macro-support", -] - -[[package]] -name = "wasm-bindgen-macro-support" -version = "0.2.122" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "299047362ccbfce148b67ab7e73349f77748e00c8296f9542adfad2ad82c5c5e" -dependencies = [ - "bumpalo", - "proc-macro2", - "quote", - "syn", - "wasm-bindgen-shared", -] - -[[package]] -name = "wasm-bindgen-shared" -version = "0.2.122" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a929b2c61f11ba3e9bc35b50c1f25cb38e0e892c0c231ae2b8cf78d5dad4437" -dependencies = [ - "unicode-ident", -] - [[package]] name = "wasm-encoder" version = "0.244.0" @@ -1961,40 +1428,19 @@ version = "0.244.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "47b807c72e1bac69382b3a6fb3dbe8ea4c0ed87ff5629b8685ae6b9a611028fe" dependencies = [ - "bitflags 2.11.1", + "bitflags", "hashbrown 0.15.5", "indexmap", "semver", ] -[[package]] -name = "web-sys" -version = "0.3.99" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6d621441cfc37b84979402712047321980c178f299193a3589d05b99e8763436" -dependencies = [ - "js-sys", - "wasm-bindgen", -] - -[[package]] -name = "whoami" -version = "1.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d4a4db5077702ca3015d3d02d74974948aba2ad9e12ab7df718ee64ccd7e97d" -dependencies = [ - "libredox", - "wasite", - "web-sys", -] - [[package]] name = "winapi-util" version = "0.1.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" dependencies = [ - "windows-sys 0.61.2", + "windows-sys", ] [[package]] @@ -2003,15 +1449,6 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" -[[package]] -name = "windows-sys" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" -dependencies = [ - "windows-targets", -] - [[package]] name = "windows-sys" version = "0.61.2" @@ -2021,79 +1458,6 @@ dependencies = [ "windows-link", ] -[[package]] -name = "windows-targets" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" -dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", -] - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" - -[[package]] -name = "windows_i686_gnu" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" - -[[package]] -name = "windows_i686_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" - -[[package]] -name = "windows_i686_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.52.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" - -[[package]] -name = "winnow" -version = "0.7.15" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "df79d97927682d2fd8adb29682d1140b343be4ac0f08fd68b7765d9c059d3945" -dependencies = [ - "memchr", -] - [[package]] name = "wit-bindgen" version = "0.46.0" @@ -2164,7 +1528,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9d66ea20e9553b30172b5e831994e35fbde2d165325bec84fc43dbf6f4eb9cb2" dependencies = [ "anyhow", - "bitflags 2.11.1", + "bitflags", "indexmap", "log", "serde", @@ -2214,40 +1578,8 @@ dependencies = [ "syn", ] -[[package]] -name = "zeromq-src" -version = "0.2.6+4.3.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc120b771270365d5ed0dfb4baf1005f2243ae1ae83703265cb3504070f4160b" -dependencies = [ - "cc", - "dircpy", -] - [[package]] name = "zmij" version = "1.0.21" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa" - -[[package]] -name = "zmq" -version = "0.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd3091dd571fb84a9b3e5e5c6a807d186c411c812c8618786c3c30e5349234e7" -dependencies = [ - "bitflags 1.3.2", - "libc", - "zmq-sys", -] - -[[package]] -name = "zmq-sys" -version = "0.12.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e8351dc72494b4d7f5652a681c33634063bbad58046c1689e75270908fdc864" -dependencies = [ - "libc", - "system-deps", - "zeromq-src", -] diff --git a/artifacts/verification/FV-FALCON-FAULT-003.yaml b/artifacts/verification/FV-FALCON-FAULT-003.yaml new file mode 100644 index 0000000..09a953a --- /dev/null +++ b/artifacts/verification/FV-FALCON-FAULT-003.yaml @@ -0,0 +1,108 @@ +artifacts: + - id: FV-FALCON-FAULT-003 + type: sw-verification + title: "Single-rotor-out FULL-LOOP recovery — parasitic-free rank-3 allocation + production-core spin-descent oracle (v1.114)" + status: implemented + release: falcon-v1.114.0 + description: > + Strengthens FV-FALCON-FAULT-002. That artifact verified the + rotor-out chain only in an IDEALISED attitude-only harness + (falcon-sitl-gz fault_tolerance_chain_recovers_from_rotor_loss: + constant hover thrust, TRUE-state feedback, level b3_d) — under + which the recovery merely leans and stays < 80°. The FULL + production loop (IEKF estimator + altitude loop + the FDI + attitude/rate gate) was never exercised, and it did NOT actually + recover: it flipped to ~180° (reproduced on the real gz plant and, + now, offline). v1.114 finds the cause, fixes it, and adds the + first full-loop recovery oracle. + + ROOT CAUSE (mechanically proven, no sim needed). + The reconfigured allocator reused the 4-rotor MIXER_X rows with the + failed rotor zeroed. Zeroing one rotor breaks the zero-sum property + of each torque column, so the collective THRUST leaks into + roll/pitch — a parasitic body moment ≈ collective toward the dead + corner. At hover (collective 0.585) that is roll +0.585 / pitch + −0.585 (58% of full authority) at ZERO commanded torque. The + reduced-attitude law can only oppose it from a growing tilt; once + the altitude loop raises thrust to replace the lost lift the + parasitic grows too → runaway → flip. + - Test: relay-mix-quad mix_p08_rotor_out_zero_torque_has_no_parasitic_moment + (asserts produced roll/pitch ≈ 0 at zero command, for every + failed index — the guard that would have caught this). + + FIX — RANK-3 allocation (MIX-P08, v1.114). + mix_rotor_out now SOLVES the 3×3 system for the three healthy rotor + commands that produce exactly (collective, roll, pitch), yaw free, + via a collective base + a zero-collective moment delta scaled by the + existing scale_to_fit desaturation (so moment DIRECTION and + collective are preserved and a clamp cannot flip a moment's sign). + This kills the diagonal-opposite rotor and drives the through-CoM + pair: pure lift, zero parasitic tilt, controlled yaw spin. Floor + lowered to 0 (the opposite rotor legitimately rests at 0). A + TAU_BOUND input clamp keeps the Cramer solve below f32 overflow. + - Kani: verify_mix_rotor_out_bound VERIFICATION SUCCESSFUL + (0 of 377 checks failed) — the MIX-P08 bound (failed rotor 0, + healthy ∈ [floor,1], all finite, yaw relinquished) still holds + for ANY input and ANY failed index < 4 under the new solve. + - Tests: relay-mix-quad + mix_p08_rotor_out_preserves_moment_direction (produced moment is + s·commanded for s∈[0,1] — parallel, no sign flip, no overshoot), + mix_p08_rotor_out_small_command_is_exact (feasible small command + realised exactly), mix_p08_rotor_out_pins_failed_and_bounds_healthy. + + FULL-LOOP RECOVERY ORACLE (the gap FV-002 left). + falcon-core survives_single_rotor_failure_without_flipping flies the + PRODUCTION FlightCore (verified IEKF → geometric SE(3) → + reduced-attitude + rank-3 allocation, driven by the real FDI on ESC + RPM) on the analytic rigid-body plant. After a rotor-0 loss from a + settled hover the FDI isolates within a step, and the vehicle holds + the thrust axis near-level (peak tilt < 0.5 rad; steady descent + < ~6°), keeps the relinquished-yaw spin bounded (< 10 rad/s), and + lands near-upright (touchdown tilt < 0.35 rad) — a controlled + spin-descent, not a tumble. + + DISPERSED GUARD — closing the campaign blind spot (v1.114). + The Monte-Carlo `run_motor_out_campaign` dispersed over the SAME + idealised attitude-only sim as FV-002 (its header says so), so + thousands of trials could not falsify the flip — dispersion adds + breadth, not fidelity. New `run_fullloop_motor_out_campaign` disperses + 200 trials over the FULL production FlightCore (IEKF + altitude + FDI + + rank-3 allocation, rot_drag plant) across failed-rotor, hover altitude, + rotational drag, and sensor-noise axes, asserting no-flip + bounded + spin + correct FDI + no-NaN. Measured worst case: peak tilt 0.274 rad, + yaw 5.7 rad/s, FDI detect 5 steps, 0/200 failures. A parasitic-moment + regression drives peak tilt toward π and trips this guard — the guard + the idealised campaign could not provide. (Altitude after the loss is + the FlightSupervisor's LAND job — reported, not gated, here.) + + PLANT FIDELITY (v1.114). + SimBackend gains quadratic ROTATIONAL aero drag (rot_drag, τ = + −c·ω·|ω|, opt-in like drag_quad/wind/turbulence, default off). + Without it the analytic plant has no torque to oppose the rotor-out + yaw reaction (both surviving lift rotors share a spin sense), so the + spin runs away and corrupts the estimator; real air bounds it. This + is why the idealised FV-002 harness (no estimator, constant thrust) + never saw the failure. SimBackend also gains fail_rotor + a + read_motor_rpm implementation so the same rigid-body plant now + exercises the FDI + recovery end to end. + + FALSIFICATION: this recovery is wrong if a single-rotor-out from a + settled hover is observed to exceed ~45° tilt, spin past ~10 rad/s, + or fail to reach a near-upright landing — on the analytic plant + (this oracle) or the gz plant. + tags: [verification, falcon, fault-tolerance, rotor-loss, control-allocation, kani, full-loop, v1.114] + fields: + method: formal-verification + steps: + - run: cargo kani -p relay-mix-quad --harness verify_mix_rotor_out_bound + - run: cargo test -p relay-mix-quad mix_p08_rotor_out_zero_torque_has_no_parasitic_moment + - run: cargo test -p relay-mix-quad mix_p08_rotor_out_preserves_moment_direction + - run: cargo test -p relay-mix-quad mix_p08_rotor_out_small_command_is_exact + - run: cargo test -p falcon-core survives_single_rotor_failure_without_flipping + - run: cargo test -p falcon-sitl-gz fullloop_motor_out_monte_carlo_campaign + - run: cargo test -p falcon-sitl-gz fault_tolerance_chain_recovers_from_rotor_loss + links: + - type: verifies + target: SWREQ-FALCON-FAULT-P02 + - type: refines + target: FV-FALCON-FAULT-002 diff --git a/artifacts/verification/FV-FALCON-SITLCORE-001.yaml b/artifacts/verification/FV-FALCON-SITLCORE-001.yaml new file mode 100644 index 0000000..8913ba1 --- /dev/null +++ b/artifacts/verification/FV-FALCON-SITLCORE-001.yaml @@ -0,0 +1,74 @@ +artifacts: + - id: FV-FALCON-SITLCORE-001 + type: sw-verification + title: "FlightCore-in-the-loop — the production core flies the SITL plant through the FlightBackend seam (v1.113)" + status: implemented + release: falcon-v1.113.0 + description: > + Closes the orphaned-core gap in the Gazebo SITL bench: every prior + `run_*` scenario re-implemented the flight cascade by hand against the + example's `Physics` plant, so the sim exercised a PARALLEL controller, not + the one that ships. This verification flies the PRODUCTION + `falcon_core::FlightCore` (the verified IEKF → geometric-SE(3) → ADRC → + mixer cascade with the single-rotor-out FDI + degraded-allocator recovery) + through the SITL plant via an ADAPTER — `SitlBackend` + (examples/falcon-sitl-gz/src/flightcore.rs), which implements + falcon-core's `FlightBackend` seam on top of a `&mut dyn Physics` plant. + The same byte-for-byte core that targets real hardware now flies the sim + (mock today; the real Gazebo 6-DOF plant under `--features gazebo`, + `--scenario=flightcore`). + + Seam reconciliation: `FlightBackend` is a sensor/actuator port (the core + pulls sensors, pushes motors); `Physics` is a plant (push motor PWMs + + step(dt), then measure()). The adapter advances the plant inside + write_motors — the tail of one FlightCore::step — so each control tick is + exactly one physics step in causal order (sense state(t) → decide → + actuate-and-advance to state(t+dt)). GNSS fixes are offered at 5 Hz + (divisor 50 @ 250 Hz), so the core dead-reckons on the IMU between fixes + exactly as against a real receiver. + + SUPPORTING CORRECTNESS FIX (MockPhysics): the toy plant previously + reported a CONSTANT [0,0,−g] accelerometer, which starves a full IEKF's + vertical-velocity estimate (the filter's process model says "not + accelerating" while GNSS says "climbing" → the two fight into a lag → + runaway climb, measured 704 m off before the fix). An accelerometer + measures SPECIFIC FORCE (a_kinematic − g_gravity); MockPhysics now reports + the true NED specific force (thrust − drag) rotated into the body frame — + faithful, and backward-compatible at hover/level where it reduces to the + old [0,0,−g] constant (the near-level hand-cascade scenarios and the + existing IMU unit tests are unchanged). + + GATE 1 (production_flightcore_holds_altitude_through_sitl_plant): the + shipping FlightCore commanded a 2 m altitude hold through the plant and + settled within 0.5 m with < 1.0 m steady RMS — the same bar as the + hand-rolled altitude scenario, met by the code that flies rather than a + bench re-implementation. Measured: final 0.04 m, steady RMS 0.06 m, no + NaN/divergence. + + GATE 2 (production_flightcore_fdi_isolates_failed_rotor_through_sitl_seam): + the single-rotor-out FDI fires THROUGH the SITL seam. The seam gained a + per-rotor RPM channel (Physics::motor_rpm) + a fault-injection hook + (Physics::fail_rotor), and the adapter now forwards read_motor_rpm — before + this the FDI was INERT in SITL (no ESC telemetry reached the core). A rotor + is failed mid-flight; the mock reports it at 0 RPM; the core's + commanded-vs-achieved CUSUM (relay-iekf RotorFaultDetector) isolates + EXACTLY the failed rotor (verified for rotors 0 and 2). This gates the + telemetry PLUMBING the recovery depends on. Full falcon-sitl-gz suite + 29/29 green. + + Honest scope: the mock plant responds to collective thrust only + (differential-motor torque is approximated as zero), so it can prove FDI + ISOLATION (the RPM residual) but NOT the 3D tilt-recovery or a post-failure + altitude hold — those need the full-physics `--features gazebo` backend + (the recordable `--scenario=flightcore-rotorout` flight, now reachable via + this seam, is the next slice). + tags: [verification, falcon, simulation, sitl, flightcore, hal, seam, v1.113] + fields: + method: simulation + steps: + - run: "cargo test -p falcon-sitl-gz production_flightcore # bench-only (builds the campaign suite; runs in the closed-loop-sim CI job + workspace test)" + links: + - type: verifies + target: SWREQ-FALCON-HAL-P01 + - type: verifies + target: SWREQ-FALCON-FAULT-P02 diff --git a/crates/falcon-core/plain/src/lib.rs b/crates/falcon-core/plain/src/lib.rs index b4be0dc..f6340ed 100644 --- a/crates/falcon-core/plain/src/lib.rs +++ b/crates/falcon-core/plain/src/lib.rs @@ -46,6 +46,16 @@ pub trait FlightBackend { /// Latest magnetometer field in the body frame (direction only), or /// `None` if unavailable. fn read_mag(&mut self) -> Option; + /// Latest absolute heading (yaw, rad, NED — CW from North), or `None` if no + /// heading source. A DIRECT yaw reference (a fused compass, a dual-antenna + /// GNSS heading, or a sim's truth heading) fed straight to the IEKF's + /// `update_yaw` — the alternative to inferring yaw from the raw + /// magnetometer field via `read_mag`, for backends that already resolve a + /// clean heading. Default `None` (v1.113). A backend supplies at most one of + /// `read_mag`/`read_heading`; both feed the same unobservable yaw state. + fn read_heading(&mut self) -> Option { + None + } /// Write the per-rotor commands ∈ [0,1] to the actuators. fn write_motors(&mut self, motors: &[f32]); /// Control period (s) for this tick. @@ -124,6 +134,28 @@ pub struct FlightCore { fdi: RotorFaultDetector, /// The isolated failed rotor (latched), or `None`. Drives the degraded path. failed_motor: Option, + /// 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 + /// would false-trip the CUSUM and drop a healthy vehicle into the degraded + /// 3-rotor law. The FDI is held off until the actuators have spun up + /// (`fdi_warmup_steps`), then runs normally (v1.113 — caught flying the + /// production core against the gz plant, where achieved starts at 0). + step_count: u32, + fdi_warmup_steps: u32, + /// Commanded heading (yaw, rad, NED). A quad HOLDS its launch heading, not + /// North — so when a heading reference first arrives (`read_heading`), the + /// initial heading is captured here and the geometric controller drives yaw + /// to it (not to 0, which would demand an unachievable snap-to-North on + /// takeoff and saturate the mixer). v1.113. Stays 0 for backends with no + /// heading source (unchanged behaviour). + yaw_setpoint: f32, + yaw_captured: bool, + /// Diagnostics: the last geometric desired body rate + ADRC torque (for the + /// gz yaw-loop investigation). Not part of the control state. + last_omega_d: Vec3, + last_torque: Vec3, /// Sensor calibration applied to raw IMU/mag samples before the estimator /// (gyro/accel bias+scale, mag hard/soft-iron). Identity until /// `set_calibration` installs solved offsets — the explicit replacement for @@ -166,6 +198,15 @@ impl FlightCore { // CUSUM thresholds matching the SITL-verified fault-tolerance chain. fdi: RotorFaultDetector::new(0.5, 0.1), failed_motor: None, + step_count: 0, + yaw_setpoint: 0.0, + yaw_captured: false, + last_omega_d: [0.0; 3], + last_torque: [0.0; 3], + // Hold the FDI off for ~0.2 s of spin-up (≥10 steps floor); the + // achieved rotor state has caught the command by then, so the + // effectiveness residual reflects real faults, not the spin-up jump. + fdi_warmup_steps: ((loop_hz * 0.2) as u32).max(10), calib: relay_calib::CalParams::identity(), } } @@ -208,6 +249,19 @@ impl FlightCore { self.failed_motor } + /// The captured heading-hold setpoint (yaw, rad, NED). For telemetry/tests. + pub fn yaw_setpoint(&self) -> f32 { + self.yaw_setpoint + } + + /// Diagnostics: last geometric desired body rate + last ADRC torque. + pub fn last_omega_d(&self) -> Vec3 { + self.last_omega_d + } + pub fn last_torque(&self) -> Vec3 { + self.last_torque + } + /// Command a target altitude (NED z, metres; negative = up). v1.2. pub fn set_altitude(&mut self, ned_z: f32) { self.setpoint[2] = ned_z; @@ -249,6 +303,23 @@ impl FlightCore { self.pos_var = var; } + /// Set the altitude P and D gains (v1.113). The defaults (0.05, 0.30) are + /// gentle, tuned for the analytic plant; a stiffer plant (real gz) wants a + /// higher kp to hold altitude firmly and a matched kd to damp the slow + /// climb/overshoot oscillation the soft loop leaves. + pub fn set_altitude_gains(&mut self, kp: f32, kd: f32) { + self.kp_alt = kp; + self.kd_alt = kd; + } + + /// Set the IEKF velocity/position covariance-diagonal floor (m²/s², m²) + /// (v1.113). 0 = off (default). Keeps the estimator from going deaf on a + /// long static hover (the covariance would otherwise collapse and the NIS + /// gate reject the correct fixes). See `Iekf::set_process_floor`. + pub fn set_process_floor(&mut self, vel: f32, pos: f32) { + self.iekf.set_process_floor(vel, pos); + } + /// The estimated nav state (for telemetry / tests). pub fn state(&self) -> NavState { self.iekf.state() @@ -274,6 +345,35 @@ impl FlightCore { if let Some(m) = b.read_mag() { 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 + // straight to the IEKF, making the otherwise-unobservable yaw observable + // without inferring it from a raw magnetometer field. + if let Some(yaw) = b.read_heading() { + self.iekf.update_yaw(yaw, self.mag_var); + // Capture the launch heading as the hold setpoint — the vehicle holds + // THIS heading, not North. Captured AFTER the estimator+heading have + // settled (past the warmup), NOT on the first raw sample, which can + // be an init transient (gz's first Pose_V gave 1.57 rad while the + // true heading was 0.94 — a frozen 0.6 rad error saturated the mixer). + if !self.yaw_captured { + // TRACK the ESTIMATE's yaw (not the raw heading) as the setpoint + // while it settles, then FREEZE. Tracking the estimate keeps the + // yaw error ≈ 0 during startup, so NO yaw torque is commanded — + // tracking the raw heading instead left a transient mismatch + // (heading 1.57 vs a lagging est 0.14) that commanded an initial + // yaw torque and spun the airframe up. Freeze at 5× the warmup + // (~1 s), by when the estimate has converged to the true heading. + let e = self.iekf.state(); + self.yaw_setpoint = relay_math::atan2f( + 2.0 * (e.q[0] * e.q[3] + e.q[1] * e.q[2]), + 1.0 - 2.0 * (e.q[2] * e.q[2] + e.q[3] * e.q[3]), + ); + if self.step_count >= self.fdi_warmup_steps * 5 { + self.yaw_captured = true; + } + } + } // v1.20 — barometer: feed it into the verified IEKF as a vertical anchor // (a position update whose horizontal is the current estimate, a no-op, // and whose z is the baro). This keeps the IEKF's altitude AND vertical @@ -351,15 +451,40 @@ impl FlightCore { self.mixer.mix_rotor_out(failed, torque, thrust, ROTOR_OUT_FLOOR) } else { // NORMAL: full-attitude geometric desired-rate → ADRC torque → mix. - let omega_d = self.geo.desired_rate(est.q, a_cmd, 0.0); + let omega_d = self.geo.desired_rate(est.q, a_cmd, self.yaw_setpoint); let torque = self.adrc.tick(gyro_f, omega_d, dt); - self.mixer.mix(torque, thrust) + self.last_omega_d = omega_d; + self.last_torque = torque; + // THRUST-PRIORITY mix (MIX-P05): scale torque down to keep every + // motor in [floor,1] rather than sacrificing collective — so a large + // transient torque (e.g. the yaw slew to the launch heading) cannot + // steal lift and pin the vehicle to the ground. The gz bench needs + // this: attitude-priority `mix` collapsed collective near saturation + // → never lifted. Zero-sum torque columns keep the mean (collective) + // exactly `thrust` (v1.113). + self.mixer.mix_thrust_floor(torque, thrust, 0.0) }; // ── Single-rotor-out FDI ── form the per-rotor effectiveness residual // |commanded − achieved| from ESC RPM telemetry and feed the CUSUM; on // isolation, latch the failed rotor (next step runs the degraded path). - if self.failed_motor.is_none() { + // Held off for `fdi_warmup_steps` (spin-up guard) — see the field docs. + self.step_count = self.step_count.saturating_add(1); + // ATTITUDE/RATE GATE: a rotor-out residual is only DISTINGUISHABLE in + // near-level, low-rate flight. During an attitude transient the + // controller commands large asymmetric motors, so |commanded − achieved| + // spikes on the throttled/saturated rotors and would false-trip the CUSUM + // — you cannot diagnose a dead rotor while the airframe is tumbling. Only + // run the detector when the vehicle is roughly level and not spinning + // (v1.113 — caught a phantom rotor-out during an attitude transient on gz). + let tilt_cos = 1.0 - 2.0 * (est.q[1] * est.q[1] + est.q[2] * est.q[2]); // R[2][2] + let rate2 = + gyro_f[0] * gyro_f[0] + gyro_f[1] * gyro_f[1] + gyro_f[2] * gyro_f[2]; + let fdi_steady = tilt_cos > 0.90 && rate2 < 1.0; // ≲26° tilt, ≲1 rad/s + if self.failed_motor.is_none() + && self.step_count >= self.fdi_warmup_steps + && fdi_steady + { if let Some(rpm) = b.read_motor_rpm() { let mut resid = [0.0f32; 4]; let mut i = 0; @@ -398,10 +523,15 @@ const WAYPOINT_RADIUS: f32 = 1.2; /// residual (v1.103). Only the ratio matters; a per-airframe value scales it. const ESC_RPM_FULL: f32 = 8000.0; -/// Thrust floor for the reconfigured (single-rotor-out) allocator — the three -/// healthy rotors are kept at/above this so the body retains tilt authority while -/// it stabilises on S². Matches the SITL-verified fault-tolerance composition. -const ROTOR_OUT_FLOOR: f32 = 0.15; +/// Thrust floor for the reconfigured (single-rotor-out) allocator. With the +/// rank-3 allocation (v1.114) the diagonal-opposite of the failed rotor +/// legitimately rests at 0 — that couple-cancelling zero is what gives the +/// pair a PARASITIC-FREE pure-lift axis — so the floor is 0: a positive floor +/// would clamp that rotor up and reinject the very moment the allocation +/// removes (and, being below the desaturation base, would collapse all +/// roll/pitch authority to zero). Tilt authority now comes from the solved +/// allocation, not a floor. +const ROTOR_OUT_FLOOR: f32 = 0.0; /// Pre-arm estimator-convergence ceiling (v1.99): the IEKF tilt-uncertainty /// (roll²+pitch², rad²) must be at/below this for `estimator_converged` to pass. @@ -1007,6 +1137,14 @@ pub struct SimBackend { /// wind term it grows with v², so it dominates during fast motion (mission /// legs) and caps the drift speed; it is also a stabilising damping force. pub drag_quad: f32, + /// Quadratic ROTATIONAL (aerodynamic) drag coefficient (v1.114): a body-rate + /// damping torque τ = −c·ω·|ω| per axis. Quadratic so it is negligible at + /// normal attitude rates (≲1 rad/s) yet bounds a runaway spin — the effect + /// that caps a real single-rotor-out quad's yaw spin (both surviving lift + /// rotors share a spin sense, so the reaction torque is otherwise + /// unopposed). Off (0) by default so existing tests are unchanged; the + /// rotor-out recovery opts in, as it does for the other aero pathologies. + pub rot_drag: f32, /// Barometer present? (v1.20) When true, `read_baro` returns the altitude. pub baro_enabled: bool, /// Barometric altitude noise stddev (m) (v1.20). @@ -1050,6 +1188,11 @@ pub struct SimBackend { rng: u32, /// Dryden-like turbulence gust state (OU process, horizontal x/y) (v1.25). turb_state: Vec3, + /// Injected SINGLE-ROTOR FAILURE (v1.114): the named rotor produces no + /// thrust or torque (its lagged actual is pinned to 0) and its ESC reports + /// RPM 0, so the same rigid-body plant exercises the FDI + reduced-attitude + /// recovery that only the gz bench used to see. `None` = all healthy. + failed_rotor: Option, } const GRAVITY: f32 = 9.81; @@ -1070,6 +1213,7 @@ impl SimBackend { wind: [0.0; 3], gust_amp: 0.0, drag_quad: 0.0, + rot_drag: 0.0, baro_enabled: false, baro_noise: 0.0, battery_v: 16.0, @@ -1085,6 +1229,7 @@ impl SimBackend { gyro_bias: [0.0; 3], rng: 0x9E3779B9, // a fixed, non-zero seed (golden-ratio constant) turb_state: [0.0; 3], + failed_rotor: None, } } @@ -1094,6 +1239,20 @@ impl SimBackend { self } + /// Kill rotor `i` (v1.114): from now on it produces no thrust/torque and its + /// ESC reports RPM 0, driving the core's FDI + reduced-attitude recovery on + /// the analytic plant (the offline twin of the gz rotor-out bench). + pub fn fail_rotor(&mut self, i: usize) { + self.failed_rotor = Some(i); + } + + /// Reseed the deterministic noise LCG (v1.114): lets a Monte-Carlo campaign + /// give each trial an independent noise realisation (otherwise every fresh + /// backend replays the same sequence). A zero seed is nudged off 0. + pub fn reseed(&mut self, seed: u32) { + self.rng = if seed == 0 { 0x9E37_79B9 } else { seed }; + } + /// Body-frame tilt from level (rad): the angle of the body z-axis from NED down. pub fn tilt(&self) -> f32 { relay_math::acosf(self.r[2][2].clamp(-1.0, 1.0)) @@ -1114,7 +1273,10 @@ impl SimBackend { self.omega[0] * jo[1] - self.omega[1] * jo[0], ]; for i in 0..3 { - self.omega[i] += self.dt * (torque[i] - gyro[i]) / self.j[i]; + // Quadratic rotational aero drag τ_d = −c·ω·|ω| (v1.114): bounds a + // rotor-out spin, negligible at normal rates. Uses the pre-update ω. + let drag = self.rot_drag * self.omega[i] * relay_math::fabsf(self.omega[i]); + 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]; @@ -1203,7 +1365,6 @@ impl FlightBackend for SimBackend { // ACTUAL, not the command — exactly the actuator lag the ADRC ESO is // built to absorb (v0.25). τ = 0 ⇒ instantaneous (prior behaviour). let mut m4 = [0.0f32; 4]; - let mut collective = 0.0f32; for i in 0..4 { let cmd = motors.get(i).copied().unwrap_or(0.0); if self.motor_tau > 0.0 { @@ -1212,8 +1373,18 @@ impl FlightBackend for SimBackend { } else { m4[i] = cmd; } - collective += m4[i]; } + // v1.114 — a FAILED rotor produces no thrust/torque regardless of the + // (lagged) command. Pin it to 0 BEFORE the collective/torque sums so + // the plant experiences the true rotor-out upset the recovery fights. + if let Some(f) = self.failed_rotor { + m4[f] = 0.0; + } + let mut collective = 0.0f32; + for &v in m4.iter() { + collective += v; + } + self.motor_state = m4; // achieved state (what the ESCs report back) // attitude: allocated torque + the injected disturbance the ESO rejects let tq = motors_to_torque_signs(m4); let torque = [ @@ -1327,6 +1498,16 @@ impl FlightBackend for SimBackend { None } } + fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + // ACHIEVED per-rotor RPM from the lagged actual motor state — a failed + // rotor reads 0 (it was pinned in `write_motors`). This is the FDI's + // commanded-vs-achieved source; with no failure it tracks the command. + let mut rpm = [0i32; 4]; + for (r, &s) in rpm.iter_mut().zip(self.motor_state.iter()) { + *r = (s * ESC_RPM_FULL) as i32; + } + Some(rpm) + } } #[cfg(test)] @@ -1545,6 +1726,81 @@ mod tests { } } + /// v1.114 — the RECOVERY, not just the isolation: the production core flown + /// on the analytic rigid-body plant survives a real single-rotor loss + /// without flipping. This is the offline twin of the gz rotor-out bench — + /// before v1.114 the rotor-out mixer injected a parasitic moment ≈ + /// collective toward the dead corner (proven by the mix-quad diagnostic), + /// tipping the vehicle monotonically past 90° into an inverted crash on gz. + /// The rank-3 allocation removes that moment, so the reduced-attitude law + /// keeps the thrust axis upright while the body spins freely in yaw. + #[test] + fn survives_single_rotor_failure_without_flipping() { + let dt = 0.004f32; + let r0 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; // level + let mut backend = SimBackend::new(r0, dt); + // Realistic aero for a rotor-out: rotational drag bounds the yaw spin + // (both surviving lift rotors share a spin sense), and ground contact + // lets the controlled spin-descent settle instead of falling through. + backend.rot_drag = 0.02; + backend.ground_contact = true; + let mut core = FlightCore::new(0.5, 1.0 / dt); + core.set_altitude(-3.0); // hold ~3 m + + // Reach a settled, level hover first (the FDI's gate only isolates a + // dead rotor from a near-level, low-rate state — you cannot diagnose + // one mid-tumble). + for _ in 0..3000 { + 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"); + + // 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 + // vehicle comes down and lands. Track the worst tilt + yaw rate while + // AIRBORNE, and capture the attitude at touchdown. + backend.fail_rotor(0); + let mut peak_tilt = 0.0f32; + let mut peak_yaw = 0.0f32; + let mut touchdown_tilt: Option = None; + for _ in 0..2000 { + core.step(&mut backend); + let alt = -backend.pos[2]; + if alt > 0.15 { + // airborne — this is the recovery flight we're grading + peak_tilt = peak_tilt.max(backend.tilt()); + peak_yaw = peak_yaw.max(relay_math::fabsf(backend.omega[2])); + } else if touchdown_tilt.is_none() { + touchdown_tilt = Some(backend.tilt()); // attitude as it lands + } + } + + 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 + // trajectory stayed < ~6° in steady descent, with a brief startup + // transient). + assert!( + peak_tilt < 0.5, + "vehicle must stay near-level after rotor-out: peak tilt {peak_tilt} \ + rad ({} deg)", + peak_tilt * 180.0 / core::f32::consts::PI, + ); + // Yaw is relinquished, so the body spins — but rotational drag bounds + // it (an unbounded spin corrupts the estimator). A few rad/s, not tens. + assert!( + peak_yaw < 10.0, + "rotor-out yaw spin must stay bounded: peak {peak_yaw} rad/s", + ); + // It descended and LANDED (a 3-rotor quad must come down), arriving + // near-upright — a controlled spin-landing, not a sideways crash. + let td = touchdown_tilt.expect("vehicle must descend and land"); + assert!(td < 0.35, "must land near-upright: touchdown tilt {td} rad"); + } + #[test] fn arbitrary_backend_drives_the_core() { struct NullBackend { diff --git a/crates/relay-adrc/plain/src/lib.rs b/crates/relay-adrc/plain/src/lib.rs index 971aacc..5c5cc01 100644 --- a/crates/relay-adrc/plain/src/lib.rs +++ b/crates/relay-adrc/plain/src/lib.rs @@ -272,14 +272,20 @@ impl AdrcRate { /// whose actuator lag ADRC is here to reject). pub fn falcon_quad() -> Self { Self::new([ - AdrcGains::with_tau(40.0, 12.0, 30.0, 0.0125), - AdrcGains::with_tau(40.0, 12.0, 30.0, 0.0125), + // b0 (control effectiveness ≈ τ_max/J) raised 30→60 for the gz + // plant: the real gz falcon-quad is ~2× more responsive than the + // analytic SimBackend the ADRC was matched to (motorConstant 8.55e-6 + // × arm 0.088 / J 0.0217 → higher α per unit torque), so b0=30 left + // the rate-loop gain ~2× too high → tumble. The ESO absorbs the + // residual b0 error for the analytic plant (46) too. v1.113. + AdrcGains::with_tau(40.0, 12.0, 60.0, 0.0125), + AdrcGains::with_tau(40.0, 12.0, 60.0, 0.0125), // Yaw: ω_o high (fast observer) but ω_c low (control bw below // the motor pole 1/τ≈40), AND the actuator lag τ modelled in // the ESO (the dominant v0.25 fix — yaw drives large slow Δω // through this lag, so it must be in the plant, not left as an // "unmodeled disturbance" the ESO destabilisingly cancels). - AdrcGains::with_tau(30.0, 3.0, 6.0, 0.025), + AdrcGains::with_tau(30.0, 3.0, 18.0, 0.025), ]) } diff --git a/crates/relay-iekf/plain/src/lib.rs b/crates/relay-iekf/plain/src/lib.rs index be9cbce..c42552c 100644 --- a/crates/relay-iekf/plain/src/lib.rs +++ b/crates/relay-iekf/plain/src/lib.rs @@ -354,6 +354,17 @@ pub struct Iekf { /// Error-state covariance (ordering [δθ, δv, δp, δb_g, δb_a]). p: Mat, cfg: IekfConfig, + /// EXTRA velocity/position process-noise rate (variance per second) added to + /// the δv/δp diagonal each propagate — on TOP of `q_accel`. This is additive + /// process noise, NOT a diagonal clamp: the propagation turns the sustained + /// δv uncertainty into a live δv↔δp CORRELATION, which is what lets a GNSS + /// position fix correct VELOCITY (a diagonal clamp leaves that correlation + /// collapsed → the velocity free-runs). Keeps the filter from going deaf on + /// a long static hover (P would otherwise collapse, the gain with it, and the + /// NIS gate reject the correct fixes). 0 = off (default; the estimator + /// campaigns are unchanged). Set via `set_process_floor`. + q_vel_extra: f32, + q_pos_extra: f32, } impl Iekf { @@ -372,13 +383,23 @@ impl Iekf { p[blk * 3 + i][blk * 3 + i] = v[blk]; } } - Iekf { state, p, cfg } + Iekf { state, p, cfg, q_vel_extra: 0.0, q_pos_extra: 0.0 } } pub fn level() -> Self { Self::new(NavState::identity()) } + /// Set the EXTRA velocity/position process-noise rate (variance/s), added to + /// the base `q_accel` (see `q_vel_extra`). 0 disables it (default). A + /// metre-class deployment on a long static hover wants a velocity term + /// comparable to a few × `q_accel` so the δv covariance — and its coupling + /// to δp — never collapses and the filter keeps trusting its position fixes. + pub fn set_process_floor(&mut self, vel: f32, pos: f32) { + self.q_vel_extra = vel.max(0.0); + self.q_pos_extra = pos.max(0.0); + } + pub fn state(&self) -> NavState { self.state } @@ -522,6 +543,15 @@ impl Iekf { self.p[9 + i][9 + i] += self.cfg.q_bias_gyro * dt; // δb_g self.p[12 + i][12 + i] += self.cfg.q_bias_accel * dt; // δb_a } + // Extra δv/δp process noise (0 = off): additive, so the propagation + // builds the δv↔δp correlation a GNSS fix needs to correct velocity — + // keeps a long static hover from starving the filter into rejecting its + // own fixes. (A diagonal clamp would leave the correlation collapsed and + // the velocity would free-run.) + for i in 0..3 { + self.p[3 + i][3 + i] += self.q_vel_extra * dt; // δv + self.p[6 + i][6 + i] += self.q_pos_extra * dt; // δp + } symmetrise(&mut self.p); } diff --git a/crates/relay-mix-quad/plain/src/lib.rs b/crates/relay-mix-quad/plain/src/lib.rs index e80266d..28c6c26 100644 --- a/crates/relay-mix-quad/plain/src/lib.rs +++ b/crates/relay-mix-quad/plain/src/lib.rs @@ -367,24 +367,119 @@ impl QuadMixer { ) -> [f32; 4] { let t = clamp01(sanitise(thrust)); let floor = clamp01(sanitise(floor)); - let r = sanitise(torque_body[0]); - let p = sanitise(torque_body[1]); + // Bound the (normalised) torque command: real controller output is + // |τ| ≲ 1, but this keeps the 3×3 Cramer solve below f32 overflow so a + // pathological huge input cannot make `det3` reach ∞ and thence + // ∞−∞ = NaN (MIX-P08 Kani: "NaN on division"). Well outside any real + // command, so it never bites in flight. + const TAU_BOUND: f32 = 8.0; + let r = sanitise(torque_body[0]).clamp(-TAU_BOUND, TAU_BOUND); + let p = sanitise(torque_body[1]).clamp(-TAU_BOUND, TAU_BOUND); // yaw = torque_body[2] is RELINQUISHED — never allocated. - let mut m = [0.0_f32; 4]; - for i in 0..4 { - if i == failed { - m[i] = 0.0; // failed rotor OFF (below the healthy floor, by design) - } else { + + if failed >= 4 { + // No failure: allocate thrust + roll + pitch over all four (no + // yaw), clamped to [floor, 1] — the zero-sum torque columns keep + // the collective decoupled, so no parasitic moment arises here. + let mut m = [0.0_f32; 4]; + for i in 0..4 { let row = &MIXER_X[i]; - // thrust + roll + pitch only (no yaw term), clamped to [floor,1]. m[i] = clamp_floor(sanitise(t + row[1] * r + row[2] * p), floor); } + self.last_motors = m; + return m; + } + + // RANK-3 allocation over the three HEALTHY rotors (v1.114). Reusing + // the 4-rotor rows with one rotor zeroed breaks the zero-sum of each + // torque column, so the collective leaks into roll/pitch — a + // parasitic moment ≈ `collective` toward the dead corner that the + // reduced-attitude law can only fight from a growing tilt (proven by + // `rotor_out_parasitic_moment_diagnostic`; it is what tipped the gz + // recovery into a flip). Instead SOLVE the 3×3 system for the healthy + // commands that produce exactly (collective, roll, pitch), yaw free: + // this kills the diagonal-opposite rotor and drives the through-CoM + // pair — pure lift, zero parasitic tilt, controlled yaw spin. + let h = [(failed + 1) % 4, (failed + 2) % 4, (failed + 3) % 4]; + // Collective SUM target over the healthy rotors: 3·t keeps the + // average healthy command at the per-rotor baseline `t` and — unlike + // targeting the full 4·t hover collective — leaves every healthy + // rotor headroom to swing for ROLL/PITCH authority rather than + // pinning them saturated (attitude authority > altitude hold in a + // rotor-out; the residual lift deficit is an honest controlled + // descent the altitude loop rides down). + let c = 3.0 * t; + let a = [ + [1.0, 1.0, 1.0], + [MIXER_X[h[0]][1], MIXER_X[h[1]][1], MIXER_X[h[2]][1]], + [MIXER_X[h[0]][2], MIXER_X[h[1]][2], MIXER_X[h[2]][2]], + ]; + // Split the allocation into a pure-COLLECTIVE base and a + // zero-collective MOMENT delta (the solve is linear, so + // `solve(c,r,p) = solve(c,0,0) + solve(0,r,p)`). The delta's + // collective row is 0, so it never disturbs total lift — it only + // redistributes for roll/pitch. Under saturation we then scale the + // delta (not the base) so MOMENT DIRECTION and COLLECTIVE are both + // preserved and a naive clamp cannot flip a moment's sign (the same + // priority the nominal `mix_thrust_floor` gives collective over + // torque). Singular/degenerate solve ⇒ safe zero allocation. + let mut base = [0.0_f32; 4]; + let mut delta = [0.0_f32; 4]; + if let (Some(b), Some(d)) = (solve3(a, [c, 0.0, 0.0]), solve3(a, [0.0, r, p])) { + for k in 0..3 { + base[h[k]] = b[k]; + delta[h[k]] = d[k]; + } + } + let s = scale_to_fit(&base, &delta, floor); + let mut m = [0.0_f32; 4]; + for i in 0..4 { + if i != failed { + m[i] = clamp_floor(sanitise(base[i] + s * delta[i]), floor); + } } + // failed rotor stays 0 (never allocated). self.last_motors = m; m } } +/// Determinant of a 3×3 matrix (row-major), by cofactor expansion. +#[inline] +fn det3(a: [[f32; 3]; 3]) -> f32 { + a[0][0] * (a[1][1] * a[2][2] - a[1][2] * a[2][1]) + - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]) +} + +/// Solve `A·x = rhs` (3×3) by Cramer's rule. Returns `None` when `A` is +/// singular or non-finite (|det| below eps) so the caller can fall back to a +/// safe zero allocation rather than emit NaNs. Used by the rotor-out +/// allocator to map (collective, roll, pitch) onto the healthy rotors. +#[inline] +fn solve3(a: [[f32; 3]; 3], rhs: [f32; 3]) -> Option<[f32; 3]> { + let det = det3(a); + if !det.is_finite() || libm_fabsf(det) < 1.0e-6 { + return None; + } + let mut x = [0.0_f32; 3]; + for k in 0..3 { + let mut ak = a; + for (row, rk) in ak.iter_mut().zip(rhs.iter()) { + row[k] = *rk; + } + x[k] = det3(ak) / det; + } + Some(x) +} + +/// `|x|` without pulling a math crate into this `no_std` leaf (the sign bit +/// mask is exact and branch-free — this stays on the verified floor). +#[inline] +fn libm_fabsf(x: f32) -> f32 { + f32::from_bits(x.to_bits() & 0x7fff_ffff) +} + /// Clamp `x` into `[lo, 1]`. Total over all f32: NaN and values below /// `lo` map to `lo`, values above 1 map to 1. `lo` is assumed in /// `[0, 1]` (the caller passes a sanitised + clamped floor). @@ -928,6 +1023,83 @@ mod tests { } } + #[test] + fn mix_p08_rotor_out_zero_torque_has_no_parasitic_moment() { + // MIX-P08 (v1.114): the rank-3 rotor-out allocator must NOT inject a + // parasitic roll/pitch moment at zero commanded torque. The naive + // "zero the failed rotor, keep the 4-rotor rows" allocation produced + // roll≈+collective / pitch≈−collective toward the dead corner (the + // gz-flip cause); the solved allocation produces ~0. + let mut m = QuadMixer::new(); + let thrust = 0.585_f32; // gz hover collective + for failed in 0..4 { + let motors = m.mix_rotor_out(failed, [0.0, 0.0, 0.0], thrust, 0.0); + assert_eq!(motors[failed], 0.0, "failed rotor {failed} must be OFF"); + let tq = motors_to_torque_signs(motors); + assert!( + libm_fabsf(tq[0]) < 1.0e-3 && libm_fabsf(tq[1]) < 1.0e-3, + "parasitic moment for failed={failed}: motors={motors:?} \ + roll={} pitch={}", + tq[0], + tq[1], + ); + } + } + + #[test] + fn mix_p08_rotor_out_preserves_moment_direction() { + // The solved allocation realises the COMMANDED roll/pitch when + // feasible, and under saturation produces `s·(roll,pitch)` for some + // `s ∈ [0,1]` — same DIRECTION, never overshooting or flipping sign + // (the base+delta / scale_to_fit split, mirroring `mix_thrust_floor`). + let mut m = QuadMixer::new(); + let thrust = 0.5_f32; + let (rc, pc) = (0.1_f32, -0.08_f32); + for failed in 0..4 { + let motors = m.mix_rotor_out(failed, [rc, pc, 0.3], thrust, 0.0); + let tq = motors_to_torque_signs(motors); + // Parallel to the command (produced × command = 0) ⇒ direction kept. + let cross = tq[0] * pc - tq[1] * rc; + assert!( + libm_fabsf(cross) < 1.0e-3, + "failed={failed}: produced moment ({},{}) not parallel to \ + command ({rc},{pc}) — motors={motors:?}", + tq[0], + tq[1], + ); + // No overshoot on either axis, and correct sign. + assert!( + tq[0] * rc >= -1.0e-6 && libm_fabsf(tq[0]) <= libm_fabsf(rc) + 1.0e-3, + "failed={failed}: roll {} overshoots/flips command {rc}", + tq[0], + ); + assert!( + tq[1] * pc >= -1.0e-6 && libm_fabsf(tq[1]) <= libm_fabsf(pc) + 1.0e-3, + "failed={failed}: pitch {} overshoots/flips command {pc}", + tq[1], + ); + } + } + + #[test] + fn mix_p08_rotor_out_small_command_is_exact() { + // A small command in a FEASIBLE direction (the yaw spin rotates the + // 3-rotor authority cone, so only one side is reachable at a time) + // must be realised EXACTLY (s=1). failed=0 loses M0(FR); the naturally + // -off rotor is M2(BL), and a +roll/−pitch command raises it, staying + // interior. + let mut m = QuadMixer::new(); + let motors = m.mix_rotor_out(0, [0.05, -0.05, 0.2], 0.5, 0.0); + let tq = motors_to_torque_signs(motors); + assert!( + libm_fabsf(tq[0] - 0.05) < 1.0e-3 && libm_fabsf(tq[1] + 0.05) < 1.0e-3, + "small feasible command not exact: produced roll={} pitch={} \ + (motors={motors:?})", + tq[0], + tq[1], + ); + } + #[test] fn mix_p05_thrust_floor_never_starves_collective() { // Aggressive torque at hover thrust: attitude-priority `mix` diff --git a/examples/falcon-sitl-gz/Cargo.toml b/examples/falcon-sitl-gz/Cargo.toml index fb702d5..52ffb5c 100644 --- a/examples/falcon-sitl-gz/Cargo.toml +++ b/examples/falcon-sitl-gz/Cargo.toml @@ -15,12 +15,13 @@ path = "src/main.rs" # Default: stub GazeboPhysics (v0.16.1 contract) — every default build # stays lean. No gz-transport deps. default = [] -# v0.18: real gz-transport bridge. Pulls in tokio + gz-transport-rs + -# zmq (libzmq compiled from C source via zeromq-src). ~30-60 s extra -# build time; opt-in only. v0.19.2 added `prost` direct dep for the -# locally-defined gz.msgs.Actuators message (not exported by -# gz-transport-rs 0.1.0). -gazebo = ["dep:tokio", "dep:gz-transport-rs", "dep:prost"] +# v1.113: real gz bridge on the C++-backed `gz-transport` crate (links the +# installed gz-transport13 via gz-transport-sys + pkg-config). Replaces the +# pure-Rust `gz-transport-rs` 0.1.0, which discovers but cannot complete the +# subscription handshake with Harmonic (see the gz-transport-rs diagnosis). +# Needs the gz-transport13 dev libs + PKG_CONFIG_PATH incl. the brew pkgconfig +# dir. Opt-in only. gz-msgs supplies the protobuf message types. +gazebo = ["dep:gz-transport", "dep:gz-msgs", "dep:crossbeam-channel"] [dependencies] relay-ekf = { path = "../../crates/relay-ekf" } @@ -36,12 +37,15 @@ falcon-config = { path = "../../host/falcon-config" } relay-pos = { path = "../../crates/relay-pos" } relay-lc = { path = "../../crates/relay-lc" } relay-sc = { path = "../../crates/relay-sc" } +# v1.113 — the PRODUCTION flight core, flown through the SITL plant via the +# `FlightBackend`→`Physics` adapter (src/flightcore.rs). The sim now exercises +# the same verified cascade + rotor-out FDI that ships, not a parallel copy. +falcon-core = { path = "../../crates/falcon-core" } libm = "0.2" -# Optional — only when feature `gazebo` is on. -tokio = { version = "1", features = ["rt", "rt-multi-thread", "sync", "macros"], optional = true } -gz-transport-rs = { version = "0.1", optional = true } -# v0.19.2 — local `gz.msgs.Actuators` definition so the bridge can talk -# to Harmonic's MulticopterMotorModel plugin (which subscribes Actuators, -# not the per-rotor Double the v0.18 bridge published). -prost = { version = "0.14", features = ["derive"], optional = true } +# Optional — only when feature `gazebo` is on. C++-backed gz-transport that +# links the installed gz-transport13 (Harmonic); gz-msgs supplies the protobuf +# message types (IMU/NavSat/Actuators/Pose_V/Magnetometer/Odometry). +gz-transport = { version = "0.10", features = ["harmonic"], optional = true } +gz-msgs = { version = "0.10", features = ["harmonic"], optional = true } +crossbeam-channel = { version = "0.5", optional = true } diff --git a/examples/falcon-sitl-gz/src/campaign.rs b/examples/falcon-sitl-gz/src/campaign.rs index 1c457f8..b401926 100644 --- a/examples/falcon-sitl-gz/src/campaign.rs +++ b/examples/falcon-sitl-gz/src/campaign.rs @@ -194,7 +194,11 @@ fn run_motor_out_trial(t: MotorOutTrial) -> MotorOutOutcome { for step in 0..4000u32 { let (_torque, motors_cmd) = if let Some(f) = isolated { let tq = ctrl.moment_reduced(&r, omega, b3_d); - (tq, QuadMixer::new().mix_rotor_out(f, tq, HOVER, FLOOR)) + // Rank-3 allocation (v1.114): the diagonal-opposite rotor must be + // free to reach 0 (a positive floor pins it up, reinjecting the + // parasitic moment and starving roll/pitch authority) — so pass 0, + // matching the production ROTOR_OUT_FLOOR. + (tq, QuadMixer::new().mix_rotor_out(f, tq, HOVER, 0.0)) } else { let tq = ctrl.moment(&r, omega, &level); (tq, QuadMixer::new().mix_thrust_floor(tq, HOVER, FLOOR)) @@ -206,7 +210,8 @@ fn run_motor_out_trial(t: MotorOutTrial) -> MotorOutOutcome { mix_ok = false; } for (i, &v) in motors_cmd.iter().enumerate() { - if i != f && !(FLOOR - 1e-6..=1.0 + 1e-6).contains(&v) { + // Rank-3 floor is 0 (the diagonal-opposite rotor rests at 0). + if i != f && !(-1e-6..=1.0 + 1e-6).contains(&v) { mix_ok = false; } } @@ -377,6 +382,236 @@ mod tests { } } +// ── FULL-LOOP motor-out campaign (v1.114) ──────────────────────────────────── +// +// The `run_motor_out_campaign` above disperses over an ATTITUDE-ONLY rigid-body +// sim (constant thrust, TRUE-state feedback, no estimator, no rotational drag) — +// the same fidelity ceiling as the point-test it mirrors. That ceiling HID the +// v1.114 flip: the real failure is a coupling (parasitic lean → descent → +// estimator divergence + thrust collapse → tumble) whose three ingredients — +// the altitude loop, the IEKF, and drag-bounded spin — none exist in that sim. +// Dispersion adds breadth, not fidelity; thousands of trials of a gap-blind +// model can't falsify a bug the model can't express. +// +// THIS campaign closes the gap: it disperses over the FULL production FlightCore +// (verified IEKF → geometric SE(3) → reduced-attitude + rank-3 allocation, +// driven by the real FDI on ESC RPM) flown on the analytic SimBackend with +// rotational drag + ground contact — the same harness as the +// `survives_single_rotor_failure_without_flipping` oracle, now under dispersion. + +use falcon_core::{FlightCore, Pathology, SimBackend}; + +const FL_DT: f32 = 0.004; +const FL_WARMUP: u32 = 3000; // steps to settle a level hover before the failure +const FL_RECOVER: u32 = 2000; // steps to fly out the recovery/descent + +/// One dispersed full-loop rotor-out trial, sampled inside a RECOVERABLE +/// envelope (a rotor-out quad with too little rotational drag spins up beyond +/// estimator observability — that is physics, not a controller failure, so it +/// is excluded rather than counted as a loss). +#[derive(Clone, Debug)] +struct FullLoopTrial { + failed_rotor: usize, + setpoint_alt: f32, // NED z setpoint (m, negative up) + rot_drag: f32, // quadratic rotational drag coefficient + gyro_white: f32, // gyro noise σ (rad/s) + gps_noise: f32, // GNSS noise σ (m) + seed: u32, // per-trial noise realisation +} + +fn sample_fullloop(rng: &mut SplitMix64, index: u32) -> FullLoopTrial { + FullLoopTrial { + failed_rotor: (rng.next_u64() % 4) as usize, + setpoint_alt: -rng.range(2.0, 4.0), // hold 2..4 m + rot_drag: rng.range(0.015, 0.035), // realistic drag-bounded spin + // Sensor noise within the FDI's SPECIFIED range (its detection gate is + // near-level/low-rate; heavier noise degrades detection — a separate, + // pre-existing FDI-robustness question, not part of the v1.114 + // recovery-allocation guard this campaign exists to provide). + gyro_white: rng.range(0.0, 0.006), + gps_noise: rng.range(0.0, 0.12), + seed: (index.wrapping_mul(2_654_435_761)) ^ 0x5151_5151, + } +} + +#[derive(Clone, Debug, Default)] +pub struct FullLoopReport { + pub trials: u32, + pub failures: u32, + pub worst_peak_tilt: f32, // worst tilt after the failure (rad) + pub worst_yaw_rate: f32, // worst |yaw rate| (rad/s) — the relinquished spin + pub least_descent: f32, // smallest net altitude lost (m) — must stay > 0 + pub worst_detect_latency_steps: u32, + pub failing: Vec<(u32, String)>, +} + +struct FullLoopOutcome { + isolated: Option, + detect_latency_steps: u32, + peak_tilt: f32, // worst tilt over the whole recovery (rad) + peak_yaw: f32, // worst |yaw rate| (rad/s) — the relinquished spin + net_descent: f32, // altitude LOST from the failure point (m) — must be > 0 + nan: bool, +} + +fn run_fullloop_trial(t: &FullLoopTrial) -> FullLoopOutcome { + let level = [[1.0f32, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + let mut backend = SimBackend::new(level, FL_DT).with_pathology(Pathology { + gyro_white: t.gyro_white, + gps_noise: t.gps_noise, + ..Default::default() + }); + backend.reseed(t.seed); + backend.rot_drag = t.rot_drag; + backend.ground_contact = true; + let mut core = FlightCore::new(0.5, 1.0 / FL_DT); + core.set_altitude(t.setpoint_alt); + + for _ in 0..FL_WARMUP { + core.step(&mut backend); + } + // If the hover never settled (tilt or NaN), this trial's IC is unusable — + // treat as NaN so the caller reports it rather than silently passing. + let hover_tilt = backend.tilt(); + if !hover_tilt.is_finite() || hover_tilt > 0.15 { + return FullLoopOutcome { + isolated: None, + detect_latency_steps: u32::MAX, + peak_tilt: hover_tilt, + peak_yaw: 0.0, + net_descent: 0.0, + nan: !hover_tilt.is_finite(), + }; + } + let alt_at_failure = -backend.pos[2]; + + backend.fail_rotor(t.failed_rotor); + let mut peak_tilt = 0.0f32; + let mut peak_yaw = 0.0f32; + let mut detect_latency = u32::MAX; + let mut nan = false; + for k in 0..FL_RECOVER { + core.step(&mut backend); + if detect_latency == u32::MAX && core.failed_motor().is_some() { + detect_latency = k; + } + let tilt = backend.tilt(); + if !tilt.is_finite() { + nan = true; + break; + } + peak_tilt = peak_tilt.max(tilt); + let yaw = backend.omega[2]; + peak_yaw = peak_yaw.max(if yaw < 0.0 { -yaw } else { yaw }); + } + // A rotor-out quad CANNOT hold altitude — the honest success criterion is a + // CONTROLLED DESCENT (net altitude lost, thrust axis kept upright), not + // "reached the ground within the window". + let net_descent = alt_at_failure - (-backend.pos[2]); + FullLoopOutcome { + isolated: core.failed_motor(), + detect_latency_steps: detect_latency, + peak_tilt, + peak_yaw, + net_descent, + nan, + } +} + +/// Run `n` dispersed FULL-LOOP motor-out recovery trials from `campaign_seed`. +pub fn run_fullloop_motor_out_campaign(n: u32, campaign_seed: u64) -> FullLoopReport { + let mut rep = FullLoopReport { + trials: n, + least_descent: f32::INFINITY, + ..Default::default() + }; + for i in 0..n { + let mut rng = trial_rng(campaign_seed, i); + let t = sample_fullloop(&mut rng, i); + let o = run_fullloop_trial(&t); + + rep.worst_peak_tilt = rep.worst_peak_tilt.max(o.peak_tilt); + rep.worst_yaw_rate = rep.worst_yaw_rate.max(o.peak_yaw); + rep.least_descent = rep.least_descent.min(o.net_descent); + if o.detect_latency_steps != u32::MAX { + rep.worst_detect_latency_steps = + rep.worst_detect_latency_steps.max(o.detect_latency_steps); + } + + let mut reason = String::new(); + if o.nan { + reason = "NaN in full-loop cascade".into(); + } else if o.isolated != Some(t.failed_rotor) { + reason = format!("FDI isolated {:?}, expected {}", o.isolated, t.failed_rotor); + } else if o.peak_tilt >= 1.0 { + reason = format!("tumbled: peak tilt {:.3} rad", o.peak_tilt); + } else if o.peak_yaw >= 12.0 { + reason = format!("yaw spin ran away: {:.1} rad/s", o.peak_yaw); + } + // NOTE: altitude after the loss is deliberately NOT gated here. Two + // rotors at max thrust equal the hover weight on this plant (k_thrust is + // sized for 4×0.5), so a rotor-out quad is near neutrally-buoyant; + // bringing it DOWN is the FlightSupervisor's job (it commands LAND on a + // motor failure — covered by falcon-core `motor_failure_commands_land`). + // This campaign guards the ATTITUDE-domain invariant v1.114 fixed (no + // parasitic-moment flip); `least_descent` is REPORTED, not asserted. + if !reason.is_empty() { + rep.failures += 1; + if rep.failing.len() < 20 { + rep.failing.push((i, format!("{t:?}: {reason}"))); + } + } + } + rep +} + +#[cfg(test)] +mod fullloop_tests { + use super::*; + + // Reproducible seed (bump to resample). The full-loop trial is ~50× the + // cost of the attitude-only one (a 15-state IEKF per step), so the deck is + // smaller — 300 clean trials still give ≥99% success at 95% confidence + // within the recoverable envelope, and the coverage is FIDELITY not just + // breadth (real estimator + altitude loop + drag-bounded spin). + const FL_SEED: u64 = 0x00FA_11C0_DE00_1114; + const FL_TRIALS: u32 = 200; + + #[test] + fn fullloop_motor_out_monte_carlo_campaign() { + 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 + ); + + // Primary safety assertion: not one trial in the envelope fails — the + // production loop (IEKF + altitude + FDI + rank-3 allocation) recovers a + // single-rotor loss across the whole dispersion. This is the FIDELITY + // guard the idealized campaign could not provide: a parasitic-moment + // regression would drive `worst_peak_tilt` toward π and trip here. + assert_eq!( + rep.failures, 0, + "full-loop motor-out recovery failed in {}/{} dispersed trials; first failures: {:#?}", + rep.failures, rep.trials, rep.failing + ); + // Physical safety bounds: never tumbles (< 1.0 rad), spin stays bounded + // (< 12 rad/s), and every trial makes a CONTROLLED DESCENT (> 0.3 m lost + // — a 3-rotor quad must come down, not fly off). + assert!( + rep.worst_peak_tilt < 1.0, + "worst tilt {:.3} rad exceeded the 1.0 tumble bound", + rep.worst_peak_tilt + ); + assert!( + rep.worst_yaw_rate < 12.0, + "worst yaw spin {:.1} rad/s exceeded the 12 rad/s bound", + rep.worst_yaw_rate + ); + } +} + // ── Attitude-stabilisation campaign (random tilt, no fault) ────────────────── /// A single dispersed attitude-recovery trial: the aircraft starts tilted and @@ -1320,7 +1555,11 @@ fn run_motor_out_disp_trial(t: MotorOutDispTrial, rng: &mut SplitMix64) -> Motor for step in 0..4000u32 { let (_torque, motors_cmd) = if let Some(f) = isolated { let tq = ctrl.moment_reduced(&r, omega, b3_d); - (tq, QuadMixer::new().mix_rotor_out(f, tq, HOVER, FLOOR)) + // Rank-3 allocation (v1.114): the diagonal-opposite rotor must be + // free to reach 0 (a positive floor pins it up, reinjecting the + // parasitic moment and starving roll/pitch authority) — so pass 0, + // matching the production ROTOR_OUT_FLOOR. + (tq, QuadMixer::new().mix_rotor_out(f, tq, HOVER, 0.0)) } else { let tq = ctrl.moment(&r, omega, &level); (tq, QuadMixer::new().mix_thrust_floor(tq, HOVER, FLOOR)) @@ -1330,7 +1569,8 @@ fn run_motor_out_disp_trial(t: MotorOutDispTrial, rng: &mut SplitMix64) -> Motor mix_ok = false; } for (i, &v) in motors_cmd.iter().enumerate() { - if i != f && !(FLOOR - 1e-6..=1.0 + 1e-6).contains(&v) { + // Rank-3 floor is 0 (the diagonal-opposite rotor rests at 0). + if i != f && !(-1e-6..=1.0 + 1e-6).contains(&v) { mix_ok = false; } } diff --git a/examples/falcon-sitl-gz/src/flightcore.rs b/examples/falcon-sitl-gz/src/flightcore.rs new file mode 100644 index 0000000..5617610 --- /dev/null +++ b/examples/falcon-sitl-gz/src/flightcore.rs @@ -0,0 +1,163 @@ +//! `FlightCore`-in-the-loop — flying the PRODUCTION core through the SITL plant. +//! +//! Every other `run_*` scenario in this bench re-implements the flight cascade +//! (estimator → attitude → rate → mixer) by hand against the [`Physics`] trait. +//! That means the sim exercises a *parallel* controller, not the one that +//! ships. The production controller — [`falcon_core::FlightCore`], the verified +//! IEKF → geometric-SE(3) → ADRC → mixer cascade with the single-rotor-out FDI +//! and degraded-allocator recovery — lives behind its OWN hardware seam, +//! [`falcon_core::FlightBackend`], and was flown only against falcon-core's +//! in-crate `SimBackend`. +//! +//! This module closes that gap with an **adapter**, not a rewrite: [`SitlBackend`] +//! implements `FlightBackend` on top of a `&mut dyn Physics` plant, so the same +//! byte-for-byte production core flies the SITL plant (mock today, real Gazebo +//! under `--features gazebo`). What the sim validates is now what flies. +//! +//! ## Seam mapping +//! +//! `FlightBackend` is a *sensor/actuator port* (the core pulls sensors, pushes +//! motors); `Physics` is a *plant* (push motor PWMs + `step(dt)`, then +//! `measure()`). The adapter reconciles them by advancing the plant inside +//! `write_motors` — the tail of one `FlightCore::step` — so each control tick is +//! exactly one physics step, in causal order: sense state(t) → decide → +//! actuate-and-advance to state(t+dt). + +use crate::physics::Physics; +use falcon_core::{FlightBackend, ImuSample as CoreImu}; +use relay_iekf::Vec3; + +/// Adapts a SITL [`Physics`] plant to falcon-core's [`FlightBackend`] seam. +/// +/// Owns the plant for the whole run. `read_imu` samples the plant and caches +/// the true position so the same-tick `read_position` returns a coherent fix; +/// `write_motors` is the one place the plant is stepped, once per control tick. +pub struct SitlBackend<'a> { + plant: &'a mut dyn Physics, + dt: f32, + /// IMU measurement noise σ passed to `Physics::measure` (0 for the mock). + imu_noise: f32, + /// True position cached by the most recent `read_imu`, surfaced to + /// `read_position` (and to the caller for evidence logging). + last_pos: [f32; 3], + /// GNSS is slower than the control loop; a position fix is offered only + /// every `gnss_div` ticks (1 = every tick). + gnss_div: u32, + tick: u32, + /// The last per-rotor command the core wrote (for evidence logging). + last_motors: [f32; 4], + /// The last IMU sample handed to the core (for evidence logging). + last_imu: CoreImu, +} + +impl<'a> SitlBackend<'a> { + /// New adapter. `dt` is the control period (s); `imu_noise` the measurement + /// σ; `gnss_div` the loop-ticks-per-GNSS-fix divisor. + pub fn new(plant: &'a mut dyn Physics, dt: f32, imu_noise: f32, gnss_div: u32) -> Self { + SitlBackend { + plant, + dt, + imu_noise, + last_pos: [0.0; 3], + gnss_div: gnss_div.max(1), + tick: 0, + last_motors: [0.0; 4], + last_imu: CoreImu { + accel: [0.0; 3], + gyro: [0.0; 3], + }, + } + } + + /// The true NED position sampled by the last `read_imu` (for evidence). + pub fn last_true_pos(&self) -> [f32; 3] { + self.last_pos + } + + /// The per-rotor command from the most recent control tick (for evidence). + pub fn last_motors(&self) -> [f32; 4] { + self.last_motors + } + + /// The IMU sample from the most recent control tick (for evidence). + pub fn last_imu(&self) -> ([f32; 3], [f32; 3]) { + (self.last_imu.accel, self.last_imu.gyro) + } + + /// Plant diagnostic counters `(imu_recv, navsat_recv, motor_send)`. + pub fn counters(&self) -> Option<(u64, u64, u64)> { + self.plant.counters() + } + + /// Stage a single-rotor failure on the underlying plant (the scenario calls + /// this at the injection time; the adapter owns the plant, so the fault must + /// route through here rather than a separate borrow). + pub fn fail_rotor(&mut self, rotor: usize) { + self.plant.fail_rotor(rotor); + } +} + +impl FlightBackend for SitlBackend<'_> { + fn read_imu(&mut self) -> CoreImu { + // `measure` returns (IMU, true position); cache the position so the + // same tick's `read_position` is coherent with this IMU sample. + let (s, p) = self.plant.measure(self.imu_noise); + self.last_pos = p; + let sample = CoreImu { + accel: s.accel_body, + gyro: s.gyro_body, + }; + self.last_imu = sample; + sample + } + + fn read_position(&mut self) -> Option { + // Offer a fix only on GNSS ticks; between fixes the core dead-reckons + // on the IMU (exactly as it must against a real 5 Hz receiver). + self.tick = self.tick.wrapping_add(1); + if self.tick % self.gnss_div == 0 { + Some(self.last_pos) + } else { + None + } + } + + fn read_mag(&mut self) -> Option { + // MockPhysics has no magnetometer (returns None); the real gz bridge + // supplies one. Forward whatever the plant exposes. + self.plant.mag_body_ned() + } + + fn read_heading(&mut self) -> Option { + // The clean absolute yaw (NED). MockPhysics has none; the gz bridge + // resolves it from the Pose_V truth orientation — the "compass" that + // makes yaw observable (the raw gz-mag frame is unvalidated, so this is + // the heading reference the hover uses). + self.plant.heading_ned() + } + + fn read_motor_rpm(&mut self) -> Option<[i32; 4]> { + // Carry the plant's per-rotor ESC telemetry to the core's rotor-out FDI. + // Without this the FDI is inert in SITL (a rotor loss goes undetected), + // so this is the plumbing a recordable rotor-out flight depends on. + self.plant.motor_rpm() + } + + fn write_motors(&mut self, motors: &[f32]) { + // The tail of one control tick: actuate and advance the plant by dt. + // `mix` yields 4 quad motors; pad defensively in case a backend ever + // hands us fewer. + let m = [ + motors.first().copied().unwrap_or(0.0), + motors.get(1).copied().unwrap_or(0.0), + motors.get(2).copied().unwrap_or(0.0), + motors.get(3).copied().unwrap_or(0.0), + ]; + self.last_motors = m; + self.plant.step(m, self.dt); + } + + fn dt(&self) -> f32 { + self.dt + } +} diff --git a/examples/falcon-sitl-gz/src/main.rs b/examples/falcon-sitl-gz/src/main.rs index 2f9ade6..872b265 100644 --- a/examples/falcon-sitl-gz/src/main.rs +++ b/examples/falcon-sitl-gz/src/main.rs @@ -18,9 +18,12 @@ //! `--scenario=open-loop-climb`. mod campaign; +mod flightcore; mod pace; mod physics; +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}; @@ -184,10 +187,17 @@ fn run_scenario( "frame-roll" => run_frame_check(physics, 0, duration_s), "frame-pitch" => run_frame_check(physics, 1, duration_s), "frame-yaw" => run_frame_check(physics, 2, duration_s), + "yaw-probe" => run_yaw_probe(physics, duration_s), "arming" => run_arming_check(physics, duration_s, true), "arming-ungated" => run_arming_check(physics, duration_s, false), "geo-hover" => run_geo_hover(physics, duration_s, evidence), "mission" => run_mission(physics, duration_s, evidence), + "flightcore" => run_flightcore(physics, 2.0, duration_s, None, evidence), + "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) + } other => { eprintln!( " scenario {other} not yet wired; falling back to closed-loop hover", @@ -259,6 +269,70 @@ fn run_frame_check(physics: &mut dyn Physics, axis: usize, duration_s: f32) -> b agrees } +/// v1.113 — YAW-SIGN ORACLE. Commands a constant OPEN-LOOP +yaw torque (no +/// attitude control) and runs the IEKF alongside, so all four links of the yaw +/// chain can be compared in one run: +/// commanded +yaw torque → (a) gz TRUTH heading rate [mixer→gz mapping] +/// → (b) sensed gyro_z [gyro enu→ned] +/// → (c) IEKF est-yaw rate [heading→estimator] +/// For a stable closed loop all three must share the sign of the command. The +/// closed-loop spin with a CORRECT gyro sign (frame-yaw AGREES) points at (c): +/// the est-yaw sign disagreeing with the physical/gyro convention makes the +/// geometric controller drive yaw the wrong way. This oracle prints the signs. +fn run_yaw_probe(physics: &mut dyn Physics, duration_s: f32) -> bool { + let mut mixer = QuadMixer::new(); + let mut iekf = Iekf::level(); + let dt = 0.004_f32; + let n = (duration_s / dt) as u32; + let tick_period = Duration::from_secs_f32(dt); + let pace_real_time = physics.counters().is_some(); + let hover = 0.72_f32; + let yaw_cmd = 0.15_f32; // constant +yaw torque + + let yaw_of = |q: [f32; 4]| -> f32 { + 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 mut h_prev: Option = None; + let mut ey_prev: Option = None; + + for step in 0..n { + let tick_start = Instant::now(); + 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.update_gravity(imu.accel_body, 0.5); + iekf.update_position(pos, 0.01); + if let Some(h) = physics.heading_ned() { + iekf.update_yaw(h, 0.1); + } + let motors = mixer.mix([0.0, 0.0, yaw_cmd], hover); + physics.step(motors, dt); + + if step % 125 == 0 { + let truth_h = physics.heading_ned().unwrap_or(f32::NAN); + let est_yaw = yaw_of(iekf.state().q); + let dh = h_prev.map(|p| truth_h - p).unwrap_or(0.0); + let dey = ey_prev.map(|p| est_yaw - p).unwrap_or(0.0); + eprintln!( + "t={:.2} cmd_yaw=+{:.2} truth_head={:.2} (Δ{:+.2}) est_yaw={:.2} (Δ{:+.2}) gyro_z={:+.2}", + t, yaw_cmd, truth_h, dh, est_yaw, dey, imu.gyro_body[2], + ); + h_prev = Some(truth_h); + ey_prev = Some(est_yaw); + } + if pace_real_time { + let used = tick_start.elapsed(); + 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=+"); + true +} + /// v0.19.9 — arming-sequencer ORACLE + position-hold DIAGNOSTIC. /// /// Runs the verified `ArmingSequencer` ahead of the full cascade against @@ -1223,6 +1297,189 @@ fn run_alt_only_hover( final_dist < 0.5 && rms_steady < 1.0 } +/// 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) +/// through the SITL plant via the [`SitlBackend`] adapter — instead of the +/// hand-rolled cascade every other scenario re-implements. The sim now exercises +/// the code that SHIPS, not a parallel copy of it. +/// +/// Commands an altitude hold at `target_alt_m` — the closed loop the mock plant +/// can demonstrate (it responds to collective thrust; differential-motor torque +/// is approximated as zero, so horizontal translation and rotor-out recovery +/// need the full-physics `--features gazebo` backend, the next slice). The same +/// `FlightCore::step` drives both plants unchanged. +/// +/// PASS = the true altitude settles within 0.5 m of the target with < 1.0 m +/// steady RMS — the same bar as [`run_alt_only_hover`], but met by the *shipping* +/// core rather than a bench re-implementation. +fn run_flightcore( + physics: &mut dyn Physics, + target_alt_m: f32, + duration_s: f32, + fail: Option<(usize, f32)>, + mut evidence: Option<&mut EvidenceSink>, +) -> bool { + let dt = 0.004_f32; // 250 Hz — the falcon-quad control-loop rate. + let n = (duration_s / dt) as u32; + let fail_step = fail.map(|(r, ts)| (r, (ts / dt) as u32)); + let tick_period = Duration::from_secs_f32(dt); + // Capture these before the adapter borrows the plant for the whole run. + let name = physics.name(); + let pace_real_time = physics.counters().is_some(); + // Mock plant hovers at ~0.49 (THRUST_SCALE 20 m/s² vs g 9.81); the real gz + // falcon-quad hovers at ~0.57 (ω_hover≈757 of maxRotVel 1000; pwm=(757/1000)² + // via the √pwm thrust map). A too-high hover feedforward leaves a steady-state + // altitude offset the P-loop can't null, so the altitude INTEGRAL trims it. + let hover_thrust = if name == "mock" { 0.49 } else { 0.585 }; + + let mut core = FlightCore::new(hover_thrust, 1.0 / dt); + core.set_altitude(-target_alt_m); // NED z: negative = up. + if name != "mock" { + // The default pos_var (0.01 = 1 cm²) over-trusts the gz NavSat: on a long + // static hover the position covariance COLLAPSES, so the NIS outlier gate + // then rejects the (correct) fixes and the estimate goes deaf → the true + // altitude drifts unbounded. A realistic metre-class variance keeps P + // alive and the fixes accepted (diagnosed v1.113 — est_z froze at ~17 s). + core.set_pos_var(0.25); + // Extra δv/δp process noise (variance/s) so a long static hover can't + // starve the filter: velocity ~5× the base q_accel keeps the δv↔δp + // correlation alive → GNSS keeps correcting velocity → the estimate + // tracks the truth indefinitely (a diagonal clamp let the velocity + // free-run and the estimate diverged; additive Q is the fix). + core.set_process_floor(0.30, 0.05); + // Stiffen the altitude loop for the gz plant: the gentle default (0.05, + // 0.30) climbs asymptotically to below the target then slowly oscillates + // back down (underdamped/too-soft). A firmer kp holds altitude; kd damps. + core.set_altitude_gains(0.15, 1.00); + // Small integral to null the residual settle offset (hover feedforward + // isn't exact + run-to-run variation). Safe now: the loop is well-damped + // and the estimate tracks, so it trims to 2 m without the windup that hit + // when the estimator was still diverging. + core.set_altitude_integral_gain(0.03); + } + // No altitude integral: with hover_thrust matched to the real gz hover (0.57) + // the P-D loop settles at the target with ~0 steady-state error, and the + // integral wound up into a slow runaway on the gz plant's lagged thrust. + + let mut peak_dist_err = 0.0_f32; + let mut min_dist_seen = f32::INFINITY; + let mut sum_sq_steady = 0.0_f32; + let mut steady_count = 0usize; + let steady_start_t = (duration_s - 5.0).max(0.0); + let mut last_true = [0.0_f32; 3]; + + let started_at = Instant::now(); + // The adapter owns the plant for the whole run (its 5 Hz GNSS-divisor tick + // counter must persist across steps — reconstructing it per tick would + // suppress every fix). GNSS divisor 50 @ 250 Hz = 5 Hz fixes. + { + let mut backend = SitlBackend::new(physics, dt, 0.0, 50); + for step in 0..n { + let tick_start = Instant::now(); + let t = step as f32 * dt; + + // Stage the rotor failure at its scheduled step (the fault a real + // airframe suffers mid-flight). + if let Some((rotor, fs)) = fail_step { + if step == fs { + backend.fail_rotor(rotor); + } + } + + core.step(&mut backend); // ← one PRODUCTION control tick + plant step + last_true = backend.last_true_pos(); + + // FC_DEBUG — per-tick estimator trace to diagnose the gz divergence. + if std::env::var_os("FC_DEBUG").is_some() && step % 25 == 0 { + let e = core.state(); + let (a, g) = backend.last_imu(); + let q = e.q; // [w,x,y,z] + let yaw = 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 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; + eprintln!( + "t={:.2} true_z={:.2} tilt={:.0}deg gyro=[{:+.1},{:+.1},{:+.1}] failed={:?} mot=[{:.2},{:.2},{:.2},{:.2}]", + t, last_true[2], tilt_deg, g[0], g[1], g[2], core.failed_motor(), + m[0], m[1], m[2], m[3], + ); + } + + let alt_err = -target_alt_m - last_true[2]; // NED z error + let dist = alt_err.abs(); + 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 { + let (accel, gyro) = backend.last_imu(); + e.write_tick(step, t, last_true, accel, gyro, backend.last_motors(), None); + } + if pace_real_time { + let used = tick_start.elapsed(); + if used < tick_period { + std::thread::sleep(tick_period - used); + } + } + } + } // backend dropped — the plant borrow is released for the verdict below. + + let wall = started_at.elapsed(); + let final_dist = (-target_alt_m - last_true[2]).abs(); + let rms_steady = if steady_count > 0 { + (sum_sq_steady / steady_count as f32).sqrt() + } else { + f32::NAN + }; + let counters = physics.counters(); + let est = core.state(); + // 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" }; + 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(), + ); + if let Some((imu_recv, navsat_recv, motor_send)) = counters { + println!( + " counters: imu_recv={imu_recv} navsat_recv={navsat_recv} motor_send={motor_send}", + ); + } + 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); + } + match fail { + // Rotor-out: PASS = the production FDI ISOLATED the CORRECT (failed) + // rotor — the full chain fired across the SITL seam (plant RPM → adapter + // read_motor_rpm → FlightCore's commanded-vs-achieved residual → CUSUM → + // latch) — and the sim stayed finite (no divergence). The mock has NO + // differential-motor torque, so it CANNOT demonstrate the 3D + // tilt-recovery or a post-failure altitude hold; asserting those on it + // would be an artifact, not a proof. The full recovery is the gz-backed + // recordable flight (real per-rotor torque). Honest scope: this gates + // the FDI TELEMETRY PLUMBING the recovery depends on. + Some((rotor, _)) => { + let finite = + last_true.iter().all(|v| v.is_finite()) && est.p.iter().all(|v| v.is_finite()); + isolated == Some(rotor) && finite + } + // Nominal hover: the tight altitude bar. + None => final_dist < 0.5 && rms_steady < 1.0, + } +} + /// v0.19.4 — closed-loop hover. Mirrors `falcon-sitl-hover`'s /// `run_mission` cascade pattern, but inputs come from the bridge /// (gz IMU + NavSat) and outputs feed the bridge's motor publish. @@ -1782,6 +2039,57 @@ mod tests { } } + /// v1.113 — the PRODUCTION `falcon_core::FlightCore` (the verified + /// IEKF → geo → ADRC → mixer cascade that ships) holds altitude when flown + /// through the SITL plant via the `SitlBackend` adapter. This is the + /// orphan-elimination gate: the sim exercises the shipping core, not a + /// bench re-implementation of it. Same PASS bar as the hand-rolled + /// altitude scenarios (settles < 0.5 m, steady RMS < 1.0 m). + #[test] + 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"); + // 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]); + assert!(p.v_ned[i].is_finite(), "v_ned[{i}] = {}", p.v_ned[i]); + } + // Settled near the 2 m (NED z = −2) target, not drifted off. + assert!( + (p.p_ned[2] + 2.0).abs() < 0.5, + "final NED z = {} (want ≈ −2)", + p.p_ned[2] + ); + } + + /// v1.113 — the production FlightCore's single-rotor-out FDI fires THROUGH + /// the SITL seam. A rotor is failed mid-flight; the mock reports its dead + /// rotor at 0 RPM, the `SitlBackend` adapter carries that ESC telemetry to + /// the core's `read_motor_rpm`, and the core's CUSUM must ISOLATE exactly + /// the failed rotor. This gates the telemetry plumbing the rotor-out + /// recovery depends on — before this wiring the FDI was inert in SITL. The + /// mock has no differential-motor torque, so the 3D tilt-recovery itself is + /// the gz-backed recordable flight, not this test. + #[test] + fn production_flightcore_fdi_isolates_failed_rotor_through_sitl_seam() { + let mut p = MockPhysics::at_rest(); + // Rotor 2 fails at t = 12 s of a 28 s flight. + let ok = run_flightcore(&mut p, 2.0, 28.0, Some((2, 12.0)), None); + assert!( + ok, + "production FDI failed to isolate the failed rotor through the SITL seam" + ); + // The plant actually staged the failure and stayed finite. + assert_eq!(p.failed_rotor, Some(2), "plant should have rotor 2 failed"); + for i in 0..3 { + assert!(p.p_ned[i].is_finite(), "p_ned[{i}] = {}", p.p_ned[i]); + } + // The dead rotor reports 0 RPM (the residual source). + let rpm = p.motor_rpm().expect("mock reports RPM"); + assert_eq!(rpm[2], 0, "failed rotor must report 0 RPM, got {}", rpm[2]); + } + /// v0.19.0 — when --evidence-dir is set, the runner produces two /// files: a harness log and a per-tick CSV. Smoke-test that both /// files materialise + carry the right header/footer. @@ -1847,7 +2155,9 @@ mod tests { // then reduced-attitude + the reconfigured allocator). let (torque, motors_cmd) = if let Some(f) = isolated { let tq = ctrl.moment_reduced(&r, omega, b3_d); - (tq, QuadMixer::new().mix_rotor_out(f, tq, hover, floor)) + // Rank-3 allocation (v1.114): floor 0 so the diagonal-opposite + // rotor can rest at 0 (matching production ROTOR_OUT_FLOOR). + (tq, QuadMixer::new().mix_rotor_out(f, tq, hover, 0.0)) } else { let tq = ctrl.moment(&r, omega, &level); (tq, QuadMixer::new().mix_thrust_floor(tq, hover, floor)) @@ -1861,7 +2171,8 @@ mod tests { assert_eq!(motors_cmd[f], 0.0, "failed rotor must be commanded 0"); for (i, &v) in motors_cmd.iter().enumerate() { if i != f { - assert!(v >= floor - 1e-6 && v <= 1.0 + 1e-6, "healthy motor bound: {v}"); + // Rank-3 floor is 0 (diagonal-opposite rotor rests at 0). + assert!(v >= -1e-6 && v <= 1.0 + 1e-6, "healthy motor bound: {v}"); } } } diff --git a/examples/falcon-sitl-gz/src/physics.rs b/examples/falcon-sitl-gz/src/physics.rs index 39a04bf..850c5ff 100644 --- a/examples/falcon-sitl-gz/src/physics.rs +++ b/examples/falcon-sitl-gz/src/physics.rs @@ -68,6 +68,22 @@ pub trait Physics { /// 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 } + + /// 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 + /// production `FlightCore` compares against the commanded throttle to form + /// the single-rotor-out FDI residual (v1.103): a dead rotor reads ~0 RPM + /// while the controller still commands it → the residual fires the CUSUM. + /// 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 } + + /// 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 + /// uses to stage the fault a real airframe would suffer. Default no-op (a + /// plant that cannot model a rotor loss simply ignores it). + fn fail_rotor(&mut self, _rotor: usize) {} } /// In-process reference impl — same toy integrator as @@ -82,6 +98,20 @@ pub struct MockPhysics { pub p_ned: [f32; 3], /// Velocity in NED frame (m/s). pub v_ned: [f32; 3], + /// Specific force in the NED frame from the last `step` — the non-gravity + /// part of the kinematic acceleration (`thrust − drag`). An accelerometer + /// measures specific force (`a_kinematic − g_gravity`), so `measure` rotates + /// this into the body frame. At hover it is `[0,0,−g]` (the reaction to + /// gravity), matching the pre-v1.113 constant; during a climb it carries the + /// thrust reaction the full IEKF needs to keep vertical velocity observable. + pub spec_force_ned: [f32; 3], + /// A single failed rotor (its thrust + RPM forced to zero), or `None`. + /// Set by `fail_rotor`; defaults off so every existing scenario is + /// unaffected. + pub failed_rotor: Option, + /// The ACHIEVED per-rotor throttle from the last `step` (post-failure) — + /// the basis for the reported RPM the production FDI consumes. + pub achieved_motors: [f32; 4], /// xorshift state for the IMU noise generator. pub rng: u64, } @@ -93,6 +123,9 @@ impl MockPhysics { q: [1.0, 0.0, 0.0, 0.0], p_ned: [0.0; 3], v_ned: [0.0; 3], + spec_force_ned: [0.0, 0.0, -GRAVITY], // at rest: reaction to gravity + failed_rotor: None, + achieved_motors: [0.0; 4], rng: 0xCAFE_BABE_DEAD_BEEF, } } @@ -122,8 +155,19 @@ impl Physics for MockPhysics { // full mixer-to-physics torque mapping lives in falcon-sitl- // hover; this stub keeps the cascade running so the scaffold // ends with a complete loop). + // A failed rotor delivers no thrust regardless of command; record the + // ACHIEVED per-rotor throttle so the reported RPM reflects the loss (the + // production FDI keys off commanded-vs-achieved). With no failure this + // is the command verbatim, so nominal flight is byte-for-byte unchanged. + let mut achieved = motor_pwm; + if let Some(f) = self.failed_rotor { + if f < 4 { + achieved[f] = 0.0; + } + } + self.achieved_motors = achieved; let thrust_normalised = - ((motor_pwm[0] + motor_pwm[1] + motor_pwm[2] + motor_pwm[3]) / 4.0).clamp(0.0, 1.0); + ((achieved[0] + achieved[1] + achieved[2] + achieved[3]) / 4.0).clamp(0.0, 1.0); // Rotational dynamics under (zero) torque + friction. for i in 0..3 { @@ -157,6 +201,10 @@ impl Physics for MockPhysics { let g = if i == 2 { GRAVITY } else { 0.0 }; let drag = DRAG_COEFFICIENT * self.v_ned[i]; let a = thrust_ned[i] + g - drag; + // Specific force = kinematic accel − gravity = thrust − drag. This + // is what an accelerometer reads (the full IEKF integrates it, so a + // constant-gravity fake would starve its vertical-velocity estimate). + self.spec_force_ned[i] = thrust_ned[i] - drag; self.v_ned[i] += a * dt; self.p_ned[i] += self.v_ned[i] * dt; } @@ -168,14 +216,19 @@ impl Physics for MockPhysics { self.omega[1] + noise_std * self.next_unit_normal(), self.omega[2] + noise_std * self.next_unit_normal(), ]; - // Body-frame accel: gravity rotated into body via q. Simplified - // — at small attitude angles the accel reads [0, 0, -g] body - // plus thrust contribution; we approximate as the body-frame - // thrust the controller would feel for closed-loop testing. + // Body-frame accelerometer: the NED specific force (thrust − drag, + // computed in `step`) rotated into the body frame via conj(q). At + // hover/level this reduces to [0, 0, −g] (the reaction to gravity), so + // the near-level scenarios are unchanged; under a climb it carries the + // thrust reaction the full IEKF integrates for vertical velocity. + let f = self.spec_force_ned; + let qv = [0.0, f[0], f[1], f[2]]; + let qc = [self.q[0], -self.q[1], -self.q[2], -self.q[3]]; + let fb = quat_mul(qc, quat_mul(qv, self.q)); // v_body = conj(q)·v_ned·q let accel_body = [ - noise_std * self.next_unit_normal(), - noise_std * self.next_unit_normal(), - -GRAVITY + noise_std * self.next_unit_normal(), + fb[1] + noise_std * self.next_unit_normal(), + fb[2] + noise_std * self.next_unit_normal(), + fb[3] + noise_std * self.next_unit_normal(), ]; let sample = ImuSample { time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, @@ -184,6 +237,24 @@ impl Physics for MockPhysics { }; (sample, self.p_ned) } + + fn motor_rpm(&self) -> Option<[i32; 4]> { + // rpm = achieved throttle × full-scale RPM, matching falcon-core's + // ESC_RPM_FULL (8000) so the FDI residual (rpm/8000 vs command) is + // scaled correctly. The failed rotor reads 0 → the residual fires. + const ESC_RPM_FULL: f32 = 8000.0; + let a = &self.achieved_motors; + Some([ + (a[0] * ESC_RPM_FULL) as i32, + (a[1] * ESC_RPM_FULL) as i32, + (a[2] * ESC_RPM_FULL) as i32, + (a[3] * ESC_RPM_FULL) as i32, + ]) + } + + fn fail_rotor(&mut self, rotor: usize) { + self.failed_rotor = Some(rotor); + } } // ─── GazeboPhysics ──────────────────────────────────────────────────── @@ -255,76 +326,20 @@ impl Physics for GazeboPhysics { #[cfg(feature = "gazebo")] mod gz_real { use super::{Physics, ImuSample}; - use std::sync::{Arc, Mutex}; - use std::sync::atomic::{AtomicU64, Ordering}; - use tokio::sync::mpsc; - - /// `gz.msgs.Actuators` — locally defined because gz-transport-rs - /// 0.1.0 doesn't ship `actuators.proto`. Wire-compatible with - /// Harmonic's MulticopterMotorModel plugin, which subscribes - /// this message type on the model-level `//cmd_vel` topic - /// (each plugin reads its `` index from `velocity`). - /// - /// Proto definition (gz/msgs/actuators.proto): - /// ```proto - /// syntax = "proto3"; - /// package gz.msgs; - /// import "gz/msgs/header.proto"; - /// message Actuators { - /// Header header = 1; - /// repeated double position = 2; - /// repeated double velocity = 3; - /// repeated double normalized = 4; - /// } - /// ``` - /// - /// Discovered the per-rotor `gz.msgs.Double` publish *didn't* drive - /// the rotors on 2026-05-26 — the first gz-sim bench evidence - /// showed `motor_send=1000` but `climb=0`. See - /// `bench-evidence/gz-sim/2026-05-26-first-bench-findings.md`. - #[derive(Clone, PartialEq, prost::Message)] - pub struct Actuators { - #[prost(message, optional, tag = "1")] - pub header: ::core::option::Option, - #[prost(double, repeated, packed = "true", tag = "2")] - pub position: ::prost::alloc::vec::Vec, - #[prost(double, repeated, packed = "true", tag = "3")] - pub velocity: ::prost::alloc::vec::Vec, - #[prost(double, repeated, packed = "true", tag = "4")] - pub normalized: ::prost::alloc::vec::Vec, - } - - /// v0.19.7 — minimal `gz.msgs.Odometry` (twist only) for the true body - /// velocity. (The OdometryPublisher leaves `pose.orientation` unset = - /// zero, so heading comes from the `/model/.../pose` Pose_V instead — - /// see `PoseV`.) prost skips the unparsed `header`(1)/`pose`(2) tags. - #[derive(Clone, PartialEq, prost::Message)] - pub struct Odometry { - #[prost(message, optional, tag = "3")] - pub twist: ::core::option::Option, - } - - /// v0.22 — `gz.msgs.Pose_V` (repeated Pose) on `/model//pose`. The - /// model root pose carries the TRUE orientation (the OdometryPublisher - /// does not), our heading reference / "compass" — yaw is unobservable - /// from IMU+GPS alone. - #[derive(Clone, PartialEq, prost::Message)] - pub struct PoseV { - // gz.msgs.Pose_V = { Header header = 1; repeated Pose pose = 2; } - #[prost(message, repeated, tag = "2")] - pub pose: ::std::vec::Vec, - } - - /// v0.22 — `gz.msgs.Magnetometer` on the mag_sensor topic. The REAL, - /// non-truth heading reference: the Earth field (world `magnetic_field`) - /// read in body frame. `{ Header header=1; Vector3d field_tesla=2; }` — - /// prost skips the unparsed header. Replaces the gz-truth `PoseV` - /// heading cheat once the body-frame conversion is validated. - #[derive(Clone, PartialEq, prost::Message)] - pub struct Magnetometer { - #[prost(message, optional, tag = "2")] - pub field_tesla: ::core::option::Option, - } + use std::sync::Mutex; + use std::sync::atomic::{AtomicI32, AtomicU64, Ordering}; + use crossbeam_channel::Receiver; + use gz_msgs::actuators::Actuators; + use gz_msgs::imu::IMU; + use gz_msgs::magnetometer::Magnetometer; + use gz_msgs::navsat::NavSat; + use gz_msgs::odometry::Odometry; + use gz_msgs::pose_v::Pose_V; + use gz_transport::{Node, Publisher}; + + // The gz.msgs types (Actuators / IMU / NavSat / Pose_V / Magnetometer / + // Odometry) now come from the `gz-msgs` crate (protobuf), imported above — + // the pure-Rust bridge's hand-written prost definitions are gone. /// gz-sim uses ENU body frame (X forward, Y left, Z up); /// falcon uses NED body frame (X forward, Y right, Z down). @@ -382,355 +397,266 @@ mod gz_real { pub model_name: String, /// Launch-site anchor for the NavSat → NED projection. pub home: Home, - /// Latest IMU sample observed on the imu_sensor topic. - /// `None` until first frame arrives. - latest_imu: Arc>>, - /// Latest NED position (m). v0.18.0: stub `[0,0,0]`. v0.18.1: - /// populated from `gz.msgs.NavSat` on the navsat topic, via - /// `Home::project_to_ned_m`. - latest_position_ned_m: Arc>, - /// v0.19.7 — latest TRUE NED body velocity (m/s) from the - /// OdometryPublisher twist. Deterministic, unlike finite-diff - /// NavSat. `None` until the first odometry frame. - latest_velocity_ned: Arc>>, - /// v0.22 — latest TRUE NED heading (yaw, rad) from the - /// OdometryPublisher pose orientation. The compass reference. - latest_heading_ned: Arc>>, - /// v0.22 — latest body-frame magnetometer reading (Tesla, NED body - /// frame). The REAL (non-truth) heading source from the SDF - /// magnetometer sensor; `None` until the first frame. - latest_mag_body_ned: Arc>>, - /// v0.19.2 — single mpsc carrying all 4 motor velocities. - /// One receiver task owns the gz-transport Publisher and - /// emits a single `gz.msgs.Actuators` message per send. - /// Replaced the v0.18 per-rotor 4× fanout after the - /// 2026-05-26 bench evidence showed plugins subscribe - /// `gz.msgs.Actuators` on a shared `//cmd_vel`, not - /// per-rotor Double topics. - rotors_tx: mpsc::UnboundedSender<[f32; 4]>, - /// v0.19 diagnostic counters — incremented from the async - /// subscriber tasks (`imu_recv`, `navsat_recv`) and from - /// `step()` itself (`motor_send`). Surface through - /// `Physics::counters()` so a bench operator can read - /// "did gz publish anything?" without scraping logs. - imu_recv: Arc, - navsat_recv: Arc, - motor_send: Arc, - /// Tokio runtime kept alive for the duration of this instance. - /// Dropped on shutdown which joins subscriber + publisher tasks. - _runtime: tokio::runtime::Runtime, + /// The gz-transport node — owns the subscriptions + publisher; kept + /// alive for the instance's lifetime (dropping it tears down transport). + _node: Node, + /// Actuators publisher on `//command/motor_speed`. + publisher: Publisher, + // Inbound channels — filled asynchronously by gz-transport's C++ + // receiver threads; drained to the caches below by `pump()`. + imu_rx: Receiver, + navsat_rx: Receiver, + odom_rx: Receiver, + pose_rx: Receiver, + mag_rx: Option>, + // Latest-value caches (persist between the sensors' own update rates). + latest_imu: Mutex>, + latest_position_ned_m: Mutex<[f32; 3]>, + latest_velocity_ned: Mutex>, + latest_heading_ned: Mutex>, + latest_mag_body_ned: Mutex>, + imu_recv: AtomicU64, + navsat_recv: AtomicU64, + motor_send: AtomicU64, + /// Staged single-rotor failure (0-3, or -1 = none). See the rotor-out slice. + failed_rotor: AtomicI32, + /// ACHIEVED per-rotor throttle from the last `step` (post-failure) — the + /// RPM source the production FDI consumes. + last_achieved: Mutex<[f32; 4]>, } impl GazeboPhysics { - /// Alias for `connect` — mirrors the stub `GazeboPhysics::new` - /// signature so the CLI binary uses the same call regardless - /// of feature flag. Panics on connect failure (the connect - /// attempt is a programmer error in the CLI surface; library - /// users should call `connect` directly for `Result`). - pub fn new( - world: impl Into, - model: impl Into, - ) -> Self { - Self::connect_with_home(world, model, Home::ORIGIN) - .expect("GazeboPhysics::new: gz-transport connect failed; is `gz sim` running?") + /// Alias for `connect` mirroring the stub signature; panics on failure + /// (the CLI surface treats a failed connect as a programmer error). + pub fn new(world: impl Into, model: impl Into) -> Self { + Self::connect_with_home(world, model, Home::ORIGIN).expect( + "GazeboPhysics::new: gz-transport Node::new failed; is `gz sim` running \ + and gz-transport13 on the library path?", + ) } - /// Connect to the gz-transport network and start subscriber - /// + publisher tasks. Blocks until the Node is online. - /// Home anchor defaults to world origin (0,0,0); use - /// `connect_with_home` to supply a launch-site lat/lon/alt. - pub fn connect( - world: impl Into, - model: impl Into, - ) -> Result { + /// Connect with the world-origin home anchor. + pub fn connect(world: impl Into, model: impl Into) -> Option { Self::connect_with_home(world, model, Home::ORIGIN) } - /// v0.18.1 — connect + supply the launch-site home for the - /// NavSat → NED projection. Without this the NavSat - /// subscriber still runs but `measure()` returns positions - /// relative to `Home::ORIGIN` (lat=0, lon=0, alt=0), which - /// is almost certainly not what you want for a bench run. + /// Connect via the C++-backed `gz-transport`: create a Node, subscribe + /// the sensor topics (channel-backed), advertise the Actuators + /// publisher. The C++ transport honours `GZ_IP` / `GZ_PARTITION` from + /// the environment, so no manual partition handling is needed (unlike + /// the old pure-Rust bridge). Returns `None` if the node or any + /// essential subscription/advertise fails. pub fn connect_with_home( world: impl Into, model: impl Into, home: Home, - ) -> Result { + ) -> Option { let world = world.into(); let model = model.into(); - - // v0.19.2 — `multi_thread` worker pool instead of - // `current_thread`. Spawned subscriber + publisher - // tasks need a runtime that actively drives them after - // `block_on(setup)` returns; current_thread only drives - // during explicit block_on and orphans everything else. - // First v0.19.2 bench round showed Actuators publish - // never reached the wire (`gz topic -i` 0 publishers - // mid-run) — root cause was the runtime model. - let runtime = tokio::runtime::Builder::new_multi_thread() - .worker_threads(2) - .enable_all() - .build() - .expect("tokio runtime"); - - let latest_imu: Arc>> = Arc::new(Mutex::new(None)); - let latest_position_ned_m = Arc::new(Mutex::new([0.0_f32; 3])); - let latest_velocity_ned: Arc>> = Arc::new(Mutex::new(None)); - let latest_heading_ned: Arc>> = Arc::new(Mutex::new(None)); - let latest_mag_body_ned: Arc>> = Arc::new(Mutex::new(None)); - let imu_recv = Arc::new(AtomicU64::new(0)); - let navsat_recv = Arc::new(AtomicU64::new(0)); - let motor_send = Arc::new(AtomicU64::new(0)); - - // v0.19.2 — one channel carrying [m0, m1, m2, m3] tuples. - let (rotors_tx, mut rotors_rx) = mpsc::unbounded_channel::<[f32; 4]>(); - - // Tasks run on the runtime; the result of the setup - // (Node + publishers) returns to the caller, errors - // propagate. - let imu_ref = latest_imu.clone(); - let position_ref = latest_position_ned_m.clone(); - let velocity_ref = latest_velocity_ned.clone(); - let heading_ref = latest_heading_ned.clone(); - let mag_ref = latest_mag_body_ned.clone(); - let imu_recv_ref = imu_recv.clone(); - let navsat_recv_ref = navsat_recv.clone(); - let home_for_setup = home; - let world_for_setup = world.clone(); - let model_for_setup = model.clone(); - runtime.block_on(async move { - use gz_transport_rs::Node; - use gz_transport_rs::msgs::{Double, Imu, NavSat}; - - let mut node = Node::new(None).await?; - // v0.19.3 — gz CLI uses the node's effective partition - // (GZ_PARTITION env or `hostname:username` default) in - // the topic FQN. Publishing with an empty partition - // produces a different FQN, and gz-sim's plugins — - // which subscribed on the default partition — never - // see our messages. v0.19.2 confirmed: bridge motor_send - // ticked 1000:1000 but body never moved while gz CLI on - // the same topic + msg lifted it. - let node_partition = node.partition(); - let imu_topic = format!( - "/world/{world_for_setup}/model/{model_for_setup}/link/base_link/sensor/imu_sensor/imu" - ); - let mut sub = node.subscribe::(&imu_topic).await?; - tokio::spawn(async move { - while let Some((msg, _meta)) = sub.recv().await { - let (ax, ay, az) = msg.linear_acceleration - .as_ref() - .map(|v| (v.x as f32, v.y as f32, v.z as f32)) - .unwrap_or((0.0, 0.0, 0.0)); - let (gx, gy, gz) = msg.angular_velocity - .as_ref() - .map(|v| (v.x as f32, v.y as f32, v.z as f32)) - .unwrap_or((0.0, 0.0, 0.0)); - let sample = ImuSample { - time: relay_ekf::Timestamp { seconds: 0, fraction: 0 }, - accel_body: enu_to_ned([ax, ay, az]), - gyro_body: enu_to_ned([gx, gy, gz]), + 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 odom_topic = format!("/model/{model}/odometry"); + let pose_topic = format!("/model/{model}/pose"); + let mag_topic = format!( + "/world/{world}/model/{model}/link/base_link/sensor/mag_sensor/magnetometer" + ); + // The SDF's MulticopterMotorModel = command/motor_speed. + let cmd_topic = format!("/{model}/command/motor_speed"); + + let imu_rx = node.subscribe_channel::(&imu_topic, 200)?; + let navsat_rx = node.subscribe_channel::(&navsat_topic, 50)?; + let odom_rx = node.subscribe_channel::(&odom_topic, 50)?; + let pose_rx = node.subscribe_channel::(&pose_topic, 50)?; + // Magnetometer is optional (best-effort heading enhancement). + let mag_rx = node.subscribe_channel::(&mag_topic, 50); + let publisher = node.advertise::(&cmd_topic)?; + + // Auto-capture the LAUNCH DATUM: if no explicit home was supplied + // (still ORIGIN), take the first NavSat fix as the home reference so + // NED position is relative to the launch point. The gz world sits at + // a non-zero MSL elevation (e.g. falcon-quad.sdf = 488 m Zürich); + // without this the raw MSL altitude leaks straight into the + // estimator as a −488 m offset (diagnosed flying the production core: + // the vehicle looked 488 m "runaway" when it was sitting still). + let mut home = home; + if home.lat_deg == 0.0 && home.lon_deg == 0.0 && home.alt_m == 0.0 { + let start = std::time::Instant::now(); + while start.elapsed().as_secs_f32() < 3.0 { + if let Ok(fix) = + navsat_rx.recv_timeout(std::time::Duration::from_millis(100)) + { + home = Home { + lat_deg: fix.latitude_deg, + lon_deg: fix.longitude_deg, + alt_m: fix.altitude, }; - *imu_ref.lock().unwrap() = Some(sample); - imu_recv_ref.fetch_add(1, Ordering::Relaxed); + break; } - }); + } + } - // v0.18.1 — NavSat subscriber: lat/lon/alt deg from - // the SDF NavSat plugin → local NED via Home. - let navsat_topic = format!( - "/world/{world_for_setup}/model/{model_for_setup}/link/base_link/sensor/navsat_sensor/navsat" - ); - let mut navsat_sub = node.subscribe::(&navsat_topic).await?; - tokio::spawn(async move { - while let Some((msg, _meta)) = navsat_sub.recv().await { - let ned = home_for_setup.project_to_ned_m( - msg.latitude_deg, - msg.longitude_deg, - msg.altitude, - ); - *position_ref.lock().unwrap() = ned; - navsat_recv_ref.fetch_add(1, Ordering::Relaxed); - } - }); + Some(Self { + world_name: world, + model_name: model, + home, + _node: node, + publisher, + imu_rx, + navsat_rx, + odom_rx, + pose_rx, + mag_rx, + latest_imu: Mutex::new(None), + latest_position_ned_m: Mutex::new([0.0; 3]), + latest_velocity_ned: Mutex::new(None), + latest_heading_ned: Mutex::new(None), + latest_mag_body_ned: Mutex::new(None), + imu_recv: AtomicU64::new(0), + navsat_recv: AtomicU64::new(0), + motor_send: AtomicU64::new(0), + failed_rotor: AtomicI32::new(-1), + last_achieved: Mutex::new([0.0; 4]), + }) + } - // v0.19.7 — Odometry subscriber: TRUE body velocity - // (twist.linear, ENU) → NED. Deterministic velocity for - // the altitude velocity-cascade; finite-diff NavSat left - // it marginally stable. - let odom_topic = format!("/model/{model_for_setup}/odometry"); - let mut odom_sub = node.subscribe::(&odom_topic).await?; - tokio::spawn(async move { - while let Some((msg, _meta)) = odom_sub.recv().await { - if let Some(tw) = msg.twist.as_ref() { - if let Some(lin) = tw.linear.as_ref() { - let v = enu_to_ned([lin.x as f32, lin.y as f32, lin.z as f32]); - *velocity_ref.lock().unwrap() = Some(v); - } - } - } + /// Drain all pending inbound messages into the latest-value caches. + /// Called at the top of `measure` (the per-tick entry) so the `&self` + /// read methods see fresh values; keeps only the newest per topic. + fn pump(&self) { + // IMU: ENU body → NED body (accel + gyro). + let mut got_imu = 0u64; + let mut last_imu = None; + for msg in self.imu_rx.try_iter() { + got_imu += 1; + let accel = msg + .linear_acceleration + .as_ref() + .map(|v| [v.x as f32, v.y as f32, v.z as f32]) + .unwrap_or([0.0; 3]); + let gyro = msg + .angular_velocity + .as_ref() + .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 }, + accel_body: enu_to_ned(accel), + gyro_body: enu_to_ned(gyro), }); - - // v0.22 — Pose_V subscriber for the TRUE heading. The - // model root pose's ENU orientation → NED yaw. (The - // OdometryPublisher leaves orientation unset.) - let pose_topic = format!("/model/{model_for_setup}/pose"); - let mut pose_sub = node.subscribe::(&pose_topic).await?; - tokio::spawn(async move { - while let Some((msg, _meta)) = pose_sub.recv().await { - // Model root pose: the first entry with a non-zero - // (set) orientation quaternion. - for p in &msg.pose { - if let Some(o) = p.orientation.as_ref() { - let n2 = o.w * o.w + o.x * o.x + o.y * o.y + o.z * o.z; - if n2 > 0.5 { - let yaw = enu_quat_to_ned_yaw(o.w, o.x, o.y, o.z); - if yaw.is_finite() { - *heading_ref.lock().unwrap() = Some(yaw); - } - break; - } - } - } + } + if let Some(s) = last_imu { + *self.latest_imu.lock().unwrap() = Some(s); + self.imu_recv.fetch_add(got_imu, Ordering::Relaxed); + } + // NavSat: lat/lon/alt → local NED via Home. + let mut got_nav = 0u64; + let mut last_pos = None; + for msg in self.navsat_rx.try_iter() { + got_nav += 1; + last_pos = Some(self.home.project_to_ned_m( + msg.latitude_deg, + msg.longitude_deg, + msg.altitude, + )); + } + if let Some(p) = last_pos { + *self.latest_position_ned_m.lock().unwrap() = p; + self.navsat_recv.fetch_add(got_nav, Ordering::Relaxed); + } + // Odometry twist: TRUE body velocity ENU → NED. + let mut last_vel = None; + 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])); } - }); - - // v0.22 — Magnetometer subscriber: the REAL heading source. - // The SDF mag_sensor reports the world magnetic_field in - // BODY frame (gz ENU body); convert to NED body with the - // same flip as the IMU. - let mag_topic = format!( - "/world/{world_for_setup}/model/{model_for_setup}/link/base_link/sensor/mag_sensor/magnetometer" - ); - if let Ok(mut mag_sub) = node.subscribe::(&mag_topic).await { - tokio::spawn(async move { - while let Some((msg, _meta)) = mag_sub.recv().await { - if let Some(f) = msg.field_tesla.as_ref() { - // gz's magnetometer is NED-NATIVE (legacy - // ArduPilot/PX4 heritage: it converts the - // world ENU→NED internally), - // UNLIKE the IMU (ENU body). Empirically (vs - // the truth-heading oracle at NED yaw 90°) - // the gz mag frame relates to our NED body - // by Rz(−90°): (x,y,z)_ned = (y, −x, z)_gz. - // [Caveat: confirmed at one heading; a - // yaw-sweep oracle would fully pin it.] - let (x, y, z) = (f.x as f32, f.y as f32, f.z as f32); - let v = [y, -x, z]; - if v.iter().all(|c| c.is_finite()) { - *mag_ref.lock().unwrap() = Some(v); - } + } + } + if let Some(v) = last_vel { + *self.latest_velocity_ned.lock().unwrap() = Some(v); + } + // Pose_V: model-root orientation → NED yaw (heading "compass"). + let mut last_yaw = None; + for msg in self.pose_rx.try_iter() { + for p in &msg.pose { + if let Some(o) = p.orientation.as_ref() { + let n2 = o.w * o.w + o.x * o.x + o.y * o.y + o.z * o.z; + if n2 > 0.5 { + let yaw = enu_quat_to_ned_yaw(o.w, o.x, o.y, o.z); + if yaw.is_finite() { + last_yaw = Some(yaw); } + break; } - }); + } } - - // v0.19.2 — single publisher emitting one - // `gz.msgs.Actuators` per tick. The four - // MulticopterMotorModel plugins share this topic and - // pick their `` index from `velocity`. - // - // v0.19.3 — topic name aligned with PX4's standard - // (`command/motor_speed`). v0.19.2's `//cmd_vel` - // worked at the wire-protocol level but the v0.19.2 - // bench evidence + first-light SDF showed the - // MulticopterMotorModel plugin's `` - // value is what gz constructs the topic from — - // `cmd_vel` → - // `//cmd_vel`, - // `command/motor_speed` - // → `//command/motor_speed`. The SDF + bridge - // must agree; v0.19.3 picks PX4's standard naming so - // the bench world drops cleanly into PX4-x500-derived - // muscle memory. - let actuators_topic = format!("/{model_for_setup}/command/motor_speed"); - let publisher = node - .advertise::(&actuators_topic, "gz.msgs.Actuators") - .await?; - let publish_partition = node_partition.clone(); - tokio::spawn(async move { - while let Some(cmd) = rotors_rx.recv().await { - let msg = Actuators { - header: None, - position: Vec::new(), - velocity: vec![ - cmd[0] as f64, - cmd[1] as f64, - cmd[2] as f64, - cmd[3] as f64, - ], - normalized: Vec::new(), - }; - // v0.19.3 — pass node's effective partition so - // FQN matches what gz-sim's subscribers expect. - let _ = publisher.publish(&publish_partition, &msg); + } + if let Some(y) = last_yaw { + *self.latest_heading_ned.lock().unwrap() = Some(y); + } + // Magnetometer (optional): gz NED-native field → our NED body via Rz(−90°). + if let Some(rx) = &self.mag_rx { + let mut last_mag = None; + for msg in rx.try_iter() { + if let Some(f) = msg.field_tesla.as_ref() { + let (x, y, z) = (f.x as f32, f.y as f32, f.z as f32); + let v = [y, -x, z]; + if v.iter().all(|c| c.is_finite()) { + last_mag = Some(v); + } } - }); - - // Double + NavSat imports preserved for any downstream - // re-extension; warning-suppressed below. - let _ = std::mem::size_of::(); - Ok::<_, gz_transport_rs::Error>(()) - })?; - - Ok(Self { - world_name: world, - model_name: model, - home, - latest_imu, - latest_position_ned_m, - latest_velocity_ned, - latest_heading_ned, - latest_mag_body_ned, - rotors_tx, - imu_recv, - navsat_recv, - motor_send, - _runtime: runtime, - }) + } + if let Some(m) = last_mag { + *self.latest_mag_body_ned.lock().unwrap() = Some(m); + } + } } - /// Map a [0, 1] mixer output (normalised THRUST fraction) to a - /// Gazebo motor command (rad/s). - /// - /// v0.25 — SQRT map. gz's MulticopterMotorModel produces - /// `thrust = motorConstant·ω²`, so a LINEAR `ω = pwm·max` made - /// actual thrust ∝ pwm², violating the mixer's linear-thrust - /// assumption and making the effective control gain - /// throttle-dependent (∂τ/∂pwm ∝ pwm) — a gain-scheduling hazard - /// that destabilised the (weakest, laggiest) yaw axis - /// conditionally/bistably. Mapping `ω = √pwm · max` makes - /// `thrust ∝ ω² ∝ pwm` (linear) and the gain throttle-invariant — - /// the "monotone thrust→PWM curve" the allocation SOTA calls for. - fn pwm_to_rad_per_s(pwm: f32) -> f32 { + /// Map a [0,1] mixer output (normalised thrust) to a gz motor command + /// (rad/s). SQRT map so gz's `thrust ∝ ω²` is LINEAR in the mixer + /// command (throttle-invariant gain) — the v0.25 stability fix. + pub fn pwm_to_rad_per_s(pwm: f32) -> f32 { const MAX_MOTOR_RAD_S: f32 = 1000.0; libm::sqrtf(pwm.clamp(0.0, 1.0)) * MAX_MOTOR_RAD_S } } impl Physics for GazeboPhysics { - fn name(&self) -> &'static str { "gazebo" } + fn name(&self) -> &'static str { + "gazebo" + } fn step(&mut self, motor_pwm: [f32; 4], _dt: f32) { - // v0.19.2 — send one [4×rad/s] tuple per tick on the - // single mpsc; the publisher task encodes a single - // gz.msgs.Actuators and writes it to //cmd_vel. - let rad_per_s = [ - Self::pwm_to_rad_per_s(motor_pwm[0]), - Self::pwm_to_rad_per_s(motor_pwm[1]), - Self::pwm_to_rad_per_s(motor_pwm[2]), - Self::pwm_to_rad_per_s(motor_pwm[3]), - ]; - let _ = self.rotors_tx.send(rad_per_s); - // One `motor_send` tick per call. With the v0.19.2 fix - // this is also one Actuators message published per tick - // (the v0.18 path was 4× Double messages per tick to the - // wrong topics — first gz bench showed motor_send=1000 - // with climb=0). + // Apply a staged rotor failure: the failed rotor → 0 rad/s, so the + // real MulticopterMotorModel loses its thrust + reaction torque. + let failed = self.failed_rotor.load(Ordering::Relaxed); + let mut achieved = motor_pwm; + if (0..4).contains(&failed) { + achieved[failed as usize] = 0.0; + } + *self.last_achieved.lock().unwrap() = achieved; + let msg = Actuators { + velocity: vec![ + Self::pwm_to_rad_per_s(achieved[0]) as f64, + Self::pwm_to_rad_per_s(achieved[1]) as f64, + Self::pwm_to_rad_per_s(achieved[2]) as f64, + Self::pwm_to_rad_per_s(achieved[3]) as f64, + ], + ..Default::default() + }; + let _ = self.publisher.publish(&msg); self.motor_send.fetch_add(1, Ordering::Relaxed); } 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], @@ -757,7 +683,42 @@ mod gz_real { } fn mag_body_ned(&self) -> Option<[f32; 3]> { - *self.latest_mag_body_ned.lock().unwrap() + // DISABLED: the gz-mag→NED-body frame conversion is UNVALIDATED (its + // own note: "confirmed at one heading; a yaw-sweep oracle would pin + // it"). Feeding it drove the IEKF's yaw to ~0.88 rad (50°) at rest, + // so the geometric controller fought a phantom yaw error → the + // attitude-priority mixer saturated to [1,0,1,0] (pure yaw) → lost + // collective → the quad never left the ground (diagnosed v1.113). + // Without a heading reference the IEKF holds yaw at its initial 0, + // which is correct for a hover demo. Re-enable once the mag frame is + // pinned with a yaw-sweep oracle. (`latest_mag_body_ned` still + // populated for that future validation.) + None + } + + fn motor_rpm(&self) -> Option<[i32; 4]> { + // FC_NO_RPM=1 — diagnostic: withhold ESC telemetry so the core's FDI + // is fully inert, isolating whether an instability is FDI-driven or + // an independent attitude/frame issue. + if std::env::var_os("FC_NO_RPM").is_some() { + return None; + } + // ACHIEVED throttle × ESC_RPM_FULL (8000, matching falcon-core): the + // failed rotor reads 0 → the production FDI residual fires. + const ESC_RPM_FULL: f32 = 8000.0; + let a = *self.last_achieved.lock().unwrap(); + Some([ + (a[0] * ESC_RPM_FULL) as i32, + (a[1] * ESC_RPM_FULL) as i32, + (a[2] * ESC_RPM_FULL) as i32, + (a[3] * ESC_RPM_FULL) as i32, + ]) + } + + fn fail_rotor(&mut self, rotor: usize) { + if rotor < 4 { + self.failed_rotor.store(rotor as i32, Ordering::Relaxed); + } } } diff --git a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf index 19bc074..8815ab1 100644 --- a/examples/falcon-sitl-gz/worlds/falcon-quad.sdf +++ b/examples/falcon-sitl-gz/worlds/falcon-quad.sdf @@ -314,7 +314,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_0_joint rotor_0 - ccw + cw 0.0125 0.025 1000.0 @@ -331,7 +331,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_1_joint rotor_1 - cw + ccw 0.0125 0.025 1000.0 @@ -348,7 +348,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_2_joint rotor_2 - ccw + cw 0.0125 0.025 1000.0 @@ -365,7 +365,7 @@ name="gz::sim::systems::MulticopterMotorModel"> rotor_3_joint rotor_3 - cw + ccw 0.0125 0.025 1000.0