diff --git a/Cargo.lock b/Cargo.lock index ad34a625..1a8ee7e6 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -82,15 +82,24 @@ dependencies = [ [[package]] name = "async-trait" -version = "0.1.60" +version = "0.1.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "677d1d8ab452a3936018a687b20e6f7cf5363d713b732b8884001317b0e48aa3" +checksum = "705339e0e4a9690e2908d2b3d049d85682cf19fbd5782494498fbf7003a6a282" dependencies = [ "proc-macro2", "quote", "syn", ] +[[package]] +name = "atomic-polyfill" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3ff7eb3f316534d83a8a2c3d1674ace8a5a71198eba31e2e2b597833f699b28" +dependencies = [ + "critical-section", +] + [[package]] name = "atty" version = "0.2.14" @@ -369,13 +378,13 @@ dependencies = [ [[package]] name = "clap" -version = "4.0.32" +version = "4.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7db700bc935f9e43e88d00b0850dae18a63773cfbec6d8e070fccf7fef89a39" +checksum = "4ec7a4128863c188deefe750ac1d1dfe66c236909f845af04beed823638dc1b2" dependencies = [ "bitflags", - "clap_derive 4.0.21", - "clap_lex 0.3.0", + "clap_derive 4.1.0", + "clap_lex 0.3.1", "is-terminal", "once_cell", "strsim 0.10.0", @@ -397,9 +406,9 @@ dependencies = [ [[package]] name = "clap_derive" -version = "4.0.21" +version = "4.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0177313f9f02afc995627906bbd8967e2be069f5261954222dac78290c2b9014" +checksum = "684a277d672e91966334af371f1a7b5833f9aa00b07c84e92fbce95e00208ce8" dependencies = [ "heck", "proc-macro-error", @@ -419,9 +428,9 @@ dependencies = [ [[package]] name = "clap_lex" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d4198f73e42b4936b35b5bb248d81d2b595ecb170da0bac7655c54eedfa8da8" +checksum = "783fe232adfca04f90f56201b26d79682d4cd2625e0bc7290b95123afe558ade" dependencies = [ "os_str_bytes", ] @@ -494,6 +503,12 @@ dependencies = [ "libc", ] +[[package]] +name = "critical-section" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6548a0ad5d2549e111e1f6a11a6c2e2d00ce6a3dafe22948d67c2b443f775e52" + [[package]] name = "crypto-common" version = "0.1.6" @@ -506,9 +521,9 @@ dependencies = [ [[package]] name = "cxx" -version = "1.0.85" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5add3fc1717409d029b20c5b6903fc0c0b02fa6741d820054f4a2efa5e5816fd" +checksum = "51d1075c37807dcf850c379432f0df05ba52cc30f279c5cfc43cc221ce7f8579" dependencies = [ "cc", "cxxbridge-flags", @@ -518,9 +533,9 @@ dependencies = [ [[package]] name = "cxx-gen" -version = "0.7.85" +version = "0.7.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ccca653bd8a21c5cfe696cd5347729d43f651298459b22e57c60fbae1cd49fec" +checksum = "b9165c9bac193c3243e70ae51a73964dd53585234b304b5448dfde4ed85008fc" dependencies = [ "codespan-reporting", "proc-macro2", @@ -530,15 +545,15 @@ dependencies = [ [[package]] name = "cxxbridge-flags" -version = "1.0.85" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "69a3e162fde4e594ed2b07d0f83c6c67b745e7f28ce58c6df5e6b6bef99dfb59" +checksum = "61b50bc93ba22c27b0d31128d2d130a0a6b3d267ae27ef7e4fae2167dfe8781c" [[package]] name = "cxxbridge-macro" -version = "1.0.85" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3e7e2adeb6a0d4a282e581096b06e1791532b7d576dcde5ccd9382acf55db8e6" +checksum = "39e61fda7e62115119469c7b3591fd913ecca96fb766cfd3f2e2502ab7bc87a5" dependencies = [ "proc-macro2", "quote", @@ -745,9 +760,12 @@ name = "firmware_protocol" version = "0.0.0" dependencies = [ "deku", + "derive_more", + "heapless", "nalgebra 0.30.1", "nalgebra 0.31.4", "nalgebra 0.32.1", + "replace_with", ] [[package]] @@ -942,9 +960,9 @@ dependencies = [ [[package]] name = "glob" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b919933a397b79c37e33b77bb2aa3dc8eb6e165ad809e58ff75bc7db2e34574" +checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" [[package]] name = "h2" @@ -965,12 +983,34 @@ dependencies = [ "tracing", ] +[[package]] +name = "hash32" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" +[[package]] +name = "heapless" +version = "0.7.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743" +dependencies = [ + "atomic-polyfill", + "hash32", + "rustc_version", + "spin", + "stable_deref_trait", +] + [[package]] name = "heck" version = "0.4.0" @@ -1130,19 +1170,19 @@ dependencies = [ [[package]] name = "io-lifetimes" -version = "1.0.3" +version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46112a93252b123d31a119a8d1a1ac19deac4fac6e0e8b0df58f0d4e5870e63c" +checksum = "e7d6c6f8c91b4b9ed43484ad1a938e393caf35960fce7f82a040497207bd8e9e" dependencies = [ "libc", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] name = "ipnet" -version = "2.7.0" +version = "2.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "11b0d96e660696543b251e58030cf9787df56da39dab19ad60eae7353040917e" +checksum = "30e22bd8629359895450b59ea7a776c850561b96a3b1d31321c1949d9e6c9146" [[package]] name = "is-terminal" @@ -1153,7 +1193,7 @@ dependencies = [ "hermit-abi 0.2.6", "io-lifetimes", "rustix", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] @@ -1348,7 +1388,7 @@ dependencies = [ "libc", "log", "wasi", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] @@ -1450,9 +1490,9 @@ dependencies = [ [[package]] name = "nom" -version = "7.1.1" +version = "7.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8903e5a29a317527874d0402f867152a3d21c908bb0b933e416c65e301d4c36" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" dependencies = [ "memchr", "minimal-lexical", @@ -1529,18 +1569,18 @@ dependencies = [ [[package]] name = "object" -version = "0.30.0" +version = "0.30.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "239da7f290cfa979f43f85a8efeee9a8a76d0827c356d37f9d3d7254d6b537fb" +checksum = "2b8c786513eb403643f2a88c244c2aaa270ef2153f55094587d0c48a3cf22a83" dependencies = [ "memchr", ] [[package]] name = "once_cell" -version = "1.16.0" +version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86f0b0d4bf799edbc74508c1e8bf170ff5f41238e5f8225603ca7caaae2b7860" +checksum = "6f61fba1741ea2b3d6a1e3178721804bb716a68a6aeba1149b5d52e3d464ea66" [[package]] name = "openssl" @@ -1661,15 +1701,15 @@ dependencies = [ [[package]] name = "parking_lot_core" -version = "0.9.5" +version = "0.9.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ff9f3fef3968a3ec5945535ed654cb38ff72d7495a25619e2247fb15a2ed9ba" +checksum = "ba1ef8814b5c993410bb3adfad7a5ed269563e4a2f90c41f5d85be7fb47133bf" dependencies = [ "cfg-if", "libc", "redox_syscall", "smallvec", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] @@ -1784,9 +1824,9 @@ checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068" [[package]] name = "proc-macro2" -version = "1.0.49" +version = "1.0.50" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57a8eca9f9c4ffde41714334dee777596264c7825420f521abc92b5b5deb63a5" +checksum = "6ef7d57beacfaf2d8aee5937dab7b7f28de3cb8b1828479bb5de2a7106f2bae2" dependencies = [ "unicode-ident", ] @@ -1859,9 +1899,9 @@ dependencies = [ [[package]] name = "regex" -version = "1.7.0" +version = "1.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e076559ef8e241f2ae3479e36f97bd5741c0330689e217ad51ce2c76808b868a" +checksum = "48aaa5748ba571fb95cd2c85c09f629215d3a6ece942baa100950af03a34f733" dependencies = [ "aho-corasick", "memchr", @@ -1883,6 +1923,12 @@ dependencies = [ "winapi", ] +[[package]] +name = "replace_with" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3a8614ee435691de62bcffcf4a66d91b3594bf1428a5722e79103249a095690" + [[package]] name = "reqwest" version = "0.11.13" @@ -1943,16 +1989,16 @@ dependencies = [ [[package]] name = "rustix" -version = "0.36.5" +version = "0.36.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a3807b5d10909833d3e9acd1eb5fb988f79376ff10fce42937de71a449c4c588" +checksum = "4feacf7db682c6c329c4ede12649cd36ecab0f3be5b7d74e6a20304725db4549" dependencies = [ "bitflags", "errno", "io-lifetimes", "libc", "linux-raw-sys", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] @@ -1978,12 +2024,11 @@ dependencies = [ [[package]] name = "schannel" -version = "0.1.20" +version = "0.1.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88d6731146462ea25d9244b2ed5fd1d716d25c52e4d54aa4fb0f3c4e9854dbe2" +checksum = "713cfb06c7059f3588fb8044c0fad1d09e3c01d225e25b9220dbfdcf16dbb1b3" dependencies = [ - "lazy_static", - "windows-sys 0.36.1", + "windows-sys", ] [[package]] @@ -2200,7 +2245,7 @@ checksum = "a4f120bb98cb4cb0dab21c882968c3cbff79dd23b46f07b1cf5c25044945ce84" name = "slimevr_overlay" version = "0.0.0" dependencies = [ - "clap 4.0.32", + "clap 4.1.1", "color-eyre", "eyre", "git-version", @@ -2255,6 +2300,21 @@ dependencies = [ "flatbuffers", ] +[[package]] +name = "spin" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f6002a767bff9e83f8eeecf883ecb8011875a21ae8da43bffb817a57e78cc09" +dependencies = [ + "lock_api", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + [[package]] name = "stackvec" version = "0.2.1" @@ -2319,9 +2379,9 @@ dependencies = [ [[package]] name = "termcolor" -version = "1.1.3" +version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab24d30b911b2376f3a13cc2cd443142f0c81dda04c118693e35b3835757755" +checksum = "be55cf8942feac5c765c2c993422806843c9a9a45d4d5c407ad6dd2ea95eb9b6" dependencies = [ "winapi-util", ] @@ -2387,9 +2447,9 @@ checksum = "cda74da7e1a664f795bb1f8a87ec406fb89a02522cf6e50620d016add6dbbf5c" [[package]] name = "tokio" -version = "1.23.0" +version = "1.24.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eab6d665857cc6ca78d6e80303a02cea7a7851e85dfbd77cbdc09bd129f1ef46" +checksum = "1d9f76183f91ecfb55e1d7d5602bd1d979e38a3a522fe900241cf195624d67ae" dependencies = [ "autocfg", "bytes", @@ -2402,7 +2462,7 @@ dependencies = [ "signal-hook-registry", "socket2", "tokio-macros", - "windows-sys 0.42.0", + "windows-sys", ] [[package]] @@ -2526,9 +2586,9 @@ dependencies = [ [[package]] name = "try-lock" -version = "0.2.3" +version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "59547bce71d9c38b83d9c0e92b6066c4253371f15005def0c30d9657f50c7642" +checksum = "3528ecfd12c466c6f163363caf2d02a71161dd5e1cc6ae7b34207ea2d42d81ed" [[package]] name = "tungstenite" @@ -2768,19 +2828,6 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" -[[package]] -name = "windows-sys" -version = "0.36.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ea04155a16a59f9eab786fe12a4a450e75cdb175f9e0d80da1e17db09f55b8d2" -dependencies = [ - "windows_aarch64_msvc 0.36.1", - "windows_i686_gnu 0.36.1", - "windows_i686_msvc 0.36.1", - "windows_x86_64_gnu 0.36.1", - "windows_x86_64_msvc 0.36.1", -] - [[package]] name = "windows-sys" version = "0.42.0" @@ -2788,85 +2835,55 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a3e1820f08b8513f676f7ab6c1f99ff312fb97b553d30ff4dd86f9f15728aa7" dependencies = [ "windows_aarch64_gnullvm", - "windows_aarch64_msvc 0.42.0", - "windows_i686_gnu 0.42.0", - "windows_i686_msvc 0.42.0", - "windows_x86_64_gnu 0.42.0", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_msvc", + "windows_x86_64_gnu", "windows_x86_64_gnullvm", - "windows_x86_64_msvc 0.42.0", + "windows_x86_64_msvc", ] [[package]] name = "windows_aarch64_gnullvm" -version = "0.42.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "41d2aa71f6f0cbe00ae5167d90ef3cfe66527d6f613ca78ac8024c3ccab9a19e" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.36.1" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9bb8c3fd39ade2d67e9874ac4f3db21f0d710bee00fe7cab16949ec184eeaa47" +checksum = "8c9864e83243fdec7fc9c5444389dcbbfd258f745e7853198f365e3c4968a608" [[package]] name = "windows_aarch64_msvc" -version = "0.42.0" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd0f252f5a35cac83d6311b2e795981f5ee6e67eb1f9a7f64eb4500fbc4dcdb4" +checksum = "4c8b1b673ffc16c47a9ff48570a9d85e25d265735c503681332589af6253c6c7" [[package]] name = "windows_i686_gnu" -version = "0.36.1" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "180e6ccf01daf4c426b846dfc66db1fc518f074baa793aa7d9b9aaeffad6a3b6" - -[[package]] -name = "windows_i686_gnu" -version = "0.42.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fbeae19f6716841636c28d695375df17562ca208b2b7d0dc47635a50ae6c5de7" - -[[package]] -name = "windows_i686_msvc" -version = "0.36.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2e7917148b2812d1eeafaeb22a97e4813dfa60a3f8f78ebe204bcc88f12f024" +checksum = "de3887528ad530ba7bdbb1faa8275ec7a1155a45ffa57c37993960277145d640" [[package]] name = "windows_i686_msvc" -version = "0.42.0" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "84c12f65daa39dd2babe6e442988fc329d6243fdce47d7d2d155b8d874862246" +checksum = "bf4d1122317eddd6ff351aa852118a2418ad4214e6613a50e0191f7004372605" [[package]] name = "windows_x86_64_gnu" -version = "0.36.1" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4dcd171b8776c41b97521e5da127a2d86ad280114807d0b2ab1e462bc764d9e1" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.42.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf7b1b21b5362cbc318f686150e5bcea75ecedc74dd157d874d754a2ca44b0ed" +checksum = "c1040f221285e17ebccbc2591ffdc2d44ee1f9186324dd3e84e99ac68d699c45" [[package]] name = "windows_x86_64_gnullvm" -version = "0.42.0" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "09d525d2ba30eeb3297665bd434a54297e4170c7f1a44cad4ef58095b4cd2028" +checksum = "628bfdf232daa22b0d64fdb62b09fcc36bb01f05a3939e20ab73aaf9470d0463" [[package]] name = "windows_x86_64_msvc" -version = "0.36.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c811ca4a8c853ef420abd8592ba53ddbbac90410fab6903b3e79972a631f7680" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.42.0" +version = "0.42.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f40009d85759725a34da6d89a94e63d7bdc50a862acf0dbc7c8e488f1edcb6f5" +checksum = "447660ad36a13288b1db4d4248e857b510e8c3a225c822ba4fb748c0aafecffd" [[package]] name = "winreg" diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index 3c02f584..bbfa7553 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -234,6 +234,12 @@ dependencies = [ "tracing-error", ] +[[package]] +name = "convert_case" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6245d59a3e82a7fc217c5828a6692dbc6dfb63a0c8c90495621f7b9d79704a0e" + [[package]] name = "core-isa-parser" version = "0.2.0" @@ -443,6 +449,19 @@ dependencies = [ "syn", ] +[[package]] +name = "derive_more" +version = "0.99.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4fb810d30a7c1953f91334de7244731fc3f3c10d7fe163338a35b9f640960321" +dependencies = [ + "convert_case", + "proc-macro2", + "quote", + "rustc_version 0.4.0", + "syn", +] + [[package]] name = "embassy-cortex-m" version = "0.1.0" @@ -949,7 +968,10 @@ name = "firmware_protocol" version = "0.0.0" dependencies = [ "deku", + "derive_more", + "heapless", "nalgebra", + "replace_with", ] [[package]] @@ -1497,6 +1519,12 @@ version = "0.6.28" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "456c603be3e8d448b072f410900c09faf164fbce2d480456f50eea6e25f9c848" +[[package]] +name = "replace_with" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3a8614ee435691de62bcffcf4a66d91b3594bf1428a5722e79103249a095690" + [[package]] name = "riscv" version = "0.8.0" diff --git a/firmware/src/main.rs b/firmware/src/main.rs index f76c8f69..dba6fa96 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -60,7 +60,7 @@ fn main() -> ! { static EXECUTOR: StaticCell = StaticCell::new(); EXECUTOR.init(Executor::new()).run(move |s| { - s.spawn(crate::networking::protocol::control_task(packets, quat)) + s.spawn(crate::networking::protocol::protocol_task(packets, quat)) .unwrap(); s.spawn(crate::networking::network_task(packets)).unwrap(); s.spawn(crate::imu::imu_task(quat, p.i2c, p.delay)).unwrap(); diff --git a/firmware/src/networking/protocol/mod.rs b/firmware/src/networking/protocol/mod.rs index 6b31526a..461d768c 100644 --- a/firmware/src/networking/protocol/mod.rs +++ b/firmware/src/networking/protocol/mod.rs @@ -5,82 +5,61 @@ pub use self::packets::Packets; use defmt::debug; use embassy_executor::task; +use embassy_futures::select::select; use firmware_protocol::{ - BoardType, CbPacket, ImuType, McuType, SbPacket, SensorDataType, SensorStatus, + sansio::{self, SbBuf}, + CbPacket, SlimeQuaternion, }; use crate::imu::Quat; -use crate::utils::Unreliable; - -#[allow(dead_code)] -mod v2; +use crate::utils::{Reliable, Unreliable}; #[task] -pub async fn control_task( +pub async fn protocol_task( packets: &'static Packets, quat: &'static Unreliable, ) -> ! { debug!("Control task!"); async { + let mut proto_state = sansio::State::new(); + let mut sb_buf = SbBuf::new(); loop { - do_work(packets, quat).await; + proto_state = match proto_state { + sansio::State::Disconnected(s) => { + while_disconnected(s, &mut sb_buf, &packets.clientbound).await + } + sansio::State::Connected(s) => { + while_connected(s, &mut sb_buf, &packets.clientbound, quat).await + } + }; + while !sb_buf.is_empty() { + let sb = sb_buf.pop().unwrap(); + packets.serverbound.send(sb).await; + } } } .await } -async fn do_work(packets: &Packets, quat: &Unreliable) { - match packets.clientbound.recv().await { - // Identify ourself when discovery packet is received - CbPacket::Discovery => { - packets - .serverbound - .send(SbPacket::Handshake { - // TODO: Compile time constants for board and MCU - board: BoardType::Custom, - // Should this IMU type be whatever the first IMU of the system is? - imu: ImuType::Unknown(0xFF), - mcu: McuType::Esp32, - imu_info: (0, 0, 0), // These appear to be inert - // Needs to be >=9 to use newer protocol, this is hard-coded in - // the java server :( - build: 10, - firmware: "SlimeVR-Rust".into(), - mac_address: [0; 6], - }) - .await; - debug!("Handshake"); +async fn while_disconnected( + state: sansio::Disconnected, + sb_buf: &mut SbBuf, + cb: &Reliable, +) -> sansio::State { + let cb_msg = cb.recv().await; + state.received_msg(cb_msg, sb_buf) +} - // After handshake, we are supposed to send `SensorInfo` only once. - packets - .serverbound - .send(SbPacket::SensorInfo { - sensor_id: 0, // First sensor (of two) - sensor_status: SensorStatus::Ok, - sensor_type: ImuType::Unknown(0xFF), - }) - .await; - debug!("SensorInfo"); - } - // When heartbeat is received, we should reply with heartbeat 0 aka Discovery - // The protocol is asymmetric so its a bit unintuitive. - CbPacket::Heartbeat => { - packets.serverbound.send(SbPacket::Heartbeat).await; - } - // Pings are basically like heartbeats, just echo data back - CbPacket::Ping { challenge } => { - packets.serverbound.send(SbPacket::Ping { challenge }).await; - } - _ => (), +async fn while_connected( + state: sansio::Connected, + sb_buf: &mut SbBuf, + cb: &Reliable, + quat: &Unreliable, +) -> sansio::State { + let event = select(cb.recv(), quat.wait()).await; + use embassy_futures::select::Either; + match event { + Either::First(cb_msg) => state.received_msg(cb_msg, sb_buf), + Either::Second(quat) => state.send_imu(0, SlimeQuaternion::from(quat), sb_buf), } - - packets - .serverbound - .send(SbPacket::RotationData { - sensor_id: 0, // First sensor - data_type: SensorDataType::Normal, // Rotation data without magnetometer correction. - quat: quat.wait().await.into_inner().into(), - calibration_info: 0, - }) - .await; } diff --git a/networking/firmware_protocol/Cargo.toml b/networking/firmware_protocol/Cargo.toml index 70dfac57..fecab54c 100644 --- a/networking/firmware_protocol/Cargo.toml +++ b/networking/firmware_protocol/Cargo.toml @@ -6,11 +6,16 @@ edition = "2021" [dependencies] deku = { version = "0.15", default-features = false, features = ["alloc"] } +heapless = { version = "0.7", default-features = false } +replace_with = { version = "0.1", default-features = false } +derive_more = "0.99" + # We support multiple versions of nalgebra since it changes so much. nalgebra032 = { package = "nalgebra", version = "0.32", default-features = false, optional = true } nalgebra031 = { package = "nalgebra", version = "0.31", default-features = false, optional = true } nalgebra030 = { package = "nalgebra", version = "0.30", default-features = false, optional = true } + [dev-dependencies] nalgebra032 = { package = "nalgebra", version = "0.32" } nalgebra031 = { package = "nalgebra", version = "0.31" } diff --git a/networking/firmware_protocol/src/lib.rs b/networking/firmware_protocol/src/lib.rs index 83d7ff74..46713105 100644 --- a/networking/firmware_protocol/src/lib.rs +++ b/networking/firmware_protocol/src/lib.rs @@ -3,6 +3,8 @@ extern crate alloc; mod clientbound; +pub mod sansio; +mod serialization; mod serverbound; pub use clientbound::*; @@ -20,9 +22,9 @@ use deku::prelude::*; #[derive(Debug, PartialEq, DekuRead, DekuWrite)] #[deku(endian = "e", ctx = "e: deku::ctx::Endian")] pub struct SlimeQuaternion { - pub i: f32, - pub j: f32, - pub k: f32, + pub x: f32, + pub y: f32, + pub z: f32, pub w: f32, } @@ -33,16 +35,27 @@ macro_rules! impl_Nalgebra { impl From> for SlimeQuaternion { fn from(q: Quaternion) -> Self { Self { - i: q.i, - j: q.j, - k: q.k, + x: q.i, + y: q.j, + z: q.k, w: q.w, } } } impl From for Quaternion { fn from(q: SlimeQuaternion) -> Self { - Self::new(q.w, q.i, q.j, q.k) + Self::new(q.w, q.x, q.y, q.z) + } + } + + impl From> for SlimeQuaternion { + fn from(q: UnitQuaternion) -> Self { + Self { + x: q.i, + y: q.j, + z: q.k, + w: q.w, + } } } }; @@ -50,17 +63,17 @@ macro_rules! impl_Nalgebra { #[cfg(any(test, feature = "nalgebra032"))] mod nalgebra032_impls { - use nalgebra032::Quaternion; + use nalgebra032::{Quaternion, UnitQuaternion}; impl_Nalgebra!(); } #[cfg(any(test, feature = "nalgebra031"))] mod nalgebra031_impls { - use nalgebra031::Quaternion; + use nalgebra031::{Quaternion, UnitQuaternion}; impl_Nalgebra!(); } #[cfg(any(test, feature = "nalgebra030"))] mod nalgebra030_impls { - use nalgebra030::Quaternion; + use nalgebra030::{Quaternion, UnitQuaternion}; impl_Nalgebra!(); } diff --git a/networking/firmware_protocol/src/sansio/mod.rs b/networking/firmware_protocol/src/sansio/mod.rs new file mode 100644 index 00000000..2e496c15 --- /dev/null +++ b/networking/firmware_protocol/src/sansio/mod.rs @@ -0,0 +1,175 @@ +//! A WIP sans-io implementation of the firmware protocol. +//! +//! sans-io means that it performs no io and can be used in async or non async code. +//! +//! TODO: All of this code operates on deku stuff, can we make it generic? + +use crate::{ + BoardType, CbPacket, ImuType, McuType, SbPacket, SensorDataType, SensorStatus, + SlimeQuaternion, +}; + +use replace_with::replace_with; + +/// A buffer of `SbPacket` that *must all be sent* before other functions on the +/// protocol are invoked. +#[derive(Debug)] +pub struct SbBuf(heapless::Deque); +impl SbBuf { + pub const fn new() -> Self { + Self(heapless::Deque::new()) + } + + pub fn peek(&self) -> Option<&SbPacket> { + self.0.front() + } + + pub fn pop(&mut self) -> Option { + self.0.pop_front() + } + + pub const fn len(&self) -> usize { + self.0.len() + } + + pub fn is_empty(&self) -> bool { + self.0.is_empty() + } + + fn push(&mut self, sb: SbPacket) { + self.0.push_back(sb).expect( + "Unexpectedly ran out of space in deque, this should have been impossible.", + ) + } +} + +/// Panics with a standardized message if the `SbBuf` is empty. +macro_rules! assert_empty { + ($sb_buf:expr) => { + assert!(!$sb_buf.is_empty(), "`SbBuf` must be empty!"); + }; +} + +#[derive(Debug, derive_more::From)] +pub enum State { + Disconnected(Disconnected), + Connected(Connected), +} +impl State { + pub const fn new() -> Self { + Self::Disconnected(Disconnected) + } + + /// Process a newly received message, enqueueing any serverbound messages in + /// `sb_buf`. + /// + /// # Panics + /// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when + /// all serverbound packets have already been sent. + pub fn received_msg<'b>(&mut self, cb: CbPacket, sb_buf: &'b mut SbBuf) { + assert_empty!(sb_buf); + // This allows us to temporarily steal from `self` even though we don't have + // ownership. + replace_with( + self, + // TODO: This will be a double panic and halt, making it hard to debug if we + // ever actually hit this. Is there a better way? Do we even care? + || unreachable!("Panic should be impossible because we already asserted"), + |taken| { + // Now we can use the owned value in functions that expect an owned type. + match taken { + State::Disconnected(s) => s.received_msg(cb, sb_buf), + State::Connected(s) => s.received_msg(cb, sb_buf), + } + }, + ); + } +} + +#[derive(Debug)] +pub struct Disconnected; +impl Disconnected { + /// Process a newly received message, enqueueing any serverbound messages in + /// `sb_buf`. + /// + /// # Panics + /// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when + /// all serverbound packets have already been sent. + pub fn received_msg<'b>(self, cb: CbPacket, sb_buf: &'b mut SbBuf) -> State { + assert_empty!(sb_buf); + match cb { + CbPacket::Discovery => { + sb_buf.push(SbPacket::Handshake { + // TODO: Compile time constants for board and MCU + board: BoardType::Custom, + // Should this IMU type be whatever the first IMU of the system is? + imu: ImuType::Unknown(0xFF), + mcu: McuType::Esp32, + imu_info: (0, 0, 0), // These appear to be inert + // Needs to be >=9 to use newer protocol, this is hard-coded in + // the java server :( + build: 10, + firmware: "SlimeVR-Rust".into(), + mac_address: [0; 6], + }); + + sb_buf.push(SbPacket::SensorInfo { + sensor_id: 0, // First sensor (of two) + sensor_status: SensorStatus::Ok, + sensor_type: ImuType::Unknown(0xFF), + }); + + self.into() + } + _ => self.into(), // Don't care about other packet types. + } + } +} + +#[derive(Debug)] +pub struct Connected {} +impl Connected { + /// Process a newly received message, enqueueing any serverbound messages in + /// `sb_buf`. + /// + /// # Panics + /// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when + /// all serverbound packets have already been sent. + pub fn received_msg<'b>(self, cb: CbPacket, sb_buf: &'b mut SbBuf) -> State { + assert_empty!(sb_buf); + match cb { + // When heartbeat is received, we should reply with another heartbeat. + CbPacket::Heartbeat => { + sb_buf.push(SbPacket::Heartbeat); + self.into() + } + // Pings are basically like heartbeats, but they also echo data back + CbPacket::Ping { challenge } => { + sb_buf.push(SbPacket::Ping { challenge }); + self.into() + } + _ => self.into(), // Don't care about other packet types. + } + } + + /// Enqueues an imu update in `SbBuf`. + /// + /// # Panics + /// Panics if `!sb_buf.is_empty()`. You are only supposed to call this function when + /// all serverbound packets have already been sent. + pub fn send_imu( + self, + sensor_id: u8, + quat: SlimeQuaternion, + sb_buf: &mut SbBuf, + ) -> State { + assert_empty!(sb_buf); + sb_buf.push(SbPacket::RotationData { + sensor_id, + data_type: SensorDataType::Normal, + quat, + calibration_info: 0, + }); + self.into() + } +} diff --git a/networking/firmware_protocol/src/serialization.rs b/networking/firmware_protocol/src/serialization.rs new file mode 100644 index 00000000..cdafc257 --- /dev/null +++ b/networking/firmware_protocol/src/serialization.rs @@ -0,0 +1,25 @@ +/// A simpler alternative to [`SerializeExact`], which will serialize without needing an +/// exact buffer size. +pub trait Serialize { + type Error; + /// Serializes into `buf`, returning the number of bytes written, or an error. + /// Note that this must not return `Ok` if only part of `self` was actually serialized. + fn serialize(&mut self, buf: &mut [u8]) -> Result; +} + +/// Serializes directly into a buffer, such as directly into the tcp/udp buffers with no +/// the exact size known. +pub trait SerializeExact { + type Error; + /// Serializes the packet into the provided buffer. Implementations may choose to steal + /// `self`, or perform a copy, or they may have already serialized into bytes preemptively + /// (such as with flatbuffers). + /// + /// # Panics + /// May panic if `f` returns an Ok variant with a buffer that is not the exact size as + /// `f`'s argument. + fn serialize_exact<'a, 'b>( + &'a mut self, + f: impl FnOnce(usize) -> Result<&'b mut [u8], Self::Error>, + ) -> Result<(), Self::Error>; +} diff --git a/networking/firmware_protocol/src/serverbound.rs b/networking/firmware_protocol/src/serverbound.rs index 51a06ad7..fc1332fa 100644 --- a/networking/firmware_protocol/src/serverbound.rs +++ b/networking/firmware_protocol/src/serverbound.rs @@ -258,9 +258,9 @@ mod tests { sensor_id: 40, data_type: SensorDataType::Normal, quat: SlimeQuaternion { - i: f32::from_be_bytes([00, 01, 02, 03]), - j: f32::from_be_bytes([10, 11, 12, 13]), - k: f32::from_be_bytes([20, 21, 22, 23]), + x: f32::from_be_bytes([00, 01, 02, 03]), + y: f32::from_be_bytes([10, 11, 12, 13]), + z: f32::from_be_bytes([20, 21, 22, 23]), w: f32::from_be_bytes([30, 31, 32, 33]), }, calibration_info: 127,