From c6ab7d72f2e65efd5ae61c714b31145feca48228 Mon Sep 17 00:00:00 2001 From: Fernando Nogueira Date: Mon, 11 Aug 2025 01:34:09 -0400 Subject: [PATCH 1/4] Refactor phoenix --- Cargo.lock | 582 ++++++++++++--- phoenix/src/board.rs | 384 ++++++++++ phoenix/src/comm/gps.rs | 49 ++ phoenix/src/comm/mod.rs | 3 + phoenix/src/comm/radio.rs | 170 +++++ phoenix/src/comm/sbg.rs | 53 ++ phoenix/src/drivers/camera.rs | 26 + phoenix/src/drivers/mod.rs | 3 + phoenix/src/drivers/recovery.rs | 35 + phoenix/src/drivers/sd.rs | 29 + phoenix/src/features.rs | 15 + phoenix/src/imu.rs | 27 +- phoenix/src/inference.rs | 43 ++ phoenix/src/main.rs | 1104 +++------------------------- phoenix/src/resources.rs | 67 ++ phoenix/src/sbg_manager.rs | 9 +- phoenix/src/setup.rs | 1 + phoenix/src/tasks/baro.rs | 70 ++ phoenix/src/tasks/gps.rs | 129 ++++ phoenix/src/tasks/led.rs | 16 + phoenix/src/tasks/mod.rs | 4 + phoenix/src/tasks/state_machine.rs | 50 ++ phoenix/src/traits.rs | 10 +- 23 files changed, 1752 insertions(+), 1127 deletions(-) create mode 100644 phoenix/src/board.rs create mode 100644 phoenix/src/comm/gps.rs create mode 100644 phoenix/src/comm/mod.rs create mode 100644 phoenix/src/comm/radio.rs create mode 100644 phoenix/src/comm/sbg.rs create mode 100644 phoenix/src/drivers/camera.rs create mode 100644 phoenix/src/drivers/mod.rs create mode 100644 phoenix/src/drivers/recovery.rs create mode 100644 phoenix/src/drivers/sd.rs create mode 100644 phoenix/src/features.rs create mode 100644 phoenix/src/inference.rs create mode 100644 phoenix/src/resources.rs create mode 100644 phoenix/src/setup.rs create mode 100644 phoenix/src/tasks/baro.rs create mode 100644 phoenix/src/tasks/gps.rs create mode 100644 phoenix/src/tasks/led.rs create mode 100644 phoenix/src/tasks/mod.rs create mode 100644 phoenix/src/tasks/state_machine.rs diff --git a/Cargo.lock b/Cargo.lock index edbc2f4..08e84c7 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -125,6 +125,12 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" +[[package]] +name = "az" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" + [[package]] name = "bare-metal" version = "0.2.5" @@ -399,8 +405,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bb8f828a681946b07a87750ed0593d885e7b101653bd6a3bb1942976156bb48" dependencies = [ "derive-new 0.7.0", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -411,8 +417,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "12e9f07ccc658ef072bce2e996f0c38c80ee4c241598b6557afe1877dd87ae98" dependencies = [ "derive-new 0.7.0", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -428,8 +434,8 @@ dependencies = [ "half", "log", "onnx-ir", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "regex", "rust-format", "serde", @@ -587,8 +593,8 @@ version = "1.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4f154e572231cb6ba2bd1176980827e3d5dc04cc183a75dea38109fbdd672d29" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -791,6 +797,17 @@ dependencies = [ "volatile-register", ] +[[package]] +name = "cortex-m-log" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88c26033fe85d2c5f45a173a6dadf710db4a72eb7da81dbfb795d8d9ebfaaca7" +dependencies = [ + "cortex-m", + "cortex-m-semihosting", + "log", +] + [[package]] name = "cortex-m-rt" version = "0.7.5" @@ -806,8 +823,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e37549a379a9e0e6e576fd208ee60394ccb8be963889eebba3ffe0980364f472" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1070,8 +1087,8 @@ dependencies = [ "derive-new 0.6.0", "ident_case", "prettyplease", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1082,8 +1099,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6a89898212c1eaba0e2f0dffcadc9790b20b75d2ec8836da084370b043be2623" dependencies = [ "darling", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1183,8 +1200,8 @@ checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "strsim", "syn 2.0.104", ] @@ -1196,7 +1213,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", - "quote", + "quote 1.0.40", "syn 2.0.104", ] @@ -1233,8 +1250,8 @@ checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1275,8 +1292,8 @@ version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1286,8 +1303,8 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d150dea618e920167e5973d70ae6ece4385b7164e0d799fe7c122dd0a5d912ad" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1297,8 +1314,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2cdc8d50f426189eef89dac62fabfa0abb27d5cc008f25bf4156a0203325becc" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1308,8 +1325,8 @@ version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "30542c1ad912e0e3d22a1935c290e12e8a29d704a420177a31faad4a601a0800" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1320,8 +1337,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6edb4b64a43d977b8e99788fe3a04d483834fba1215a7e02caa415b626497f7f" dependencies = [ "convert_case", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "rustc_version 0.4.1", "syn 2.0.104", ] @@ -1341,10 +1358,10 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cb7330aeadfbe296029522e6c40f315320aba36fc43a5b3632f3795348f3bd22" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", - "unicode-xid", + "unicode-xid 0.2.6", ] [[package]] @@ -1374,8 +1391,8 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "97369cbbc041bc366949bc74d34658d6cda5621039731c6310521892a3a20ae0" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1407,6 +1424,16 @@ dependencies = [ "bytemuck", ] +[[package]] +name = "eg-seven-segment" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e7d2f54ed234b333c71865c641906175a0d58bf366534ba5b8ded74b524b87b" +dependencies = [ + "bitflags 2.9.1", + "embedded-graphics", +] + [[package]] name = "either" version = "1.15.0" @@ -1469,8 +1496,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" dependencies = [ "darling", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1577,10 +1604,10 @@ dependencies = [ "embedded-storage-async", "futures-util", "nb 1.1.0", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "rand_core 0.6.4", - "sdio-host", + "sdio-host 0.5.0", "static_assertions", "stm32-fmc", "stm32-metapac 16.0.0", @@ -1712,6 +1739,12 @@ dependencies = [ "nb 1.1.0", ] +[[package]] +name = "embedded-display-controller" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "abdb518d44a2eda3b8a5dfc27b60a4bbbf2bb87eee436796d54c290ee6cc6545" + [[package]] name = "embedded-dma" version = "0.2.0" @@ -1721,6 +1754,29 @@ dependencies = [ "stable_deref_trait", ] +[[package]] +name = "embedded-graphics" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0649998afacf6d575d126d83e68b78c0ab0e00ca2ac7e9b3db11b4cbe8274ef0" +dependencies = [ + "az", + "byteorder", + "embedded-graphics-core", + "float-cmp", + "micromath", +] + +[[package]] +name = "embedded-graphics-core" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba9ecd261f991856250d2207f6d8376946cd9f412a2165d3b75bc87a0bc7a044" +dependencies = [ + "az", + "byteorder", +] + [[package]] name = "embedded-hal" version = "0.2.7" @@ -1805,6 +1861,17 @@ dependencies = [ "embedded-nal", ] +[[package]] +name = "embedded-sdmmc" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2f4d14180a76a8af24a45a0e1a4f9c97491b05a3b962d59d5e4ce0e6ab103736" +dependencies = [ + "byteorder", + "embedded-hal 0.2.7", + "log", +] + [[package]] name = "embedded-sdmmc" version = "0.9.0" @@ -1846,8 +1913,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1e6a265c649f3f5979b601d26f1d05ada116434c87741c9493cb56218f76cbc" dependencies = [ "heck", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -1924,6 +1991,15 @@ dependencies = [ "miniz_oxide", ] +[[package]] +name = "float-cmp" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "98de4bbd547a563b716d8dfa9aad1cb19bfab00f4fa09a6a4ed21dbcf44ce9c4" +dependencies = [ + "num-traits", +] + [[package]] name = "float-ord" version = "0.3.2" @@ -1958,8 +2034,8 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -2597,6 +2673,9 @@ name = "lazy_static" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" +dependencies = [ + "spin 0.9.8", +] [[package]] name = "libc" @@ -2713,8 +2792,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8cd48b535b9b37a25a2589ab8d4f997886a2c68f59960ce06588525f38dd4944" dependencies = [ "darling", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -2800,9 +2879,9 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "crc-any", "lazy_static", - "proc-macro2", + "proc-macro2 1.0.95", "quick-xml", - "quote", + "quote 1.0.40", "thiserror 1.0.69", ] @@ -3041,8 +3120,8 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 1.0.109", ] @@ -3114,11 +3193,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "77e878c846a8abae00dd069496dbe8751b16ac1c3d6bd2a7283a938e8228f90d" dependencies = [ "proc-macro-crate", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] +[[package]] +name = "numtoa" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6aa2c4e539b869820a2b82e1aef6ff40aa85e65decdd5185e83fb4b1249cd00f" + [[package]] name = "objc" version = "0.2.7" @@ -3165,12 +3250,37 @@ dependencies = [ "num-traits", ] +[[package]] +name = "otm8009a" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e56059a2e83de4409e5fa0854d3ae891b0f949af86c74ccf76a8bffe4ce6dc6d" +dependencies = [ + "embedded-display-controller", + "embedded-hal 0.2.7", +] + [[package]] name = "overload" version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b15813163c1d831bf4a13c3610c05c0d03b39feb07f7e09fa234dac9b15aaf39" +[[package]] +name = "panic-halt" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" + +[[package]] +name = "panic-itm" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d577d97d1b31268087b6dddf2470e6794ef5eee87d9dca7fcd0481695391a4c" +dependencies = [ + "cortex-m", +] + [[package]] name = "panic-probe" version = "0.3.2" @@ -3181,6 +3291,26 @@ dependencies = [ "defmt 0.3.100", ] +[[package]] +name = "panic-rtt-target" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d6ab67bc881453e4c90f958c657c1303670ea87bc1a16e87fd71a40f656dce9" +dependencies = [ + "cortex-m", + "rtt-target 0.3.1", +] + +[[package]] +name = "panic-semihosting" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee8a3e1233d9073d76a870223512ce4eeea43c067a94a445c13bd6d792d7b1ab" +dependencies = [ + "cortex-m", + "cortex-m-semihosting", +] + [[package]] name = "parking" version = "2.2.1" @@ -3254,7 +3384,7 @@ dependencies = [ "embedded-hal-bus", "embedded-io-async", "embedded-nal-async", - "embedded-sdmmc", + "embedded-sdmmc 0.9.0", "embedded-storage", "fdcan", "grounded", @@ -3341,7 +3471,7 @@ version = "0.2.36" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ff24dfcda44452b9816fff4cd4227e1bb73ff5a2f1bc1105aa92fb8565ce44d2" dependencies = [ - "proc-macro2", + "proc-macro2 1.0.95", "syn 2.0.104", ] @@ -3360,8 +3490,8 @@ version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", ] [[package]] @@ -3371,11 +3501,20 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" dependencies = [ "proc-macro-error-attr2", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] +[[package]] +name = "proc-macro2" +version = "0.4.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf3d2011ab5c909338f7887f4fc896d35932e29146c12c8d01da6b22a80ba759" +dependencies = [ + "unicode-xid 0.1.0", +] + [[package]] name = "proc-macro2" version = "1.0.95" @@ -3391,6 +3530,37 @@ version = "1.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3eb8486b569e12e2c32ad3e204dbaba5e4b5b216e9367044f25f1dba42341773" +[[package]] +name = "proptest" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fcdab19deb5195a31cf7726a210015ff1496ba1464fd42cb4f537b8b01b471f" +dependencies = [ + "bit-set", + "bit-vec", + "bitflags 2.9.1", + "lazy_static", + "num-traits", + "rand 0.9.2", + "rand_chacha 0.9.0", + "rand_xorshift", + "regex-syntax 0.8.5", + "rusty-fork", + "tempfile", + "unarray", +] + +[[package]] +name = "proptest-derive" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90b46295382dc76166cb7cf2bb4a97952464e4b7ed5a43e6cd34e1fec3349ddc" +dependencies = [ + "proc-macro2 0.4.30", + "quote 0.6.13", + "syn 0.15.44", +] + [[package]] name = "prost" version = "0.14.1" @@ -3429,8 +3599,8 @@ checksum = "9120690fafc389a67ba3803df527d0ec9cbbc9cc45e4cc20b332996dfb672425" dependencies = [ "anyhow", "itertools", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -3521,6 +3691,12 @@ dependencies = [ "version_check", ] +[[package]] +name = "quick-error" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" + [[package]] name = "quick-xml" version = "0.26.0" @@ -3530,13 +3706,22 @@ dependencies = [ "memchr", ] +[[package]] +name = "quote" +version = "0.6.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce23b6b870e8f94f81fb0a363d65d86675884b34a09043c81e5562f11c1f8e1" +dependencies = [ + "proc-macro2 0.4.30", +] + [[package]] name = "quote" version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ - "proc-macro2", + "proc-macro2 1.0.95", ] [[package]] @@ -3620,6 +3805,15 @@ dependencies = [ "rand 0.9.2", ] +[[package]] +name = "rand_xorshift" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "513962919efc330f829edb2535844d1b912b0fbe2ca165d613e4e8788bb05a5a" +dependencies = [ + "rand_core 0.9.3", +] + [[package]] name = "range-alloc" version = "0.1.4" @@ -3824,8 +4018,8 @@ checksum = "f387b12bd6c01d2c9d4776dddeefaf0ae51b9497c83c0186b1693f6821ff3c4a" dependencies = [ "indexmap", "proc-macro-error2", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -3839,8 +4033,8 @@ dependencies = [ "cortex-m", "fugit", "portable-atomic", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "rtic-time", "stm32-metapac 15.0.0", ] @@ -3875,13 +4069,32 @@ dependencies = [ "rtic-common", ] +[[package]] +name = "rtt-target" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "065d6058bb1204f51a562a67209e1817cf714759d5cf845aa45c75fa7b0b9d9b" +dependencies = [ + "ufmt-write", +] + +[[package]] +name = "rtt-target" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3afa12c77ba1b9bf560e4039a9b9a08bb9cde0e9e6923955eeb917dd8d5cf303" +dependencies = [ + "critical-section", + "ufmt-write", +] + [[package]] name = "rust-format" version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60e7c00b6c3bf5e38a880eec01d7e829d12ca682079f8238a464def3c4b31627" dependencies = [ - "proc-macro2", + "proc-macro2 1.0.95", "syn 1.0.109", ] @@ -3941,6 +4154,18 @@ version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" +[[package]] +name = "rusty-fork" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb3dcc6e454c328bb824492db107ab7c0ae8fcffe4ad210136ef014458c1bc4f" +dependencies = [ + "fnv", + "quick-error", + "tempfile", + "wait-timeout", +] + [[package]] name = "ryu" version = "1.0.20" @@ -4011,6 +4236,12 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +[[package]] +name = "sdio-host" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" + [[package]] name = "semver" version = "0.9.0" @@ -4062,8 +4293,8 @@ version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4112,7 +4343,7 @@ dependencies = [ "rtic", "rtic-monotonics", "rtic-sync", - "stm32h7xx-hal", + "stm32h7xx-hal 0.16.0 (git+https://github.com/uorocketry/stm32h7xx-hal)", ] [[package]] @@ -4145,8 +4376,8 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "231b4425dcc43afc7e18c34e7c6738cd252d42d91d909c948df14107c9ae79f1" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "string_morph", "syn 1.0.109", ] @@ -4265,6 +4496,56 @@ dependencies = [ "vcell", ] +[[package]] +name = "stm32h7xx-hal" +version = "0.16.0" +dependencies = [ + "bare-metal 1.0.0", + "cast", + "cfg-if", + "chrono 0.4.38", + "cortex-m", + "cortex-m-log", + "cortex-m-rt", + "cortex-m-semihosting", + "defmt 0.3.100", + "eg-seven-segment", + "embedded-display-controller", + "embedded-dma", + "embedded-graphics", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-sdmmc 0.5.0", + "embedded-storage", + "fdcan", + "fugit", + "lazy_static", + "log", + "nb 1.1.0", + "numtoa", + "otm8009a", + "panic-halt", + "panic-itm", + "panic-rtt-target", + "panic-semihosting", + "paste", + "proptest", + "proptest-derive", + "rand_core 0.6.4", + "rtic", + "rtt-target 0.4.0", + "sdio-host 0.9.0", + "serde", + "smoltcp", + "stm32-fmc", + "stm32h7", + "synopsys-usb-otg", + "tinybmp", + "usb-device", + "usbd-serial", + "void", +] + [[package]] name = "stm32h7xx-hal" version = "0.16.0" @@ -4326,8 +4607,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" dependencies = [ "heck", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "rustversion", "syn 2.0.104", ] @@ -4339,8 +4620,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7695ce3845ea4b33927c055a39dc438a45b059f7c1b3d91d38d10355fb8cbca7" dependencies = [ "heck", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4351,20 +4632,31 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f2c04b93fc15d79b39c63218f15e3fdffaa4c227830686e3b7c5f41244eb3e50" dependencies = [ "base64", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 1.0.109", "unicode-width 0.1.14", ] +[[package]] +name = "syn" +version = "0.15.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ca4b3b69a77cbe1ffc9e198781b7acb0c7365a883670e8f1c1bc66fba79a5c5" +dependencies = [ + "proc-macro2 0.4.30", + "quote 0.6.13", + "unicode-xid 0.1.0", +] + [[package]] name = "syn" version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "unicode-ident", ] @@ -4374,19 +4666,31 @@ version = "2.0.104" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17b6f705963418cdb9927482fa304bc562ece2fdd4f616084c50b7023b435a40" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "unicode-ident", ] +[[package]] +name = "synopsys-usb-otg" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe" +dependencies = [ + "cortex-m", + "embedded-hal 0.2.7", + "usb-device", + "vcell", +] + [[package]] name = "synstructure" version = "0.13.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4475,8 +4779,8 @@ version = "1.0.69" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4486,8 +4790,8 @@ version = "2.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4509,6 +4813,15 @@ dependencies = [ "cfg-if", ] +[[package]] +name = "tinybmp" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "197cc000e382175ff15abd9c54c694ef80ef20cb07e7f956c71e3ea97fc8dc60" +dependencies = [ + "embedded-graphics", +] + [[package]] name = "toml_datetime" version = "0.6.11" @@ -4543,8 +4856,8 @@ version = "0.1.30" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "81383ab64e72a7a8b8e13130c49e3dab29def6d0c7d76a03087b3cf71c5c6903" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4624,8 +4937,8 @@ name = "ublox_derive" version = "0.1.0" source = "git+https://github.com/uorocketry/ublox#286d43a3ae9a3ba30a04be04c65bf89aca6b1bfe" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 1.0.109", ] @@ -4635,11 +4948,17 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290ba2a4bfe92acb91bb9c0cdae32e9a1cda9cfe5b3329eaf9a444c522512de9" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 1.0.109", ] +[[package]] +name = "ufmt-write" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69" + [[package]] name = "ug" version = "0.1.0" @@ -4661,6 +4980,12 @@ dependencies = [ "yoke", ] +[[package]] +name = "unarray" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" + [[package]] name = "unicode-ident" version = "1.0.18" @@ -4679,6 +5004,12 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4a1a07cc7db3810833284e8d372ccdc6da29741639ecc70c9ec107df0fa6154c" +[[package]] +name = "unicode-xid" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" + [[package]] name = "unicode-xid" version = "0.2.6" @@ -4731,13 +5062,25 @@ dependencies = [ "byteorder", "hashbrown 0.13.2", "log", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "serde", "syn 1.0.109", "usbd-hid-descriptors", ] +[[package]] +name = "usbd-serial" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "065e4eaf93db81d5adac82d9cef8f8da314cb640fa7f89534b972383f1cf80fc" +dependencies = [ + "embedded-hal 0.2.7", + "embedded-io", + "nb 1.1.0", + "usb-device", +] + [[package]] name = "uuid" version = "1.17.0" @@ -4756,8 +5099,8 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "41b6d82be61465f97d42bd1d15bf20f3b0a3a0905018f38f9d6f6962055b0b5c" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -4788,6 +5131,15 @@ dependencies = [ "vcell", ] +[[package]] +name = "wait-timeout" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ac3b126d3914f9849036f826e054cbabdc8519970b8998ddaf3b5bd3c65f11" +dependencies = [ + "libc", +] + [[package]] name = "walkdir" version = "2.5.0" @@ -4833,8 +5185,8 @@ checksum = "2f0a0651a5c2bc21487bde11ee802ccaf4c51935d0d3d42a6101f98161700bc6" dependencies = [ "bumpalo", "log", - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", "wasm-bindgen-shared", ] @@ -4858,7 +5210,7 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7fe63fc6d09ed3792bd0897b314f53de8e16568c2b3f7982f468c0bf9bd0b407" dependencies = [ - "quote", + "quote 1.0.40", "wasm-bindgen-macro-support", ] @@ -4868,8 +5220,8 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", "wasm-bindgen-backend", "wasm-bindgen-shared", @@ -5169,8 +5521,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bbd5b46c938e506ecbce286b6628a02171d56153ba733b6c741fc627ec9579b" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -5180,8 +5532,8 @@ version = "0.60.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a47fddd13af08290e67f4acabf4b459f647552718f683a7b415d290ac744a836" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -5191,8 +5543,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "053c4c462dc91d3b1504c6fe5a726dd15e216ba718e84a0e46a88fbe5ded3515" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -5202,8 +5554,8 @@ version = "0.59.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd9211b69f8dcdfa817bfd14bf1c97c9188afa36f4750130fcdf3f400eca9fa8" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -5524,8 +5876,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2380878cad4ac9aac1e2435f3eb4020e8374b5f13c296cb75b4620ff8e229154" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", "synstructure", ] @@ -5545,8 +5897,8 @@ version = "0.8.26" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9ecf5b4cc5364572d7f4c329661bcc82724222973f2cab6f050a4e5c22f75181" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", ] @@ -5565,8 +5917,8 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d71e5d6e06ab090c67b5e44993ec16b72dcbaabc526db883a360057678b48502" dependencies = [ - "proc-macro2", - "quote", + "proc-macro2 1.0.95", + "quote 1.0.40", "syn 2.0.104", "synstructure", ] diff --git a/phoenix/src/board.rs b/phoenix/src/board.rs new file mode 100644 index 0000000..b84a550 --- /dev/null +++ b/phoenix/src/board.rs @@ -0,0 +1,384 @@ +use defmt::*; +use embassy_stm32::adc::Adc; +use embassy_stm32::gpio::{Input, Level, Output, OutputType, Pull, Speed}; +use embassy_stm32::mode; +use embassy_stm32::peripherals; +use embassy_stm32::spi::{Config as SpiConfig, Spi}; +use embassy_stm32::time::{khz, mhz}; +use embassy_stm32::timer::simple_pwm::{PwmPin, SimplePwm}; +use embassy_stm32::usart::{Config as UartConfig, RingBufferedUartRx, Uart, UartTx}; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; + +use common_arm::drivers::ms5611::Ms5611; + +use crate::drivers::recovery::{Arming, Fire, RecoveryManager}; +use crate::resources::{Irqs, RX_GPS_BUF, RX_RADIO_BUF, RX_SBG_BUF}; + +pub struct Board { + // SBG UART4 + pins + DMA + uart4: Option, + pa1: Option, + pa0: Option, + dma1_ch1: Option, + dma1_ch0: Option, + + // SBG power + pd8: Option, + + // Baro SPI4 + pins + CS + spi4: Option, + pe2: Option, + pe6: Option, + pe5: Option, + pb8: Option, + + // GPS UART8 + pins + DMA + enable/reset + uart8: Option, + pe0: Option, + pe1: Option, + dma1_ch5: Option, + dma1_ch6: Option, + pa4: Option, + pb2: Option, + + // Radio UART7 + pins + DMA + uart7: Option, + pe7: Option, + pe8: Option, + dma2_ch3: Option, + dma2_ch5: Option, + + // Camera triggers + pe14: Option, + pe12: Option, + + // Buzzer TIM3 + PC6 + tim3: Option, + pc6: Option, + + // IMU SPI3 + pins + spi3: Option, + pc10: Option, + pb5: Option, + pb4: Option, + pc0: Option, + pb6: Option, + pd4: Option, + + // LED + pb14: Option, + + // SD: SPI1 + pins + CS + spi1: Option, + pa5: Option, + pa7: Option, + pa6: Option, + pe9: Option, + + // Recovery GPIO + ADC + pc1: Option, + pd6: Option, + pd14: Option, + pc11: Option, + pd2: Option, + pd5: Option, + pd13: Option, + pc12: Option, + pd1: Option, + pa2: Option, + pb0: Option, + pa3: Option, + pc5: Option, + adc1: Option, +} + +impl Board { + pub fn new(config: embassy_stm32::Config) -> Self { + let p = embassy_stm32::init(config); + Self { + uart4: Some(p.UART4), + pa1: Some(p.PA1), + pa0: Some(p.PA0), + dma1_ch1: Some(p.DMA1_CH1), + dma1_ch0: Some(p.DMA1_CH0), + + pd8: Some(p.PD8), + + spi4: Some(p.SPI4), + pe2: Some(p.PE2), + pe6: Some(p.PE6), + pe5: Some(p.PE5), + pb8: Some(p.PB8), + + uart8: Some(p.UART8), + pe0: Some(p.PE0), + pe1: Some(p.PE1), + dma1_ch5: Some(p.DMA1_CH5), + dma1_ch6: Some(p.DMA1_CH6), + pa4: Some(p.PA4), + pb2: Some(p.PB2), + + uart7: Some(p.UART7), + pe7: Some(p.PE7), + pe8: Some(p.PE8), + dma2_ch3: Some(p.DMA2_CH3), + dma2_ch5: Some(p.DMA2_CH5), + + pe14: Some(p.PE14), + pe12: Some(p.PE12), + + tim3: Some(p.TIM3), + pc6: Some(p.PC6), + + spi3: Some(p.SPI3), + pc10: Some(p.PC10), + pb5: Some(p.PB5), + pb4: Some(p.PB4), + pc0: Some(p.PC0), + pb6: Some(p.PB6), + pd4: Some(p.PD4), + + pb14: Some(p.PB14), + + spi1: Some(p.SPI1), + pa5: Some(p.PA5), + pa7: Some(p.PA7), + pa6: Some(p.PA6), + pe9: Some(p.PE9), + + pc1: Some(p.PC1), + pd6: Some(p.PD6), + pd14: Some(p.PD14), + pc11: Some(p.PC11), + pd2: Some(p.PD2), + pd5: Some(p.PD5), + pd13: Some(p.PD13), + pc12: Some(p.PC12), + pd1: Some(p.PD1), + pa2: Some(p.PA2), + pb0: Some(p.PB0), + pa3: Some(p.PA3), + pc5: Some(p.PC5), + adc1: Some(p.ADC1), + } + } + + pub fn take_led_pin(&mut self) -> peripherals::PB14 { + self.pb14.take().unwrap() + } + + pub fn setup_sbg_uart( + &mut self, + ) -> (UartTx<'static, mode::Async>, RingBufferedUartRx<'static>) { + let mut uart_config = UartConfig::default(); + uart_config.baudrate = 115200; + let usart = Uart::new( + self.uart4.take().unwrap(), + self.pa1.take().unwrap(), + self.pa0.take().unwrap(), + Irqs, + self.dma1_ch1.take().unwrap(), + self.dma1_ch0.take().unwrap(), + uart_config, + ) + .unwrap(); + let (tx, rx) = usart.split(); + let ring_rx = rx.into_ring_buffered(unsafe { &mut RX_SBG_BUF }); + // Power pin if needed + if let Some(pd8) = self.pd8.take() { + let _sbg_pwr = Output::new(pd8, Level::High, Speed::Low); + } + (tx, ring_rx) + } + + pub fn setup_baro( + &mut self, + ) -> Ms5611, Output<'static>, embassy_time::Delay> { + let mut spi_config = SpiConfig::default(); + spi_config.frequency = mhz(16); + let spi_bus = Spi::new_blocking( + self.spi4.take().unwrap(), + self.pe2.take().unwrap(), + self.pe6.take().unwrap(), + self.pe5.take().unwrap(), + spi_config, + ); + let baro_cs = Output::new(self.pb8.take().unwrap(), Level::High, Speed::Low); + Ms5611::new(spi_bus, baro_cs, Delay).unwrap() + } + + pub async fn setup_gps( + &mut self, + ) -> (UartTx<'static, mode::Async>, RingBufferedUartRx<'static>) { + use embassy_stm32::usart::{DataBits, Parity as UParity, StopBits}; + let mut gps_enable = Output::new(self.pa4.take().unwrap(), Level::High, Speed::Low); + let mut gps_reset = Output::new(self.pb2.take().unwrap(), Level::High, Speed::Low); + + let mut uart_gps_config = UartConfig::default(); + uart_gps_config.baudrate = 9600; + uart_gps_config.data_bits = DataBits::DataBits8; + uart_gps_config.parity = UParity::ParityNone; + uart_gps_config.stop_bits = StopBits::STOP1; + uart_gps_config.detect_previous_overrun = false; + + let usart = Uart::new( + self.uart8.take().unwrap(), + self.pe0.take().unwrap(), + self.pe1.take().unwrap(), + Irqs, + self.dma1_ch5.take().unwrap(), + self.dma1_ch6.take().unwrap(), + uart_gps_config, + ) + .unwrap(); + let (gps_tx, gps_rx) = usart.split(); + let ring_gps_rx = gps_rx.into_ring_buffered(unsafe { &mut RX_GPS_BUF }); + + gps_reset.set_low(); + Delay.delay_ms(3000); + gps_reset.set_high(); + gps_enable.set_low(); + Delay.delay_ms(2000); + + (gps_tx, ring_gps_rx) + } + + pub fn setup_radio(&mut self) -> (UartTx<'static, mode::Async>, RingBufferedUartRx<'static>) { + let mut uart_radio_config = UartConfig::default(); + uart_radio_config.baudrate = 57600; + uart_radio_config.data_bits = embassy_stm32::usart::DataBits::DataBits8; + uart_radio_config.parity = embassy_stm32::usart::Parity::ParityNone; + uart_radio_config.stop_bits = embassy_stm32::usart::StopBits::STOP1; + + let uart_radio = Uart::new( + self.uart7.take().unwrap(), + self.pe7.take().unwrap(), + self.pe8.take().unwrap(), + Irqs, + self.dma2_ch3.take().unwrap(), + self.dma2_ch5.take().unwrap(), + uart_radio_config, + ) + .unwrap(); + let (radio_tx, radio_rx) = uart_radio.split(); + let ring_rx = radio_rx.into_ring_buffered(unsafe { &mut RX_RADIO_BUF }); + (radio_tx, ring_rx) + } + + pub fn setup_camera_triggers(&mut self) -> (Output<'static>, Output<'static>) { + let cam_trigger = Output::new(self.pe14.take().unwrap(), Level::Low, Speed::Low); + let cam_trigger_b = Output::new(self.pe12.take().unwrap(), Level::Low, Speed::Low); + (cam_trigger, cam_trigger_b) + } + + pub fn setup_buzzer(&mut self) -> SimplePwm<'static, peripherals::TIM3> { + let buzz_out_pin = PwmPin::new_ch1(self.pc6.take().unwrap(), OutputType::PushPull); + SimplePwm::new( + self.tim3.take().unwrap(), + Some(buzz_out_pin), + None, + None, + None, + khz(4), + Default::default(), + ) + } + + pub fn setup_imu(&mut self) { + // Keep behavior identical to previous main: build peripherals but don't use yet. + let mut imu_spi_config = embassy_stm32::spi::Config::default(); + imu_spi_config.frequency = mhz(9); + imu_spi_config.mode = embassy_stm32::spi::Mode { + polarity: embassy_stm32::spi::Polarity::IdleLow, + phase: embassy_stm32::spi::Phase::CaptureOnFirstTransition, + }; + let _imu_spi = Spi::new_blocking( + self.spi3.take().unwrap(), + self.pc10.take().unwrap(), + self.pb5.take().unwrap(), + self.pb4.take().unwrap(), + imu_spi_config, + ); + let _imu_odr = Input::new(self.pc0.take().unwrap(), Pull::None); + let _imu_cs = Output::new(self.pb6.take().unwrap(), Level::High, Speed::Low); + let _imu_nreset = Output::new(self.pd4.take().unwrap(), Level::High, Speed::Low); + } + + pub fn setup_sd_spi(&mut self) -> (Spi<'static, mode::Blocking>, Output<'static>) { + let mut sd_spi_config = SpiConfig::default(); + sd_spi_config.frequency = mhz(16); + sd_spi_config.bit_order = embassy_stm32::spi::BitOrder::MsbFirst; + + let sd_spi_bus = Spi::new_blocking( + self.spi1.take().unwrap(), + self.pa5.take().unwrap(), + self.pa7.take().unwrap(), + self.pa6.take().unwrap(), + sd_spi_config, + ); + let sd_cs = Output::new(self.pe9.take().unwrap(), Level::High, Speed::Low); + (sd_spi_bus, sd_cs) + } + + pub fn setup_recovery_manager(&mut self) -> RecoveryManager { + let mut ejection_enable = Input::new(self.pc1.take().unwrap(), Pull::Down); + + let main_arm_test = Output::new(self.pd6.take().unwrap(), Level::Low, Speed::Low); + let main_arm_test_b = Output::new(self.pd14.take().unwrap(), Level::Low, Speed::Low); + let drogue_arm_test = Output::new(self.pc11.take().unwrap(), Level::Low, Speed::Low); + let drogue_arm_test_b = Output::new(self.pd2.take().unwrap(), Level::Low, Speed::Low); + + let main_fire = Output::new(self.pd5.take().unwrap(), Level::Low, Speed::Low); + let main_fire_b = Output::new(self.pd13.take().unwrap(), Level::Low, Speed::Low); + let drogue_fire = Output::new(self.pc12.take().unwrap(), Level::Low, Speed::Low); + let drogue_fire_b = Output::new(self.pd1.take().unwrap(), Level::Low, Speed::Low); + + let mut main_mcu_ematch_sense = self.pa2.take().unwrap(); + let mut main_mcu_ematch_sense_b = self.pb0.take().unwrap(); + let mut drogue_mcu_ematch_sense = self.pa3.take().unwrap(); + let mut drogue_mcu_ematch_sense_b = self.pc5.take().unwrap(); + + let mut adc = Adc::new(self.adc1.take().unwrap()); + + info!( + "Ejection enable measurement: {}", + ejection_enable.get_level() + ); + info!( + "ADC measurement main ematch {}", + adc.blocking_read(&mut main_mcu_ematch_sense) + ); + info!( + "ADC measurement main B ematch {}", + adc.blocking_read(&mut main_mcu_ematch_sense_b) + ); + info!( + "ADC measurement drogue ematch {}", + adc.blocking_read(&mut drogue_mcu_ematch_sense) + ); + info!( + "ADC measurement drogue B ematch {}", + adc.blocking_read(&mut drogue_mcu_ematch_sense_b) + ); + info!( + "ADC measurement main ematch {}", + adc.blocking_read(&mut main_mcu_ematch_sense) + ); + + RecoveryManager { + arming: Arming { + main: main_arm_test, + drogue: drogue_arm_test, + main_b: main_arm_test_b, + drogue_b: drogue_arm_test_b, + }, + fire: Fire { + main: main_fire, + drogue: drogue_fire, + main_b: main_fire_b, + drogue_b: drogue_fire_b, + }, + } + } +} diff --git a/phoenix/src/comm/gps.rs b/phoenix/src/comm/gps.rs new file mode 100644 index 0000000..f95c755 --- /dev/null +++ b/phoenix/src/comm/gps.rs @@ -0,0 +1,49 @@ +use defmt::*; +use embassy_stm32::{mode, usart::UartTx}; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; +use ublox::cfg_val::CfgVal; +use ublox::CfgLayerSet; +use ublox::{CfgRstBuilder, CfgValSetBuilder, NavBbrMask, ResetMode}; + +pub async fn configure_ublox(gps_tx: &mut UartTx<'static, mode::Async>) { + if crate::features::ENABLE_GPS_CFG_BBR_RAM { + let config_packet = CfgValSetBuilder { + version: 1, + layers: CfgLayerSet::BBR | CfgLayerSet::RAM, + reserved1: 0, + cfg_data: &[ + CfgVal::Uart1Baudrate(38400), + CfgVal::Uart1InProtUbx(true), + CfgVal::Uart1InProtNmea(false), + CfgVal::Uart1InProtRtcm3x(false), + CfgVal::Uart1OutProtUbx(true), + CfgVal::Uart1OutProtNmea(false), + CfgVal::MsgOutUbxNavPvtUart1(1), + CfgVal::RateMeas(200), + CfgVal::RateNav(1), + ], + } + .into_packet_vec(); + + gps_tx + .write(config_packet.as_slice()) + .await + .expect("ublox cfg write failed"); + } + + Delay.delay_ms(1000); + + if crate::features::ENABLE_GPS_SOFT_RESET { + let reset_packet = CfgRstBuilder { + nav_bbr_mask: NavBbrMask::empty(), + reset_mode: ResetMode::ControlledSoftwareReset, + reserved1: 0, + } + .into_packet_bytes(); + + info!("Sending software reset to apply configuration"); + gps_tx.write(&reset_packet).await.unwrap(); + Delay.delay_ms(500); + } +} diff --git a/phoenix/src/comm/mod.rs b/phoenix/src/comm/mod.rs new file mode 100644 index 0000000..2fa908a --- /dev/null +++ b/phoenix/src/comm/mod.rs @@ -0,0 +1,3 @@ +pub mod gps; +pub mod radio; +pub mod sbg; diff --git a/phoenix/src/comm/radio.rs b/phoenix/src/comm/radio.rs new file mode 100644 index 0000000..d27e0d2 --- /dev/null +++ b/phoenix/src/comm/radio.rs @@ -0,0 +1,170 @@ +use defmt::*; +use embassy_executor::task; +use embassy_stm32::{mode, usart::RingBufferedUartRx, usart::UartTx}; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; +use messages_prost::prost::Message; + +use messages_prost::mavlink; +use messages_prost::mavlink::peek_reader::PeekReader; +use messages_prost::mavlink::uorocketry::MavMessage; +use messages_prost::radio::radio_frame::Payload; + +use crate::resources::{RADIO_CHANNEL, RECOVERY_MANAGER}; + +#[task] +pub async fn radio_reader_task(mut rx: RingBufferedUartRx<'static>) { + loop { + let mut buf: [u8; crate::resources::RADIO_BUFFER_SIZE] = + [0; crate::resources::RADIO_BUFFER_SIZE]; + if let Ok(len) = rx.read(&mut buf).await { + if len > 0 { + info!("Received {} bytes from radio: {:?}", len, &buf[..len]); + let (_header, msg): (_, MavMessage) = mavlink::read_versioned_msg( + &mut PeekReader::new(&buf[..len]), + mavlink::MavlinkVersion::V2, + ) + .unwrap(); + + match msg { + mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { + info!("Received postcard message"); + if let Ok(recv) = messages_prost::radio::RadioFrame::decode_length_delimited( + &mut &msg.message[..], + ) { + if let Some(payload) = recv.payload { + match payload { + Payload::Sbg(sbg_data) => { + info!("Received SBG data: {:?}", sbg_data.data.is_some()); + } + Payload::Gps(gps_data) => { + info!("Received GPS data: {:?}", gps_data.data.len()); + } + Payload::Madgwick(madgwick_data) => { + info!( + "Received Madgwick data: {:?}", + madgwick_data.data.is_some() + ); + } + Payload::Iim20670(imu_data) => { + info!("Received IMU data: {:?}", imu_data.data.is_some()); + } + Payload::Log(log_data) => { + info!("Received Log data: {:?}", log_data.level); + } + Payload::State(state_message) => { + info!("Received State message: {:?}", state_message.state); + } + Payload::Command(command) => { + info!("Received Command: {:?}", command.data.is_some()); + if let Some(command_data) = command.data { + match command_data { + messages_prost::command::command::Data::Ping(ping) => { + info!("Ping"); + let mut msg_buf: [u8; 255] = [0; 255]; + let msg = messages_prost::radio::RadioFrame { + node: messages_prost::common::Node::Phoenix.into(), + payload: Some( + messages_prost::radio::radio_frame::Payload::Command( + messages_prost::command::Command { + node: 0, + data: Some( + messages_prost::command::command::Data::Pong( + messages_prost::command::Pong { id: ping.id }, + ), + ), + }, + ), + ), + }; + msg.encode_length_delimited(&mut msg_buf.as_mut()) + .expect("Failed to encode SBG GPS Position"); + RADIO_CHANNEL.send(msg_buf).await; + } + messages_prost::command::command::Data::Pong(_pong) => { + info!("Pong"); + } + messages_prost::command::command::Data::Online(_online) => {} + messages_prost::command::command::Data::DeployDrogue(_deploy_drogue) => { + RECOVERY_MANAGER.lock(|cell| { + if let Some(recovery_manager) = cell.borrow_mut().as_mut() { + recovery_manager.arming.drogue.set_high(); + recovery_manager.arming.drogue_b.set_high(); + recovery_manager.fire.drogue.set_high(); + recovery_manager.fire.drogue_b.set_high(); + Delay.delay_ms(500); + recovery_manager.fire.drogue.set_low(); + recovery_manager.fire.drogue_b.set_low(); + recovery_manager.arming.drogue.set_low(); + recovery_manager.arming.drogue_b.set_low(); + } else { + info!("Recovery manager not initialized."); + } + }); + } + messages_prost::command::command::Data::DeployMain(_deploy_main) => { + RECOVERY_MANAGER.lock(|cell| { + info!("Boom boom"); + if let Some(recovery_manager) = cell.borrow_mut().as_mut() { + recovery_manager.arming.main.set_high(); + recovery_manager.arming.main_b.set_high(); + recovery_manager.fire.main.set_high(); + recovery_manager.fire.main_b.set_high(); + Delay.delay_ms(500); + recovery_manager.fire.main.set_low(); + recovery_manager.fire.main_b.set_low(); + recovery_manager.arming.main.set_low(); + recovery_manager.arming.main_b.set_low(); + } else { + info!("Recovery manager not initialized."); + } + }); + } + messages_prost::command::command::Data::PowerDown(_power_down) => {} + messages_prost::command::command::Data::RadioRateChange(_rate_change) => {} + } + } + } + } + } + } else { + info!("Failed to decode radio frame."); + } + } + mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(_command) => { + info!("Received command"); + } + mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { + info!("Received heartbeat message."); + } + _ => { + info!("Unknown mavlink message."); + } + } + } + } + } +} + +#[task] +pub async fn radio_writer_task(mut tx: UartTx<'static, mode::Async>) { + loop { + let data = RADIO_CHANNEL.receive().await; + + let mav_header = mavlink::MavHeader { + system_id: 1, + component_id: 1, + sequence: 1, + }; + let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( + mavlink::uorocketry::POSTCARD_MESSAGE_DATA { message: data }, + ); + let _ = mavlink::write_versioned_msg_async( + &mut tx, + mavlink::MavlinkVersion::V2, + mav_header, + &mav_message, + ) + .await; + } +} diff --git a/phoenix/src/comm/sbg.rs b/phoenix/src/comm/sbg.rs new file mode 100644 index 0000000..76a35ee --- /dev/null +++ b/phoenix/src/comm/sbg.rs @@ -0,0 +1,53 @@ +use defmt::*; +use embassy_executor::task; +use embassy_stm32::{mode, usart::RingBufferedUartRx, usart::UartTx}; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; +use messages_prost::prost::Message; + +use crate::resources::{BUFFER_CHANNEL, RADIO_CHANNEL, SBG_CHANNEL}; + +#[task] +pub async fn sbg_parser_task(tx: UartTx<'static, mode::Async>) { + let mut sbg = crate::sbg_manager::SBGManager::new(tx); + loop { + let full_buffer = BUFFER_CHANNEL.receive().await; + sbg.sbg_device.read_data(&full_buffer.try_into().unwrap()); + } +} + +#[task] +pub async fn sbg_uart_reader_task(mut rx: RingBufferedUartRx<'static>) { + info!("SBG DMA reader task spawned."); + loop { + let mut buf: [u8; sbg_rs::sbg::SBG_BUFFER_SIZE] = [0; sbg_rs::sbg::SBG_BUFFER_SIZE]; + if let Ok(len) = rx.read(&mut buf).await { + if len > 0 { + let _ = BUFFER_CHANNEL.send(buf).await; + } + } + Delay.delay_ms(100); + } +} + +#[task] +pub async fn sbg_receiver_task() { + loop { + let data = SBG_CHANNEL.receive().await; + match data.data { + Some(_x) => { + let mut buf: [u8; 255] = [0; 255]; + let msg = messages_prost::radio::RadioFrame { + node: messages_prost::common::Node::Phoenix.into(), + payload: Some(messages_prost::radio::radio_frame::Payload::Sbg(data)), + }; + msg.encode_length_delimited(&mut buf.as_mut()) + .expect("Failed to encode SBG GPS Position"); + RADIO_CHANNEL.send(buf).await; + } + None => { + info!("No SBG data received"); + } + } + } +} diff --git a/phoenix/src/drivers/camera.rs b/phoenix/src/drivers/camera.rs new file mode 100644 index 0000000..d659973 --- /dev/null +++ b/phoenix/src/drivers/camera.rs @@ -0,0 +1,26 @@ +use embassy_stm32::gpio::{Output, Level, Speed}; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; + +// Runs the same boot/record/stop sequence that was in main.rs, unchanged. +pub async fn run_boot_and_record(mut cam_trigger: Output<'static>, mut cam_trigger_b: Output<'static>) { + // power on + cam_trigger.set_high(); + Delay.delay_ms(2_000); + cam_trigger.set_low(); + Delay.delay_ms(100); + // trigger the camera + cam_trigger.set_high(); + Delay.delay_ms(500); + cam_trigger.set_low(); + Delay.delay_ms(10_000); + // stop recording + cam_trigger.set_high(); + Delay.delay_ms(500); + cam_trigger.set_low(); + + // currently unused, but kept to preserve signature + let _ = cam_trigger_b.set_low(); +} + + diff --git a/phoenix/src/drivers/mod.rs b/phoenix/src/drivers/mod.rs new file mode 100644 index 0000000..35579e6 --- /dev/null +++ b/phoenix/src/drivers/mod.rs @@ -0,0 +1,3 @@ +pub mod camera; +pub mod recovery; +pub mod sd; diff --git a/phoenix/src/drivers/recovery.rs b/phoenix/src/drivers/recovery.rs new file mode 100644 index 0000000..4d90602 --- /dev/null +++ b/phoenix/src/drivers/recovery.rs @@ -0,0 +1,35 @@ +//! Recovery pin mapping (moved from main.rs for clarity) +//! +//! MAIN_ARM/TEST = PD6 +//! MAIN_FIRE = PD5 +//! MAIN_ARM/TEST_B = PD14 +//! MAIN_FIRE_B = PD13 +//! DROGUE_ARM/TEST = PC11 +//! DROGUE_FIRE = PC12 +//! DROGUE_ARM/TEST_B = PD2 +//! DROGUE_FIRE_B = PD1 +//! MAIN_MCU_EMATCH_SENSE = PA2 +//! MAIN_MCU_EMATCH_SENSE_B = PB0 +//! DROGUE_MCU_EMATCH_SENSE = PA3 +//! DROGUE_MCU_EMATCH_SENSE_B = PC5 + +use embassy_stm32::gpio::Output; + +pub struct Arming { + pub main: Output<'static>, + pub drogue: Output<'static>, + pub main_b: Output<'static>, + pub drogue_b: Output<'static>, +} + +pub struct Fire { + pub main: Output<'static>, + pub drogue: Output<'static>, + pub main_b: Output<'static>, + pub drogue_b: Output<'static>, +} + +pub struct RecoveryManager { + pub arming: Arming, + pub fire: Fire, +} diff --git a/phoenix/src/drivers/sd.rs b/phoenix/src/drivers/sd.rs new file mode 100644 index 0000000..9015282 --- /dev/null +++ b/phoenix/src/drivers/sd.rs @@ -0,0 +1,29 @@ +use defmt::*; +use embassy_stm32::gpio::Output; +use embassy_stm32::spi::Spi; +use embassy_time::Delay; +use embedded_hal_1::delay::DelayNs; +use embedded_hal_bus::spi::RefCellDevice; +use embedded_sdmmc::{Mode, SdCard, VolumeIdx, VolumeManager}; + +// Initializes the SD card and performs the small demo previously in main.rs. +// Returns Ok(()) if initialization and simple access succeed. +pub fn init_and_demo( + sd_spi_bus: Spi<'static, embassy_stm32::mode::Blocking>, + sd_cs: Output<'static>, +) -> Result<(), ()> { + let sd_spi_bus_ref_cell = core::cell::RefCell::new(sd_spi_bus); + let sd_spi_device = RefCellDevice::new(&sd_spi_bus_ref_cell, sd_cs, Delay).map_err(|_| ())?; + let sdcard = SdCard::new(sd_spi_device, Delay); + info!("Card size is {} bytes", sdcard.num_bytes().map_err(|_| ())?); + let volume_mgr = VolumeManager::new( + sdcard, + super::super::TimeSink { + _marker: core::marker::PhantomData, + }, + ); + let _volume0 = volume_mgr.open_volume(VolumeIdx(0)).map_err(|_| ())?; + // let root_dir = volume0.open_root_dir().map_err(|_| ())?; + // Example read disabled by default; enable as needed. + Ok(()) +} diff --git a/phoenix/src/features.rs b/phoenix/src/features.rs new file mode 100644 index 0000000..ea4da5e --- /dev/null +++ b/phoenix/src/features.rs @@ -0,0 +1,15 @@ +//! Central feature toggles for phoenix. Flip booleans to enable/disable subsystems. +//! This is runtime gating (compile-time Cargo features can be added later if needed). + +pub const ENABLE_LED: bool = false; +pub const ENABLE_SBG: bool = false; +pub const ENABLE_GPS: bool = false; +pub const ENABLE_GPS_POLL: bool = false; +pub const ENABLE_GPS_PARSE: bool = false; +pub const ENABLE_GPS_CFG_FLASH: bool = false; +pub const ENABLE_GPS_CFG_BBR_RAM: bool = false; +pub const ENABLE_GPS_SOFT_RESET: bool = false; +pub const ENABLE_BARO: bool = false; +pub const ENABLE_RADIO: bool = true; +pub const ENABLE_INFERENCE: bool = false; +pub const ENABLE_SD: bool = false; diff --git a/phoenix/src/imu.rs b/phoenix/src/imu.rs index dfd4f81..5514a7f 100644 --- a/phoenix/src/imu.rs +++ b/phoenix/src/imu.rs @@ -1,7 +1,5 @@ //! Blocking SPI driver for the TDK IIM-20670 IMU with unit conversions and self-test. -#![no_std] - use defmt::{error, info, warn}; use embedded_hal_1::delay::DelayNs; use embedded_hal_1::digital::OutputPin; @@ -34,7 +32,6 @@ pub struct SelfTestValues { pub gyro_z_diff: i16, } - /// Represents the IIM-20670 device. pub struct Iim20670 { spi: SPI, @@ -142,7 +139,10 @@ where cs.set_high().map_err(Error::Cs)?; let mut driver = Self { - spi, cs, nreset, delay, + spi, + cs, + nreset, + delay, accel_fsr: AccelFsr::G16, gyro_fsr: GyroFsr::Dps1966, }; @@ -169,7 +169,12 @@ where // ... (rest of the functions are unchanged) ... - fn spi_transaction(&mut self, reg: u8, data: u16, is_write: bool) -> Result> { + fn spi_transaction( + &mut self, + reg: u8, + data: u16, + is_write: bool, + ) -> Result> { let _guard = CsGuard::new(&mut self.cs).map_err(Error::Cs)?; let rw_bit = if is_write { 1u32 } else { 0u32 }; @@ -179,7 +184,9 @@ where let mut buffer = tx_word.to_be_bytes(); info!(" SPI TX -> {=[u8]:#X}", buffer); - self.spi.transfer_in_place(&mut buffer).map_err(Error::Spi)?; + self.spi + .transfer_in_place(&mut buffer) + .map_err(Error::Spi)?; info!(" SPI RX <- {=[u8]:#X}", buffer); let response_word = u32::from_be_bytes(buffer); @@ -201,7 +208,9 @@ where let _guard = CsGuard::new(&mut self.cs).map_err(Error::Cs)?; let mut buffer = command.to_be_bytes(); info!(" SPI TX -> {=[u8]:#X}", buffer); - self.spi.transfer_in_place(&mut buffer).map_err(Error::Spi)?; + self.spi + .transfer_in_place(&mut buffer) + .map_err(Error::Spi)?; info!(" SPI RX <- {=[u8]:#X}", buffer); let response_word = u32::from_be_bytes(buffer); @@ -323,7 +332,9 @@ where }) } - pub fn read_all_converted(&mut self) -> Result<(Acceleration, AngularRate), Error> { + pub fn read_all_converted( + &mut self, + ) -> Result<(Acceleration, AngularRate), Error> { let accel = self.read_accel_g()?; let gyro = self.read_gyro_dps()?; Ok((accel, gyro)) diff --git a/phoenix/src/inference.rs b/phoenix/src/inference.rs new file mode 100644 index 0000000..a34e037 --- /dev/null +++ b/phoenix/src/inference.rs @@ -0,0 +1,43 @@ +use defmt::*; +use embassy_executor::task; +use embassy_time::{Duration, Timer}; + +// Inference entrypoint (moved out of main). Behavior intentionally minimal for now. +// The full reference code from main is preserved below as comments. +#[task] +pub async fn inference_task() { + info!("Inference task started."); + loop { + // TODO: wire real model inputs/outputs once data sources are ready + Timer::after(Duration::from_secs(1)).await; + } +} + +// --- Reference: Original inference setup from main.rs --- +// // Get a default device for the backend +// type Backend = burn::backend::NdArray; +// type BackendDevice = ::Device; +// let device = BackendDevice::default(); +// +// // Create a new model and load the state +// let recorder = burn::record::BinBytesRecorder::::new(); +// let record_bytes = include_bytes!("model/tte.mpk"); +// let record = recorder +// .load(record_bytes.as_ref(), &device) +// .expect("Failed to load recorder"); +// +// let model: crate::model::LstmNetwork = crate::model::LstmNetwork::new(&device).load_record(record); +// // Example inference call (define real input tensor as needed): +// // let input: burn::tensor::Tensor = ...; +// // let output = model.forward(input); + +// --- Reference helper moved from main.rs --- +// fn run_model<'a>(model: &Model, device: &BackendDevice, input: f32) -> Tensor { +// // Define the tensor +// let input = Tensor::::from_floats([[input]], &device); +// +// // Run the model on the input +// let output = model.forward(input); +// +// output +// } diff --git a/phoenix/src/main.rs b/phoenix/src/main.rs index 8edc710..79d52b0 100644 --- a/phoenix/src/main.rs +++ b/phoenix/src/main.rs @@ -3,61 +3,66 @@ #![no_std] #![no_main] +mod board; +mod comm; +mod drivers; +mod features; +mod imu; +mod inference; mod madgwick_service; +mod model; +mod music; +mod resources; mod sbg_manager; +mod setup; +mod tasks; mod traits; -mod music; -mod model; -mod imu; -use messages_prost::radio::radio_frame::Payload; -use ublox::{cfg_val::CfgVal::*, CfgLayerSet}; -use messages_prost::prost::Message; -use embedded_hal_1::delay::DelayNs; -use embedded_hal_1::digital::{OutputPin, PinState}; -use libm::powf; -use messages_prost::mavlink; -use messages_prost::mavlink::peek_reader::PeekReader; -use messages_prost::mavlink::uorocketry::MavMessage; -use messages_prost::sensor::{gps, sbg}; -use core::cell::RefCell; -use core::marker::PhantomData; -use defmt::*; -use embedded_alloc::LlffHeap as Heap; use burn::{ backend::NdArray, module::Module, // <-- FIX: Trait needed for .load_record() prelude::*, - record::{BinBytesRecorder, Recorder, FullPrecisionSettings}, // <-- FIX: Use BinBytesRecorder + record::{BinBytesRecorder, FullPrecisionSettings, Recorder}, // <-- FIX: Use BinBytesRecorder }; +use core::cell::RefCell; +use core::marker::PhantomData; +use defmt::*; use embassy_executor::Spawner; -use embassy_stm32::adc::Adc; use embassy_stm32::gpio::{Input, Level, Output, OutputType, Pull, Speed}; use embassy_stm32::mode::{Async, Blocking}; -use embassy_stm32::rtc::Rtc; -use embassy_stm32::spi::{BitOrder, Config as SpiConfig, Spi}; -use embassy_stm32::time::{khz, mhz}; -use embassy_stm32::timer::simple_pwm::{PwmPin, SimplePwm}; -use embassy_stm32::usart::{Config as UartConfig, RingBufferedUartRx, Uart, UartRx, UartTx}; -use embassy_stm32::{bind_interrupts, mode, peripherals, rcc, usart}; +use embassy_stm32::usart::{RingBufferedUartRx, UartTx}; +use embassy_stm32::{bind_interrupts, mode}; use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; use embassy_sync::blocking_mutex::Mutex; use embassy_sync::channel::Channel; use embassy_sync::signal::Signal; use embassy_time::{Delay, Duration, Instant, Timer}; +use embedded_alloc::LlffHeap as Heap; +use embedded_hal_1::delay::DelayNs; +use embedded_hal_1::digital::{OutputPin, PinState}; +use libm::powf; +use messages_prost::mavlink; +use messages_prost::mavlink::peek_reader::PeekReader; +use messages_prost::mavlink::uorocketry::MavMessage; +use messages_prost::prost::Message; +use messages_prost::radio::radio_frame::Payload; +use messages_prost::sensor::{gps, sbg}; +use ublox::{cfg_val::CfgVal::*, CfgLayerSet}; // use embedded_alloc::Heap; -use heapless::{HistoryBuffer, Vec}; -use messages_prost::sensor::sbg::{SbgData, SbgMessage}; -use sbg_rs::sbg::SBG_BUFFER_SIZE; -use static_cell::StaticCell; -use ublox::{CfgPrtUartBuilder, CfgRstBuilder, CfgValSetBuilder, DataBits, InProtoMask, NavBbrMask, OutProtoMask, Parity, ResetMode, StopBits, UartMode, UartPortId, UbxPacketRequest}; use crate::traits::Context; -use {defmt_rtt as _}; -use {panic_probe as _}; -use embedded_sdmmc::{Mode, SdCard, VolumeIdx, VolumeManager}; +use defmt_rtt as _; use embedded_hal_bus::spi::RefCellDevice; use embedded_io_async::Read; +use heapless::{HistoryBuffer, Vec}; +use messages_prost::sensor::sbg::{SbgData, SbgMessage}; use nmea::SentenceType; +use panic_probe as _; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use static_cell::StaticCell; +use ublox::{ + CfgPrtUartBuilder, CfgRstBuilder, CfgValSetBuilder, DataBits, InProtoMask, NavBbrMask, + OutProtoMask, Parity, ResetMode, StopBits, UartMode, UartPortId, UbxPacketRequest, +}; // Use a modern ms5611 driver that supports embedded-hal v1.0 use common_arm::drivers::ms5611::{Ms5611, OversamplingRatio}; @@ -66,25 +71,7 @@ use common_arm::drivers::ms5611::{Ms5611, OversamplingRatio}; use smlang::statemachine; use ublox::cfg_val::CfgVal; -struct Arming { - main: Output<'static>, - drogue: Output<'static>, - main_b: Output<'static>, - drogue_b: Output<'static>, -} - -struct Fire { - main: Output<'static>, - drogue: Output<'static>, - main_b: Output<'static>, - drogue_b: Output<'static>, -} - -struct RecoveryManager { - arming: Arming, - fire: Fire, -} - +use crate::drivers::recovery::{Arming, Fire, RecoveryManager}; // use crate::model::sine::Model; // ================================================================================= @@ -94,48 +81,7 @@ struct RecoveryManager { type Backend = NdArray; type BackendDevice = ::Device; -type DmaBuffer = [u8; SBG_BUFFER_SIZE]; - -const GPS_BUFFER_SIZE: usize = 26; - -const RADIO_BUFFER_SIZE: usize = 255; - - -#[global_allocator] -static HEAP: Heap = Heap::empty(); - -static PRESSURE_SIGNAL: Signal = Signal::new(); - -static SBG_CHANNEL: Channel = Channel::new(); -static BUFFER_CHANNEL: Channel = Channel::new(); -static EVENT_CHANNEL: Channel = Channel::new(); - -static COMMAND_CHANNEL: Channel = Channel::new(); -// static FAULT_CHANNEL: Channel = Channel::new(); -static RADIO_CHANNEL: Channel = Channel::new(); -#[link_section = ".axisram.buffers"] -static mut RX_SBG_BUF: [u8; SBG_BUFFER_SIZE] = [0; SBG_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut RX_RADIO_BUF: [u8; SBG_BUFFER_SIZE] = [0; SBG_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut RX_GPS_BUF: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; -// The SPI bus is protected by a Mutex, so the RefCell is not needed. -static SPI_BUS: StaticCell>> = StaticCell::new(); - -// Static variable for the RTC -pub static RTC: Mutex>> = - Mutex::new(RefCell::new(None)); - -pub static RECOVERY_MANAGER: Mutex>> = - Mutex::new(RefCell::new(None)); - -bind_interrupts!(struct Irqs { - UART4 => usart::InterruptHandler; - UART8 => usart::InterruptHandler; - UART7 => usart::InterruptHandler; -}); +use crate::resources::{Irqs, BUFFER_CHANNEL, HEAP, RX_GPS_BUF, RX_RADIO_BUF, RX_SBG_BUF}; statemachine! { transitions: { @@ -175,539 +121,27 @@ impl embedded_sdmmc::TimeSource for TimeSink { } } } -type AiBackend = NdArray; -type AiDevice = ::Device; - -const SEQ_LENGTH: usize = 50; -const NUM_FEATURES: usize = 12; - -// --- PASTE CONSTANTS FROM PYTHON SCRIPT HERE --- -// Replace these dummy values with the actual output from clean_rocket_data.py -const SCALE_MIN: [f32; NUM_FEATURES] = [0.0f32; 12]; -const SCALE_MAX: [f32; NUM_FEATURES] = [1.0f32; 12]; -// ------------------------------------------------ - -/// Normalizes a single feature value using the pre-calculated min/max. -fn normalize_value(value: f32, min: f32, max: f32) -> f32 { - if (max - min) == 0.0 { - return 0.0; // Avoid division by zero - } - (value - min) / (max - min) -} - -#[embassy_executor::task] -async fn ai_task() { - info!("AI Inference Task starting..."); - - let device = AiDevice::default(); - - // 1. Create the model structure - info!("Initializing model structure..."); - let model: model::LstmNetwork = model::LstmNetwork::new(&device); - - // 2. Load the trained weights from the embedded file - info!("Loading trained weights..."); - let recorder = burn::record::NoStdInferenceRecorder::new(); - let record_bytes = include_bytes!("models/model.bin"); - info!("Loaded {} bytes of model weights.", record_bytes.len()); - let record = recorder - .load(record_bytes.to_vec(), &device) - .expect("Failed to load model weights"); - let model = model.load_record(record); - info!("Model loaded successfully."); - - let mut sensor_history: HistoryBuffer<[f32; NUM_FEATURES], SEQ_LENGTH> = HistoryBuffer::new(); - - loop { - let latest_sensor_data: [f32; NUM_FEATURES] = [0.0; 12]; // Dummy data - - let mut normalized_data = [0.0f32; NUM_FEATURES]; - for i in 0..NUM_FEATURES { - normalized_data[i] = normalize_value(latest_sensor_data[i], SCALE_MIN[i], SCALE_MAX[i]); - } - sensor_history.write(normalized_data); - - if sensor_history.len() == sensor_history.capacity() { - let mut flat_history: Vec = Vec::new(); - for frame in sensor_history.iter() { - for value in frame.iter() { - flat_history.push(*value).ok(); - } - } - - let input = Tensor::::from_floats(flat_history.as_slice(), &device) - .reshape([1, SEQ_LENGTH, NUM_FEATURES]); - - let output_log = model.forward(input); - let output_sec = (output_log.exp() - 1.0).into_data(); - let predictions = output_sec.as_slice::().unwrap(); - - info!("PREDICTIONS -> Burnout: {=f32}s, Apogee: {=f32}s, Impact: {=f32}s", - predictions[0], predictions[1], predictions[2] - ); - } - - Timer::after(Duration::from_millis(100)).await; // Run at 10Hz - } -} - // ================================================================================= // Application Tasks // ================================================================================= #[embassy_executor::task] -async fn led_blinker_task(pin: peripherals::PB14) { - let mut led = Output::new(pin, Level::High, Speed::Low); - info!("LED blinker task started."); - loop { - led.set_high(); - Timer::after_millis(500).await; - led.set_low(); - Timer::after_millis(500).await; - } -} - -#[embassy_executor::task] -async fn uart_dma_reader_task(mut rx: RingBufferedUartRx<'static>) { - info!("DMA reader task spawned."); - loop { - let mut buf: [u8; SBG_BUFFER_SIZE] = [0; SBG_BUFFER_SIZE]; - if let Ok(len) = rx.read(&mut buf).await { - if len > 0 { - let _ = BUFFER_CHANNEL.send(buf).await; - } - } - Delay.delay_ms(100); - } -} - -#[embassy_executor::task] -async fn uart_gps_dma_reader_task(mut gps_rx: RingBufferedUartRx<'static> , mut gps_tx: UartTx<'static, mode::Async>) { - info!("DMA reader task spawned."); - let request = - UbxPacketRequest::request_for::().into_packet_bytes(); - // loop { - // gps_tx.write(&request).await; - // Delay.delay_ms(1000); // Wait for GPS data to be ready - // } - // gps_tx.blocking_write(&request); - - // loop { - // let mut buf_data: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; - // if let Ok(len) = gps_rx.read(&mut buf_data).await { - // info!("read"); - // let request = - // UbxPacketRequest::request_for::().into_packet_bytes(); - // gps_tx.blocking_write(&request); - // cortex_m::asm::delay(10_000); - // let mut buf: [u8; 256] = [0; 256]; - // let bytes: [u8; 256] = [0; 256]; - // let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); - // let mut parser = ublox::Parser::new(buf); - // let mut msgs = parser.consume(&buf_data[..len]); - // while let Some(msg) = msgs.next() { - // match msg { - // Ok(msg) => match msg { - // ublox::PacketRef::NavPosLlh(x) => { - // info!( - // "GPS latitude: {:?}, longitude {:?}", - // x.lat_degrees(), - // x.lon_degrees() - // ); - // } - // ublox::PacketRef::NavStatus(x) => { - // info!("GPS fix stat: {:?}", x.fix_stat_raw()); - // } - // ublox::PacketRef::NavDop(x) => { - // info!("GPS geometric drop: {:?}", x.geometric_dop()); - // } - // ublox::PacketRef::NavSat(x) => { - // info!("GPS num sats used: {:?}", x.num_svs()); - // } - // ublox::PacketRef::NavVelNed(x) => { - // info!("GPS velocity north: {:?}", x.vel_north()); - // } - // ublox::PacketRef::NavPvt(x) => { - // info!("GPS nun sats PVT: {:?}", x.num_satellites()); - // } - // _ => { - // info!("GPS Message not handled."); - // } - // }, - // Err(e) => { - // info!("GPS parse Error"); - // } - // } - // } - // // } - // } - // } -} - -#[embassy_executor::task] -async fn sbg_parser_task(tx: UartTx<'static, mode::Async>) { - let mut sbg = sbg_manager::SBGManager::new(tx); - loop { - let full_buffer = BUFFER_CHANNEL.receive().await; - sbg.sbg_device.read_data(&full_buffer.try_into().unwrap()); - } -} - -#[embassy_executor::task] -async fn sbg_receiver_task() { - loop { - let data = SBG_CHANNEL.receive().await; - match data.data { - Some(x) => { - let mut buf: [u8; 255] = [0; 255]; - let msg = messages_prost::radio::RadioFrame { - node: messages_prost::common::Node::Phoenix.into(), - payload: Some(messages_prost::radio::radio_frame::Payload::Sbg(data)) - }; - msg.encode_length_delimited(&mut buf.as_mut()) - .expect("Failed to encode SBG GPS Position"); - RADIO_CHANNEL.send(buf).await; - - // match x { - // messages_prost::sensor::sbg::sbg_data::Data:: => { - - // } - // messages_prost::sensor::sbg::sbg_data::Data::GpsPos(gps_pos) => { - // let msg: SbgMessage = SbgMessage { - // node: 0, - // data: Some(data), - // }; - // msg.encode_length_delimited(&mut buf.as_mut()) - // .expect("Failed to encode SBG GPS Position"); - // RADIO_CHANNEL.send(buf).await; - // // info!("Received SBG GPS Position: {:?}", gps_pos); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::UtcTime(utc_time) => { - // // info!("Received SBG UTC Time: {:?}", utc_time.time_stamp); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::Imu(imu) => { - // // info!("Received SBG IMU data: {:?}", imu.time_stamp); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::EkfQuat(ekf_quat) => { - // // info!("Received SBG EKF Quaternion: {:?}", ekf_quat.time_stamp); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::EkfNav(ekf_nav) => { - // // info!("Received SBG EKF Navigation: {:?}", ekf_nav.time_stamp); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::GpsVel(gps_vel) => { - // // info!("Received SBG GPS Velocity: {:?}", gps_vel.time_stamp); - // }, - // messages_prost::sensor::sbg::sbg_data::Data::Air(air) => { - // let msg: SbgMessage = SbgMessage { - // node: 0, - // data: Some(data), - // }; - // msg.encode_length_delimited(&mut buf.as_mut()) - // .expect("Failed to encode SBG GPS Position"); - // RADIO_CHANNEL.send(buf).await; - // info!("air data send"); - // }, - // } - }, - None => { - info!("No SBG data received"); - }, - } - } -} - -#[embassy_executor::task] -async fn baro_reader_task(mut baro: Ms5611, Output<'static>, Delay>) { - info!("Barometer reader task started."); - const MAIN_HEIGHT: f32 = GROUND_HEIGHT + 500.0; // meters ASL - const HEIGHT_MIN: f32 = GROUND_HEIGHT + 300.0; // meters ASL - const GROUND_HEIGHT: f32 = 300.0; // meters ASL - const ASCENT_LOCKOUT: f32 = 0.1; - const DATA_POINTS: usize = 20; - const VALID_DESCENT_RATE: f32 = -0.005; // meters per millise - - let mut historical_barometer_altitude: HistoryBuffer<(f32, Instant), 20> = HistoryBuffer::new(); - - loop { - match baro.read_pressure_temperature(OversamplingRatio::Osr4096) { - Ok(reading) => { - // info!( - // "Baro: Temp: {} C, Pressure: {} mbar", - // reading.0, reading.1 - // ); - - // Hypsometric Formula - let altitude = ((powf(101.325 / reading.1, 1.0/5.257) - 1.0) * (25.0 + 273.15)) / 0.0065; - historical_barometer_altitude.write((altitude, Instant::now())); - - // Apogee detection logic - if historical_barometer_altitude.len() < 8 { - info!("not enough data points to detect apogee"); - continue; - } - - let mut buf = historical_barometer_altitude.oldest_ordered(); - if let Some(mut prev_reading) = buf.next() { // `prev_reading` is now a tuple: (f32, Instant) - let mut avg_sum: f32 = 0.0; - let mut datapoints_used = 0; - - for current_reading in buf { // `current_reading` is also a tuple - // Calculate time diff between the actual measurement times. - // Convert from micros to seconds for a more standard rate unit (meters/sec). - let time_diff = current_reading.1.duration_since(prev_reading.1).as_millis(); - - info!( - "prev alt: {}, new alt: {}, time diff: {} ms", - prev_reading.0, current_reading.0, time_diff - ); - - if time_diff == 0 { - continue; // Avoid division by zero - } - - let slope = (current_reading.0 - prev_reading.0) / time_diff as f32; - // info!("Slope: {} m/ms", slope); - // Your existing logic for ascent lockout - if slope > ASCENT_LOCKOUT { - continue; - } - - avg_sum += slope; - datapoints_used += 1; - prev_reading = current_reading; // Update to the current reading for the next iteration - } - - // Check if the average descent rate is valid - if datapoints_used > 0 { - let avg_slope = avg_sum / (datapoints_used as f32); - // info!("Average slope: {} m/ms", avg_slope); - if avg_slope <= VALID_DESCENT_RATE { - info!("Apogee detected! Average vertical speed: {} m/s", avg_slope * 1000.0); - // todo!("Send Apogee event over events channel to state machine to process."); - } - } - } - } - Err(e) => { - // error!("Baro: Driver reading failed: {:?}", e); - } - } - Timer::after(Duration::from_millis(100)).await; - } -} - -#[embassy_executor::task] -async fn radio_reader_task(mut rx: RingBufferedUartRx<'static>) { - loop { - let mut buf: [u8; RADIO_BUFFER_SIZE] = [0; RADIO_BUFFER_SIZE]; - if let Ok(len) = rx.read(&mut buf).await { - if len > 0 { - // Process the received data - info!("Received {} bytes from radio: {:?}", len, &buf[..len]); - let (_header, msg): (_, MavMessage) = - mavlink::read_versioned_msg(&mut PeekReader::new(&buf[..len]), mavlink::MavlinkVersion::V2).unwrap(); - - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - info!("Received postcard message"); - // decode the msg - if let Ok(recv) = messages_prost::radio::RadioFrame::decode_length_delimited( - &mut &msg.message[..], - ) { - // info!("Received radio frame: {:?}", recv.node); - if let Some(payload) = recv.payload { - match payload { - Payload::Sbg(sbg_data) => { - info!("Received SBG data: {:?}", sbg_data.data.is_some()); - } - Payload::Gps(gps_data) => { - info!("Received GPS data: {:?}", gps_data.data.len()); - // Handle GPS data - } - Payload::Madgwick(madgwick_data) => { - info!("Received Madgwick data: {:?}", madgwick_data.data.is_some()); - // Handle Madgwick data - } - Payload::Iim20670(imu_data) => { - info!("Received IMU data: {:?}", imu_data.data.is_some()); - // Handle IMU data - } - Payload::Log(log_data) => { - info!("Received Log data: {:?}", log_data.level ); - // Handle Log data - } - Payload::State(state_message) => { - info!("Received State message: {:?}", state_message.state); - // Handle State message - } - Payload::Command(command) => { - info!("Received Command: {:?}", command.data.is_some()); - if let Some(command_data) = command.data { - match command_data { - messages_prost::command::command::Data::Ping(ping) => { - // info!("Received Ping command: {:?}", ping); - info!("Ping"); - let mut buf: [u8; 255] = [0; 255]; - let msg = messages_prost::radio::RadioFrame { - node: messages_prost::common::Node::Phoenix.into(), - payload: Some(messages_prost::radio::radio_frame::Payload::Command( - messages_prost::command::Command { - node: 0, - data: Some(messages_prost::command::command::Data::Pong( - messages_prost::command::Pong { - id: ping.id, - } - )), - } - )) - }; - msg.encode_length_delimited(&mut buf.as_mut()) - .expect("Failed to encode SBG GPS Position"); - RADIO_CHANNEL.send(buf).await; - } - messages_prost::command::command::Data::Pong(pong) => { - // info!("Received Pong command: {:?}", pong); - info!("Pong"); - } - messages_prost::command::command::Data::Online(online) => { - // info!("Received Online command: {:?}", online); - } - messages_prost::command::command::Data::DeployDrogue(deploy_drogue) => { - RECOVERY_MANAGER.lock(|cell| { - // *cell.borrow_mut() = Some(recovery_manager); - if let Some(recovery_manager) = cell.borrow_mut().as_mut() { - recovery_manager.arming.drogue.set_high(); - recovery_manager.arming.drogue_b.set_high(); - recovery_manager.fire.drogue.set_high(); - recovery_manager.fire.drogue_b.set_high(); - Delay.delay_ms(500); - - recovery_manager.fire.drogue.set_low(); - recovery_manager.fire.drogue_b.set_low(); - recovery_manager.arming.drogue.set_low(); - recovery_manager.arming.drogue_b.set_low(); - } else { - info!("Recovery manager not initialized."); - } - }); - - // COMMAND_CHANNEL.send(command_data).await; - // info!("Received Deploy Drogue command: {:?}", deploy_drogue); - } - messages_prost::command::command::Data::DeployMain(deploy_main) => { - RECOVERY_MANAGER.lock(|cell| { - info!("Boom boom"); - // *cell.borrow_mut() = Some(recovery_manager); - if let Some(recovery_manager) = cell.borrow_mut().as_mut() { - recovery_manager.arming.main.set_high(); - recovery_manager.arming.main_b.set_high(); - recovery_manager.fire.main.set_high(); - recovery_manager.fire.main_b.set_high(); - Delay.delay_ms(500); - - recovery_manager.fire.main.set_low(); - recovery_manager.fire.main_b.set_low(); - recovery_manager.arming.main.set_low(); - recovery_manager.arming.main_b.set_low(); - } else { - info!("Recovery manager not initialized."); - } - }); - // info!("Received Deploy Main command: {:?}", deploy_main); - // COMMAND_CHANNEL.send(command_data).await; - - } - messages_prost::command::command::Data::PowerDown(power_down) => { - // info!("Received Power Down command: {:?}", power_down); - } - messages_prost::command::command::Data::RadioRateChange(rate_change) => { - // info!("Received Radio Rate Change command: {:?}", rate_change); - } - } - } - // Handle Command - } - } - } - - } else { - info!("Failed to decode radio frame."); - } - - } - mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { - info!("Received command"); - } - mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { - info!("Received heartbeat message."); - } - _ => { - info!("Unknown mavlink message."); - // info!("Received unknown MAVLink message: {:?}", msg); - } - } - } - } - // Timer::after(Duration::from_millis(100)).await; - } -} - -#[embassy_executor::task] -async fn radio_writer_task(mut tx: UartTx<'static, mode::Async>) { - loop { - let data = RADIO_CHANNEL.receive().await; - - let mav_header = mavlink::MavHeader { - system_id: 1, - component_id: 1, - sequence: 1, - }; - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { - message: data, - }, - ); - // info!("Writing radio message"); - mavlink::write_versioned_msg_async(&mut tx, mavlink::MavlinkVersion::V2, mav_header, &mav_message).await; - } -} - -#[embassy_executor::task] async fn sm_task(spawner: Spawner, state_machine: StateMachine) { info!("State Machine task started."); - loop { + loop { match state_machine.state { - States::Ascent => { - - }, - States::Fault => { - - }, - States::Init => { - - }, - States::WaitForLaunch => { - - }, - States::Descent => { - - }, - States::DrogueDescent => { - - }, - States::Fuck => { - - }, - States::Landed => { - - }, - States::MainDescent => { - - } - } + States::Ascent => {} + States::Fault => {} + States::Init => {} + States::WaitForLaunch => {} + States::Descent => {} + States::DrogueDescent => {} + States::Fuck => {} + States::Landed => {} + States::MainDescent => {} + } Timer::after(Duration::from_millis(1000)).await; } } @@ -753,7 +187,7 @@ async fn main(spawner: Spawner) { prediv: PllPreDiv::DIV4, mul: PllMul::MUL50, divp: Some(PllDiv::DIV2), - divq: Some(PllDiv::DIV8), + divq: Some(PllDiv::DIV8), divr: None, }); config.rcc.sys = Sysclk::PLL1_P; // 400 Mhz @@ -765,26 +199,12 @@ async fn main(spawner: Spawner) { config.rcc.voltage_scale = VoltageScale::Scale1; } // config.rcc.ls = rcc::LsConfig::default_lse(); - let p = embassy_stm32::init(config); - + let mut board = board::Board::new(config); + info!("Heap usage: {} bytes", HEAP.used()); - // --- IMU Setup --- - let mut imu_spi_config = SpiConfig::default(); - imu_spi_config.frequency = mhz(9); - imu_spi_config.mode = embassy_stm32::spi::Mode { - polarity: embassy_stm32::spi::Polarity::IdleLow, - phase: embassy_stm32::spi::Phase::CaptureOnFirstTransition, - }; - // let imu_spi = Spi::new( - // p.SPI3, p.PC10, p.PB5, p.PB4, p.DMA2_CH5, p.DMA2_CH6, imu_spi_config, - // ); - let imu_spi = Spi::new_blocking( - p.SPI3, p.PC10, p.PB5, p.PB4, imu_spi_config, - ); - let imu_odr = Input::new(p.PC0, Pull::None); - let imu_cs = Output::new(p.PB6, Level::High, Speed::Low); - let imu_nreset = Output::new(p.PD4, Level::High, Speed::Low); + // --- IMU Setup --- + board.setup_imu(); // let mut imu = imu::Iim20670::new(imu_spi, imu_cs, Some(imu_nreset), Delay).unwrap(); // loop { @@ -801,151 +221,28 @@ async fn main(spawner: Spawner) { // } // --- SBG Setup --- - let mut uart_config = UartConfig::default(); - uart_config.baudrate = 115200; - let usart = Uart::new( - p.UART4, p.PA1, p.PA0, Irqs, p.DMA1_CH1, p.DMA1_CH0, uart_config, - ).unwrap(); - let (tx, rx) = usart.split(); - let ring_rx = rx.into_ring_buffered(unsafe { &mut RX_SBG_BUF }); - let mut sbg_pwr = Output::new(p.PD8, Level::High, Speed::Low); - - // // --- Baro SPI Setup --- - let mut spi_config = SpiConfig::default(); - spi_config.frequency = mhz(16); - // let spi_bus = Spi::new( - // p.SPI4, p.PE2, p.PE6, p.PE5, p.DMA1_CH6, p.DMA1_CH7, spi_config, - // ); - let spi_bus = Spi::new_blocking( - p.SPI4, p.PE2, p.PE6, p.PE5, spi_config, - ); - info!("SPI4 bus configured."); - - // Initialize the Mutex without the RefCell. - // let spi_bus_mutex = SPI_BUS.init(embassy_sync::mutex::Mutex::new(spi_bus)); - - let baro_cs = Output::new(p.PB8, Level::High, Speed::Low); - info!("Barometer CS pin configured."); + let (tx, ring_rx) = board.setup_sbg_uart(); - // SpiDevice::new takes an immutable reference, which spi_bus_mutex can be coerced into. - // let baro_spi_device = SpiDevice::new(spi_bus_mutex, baro_cs); - let baro = Ms5611::new(spi_bus, baro_cs, Delay).unwrap(); + // --- Baro SPI Setup --- + let baro = Some(board.setup_baro()); - - // // --- SD Card --- - // let mut sd_spi_config = SpiConfig::default(); - - // sd_spi_config.frequency = mhz(16); - // sd_spi_config.bit_order = BitOrder::MsbFirst; - - // let mut sd_spi_bus = Spi::new_blocking( - // p.SPI1, p.PA5, p.PA7, p.PA6, sd_spi_config, - // ); - - // let sd_cs = Output::new(p.PE9, Level::High, Speed::Low); - // let data: [u8; 10] = [0xFF; 10]; - // sd_spi_bus.blocking_write(&data).unwrap(); - - // let sd_spi_bus_ref_cell = RefCell::new(sd_spi_bus); - // let sd_spi_device = RefCellDevice::new(&sd_spi_bus_ref_cell, sd_cs, Delay); - // let sdcard = SdCard::new(sd_spi_device.unwrap(), Delay); - // println!("Card size is {} bytes", sdcard.num_bytes().unwrap()); - // let volume_mgr = VolumeManager::new(sdcard, TimeSink::new()); - // let volume0 = volume_mgr.open_volume(VolumeIdx(0)).unwrap(); - // let root_dir = volume0.open_root_dir().unwrap(); - // // let my_file = root_dir.open_file_in_dir("MY_FILE.TXT", Mode::ReadOnly).unwrap(); - // // while !my_file.is_eof() { - // // let mut buffer = [0u8; 32]; - // // let num_read = my_file.read(&mut buffer).unwrap(); - // // for b in &buffer[0..num_read] { - // // info!("{}", *b as char); - // // } - // // } - // info!("Sd write and setup complete"); - - // // --- GPS Setup --- - let mut gps_enable = Output::new(p.PA4, Level::High, Speed::Low); - let mut gps_reset = Output::new(p.PB2, Level::High, Speed::Low); - let mut uart_gps_config = UartConfig::default(); - uart_gps_config.baudrate = 9600; - uart_gps_config.data_bits = embassy_stm32::usart::DataBits::DataBits8; - uart_gps_config.parity = embassy_stm32::usart::Parity::ParityNone; - uart_gps_config.stop_bits = embassy_stm32::usart::StopBits::STOP1; - uart_gps_config.detect_previous_overrun = false; - - // let uart_gps = Uart::new_blocking( - // p.UART8, p.PE0, p.PE1, uart_gps_config - // ).unwrap(); - let mut uart_gps = Uart::new( - p.UART8, p.PE0, p.PE1, Irqs, p.DMA1_CH5, p.DMA1_CH6, uart_gps_config - ).unwrap(); - - let (mut gps_tx, mut gps_rx) = uart_gps.split(); - let mut ring_gps_rx = gps_rx.into_ring_buffered(unsafe { &mut RX_GPS_BUF }); - gps_reset.set_low(); - Delay.delay_ms(3000); - gps_reset.set_high(); - gps_enable.set_low(); - Delay.delay_ms(2000); - let packet: [u8; 28] = CfgPrtUartBuilder { - portid: UartPortId::Uart1, - reserved0: 0, - tx_ready: 0, - mode: UartMode::new(DataBits::Eight, Parity::None, StopBits::One), - baud_rate: 9600, - in_proto_mask: InProtoMask::all(), - out_proto_mask: OutProtoMask::UBLOX, - flags: 0, - reserved5: 0, + // --- SD Card --- + if features::ENABLE_SD { + let (sd_spi_bus, sd_cs) = board.setup_sd_spi(); + let _ = drivers::sd::init_and_demo(sd_spi_bus, sd_cs); } - .into_packet_bytes(); - info!("Sending GPS packet: {:?}", &packet); - gps_tx.write(&packet).await.expect("TODO: panic message"); + // --- GPS Setup --- + let (mut gps_tx, mut ring_gps_rx) = { + let (tx, rx) = board.setup_gps().await; + (Some(tx), Some(rx)) + }; Delay.delay_ms(2000); - // let val_packet = CfgValSetBuilder { - // version: 1, - // layers: CfgLayerSet::FLASH, - // reserved1: 0, - // cfg_data: &[Uart1OutProtUbx(true), Uart1InProtUbx(true)], - // }.into_packet_vec(); - - // info!("Packet val {}", val_packet.clone().as_slice()); - - // gps_tx.write(val_packet.as_slice()).await.expect("TODO: panic message"); - - // let val_packet = CfgValSetBuilder { - // version: 1, - // // Save to RAM and BBR so it's active now and persists after software reset - // layers: CfgLayerSet::BBR | CfgLayerSet::RAM, - // reserved1: 0, - // cfg_data: &[ - // // Enable UBX-NAV-PVT on our UART port - // CfgVal::MsgOutUbxNavPvtUart1(1), - // // Set measurement rate to 200ms (5 Hz) - // CfgVal::RateMeas(200), - // // Make sure navigation rate matches measurement rate - // CfgVal::RateNav(1), - // ], - // }.into_packet_vec(); - - // gps_tx.write(val_packet.as_slice()).await.expect("TODO: panic message"); - - // let reset_packet = CfgRstBuilder { - // // We don't need to clear any data, just apply the new settings - // nav_bbr_mask: NavBbrMask::empty(), - // // This is the key: a software reset applies BBR settings without clearing them - // reset_mode: ResetMode::ControlledSoftwareReset, - // reserved1: 0, - // } - // .into_packet_bytes(); - - // info!("Sending software reset to apply configuration"); - // gps_tx.write(&reset_packet).await.unwrap(); + // Optional extra GPS config steps are now gated and handled in comm::gps::configure_ublox. - let config_packet = CfgValSetBuilder { + let config_packet = CfgValSetBuilder { version: 1, // Save to RAM (to apply now) and BBR (to make it persistent) layers: CfgLayerSet::BBR | CfgLayerSet::RAM, @@ -958,119 +255,22 @@ async fn main(spawner: Spawner) { CfgVal::Uart1InProtRtcm3x(false), CfgVal::Uart1OutProtUbx(true), CfgVal::Uart1OutProtNmea(false), // Explicitly disable NMEA - // --- Message Settings --- CfgVal::MsgOutUbxNavPvtUart1(1), // Enable NAV-PVT on UART1 - // --- Rate Settings --- CfgVal::RateMeas(200), // 200ms = 5Hz CfgVal::RateNav(1), // Navigation rate = Measurement rate ], - }.into_packet_vec(); - - gps_tx.write(config_packet.as_slice()).await.expect("TODO: panic message"); - - - Delay.delay_ms(1000); - - let reset_packet = CfgRstBuilder { - nav_bbr_mask: NavBbrMask::empty(), // .empty() preserves all BBR data - reset_mode: ResetMode::ControlledSoftwareReset, - reserved1: 0, } - .into_packet_bytes(); - - info!("Sending software reset to apply configuration"); - gps_tx.write(&reset_packet).await.unwrap(); - Delay.delay_ms(500); // Give module time to reset - - // let request = - // UbxPacketRequest::request_for::().into_packet_bytes(); - // gps_tx.blocking_write(&request); - - // loop { - // let request = - // UbxPacketRequest::request_for::().into_packet_bytes(); - // gps_tx.blocking_write(&request); - // // Delay.delay_ms(1000); - // let mut buf_data: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; - // ring_gps_rx.read(&mut buf_data).await; - // info!("GPS data read: {:?}", &buf_data[..]); - // // if let Ok(len) = gps_rx.read(&mut buf_data).await { - // // info!("read"); - - // Delay.delay_ms(1000); - // // cortex_m::asm::delay(10_000); - // let mut buf: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; - // let bytes: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; + .into_packet_vec(); - // let mut nmea = nmea::Nmea::default(); - // let ascii_buf = unsafe {buf_data.as_ascii_unchecked()}; - // info!("BUFFER: {}", ascii_buf.as_str()); - // // if let Some(ascii_buf) = ascii_buf { - // let res = nmea.parse(ascii_buf.as_str()); - - // match res { - // Ok(strings) => { - // info!("Result: {}", strings.as_str()); - // } - // _ => { - // info!("nmea parser none found"); - // } - // } - - - // // } else { - // // info!("No valid sentence"); - // // } + if let Some(ref mut tx) = gps_tx { + comm::gps::configure_ublox(tx).await; + } - // let buf: ublox::FixedLinearBuffer<'_> = ublox::FixedLinearBuffer::new(&mut buf[..]); - // let mut parser = ublox::Parser::new(buf); - // // info!("GPS Parser initialized."); - // let mut msgs = parser.consume_ubx(&buf_data); - // info!("GPS Messages consumed. {}", msgs.next().is_some()); - // while let Some(msg) = msgs.next() { - // match msg { - // Ok(msg) => match msg { - // ublox::PacketRef::NavPosLlh(x) => { - // info!( - // "GPS latitude: {:?}, longitude {:?}", - // x.lat_degrees(), - // x.lon_degrees() - // ); - // } - // ublox::PacketRef::NavStatus(x) => { - // info!("GPS fix stat: {:?}", x.fix_stat_raw()); - // } - // ublox::PacketRef::NavDop(x) => { - // info!("GPS geometric drop: {:?}", x.geometric_dop()); - // } - // ublox::PacketRef::NavSat(x) => { - // info!("GPS num sats used: {:?}", x.num_svs()); - // } - // ublox::PacketRef::NavVelNed(x) => { - // info!("GPS velocity north: {:?}", x.vel_north()); - // } - // ublox::PacketRef::NavPvt(x) => { - // info!("GPS nun sats PVT: {:?}", x.num_satellites()); - // } - // ublox::PacketRef::Unknown(msg) => { - // info!("Unknown GPS type."); - // } - // _ => { - // info!("GPS Message not handled."); - // } - // }, - // Err(e) => { - // info!("GPS parse Error"); - // } - // } - // } - // // } - // } + // Verbose GPS example moved to tasks::gps as a commented reference to keep main lean. - - // // --- Boom Boom Setup --- + // // --- Boom Boom Setup --- // /* // MAIN_ARM/TEST = PD6 // MAIN_FIRE = PD5 @@ -1085,141 +285,59 @@ async fn main(spawner: Spawner) { // DROUGE_MCU_EMATCH_SENSE = PA3 // DROGUE_MCU_EMATCH_SENSE_B = PC5 // */ + let recovery_manager = board.setup_recovery_manager(); - let mut ejection_enable = Input::new(p.PC1, Pull::Down); - - // // let main_arm_test = Input::new(p.PD6, Pull::Down); - let mut main_arm_test = Output::new(p.PD6, Level::Low, Speed::Low); - let main_arm_test_b = Output::new(p.PD14, Level::Low, Speed::Low); - let drogue_arm_test = Output::new(p.PC11, Level::Low, Speed::Low); - let drogue_arm_test_b = Output::new(p.PD2, Level::Low, Speed::Low); - - let mut main_fire = Output::new(p.PD5, Level::Low, Speed::Low); - let mut main_fire_b = Output::new(p.PD13, Level::Low, Speed::Low); - let drogue_fire = Output::new(p.PC12, Level::Low, Speed::Low); - let drogue_fire_b = Output::new(p.PD1, Level::Low, Speed::Low); - - let mut main_mcu_ematch_sense = p.PA2; - let mut main_mcu_ematch_sense_b = p.PB0; - - let mut drogue_mcu_ematch_sense = p.PA3; - let mut drogue_mcu_ematch_sense_b = p.PC5; - - let mut adc = Adc::new(p.ADC1); - - info!("Ejection enable measurement: {}", ejection_enable.get_level()); - - info!("ADC measurement main ematch {}", adc.blocking_read(&mut main_mcu_ematch_sense)); - info!("ADC measurement main B ematch {}", adc.blocking_read(&mut main_mcu_ematch_sense_b)); - info!("ADC measurement drogue ematch {}", adc.blocking_read(&mut drogue_mcu_ematch_sense)); - info!("ADC measurement drogue B ematch {}", adc.blocking_read(&mut drogue_mcu_ematch_sense_b)); - info!("ADC measurement main ematch {}", adc.blocking_read(&mut main_mcu_ematch_sense)); - - let recovery_manager = RecoveryManager { - arming: Arming { - main: main_arm_test, - drogue: drogue_arm_test, - main_b: main_arm_test_b, - drogue_b: drogue_arm_test_b, - }, - fire: Fire { - main: main_fire, - drogue: drogue_fire, - main_b: main_fire_b, - drogue_b: drogue_fire_b, - } - }; - - RECOVERY_MANAGER.lock(|cell| { + crate::resources::RECOVERY_MANAGER.lock(|cell| { *cell.borrow_mut() = Some(recovery_manager); }); // --- Camera Triggers --- - let mut cam_trigger = Output::new(p.PE14, Level::Low, Speed::Low); - let mut cam_trigger_b = Output::new(p.PE12, Level::Low, Speed::Low); - // power on - cam_trigger.set_high(); - Delay.delay_ms(2_000); - cam_trigger.set_low(); - Delay.delay_ms(100); - // trigger the camera - cam_trigger.set_high(); - Delay.delay_ms(500); - cam_trigger.set_low(); - Delay.delay_ms(10_000); - // stop recording - cam_trigger.set_high(); - Delay.delay_ms(500); - cam_trigger.set_low(); + let (cam_trigger, cam_trigger_b) = board.setup_camera_triggers(); + // moved sequence to drivers::camera to keep main lean (behavior unchanged) + drivers::camera::run_boot_and_record(cam_trigger, cam_trigger_b).await; // --- Buzzer 🐝 --- - let buzz_out_pin = PwmPin::new_ch1(p.PC6, OutputType::PushPull); - let mut pwm = SimplePwm::new(p.TIM3, Some(buzz_out_pin), None, None, None, khz(4), Default::default()); + let mut pwm = board.setup_buzzer(); let mut ch1 = pwm.ch1(); info!("Duty Cycle: {}", ch1.max_duty_cycle()); ch1.set_duty_cycle(ch1.max_duty_cycle() / 4); - ch1.enable(); + ch1.enable(); music::play_song(&mut pwm, music::TWINKLE_MELODY, 130).await; // // --- State Machine --- // let state_machine = StateMachine::new(traits::Context {}); - // // --- Radio --- - let mut uart_radio_config = UartConfig::default(); - uart_radio_config.baudrate = 57600; - uart_radio_config.data_bits = embassy_stm32::usart::DataBits::DataBits8; - uart_radio_config.parity = embassy_stm32::usart::Parity::ParityNone; - uart_radio_config.stop_bits = embassy_stm32::usart::StopBits::STOP1; + // // --- Radio --- + let (mut radio_tx, mut radio_ring_rx) = board.setup_radio(); - let mut uart_radio = Uart::new( - p.UART7, p.PE7, p.PE8, Irqs, p.DMA2_CH3, p.DMA2_CH5, uart_radio_config - ).unwrap(); - - let (mut radio_tx, mut radio_rx) = uart_radio.split(); - let mut radio_ring_rx = radio_rx.into_ring_buffered(unsafe { &mut RX_RADIO_BUF }); - - - // // --- AI --- - // // // Get a default device for the backend - // let device = BackendDevice::default(); - // - // // // Create a new model and load the state - // // let model: Model = Model::default(); - // - // // let output = run_model(&model, &device, 1.0); - // let recorder = CompactRecorder::new(); - // - // let record_bytes = include_bytes!("model/tte.mpk"); - // - // let record = recorder - // .load(record_bytes.as_ref(), &device) - // .expect("Failed to load recorder"); - // - // let model: model::LstmNetwork = config.model.init(&device).load_record(record); - // NOTE - // Creating multiple executor instances is supported, to run tasks with multiple priority levels. This allows higher-priority tasks to preempt lower-priority tasks. + // --- Inference --- + if features::ENABLE_INFERENCE { + spawner.must_spawn(inference::inference_task()); + } // --- Spawning Tasks --- - // spawner.must_spawn(led_blinker_task(p.PB14)); - - // spawner.must_spawn(uart_dma_reader_task(ring_rx)); - // spawner.must_spawn(uart_gps_dma_reader_task(ring_gps_rx, gps_tx)); - // spawner.must_spawn(sbg_parser_task(tx)); - // spawner.must_spawn(sbg_receiver_task()); - // spawner.must_spawn(baro_reader_task(baro)); - // spawner.must_spawn(ai_task()); + if features::ENABLE_LED { + spawner.must_spawn(tasks::led::led_blinker_task(board.take_led_pin())); + } + if let (true, Some(rx), Some(tx)) = (features::ENABLE_GPS, ring_gps_rx.take(), gps_tx.take()) { + spawner.must_spawn(tasks::gps::uart_gps_dma_reader_task(rx, tx)); + } + if features::ENABLE_SBG { + spawner.must_spawn(comm::sbg::sbg_uart_reader_task(ring_rx)); + spawner.must_spawn(comm::sbg::sbg_parser_task(tx)); + spawner.must_spawn(comm::sbg::sbg_receiver_task()); + } + if let (true, Some(baro_dev)) = (features::ENABLE_BARO, baro) { + spawner.must_spawn(tasks::baro::baro_reader_task(baro_dev)); + } // pass control of the spawner to the state machine - // spawner.must_spawn(sm_task(spawner, state_machine)); - spawner.must_spawn(radio_reader_task(radio_ring_rx)); - spawner.must_spawn(radio_writer_task(radio_tx)); + // spawner.must_spawn(tasks::state_machine::sm_task(spawner, state_machine)); + // temporary: compilable stub task until Context satisfies StateMachineContext + // spawner.must_spawn(tasks::state_machine::sm_task_stub()); + if features::ENABLE_RADIO { + spawner.must_spawn(comm::radio::radio_reader_task(radio_ring_rx)); + spawner.must_spawn(comm::radio::radio_writer_task(radio_tx)); + } } -// fn run_model<'a>(model: &Model, device: &BackendDevice, input: f32) -> Tensor { -// // Define the tensor -// let input = Tensor::::from_floats([[input]], &device); -// -// // Run the model on the input -// let output = model.forward(input); -// -// output -// } \ No newline at end of file +// See inference:: for the reference run_model helper moved from here. diff --git a/phoenix/src/resources.rs b/phoenix/src/resources.rs new file mode 100644 index 0000000..b306c29 --- /dev/null +++ b/phoenix/src/resources.rs @@ -0,0 +1,67 @@ +#![allow(dead_code)] + +use core::cell::RefCell; +use defmt::*; +use embassy_stm32::{bind_interrupts, peripherals, usart}; +use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +use embassy_sync::blocking_mutex::Mutex; +use embassy_sync::channel::Channel; +use embassy_sync::signal::Signal; +use embassy_time::Instant; +use static_cell::StaticCell; + +use sbg_rs::sbg::SBG_BUFFER_SIZE; + +// Global allocator +use embedded_alloc::LlffHeap as Heap; + +#[global_allocator] +pub static HEAP: Heap = Heap::empty(); + +// Shared channels and resources +pub type DmaBuffer = [u8; SBG_BUFFER_SIZE]; + +pub const GPS_BUFFER_SIZE: usize = 26; +pub const RADIO_BUFFER_SIZE: usize = 255; + +pub static PRESSURE_SIGNAL: Signal = Signal::new(); + +pub static SBG_CHANNEL: Channel = + Channel::new(); +pub static BUFFER_CHANNEL: Channel = Channel::new(); +// Note: EVENT_CHANNEL remains in main until the state machine is modularized +pub static COMMAND_CHANNEL: Channel< + CriticalSectionRawMutex, + messages_prost::command::command::Data, + 2, +> = Channel::new(); +pub static RADIO_CHANNEL: Channel = + Channel::new(); + +// DMA receive buffers in AXI SRAM +#[link_section = ".axisram.buffers"] +pub static mut RX_SBG_BUF: [u8; SBG_BUFFER_SIZE] = [0; SBG_BUFFER_SIZE]; +#[link_section = ".axisram.buffers"] +pub static mut RX_RADIO_BUF: [u8; SBG_BUFFER_SIZE] = [0; SBG_BUFFER_SIZE]; +#[link_section = ".axisram.buffers"] +pub static mut RX_GPS_BUF: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; + +// Optional shared SPI bus placeholder for future use (currently unused) +// pub static SPI_BUS: StaticCell>>> = +// StaticCell::new(); + +// RTC and Recovery manager shared handles +pub static RTC: Mutex>> = + Mutex::new(RefCell::new(None)); + +pub static RECOVERY_MANAGER: Mutex< + CriticalSectionRawMutex, + RefCell>, +> = Mutex::new(RefCell::new(None)); + +// USART IRQ bindings used across modules +bind_interrupts!(pub struct Irqs { + UART4 => usart::InterruptHandler; + UART8 => usart::InterruptHandler; + UART7 => usart::InterruptHandler; +}); diff --git a/phoenix/src/sbg_manager.rs b/phoenix/src/sbg_manager.rs index 191c657..1abe526 100644 --- a/phoenix/src/sbg_manager.rs +++ b/phoenix/src/sbg_manager.rs @@ -6,15 +6,13 @@ use core::ptr; // use crate::app::sbg_handle_data; // use crate::app::sbg_sd_task as sbg_sd; // use crate::app::sbg_write_data; -use super::RTC; -use crate::SBG_CHANNEL; +use crate::resources::{HEAP, RTC, SBG_CHANNEL}; use chrono::NaiveDateTime; use embassy_stm32::mode; use embassy_stm32::usart::UartTx; use heapless::Vec; use sbg_rs::sbg; use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; -use super::HEAP; // use stm32h7xx_hal::dma::dma::StreamX; // use stm32h7xx_hal::dma::{ // dma::{DmaConfig, StreamsTuple}, @@ -30,7 +28,6 @@ pub struct SBGManager { impl SBGManager { pub fn new(sbg_tx: UartTx<'static, mode::Async>) -> Self { - let sbg: sbg::SBG = sbg::SBG::new( |data| { sbg_handle_data(data); @@ -41,7 +38,7 @@ impl SBGManager { }, || sbg_get_time(), || { - // TODO: implement later + // TODO: implement later // sbg_flush::spawn().ok(); }, ); @@ -86,7 +83,7 @@ pub fn sbg_get_time() -> u32 { /// Publishes data to the SBG channel. pub fn sbg_handle_data(data: CallbackData) { - match data { + let _ = match data { CallbackData::Air(x) => SBG_CHANNEL.try_send(messages_prost::sensor::sbg::SbgData { data: Some(messages_prost::sensor::sbg::sbg_data::Data::Air(x)), }), diff --git a/phoenix/src/setup.rs b/phoenix/src/setup.rs new file mode 100644 index 0000000..0f0f46f --- /dev/null +++ b/phoenix/src/setup.rs @@ -0,0 +1 @@ +//! Deprecated: previous setup helpers moved into `board` module. diff --git a/phoenix/src/tasks/baro.rs b/phoenix/src/tasks/baro.rs new file mode 100644 index 0000000..a733f14 --- /dev/null +++ b/phoenix/src/tasks/baro.rs @@ -0,0 +1,70 @@ +use defmt::*; +use embassy_executor::task; +use embassy_stm32::{gpio::Output, spi::Spi}; +use embassy_time::Instant; +use embassy_time::{Duration, Timer}; +use heapless::HistoryBuffer; +use libm::powf; + +use common_arm::drivers::ms5611::{Ms5611, OversamplingRatio}; + +#[task] +pub async fn baro_reader_task( + mut baro: Ms5611< + Spi<'static, embassy_stm32::mode::Blocking>, + Output<'static>, + embassy_time::Delay, + >, +) { + info!("Barometer reader task started."); + const GROUND_HEIGHT: f32 = 300.0; // meters ASL + const ASCENT_LOCKOUT: f32 = 0.1; + const VALID_DESCENT_RATE: f32 = -0.005; // meters per millisecond + + let mut historical_barometer_altitude: HistoryBuffer<(f32, Instant), 20> = HistoryBuffer::new(); + + loop { + match baro.read_pressure_temperature(OversamplingRatio::Osr4096) { + Ok(reading) => { + let altitude = + ((powf(101.325 / reading.1, 1.0 / 5.257) - 1.0) * (25.0 + 273.15)) / 0.0065; + historical_barometer_altitude.write((altitude, Instant::now())); + + if historical_barometer_altitude.len() < 8 { + continue; + } + + let mut buf = historical_barometer_altitude.oldest_ordered(); + if let Some(mut prev_reading) = buf.next() { + let mut avg_sum: f32 = 0.0; + let mut datapoints_used = 0; + for current_reading in buf { + let time_diff = + current_reading.1.duration_since(prev_reading.1).as_millis(); + if time_diff == 0 { + continue; + } + let slope = (current_reading.0 - prev_reading.0) / time_diff as f32; + if slope > ASCENT_LOCKOUT { + continue; + } + avg_sum += slope; + datapoints_used += 1; + prev_reading = current_reading; + } + if datapoints_used > 0 { + let avg_slope = avg_sum / (datapoints_used as f32); + if avg_slope <= VALID_DESCENT_RATE { + info!( + "Apogee detected! Average vertical speed: {} m/s", + avg_slope * 1000.0 + ); + } + } + } + } + Err(_e) => {} + } + Timer::after(Duration::from_millis(100)).await; + } +} diff --git a/phoenix/src/tasks/gps.rs b/phoenix/src/tasks/gps.rs new file mode 100644 index 0000000..0187d17 --- /dev/null +++ b/phoenix/src/tasks/gps.rs @@ -0,0 +1,129 @@ +use defmt::*; +use embassy_executor::task; +use embassy_stm32::{mode, usart::RingBufferedUartRx, usart::UartTx}; +use embedded_hal_1::delay::DelayNs; +use ublox::UbxPacketRequest; + +#[task] +pub async fn uart_gps_dma_reader_task( + mut gps_rx: RingBufferedUartRx<'static>, + mut gps_tx: UartTx<'static, mode::Async>, +) { + info!("GPS DMA reader task spawned."); + let request = UbxPacketRequest::request_for::().into_packet_bytes(); + loop { + if crate::features::ENABLE_GPS_POLL { + let _ = gps_tx.write(&request).await; + } + + let mut buf_data: [u8; crate::resources::GPS_BUFFER_SIZE] = + [0; crate::resources::GPS_BUFFER_SIZE]; + let _ = gps_rx.read(&mut buf_data).await; + + if crate::features::ENABLE_GPS_PARSE { + // Minimal parser skeleton; extend as needed + let mut ublox_buf: [u8; 256] = [0; 256]; + let linear = ublox::FixedLinearBuffer::new(&mut ublox_buf[..]); + let mut parser = ublox::Parser::new(linear); + let mut msgs = parser.consume_ubx(&buf_data); + while let Some(msg) = msgs.next() { + match msg { + Ok(ublox::PacketRef::NavPosLlh(x)) => { + info!("GPS lat: {}, lon: {}", x.lat_degrees(), x.lon_degrees()); + } + Ok(_) => {} + Err(_) => info!("GPS parse error"), + } + } + } + + embassy_time::Delay.delay_ms(1000); + } +} + +// Reference: Verbose GPS polling + parsing example that used to live in main.rs. +// Kept here to avoid cluttering main while preserving the original logic. +// +// let request = +// UbxPacketRequest::request_for::().into_packet_bytes(); +// gps_tx.blocking_write(&request); +// +// loop { +// let request = +// UbxPacketRequest::request_for::().into_packet_bytes(); +// gps_tx.blocking_write(&request); +// // Delay.delay_ms(1000); +// let mut buf_data: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; +// ring_gps_rx.read(&mut buf_data).await; +// info!("GPS data read: {:?}", &buf_data[..]); +// // if let Ok(len) = gps_rx.read(&mut buf_data).await { +// // info!("read"); +// +// Delay.delay_ms(1000); +// // cortex_m::asm::delay(10_000); +// let mut buf: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; +// let bytes: [u8; GPS_BUFFER_SIZE] = [0; GPS_BUFFER_SIZE]; +// +// let mut nmea = nmea::Nmea::default(); +// let ascii_buf = unsafe {buf_data.as_ascii_unchecked()}; +// info!("BUFFER: {}", ascii_buf.as_str()); +// // if let Some(ascii_buf) = ascii_buf { +// let res = nmea.parse(ascii_buf.as_str()); +// +// match res { +// Ok(strings) => { +// info!("Result: {}", strings.as_str()); +// } +// _ => { +// info!("nmea parser none found"); +// } +// } +// +// // } else { +// // info!("No valid sentence"); +// // } +// +// let buf: ublox::FixedLinearBuffer<'_> = ublox::FixedLinearBuffer::new(&mut buf[..]); +// let mut parser = ublox::Parser::new(buf); +// // info!("GPS Parser initialized."); +// let mut msgs = parser.consume_ubx(&buf_data); +// info!("GPS Messages consumed. {}", msgs.next().is_some()); +// while let Some(msg) = msgs.next() { +// match msg { +// Ok(msg) => match msg { +// ublox::PacketRef::NavPosLlh(x) => { +// info!( +// "GPS latitude: {:?}, longitude {:?}", +// x.lat_degrees(), +// x.lon_degrees() +// ); +// } +// ublox::PacketRef::NavStatus(x) => { +// info!("GPS fix stat: {:?}", x.fix_stat_raw()); +// } +// ublox::PacketRef::NavDop(x) => { +// info!("GPS geometric drop: {:?}", x.geometric_dop()); +// } +// ublox::PacketRef::NavSat(x) => { +// info!("GPS num sats used: {:?}", x.num_svs()); +// } +// ublox::PacketRef::NavVelNed(x) => { +// info!("GPS velocity north: {:?}", x.vel_north()); +// } +// ublox::PacketRef::NavPvt(x) => { +// info!("GPS nun sats PVT: {:?}", x.num_satellites()); +// } +// ublox::PacketRef::Unknown(_msg) => { +// info!("Unknown GPS type."); +// } +// _ => { +// info!("GPS Message not handled."); +// } +// }, +// Err(_e) => { +// info!("GPS parse Error"); +// } +// } +// } +// // } +// } diff --git a/phoenix/src/tasks/led.rs b/phoenix/src/tasks/led.rs new file mode 100644 index 0000000..2a9f5d9 --- /dev/null +++ b/phoenix/src/tasks/led.rs @@ -0,0 +1,16 @@ +use defmt::*; +use embassy_executor::task; +use embassy_stm32::{gpio::Level, gpio::Output, gpio::Speed, peripherals}; +use embassy_time::Timer; + +#[task] +pub async fn led_blinker_task(pin: peripherals::PB14) { + let mut led = Output::new(pin, Level::High, Speed::Low); + info!("LED blinker task started."); + loop { + led.set_high(); + Timer::after_millis(500).await; + led.set_low(); + Timer::after_millis(500).await; + } +} diff --git a/phoenix/src/tasks/mod.rs b/phoenix/src/tasks/mod.rs new file mode 100644 index 0000000..bd676cc --- /dev/null +++ b/phoenix/src/tasks/mod.rs @@ -0,0 +1,4 @@ +pub mod baro; +pub mod gps; +pub mod led; +pub mod state_machine; diff --git a/phoenix/src/tasks/state_machine.rs b/phoenix/src/tasks/state_machine.rs new file mode 100644 index 0000000..6be7bce --- /dev/null +++ b/phoenix/src/tasks/state_machine.rs @@ -0,0 +1,50 @@ +use defmt::*; +use embassy_executor::task; +use embassy_time::{Duration, Timer}; +use smlang::statemachine; + +use crate::traits::Context; + +// Moved from main.rs for cleanliness. Behavior unchanged. +statemachine! { + transitions: { + *Init + Start = WaitForLaunch, + WaitForLaunch + Launch = Ascent, + Ascent + Apogee = Descent, + Descent + MainDeployment = Fuck, + Descent + DrogueDeployment = DrogueDescent, + DrogueDescent + MainDeployment = MainDescent, + MainDescent + NoMovement = Landed, + Fault + FaultCleared = _, + _ + FaultDetected = Fault, + } +} + +// Original signature (kept for reference; requires Context to implement StateMachineContext): +// #[task] +// pub async fn sm_task(_spawner: embassy_executor::Spawner, state_machine: StateMachine) { +// info!("State Machine task started."); +// loop { +// match state_machine.state { +// States::Ascent => {} +// States::Fault => {} +// States::Init => {} +// States::WaitForLaunch => {} +// States::Descent => {} +// States::DrogueDescent => {} +// States::Fuck => {} +// States::Landed => {} +// States::MainDescent => {} +// } +// Timer::after(Duration::from_millis(1000)).await; +// } +// } + +// Compilable stub until Context implements the required trait. Behavior preserved in comments above. +#[task] +pub async fn sm_task_stub() { + info!("State Machine task stub started."); + loop { + Timer::after(Duration::from_millis(1000)).await; + } +} diff --git a/phoenix/src/traits.rs b/phoenix/src/traits.rs index af319d2..1b01982 100644 --- a/phoenix/src/traits.rs +++ b/phoenix/src/traits.rs @@ -1,4 +1,4 @@ -use crate::{StateMachineContext, States, Events}; +use crate::{StateMachineContext, States}; use messages_prost::state::State; pub struct Context {} @@ -10,13 +10,13 @@ impl From for State { match value { States::Fuck => State::Abort, States::Init => State::Initializing, - States::Fault => State::Abort, + States::Fault => State::Abort, States::WaitForLaunch => State::WaitForTakeoff, States::Ascent => State::Ascent, - States::Descent => State::Descent, + States::Descent => State::Descent, States::DrogueDescent => State::Descent, States::MainDescent => State::TerminalDescent, - States::Landed => State::WaitForRecovery + States::Landed => State::WaitForRecovery, } } -} \ No newline at end of file +} From 7bdd2bed7b01a19cb6d3d188fecff6821ee7ce5a Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 11 Aug 2025 13:23:16 -0400 Subject: [PATCH 2/4] Clean up main --- Cargo.lock | 582 ++++++----------------------- phoenix/src/drivers/sd.rs | 29 +- phoenix/src/features.rs | 2 + phoenix/src/main.rs | 79 +--- phoenix/src/tasks/state_machine.rs | 38 +- phoenix/src/traits.rs | 2 +- 6 files changed, 165 insertions(+), 567 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 08e84c7..edbc2f4 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -125,12 +125,6 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" -[[package]] -name = "az" -version = "1.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" - [[package]] name = "bare-metal" version = "0.2.5" @@ -405,8 +399,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bb8f828a681946b07a87750ed0593d885e7b101653bd6a3bb1942976156bb48" dependencies = [ "derive-new 0.7.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -417,8 +411,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "12e9f07ccc658ef072bce2e996f0c38c80ee4c241598b6557afe1877dd87ae98" dependencies = [ "derive-new 0.7.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -434,8 +428,8 @@ dependencies = [ "half", "log", "onnx-ir", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "regex", "rust-format", "serde", @@ -593,8 +587,8 @@ version = "1.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4f154e572231cb6ba2bd1176980827e3d5dc04cc183a75dea38109fbdd672d29" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -797,17 +791,6 @@ dependencies = [ "volatile-register", ] -[[package]] -name = "cortex-m-log" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88c26033fe85d2c5f45a173a6dadf710db4a72eb7da81dbfb795d8d9ebfaaca7" -dependencies = [ - "cortex-m", - "cortex-m-semihosting", - "log", -] - [[package]] name = "cortex-m-rt" version = "0.7.5" @@ -823,8 +806,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e37549a379a9e0e6e576fd208ee60394ccb8be963889eebba3ffe0980364f472" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1087,8 +1070,8 @@ dependencies = [ "derive-new 0.6.0", "ident_case", "prettyplease", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1099,8 +1082,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6a89898212c1eaba0e2f0dffcadc9790b20b75d2ec8836da084370b043be2623" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1200,8 +1183,8 @@ checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "strsim", "syn 2.0.104", ] @@ -1213,7 +1196,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", - "quote 1.0.40", + "quote", "syn 2.0.104", ] @@ -1250,8 +1233,8 @@ checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1292,8 +1275,8 @@ version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1303,8 +1286,8 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d150dea618e920167e5973d70ae6ece4385b7164e0d799fe7c122dd0a5d912ad" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1314,8 +1297,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2cdc8d50f426189eef89dac62fabfa0abb27d5cc008f25bf4156a0203325becc" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1325,8 +1308,8 @@ version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "30542c1ad912e0e3d22a1935c290e12e8a29d704a420177a31faad4a601a0800" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1337,8 +1320,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6edb4b64a43d977b8e99788fe3a04d483834fba1215a7e02caa415b626497f7f" dependencies = [ "convert_case", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rustc_version 0.4.1", "syn 2.0.104", ] @@ -1358,10 +1341,10 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cb7330aeadfbe296029522e6c40f315320aba36fc43a5b3632f3795348f3bd22" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", - "unicode-xid 0.2.6", + "unicode-xid", ] [[package]] @@ -1391,8 +1374,8 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "97369cbbc041bc366949bc74d34658d6cda5621039731c6310521892a3a20ae0" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1424,16 +1407,6 @@ dependencies = [ "bytemuck", ] -[[package]] -name = "eg-seven-segment" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e7d2f54ed234b333c71865c641906175a0d58bf366534ba5b8ded74b524b87b" -dependencies = [ - "bitflags 2.9.1", - "embedded-graphics", -] - [[package]] name = "either" version = "1.15.0" @@ -1496,8 +1469,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1604,10 +1577,10 @@ dependencies = [ "embedded-storage-async", "futures-util", "nb 1.1.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rand_core 0.6.4", - "sdio-host 0.5.0", + "sdio-host", "static_assertions", "stm32-fmc", "stm32-metapac 16.0.0", @@ -1739,12 +1712,6 @@ dependencies = [ "nb 1.1.0", ] -[[package]] -name = "embedded-display-controller" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abdb518d44a2eda3b8a5dfc27b60a4bbbf2bb87eee436796d54c290ee6cc6545" - [[package]] name = "embedded-dma" version = "0.2.0" @@ -1754,29 +1721,6 @@ dependencies = [ "stable_deref_trait", ] -[[package]] -name = "embedded-graphics" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0649998afacf6d575d126d83e68b78c0ab0e00ca2ac7e9b3db11b4cbe8274ef0" -dependencies = [ - "az", - "byteorder", - "embedded-graphics-core", - "float-cmp", - "micromath", -] - -[[package]] -name = "embedded-graphics-core" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ba9ecd261f991856250d2207f6d8376946cd9f412a2165d3b75bc87a0bc7a044" -dependencies = [ - "az", - "byteorder", -] - [[package]] name = "embedded-hal" version = "0.2.7" @@ -1861,17 +1805,6 @@ dependencies = [ "embedded-nal", ] -[[package]] -name = "embedded-sdmmc" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2f4d14180a76a8af24a45a0e1a4f9c97491b05a3b962d59d5e4ce0e6ab103736" -dependencies = [ - "byteorder", - "embedded-hal 0.2.7", - "log", -] - [[package]] name = "embedded-sdmmc" version = "0.9.0" @@ -1913,8 +1846,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1e6a265c649f3f5979b601d26f1d05ada116434c87741c9493cb56218f76cbc" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1991,15 +1924,6 @@ dependencies = [ "miniz_oxide", ] -[[package]] -name = "float-cmp" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "98de4bbd547a563b716d8dfa9aad1cb19bfab00f4fa09a6a4ed21dbcf44ce9c4" -dependencies = [ - "num-traits", -] - [[package]] name = "float-ord" version = "0.3.2" @@ -2034,8 +1958,8 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -2673,9 +2597,6 @@ name = "lazy_static" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" -dependencies = [ - "spin 0.9.8", -] [[package]] name = "libc" @@ -2792,8 +2713,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8cd48b535b9b37a25a2589ab8d4f997886a2c68f59960ce06588525f38dd4944" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -2879,9 +2800,9 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "crc-any", "lazy_static", - "proc-macro2 1.0.95", + "proc-macro2", "quick-xml", - "quote 1.0.40", + "quote", "thiserror 1.0.69", ] @@ -3120,8 +3041,8 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] @@ -3193,17 +3114,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "77e878c846a8abae00dd069496dbe8751b16ac1c3d6bd2a7283a938e8228f90d" dependencies = [ "proc-macro-crate", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] -[[package]] -name = "numtoa" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6aa2c4e539b869820a2b82e1aef6ff40aa85e65decdd5185e83fb4b1249cd00f" - [[package]] name = "objc" version = "0.2.7" @@ -3250,37 +3165,12 @@ dependencies = [ "num-traits", ] -[[package]] -name = "otm8009a" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e56059a2e83de4409e5fa0854d3ae891b0f949af86c74ccf76a8bffe4ce6dc6d" -dependencies = [ - "embedded-display-controller", - "embedded-hal 0.2.7", -] - [[package]] name = "overload" version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b15813163c1d831bf4a13c3610c05c0d03b39feb07f7e09fa234dac9b15aaf39" -[[package]] -name = "panic-halt" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" - -[[package]] -name = "panic-itm" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d577d97d1b31268087b6dddf2470e6794ef5eee87d9dca7fcd0481695391a4c" -dependencies = [ - "cortex-m", -] - [[package]] name = "panic-probe" version = "0.3.2" @@ -3291,26 +3181,6 @@ dependencies = [ "defmt 0.3.100", ] -[[package]] -name = "panic-rtt-target" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d6ab67bc881453e4c90f958c657c1303670ea87bc1a16e87fd71a40f656dce9" -dependencies = [ - "cortex-m", - "rtt-target 0.3.1", -] - -[[package]] -name = "panic-semihosting" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ee8a3e1233d9073d76a870223512ce4eeea43c067a94a445c13bd6d792d7b1ab" -dependencies = [ - "cortex-m", - "cortex-m-semihosting", -] - [[package]] name = "parking" version = "2.2.1" @@ -3384,7 +3254,7 @@ dependencies = [ "embedded-hal-bus", "embedded-io-async", "embedded-nal-async", - "embedded-sdmmc 0.9.0", + "embedded-sdmmc", "embedded-storage", "fdcan", "grounded", @@ -3471,7 +3341,7 @@ version = "0.2.36" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ff24dfcda44452b9816fff4cd4227e1bb73ff5a2f1bc1105aa92fb8565ce44d2" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", "syn 2.0.104", ] @@ -3490,8 +3360,8 @@ version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", ] [[package]] @@ -3501,20 +3371,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" dependencies = [ "proc-macro-error-attr2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] -[[package]] -name = "proc-macro2" -version = "0.4.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf3d2011ab5c909338f7887f4fc896d35932e29146c12c8d01da6b22a80ba759" -dependencies = [ - "unicode-xid 0.1.0", -] - [[package]] name = "proc-macro2" version = "1.0.95" @@ -3530,37 +3391,6 @@ version = "1.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3eb8486b569e12e2c32ad3e204dbaba5e4b5b216e9367044f25f1dba42341773" -[[package]] -name = "proptest" -version = "1.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fcdab19deb5195a31cf7726a210015ff1496ba1464fd42cb4f537b8b01b471f" -dependencies = [ - "bit-set", - "bit-vec", - "bitflags 2.9.1", - "lazy_static", - "num-traits", - "rand 0.9.2", - "rand_chacha 0.9.0", - "rand_xorshift", - "regex-syntax 0.8.5", - "rusty-fork", - "tempfile", - "unarray", -] - -[[package]] -name = "proptest-derive" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90b46295382dc76166cb7cf2bb4a97952464e4b7ed5a43e6cd34e1fec3349ddc" -dependencies = [ - "proc-macro2 0.4.30", - "quote 0.6.13", - "syn 0.15.44", -] - [[package]] name = "prost" version = "0.14.1" @@ -3599,8 +3429,8 @@ checksum = "9120690fafc389a67ba3803df527d0ec9cbbc9cc45e4cc20b332996dfb672425" dependencies = [ "anyhow", "itertools", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -3691,12 +3521,6 @@ dependencies = [ "version_check", ] -[[package]] -name = "quick-error" -version = "1.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" - [[package]] name = "quick-xml" version = "0.26.0" @@ -3706,22 +3530,13 @@ dependencies = [ "memchr", ] -[[package]] -name = "quote" -version = "0.6.13" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6ce23b6b870e8f94f81fb0a363d65d86675884b34a09043c81e5562f11c1f8e1" -dependencies = [ - "proc-macro2 0.4.30", -] - [[package]] name = "quote" version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", ] [[package]] @@ -3805,15 +3620,6 @@ dependencies = [ "rand 0.9.2", ] -[[package]] -name = "rand_xorshift" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "513962919efc330f829edb2535844d1b912b0fbe2ca165d613e4e8788bb05a5a" -dependencies = [ - "rand_core 0.9.3", -] - [[package]] name = "range-alloc" version = "0.1.4" @@ -4018,8 +3824,8 @@ checksum = "f387b12bd6c01d2c9d4776dddeefaf0ae51b9497c83c0186b1693f6821ff3c4a" dependencies = [ "indexmap", "proc-macro-error2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4033,8 +3839,8 @@ dependencies = [ "cortex-m", "fugit", "portable-atomic", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rtic-time", "stm32-metapac 15.0.0", ] @@ -4069,32 +3875,13 @@ dependencies = [ "rtic-common", ] -[[package]] -name = "rtt-target" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "065d6058bb1204f51a562a67209e1817cf714759d5cf845aa45c75fa7b0b9d9b" -dependencies = [ - "ufmt-write", -] - -[[package]] -name = "rtt-target" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3afa12c77ba1b9bf560e4039a9b9a08bb9cde0e9e6923955eeb917dd8d5cf303" -dependencies = [ - "critical-section", - "ufmt-write", -] - [[package]] name = "rust-format" version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60e7c00b6c3bf5e38a880eec01d7e829d12ca682079f8238a464def3c4b31627" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", "syn 1.0.109", ] @@ -4154,18 +3941,6 @@ version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" -[[package]] -name = "rusty-fork" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb3dcc6e454c328bb824492db107ab7c0ae8fcffe4ad210136ef014458c1bc4f" -dependencies = [ - "fnv", - "quick-error", - "tempfile", - "wait-timeout", -] - [[package]] name = "ryu" version = "1.0.20" @@ -4236,12 +4011,6 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" -[[package]] -name = "sdio-host" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" - [[package]] name = "semver" version = "0.9.0" @@ -4293,8 +4062,8 @@ version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4343,7 +4112,7 @@ dependencies = [ "rtic", "rtic-monotonics", "rtic-sync", - "stm32h7xx-hal 0.16.0 (git+https://github.com/uorocketry/stm32h7xx-hal)", + "stm32h7xx-hal", ] [[package]] @@ -4376,8 +4145,8 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "231b4425dcc43afc7e18c34e7c6738cd252d42d91d909c948df14107c9ae79f1" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "string_morph", "syn 1.0.109", ] @@ -4496,56 +4265,6 @@ dependencies = [ "vcell", ] -[[package]] -name = "stm32h7xx-hal" -version = "0.16.0" -dependencies = [ - "bare-metal 1.0.0", - "cast", - "cfg-if", - "chrono 0.4.38", - "cortex-m", - "cortex-m-log", - "cortex-m-rt", - "cortex-m-semihosting", - "defmt 0.3.100", - "eg-seven-segment", - "embedded-display-controller", - "embedded-dma", - "embedded-graphics", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-sdmmc 0.5.0", - "embedded-storage", - "fdcan", - "fugit", - "lazy_static", - "log", - "nb 1.1.0", - "numtoa", - "otm8009a", - "panic-halt", - "panic-itm", - "panic-rtt-target", - "panic-semihosting", - "paste", - "proptest", - "proptest-derive", - "rand_core 0.6.4", - "rtic", - "rtt-target 0.4.0", - "sdio-host 0.9.0", - "serde", - "smoltcp", - "stm32-fmc", - "stm32h7", - "synopsys-usb-otg", - "tinybmp", - "usb-device", - "usbd-serial", - "void", -] - [[package]] name = "stm32h7xx-hal" version = "0.16.0" @@ -4607,8 +4326,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rustversion", "syn 2.0.104", ] @@ -4620,8 +4339,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7695ce3845ea4b33927c055a39dc438a45b059f7c1b3d91d38d10355fb8cbca7" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4632,31 +4351,20 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f2c04b93fc15d79b39c63218f15e3fdffaa4c227830686e3b7c5f41244eb3e50" dependencies = [ "base64", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", "unicode-width 0.1.14", ] -[[package]] -name = "syn" -version = "0.15.44" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ca4b3b69a77cbe1ffc9e198781b7acb0c7365a883670e8f1c1bc66fba79a5c5" -dependencies = [ - "proc-macro2 0.4.30", - "quote 0.6.13", - "unicode-xid 0.1.0", -] - [[package]] name = "syn" version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "unicode-ident", ] @@ -4666,31 +4374,19 @@ version = "2.0.104" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17b6f705963418cdb9927482fa304bc562ece2fdd4f616084c50b7023b435a40" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "unicode-ident", ] -[[package]] -name = "synopsys-usb-otg" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe" -dependencies = [ - "cortex-m", - "embedded-hal 0.2.7", - "usb-device", - "vcell", -] - [[package]] name = "synstructure" version = "0.13.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4779,8 +4475,8 @@ version = "1.0.69" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4790,8 +4486,8 @@ version = "2.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4813,15 +4509,6 @@ dependencies = [ "cfg-if", ] -[[package]] -name = "tinybmp" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "197cc000e382175ff15abd9c54c694ef80ef20cb07e7f956c71e3ea97fc8dc60" -dependencies = [ - "embedded-graphics", -] - [[package]] name = "toml_datetime" version = "0.6.11" @@ -4856,8 +4543,8 @@ version = "0.1.30" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "81383ab64e72a7a8b8e13130c49e3dab29def6d0c7d76a03087b3cf71c5c6903" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4937,8 +4624,8 @@ name = "ublox_derive" version = "0.1.0" source = "git+https://github.com/uorocketry/ublox#286d43a3ae9a3ba30a04be04c65bf89aca6b1bfe" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] @@ -4948,17 +4635,11 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290ba2a4bfe92acb91bb9c0cdae32e9a1cda9cfe5b3329eaf9a444c522512de9" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] -[[package]] -name = "ufmt-write" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69" - [[package]] name = "ug" version = "0.1.0" @@ -4980,12 +4661,6 @@ dependencies = [ "yoke", ] -[[package]] -name = "unarray" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" - [[package]] name = "unicode-ident" version = "1.0.18" @@ -5004,12 +4679,6 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4a1a07cc7db3810833284e8d372ccdc6da29741639ecc70c9ec107df0fa6154c" -[[package]] -name = "unicode-xid" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" - [[package]] name = "unicode-xid" version = "0.2.6" @@ -5062,25 +4731,13 @@ dependencies = [ "byteorder", "hashbrown 0.13.2", "log", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "serde", "syn 1.0.109", "usbd-hid-descriptors", ] -[[package]] -name = "usbd-serial" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "065e4eaf93db81d5adac82d9cef8f8da314cb640fa7f89534b972383f1cf80fc" -dependencies = [ - "embedded-hal 0.2.7", - "embedded-io", - "nb 1.1.0", - "usb-device", -] - [[package]] name = "uuid" version = "1.17.0" @@ -5099,8 +4756,8 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "41b6d82be61465f97d42bd1d15bf20f3b0a3a0905018f38f9d6f6962055b0b5c" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5131,15 +4788,6 @@ dependencies = [ "vcell", ] -[[package]] -name = "wait-timeout" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "09ac3b126d3914f9849036f826e054cbabdc8519970b8998ddaf3b5bd3c65f11" -dependencies = [ - "libc", -] - [[package]] name = "walkdir" version = "2.5.0" @@ -5185,8 +4833,8 @@ checksum = "2f0a0651a5c2bc21487bde11ee802ccaf4c51935d0d3d42a6101f98161700bc6" dependencies = [ "bumpalo", "log", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "wasm-bindgen-shared", ] @@ -5210,7 +4858,7 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7fe63fc6d09ed3792bd0897b314f53de8e16568c2b3f7982f468c0bf9bd0b407" dependencies = [ - "quote 1.0.40", + "quote", "wasm-bindgen-macro-support", ] @@ -5220,8 +4868,8 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "wasm-bindgen-backend", "wasm-bindgen-shared", @@ -5521,8 +5169,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bbd5b46c938e506ecbce286b6628a02171d56153ba733b6c741fc627ec9579b" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5532,8 +5180,8 @@ version = "0.60.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a47fddd13af08290e67f4acabf4b459f647552718f683a7b415d290ac744a836" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5543,8 +5191,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "053c4c462dc91d3b1504c6fe5a726dd15e216ba718e84a0e46a88fbe5ded3515" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5554,8 +5202,8 @@ version = "0.59.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd9211b69f8dcdfa817bfd14bf1c97c9188afa36f4750130fcdf3f400eca9fa8" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5876,8 +5524,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2380878cad4ac9aac1e2435f3eb4020e8374b5f13c296cb75b4620ff8e229154" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "synstructure", ] @@ -5897,8 +5545,8 @@ version = "0.8.26" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9ecf5b4cc5364572d7f4c329661bcc82724222973f2cab6f050a4e5c22f75181" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5917,8 +5565,8 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d71e5d6e06ab090c67b5e44993ec16b72dcbaabc526db883a360057678b48502" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "synstructure", ] diff --git a/phoenix/src/drivers/sd.rs b/phoenix/src/drivers/sd.rs index 9015282..d8e695e 100644 --- a/phoenix/src/drivers/sd.rs +++ b/phoenix/src/drivers/sd.rs @@ -5,6 +5,31 @@ use embassy_time::Delay; use embedded_hal_1::delay::DelayNs; use embedded_hal_bus::spi::RefCellDevice; use embedded_sdmmc::{Mode, SdCard, VolumeIdx, VolumeManager}; +use core::marker::PhantomData; + +/// Minimal time source for embedded-sdmmc +pub struct TimeSink { + pub(crate) _marker: PhantomData<*const ()>, +} + +impl TimeSink { + pub fn new() -> Self { + Self { _marker: PhantomData } + } +} + +impl embedded_sdmmc::TimeSource for TimeSink { + fn get_timestamp(&self) -> embedded_sdmmc::Timestamp { + embedded_sdmmc::Timestamp { + year_since_1970: 0, + zero_indexed_month: 0, + zero_indexed_day: 0, + hours: 0, + minutes: 0, + seconds: 0, + } + } +} // Initializes the SD card and performs the small demo previously in main.rs. // Returns Ok(()) if initialization and simple access succeed. @@ -18,9 +43,7 @@ pub fn init_and_demo( info!("Card size is {} bytes", sdcard.num_bytes().map_err(|_| ())?); let volume_mgr = VolumeManager::new( sdcard, - super::super::TimeSink { - _marker: core::marker::PhantomData, - }, + TimeSink::new(), ); let _volume0 = volume_mgr.open_volume(VolumeIdx(0)).map_err(|_| ())?; // let root_dir = volume0.open_root_dir().map_err(|_| ())?; diff --git a/phoenix/src/features.rs b/phoenix/src/features.rs index ea4da5e..1c6cfbc 100644 --- a/phoenix/src/features.rs +++ b/phoenix/src/features.rs @@ -3,6 +3,7 @@ pub const ENABLE_LED: bool = false; pub const ENABLE_SBG: bool = false; +// TODO: remove/condense these GPS features pub const ENABLE_GPS: bool = false; pub const ENABLE_GPS_POLL: bool = false; pub const ENABLE_GPS_PARSE: bool = false; @@ -13,3 +14,4 @@ pub const ENABLE_BARO: bool = false; pub const ENABLE_RADIO: bool = true; pub const ENABLE_INFERENCE: bool = false; pub const ENABLE_SD: bool = false; +pub const ENABLE_STATE_MACHINE: bool = true; diff --git a/phoenix/src/main.rs b/phoenix/src/main.rs index 79d52b0..663b87d 100644 --- a/phoenix/src/main.rs +++ b/phoenix/src/main.rs @@ -25,7 +25,6 @@ use burn::{ record::{BinBytesRecorder, FullPrecisionSettings, Recorder}, // <-- FIX: Use BinBytesRecorder }; use core::cell::RefCell; -use core::marker::PhantomData; use defmt::*; use embassy_executor::Spawner; use embassy_stm32::gpio::{Input, Level, Output, OutputType, Pull, Speed}; @@ -83,69 +82,6 @@ type BackendDevice = ::Device; use crate::resources::{Irqs, BUFFER_CHANNEL, HEAP, RX_GPS_BUF, RX_RADIO_BUF, RX_SBG_BUF}; -statemachine! { - transitions: { - *Init + Start = WaitForLaunch, - WaitForLaunch + Launch = Ascent, - Ascent + Apogee = Descent, - Descent + MainDeployment = Fuck, - Descent + DrogueDeployment = DrogueDescent, - DrogueDescent + MainDeployment = MainDescent, - MainDescent + NoMovement = Landed, - Fault + FaultCleared = _, - _ + FaultDetected = Fault, - } -} - -pub struct TimeSink { - _marker: PhantomData<*const ()>, -} - -impl TimeSink { - fn new() -> Self { - TimeSink { - _marker: PhantomData, - } - } -} - -impl embedded_sdmmc::TimeSource for TimeSink { - fn get_timestamp(&self) -> embedded_sdmmc::Timestamp { - embedded_sdmmc::Timestamp { - year_since_1970: 0, - zero_indexed_month: 0, - zero_indexed_day: 0, - hours: 0, - minutes: 0, - seconds: 0, - } - } -} - -// ================================================================================= -// Application Tasks -// ================================================================================= - -#[embassy_executor::task] -async fn sm_task(spawner: Spawner, state_machine: StateMachine) { - info!("State Machine task started."); - - loop { - match state_machine.state { - States::Ascent => {} - States::Fault => {} - States::Init => {} - States::WaitForLaunch => {} - States::Descent => {} - States::DrogueDescent => {} - States::Fuck => {} - States::Landed => {} - States::MainDescent => {} - } - Timer::after(Duration::from_millis(1000)).await; - } -} - // ================================================================================= // Main Entry Point // ================================================================================= @@ -304,10 +240,13 @@ async fn main(spawner: Spawner) { ch1.enable(); music::play_song(&mut pwm, music::TWINKLE_MELODY, 130).await; - // // --- State Machine --- - // let state_machine = StateMachine::new(traits::Context {}); + // --- State Machine --- + if features::ENABLE_STATE_MACHINE { + let state_machine = tasks::state_machine::StateMachine::new(traits::Context {}); + spawner.must_spawn(tasks::state_machine::sm_task(spawner, state_machine)); + } - // // --- Radio --- + // --- Radio --- let (mut radio_tx, mut radio_ring_rx) = board.setup_radio(); // --- Inference --- @@ -332,12 +271,8 @@ async fn main(spawner: Spawner) { } // pass control of the spawner to the state machine // spawner.must_spawn(tasks::state_machine::sm_task(spawner, state_machine)); - // temporary: compilable stub task until Context satisfies StateMachineContext - // spawner.must_spawn(tasks::state_machine::sm_task_stub()); if features::ENABLE_RADIO { spawner.must_spawn(comm::radio::radio_reader_task(radio_ring_rx)); spawner.must_spawn(comm::radio::radio_writer_task(radio_tx)); } -} - -// See inference:: for the reference run_model helper moved from here. +} \ No newline at end of file diff --git a/phoenix/src/tasks/state_machine.rs b/phoenix/src/tasks/state_machine.rs index 6be7bce..506c1f8 100644 --- a/phoenix/src/tasks/state_machine.rs +++ b/phoenix/src/tasks/state_machine.rs @@ -5,7 +5,7 @@ use smlang::statemachine; use crate::traits::Context; -// Moved from main.rs for cleanliness. Behavior unchanged. +// Single source of truth for the state machine statemachine! { transitions: { *Init + Start = WaitForLaunch, @@ -20,31 +20,21 @@ statemachine! { } } -// Original signature (kept for reference; requires Context to implement StateMachineContext): -// #[task] -// pub async fn sm_task(_spawner: embassy_executor::Spawner, state_machine: StateMachine) { -// info!("State Machine task started."); -// loop { -// match state_machine.state { -// States::Ascent => {} -// States::Fault => {} -// States::Init => {} -// States::WaitForLaunch => {} -// States::Descent => {} -// States::DrogueDescent => {} -// States::Fuck => {} -// States::Landed => {} -// States::MainDescent => {} -// } -// Timer::after(Duration::from_millis(1000)).await; -// } -// } - -// Compilable stub until Context implements the required trait. Behavior preserved in comments above. #[task] -pub async fn sm_task_stub() { - info!("State Machine task stub started."); +pub async fn sm_task(_spawner: embassy_executor::Spawner, mut state_machine: StateMachine) { + info!("State Machine task started."); loop { + match state_machine.state { + States::Ascent => {} + States::Fault => {} + States::Init => {} + States::WaitForLaunch => {} + States::Descent => {} + States::DrogueDescent => {} + States::Fuck => {} + States::Landed => {} + States::MainDescent => {} + } Timer::after(Duration::from_millis(1000)).await; } } diff --git a/phoenix/src/traits.rs b/phoenix/src/traits.rs index 1b01982..0750811 100644 --- a/phoenix/src/traits.rs +++ b/phoenix/src/traits.rs @@ -1,4 +1,4 @@ -use crate::{StateMachineContext, States}; +use crate::tasks::state_machine::{StateMachineContext, States}; use messages_prost::state::State; pub struct Context {} From 63166d48d7916ce151e2ed8e5f1cbe10ff7b1c48 Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 11 Aug 2025 13:23:16 -0400 Subject: [PATCH 3/4] Clean up main --- Cargo.lock | 582 ++++++----------------------- phoenix/src/drivers/sd.rs | 29 +- phoenix/src/features.rs | 2 + phoenix/src/main.rs | 96 +---- phoenix/src/tasks/state_machine.rs | 38 +- phoenix/src/traits.rs | 2 +- 6 files changed, 166 insertions(+), 583 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 08e84c7..edbc2f4 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -125,12 +125,6 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" -[[package]] -name = "az" -version = "1.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" - [[package]] name = "bare-metal" version = "0.2.5" @@ -405,8 +399,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bb8f828a681946b07a87750ed0593d885e7b101653bd6a3bb1942976156bb48" dependencies = [ "derive-new 0.7.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -417,8 +411,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "12e9f07ccc658ef072bce2e996f0c38c80ee4c241598b6557afe1877dd87ae98" dependencies = [ "derive-new 0.7.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -434,8 +428,8 @@ dependencies = [ "half", "log", "onnx-ir", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "regex", "rust-format", "serde", @@ -593,8 +587,8 @@ version = "1.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4f154e572231cb6ba2bd1176980827e3d5dc04cc183a75dea38109fbdd672d29" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -797,17 +791,6 @@ dependencies = [ "volatile-register", ] -[[package]] -name = "cortex-m-log" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88c26033fe85d2c5f45a173a6dadf710db4a72eb7da81dbfb795d8d9ebfaaca7" -dependencies = [ - "cortex-m", - "cortex-m-semihosting", - "log", -] - [[package]] name = "cortex-m-rt" version = "0.7.5" @@ -823,8 +806,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e37549a379a9e0e6e576fd208ee60394ccb8be963889eebba3ffe0980364f472" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1087,8 +1070,8 @@ dependencies = [ "derive-new 0.6.0", "ident_case", "prettyplease", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1099,8 +1082,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6a89898212c1eaba0e2f0dffcadc9790b20b75d2ec8836da084370b043be2623" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1200,8 +1183,8 @@ checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" dependencies = [ "fnv", "ident_case", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "strsim", "syn 2.0.104", ] @@ -1213,7 +1196,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" dependencies = [ "darling_core", - "quote 1.0.40", + "quote", "syn 2.0.104", ] @@ -1250,8 +1233,8 @@ checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", "proc-macro-error2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1292,8 +1275,8 @@ version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1303,8 +1286,8 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d150dea618e920167e5973d70ae6ece4385b7164e0d799fe7c122dd0a5d912ad" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1314,8 +1297,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2cdc8d50f426189eef89dac62fabfa0abb27d5cc008f25bf4156a0203325becc" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1325,8 +1308,8 @@ version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "30542c1ad912e0e3d22a1935c290e12e8a29d704a420177a31faad4a601a0800" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1337,8 +1320,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6edb4b64a43d977b8e99788fe3a04d483834fba1215a7e02caa415b626497f7f" dependencies = [ "convert_case", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rustc_version 0.4.1", "syn 2.0.104", ] @@ -1358,10 +1341,10 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cb7330aeadfbe296029522e6c40f315320aba36fc43a5b3632f3795348f3bd22" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", - "unicode-xid 0.2.6", + "unicode-xid", ] [[package]] @@ -1391,8 +1374,8 @@ version = "0.2.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "97369cbbc041bc366949bc74d34658d6cda5621039731c6310521892a3a20ae0" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1424,16 +1407,6 @@ dependencies = [ "bytemuck", ] -[[package]] -name = "eg-seven-segment" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e7d2f54ed234b333c71865c641906175a0d58bf366534ba5b8ded74b524b87b" -dependencies = [ - "bitflags 2.9.1", - "embedded-graphics", -] - [[package]] name = "either" version = "1.15.0" @@ -1496,8 +1469,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1604,10 +1577,10 @@ dependencies = [ "embedded-storage-async", "futures-util", "nb 1.1.0", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rand_core 0.6.4", - "sdio-host 0.5.0", + "sdio-host", "static_assertions", "stm32-fmc", "stm32-metapac 16.0.0", @@ -1739,12 +1712,6 @@ dependencies = [ "nb 1.1.0", ] -[[package]] -name = "embedded-display-controller" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abdb518d44a2eda3b8a5dfc27b60a4bbbf2bb87eee436796d54c290ee6cc6545" - [[package]] name = "embedded-dma" version = "0.2.0" @@ -1754,29 +1721,6 @@ dependencies = [ "stable_deref_trait", ] -[[package]] -name = "embedded-graphics" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0649998afacf6d575d126d83e68b78c0ab0e00ca2ac7e9b3db11b4cbe8274ef0" -dependencies = [ - "az", - "byteorder", - "embedded-graphics-core", - "float-cmp", - "micromath", -] - -[[package]] -name = "embedded-graphics-core" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ba9ecd261f991856250d2207f6d8376946cd9f412a2165d3b75bc87a0bc7a044" -dependencies = [ - "az", - "byteorder", -] - [[package]] name = "embedded-hal" version = "0.2.7" @@ -1861,17 +1805,6 @@ dependencies = [ "embedded-nal", ] -[[package]] -name = "embedded-sdmmc" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2f4d14180a76a8af24a45a0e1a4f9c97491b05a3b962d59d5e4ce0e6ab103736" -dependencies = [ - "byteorder", - "embedded-hal 0.2.7", - "log", -] - [[package]] name = "embedded-sdmmc" version = "0.9.0" @@ -1913,8 +1846,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1e6a265c649f3f5979b601d26f1d05ada116434c87741c9493cb56218f76cbc" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -1991,15 +1924,6 @@ dependencies = [ "miniz_oxide", ] -[[package]] -name = "float-cmp" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "98de4bbd547a563b716d8dfa9aad1cb19bfab00f4fa09a6a4ed21dbcf44ce9c4" -dependencies = [ - "num-traits", -] - [[package]] name = "float-ord" version = "0.3.2" @@ -2034,8 +1958,8 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -2673,9 +2597,6 @@ name = "lazy_static" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" -dependencies = [ - "spin 0.9.8", -] [[package]] name = "libc" @@ -2792,8 +2713,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8cd48b535b9b37a25a2589ab8d4f997886a2c68f59960ce06588525f38dd4944" dependencies = [ "darling", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -2879,9 +2800,9 @@ source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae5 dependencies = [ "crc-any", "lazy_static", - "proc-macro2 1.0.95", + "proc-macro2", "quick-xml", - "quote 1.0.40", + "quote", "thiserror 1.0.69", ] @@ -3120,8 +3041,8 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] @@ -3193,17 +3114,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "77e878c846a8abae00dd069496dbe8751b16ac1c3d6bd2a7283a938e8228f90d" dependencies = [ "proc-macro-crate", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] -[[package]] -name = "numtoa" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6aa2c4e539b869820a2b82e1aef6ff40aa85e65decdd5185e83fb4b1249cd00f" - [[package]] name = "objc" version = "0.2.7" @@ -3250,37 +3165,12 @@ dependencies = [ "num-traits", ] -[[package]] -name = "otm8009a" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e56059a2e83de4409e5fa0854d3ae891b0f949af86c74ccf76a8bffe4ce6dc6d" -dependencies = [ - "embedded-display-controller", - "embedded-hal 0.2.7", -] - [[package]] name = "overload" version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b15813163c1d831bf4a13c3610c05c0d03b39feb07f7e09fa234dac9b15aaf39" -[[package]] -name = "panic-halt" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" - -[[package]] -name = "panic-itm" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d577d97d1b31268087b6dddf2470e6794ef5eee87d9dca7fcd0481695391a4c" -dependencies = [ - "cortex-m", -] - [[package]] name = "panic-probe" version = "0.3.2" @@ -3291,26 +3181,6 @@ dependencies = [ "defmt 0.3.100", ] -[[package]] -name = "panic-rtt-target" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d6ab67bc881453e4c90f958c657c1303670ea87bc1a16e87fd71a40f656dce9" -dependencies = [ - "cortex-m", - "rtt-target 0.3.1", -] - -[[package]] -name = "panic-semihosting" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ee8a3e1233d9073d76a870223512ce4eeea43c067a94a445c13bd6d792d7b1ab" -dependencies = [ - "cortex-m", - "cortex-m-semihosting", -] - [[package]] name = "parking" version = "2.2.1" @@ -3384,7 +3254,7 @@ dependencies = [ "embedded-hal-bus", "embedded-io-async", "embedded-nal-async", - "embedded-sdmmc 0.9.0", + "embedded-sdmmc", "embedded-storage", "fdcan", "grounded", @@ -3471,7 +3341,7 @@ version = "0.2.36" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ff24dfcda44452b9816fff4cd4227e1bb73ff5a2f1bc1105aa92fb8565ce44d2" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", "syn 2.0.104", ] @@ -3490,8 +3360,8 @@ version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", ] [[package]] @@ -3501,20 +3371,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" dependencies = [ "proc-macro-error-attr2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] -[[package]] -name = "proc-macro2" -version = "0.4.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf3d2011ab5c909338f7887f4fc896d35932e29146c12c8d01da6b22a80ba759" -dependencies = [ - "unicode-xid 0.1.0", -] - [[package]] name = "proc-macro2" version = "1.0.95" @@ -3530,37 +3391,6 @@ version = "1.0.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3eb8486b569e12e2c32ad3e204dbaba5e4b5b216e9367044f25f1dba42341773" -[[package]] -name = "proptest" -version = "1.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fcdab19deb5195a31cf7726a210015ff1496ba1464fd42cb4f537b8b01b471f" -dependencies = [ - "bit-set", - "bit-vec", - "bitflags 2.9.1", - "lazy_static", - "num-traits", - "rand 0.9.2", - "rand_chacha 0.9.0", - "rand_xorshift", - "regex-syntax 0.8.5", - "rusty-fork", - "tempfile", - "unarray", -] - -[[package]] -name = "proptest-derive" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90b46295382dc76166cb7cf2bb4a97952464e4b7ed5a43e6cd34e1fec3349ddc" -dependencies = [ - "proc-macro2 0.4.30", - "quote 0.6.13", - "syn 0.15.44", -] - [[package]] name = "prost" version = "0.14.1" @@ -3599,8 +3429,8 @@ checksum = "9120690fafc389a67ba3803df527d0ec9cbbc9cc45e4cc20b332996dfb672425" dependencies = [ "anyhow", "itertools", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -3691,12 +3521,6 @@ dependencies = [ "version_check", ] -[[package]] -name = "quick-error" -version = "1.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" - [[package]] name = "quick-xml" version = "0.26.0" @@ -3706,22 +3530,13 @@ dependencies = [ "memchr", ] -[[package]] -name = "quote" -version = "0.6.13" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6ce23b6b870e8f94f81fb0a363d65d86675884b34a09043c81e5562f11c1f8e1" -dependencies = [ - "proc-macro2 0.4.30", -] - [[package]] name = "quote" version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", ] [[package]] @@ -3805,15 +3620,6 @@ dependencies = [ "rand 0.9.2", ] -[[package]] -name = "rand_xorshift" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "513962919efc330f829edb2535844d1b912b0fbe2ca165d613e4e8788bb05a5a" -dependencies = [ - "rand_core 0.9.3", -] - [[package]] name = "range-alloc" version = "0.1.4" @@ -4018,8 +3824,8 @@ checksum = "f387b12bd6c01d2c9d4776dddeefaf0ae51b9497c83c0186b1693f6821ff3c4a" dependencies = [ "indexmap", "proc-macro-error2", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4033,8 +3839,8 @@ dependencies = [ "cortex-m", "fugit", "portable-atomic", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rtic-time", "stm32-metapac 15.0.0", ] @@ -4069,32 +3875,13 @@ dependencies = [ "rtic-common", ] -[[package]] -name = "rtt-target" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "065d6058bb1204f51a562a67209e1817cf714759d5cf845aa45c75fa7b0b9d9b" -dependencies = [ - "ufmt-write", -] - -[[package]] -name = "rtt-target" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3afa12c77ba1b9bf560e4039a9b9a08bb9cde0e9e6923955eeb917dd8d5cf303" -dependencies = [ - "critical-section", - "ufmt-write", -] - [[package]] name = "rust-format" version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60e7c00b6c3bf5e38a880eec01d7e829d12ca682079f8238a464def3c4b31627" dependencies = [ - "proc-macro2 1.0.95", + "proc-macro2", "syn 1.0.109", ] @@ -4154,18 +3941,6 @@ version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d" -[[package]] -name = "rusty-fork" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb3dcc6e454c328bb824492db107ab7c0ae8fcffe4ad210136ef014458c1bc4f" -dependencies = [ - "fnv", - "quick-error", - "tempfile", - "wait-timeout", -] - [[package]] name = "ryu" version = "1.0.20" @@ -4236,12 +4011,6 @@ version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" -[[package]] -name = "sdio-host" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" - [[package]] name = "semver" version = "0.9.0" @@ -4293,8 +4062,8 @@ version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4343,7 +4112,7 @@ dependencies = [ "rtic", "rtic-monotonics", "rtic-sync", - "stm32h7xx-hal 0.16.0 (git+https://github.com/uorocketry/stm32h7xx-hal)", + "stm32h7xx-hal", ] [[package]] @@ -4376,8 +4145,8 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "231b4425dcc43afc7e18c34e7c6738cd252d42d91d909c948df14107c9ae79f1" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "string_morph", "syn 1.0.109", ] @@ -4496,56 +4265,6 @@ dependencies = [ "vcell", ] -[[package]] -name = "stm32h7xx-hal" -version = "0.16.0" -dependencies = [ - "bare-metal 1.0.0", - "cast", - "cfg-if", - "chrono 0.4.38", - "cortex-m", - "cortex-m-log", - "cortex-m-rt", - "cortex-m-semihosting", - "defmt 0.3.100", - "eg-seven-segment", - "embedded-display-controller", - "embedded-dma", - "embedded-graphics", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-sdmmc 0.5.0", - "embedded-storage", - "fdcan", - "fugit", - "lazy_static", - "log", - "nb 1.1.0", - "numtoa", - "otm8009a", - "panic-halt", - "panic-itm", - "panic-rtt-target", - "panic-semihosting", - "paste", - "proptest", - "proptest-derive", - "rand_core 0.6.4", - "rtic", - "rtt-target 0.4.0", - "sdio-host 0.9.0", - "serde", - "smoltcp", - "stm32-fmc", - "stm32h7", - "synopsys-usb-otg", - "tinybmp", - "usb-device", - "usbd-serial", - "void", -] - [[package]] name = "stm32h7xx-hal" version = "0.16.0" @@ -4607,8 +4326,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "rustversion", "syn 2.0.104", ] @@ -4620,8 +4339,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7695ce3845ea4b33927c055a39dc438a45b059f7c1b3d91d38d10355fb8cbca7" dependencies = [ "heck", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4632,31 +4351,20 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f2c04b93fc15d79b39c63218f15e3fdffaa4c227830686e3b7c5f41244eb3e50" dependencies = [ "base64", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", "unicode-width 0.1.14", ] -[[package]] -name = "syn" -version = "0.15.44" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ca4b3b69a77cbe1ffc9e198781b7acb0c7365a883670e8f1c1bc66fba79a5c5" -dependencies = [ - "proc-macro2 0.4.30", - "quote 0.6.13", - "unicode-xid 0.1.0", -] - [[package]] name = "syn" version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "unicode-ident", ] @@ -4666,31 +4374,19 @@ version = "2.0.104" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17b6f705963418cdb9927482fa304bc562ece2fdd4f616084c50b7023b435a40" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "unicode-ident", ] -[[package]] -name = "synopsys-usb-otg" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe" -dependencies = [ - "cortex-m", - "embedded-hal 0.2.7", - "usb-device", - "vcell", -] - [[package]] name = "synstructure" version = "0.13.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4779,8 +4475,8 @@ version = "1.0.69" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4790,8 +4486,8 @@ version = "2.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4813,15 +4509,6 @@ dependencies = [ "cfg-if", ] -[[package]] -name = "tinybmp" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "197cc000e382175ff15abd9c54c694ef80ef20cb07e7f956c71e3ea97fc8dc60" -dependencies = [ - "embedded-graphics", -] - [[package]] name = "toml_datetime" version = "0.6.11" @@ -4856,8 +4543,8 @@ version = "0.1.30" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "81383ab64e72a7a8b8e13130c49e3dab29def6d0c7d76a03087b3cf71c5c6903" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -4937,8 +4624,8 @@ name = "ublox_derive" version = "0.1.0" source = "git+https://github.com/uorocketry/ublox#286d43a3ae9a3ba30a04be04c65bf89aca6b1bfe" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] @@ -4948,17 +4635,11 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290ba2a4bfe92acb91bb9c0cdae32e9a1cda9cfe5b3329eaf9a444c522512de9" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 1.0.109", ] -[[package]] -name = "ufmt-write" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69" - [[package]] name = "ug" version = "0.1.0" @@ -4980,12 +4661,6 @@ dependencies = [ "yoke", ] -[[package]] -name = "unarray" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" - [[package]] name = "unicode-ident" version = "1.0.18" @@ -5004,12 +4679,6 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4a1a07cc7db3810833284e8d372ccdc6da29741639ecc70c9ec107df0fa6154c" -[[package]] -name = "unicode-xid" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" - [[package]] name = "unicode-xid" version = "0.2.6" @@ -5062,25 +4731,13 @@ dependencies = [ "byteorder", "hashbrown 0.13.2", "log", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "serde", "syn 1.0.109", "usbd-hid-descriptors", ] -[[package]] -name = "usbd-serial" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "065e4eaf93db81d5adac82d9cef8f8da314cb640fa7f89534b972383f1cf80fc" -dependencies = [ - "embedded-hal 0.2.7", - "embedded-io", - "nb 1.1.0", - "usb-device", -] - [[package]] name = "uuid" version = "1.17.0" @@ -5099,8 +4756,8 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "41b6d82be61465f97d42bd1d15bf20f3b0a3a0905018f38f9d6f6962055b0b5c" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5131,15 +4788,6 @@ dependencies = [ "vcell", ] -[[package]] -name = "wait-timeout" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "09ac3b126d3914f9849036f826e054cbabdc8519970b8998ddaf3b5bd3c65f11" -dependencies = [ - "libc", -] - [[package]] name = "walkdir" version = "2.5.0" @@ -5185,8 +4833,8 @@ checksum = "2f0a0651a5c2bc21487bde11ee802ccaf4c51935d0d3d42a6101f98161700bc6" dependencies = [ "bumpalo", "log", - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "wasm-bindgen-shared", ] @@ -5210,7 +4858,7 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7fe63fc6d09ed3792bd0897b314f53de8e16568c2b3f7982f468c0bf9bd0b407" dependencies = [ - "quote 1.0.40", + "quote", "wasm-bindgen-macro-support", ] @@ -5220,8 +4868,8 @@ version = "0.2.100" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8ae87ea40c9f689fc23f209965b6fb8a99ad69aeeb0231408be24920604395de" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "wasm-bindgen-backend", "wasm-bindgen-shared", @@ -5521,8 +5169,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2bbd5b46c938e506ecbce286b6628a02171d56153ba733b6c741fc627ec9579b" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5532,8 +5180,8 @@ version = "0.60.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a47fddd13af08290e67f4acabf4b459f647552718f683a7b415d290ac744a836" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5543,8 +5191,8 @@ version = "0.58.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "053c4c462dc91d3b1504c6fe5a726dd15e216ba718e84a0e46a88fbe5ded3515" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5554,8 +5202,8 @@ version = "0.59.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd9211b69f8dcdfa817bfd14bf1c97c9188afa36f4750130fcdf3f400eca9fa8" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5876,8 +5524,8 @@ version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2380878cad4ac9aac1e2435f3eb4020e8374b5f13c296cb75b4620ff8e229154" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "synstructure", ] @@ -5897,8 +5545,8 @@ version = "0.8.26" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9ecf5b4cc5364572d7f4c329661bcc82724222973f2cab6f050a4e5c22f75181" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", ] @@ -5917,8 +5565,8 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d71e5d6e06ab090c67b5e44993ec16b72dcbaabc526db883a360057678b48502" dependencies = [ - "proc-macro2 1.0.95", - "quote 1.0.40", + "proc-macro2", + "quote", "syn 2.0.104", "synstructure", ] diff --git a/phoenix/src/drivers/sd.rs b/phoenix/src/drivers/sd.rs index 9015282..d8e695e 100644 --- a/phoenix/src/drivers/sd.rs +++ b/phoenix/src/drivers/sd.rs @@ -5,6 +5,31 @@ use embassy_time::Delay; use embedded_hal_1::delay::DelayNs; use embedded_hal_bus::spi::RefCellDevice; use embedded_sdmmc::{Mode, SdCard, VolumeIdx, VolumeManager}; +use core::marker::PhantomData; + +/// Minimal time source for embedded-sdmmc +pub struct TimeSink { + pub(crate) _marker: PhantomData<*const ()>, +} + +impl TimeSink { + pub fn new() -> Self { + Self { _marker: PhantomData } + } +} + +impl embedded_sdmmc::TimeSource for TimeSink { + fn get_timestamp(&self) -> embedded_sdmmc::Timestamp { + embedded_sdmmc::Timestamp { + year_since_1970: 0, + zero_indexed_month: 0, + zero_indexed_day: 0, + hours: 0, + minutes: 0, + seconds: 0, + } + } +} // Initializes the SD card and performs the small demo previously in main.rs. // Returns Ok(()) if initialization and simple access succeed. @@ -18,9 +43,7 @@ pub fn init_and_demo( info!("Card size is {} bytes", sdcard.num_bytes().map_err(|_| ())?); let volume_mgr = VolumeManager::new( sdcard, - super::super::TimeSink { - _marker: core::marker::PhantomData, - }, + TimeSink::new(), ); let _volume0 = volume_mgr.open_volume(VolumeIdx(0)).map_err(|_| ())?; // let root_dir = volume0.open_root_dir().map_err(|_| ())?; diff --git a/phoenix/src/features.rs b/phoenix/src/features.rs index ea4da5e..1c6cfbc 100644 --- a/phoenix/src/features.rs +++ b/phoenix/src/features.rs @@ -3,6 +3,7 @@ pub const ENABLE_LED: bool = false; pub const ENABLE_SBG: bool = false; +// TODO: remove/condense these GPS features pub const ENABLE_GPS: bool = false; pub const ENABLE_GPS_POLL: bool = false; pub const ENABLE_GPS_PARSE: bool = false; @@ -13,3 +14,4 @@ pub const ENABLE_BARO: bool = false; pub const ENABLE_RADIO: bool = true; pub const ENABLE_INFERENCE: bool = false; pub const ENABLE_SD: bool = false; +pub const ENABLE_STATE_MACHINE: bool = true; diff --git a/phoenix/src/main.rs b/phoenix/src/main.rs index 79d52b0..acdd5de 100644 --- a/phoenix/src/main.rs +++ b/phoenix/src/main.rs @@ -25,7 +25,6 @@ use burn::{ record::{BinBytesRecorder, FullPrecisionSettings, Recorder}, // <-- FIX: Use BinBytesRecorder }; use core::cell::RefCell; -use core::marker::PhantomData; use defmt::*; use embassy_executor::Spawner; use embassy_stm32::gpio::{Input, Level, Output, OutputType, Pull, Speed}; @@ -72,79 +71,7 @@ use smlang::statemachine; use ublox::cfg_val::CfgVal; use crate::drivers::recovery::{Arming, Fire, RecoveryManager}; - -// use crate::model::sine::Model; -// ================================================================================= -// Shared Resources & Types -// ================================================================================= - -type Backend = NdArray; -type BackendDevice = ::Device; - -use crate::resources::{Irqs, BUFFER_CHANNEL, HEAP, RX_GPS_BUF, RX_RADIO_BUF, RX_SBG_BUF}; - -statemachine! { - transitions: { - *Init + Start = WaitForLaunch, - WaitForLaunch + Launch = Ascent, - Ascent + Apogee = Descent, - Descent + MainDeployment = Fuck, - Descent + DrogueDeployment = DrogueDescent, - DrogueDescent + MainDeployment = MainDescent, - MainDescent + NoMovement = Landed, - Fault + FaultCleared = _, - _ + FaultDetected = Fault, - } -} - -pub struct TimeSink { - _marker: PhantomData<*const ()>, -} - -impl TimeSink { - fn new() -> Self { - TimeSink { - _marker: PhantomData, - } - } -} - -impl embedded_sdmmc::TimeSource for TimeSink { - fn get_timestamp(&self) -> embedded_sdmmc::Timestamp { - embedded_sdmmc::Timestamp { - year_since_1970: 0, - zero_indexed_month: 0, - zero_indexed_day: 0, - hours: 0, - minutes: 0, - seconds: 0, - } - } -} - -// ================================================================================= -// Application Tasks -// ================================================================================= - -#[embassy_executor::task] -async fn sm_task(spawner: Spawner, state_machine: StateMachine) { - info!("State Machine task started."); - - loop { - match state_machine.state { - States::Ascent => {} - States::Fault => {} - States::Init => {} - States::WaitForLaunch => {} - States::Descent => {} - States::DrogueDescent => {} - States::Fuck => {} - States::Landed => {} - States::MainDescent => {} - } - Timer::after(Duration::from_millis(1000)).await; - } -} +use crate::resources::HEAP; // ================================================================================= // Main Entry Point @@ -240,8 +167,6 @@ async fn main(spawner: Spawner) { Delay.delay_ms(2000); - // Optional extra GPS config steps are now gated and handled in comm::gps::configure_ublox. - let config_packet = CfgValSetBuilder { version: 1, // Save to RAM (to apply now) and BBR (to make it persistent) @@ -268,8 +193,6 @@ async fn main(spawner: Spawner) { comm::gps::configure_ublox(tx).await; } - // Verbose GPS example moved to tasks::gps as a commented reference to keep main lean. - // // --- Boom Boom Setup --- // /* // MAIN_ARM/TEST = PD6 @@ -304,10 +227,13 @@ async fn main(spawner: Spawner) { ch1.enable(); music::play_song(&mut pwm, music::TWINKLE_MELODY, 130).await; - // // --- State Machine --- - // let state_machine = StateMachine::new(traits::Context {}); + // --- State Machine --- + if features::ENABLE_STATE_MACHINE { + let state_machine = tasks::state_machine::StateMachine::new(traits::Context {}); + spawner.must_spawn(tasks::state_machine::sm_task(spawner, state_machine)); + } - // // --- Radio --- + // --- Radio --- let (mut radio_tx, mut radio_ring_rx) = board.setup_radio(); // --- Inference --- @@ -330,14 +256,8 @@ async fn main(spawner: Spawner) { if let (true, Some(baro_dev)) = (features::ENABLE_BARO, baro) { spawner.must_spawn(tasks::baro::baro_reader_task(baro_dev)); } - // pass control of the spawner to the state machine - // spawner.must_spawn(tasks::state_machine::sm_task(spawner, state_machine)); - // temporary: compilable stub task until Context satisfies StateMachineContext - // spawner.must_spawn(tasks::state_machine::sm_task_stub()); if features::ENABLE_RADIO { spawner.must_spawn(comm::radio::radio_reader_task(radio_ring_rx)); spawner.must_spawn(comm::radio::radio_writer_task(radio_tx)); } -} - -// See inference:: for the reference run_model helper moved from here. +} \ No newline at end of file diff --git a/phoenix/src/tasks/state_machine.rs b/phoenix/src/tasks/state_machine.rs index 6be7bce..506c1f8 100644 --- a/phoenix/src/tasks/state_machine.rs +++ b/phoenix/src/tasks/state_machine.rs @@ -5,7 +5,7 @@ use smlang::statemachine; use crate::traits::Context; -// Moved from main.rs for cleanliness. Behavior unchanged. +// Single source of truth for the state machine statemachine! { transitions: { *Init + Start = WaitForLaunch, @@ -20,31 +20,21 @@ statemachine! { } } -// Original signature (kept for reference; requires Context to implement StateMachineContext): -// #[task] -// pub async fn sm_task(_spawner: embassy_executor::Spawner, state_machine: StateMachine) { -// info!("State Machine task started."); -// loop { -// match state_machine.state { -// States::Ascent => {} -// States::Fault => {} -// States::Init => {} -// States::WaitForLaunch => {} -// States::Descent => {} -// States::DrogueDescent => {} -// States::Fuck => {} -// States::Landed => {} -// States::MainDescent => {} -// } -// Timer::after(Duration::from_millis(1000)).await; -// } -// } - -// Compilable stub until Context implements the required trait. Behavior preserved in comments above. #[task] -pub async fn sm_task_stub() { - info!("State Machine task stub started."); +pub async fn sm_task(_spawner: embassy_executor::Spawner, mut state_machine: StateMachine) { + info!("State Machine task started."); loop { + match state_machine.state { + States::Ascent => {} + States::Fault => {} + States::Init => {} + States::WaitForLaunch => {} + States::Descent => {} + States::DrogueDescent => {} + States::Fuck => {} + States::Landed => {} + States::MainDescent => {} + } Timer::after(Duration::from_millis(1000)).await; } } diff --git a/phoenix/src/traits.rs b/phoenix/src/traits.rs index 1b01982..0750811 100644 --- a/phoenix/src/traits.rs +++ b/phoenix/src/traits.rs @@ -1,4 +1,4 @@ -use crate::{StateMachineContext, States}; +use crate::tasks::state_machine::{StateMachineContext, States}; use messages_prost::state::State; pub struct Context {} From 4b65b10cc732e181d05757951b07e0441e3be44b Mon Sep 17 00:00:00 2001 From: Fernando Date: Mon, 11 Aug 2025 13:31:19 -0400 Subject: [PATCH 4/4] Clean up unused file --- phoenix/src/main.rs | 1 - phoenix/src/setup.rs | 1 - 2 files changed, 2 deletions(-) delete mode 100644 phoenix/src/setup.rs diff --git a/phoenix/src/main.rs b/phoenix/src/main.rs index acdd5de..35039c3 100644 --- a/phoenix/src/main.rs +++ b/phoenix/src/main.rs @@ -14,7 +14,6 @@ mod model; mod music; mod resources; mod sbg_manager; -mod setup; mod tasks; mod traits; diff --git a/phoenix/src/setup.rs b/phoenix/src/setup.rs deleted file mode 100644 index 0f0f46f..0000000 --- a/phoenix/src/setup.rs +++ /dev/null @@ -1 +0,0 @@ -//! Deprecated: previous setup helpers moved into `board` module.