diff --git a/.cargo/config b/.cargo/config.toml similarity index 69% rename from .cargo/config rename to .cargo/config.toml index bab9d3ed..4951091d 100644 --- a/.cargo/config +++ b/.cargo/config.toml @@ -1,14 +1,14 @@ -[target.'cfg(all(target_arch = "arm", target_os = "none"))'] -# This runner needs to be parametric so we can pass in the chip name -runner = "probe-rs run --chip ATSAME51J20A --protocol swd" - -rustflags = [ - "-C", "link-arg=-Tlink.x", - "-C", "link-arg=-Tdefmt.x", -] - -[env] -DEFMT_LOG="info" - -[build] -target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] +# This runner needs to be parametric so we can pass in the chip name +runner = "probe-rs run --chip STM32H733VGTx --protocol swd" +#runner = "probe-rs run --chip ATSAME51J18A --protocol swd" +rustflags = [ + "-C", "link-arg=-Tlink.x", + "-C", "link-arg=-Tdefmt.x", +] + +[env] +DEFMT_LOG="info" + +[build] +target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) \ No newline at end of file diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 06c8907b..4a90c97c 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -13,12 +13,11 @@ COPY --from=toolchain /toolchain /toolchain ENV PATH="${PATH}:/toolchain/bin" ARG DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get upgrade -y && apt-get install -y cmake pkg-config libusb-1.0-0-dev libftdi1-dev libudev-dev libssl-dev -# Necessary system packages -RUN apt-get update && apt-get install -y cmake pkg-config libusb-1.0-0-dev libftdi1-dev libudev-dev libssl-dev && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -# Install Rust crates -RUN cargo install probe-rs --features cli \ - && cargo install cargo-make \ No newline at end of file +# Install ARM GCC deps +RUN mkdir -p toolchain && \ + curl -L "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/arm-gnu-toolchain-13.2.rel1-x86_64-arm-none-eabi.tar.xz" \ + | tar --strip-components=1 -xJ -C toolchain && \ + cargo install --locked probe-rs@0.23.0 --features cli +ENV PATH="${PATH}:/toolchain/bin" \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 6ada23bc..748d56fc 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -6,5 +6,4 @@ }, "remoteUser": "root", "runArgs": ["--privileged"] - } \ No newline at end of file diff --git a/.gitconfig b/.gitconfig new file mode 100644 index 00000000..281c8ea6 --- /dev/null +++ b/.gitconfig @@ -0,0 +1,2 @@ +[core] + autocrlf = true \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index d21c5cd1..1f3b7eea 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,133 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug Recovery", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/recovery", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach nav (Debug)", + "chip": "STM32H733VGTx", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/nav", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach recovery (Release)", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/recovery", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach com (Release)", + "chip": "ATSAME51J18A", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/communication", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug Link (Release)", + "chip": "ATSAME51J18A", + "flashingConfig": { + "flashingEnabled": true, + "haltAfterReset": false, + "formatOptions": { + //!MODIFY (or remove). Valid values are: 'bin', 'hex', 'elf'(default), 'idf' + "binaryFormat": "elf" + } + }, + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/communication", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Debug nav (Debug)", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "flashingConfig": { + + "haltAfterReset": true, + // "flashingEnabled": true, + + }, + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/nav", + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach Release nav", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/nav", + // "rttEnabled": true, + // "coreIndex": 0, + } + ] + }, + { + "type": "probe-rs-debug", + "request": "attach", + "name": "Probe-rs attach Release simple example", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/simple_example", + "rttEnabled": true, + "coreIndex": 0, + } + ] + }, + { + "type": "probe-rs-debug", + "request": "launch", + "name": "Probe-rs Release nav", + "chip": "STM32H733VGTx", + "wireProtocol": "Swd", + "flashingConfig": { + "haltAfterReset": true, + "flashingEnabled": true, + }, + "coreConfigs": [ + { + "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/release/nav", + "rttEnabled": true, + "coreIndex": 0, + } + ] + }, { "type": "cortex-debug", "request": "launch", diff --git a/Cargo.lock b/Cargo.lock index ec3a974a..a19058be 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -14,15 +14,6 @@ dependencies = [ "opaque-debug", ] -[[package]] -name = "as-slice" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "516b6b4f0e40d50dcda9365d53964ec74560ad4284da2e7fc97122cd83174516" -dependencies = [ - "stable_deref_trait", -] - [[package]] name = "atomic-polyfill" version = "1.0.3" @@ -43,7 +34,8 @@ dependencies = [ "bitflags 1.3.2", "cipher", "cortex-m", - "embedded-hal", + "defmt", + "embedded-hal 0.2.7", "fugit", "mcan-core", "modular-bitfield", @@ -71,9 +63,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -90,21 +82,6 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" -[[package]] -name = "beacon" -version = "0.1.0" -dependencies = [ - "common-arm", - "common-arm-stm32l0", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "messages", - "postcard", - "stm32l0xx-hal", - "systick-monotonic", -] - [[package]] name = "bit-set" version = "0.5.3" @@ -140,9 +117,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.4.2" +version = "2.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf" +checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" dependencies = [ "serde", ] @@ -165,7 +142,7 @@ dependencies = [ "cortex-m-rtic", "defmt", "enum_dispatch", - "heapless", + "heapless 0.7.17", "messages", "postcard", "systick-monotonic", @@ -180,11 +157,11 @@ checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" [[package]] name = "cc" -version = "1.0.83" +version = "1.1.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1174fb0b6ec23863f8b971027804a42614e347eafb0a95bf0b12cdae21fc4d0" +checksum = "72db2f7947ecee9b03b510377e8bb9077afa27176fdbff55c51027e976fdcc48" dependencies = [ - "libc", + "shlex", ] [[package]] @@ -195,9 +172,9 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "chrono" -version = "0.4.35" +version = "0.4.38" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8eaf5903dcbc0a39312feb77df2ff4c76387d591b9fc7b04a238dcf8bb62639a" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" dependencies = [ "num-traits", ] @@ -213,9 +190,9 @@ dependencies = [ [[package]] name = "cmake" -version = "0.1.50" +version = "0.1.51" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a31c789563b815f77f4250caee12365734369f942439b7defd71e18a48197130" +checksum = "fb1e43aa7fd152b1f968787f7dbcdeb306d1867ff373c69955211876c053f91a" dependencies = [ "cc", ] @@ -235,11 +212,12 @@ dependencies = [ "defmt", "defmt-rtt", "derive_more", - "embedded-hal", - "embedded-sdmmc", - "heapless", + "embedded-hal 0.2.7", + "embedded-sdmmc 0.3.0", + "heapless 0.7.17", "mcan", "messages", + "ms5611-01ba", "nb 1.1.0", "postcard", ] @@ -250,7 +228,8 @@ version = "0.1.0" dependencies = [ "atsamd-hal", "common-arm", - "embedded-hal", + "embedded-hal 0.2.7", + "mcan", ] [[package]] @@ -281,25 +260,6 @@ dependencies = [ "panic-probe", ] -[[package]] -name = "communication" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "embedded-sdmmc", - "heapless", - "messages", - "postcard", - "systick-monotonic", - "typenum", -] - [[package]] name = "convert_case" version = "0.4.0" @@ -315,7 +275,7 @@ dependencies = [ "bare-metal 0.2.5", "bitfield 0.13.2", "critical-section", - "embedded-hal", + "embedded-hal 0.2.7", "volatile-register", ] @@ -334,8 +294,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] @@ -348,7 +308,7 @@ dependencies = [ "bare-metal 1.0.0", "cortex-m", "cortex-m-rtic-macros", - "heapless", + "heapless 0.7.17", "rtic-core", "rtic-monotonic", "version_check", @@ -361,8 +321,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eefb40b1ca901c759d29526e5c8a0a1b246c20caaa5b4cc5d0f0b94debecd4c7" dependencies = [ "proc-macro-error", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "rtic-syntax", "syn 1.0.109", ] @@ -378,18 +338,18 @@ dependencies = [ [[package]] name = "cpufeatures" -version = "0.2.12" +version = "0.2.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504" +checksum = "51e852e6dc9a5bed1fae92dd2375037bf2b768725bf3be87811edee3249d09ad" dependencies = [ "libc", ] [[package]] name = "crc-any" -version = "2.4.4" +version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c01a5e1f881f6fb6099a7bdf949e946719fd4f1fefa56264890574febf0eb6d0" +checksum = "a62ec9ff5f7965e4d7280bd5482acd20aadb50d632cf6c1d74493856b011fa73" [[package]] name = "critical-section" @@ -399,9 +359,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "defmt" -version = "0.3.5" +version = "0.3.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8a2d011b2fee29fb7d659b83c43fce9a2cb4df453e16d441a51448e448f3f98" +checksum = "a99dd22262668b887121d4672af5a64b238f026099f1a2a1b322066c9ecfe9e0" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -409,31 +369,31 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.6" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54f0216f6c5acb5ae1a47050a6645024e6edafc2ee32d421955eccfef12ef92e" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", ] [[package]] name = "defmt-parser" -version = "0.3.3" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "269924c02afd7f94bc4cecbfa5c379f6ffcf9766b3408fe63d22c728654eccd0" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" dependencies = [ "thiserror", ] [[package]] name = "defmt-rtt" -version = "0.4.0" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "609923761264dd99ed9c7d209718cda4631c5fe84668e0f0960124cbb844c49f" +checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" dependencies = [ "critical-section", "defmt", @@ -441,9 +401,9 @@ dependencies = [ [[package]] name = "defmt-test" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c629c79c479057fa5b63e6095c2aa94367762524dbe791ecb611ef5a25e08851" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", @@ -453,26 +413,26 @@ dependencies = [ [[package]] name = "defmt-test-macros" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94a0dfea4063d72e1ba20494dfbc4667f67420869328cf3670b5824a38a22dc1" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 1.0.109", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", ] [[package]] name = "derive_more" -version = "0.99.17" +version = "0.99.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fb810d30a7c1953f91334de7244731fc3f3c10d7fe163338a35b9f640960321" +checksum = "5f33878137e4dafd7fa914ad4e259e18a4e8e532b9617a2d0150262bf53abfce" dependencies = [ "convert_case", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "rustc_version 0.4.0", - "syn 1.0.109", + "syn 2.0.74", ] [[package]] @@ -513,6 +473,32 @@ dependencies = [ "void", ] +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-bus" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57b4e6ede84339ebdb418cd986e6320a34b017cdf99b5cc3efceec6450b06886" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", +] + [[package]] name = "embedded-io" version = "0.4.0" @@ -526,11 +512,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d3bf0a2b5becb87e9a329d9290f131e4d10fec39b56d129926826a7cbea1e7a" dependencies = [ "byteorder", - "embedded-hal", + "embedded-hal 0.2.7", "log", "nb 0.1.3", ] +[[package]] +name = "embedded-sdmmc" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "150f320125310e179b9e73b081173b349e63c5c7d4ca44db4e5b9121b10387ec" +dependencies = [ + "byteorder", + "embedded-hal 1.0.0", + "heapless 0.8.0", + "log", +] + [[package]] name = "embedded-storage" version = "0.3.1" @@ -538,41 +536,52 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" [[package]] -name = "embedded-time" -version = "0.12.1" +name = "enum_dispatch" +version = "0.3.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d7a4b4d10ac48d08bfe3db7688c402baadb244721f30a77ce360bd24c3dffe58" +checksum = "aa18ce2bc66555b3218614519ac839ddb759a7d6720732f979ef8d13be147ecd" dependencies = [ - "num", + "once_cell", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", ] [[package]] -name = "enum_dispatch" -version = "0.3.12" +name = "equivalent" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8f33313078bb8d4d05a2733a94ac4c2d8a0df9a2b84424ebf4f33bfc224a890e" -dependencies = [ - "once_cell", - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", -] +checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" [[package]] name = "errno" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] name = "fastrand" -version = "2.0.1" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5" +checksum = "9fc0510504f03c51ada170672ac806f1f105a88aa97a5281117e1ddc3368e51a" + +[[package]] +name = "fdcan" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "553fb8420a8ca433ed032a3f284deeb95c12e4d3d25cdc15196669a27b2ae34b" +dependencies = [ + "bitflags 1.3.2", + "nb 1.1.0", + "paste", + "static_assertions", + "vcell", + "volatile-register", +] [[package]] name = "fnv" @@ -589,6 +598,30 @@ dependencies = [ "gcd", ] +[[package]] +name = "futures-core" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" + +[[package]] +name = "futures-task" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" + +[[package]] +name = "futures-util" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + [[package]] name = "gcd" version = "2.3.0" @@ -607,15 +640,38 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.12" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "190092ea657667030ac6a35e305e62fc4dd69fd98ac98631e5d3a2b1575a12b5" +checksum = "c4567c8db10ae91089c99af84c68c38da3ec2f087c3f82960bcdbf3656b6f4d7" dependencies = [ "cfg-if", "libc", "wasi", ] +[[package]] +name = "gps" +version = "0.1.0" +dependencies = [ + "atsamd-hal", + "common-arm", + "common-arm-atsame", + "cortex-m", + "cortex-m-rt", + "cortex-m-rtic", + "defmt", + "defmt-rtt", + "embedded-alloc", + "embedded-sdmmc 0.8.0", + "heapless 0.7.17", + "messages", + "panic-probe", + "postcard", + "systick-monotonic", + "typenum", + "ublox", +] + [[package]] name = "hash32" version = "0.2.1" @@ -625,12 +681,27 @@ dependencies = [ "byteorder", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" + [[package]] name = "heapless" version = "0.7.17" @@ -638,13 +709,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" dependencies = [ "atomic-polyfill", - "hash32", + "hash32 0.2.1", "rustc_version 0.4.0", "serde", "spin", "stable_deref_trait", ] +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] + [[package]] name = "indexmap" version = "1.9.3" @@ -652,7 +733,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", - "hashbrown", + "hashbrown 0.12.3", +] + +[[package]] +name = "indexmap" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93ead53efc7ea8ed3cfb0c79fc8023fbb782a5432b52830b6518941cebe6505c" +dependencies = [ + "equivalent", + "hashbrown 0.14.5", ] [[package]] @@ -666,15 +757,15 @@ dependencies = [ [[package]] name = "lazy_static" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "libc" -version = "0.2.152" +version = "0.2.156" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13e3bf6590cbc649f4d1a3eefc9d5d6eb746f5200ffb04e5e142700b8faa56e7" +checksum = "a5f43f184355eefb8d17fc948dbecf6c13be3c141f20d834ae842193a448c72a" [[package]] name = "libm" @@ -682,6 +773,30 @@ version = "0.2.8" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" +[[package]] +name = "link" +version = "0.1.0" +dependencies = [ + "chrono", + "common-arm", + "common-arm-stm32h7", + "cortex-m", + "cortex-m-rt", + "defmt", + "defmt-rtt", + "embedded-alloc", + "fdcan", + "heapless 0.7.17", + "messages", + "panic-probe", + "postcard", + "rtic", + "rtic-monotonics", + "rtic-sync", + "stm32h7xx-hal", + "systick-monotonic", +] + [[package]] name = "linked_list_allocator" version = "0.10.5" @@ -690,15 +805,15 @@ checksum = "9afa463f5405ee81cdb9cc2baf37e08ec7e4c8209442b5d72c04cfb2cd6e6286" [[package]] name = "linux-raw-sys" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" +checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" [[package]] name = "lock_api" -version = "0.4.11" +version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" +checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" dependencies = [ "autocfg", "scopeguard", @@ -706,28 +821,48 @@ dependencies = [ [[package]] name = "log" -version = "0.4.20" +version = "0.4.22" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" [[package]] name = "mavlink" -version = "0.11.1" -source = "git+https://github.com/uorocketry/rust-mavlink#2ef8c4cb4fb9d944ddfae538e8e474f12253b782" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" dependencies = [ "bitflags 1.3.2", - "byteorder", - "crc-any", - "embedded-hal", - "heapless", - "lazy_static", - "nb 1.1.0", + "mavlink-bindgen", + "mavlink-core", "num-derive", "num-traits", - "proc-macro2 1.0.78", + "serde", + "serde_arrays", +] + +[[package]] +name = "mavlink-bindgen" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" +dependencies = [ + "crc-any", + "lazy_static", + "proc-macro2 1.0.86", "quick-xml", - "quote 1.0.35", + "quote 1.0.36", + "thiserror", +] + +[[package]] +name = "mavlink-core" +version = "0.13.1" +source = "git+https://github.com/uorocketry/rust-mavlink.git#e9f4057deedce85cae542fdbef75c5f56a360c9c" +dependencies = [ + "byteorder", + "crc-any", + "embedded-hal 0.2.7", + "nb 1.1.0", "serde", + "serde_arrays", "serial", ] @@ -757,32 +892,33 @@ dependencies = [ [[package]] name = "memchr" -version = "2.7.1" +version = "2.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" +checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" [[package]] name = "messages" version = "0.1.0" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.6.0", "defmt", "derive_more", "fugit", - "heapless", + "heapless 0.7.17", "mavlink", "messages-proc-macros-lib", "postcard", "proptest", "proptest-derive", "serde", + "ts-rs", ] [[package]] name = "messages-proc-macros-lib" version = "0.1.0" dependencies = [ - "quote 1.0.35", + "quote 1.0.36", "serde", ] @@ -802,11 +938,44 @@ version = "0.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a7d5f7076603ebc68de2dc6a650ec331a062a13abaa346975be747bbfa4b789" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] +[[package]] +name = "ms5611-01ba" +version = "0.1.0" +source = "git+https://github.com/NoahSprenger/ms5611-01ba?branch=embedded-hal-02#fdb9b51f953d854c7b99079ae3583f82c6378bbc" +dependencies = [ + "embedded-hal 0.2.7", +] + +[[package]] +name = "nav" +version = "0.1.0" +dependencies = [ + "chrono", + "common-arm", + "common-arm-stm32h7", + "cortex-m", + "cortex-m-rt", + "defmt", + "defmt-rtt", + "embedded-alloc", + "fdcan", + "heapless 0.7.17", + "messages", + "panic-probe", + "postcard", + "rtic", + "rtic-monotonics", + "rtic-sync", + "sbg-rs", + "stm32h7xx-hal", + "systick-monotonic", +] + [[package]] name = "nb" version = "0.1.3" @@ -822,75 +991,22 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" -[[package]] -name = "num" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" -dependencies = [ - "num-complex", - "num-integer", - "num-iter", - "num-rational", - "num-traits", -] - -[[package]] -name = "num-complex" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5" -dependencies = [ - "num-traits", -] - [[package]] name = "num-derive" version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] -[[package]] -name = "num-integer" -version = "0.1.46" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" -dependencies = [ - "num-traits", -] - -[[package]] -name = "num-iter" -version = "0.1.44" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d869c01cc0c455284163fd0092f1f93835385ccab5a98a0dcc497b2f8bf055a9" -dependencies = [ - "autocfg", - "num-integer", - "num-traits", -] - -[[package]] -name = "num-rational" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12ac428b1cb17fce6f731001d307d351ec70a6d202fc2e60f7d4c5e42d8f4f07" -dependencies = [ - "autocfg", - "num-integer", - "num-traits", -] - [[package]] name = "num-traits" -version = "0.2.17" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", "libm", @@ -904,9 +1020,9 @@ checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "opaque-debug" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "624a8340c38c1b80fd549087862da4ba43e08858af025b236e509b6649fc13d5" +checksum = "c08d65885ee38876c4f86fa503fb49d7b507c2b62552df7c70b2fce627e06381" [[package]] name = "panic-halt" @@ -916,9 +1032,9 @@ checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -926,9 +1042,27 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.14" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pin-project-lite" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "portable-atomic" +version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +checksum = "da544ee218f0d287a911e9c99a39a8c9bc8fcad3cb8db5959940044ecfc67265" [[package]] name = "postcard" @@ -939,34 +1073,18 @@ dependencies = [ "cobs", "defmt", "embedded-io", - "heapless", + "heapless 0.7.17", "serde", ] -[[package]] -name = "power" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "enum_dispatch", - "heapless", - "messages", - "postcard", - "systick-monotonic", - "typenum", -] - [[package]] name = "ppv-lite86" -version = "0.2.17" +version = "0.2.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" +checksum = "77957b295656769bb8ad2b6a6b09d897d94f05c41b069aede1fcdaa675eaea04" +dependencies = [ + "zerocopy", +] [[package]] name = "proc-macro-error" @@ -975,8 +1093,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" dependencies = [ "proc-macro-error-attr", - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", "version_check", ] @@ -987,8 +1105,8 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "version_check", ] @@ -1003,22 +1121,22 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.78" +version = "1.0.86" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2422ad645d89c99f8f3e6b88a9fdeca7fabeac836b1002371c4367c8f984aae" +checksum = "5e719e8df665df0d1c8fbfd238015744736151d4445ec0836b8e628aae103b77" dependencies = [ "unicode-ident", ] [[package]] name = "proptest" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "31b476131c3c86cb68032fdc5cb6d5a1045e3e42d96b69fa599fd77701e1f5bf" +checksum = "b4c2511913b88df1637da85cc8d96ec8e43a3f8bb8ccb71ee1ac240d6f3df58d" dependencies = [ "bit-set", "bit-vec", - "bitflags 2.4.2", + "bitflags 2.6.0", "lazy_static", "num-traits", "rand", @@ -1067,11 +1185,11 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.35" +version = "1.0.36" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" dependencies = [ - "proc-macro2 1.0.78", + "proc-macro2 1.0.86", ] [[package]] @@ -1124,36 +1242,45 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "defmt-rtt", + "embedded-hal 0.2.7", "enum_dispatch", - "heapless", + "heapless 0.7.17", "messages", + "ms5611-01ba", + "panic-probe", "postcard", "systick-monotonic", "typenum", ] [[package]] -name = "redox_syscall" -version = "0.4.1" +name = "regex-syntax" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4722d768eff46b75989dd134e5c353f0d6296e5aaa3132e776cbdb56be7731aa" -dependencies = [ - "bitflags 1.3.2", -] +checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] -name = "regex-syntax" -version = "0.8.2" +name = "rtic" +version = "2.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" +checksum = "c443db16326376bdd64377da268f6616d5f804aba8ce799bac7d1f7f244e9d51" +dependencies = [ + "atomic-polyfill", + "bare-metal 1.0.0", + "cortex-m", + "critical-section", + "rtic-core", + "rtic-macros", +] [[package]] -name = "rtcc" -version = "0.3.1" +name = "rtic-common" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f4fbd0d5bed2b76e27a7ef872568b34072c1af94c277cd52c17a89d54673b3fe" +checksum = "0786b50b81ef9d2a944a000f60405bb28bf30cd45da2d182f3fe636b2321f35c" dependencies = [ - "chrono", + "critical-section", ] [[package]] @@ -1162,24 +1289,82 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d9369355b04d06a3780ec0f51ea2d225624db777acbc60abd8ca4832da5c1a42" +[[package]] +name = "rtic-macros" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54053598ea24b1b74937724e366558412a1777eb2680b91ef646db540982789a" +dependencies = [ + "indexmap 2.4.0", + "proc-macro-error", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", +] + [[package]] name = "rtic-monotonic" version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fb8b0b822d1a366470b9cea83a1d4e788392db763539dc4ba022bcc787fece82" +[[package]] +name = "rtic-monotonics" +version = "2.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e17f88319061d17d3b99997263397b176e3260177e2b4ff4ffed0078d97894c" +dependencies = [ + "cfg-if", + "cortex-m", + "fugit", + "portable-atomic", + "proc-macro2 1.0.86", + "quote 1.0.36", + "rtic-time", + "stm32-metapac", +] + +[[package]] +name = "rtic-sync" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49b1200137ccb2bf272a1801fa6e27264535facd356cb2c1d5bc8e12aa211bad" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-bus", + "heapless 0.8.0", + "portable-atomic", + "rtic-common", +] + [[package]] name = "rtic-syntax" version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ - "indexmap", - "proc-macro2 1.0.78", - "quote 1.0.35", + "indexmap 1.9.3", + "proc-macro2 1.0.86", + "quote 1.0.36", "syn 1.0.109", ] +[[package]] +name = "rtic-time" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7b1d853fa50dc125695414ce4510567a0d420221e455b1568cfa8c9aece9614" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "embedded-hal-async", + "fugit", + "futures-util", + "rtic-common", +] + [[package]] name = "rtic_example" version = "0.1.0" @@ -1192,7 +1377,7 @@ dependencies = [ "defmt", "defmt-rtt", "fugit", - "heapless", + "heapless 0.7.17", "messages", "panic-halt", "postcard", @@ -1215,20 +1400,20 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.21", + "semver 1.0.23", ] [[package]] name = "rustix" -version = "0.38.30" +version = "0.38.34" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "322394588aaf33c24007e8bb3238ee3e4c5c09c084ab32bc73890b99ff326bca" +checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.6.0", "errno", "libc", "linux-raw-sys", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1247,15 +1432,14 @@ dependencies = [ name = "sbg-rs" version = "0.1.0" dependencies = [ - "atsamd-hal", - "bitflags 2.4.2", + "bitflags 2.6.0", "cmake", "common-arm", "cortex-m", "cortex-m-rt", "defmt", - "embedded-hal", - "heapless", + "embedded-hal 0.2.7", + "heapless 0.7.17", "messages", "nb 1.1.0", "serde", @@ -1278,9 +1462,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.21" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b97ed7a9823b74f99c7742f5336af7be5ecd3eeafcb1507d1fa93347b1d589b0" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -1288,41 +1472,6 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" -[[package]] -name = "sensor" -version = "0.1.0" -dependencies = [ - "atsamd-hal", - "common-arm", - "common-arm-atsame", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "defmt", - "embedded-alloc", - "embedded-sdmmc", - "heapless", - "messages", - "postcard", - "sbg-rs", - "systick-monotonic", - "typenum", -] - -[[package]] -name = "sensor_v2" -version = "0.1.0" -dependencies = [ - "common-arm", - "common-arm-stm32h7", - "cortex-m", - "cortex-m-rt", - "cortex-m-rtic", - "messages", - "postcard", - "stm32h7xx-hal", -] - [[package]] name = "seq-macro" version = "0.3.5" @@ -1331,22 +1480,31 @@ checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.195" +version = "1.0.208" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63261df402c67811e9ac6def069e4786148c4563f4b50fd4bf30aa370d626b02" +checksum = "cff085d2cb684faa248efb494c39b68e522822ac0de72ccf08109abde717cfb2" dependencies = [ "serde_derive", ] +[[package]] +name = "serde_arrays" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38636132857f68ec3d5f3eb121166d2af33cb55174c4d5ff645db6165cbef0fd" +dependencies = [ + "serde", +] + [[package]] name = "serde_derive" -version = "1.0.195" +version = "1.0.208" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46fe8f8603d81ba86327b23a2e9cdf49e1255fb94a4c5f297f6ee0547178ea2c" +checksum = "24008e81ff7613ed8e5ba0cfaf24e2c2f1e5b8a0495711e44fcd4882fca62bcf" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", ] [[package]] @@ -1391,17 +1549,30 @@ dependencies = [ "serial-core", ] +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + [[package]] name = "simple_example" version = "0.1.0" dependencies = [ - "atsamd-hal", - "common-arm-atsame", + "chrono", + "common-arm", + "common-arm-stm32h7", "cortex-m", "cortex-m-rt", + "cortex-m-rtic", "defmt", "defmt-rtt", - "panic-halt", + "embedded-alloc", + "heapless 0.7.17", + "messages", + "panic-probe", + "sbg-rs", + "stm32h7xx-hal", ] [[package]] @@ -1425,6 +1596,15 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" +[[package]] +name = "stm32-metapac" +version = "15.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "deabea56a8821dcea05d0109f3ab3135f31eb572444e5da203d06149c594c8c6" +dependencies = [ + "cortex-m", +] + [[package]] name = "stm32h7" version = "0.15.1" @@ -1439,16 +1619,19 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" -version = "0.15.1" -source = "git+https://github.com/stm32-rs/stm32h7xx-hal#cd7c125cceceada4a3c70cc7387d187b00f9f7d2" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3bd869329be25440b24e2b3583a1c016151b4a54bc36d96d82af7fcd9d010b98" dependencies = [ "bare-metal 1.0.0", "cast", + "chrono", "cortex-m", "defmt", "embedded-dma", - "embedded-hal", + "embedded-hal 0.2.7", "embedded-storage", + "fdcan", "fugit", "nb 1.1.0", "paste", @@ -1456,35 +1639,6 @@ dependencies = [ "void", ] -[[package]] -name = "stm32l0" -version = "0.15.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "37c35ac5e2c330137c257b7254eb4593e3e8312104dedc1880447654cb8dd50f" -dependencies = [ - "bare-metal 1.0.0", - "cortex-m", - "cortex-m-rt", - "vcell", -] - -[[package]] -name = "stm32l0xx-hal" -version = "0.10.0" -source = "git+https://github.com/stm32-rs/stm32l0xx-hal#7132578d4c1d92e8c7afa777c0ac2e29855a2bdd" -dependencies = [ - "as-slice", - "cast", - "cortex-m", - "cortex-m-rt", - "embedded-hal", - "embedded-time", - "nb 1.1.0", - "rtcc", - "stm32l0", - "void", -] - [[package]] name = "syn" version = "0.15.44" @@ -1502,19 +1656,19 @@ version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "unicode-ident", ] [[package]] name = "syn" -version = "2.0.48" +version = "2.0.74" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f3531638e407dfc0814761abb7c00a5b54992b849452a0646b7f65c9f770f3f" +checksum = "1fceb41e3d546d0bd83421d3409b1460cc7444cd389341a4c880fe7a042cb3d7" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", + "proc-macro2 1.0.86", + "quote 1.0.36", "unicode-ident", ] @@ -1531,17 +1685,26 @@ dependencies = [ [[package]] name = "tempfile" -version = "3.9.0" +version = "3.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01ce4141aa927a6d1bd34a041795abd0db1cccba5d5f24b009f694bdf3a1f3fa" +checksum = "04cbcdd0c794ebb0d4cf35e88edd2f7d2c4c3e9a5a6dab322839b321c6a87a64" dependencies = [ "cfg-if", "fastrand", - "redox_syscall", + "once_cell", "rustix", "windows-sys", ] +[[package]] +name = "termcolor" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" +dependencies = [ + "winapi-util", +] + [[package]] name = "termios" version = "0.2.2" @@ -1553,22 +1716,45 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.56" +version = "1.0.63" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d54378c645627613241d077a3a79db965db602882668f9136ac42af9ecb730ad" +checksum = "c0342370b38b6a11b6cc11d6a805569958d54cfa061a29969c3b5ce2ea405724" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.56" +version = "1.0.63" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4558b58466b9ad7ca0f102865eccc95938dca1a74a856f2b57b6629050da261" +dependencies = [ + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", +] + +[[package]] +name = "ts-rs" +version = "6.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa0faa943b50f3db30a20aa7e265dbc66076993efed8463e8de414e5d06d3471" +checksum = "4added4070a4fdf9df03457206cd2e4b12417c8560a2954d91ffcbe60177a56a" dependencies = [ - "proc-macro2 1.0.78", - "quote 1.0.35", - "syn 2.0.48", + "thiserror", + "ts-rs-macros", +] + +[[package]] +name = "ts-rs-macros" +version = "6.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f807fdb3151fee75df7485b901a89624358cd07a67a8fb1a5831bf5a07681ff" +dependencies = [ + "Inflector", + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 1.0.109", + "termcolor", ] [[package]] @@ -1577,6 +1763,30 @@ version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" +[[package]] +name = "ublox" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ad0c3df1bc466bd97f94ab437aab9116d5c15909c8a5cb9905ee3c11df998e8" +dependencies = [ + "bitflags 2.6.0", + "chrono", + "num-traits", + "serde", + "ublox_derive", +] + +[[package]] +name = "ublox_derive" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dea60f23c86d2329b7162f6ebb82854bc77b992a9984e1fdfe8707f88e0e3e69" +dependencies = [ + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 1.0.109", +] + [[package]] name = "unarray" version = "0.1.4" @@ -1603,9 +1813,9 @@ checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" [[package]] name = "version_check" -version = "0.9.4" +version = "0.9.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" [[package]] name = "void" @@ -1637,6 +1847,15 @@ version = "0.11.0+wasi-snapshot-preview1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" +[[package]] +name = "winapi-util" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +dependencies = [ + "windows-sys 0.59.0", +] + [[package]] name = "windows-sys" version = "0.52.0" @@ -1646,15 +1865,25 @@ dependencies = [ "windows-targets", ] +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + [[package]] name = "windows-targets" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ "windows_aarch64_gnullvm", "windows_aarch64_msvc", "windows_i686_gnu", + "windows_i686_gnullvm", "windows_i686_msvc", "windows_x86_64_gnu", "windows_x86_64_gnullvm", @@ -1663,42 +1892,69 @@ dependencies = [ [[package]] name = "windows_aarch64_gnullvm" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" [[package]] name = "windows_aarch64_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" [[package]] name = "windows_i686_gnu" -version = "0.52.0" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" [[package]] name = "windows_i686_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" [[package]] name = "windows_x86_64_gnu" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" [[package]] name = "windows_x86_64_gnullvm" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" [[package]] name = "windows_x86_64_msvc" -version = "0.52.0" +version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "zerocopy" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b9b4fd18abc82b8136838da5d50bae7bdea537c574d8dc1a34ed098d6c166f0" +dependencies = [ + "byteorder", + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.7.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa4f8080344d4671fb4e831a13ad1e68092748387dfc4f55e356242fae12ce3e" +dependencies = [ + "proc-macro2 1.0.86", + "quote 1.0.36", + "syn 2.0.74", +] diff --git a/Cargo.toml b/Cargo.toml index 4984755c..317c9144 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,30 +1,25 @@ [workspace] resolver = "2" -members = [ - "boards/*", - "examples/*", - "libraries/*" -] +members = ["boards/*", "examples/*", "libraries/*"] # Specify which members to build by default. Some libraries, such as messages, contain dev-dependencies that will give # compile errors if built directly. -default-members = [ - "boards/*", - "examples/*" -] +default-members = ["boards/*", "examples/*"] [workspace.dependencies.embedded-hal] -version="0.2.7" +version = "0.2.7" [workspace.dependencies.stm32h7xx-hal] -git = "https://github.com/stm32-rs/stm32h7xx-hal" +#git = "https://github.com/uorocketry/stm32h7xx-hal" +version = "0.16.0" # We use 35 even though we have the 33. -features = ["defmt", "rt", "stm32h735" ] - +features = ["defmt", "rt", "stm32h735", "can", "rtc"] [workspace.dependencies.atsamd-hal] git = "https://github.com/uorocketry/atsamd" -features = ["same51j", "same51j-rt", "dma", "can"] +#git = "https://github.com/atsamd-rs/atsamd" +#version = "0.17.0" +features = ["defmt", "same51j", "same51j-rt", "dma", "can"] [workspace.dependencies.stm32l0xx-hal] git = "https://github.com/stm32-rs/stm32l0xx-hal" @@ -44,30 +39,28 @@ features = ["critical-section-single-core"] # Using LTO causes issues with GDB. lto = false -# Only optimize dependencies for size in debug, keeping the top crate debug friendly -[profile.dev.package."*"] -opt-level = "s" +# # Try to remove this +# [profile.dev.build-override] +# #opt-level = 0 Commenting this may cause issues. +# debug = true + +[profile.release] +codegen-units = 1 +debug = 2 +debug-assertions = false # <- +incremental = false +# lto = 'thin' +opt-level = 3 # <- +overflow-checks = false # <- [profile.dev.package.sbg-rs] opt-level = 0 debug = true -# Try to remove this -[profile.dev.build-override] -#opt-level = 0 Commenting this may cause issues. -debug = true - -[profile.release] -# symbols are nice and they don't increase the size on Flash -#debug = true -debug = 1 -#lto = true There is an issue with this where it says the interrupts symbol is defined multiple times. Only happens for the STM32H7XX. -opt-level = 1 - [profile.release.package.sbg-rs] debug = true opt-level = 0 #Try to remove this [profile.release.build-override] -#opt-level = 0 \ No newline at end of file +opt-level = 0 diff --git a/Embed.toml b/Embed.toml index 5c5defed..a62c5357 100644 --- a/Embed.toml +++ b/Embed.toml @@ -10,7 +10,7 @@ protocol = "Swd" enabled = true [default.general] -chip = "ATSAME51J20A" +chip = "STM32H733VGTx" [default.probe] protocol = "Swd" diff --git a/boards/beacon/Cargo.toml b/boards/beacon/Cargo.toml deleted file mode 100644 index 064ce6b8..00000000 --- a/boards/beacon/Cargo.toml +++ /dev/null @@ -1,17 +0,0 @@ -[package] -name = "beacon" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -cortex-m-rtic = "1.1.3" -common-arm-stm32l0 = {path = "../../libraries/common-arm-stm32l0"} -common-arm = {path = "../../libraries/common-arm"} -stm32l0xx-hal = { workspace = true } -postcard = "1.0.2" -messages = { path = "../../libraries/messages" } -systick-monotonic = "1.0.1" \ No newline at end of file diff --git a/boards/beacon/src/data_manager.rs b/boards/beacon/src/data_manager.rs deleted file mode 100644 index 6d600691..00000000 --- a/boards/beacon/src/data_manager.rs +++ /dev/null @@ -1,140 +0,0 @@ -use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; - -#[derive(Clone)] -pub struct DataManager { - pub air: Option, - pub ekf_nav_1: Option, - pub ekf_nav_2: Option, - pub ekf_nav_acc: Option, - pub ekf_quat: Option, - pub imu_1: Option, - pub imu_2: Option, - pub utc_time: Option, - pub gps_vel: Option, - pub gps_vel_acc: Option, - pub gps_pos_1: Option, - pub gps_pos_2: Option, - pub gps_pos_acc: Option, - pub state: Option, - pub logging_rate: Option, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav_1: None, - ekf_nav_2: None, - ekf_nav_acc: None, - ekf_quat: None, - imu_1: None, - imu_2: None, - utc_time: None, - gps_vel: None, - gps_vel_acc: None, - gps_pos_1: None, - gps_pos_2: None, - gps_pos_acc: None, - state: None, - logging_rate: Some(RadioRate::Slow), // start slow. - } - } - - pub fn get_logging_rate(&mut self) -> RadioRate { - if let Some(rate) = self.logging_rate.take() { - let rate_cln = rate.clone(); - self.logging_rate = Some(rate); - return rate_cln; - } - self.logging_rate = Some(RadioRate::Slow); - return RadioRate::Slow; - } - - /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 13] { - [ - self.air.take(), - self.ekf_nav_1.take(), - self.ekf_nav_2.take(), - self.ekf_nav_acc.take(), - self.ekf_quat.take(), - self.imu_1.take(), - self.imu_2.take(), - self.utc_time.take(), - self.gps_vel.take(), - self.gps_vel_acc.take(), - self.gps_pos_1.take(), - self.gps_pos_2.take(), - self.gps_pos_acc.take(), - ] - } - - pub fn clone_states(&self) -> [Option; 1] { - [self.state.clone()] - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SensorData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SensorData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SensorData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SensorData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SensorData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SensorData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SensorData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SensorData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SensorData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SensorData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SensorData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SensorData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } - }, - messages::Data::State(state) => { - self.state = Some(state.data); - } - messages::Data::Command(command) => match command.data { - messages::command::CommandData::RadioRateChange(command_data) => { - self.logging_rate = Some(command_data.rate); - } - messages::command::CommandData::DeployDrogue(_) => {} - messages::command::CommandData::DeployMain(_) => {} - messages::command::CommandData::PowerDown(_) => {} - }, - _ => {} - } - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/beacon/src/main.rs b/boards/beacon/src/main.rs deleted file mode 100644 index fbf26102..00000000 --- a/boards/beacon/src/main.rs +++ /dev/null @@ -1,121 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod types; - -use common_arm::SdManager; -use common_arm::*; -use data_manager::DataManager; -// use defmt::info; -use hal::{ - gpio::*, - gpio::{ - gpioa::{PA10, PA9}, - Output, PushPull, - }, - prelude::*, - rcc::Config, -}; -use messages::sensor::Sensor; -use messages::*; -use stm32l0xx_hal as hal; - -use systick_monotonic::*; - -// use https://github.com/lora-rs/lora-rs.git - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - stm32l0xx_hal::pac::SCB::sys_reset(); -} - -// Add dispatchers -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EXTI0_1, EXTI2_3, EXTI4_15])] -mod app { - - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - } - - #[local] - struct Local { - // add leds - green_led: PA9>, - red_led: PA10>, - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let device = cx.device; - let core = cx.core; - - // configure the clock - let mut rcc = device.RCC.freeze(Config::hse(64_000_000u32.Hz())); - - // configure leds - let gpioa = device.GPIOA.split(&mut rcc); - let mut green_led = gpioa.pa9.into_push_pull_output(); - let mut red_led = gpioa.pa10.into_push_pull_output(); - - /* Monotonic clock */ - // implement the monotonic clock - let mono = Systick::new(core.SYST, rcc.clocks.sys_clk().0); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - }, - Local { green_led, red_led }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - /** - * Sends a message over CAN. - */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [&em])] - fn send_internal(cx: send_internal::Context, m: Message) { - todo!("Send messages over CAN"); - } - - #[task(capacity = 5)] - fn send_external(cx: send_external::Context, m: Message) { - todo!("Send messages over LORA"); - } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [green_led, red_led], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.red_led.toggle().ok(); - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.green_led.toggle().ok(); // doesn't matter if an LED fails to blink - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } -} diff --git a/boards/beacon/src/types.rs b/boards/beacon/src/types.rs deleted file mode 100644 index c13f876d..00000000 --- a/boards/beacon/src/types.rs +++ /dev/null @@ -1,7 +0,0 @@ -use messages::node::Node; -use messages::node::Node::BeaconBoard; - -// ------- -// Node ID -// ------- -pub static COM_ID: Node = BeaconBoard; diff --git a/boards/camera/src/communication.rs b/boards/camera/src/communication.rs index d3a36b35..4aa3a64b 100644 --- a/boards/camera/src/communication.rs +++ b/boards/camera/src/communication.rs @@ -8,8 +8,8 @@ use atsamd_hal::clock::v2::Source; use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; use atsamd_hal::pac::CAN0; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; -use common_arm::mcan::message::{rx, Raw}; +use common_arm_atsame::mcan; +use common_arm_atsame::mcan::message::{rx, Raw}; use defmt::info; use mcan::bus::Can; use mcan::embedded_can as ecan; diff --git a/boards/camera/src/main.rs b/boards/camera/src/main.rs index a99c7e69..49bda653 100644 --- a/boards/camera/src/main.rs +++ b/boards/camera/src/main.rs @@ -13,9 +13,9 @@ use crate::state_machine::{StateMachine, StateMachineContext}; use atsamd_hal as hal; use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::Source; -use common_arm::mcan; use common_arm::ErrorManager; use common_arm::*; +use common_arm_atsame::mcan; use communication::Capacities; use hal::gpio::Pins; use hal::gpio::{Pin, PushPullOutput, PB16, PB17}; diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml deleted file mode 100644 index fd97f157..00000000 --- a/boards/communication/Cargo.toml +++ /dev/null @@ -1,21 +0,0 @@ -[package] -name = "communication" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -typenum = "1.16.0" -embedded-sdmmc = "0.3.0" \ No newline at end of file diff --git a/boards/communication/src/communication.rs b/boards/communication/src/communication.rs deleted file mode 100644 index bdee0df0..00000000 --- a/boards/communication/src/communication.rs +++ /dev/null @@ -1,326 +0,0 @@ -use crate::data_manager::DataManager; -use crate::types::*; -use atsamd_hal::can::Dependencies; -use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::pclk::PclkToken; -use atsamd_hal::clock::v2::types::Can0; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{Alternate, AlternateI, Disabled, Floating, Pin, I, PA22, PA23, PB16, PB17}; -use atsamd_hal::pac::CAN0; -use atsamd_hal::pac::MCLK; -use atsamd_hal::pac::SERCOM5; -use atsamd_hal::prelude::_embedded_hal_serial_Read; -use atsamd_hal::sercom; -use atsamd_hal::sercom::uart; -use atsamd_hal::sercom::uart::Uart; -use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; -use atsamd_hal::typelevel::Increment; -use common_arm::mcan; -use common_arm::mcan::message::{rx, Raw}; -use common_arm::mcan::tx_buffers::DynTx; -use common_arm::{herror, HydraError}; -use heapless::HistoryBuffer; -use heapless::Vec; -use mavlink::embedded::Read; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::messageram::SharedMemory; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; -use messages::mavlink; -use messages::ErrorContext; -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; -use typenum::{U0, U128, U32, U64}; - -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::RecoveryBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Recovery filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::node::Node::SensorBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Sensor filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::PowerBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Power filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::GroundStation.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(_) => { - herror!(Error, ErrorContext::UnkownCanMessage); - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(_) => { - herror!(Error, ErrorContext::UnkownCanMessage); - } - } - } - } - _ => (), - } - } - } -} - -pub struct RadioDevice { - transmitter: Uart, - receiver: Uart, -} - -impl RadioDevice { - pub fn new( - radio_token: PclkToken, - mclk: &MCLK, - sercom: SERCOM5, - rx_pin: Pin>, - tx_pin: Pin>, - gclk0: S, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (pclk_radio, gclk0) = Pclk::enable(radio_token, gclk0); - let pads = uart::Pads::::default() - .rx(rx_pin) - .tx(tx_pin); - let uart = GroundStationUartConfig::new(mclk, sercom, pads, pclk_radio.freq()) - .baud( - 57600.Hz(), - uart::BaudMode::Fractional(uart::Oversampling::Bits16), - ) - .enable(); - let (mut rx, tx) = uart.split(); - // setup interrupts - rx.enable_interrupts(uart::Flags::RXC); - ( - RadioDevice { - transmitter: tx, - receiver: rx, - }, - gclk0, - ) - } -} - -pub struct RadioManager { - buf: HistoryBuffer, - radio: RadioDevice, - mav_sequence: u8, -} - -impl RadioManager { - pub fn new(radio: RadioDevice) -> Self { - let buf = HistoryBuffer::new(); - RadioManager { - buf, - radio, - mav_sequence: 0, - } - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - - let mav_header = mavlink::MavHeader { - system_id: 1, - component_id: 1, - sequence: self.increment_mav_sequence(), - }; - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { message: payload }, - ); - mavlink::write_versioned_msg( - &mut self.radio.transmitter, - mavlink::MavlinkVersion::V2, - mav_header, - &mav_message, - )?; - Ok(()) - } - pub fn increment_mav_sequence(&mut self) -> u8 { - self.mav_sequence = self.mav_sequence.wrapping_add(1); - self.mav_sequence - } - pub fn receive_message(&mut self) -> Result { - if let Ok(data) = self.radio.receiver.read() { - // lets add this data to the buffer and see if we can parse it - self.buf.write(data); - let (_header, msg) = - mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; - // Do we need the header? - match msg { - mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - return Ok(postcard::from_bytes::(&msg.message)?); - // weird Ok syntax to coerce to hydra error type. - } - _ => { - herror!(Error, ErrorContext::UnkownPostcardMessage); - return Err(mavlink::error::MessageReadError::Io.into()); - } - } - } else { - return Err(mavlink::error::MessageReadError::Io.into()); - } - } -} - -impl Read for RadioManager { - fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), mavlink::error::MessageReadError> { - let len = buf.len().min(self.buf.len()); - buf[..len].copy_from_slice(&self.buf[..len]); - Ok(()) - } - fn read_u8(&mut self) -> Result { - if !self.buf.is_empty() { - Ok(self.buf[0]) - } else { - Err(mavlink::error::MessageReadError::Io) - } - } -} diff --git a/boards/communication/src/health.rs b/boards/communication/src/health.rs deleted file mode 100644 index 1f5cbd97..00000000 --- a/boards/communication/src/health.rs +++ /dev/null @@ -1,85 +0,0 @@ -//! Health related code -//! Would've liked to have this live in common-arm-atsame but the pins and adc are not standardised -//! for all boards which poses the problem of giving an adc to a wrong pin in a generic way. - -use atsamd_hal::gpio::{Alternate, Pin, B, PB00, PB01, PB02, PB03, PB05, PB06, PB07, PB08, PB09}; -use atsamd_hal::{adc::Adc, ehal::adc::OneShot, pac::ADC0, pac::ADC1}; -use common_arm::HealthMonitorChannels; - -// make sure to define the ADC types in types.rs - -// I don't think this should own the ADC object, but rather when a call to evaluate is invoke it should be taken control -// and then released when the function returns. Refactor this later. -pub struct HealthMonitorChannelsCommunication { - reader: Adc, - reader1: Adc, - pin_3v3: Pin>, - pin_5v: Pin>, - pin_pyro: Pin>, - pin_vcc: Pin>, - pin_ext_3v3: Pin>, - pin_ext_5v: Pin>, - pin_int_5v: Pin>, - pin_int_3v3: Pin>, - pin_failover: Pin>, -} - -impl HealthMonitorChannels for HealthMonitorChannelsCommunication { - fn get_3v3(&mut self) -> Option { - self.reader.read(&mut self.pin_3v3).ok() - } - fn get_5v(&mut self) -> Option { - self.reader.read(&mut self.pin_5v).ok() - } - fn get_pyro(&mut self) -> Option { - self.reader.read(&mut self.pin_pyro).ok() - } - fn get_vcc(&mut self) -> Option { - self.reader.read(&mut self.pin_vcc).ok() - } - fn get_int_5v(&mut self) -> Option { - self.reader.read(&mut self.pin_int_5v).ok() - } - fn get_int_3v3(&mut self) -> Option { - self.reader.read(&mut self.pin_int_3v3).ok() - } - fn get_ext_5v(&mut self) -> Option { - self.reader1.read(&mut self.pin_ext_5v).ok() - } - fn get_ext_3v3(&mut self) -> Option { - self.reader1.read(&mut self.pin_ext_3v3).ok() - } - fn get_failover(&mut self) -> Option { - self.reader1.read(&mut self.pin_failover).ok() - } -} - -impl HealthMonitorChannelsCommunication { - pub fn new( - reader: Adc, - reader1: Adc, - pin_3v3: Pin>, - pin_5v: Pin>, - pin_pyro: Pin>, - pin_vcc: Pin>, - pin_ext_3v3: Pin>, - pin_ext_5v: Pin>, - pin_int_5v: Pin>, - pin_int_3v3: Pin>, - pin_failover: Pin>, - ) -> Self { - HealthMonitorChannelsCommunication { - reader, - reader1, - pin_3v3, - pin_5v, - pin_pyro, - pin_vcc, - pin_ext_3v3, - pin_ext_5v, - pin_int_5v, - pin_int_3v3, - pin_failover, - } - } -} diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs deleted file mode 100644 index dcd1ea95..00000000 --- a/boards/communication/src/main.rs +++ /dev/null @@ -1,362 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod health; -mod types; - -use atsamd_hal as hal; -use common_arm::mcan; -use common_arm::HealthManager; -use common_arm::HealthMonitor; -use common_arm::SdManager; -use common_arm::*; -use communication::Capacities; -use communication::{RadioDevice, RadioManager}; -use data_manager::DataManager; -use hal::adc::Adc; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::Source; -use hal::gpio::Pins; -use hal::gpio::{ - Alternate, Output, Pin, PushPull, PushPullOutput, C, PA05, PB12, PB13, PB14, PB15, -}; -use hal::prelude::*; -use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet1, Sercom4}; -use health::HealthMonitorChannelsCommunication; -use mcan::messageram::SharedMemory; -use messages::command::RadioRate; -use messages::health::Health; -use messages::state::State; -use messages::*; -use systick_monotonic::*; -use types::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - health_manager: HealthManager, - radio_manager: RadioManager, - can0: communication::CanDevice0, - } - - #[local] - struct Local { - led: Pin, - sd_manager: SdManager< - Spi< - Config< - Pads< - hal::pac::SERCOM4, - IoSet1, - Pin>, - Pin>, - Pin>, - >, - >, - Duplex, - >, - Pin>, - >, - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - // let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); - // let dmaChannels = dmac.split(); - - /* Logging Setup */ - HydraLogging::set_ground_station_callback(queue_gs_message); - - /* Clock setup */ - let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - let gclk0 = clocks.gclk0; - - // SAFETY: Misusing the PAC API can break the system. - // This is safe because we only steal the MCLK. - let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; - - /* CAN config */ - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can0, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - /* Radio config */ - let (radio, gclk0) = RadioDevice::new( - tokens.pclks.sercom5, - &mclk, - peripherals.SERCOM5, - pins.pb17, - pins.pb16, - gclk0, - ); - - let radio_manager = RadioManager::new(radio); - - /* SD config */ - let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); - let pads_spi = spi::Pads::::default() - .sclk(pins.pb13) - .data_in(pins.pb15) - .data_out(pins.pb12); - let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM4, pads_spi, pclk_sd.freq()) - .length::() - .bit_order(spi::BitOrder::MsbFirst) - .spi_mode(spi::MODE_0) - .enable(); - let sd_manager = SdManager::new(sdmmc_spi, pins.pb14.into_push_pull_output()); - - /* Setup ADC clocks */ - let (_pclk_adc0, gclk0) = Pclk::enable(tokens.pclks.adc0, gclk0); - let (_pclk_adc1, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); - /* Setup ADC */ - let adc0 = Adc::adc0(peripherals.ADC0, &mut mclk); - let adc1 = Adc::adc1(peripherals.ADC1, &mut mclk); - - /* Setup Health Monitor */ - let health_monitor_channels = HealthMonitorChannelsCommunication::new( - adc0, - adc1, - pins.pb01.into(), - pins.pb02.into(), - pins.pb03.into(), - pins.pb00.into(), - pins.pb06.into(), - pins.pb07.into(), - pins.pb08.into(), - pins.pb09.into(), - pins.pb05.into(), - ); - - let health_monitor = HealthMonitor::new(health_monitor_channels, 10000, 5000, 1023); - let health_manager = HealthManager::new(health_monitor); - - /* Status LED */ - let led = pins.pa05.into_push_pull_output(); - - /* Spawn tasks */ - sensor_send::spawn().ok(); - state_send::spawn().ok(); - blink::spawn().ok(); - report_health::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - health_manager, - radio_manager, - can0, - }, - Local { led, sd_manager }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - /// Handles the CAN0 interrupt. - #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] - fn can0(mut cx: can0::Context) { - cx.shared.can0.lock(|can| { - cx.shared - .data_manager - .lock(|data_manager| can.process_data(data_manager)); - }); - } - /** - * Sends a message over CAN. - */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [can0, &em])] - fn send_internal(mut cx: send_internal::Context, m: Message) { - cx.shared.em.run(|| { - cx.shared.can0.lock(|can| can.send_message(m))?; - Ok(()) - }); - } - - /// Receives a log message from the custom logger so that it can be sent over the radio. - pub fn queue_gs_message(d: impl Into) { - let message = Message::new(&monotonics::now(), COM_ID, d.into()); - - send_gs::spawn(message).ok(); - } - - /** - * Sends a message to the radio over UART. - */ - #[task(capacity = 10, shared = [&em, radio_manager])] - fn send_gs(mut cx: send_gs::Context, m: Message) { - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - radio_manager.send_message(m)?; - Ok(()) - }) - }); - } - - #[task(capacity = 10, local = [sd_manager], shared = [&em])] - fn sd_dump(cx: sd_dump::Context, m: Message) { - let manager = cx.local.sd_manager; - cx.shared.em.run(|| { - let mut buf: [u8; 255] = [0; 255]; - let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; - if let Some(mut file) = manager.file.take() { - manager.write(&mut file, &msg_ser)?; - manager.file = Some(file); - } else if let Ok(mut file) = manager.open_file("log.txt") { - manager.write(&mut file, &msg_ser)?; - manager.file = Some(file); - } - Ok(()) - }); - } - - /** - * Sends information about the sensors. - */ - #[task(shared = [data_manager, &em])] - fn sensor_send(mut cx: sensor_send::Context) { - let (sensors, logging_rate) = cx - .shared - .data_manager - .lock(|data_manager| (data_manager.take_sensors(), data_manager.get_logging_rate())); - - cx.shared.em.run(|| { - for msg in sensors { - match msg { - Some(x) => { - spawn!(send_gs, x.clone())?; - spawn!(sd_dump, x)?; - } - None => { - continue; - } - } - } - Ok(()) - }); - match logging_rate { - RadioRate::Fast => { - spawn_after!(sensor_send, ExtU64::millis(250)).ok(); - } - RadioRate::Slow => { - spawn_after!(sensor_send, ExtU64::millis(2000)).ok(); - } - } - } - - #[task(shared = [data_manager, &em])] - fn state_send(mut cx: state_send::Context) { - let state_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.state.clone()); - cx.shared.em.run(|| { - if let Some(x) = state_data { - let message = Message::new(&monotonics::now(), COM_ID, State::new(x)); - spawn!(send_gs, message)?; - } // if there is none we still return since we simply don't have data yet. - Ok(()) - }); - spawn_after!(state_send, ExtU64::secs(5)).ok(); - } - - /** - * Simple health report - */ - #[task(shared = [&em, health_manager])] - fn report_health(mut cx: report_health::Context) { - cx.shared.em.run(|| { - let msg = cx.shared.health_manager.lock(|health_manager| { - let state = health_manager.evaluate(); - Message::new( - &monotonics::now(), - COM_ID, - Health::new(health_manager.monitor.data.clone(), state), - ) - }); - spawn!(send_gs, msg)?; - spawn_after!(report_health, ExtU64::secs(5))?; - Ok(()) - }); - } - - #[task(binds = SERCOM5_2, shared = [&em, radio_manager])] - fn radio_rx(mut cx: radio_rx::Context) { - cx.shared.radio_manager.lock(|radio_manager| { - cx.shared.em.run(|| { - let msg = radio_manager.receive_message()?; - spawn!(send_internal, msg)?; // just broadcast the message throughout the system for now. - Ok(()) - }); - }); - } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [led], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - cx.local.led.toggle()?; - if cx.shared.em.has_error() { - spawn_after!(blink, ExtU64::millis(200))?; - } else { - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } - - // extern "Rust" { - // #[task(binds = DMAC_0, shared=[&em], local=[radio_manager])] - // fn radio_dma(context: radio_dma::Context); - // } -} diff --git a/boards/communication/src/types.rs b/boards/communication/src/types.rs deleted file mode 100644 index 34315d1c..00000000 --- a/boards/communication/src/types.rs +++ /dev/null @@ -1,16 +0,0 @@ -use atsamd_hal::gpio::*; -use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::{uart, IoSet1, Sercom5}; -use messages::node::Node; -use messages::node::Node::CommunicationBoard; - -// ------- -// Node ID -// ------- -pub static COM_ID: Node = CommunicationBoard; - -// ------- -// Ground Station -// ------- -pub type GroundStationPads = uart::PadsFromIds; -pub type GroundStationUartConfig = uart::Config; diff --git a/boards/sensor/Cargo.toml b/boards/gps/Cargo.toml similarity index 69% rename from boards/sensor/Cargo.toml rename to boards/gps/Cargo.toml index 13142f56..5447d4b9 100644 --- a/boards/sensor/Cargo.toml +++ b/boards/gps/Cargo.toml @@ -1,23 +1,26 @@ -[package] -name = "sensor" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -sbg-rs = {path = "../../libraries/sbg-rs"} -embedded-sdmmc = "0.3.0" # port to v5 -typenum = "1.16.0" -embedded-alloc = "0.5.0" +[package] +name = "gps" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +cortex-m = { workspace = true } +cortex-m-rt = "^0.7.0" +cortex-m-rtic = "1.1.3" +systick-monotonic = "1.0.1" +postcard = "1.0.2" +heapless = "0.7.16" +common-arm-atsame = { path = "../../libraries/common-arm-atsame" } +common-arm = { path = "../../libraries/common-arm" } +atsamd-hal = { workspace = true } +messages = { path = "../../libraries/messages" } +typenum = "1.16.0" +embedded-sdmmc = "0.8.0" +#panic-halt = "0.2.0" +defmt = "0.3" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +ublox = {version = "0.4.5", features = ['serde', 'alloc'], default-features = false} +embedded-alloc = "0.5.0" diff --git a/boards/gps/src/communication.rs b/boards/gps/src/communication.rs new file mode 100644 index 00000000..ad58f3fe --- /dev/null +++ b/boards/gps/src/communication.rs @@ -0,0 +1,388 @@ + +use crate::data_manager::DataManager; +use atsamd_hal::can::Dependencies; +use atsamd_hal::clock::v2::ahb::AhbClk; +use atsamd_hal::clock::v2::gclk::{Gclk0Id}; +use atsamd_hal::clock::v2::pclk::Pclk; +use atsamd_hal::clock::v2::types::{Can0, Can1}; +use atsamd_hal::clock::v2::Source; +use atsamd_hal::gpio::{ + Alternate, AlternateI, Pin, I, PA22, PA23, +}; +use atsamd_hal::gpio::{AlternateH, H, PB14, PB15}; +use atsamd_hal::pac::{CAN0, CAN1}; +use atsamd_hal::prelude::*; +use atsamd_hal::typelevel::Increment; +use common_arm::HydraError; +use common_arm_atsame::mcan; +use common_arm_atsame::mcan::message::{rx, Raw}; +use common_arm_atsame::mcan::tx_buffers::DynTx; +use defmt::error; +use defmt::info; +use heapless::Vec; +use mcan::bus::Can; +use mcan::embedded_can as ecan; +use mcan::interrupt::state::EnabledLine0; +use mcan::interrupt::{Interrupt, OwnedInterruptSet}; +use mcan::message::tx; +use mcan::messageram::SharedMemory; +use mcan::{ + config::{BitTiming, Mode}, + filter::{Action, Filter}, +}; +use messages::mavlink::{self}; +use messages::Message; +use postcard::from_bytes; +use systick_monotonic::*; +use typenum::{U0, U128, U32, U64}; +pub struct Capacities; + +impl mcan::messageram::Capacities for Capacities { + type StandardFilters = U128; + type ExtendedFilters = U64; + type RxBufferMessage = rx::Message<64>; + type DedicatedRxBuffers = U64; + type RxFifo0Message = rx::Message<64>; + type RxFifo0 = U64; + type RxFifo1Message = rx::Message<64>; + type RxFifo1 = U64; + type TxMessage = tx::Message<64>; + type TxBuffers = U32; + type DedicatedTxBuffers = U0; + type TxEventFifo = U32; +} + +// macro_rules! create_filter { +// ($can:expr, $action:expr, $sender:expr) => { +// $can.filters_standard() +// .push(Filter::Classic { +// action: $action, +// filter: ecan::StandardId::new($sender.into()).unwrap(), +// mask: ecan::StandardId::ZERO, +// }) +// .unwrap_or_else(|_| panic!("Filter Error")); +// }; +// } + +pub struct CanDevice0 { + pub can: Can< + 'static, + Can0, + Dependencies>, Pin>, CAN0>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanDevice0 { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN0, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = mcan::bus::CanConfigurable::new( + 200.kHz(), + can_dependencies, + can_memory, + ) + .unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: true, + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::RecoveryBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Recovery filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo1, + filter: ecan::StandardId::new(messages::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanDevice0 { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: true, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + // info!("Received message {}", data.clone()); + + data_manager.handle_data(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + + data_manager.handle_data(data); + } + Err(_) => { + // error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} + +// So I really am not a fan of this can device 0 and can device 1, I think it would be better to have a single generic can manager +// that can also take a list of filters and apply them. + +pub struct CanCommandManager { + pub can: Can< + 'static, + Can1, + Dependencies>, Pin>, CAN1>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanCommandManager { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN1, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = mcan::bus::CanConfigurable::new( + 200.kHz(), // needs a prescaler of 6 to be changed in the mcan source because mcan is meh. + can_dependencies, + can_memory, + ) + .unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: true, + data_phase_timing: BitTiming::new(fugit::RateExtU32::kHz(500)), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + // Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + // Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::RecoveryBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Recovery filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo1, + filter: ecan::StandardId::new(messages::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanCommandManager { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: true, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} \ No newline at end of file diff --git a/boards/gps/src/data_manager.rs b/boards/gps/src/data_manager.rs new file mode 100644 index 00000000..1e7ee142 --- /dev/null +++ b/boards/gps/src/data_manager.rs @@ -0,0 +1,46 @@ +use defmt::info; +use messages::command::RadioRate; +use messages::state::StateData; +use messages::Message; + +#[derive(Clone)] +pub struct DataManager { + pub state: Option, +} + +impl DataManager { + pub fn new() -> Self { + Self { + state: None, + } + } + + pub fn handle_command(&mut self, command: Message) { + info!("Handling command"); + match command.data { + messages::Data::Command(command) => match command.data { + messages::command::CommandData::RadioRateChange(command_data) => { + } + messages::command::CommandData::DeployDrogue(_) => {} + messages::command::CommandData::DeployMain(_) => {} + messages::command::CommandData::PowerDown(_) => {} + messages::command::CommandData::Online(_) => {} + }, + _ => {} + } + } + pub fn handle_data(&mut self, data: Message) { + match data.data { + messages::Data::State(state) => { + self.state = Some(state.data); + } + _ => {} + } + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/gps/src/main.rs b/boards/gps/src/main.rs new file mode 100644 index 00000000..9d46e4dc --- /dev/null +++ b/boards/gps/src/main.rs @@ -0,0 +1,666 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod types; + +use atsamd_hal as hal; +use atsamd_hal::dmac; +use atsamd_hal::dmac::DmaController; +use atsamd_hal::dmac::Transfer; +use atsamd_hal::rtc::Count32Mode; +use common_arm::*; +use common_arm_atsame::mcan; +use communication::CanCommandManager; +use communication::Capacities; +use core::cell::RefCell; +use cortex_m::interrupt::Mutex; +use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; // global logger +use fugit::ExtU64; +use fugit::RateExtU32; +use hal::clock::v2::pclk::Pclk; +use hal::clock::v2::Source; +use hal::gpio::Pins; +use hal::gpio::{Alternate, Pin, PushPullOutput, C, PA02, PA03, PA08, PA09}; +use hal::prelude::*; +use hal::sercom::uart; +use hal::sercom::IoSet1; +use mavlink::embedded::Read; +use mcan::messageram::SharedMemory; +use messages::*; +use panic_probe as _; +use systick_monotonic::*; +use types::GPSTransfer; +use types::GpsUartTx; +use types::GPSBUFFER; +use types::*; +use ublox::{ + CfgPrtUartBuilder, DataBits, InProtoMask, OutProtoMask, Parity, StopBits, UartMode, UartPortId, +}; + +pub static mut BUF_DST: GPSBUFFER = &mut [0; 256]; + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +#[global_allocator] +static HEAP: embedded_alloc::Heap = embedded_alloc::Heap::empty(); + +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop {} +} + +static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] +mod app { + + use atsamd_hal::sercom::Sercom; + use cortex_m::asm; + use ublox::{ + CfgMsgAllPortsBuilder, CfgMsgSinglePortBuilder, NavPosLlh, NavPvt, PacketRef, + UbxPacketRequest, + }; + + use super::*; + + #[shared] + struct Shared { + em: ErrorManager, + data_manager: DataManager, + can0: communication::CanDevice0, + can_command_manager: CanCommandManager, + } + + #[local] + struct Local { + led_green: Pin, + led_red: Pin, + gps_tx: GpsUartTx, + gps_dma_transfer: Option, + // sd_manager: SdManager< + // Spi< + // Config< + // Pads< + // hal::pac::SERCOM0, + // IoSet3, + // Pin>, + // Pin>, + // Pin>, + // >, + // >, + // Duplex, + // >, + // Pin>, + // >, + } + + #[monotonic(binds = SysTick, default = true)] + type MyMono = Systick<100>; // 100 Hz / 10 ms granularity + + #[init(local = [ + #[link_section = ".can"] + can_memory: SharedMemory = SharedMemory::new(), + #[link_section = ".can_command"] + can_command_memory: SharedMemory = SharedMemory::new() + ])] + fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + let mut peripherals = cx.device; + let core = cx.core; + let pins = Pins::new(peripherals.PORT); + + let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); + let dmaChannels = dmac.split(); + + /* Logging Setup */ + HydraLogging::set_ground_station_callback(queue_gs_message); + + /* Clock setup */ + let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( + peripherals.OSCCTRL, + peripherals.OSC32KCTRL, + peripherals.GCLK, + peripherals.MCLK, + &mut peripherals.NVMCTRL, + ); + + let gclk0 = clocks.gclk0; + + // SAFETY: Misusing the PAC API can break the system. + // This is safe because we only steal the MCLK. + let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; + + /* CAN config */ + + let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); + let (can0, gclk0) = communication::CanDevice0::new( + pins.pa23.into_mode(), + pins.pa22.into_mode(), + pclk_can, + clocks.ahbs.can0, + peripherals.CAN0, + gclk0, + cx.local.can_memory, + false, + ); + + let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); + let (can_command_manager, gclk0) = CanCommandManager::new( + pins.pb15.into_mode(), + pins.pb14.into_mode(), + pclk_can_command, + clocks.ahbs.can1, + peripherals.CAN1, + gclk0, + cx.local.can_command_memory, + false, + ); + + // setup external osc + // let xosc0 = atsamd_hal::clock::v2::xosc::Xosc::from_crystal( + // tokens.xosc0, + // pins.pa14, + // pins.pa15, + // 48_000_000.Hz(), + // ).current(CrystalCurrent::Medium) + // .loop_control(true) + // .low_buf_gain(true) + // .start_up_delay(StartUpDelay::Delay488us).enable(); + // while !xosc0.is_ready() { + // info!("Waiting for XOSC0 to stabilize"); + // } + + // let (mut gclk0, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk2, xosc0); + // let gclk2 = gclk2.div(gclk::GclkDiv8::Div(1)).enable(); + + // /* SD config */ + // let (mut gclk1, dfll) = + // hal::clock::v2::gclk::Gclk::from_source(tokens.gclks.gclk1, clocks.dfll); + // let gclk1 = gclk1.div(gclk::GclkDiv16::Div(3)).enable(); // 48 / 3 = 16 MHzs + // let (pclk_sd, gclk1) = Pclk::enable(tokens.pclks.sercom0, gclk1); + + // let pads_spi = spi::Pads::::default() + // .sclk(pins.pa05) + // .data_in(pins.pa07) + // .data_out(pins.pa04); + // let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + // .length::() + // .bit_order(spi::BitOrder::MsbFirst) + // .spi_mode(spi::MODE_0) + // .enable(); + // let sd_manager = SdManager::new(sdmmc_spi, pins.pa06.into_push_pull_output()); + // let (pclk_radio, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + // /* Radio config */ + // let rx: Pin> = pins.pa08.into_alternate(); + // let tx: Pin> = pins.pa09.into_alternate(); + // let pads = uart::Pads::::default() + // .rx(rx) + // .tx(tx); + // let mut uart = + // GroundStationUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_radio.freq()) + // .baud( + // RateExtU32::Hz(57600), + // uart::BaudMode::Fractional(uart::Oversampling::Bits16), + // ) + // .enable(); + + // loop { + // nb::block!(uart.write(0x55)).unwrap(); + // } + + let (pclk_gps, gclk0) = Pclk::enable(tokens.pclks.sercom2, gclk0); + //info!("here"); + // let mut rx = pins.pa13.into_push_pull_output(); + // loop { + // rx.set_high().ok(); + // cortex_m::asm::delay(300_000); + // rx.set_low().ok(); + // cortex_m::asm::delay(300_000); + // } + + let pads = hal::sercom::uart::Pads::::default() + .rx(pins.pa13) + .tx(pins.pa12); + + let mut gps_uart_config = + GpsUartConfig::new(&mclk, peripherals.SERCOM2, pads, pclk_gps.freq()).baud( + RateExtU32::Hz(9600), + uart::BaudMode::Fractional(uart::Oversampling::Bits16), + ); + + gps_uart_config = gps_uart_config.immediate_overflow_notification(false); + + // loop { + // let (x,y) = gps_uart_config.get_baud(); + // info!("Baud: {}", x); + // } + + let mut gps_uart = gps_uart_config.enable(); + let (mut gps_rx, mut gps_tx) = gps_uart.split(); + // gps_uart.enable_interrupts(hal::sercom::uart::Flags::RXC); + + /* Status LED */ + // info!("Setting up LED"); + // let led = pins.pa02.into_push_pull_output(); + let mut gps_enable = pins.pb09.into_push_pull_output(); + let mut gps_reset = pins.pb07.into_push_pull_output(); + gps_reset.set_low().ok(); + cortex_m::asm::delay(300_000); + gps_reset.set_high().ok(); + + // gps_reset.set_low().ok(); + // gps_enable.set_high().ok(); + gps_enable.set_low().ok(); + 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, + } + .into_packet_bytes(); + + for byte in packet { + // info!("Byte: {}", byte); + nb::block!(gps_tx.write(byte)).unwrap(); + } + + // let packet_two = CfgMsgAllPortsBuilder::set_rate_for::([1, 0, 0, 0, 0, 0]).into_packet_bytes(); + // for byte in packet_two { + // nb::block!(gps_uart.write(byte)).unwrap(); + // } + let mut dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); + /* DMAC config */ + dmaCh0 + .as_mut() + .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); + + + // loop { + // if gps_dma_transfer.complete() { + // info!("DMA transfer complete"); + // let (chan0, source, buf) = gps_dma_transfer.stop(); + // gps_dma_transfer = + // dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + // .unwrap() + // .begin( + // atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + // dmac::TriggerAction::BURST, + // ); + // let buf_clone = buf.clone(); + + // unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + // gps_dma_transfer.block_transfer_interrupt(); + // let request = + // UbxPacketRequest::request_for::().into_packet_bytes(); + // for byte in request { + // nb::block!(gps_tx.write(byte)).unwrap(); + // } + // cortex_m::asm::delay(300_000); + // let mut buf: [u8; 256] = [0; 256]; + // let mut bytes: [u8; 256] = [0; 256]; + // // for i in 0..buf.len() { + // // let item = nb::block!(gps_uart.read()).unwrap(); + // // // info!("Byte: {}", item); + // // bytes[i] = item; + // // } + // let buf = ublox::FixedLinearBuffer::new(&mut buf[..]); + // let mut parser = ublox::Parser::new(buf); + // let mut msgs = parser.consume(&buf_clone); + // while let Some(msg) = msgs.next() { + // match msg { + // Ok(msg) => match msg { + // ublox::PacketRef::NavPosLlh(x) => { + // let message_data = messages::sensor::NavPosLlh { + // height_msl: x.height_msl(), + // longitude: x.lon_degrees(), + // latitude: x.lat_degrees(), + // }; + // 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"); + // } + // } + // } + // } + // } + /* Spawn tasks */ + // sensor_send::spawn().ok(); + // state_send::spawn().ok(); + let rtc = hal::rtc::Rtc::clock_mode( + peripherals.RTC, + atsamd_hal::fugit::RateExtU32::Hz(1024), + &mut mclk, + ); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done + rtc.set_count32(0); + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + let mut led_green = pins.pa03.into_push_pull_output(); + let mut led_red = pins.pa02.into_push_pull_output(); + led_green.set_low(); + led_red.set_low(); + blink::spawn().ok(); + let message = Message::new( + 0, // technically true time is not known yet. + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { + online: true, + }), + }, + ); + send_command::spawn(message).ok(); + let mut gps_dma_transfer: GPSTransfer = + Transfer::new(dmaCh0, gps_rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA err") + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + ); + let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); + + ( + Shared { + em: ErrorManager::new(), + data_manager: DataManager::new(), + can0, + can_command_manager, + }, + Local { + led_green, + led_red, + gps_dma_transfer: Some(gps_dma_transfer), + gps_tx, + // sd_manager, + }, + init::Monotonics(mono), + ) + } + + /// Idle task for when no other tasks are running. + #[idle] + fn idle(_: idle::Context) -> ! { + loop {} + } + + #[task(priority = 3, binds = DMAC_0, local = [gps_dma_transfer, gps_tx], shared = [&em, data_manager])] + fn gps_dma(mut cx: gps_dma::Context) { + let mut gps_dma_transfer = cx.local.gps_dma_transfer.take(); + match gps_dma_transfer { + Some(mut gps_dma_transfer) => {info!("DMA transfer availabe"); + let mut gps_tx = cx.local.gps_tx; + if gps_dma_transfer.complete() { + info!("DMA transfer complete"); + gps_dma_transfer.block_transfer_interrupt(); + let (chan0, source, buf) = gps_dma_transfer.stop(); + *cx.local.gps_dma_transfer = Some( + dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) + .unwrap() + .begin( + atsamd_hal::sercom::Sercom2::DMA_RX_TRIGGER, + dmac::TriggerAction::BURST, + )); + let buf_clone = buf.clone(); + + unsafe { BUF_DST.copy_from_slice(&[0; 256]) }; + let request = + UbxPacketRequest::request_for::().into_packet_bytes(); + for byte in request { + nb::block!(gps_tx.write(byte)).unwrap(); + } + cortex_m::asm::delay(10_000); + let mut buf: [u8; 256] = [0; 256]; + let mut 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_clone); + while let Some(msg) = msgs.next() { + match msg { + Ok(msg) => match msg { + ublox::PacketRef::NavPosLlh(x) => { + let message_data = messages::sensor::NavPosLlh { + height_msl: x.height_msl(), + longitude: x.lon_degrees(), + latitude: x.lat_degrees(), + }; + info!( + "GPS latitude: {:?}, longitude {:?}", + x.lat_degrees(), + x.lon_degrees() + ); + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + messages::sensor::Sensor::new(message_data), + ); + spawn!(send_internal, message).ok(); + // 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"); + } + } + } + } + } + None => {} + } + } + + /// Handles the CAN1 interrupt. + #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + info!("CAN1 interrupt"); + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + /// Handles the CAN0 interrupt. + #[task(priority = 3, binds = CAN0, shared = [can0, data_manager])] + fn can0(mut cx: can0::Context) { + // info!("CAN0 interrupt"); + cx.shared.can0.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + /** + * Sends a message over CAN. + */ + #[task(capacity = 10, shared = [can0, &em])] + fn send_internal(mut cx: send_internal::Context, m: Message) { + info!("{}", m.clone()); + cx.shared.em.run(|| { + cx.shared.can0.lock(|can| can.send_message(m))?; + Ok(()) + }); + } + + /** + * Sends a message over CAN. + */ + #[task(priority = 3, capacity = 5, shared = [can_command_manager, &em])] + fn send_command(mut cx: send_command::Context, m: Message) { + info!("{}", m.clone()); + cx.shared.em.run(|| { + cx.shared + .can_command_manager + .lock(|can| can.send_message(m))?; + Ok(()) + }); + } + + /// Receives a log message from the custom logger so that it can be sent over the radio. + pub fn queue_gs_message(d: impl Into) { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + d.into(), + ); + + send_internal::spawn(message).ok(); + } + + // #[task(capacity = 10, local = [sd_manager], shared = [&em])] + // fn sd_dump(cx: sd_dump::Context, m: Message) { + // let manager = cx.local.sd_manager; + // cx.shared.em.run(|| { + // let mut buf: [u8; 255] = [0; 255]; + // let msg_ser = postcard::to_slice_cobs(&m, &mut buf)?; + // if let Some(mut file) = manager.file.take() { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } else if let Ok(mut file) = manager.open_file("log.txt") { + // manager.write(&mut file, &msg_ser)?; + // manager.file = Some(file); + // } + // Ok(()) + // }); + // } + + // /** + // * Sends information about the sensors. + // */ + // #[task(shared = [data_manager, &em])] + // fn sensor_send(mut cx: sensor_send::Context) { + // let (sensors, logging_rate) = cx + // .shared + // .data_manager + // .lock(|data_manager| (data_manager.take_sensors(), data_manager.get_logging_rate())); + + // cx.shared.em.run(|| { + // for msg in sensors { + // match msg { + // Some(x) => { + // spawn!(send_gs, x.clone())?; + // // spawn!(sd_dump, x)?; + // } + // None => { + // continue; + // } + // } + // } + // Ok(()) + // }); + // match logging_rate { + // RadioRate::Fast => { + // spawn_after!(sensor_send, ExtU64::millis(100)).ok(); + // } + // RadioRate::Slow => { + // spawn_after!(sensor_send, ExtU64::millis(250)).ok(); + // } + // } + // } + + // /// Handles the radio interrupt. + // /// This only needs to be called when the radio has data to read, this is why an interrupt handler is used above polling which would waste resources. + // /// We use a critical section to ensure that we are not interrupted while servicing the mavlink message. + // #[task(priority = 3, binds = SERCOM2_2, shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // cortex_m::interrupt::free(|cs| { + // let m = radio_manager.receive_message()?; + // info!("Received message {}", m.clone()); + // spawn!(send_command, m) + + // })?; + // Ok(()) + // }); + // }); + // } + + /** + * Simple blink task to test the system. + * Acts as a heartbeat for the system. + */ + #[task(local = [led_green, led_red], shared = [&em])] + fn blink(cx: blink::Context) { + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle()?; + spawn_after!(blink, ExtU64::millis(200))?; + } else { + cx.local.led_green.toggle()?; + spawn_after!(blink, ExtU64::secs(1))?; + } + Ok(()) + }); + } +} diff --git a/boards/gps/src/types.rs b/boards/gps/src/types.rs new file mode 100644 index 00000000..b7753ba1 --- /dev/null +++ b/boards/gps/src/types.rs @@ -0,0 +1,31 @@ +use atsamd_hal::gpio::*; +use atsamd_hal::sercom::uart::EightBit; +use atsamd_hal::sercom::{uart, uart::Duplex, IoSet1, Sercom2, IoSet3, uart::TxDuplex}; +use messages::sender::Sender; +use messages::sender::Sender::GpsBoard; +use atsamd_hal::dmac; +use atsamd_hal::dmac::BufferPair; + +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = GpsBoard; + +// ------- +// GPS UART +// ------- +pub type GpsPads = uart::PadsFromIds; +pub type GpsUartConfig = uart::Config; +pub type GpsUart = uart::Uart; +pub type GpsUartTx = uart::Uart; +pub type GPSTransfer = dmac::Transfer< + dmac::Channel, + BufferPair, GPSBUFFER>, +>; +pub type GPSBUFFER = &'static mut [u8; 256]; + +// ------- +// Ground Station +// ------- +pub type GroundStationPads = uart::PadsFromIds; +pub type GroundStationUartConfig = uart::Config; diff --git a/boards/link/Cargo.toml b/boards/link/Cargo.toml new file mode 100644 index 00000000..7e6c48ff --- /dev/null +++ b/boards/link/Cargo.toml @@ -0,0 +1,27 @@ +[package] +name = "link" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +#cortex-m = { workspace = true } +cortex-m = "0.7.4" +cortex-m-rt = "0.7.1" +rtic = {version = "2.0.0", features = ["thumbv7-backend"]} +rtic-monotonics = {version = "2.0.2", features = ["cortex-m-systick", "stm32h733vg"]} +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +common-arm = { path = "../../libraries/common-arm" } +stm32h7xx-hal = { workspace = true } +postcard = "1.0.2" +messages = { path = "../../libraries/messages" } +systick-monotonic = "1.0.1" +defmt = "0.3.2" +fdcan = "0.2" +embedded-alloc = "0.5.0" +heapless = "0.7.16" +rtic-sync = "1.3.0" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/link/src/communication.rs b/boards/link/src/communication.rs new file mode 100644 index 00000000..cbf1c6d4 --- /dev/null +++ b/boards/link/src/communication.rs @@ -0,0 +1,206 @@ +use crate::data_manager::DataManager; +use crate::types::COM_ID; +use common_arm::HydraError; +use fdcan::{ + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, +}; +use messages::Message; +use postcard::from_bytes; +use stm32h7xx_hal::prelude::*; +use mavlink::peek_reader::PeekReader; +use messages::mavlink::uorocketry::MavMessage; +use messages::mavlink::{self}; +use defmt::{info, error}; + +/// Clock configuration is out of scope for this builder +/// easiest way to avoid alloc is to use no generics +pub struct CanCommandManager { + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, +} + +impl CanCommandManager { + pub fn new( + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, + ) -> Self { + Self { can } + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + let payload = postcard::to_slice(&m, &mut buf)?; + let header = TxFrameHeader { + len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size + id: StandardId::new(COM_ID.into()).unwrap().into(), + frame_format: FrameFormat::Standard, + bit_rate_switching: false, + marker: None, + }; + self.can.transmit(header, &payload)?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + Ok(()) + } +} + + +/// Clock configuration is out of scope for this builder +/// easiest way to avoid alloc is to use no generics +pub struct CanDataManager { + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, +} + +impl CanDataManager { + pub fn new( + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, + ) -> Self { + Self { can } + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + let payload = postcard::to_slice(&m, &mut buf)?; + let header = TxFrameHeader { + len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size + id: StandardId::new(COM_ID.into()).unwrap().into(), + frame_format: FrameFormat::Fdcan, + bit_rate_switching: false, + marker: None, + }; + // self.can.abort(fdcan::Mailbox::_2); // this is needed if boards are not in sync (if they are not in sync that is a bigger problem) + + stm32h7xx_hal::nb::block!(self.can.transmit(header, &payload))?; + + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + info!("Received message {}", data.clone()); + crate::app::send_gs::spawn(data).ok(); + // data_manager.handle_data(data); + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + self.can.clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + Ok(()) + } +} + + +pub struct RadioDevice { + transmitter: stm32h7xx_hal::serial::Tx, + pub receiver: PeekReader>, +} + +impl RadioDevice { + pub fn new( + uart: stm32h7xx_hal::serial::Serial, + ) -> Self { + + let (tx, mut rx) = uart.split(); + + rx.listen(); + // setup interrupts + + RadioDevice { + transmitter: tx, + receiver: PeekReader::new(rx), + } + } +} + +pub struct RadioManager { + pub radio: RadioDevice, + mav_sequence: u8, +} + +impl RadioManager { + pub fn new(radio: RadioDevice) -> Self { + RadioManager { + radio, + mav_sequence: 0, + } + } + pub fn send_message(&mut self, payload: &[u8]) -> Result<(), HydraError> { + let mav_header = mavlink::MavHeader { + system_id: 1, + component_id: 1, + sequence: self.increment_mav_sequence(), + }; + // Create a fixed-size array and copy the payload into it + let mut fixed_payload = [0u8; 255]; + let len = payload.len().min(255); + fixed_payload[..len].copy_from_slice(&payload[..len]); + + let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( + mavlink::uorocketry::POSTCARD_MESSAGE_DATA { + message: fixed_payload, + }, + ); + mavlink::write_versioned_msg( + &mut self.radio.transmitter, + mavlink::MavlinkVersion::V2, + mav_header, + &mav_message, + )?; + Ok(()) + } + pub fn increment_mav_sequence(&mut self) -> u8 { + self.mav_sequence = self.mav_sequence.wrapping_add(1); + self.mav_sequence + } + pub fn receive_message(&mut self) -> Result { + let (_header, msg): (_, MavMessage) = + mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; + + // info!("{:?}", ); + // Do we need the header? + match msg { + mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { + return Ok(postcard::from_bytes::(&msg.message)?); + // weird Ok syntax to coerce to hydra error type. + } + mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { + info!("{}", command.command); + return Ok(postcard::from_bytes::(&command.command)?); + } + mavlink::uorocketry::MavMessage::HEARTBEAT(heartbeat) => { + info!("Heartbeat"); + return Err(mavlink::error::MessageReadError::Io.into()); + } + _ => { + error!("Error, ErrorContext::UnkownPostcardMessage"); + return Err(mavlink::error::MessageReadError::Io.into()); + } + } + } +} diff --git a/boards/communication/src/data_manager.rs b/boards/link/src/data_manager.rs similarity index 69% rename from boards/communication/src/data_manager.rs rename to boards/link/src/data_manager.rs index 6d600691..2e2b02b0 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/link/src/data_manager.rs @@ -1,7 +1,9 @@ use messages::command::RadioRate; use messages::state::StateData; use messages::Message; - +use common_arm::HydraError; +use stm32h7xx_hal::rcc::ResetReason; +use defmt::info; #[derive(Clone)] pub struct DataManager { pub air: Option, @@ -18,7 +20,10 @@ pub struct DataManager { pub gps_pos_2: Option, pub gps_pos_acc: Option, pub state: Option, + pub reset_reason: Option, pub logging_rate: Option, + pub recovery_sensing: Option, + pub nav_pos_l1h: Option, } impl DataManager { @@ -38,7 +43,10 @@ impl DataManager { gps_pos_2: None, gps_pos_acc: None, state: None, + reset_reason: None, logging_rate: Some(RadioRate::Slow), // start slow. + recovery_sensing: None, + nav_pos_l1h: None, } } @@ -53,7 +61,7 @@ impl DataManager { } /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 13] { + pub fn take_sensors(&mut self) -> [Option; 15] { [ self.air.take(), self.ekf_nav_1.take(), @@ -68,12 +76,42 @@ impl DataManager { self.gps_pos_1.take(), self.gps_pos_2.take(), self.gps_pos_acc.take(), + self.nav_pos_l1h.take(), + self.recovery_sensing.take(), ] } pub fn clone_states(&self) -> [Option; 1] { [self.state.clone()] } + + pub fn clone_reset_reason(&self) -> Option { + self.reset_reason.clone() + } + + pub fn set_reset_reason(&mut self, reset: ResetReason) { + self.reset_reason = Some(reset); + } + + pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { + match data.data { + messages::Data::Command(command) => match command.data { + messages::command::CommandData::PowerDown(_) => { + crate::app::sleep_system::spawn().ok(); + } + messages::command::CommandData::RadioRateChange(command_data) => { + self.logging_rate = Some(command_data.rate); + } + _ => { + // We don't care atm about these other commands. + } + }, + _ => { + // we can disregard all other messages for now. + } + } + Ok(()) + } pub fn handle_data(&mut self, data: Message) { match data.data { messages::Data::Sensor(ref sensor) => match sensor.data { @@ -116,18 +154,27 @@ impl DataManager { messages::sensor::SensorData::GpsPos2(_) => { self.gps_pos_2 = Some(data); } + messages::sensor::SensorData::RecoverySensing(_) => { + self.recovery_sensing = Some(data); + } + messages::sensor::SensorData::NavPosLlh(_) => { + self.nav_pos_l1h = Some(data); + } + messages::sensor::SensorData::ResetReason(_) => { + + } }, messages::Data::State(state) => { self.state = Some(state.data); } - messages::Data::Command(command) => match command.data { - messages::command::CommandData::RadioRateChange(command_data) => { - self.logging_rate = Some(command_data.rate); - } - messages::command::CommandData::DeployDrogue(_) => {} - messages::command::CommandData::DeployMain(_) => {} - messages::command::CommandData::PowerDown(_) => {} - }, + // messages::Data::Command(command) => match command.data { + // messages::command::CommandData::RadioRateChange(command_data) => { + // self.logging_rate = Some(command_data.rate); + // } + // messages::command::CommandData::DeployDrogue(_) => {} + // messages::command::CommandData::DeployMain(_) => {} + // messages::command::CommandData::PowerDown(_) => {} + // }, _ => {} } } diff --git a/boards/link/src/main.rs b/boards/link/src/main.rs new file mode 100644 index 00000000..2e492ce6 --- /dev/null +++ b/boards/link/src/main.rs @@ -0,0 +1,587 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod types; + +use stm32h7xx_hal::nb; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use common_arm::*; +use communication::{CanCommandManager, CanDataManager}; +use communication::{RadioDevice, RadioManager}; +use core::cell::RefCell; +use core::num::{NonZeroU16, NonZeroU8}; +use cortex_m::interrupt::Mutex; +use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; +use fdcan::{ + config::NominalBitTiming, + filter::{StandardFilter, StandardFilterSlot}, +}; +use messages::command::RadioRate; +use messages::{sensor, Data}; +use panic_probe as _; +use rtic_monotonics::systick::prelude::*; +use rtic_sync::{channel::*, make_channel}; +use stm32h7xx_hal::gpio::gpioa::{PA2, PA3, PA4}; +use stm32h7xx_hal::gpio::gpiob::PB4; +use stm32h7xx_hal::gpio::Speed; +use stm32h7xx_hal::gpio::{Output, PushPull}; +use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::rcc::ResetReason; +use stm32h7xx_hal::rtc; +use stm32h7xx_hal::spi; +use stm32h7xx_hal::{rcc, rcc::rec}; +use types::COM_ID; // global logger + +const DATA_CHANNEL_CAPACITY: usize = 10; + +systick_monotonic!(Mono, 500); + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + // stm32h7xx_hal::pac::SCB::sys_reset() + cortex_m::asm::udf() +} + +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] +mod app { + + use messages::Message; + use stm32h7xx_hal::gpio::{Alternate, Pin}; + + use super::*; + + #[shared] + struct SharedResources { + data_manager: DataManager, + em: ErrorManager, + // sd_manager: SdManager< + // stm32h7xx_hal::spi::Spi, + // PA4>, + // >, + radio_manager: RadioManager, + can_command_manager: CanCommandManager, + can_data_manager: CanDataManager, + sbg_power: PB4>, + } + #[local] + struct LocalResources { + led_red: PA2>, + led_green: PA3>, + buzzer: stm32h7xx_hal::pwm::Pwm + } + + #[init] + fn init(ctx: init::Context) -> (SharedResources, LocalResources) { + // channel setup + let (s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + + let core = ctx.core; + + /* Logging Setup */ + HydraLogging::set_ground_station_callback(queue_gs_message); + + let pwr = ctx.device.PWR.constrain(); + // We could use smps, but the board is not designed for it + // let pwrcfg = example_power!(pwr).freeze(); + let mut pwrcfg = pwr.freeze(); + + info!("Power enabled"); + let backup = pwrcfg.backup().unwrap(); + info!("Backup domain enabled"); + // RCC + let mut rcc = ctx.device.RCC.constrain(); + let reset = rcc.get_reset_reason(); + let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + + let ccdr = rcc + .use_hse(48.MHz()) // check the clock hardware + .sys_ck(200.MHz()) + .pll1_strategy(rcc::PllConfigStrategy::Iterative) + .pll1_q_ck(32.MHz()) + .freeze(pwrcfg, &ctx.device.SYSCFG); + info!("RCC configured"); + let fdcan_prec = ccdr + .peripheral + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + + let btr = NominalBitTiming { + prescaler: NonZeroU16::new(10).unwrap(), + seg1: NonZeroU8::new(13).unwrap(), + seg2: NonZeroU8::new(2).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), + }; + + // let data_bit_timing = DataBitTiming { + // prescaler: NonZeroU8::new(10).unwrap(), + // seg1: NonZeroU8::new(13).unwrap(), + // seg2: NonZeroU8::new(2).unwrap(), + // sync_jump_width: NonZeroU8::new(4).unwrap(), + // transceiver_delay_compensation: true, + // }; + + info!("CAN enabled"); + // GPIO + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); + let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); + let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); + + let pins = gpiob.pb14.into_alternate(); + let mut c0 = ctx + .device + .TIM12 + .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); + + c0.set_duty(c0.get_max_duty() / 4); + // PWM outputs are disabled by default + // c0.enable(); + + info!("PWM enabled"); + // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); + info!("PLL1Q:"); + // https://github.com/stm32-rs/stm32h7xx-hal/issues/369 This needs to be stolen. Grrr I hate the imaturity of the stm32-hal + let can2: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::ConfigMode, + > = { + let rx = gpiob.pb12.into_alternate().speed(Speed::VeryHigh); + let tx = gpiob.pb13.into_alternate().speed(Speed::VeryHigh); + ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) + }; + + let mut can_data = can2; + can_data.set_protocol_exception_handling(false); + + can_data.set_nominal_bit_timing(btr); + + // can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + + // can_command.set_data_bit_timing(data_bit_timing); + + can_data.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + can_data.set_standard_filter( + StandardFilterSlot::_1, + StandardFilter::accept_all_into_fifo0(), + ); + + + can_data.set_standard_filter( + StandardFilterSlot::_2, + StandardFilter::accept_all_into_fifo0(), + ); + + + can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + let config = can_data + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); + can_data.apply_config(config); + + + let can_data_manager = CanDataManager::new(can_data.into_normal()); + + /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. + let can1: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::ConfigMode, + > = { + let rx = gpioa.pa11.into_alternate().speed(Speed::VeryHigh); + let tx = gpioa.pa12.into_alternate().speed(Speed::VeryHigh); + ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) + }; + + let mut can_command = can1; + can_command.set_protocol_exception_handling(false); + + can_command.set_nominal_bit_timing(btr); + can_command.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + can_command.set_standard_filter( + StandardFilterSlot::_1, + StandardFilter::accept_all_into_fifo0(), + ); + + + can_command.set_standard_filter( + StandardFilterSlot::_2, + StandardFilter::accept_all_into_fifo0(), + ); + + // can_data.set_data_bit_timing(data_bit_timing); + can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + let config = can_command + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. + can_command.apply_config(config); + + let can_command_manager = CanCommandManager::new(can_command.into_normal()); + + // let spi_sd: stm32h7xx_hal::spi::Spi< + // stm32h7xx_hal::stm32::SPI1, + // stm32h7xx_hal::spi::Enabled, + // u8, + // > = ctx.device.SPI1.spi( + // ( + // gpioa.pa5.into_alternate::<5>(), + // gpioa.pa6.into_alternate(), + // gpioa.pa7.into_alternate(), + // ), + // spi::Config::new(spi::MODE_0), + // 16.MHz(), + // ccdr.peripheral.SPI1, + // &ccdr.clocks, + // ); + + // let cs_sd = gpioa.pa4.into_push_pull_output(); + + // let sd_manager = SdManager::new(spi_sd, cs_sd); + + // leds + let led_red = gpioa.pa2.into_push_pull_output(); + let led_green = gpioa.pa3.into_push_pull_output(); + + // sbg power pin + let mut sbg_power = gpiob.pb4.into_push_pull_output(); + sbg_power.set_high(); + + // UART for sbg + let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); + let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); + + // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + let uart_radio = ctx + .device + .UART4 + .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + + let radio = RadioDevice::new(uart_radio); + + let radio_manager = RadioManager::new(radio); + + let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( + ctx.device.RTC, + backup.RTC, + stm32h7xx_hal::rtc::RtcClock::Lsi, + &ccdr.clocks, + ); + + // TODO: Get current time from some source + let now = NaiveDate::from_ymd_opt(2001, 1, 1) + .unwrap() + .and_hms_opt(0, 0, 0) + .unwrap(); + + rtc.set_date_time(now); + + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + + /* Monotonic clock */ + Mono::start(core.SYST, 200_000_000); + + let mut data_manager = DataManager::new(); + data_manager.set_reset_reason(reset); + let em = ErrorManager::new(); + blink::spawn().ok(); + send_data_internal::spawn(r).ok(); + reset_reason_send::spawn().ok(); + state_send::spawn().ok(); + // generate_random_messages::spawn().ok(); + // sensor_send::spawn().ok(); + info!("Online"); + + ( + SharedResources { + data_manager, + em, + // sd_manager, + radio_manager, + can_command_manager, + can_data_manager, + sbg_power, + }, + LocalResources { led_red, led_green, buzzer: c0 }, + ) + } + + #[task(priority = 3, shared = [&em])] + async fn generate_random_messages(cx: generate_random_messages::Context) { + loop { + cx.shared.em.run(|| { + let message = Message::new( + 0, + COM_ID, + messages::state::State::new(messages::state::StateData::Initializing), + ); + spawn!(send_gs, message.clone())?; + // spawn!(send_data_internal, message)?; + Ok(()) + }); + Mono::delay(1.secs()).await; + } + } + + #[task(priority = 3, shared = [data_manager, &em])] + async fn reset_reason_send(mut cx: reset_reason_send::Context) { + let reason = cx + .shared + .data_manager + .lock(|data_manager| data_manager.clone_reset_reason()); + match reason { + Some(reason) => { + let x = match reason { + stm32h7xx_hal::rcc::ResetReason::BrownoutReset => sensor::ResetReason::BrownoutReset, + stm32h7xx_hal::rcc::ResetReason::CpuReset => sensor::ResetReason::CpuReset, + stm32h7xx_hal::rcc::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously => sensor::ResetReason::D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously, + stm32h7xx_hal::rcc::ResetReason::D1ExitsDStandbyMode => sensor::ResetReason::D1ExitsDStandbyMode, + stm32h7xx_hal::rcc::ResetReason::D2ExitsDStandbyMode => sensor::ResetReason::D2ExitsDStandbyMode, + stm32h7xx_hal::rcc::ResetReason::GenericWatchdogReset => sensor::ResetReason::GenericWatchdogReset, + stm32h7xx_hal::rcc::ResetReason::IndependentWatchdogReset => sensor::ResetReason::IndependentWatchdogReset, + stm32h7xx_hal::rcc::ResetReason::PinReset => sensor::ResetReason::PinReset, + stm32h7xx_hal::rcc::ResetReason::PowerOnReset => sensor::ResetReason::PowerOnReset, + stm32h7xx_hal::rcc::ResetReason::SystemReset => sensor::ResetReason::SystemReset, + stm32h7xx_hal::rcc::ResetReason::Unknown { rcc_rsr } => sensor::ResetReason::Unknown { rcc_rsr }, + stm32h7xx_hal::rcc::ResetReason::WindowWatchdogReset => sensor::ResetReason::WindowWatchdogReset, + }; + let message = messages::Message::new( + Mono::now().ticks(), + COM_ID, + sensor::Sensor::new(x), + ); + + cx.shared.em.run(|| { + spawn!(send_gs, message)?; + Ok(()) + }) + } + None => return, + } + } + + #[task(shared = [data_manager, &em])] + async fn state_send(mut cx: state_send::Context) { + let state_data = cx + .shared + .data_manager + .lock(|data_manager| data_manager.state.clone()); + cx.shared.em.run(|| { + if let Some(x) = state_data { + let message = Message::new( + Mono::now().ticks(), + COM_ID, + messages::state::State::new(x), + ); + spawn!(send_gs, message)?; + } // if there is none we still return since we simply don't have data yet. + Ok(()) + }); + Mono::delay(5.secs()).await; + // spawn_after!(state_send, ExtU64::secs(5)).ok(); + } + + /** + * Sends information about the sensors. + */ + #[task(priority = 3, shared = [data_manager, &em])] + async fn sensor_send(mut cx: sensor_send::Context) { + loop { + let (sensors, logging_rate) = cx.shared.data_manager.lock(|data_manager| { + (data_manager.take_sensors(), data_manager.get_logging_rate()) + }); + + cx.shared.em.run(|| { + for msg in sensors { + match msg { + Some(x) => { + // info!("Sending sensor data {}", x.clone()); + spawn!(send_gs, x)?; + // spawn!(sd_dump, x)?; + } + None => { + info!("No sensor data to send"); + continue; + } + } + } + + Ok(()) + }); + match logging_rate { + RadioRate::Fast => { + Mono::delay(100.millis()).await; + } + RadioRate::Slow => { + Mono::delay(250.millis()).await; + } + } + } + } + + /// Receives a log message from the custom logger so that it can be sent over the radio. + pub fn queue_gs_message(d: impl Into) { + info!("Queueing message"); + let message = messages::Message::new( + Mono::now().ticks(), + COM_ID, + d.into(), + ); + info!("{:?}", message); + // send_in::spawn(message).ok(); + } + + #[task(priority = 2, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager, &em])] + fn can_command(mut cx: can_command::Context) { + // info!("CAN Command"); + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| cx.shared.em.run(|| can.process_data(data_manager))); + }) + } + + #[task(priority = 3, shared = [sbg_power])] + async fn sbg_power_on(mut cx: sbg_power_on::Context) { + loop { + cx.shared.sbg_power.lock(|sbg| { + sbg.set_high(); + }); + Mono::delay(10000.millis()).await; + } + } + + /** + * Sends a message to the radio over UART. + */ + #[task(priority = 3, shared = [&em, radio_manager])] + async fn send_gs(mut cx: send_gs::Context, m: Message) { + // info!("{}", m.clone()); + + cx.shared.radio_manager.lock(|radio_manager| { + cx.shared.em.run(|| { + // info!("Sending message {}", m); + let mut buf = [0; 255]; + let data = postcard::to_slice(&m, &mut buf)?; + radio_manager.send_message(data)?; + Ok(()) + }) + }); + } + + // #[task(priority = 3, binds = UART4, local = [just_fired: bool = false], shared = [&em, radio_manager])] + // fn radio_rx(mut cx: radio_rx::Context) { + // if *cx.local.just_fired { + // *cx.local.just_fired = false; + // return; + // } + // info!("Radio Rx"); + + // cx.shared.radio_manager.lock(|radio_manager| { + // cx.shared.em.run(|| { + // // cortex_m::interrupt::free(|cs| { + // let m = radio_manager.receive_message()?; + // *cx.local.just_fired = true; + // info!("Received message {}", m.clone()); + // spawn!(send_command_internal, m)?; + // // })?; + // Ok(()) + // }); + // }); + // } + + #[task( priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] + fn can_data(mut cx: can_data::Context) { + cx.shared.can_data_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + #[task(priority = 2, shared = [can_data_manager, data_manager])] + async fn send_data_internal( + mut cx: send_data_internal::Context, + mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + loop { + if let Ok(m) = receiver.recv().await { + cx.shared.can_data_manager.lock(|can| { + can.send_message(m); + }); + } + } + } + + #[task(priority = 2, shared = [can_command_manager, data_manager])] + async fn send_command_internal(mut cx: send_command_internal::Context, m: Message) { + // while let Ok(m) = receiver.recv().await { + cx.shared.can_command_manager.lock(|can| { + can.send_message(m); + }); + // } + } + + #[task(priority = 1, local = [led_red, led_green, buzzer, buzzed: bool = false], shared = [&em])] + async fn blink(cx: blink::Context) { + loop { + if cx.shared.em.has_error() { + cx.local.led_red.toggle(); + if *cx.local.buzzed { + cx.local.buzzer.set_duty(0); + *cx.local.buzzed = false; + } else { + let duty = cx.local.buzzer.get_max_duty() / 4; + cx.local.buzzer.set_duty(duty); + *cx.local.buzzed = true; + } + Mono::delay(500.millis()).await; + + } else { + cx.local.led_green.toggle(); + if *cx.local.buzzed { + cx.local.buzzer.set_duty(0); + *cx.local.buzzed = false; + } else { + let duty = cx.local.buzzer.get_max_duty() / 4; + cx.local.buzzer.set_duty(duty); + *cx.local.buzzed = true; + } + Mono::delay(2000.millis()).await; + + } + } + } + + #[task(priority = 3, shared = [&em, sbg_power])] + async fn sleep_system(mut cx: sleep_system::Context) { + // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. + cx.shared.sbg_power.lock(|sbg| { + sbg.set_low(); + }); + } +} diff --git a/boards/link/src/types.rs b/boards/link/src/types.rs new file mode 100644 index 00000000..e362bf9c --- /dev/null +++ b/boards/link/src/types.rs @@ -0,0 +1,3 @@ +use messages::sender::{Sender, Sender::CommunicationBoard}; + +pub static COM_ID: Sender = CommunicationBoard; \ No newline at end of file diff --git a/boards/nav/Cargo.toml b/boards/nav/Cargo.toml new file mode 100644 index 00000000..6cf4812a --- /dev/null +++ b/boards/nav/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "nav" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +#cortex-m = { workspace = true } +cortex-m = "0.7.4" +cortex-m-rt = "0.7.1" +rtic = {version = "2.0.0", features = ["thumbv7-backend"]} +rtic-monotonics = {version = "2.0.2", features = ["cortex-m-systick", "stm32h733vg"]} +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +common-arm = { path = "../../libraries/common-arm" } +stm32h7xx-hal = { workspace = true } +postcard = "1.0.2" +messages = { path = "../../libraries/messages" } +systick-monotonic = "1.0.1" +defmt = "0.3.2" +fdcan = "0.2" +sbg-rs = {path = "../../libraries/sbg-rs"} +embedded-alloc = "0.5.0" +heapless = "0.7.16" +rtic-sync = "1.3.0" +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +chrono = {version = "0.4.0", default-features = false} \ No newline at end of file diff --git a/boards/nav/src/communication.rs b/boards/nav/src/communication.rs new file mode 100644 index 00000000..67397e34 --- /dev/null +++ b/boards/nav/src/communication.rs @@ -0,0 +1,112 @@ +use crate::data_manager::DataManager; +use crate::types::COM_ID; +use common_arm::HydraError; +use defmt::info; +use fdcan::{ + frame::{FrameFormat, TxFrameHeader}, + id::StandardId, +}; +use messages::Message; +use postcard::from_bytes; +use stm32h7xx_hal::prelude::*; + +/// Clock configuration is out of scope for this builder +/// easiest way to avoid alloc is to use no generics +pub struct CanCommandManager { + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, +} + +impl CanCommandManager { + pub fn new( + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, + ) -> Self { + Self { can } + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + let payload = postcard::to_slice(&m, &mut buf)?; + let header = TxFrameHeader { + len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size + id: StandardId::new(COM_ID.into()).unwrap().into(), + frame_format: FrameFormat::Standard, + bit_rate_switching: false, + marker: None, + }; + self.can.transmit(header, &payload)?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + info!("Received message {}", data.clone()); + data_manager.handle_command(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + Ok(()) + } +} + + +/// Clock configuration is out of scope for this builder +/// easiest way to avoid alloc is to use no generics +pub struct CanDataManager { + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, +} + +impl CanDataManager { + pub fn new( + can: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::NormalOperationMode, + >, + ) -> Self { + Self { can } + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + let payload = postcard::to_slice(&m, &mut buf)?; + let header = TxFrameHeader { + len: payload.len() as u8, // switch to const as this never changes or swtich on message type of known size + id: StandardId::new(COM_ID.into()).unwrap().into(), + frame_format: FrameFormat::Fdcan, + bit_rate_switching: true, + marker: None, + }; + // self.can.abort(fdcan::Mailbox::_2); // this is needed if boards are not in sync (if they are not in sync that is a bigger problem) + + stm32h7xx_hal::nb::block!(self.can.transmit(header, &payload))?; + + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { + let mut buf = [0u8; 64]; + for message in self.can.receive0(&mut buf) { + match from_bytes::(&buf) { + Ok(data) => { + // info!("Received message {}", data.clone()); + data_manager.handle_data(data)?; + } + Err(e) => { + info!("Error: {:?}", e) + } + } + } + self.can.clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + Ok(()) + } +} diff --git a/boards/nav/src/data_manager.rs b/boards/nav/src/data_manager.rs new file mode 100644 index 00000000..28f3e08c --- /dev/null +++ b/boards/nav/src/data_manager.rs @@ -0,0 +1,108 @@ +use common_arm::HydraError; +use messages::sensor::{ + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, SensorData, UtcTime, +}; +use messages::Message; +use heapless::Vec; + +#[derive(Clone)] +pub struct DataManager { + pub air: Option, + pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>, + pub ekf_quat: Option, + pub imu: Option<(Imu1, Imu2)>, + pub utc_time: Option, + pub gps_vel: Option<(GpsVel, GpsVelAcc)>, + pub gps_pos: Option<(GpsPos1, GpsPos2, GpsPosAcc)>, +} + +impl DataManager { + pub fn new() -> Self { + Self { + air: None, + ekf_nav: None, + ekf_quat: None, + imu: None, + utc_time: None, + gps_vel: None, + gps_pos: None, + } + } + + pub fn take_sensors(&mut self) -> Vec { + let mut sensors = Vec::new(); + + if let Some(data) = self.air.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2, data3)) = self.ekf_nav.take().map(|x| (x.0.into(), x.1.into(), x.2.into())) { + sensors.push(data1); + sensors.push(data2); + sensors.push(data3); + } + + if let Some(data) = self.ekf_quat.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2)) = self.imu.take().map(|x| (x.0.into(), x.1.into())) { + sensors.push(data1); + sensors.push(data2); + } + + if let Some(data) = self.utc_time.take().map(|x| x.into()) { + sensors.push(data); + } + + if let Some((data1, data2)) = self.gps_vel.take().map(|x| (x.0.into(), x.1.into())) { + sensors.push(data1); + sensors.push(data2); + } + + if let Some((data1, data2, data3)) = self.gps_pos.take().map(|x| (x.0.into(), x.1.into(), x.2.into())) { + sensors.push(data1); + sensors.push(data2); + sensors.push(data3); + } + + sensors + } + + pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { + match data.data { + messages::Data::Sensor(sensor) => match sensor.data { + _ => { + } + }, + _ => { + // we can disregard all other messages for now. + } + } + Ok(()) + } + + pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { + match data.data { + messages::Data::Command(command) => match command.data { + messages::command::CommandData::PowerDown(_) => { + crate::app::sleep_system::spawn().ok(); + } + _ => { + // We don't care atm about these other commands. + } + }, + _ => { + // we can disregard all other messages for now. + } + } + Ok(()) + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/nav/src/main.rs b/boards/nav/src/main.rs new file mode 100644 index 00000000..aa1cdce0 --- /dev/null +++ b/boards/nav/src/main.rs @@ -0,0 +1,474 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod sbg_manager; +mod types; + +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use common_arm::*; +use communication::{CanCommandManager, CanDataManager}; +use core::cell::RefCell; +use core::num::{NonZeroU16, NonZeroU8}; +use cortex_m::interrupt::Mutex; +use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; +use fdcan::{ + config::{DataBitTiming, NominalBitTiming}, + filter::{StandardFilter, StandardFilterSlot}, +}; +use heapless::Vec; +use messages::sensor::Sensor; +use messages::Data; +use panic_probe as _; +use rtic_monotonics::systick::prelude::*; +use rtic_sync::{channel::*, make_channel}; +use sbg_manager::{sbg_dma, sbg_flush, sbg_handle_data, sbg_sd_task, sbg_write_data}; +use sbg_rs::sbg::CallbackData; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use stm32h7xx_hal::dma::dma::StreamsTuple; +use stm32h7xx_hal::gpio::gpioa::{PA2, PA3, PA4}; +use stm32h7xx_hal::gpio::gpiob::PB4; +use stm32h7xx_hal::gpio::Speed; +use stm32h7xx_hal::gpio::{Output, PushPull}; +use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::rtc; +use stm32h7xx_hal::spi; +use stm32h7xx_hal::{rcc, rcc::rec}; +use types::COM_ID; // global logger + +const DATA_CHANNEL_CAPACITY: usize = 10; + +systick_monotonic!(Mono, 500); // 2ms ticks + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + stm32h7xx_hal::pac::SCB::sys_reset() +} + +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] +mod app { + use chrono::Timelike; + use messages::Message; + use stm32h7xx_hal::gpio::{Alternate, Pin}; + + use super::*; + + #[shared] + struct SharedResources { + data_manager: DataManager, + em: ErrorManager, + sd_manager: SdManager< + stm32h7xx_hal::spi::Spi, + PA4>, + >, + sbg_manager: sbg_manager::SBGManager, + can_command_manager: CanCommandManager, + can_data_manager: CanDataManager, + sbg_power: PB4>, + } + #[local] + struct LocalResources { + led_red: PA2>, + led_green: PA3>, + } + + #[init] + fn init(ctx: init::Context) -> (SharedResources, LocalResources) { + // channel setup + let (mut s, mut r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + let (mut s_command, mut r_command) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + + let core = ctx.core; + + /* Logging Setup */ + HydraLogging::set_ground_station_callback(queue_gs_message); + + let pwr = ctx.device.PWR.constrain(); + // We could use smps, but the board is not designed for it + // let pwrcfg = example_power!(pwr).freeze(); + let mut pwrcfg = pwr.freeze(); + + info!("Power enabled"); + let backup = pwrcfg.backup().unwrap(); + info!("Backup domain enabled"); + // RCC + let mut rcc = ctx.device.RCC.constrain(); + let reset = rcc.get_reset_reason(); + info!("Reset reason: {:?}", reset); + // match reset { + // // rcc::ResetReason::PowerOnReset => { + // // stm32h7xx_hal::pac::SCB::sys_reset(); + // // info!("Power on reset"); + // // } + // _ => { + // info!("Reset reason: {:?}", reset); + // } + // } + let fdcan_prec_unsafe = unsafe { rcc.steal_peripheral_rec() } + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + + let ccdr = rcc + .use_hse(48.MHz()) // check the clock hardware + .sys_ck(200.MHz()) + .pll1_strategy(rcc::PllConfigStrategy::Iterative) + .pll1_q_ck(32.MHz()) + .freeze(pwrcfg, &ctx.device.SYSCFG); + info!("RCC configured"); + let fdcan_prec = ccdr + .peripheral + .FDCAN + .kernel_clk_mux(rec::FdcanClkSel::Pll1Q); + + let btr = NominalBitTiming { + prescaler: NonZeroU16::new(10).unwrap(), + seg1: NonZeroU8::new(13).unwrap(), + seg2: NonZeroU8::new(2).unwrap(), + sync_jump_width: NonZeroU8::new(1).unwrap(), + }; + + // let data_bit_timing = DataBitTiming { + // prescaler: NonZeroU8::new(10).unwrap(), + // seg1: NonZeroU8::new(13).unwrap(), + // seg2: NonZeroU8::new(2).unwrap(), + // sync_jump_width: NonZeroU8::new(4).unwrap(), + // transceiver_delay_compensation: true, + // }; + + info!("CAN enabled"); + // GPIO + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); + let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); + let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); + + let pins = gpiob.pb14.into_alternate(); + let mut c0 = ctx + .device + .TIM12 + .pwm(pins, 4.kHz(), ccdr.peripheral.TIM12, &ccdr.clocks); + + c0.set_duty(c0.get_max_duty() / 4); + // PWM outputs are disabled by default + c0.enable(); + + info!("PWM enabled"); + // assert_eq!(ccdr.clocks.pll1_q_ck().unwrap().raw(), 32_000_000); + info!("PLL1Q:"); + // https://github.com/stm32-rs/stm32h7xx-hal/issues/369 This needs to be stolen. Grrr I hate the imaturity of the stm32-hal + + let can2: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::ConfigMode, + > = { + let rx = gpiob.pb12.into_alternate().speed(Speed::VeryHigh); + let tx = gpiob.pb13.into_alternate().speed(Speed::VeryHigh); + ctx.device.FDCAN2.fdcan(tx, rx, fdcan_prec) + }; + + let mut can_data = can2; + can_data.set_protocol_exception_handling(false); + + can_data.set_nominal_bit_timing(btr); + + can_data.set_automatic_retransmit(false); // data can be dropped due to its volume. + + // can_command.set_data_bit_timing(data_bit_timing); + + can_data.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + + can_data.set_standard_filter( + StandardFilterSlot::_1, + StandardFilter::accept_all_into_fifo0(), + ); + + can_data.set_standard_filter( + StandardFilterSlot::_2, + StandardFilter::accept_all_into_fifo0(), + ); + + let config = can_data + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCan); + can_data.apply_config(config); + + can_data.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_data.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + let can_data_manager = CanDataManager::new(can_data.into_normal()); + + /// SAFETY: This is done as a result of a single memory mapped bit in hardware. Safe in this context. + let can1: fdcan::FdCan< + stm32h7xx_hal::can::Can, + fdcan::ConfigMode, + > = { + let rx = gpioa.pa11.into_alternate().speed(Speed::VeryHigh); + let tx = gpioa.pa12.into_alternate().speed(Speed::VeryHigh); + ctx.device.FDCAN1.fdcan(tx, rx, fdcan_prec_unsafe) + }; + + let mut can_command = can1; + can_command.set_protocol_exception_handling(false); + + can_command.set_nominal_bit_timing(btr); + + can_command.set_standard_filter( + StandardFilterSlot::_0, + StandardFilter::accept_all_into_fifo0(), + ); + // can_data.set_data_bit_timing(data_bit_timing); + + let config = can_command + .get_config() + .set_frame_transmit(fdcan::config::FrameTransmissionConfig::AllowFdCanAndBRS); // check this maybe don't bit switch allow. + can_command.apply_config(config); + can_command.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); + + can_command.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true); + + let can_command_manager = CanCommandManager::new(can_command.into_normal()); + + let spi_sd: stm32h7xx_hal::spi::Spi< + stm32h7xx_hal::stm32::SPI1, + stm32h7xx_hal::spi::Enabled, + u8, + > = ctx.device.SPI1.spi( + ( + gpioa.pa5.into_alternate::<5>(), + gpioa.pa6.into_alternate(), + gpioa.pa7.into_alternate(), + ), + spi::Config::new(spi::MODE_0), + 16.MHz(), + ccdr.peripheral.SPI1, + &ccdr.clocks, + ); + + let cs_sd = gpioa.pa4.into_push_pull_output(); + + let sd_manager = SdManager::new(spi_sd, cs_sd); + + // leds + let led_red = gpioa.pa2.into_push_pull_output(); + let led_green = gpioa.pa3.into_push_pull_output(); + + // sbg power pin + let mut sbg_power = gpiob.pb4.into_push_pull_output(); + sbg_power.set_high(); + + // UART for sbg + let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); + let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); + + let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + let uart_sbg = ctx + .device + .UART4 + .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + + let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( + ctx.device.RTC, + backup.RTC, + stm32h7xx_hal::rtc::RtcClock::Lsi, + &ccdr.clocks, + ); + + // TODO: Get current time from some source + let now = NaiveDate::from_ymd_opt(2001, 1, 1) + .unwrap() + .and_hms_opt(0, 0, 0) + .unwrap(); + + rtc.set_date_time(now); + + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + + /* Monotonic clock */ + // let mono = Systick::new(core.SYST, ccdr.clocks.sysclk().to_Hz()); + // blink::spawn().ok(); + // display_data::spawn().ok(); + Mono::start(core.SYST, 200_000_000); + + info!("Online"); + let data_manager = DataManager::new(); + let em = ErrorManager::new(); + // let mono_token = rtic_monotonics::create_systick_token!(); + // let mono = Systick::start(ctx.core.SYST, 36_000_000, mono_token); + blink::spawn().ok(); + send_data_internal::spawn(r).ok(); + display_data::spawn(s).ok(); + // sbg_power_on::spawn().ok(); + let message = Message::new( + 0, + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { + online: true, + }), + }, + ); + send_command_internal::spawn(r_command).ok(); + async { + s_command.send(message).await; + }; + + ( + SharedResources { + data_manager, + em, + sd_manager, + sbg_manager, + can_command_manager, + can_data_manager, + sbg_power, + }, + LocalResources { led_red, led_green }, + ) + } + + #[idle] + fn idle(ctx: idle::Context) -> ! { + loop { + // info!("Idle"); + // cortex_m::asm::wfi(); // could case issue with CAN Bus. Was an issue with ATSAME51. + } + } + + #[task(priority = 2, shared = [&em, data_manager])] + async fn display_data( + mut cx: display_data::Context, + mut sender: Sender<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + loop { + let data = cx + .shared + .data_manager + .lock(|manager| manager.take_sensors()); + // info!("{:?}", data.clone()); + for msg in data { + info!("{:?}", msg); + sender + .send(Message::new(Mono::now().ticks(), COM_ID, Sensor::new(msg))) + .await; + } + Mono::delay(200.millis()).await; // what if there was no delay and we used chanenls to control the rate of messages. + } + } + + /// Receives a log message from the custom logger so that it can be sent over the radio. + pub fn queue_gs_message(d: impl Into) { + info!("Queueing message"); + let message = messages::Message::new(Mono::now().ticks(), COM_ID, d.into()); + info!("{:?}", message); + // send_in::spawn(message).ok(); + } + + #[task(priority = 3, binds = FDCAN1_IT0, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + info!("CAN Command"); + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + #[task(priority = 3, shared = [sbg_power])] + async fn sbg_power_on(mut cx: sbg_power_on::Context) { + loop { + cx.shared.sbg_power.lock(|sbg| { + sbg.set_high(); + }); + Mono::delay(10000.millis()).await; + } + } + + // Might be the wrong interrupt + #[task(priority = 3, binds = FDCAN2_IT0, shared = [can_data_manager, data_manager])] + fn can_data(mut cx: can_data::Context) { + cx.shared.can_data_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + #[task(priority = 2, shared = [can_data_manager, data_manager])] + async fn send_data_internal( + mut cx: send_data_internal::Context, + mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + loop { + if let Ok(m) = receiver.recv().await { + cx.shared.can_data_manager.lock(|can| { + can.send_message(m); + }); + } + } + } + + #[task(priority = 2, shared = [can_command_manager, data_manager])] + async fn send_command_internal( + mut cx: send_command_internal::Context, + mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, + ) { + while let Ok(m) = receiver.recv().await { + cx.shared.can_command_manager.lock(|can| { + can.send_message(m); + }); + } + } + + #[task(priority = 1, local = [led_red, led_green], shared = [&em])] + async fn blink(cx: blink::Context) { + loop { + cx.shared.em.run(|| { + if cx.shared.em.has_error() { + cx.local.led_red.toggle(); + } else { + cx.local.led_green.toggle(); + } + Ok(()) + }); + Mono::delay(1000.millis()).await; + } + } + + #[task(priority = 3, shared = [&em, sbg_power])] + async fn sleep_system(mut cx: sleep_system::Context) { + // Turn off the SBG and CAN, also start a timer to wake up the system. Put the chip in sleep mode. + cx.shared.sbg_power.lock(|sbg| { + sbg.set_low(); + }); + } + + extern "Rust" { + #[task(priority = 1, shared = [&em, sd_manager])] + async fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + fn sbg_dma(mut context: sbg_dma::Context); + + #[task(priority = 2, shared = [data_manager])] + async fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_flush(context: sbg_flush::Context); + + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + } +} diff --git a/boards/nav/src/sbg_manager.rs b/boards/nav/src/sbg_manager.rs new file mode 100644 index 00000000..4caef8a6 --- /dev/null +++ b/boards/nav/src/sbg_manager.rs @@ -0,0 +1,277 @@ +use core::alloc::{GlobalAlloc, Layout}; +use core::ffi::c_void; +use core::mem::size_of; +use core::ptr; +// use atsamd_hal::time::*; +use crate::app::sbg_flush; +use crate::app::sbg_handle_data; +// use crate::app::sbg_sd_task as sbg_sd; +use crate::app::sbg_write_data; +use crate::RTC; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use core::mem::MaybeUninit; +use defmt::info; +use embedded_alloc::Heap; +use heapless::Vec; +use messages::mavlink::embedded::Write; +use sbg_rs::sbg; +use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamX; +use stm32h7xx_hal::dma::{ + dma::{DmaConfig, StreamsTuple}, + PeripheralToMemory, Transfer, +}; +use stm32h7xx_hal::pac::UART4; +use stm32h7xx_hal::serial::{Rx, Tx}; +// use cortex_m::{asm}; +use rtic::Mutex; + +//#[link_section = ".axisram.buffers"] +//static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); +// must have this link section. +#[link_section = ".axisram.buffers"] +pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +// Simple heap required by the SBG library +static HEAP: Heap = Heap::empty(); + +pub struct SBGManager { + pub sbg_device: SBG, + xfer: Option< + Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + &'static mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + >, + >, + sbg_tx: Tx, +} + +impl SBGManager { + pub fn new( + serial: stm32h7xx_hal::serial::Serial, + stream_tuple: StreamsTuple, + //mut dma_channel: dmac::Channel, + ) -> Self { + /* Initialize the Heap */ + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + // TODO: Could add a link section here to memory. + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + + let (sbg_tx, sbg_rx) = serial.split(); + + // TODO: This could be wrong. It's a bit of a guess. + // let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; + unsafe { + // Convert an uninitialised array into an array of uninitialised + let buf: &mut [core::mem::MaybeUninit; SBG_BUFFER_SIZE] = + &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _); + buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); + } + + let config = DmaConfig::default() + .memory_increment(true) + .transfer_complete_interrupt(true); + let mut transfer: Transfer< + StreamX, + Rx, + PeripheralToMemory, + &mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + > = Transfer::init( + stream_tuple.1, + sbg_rx, + unsafe { SBG_BUFFER.assume_init_mut() }, // Uninitialised memory + None, + config, + ); + + info!("Starting transfer"); + + // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. + + transfer.start(|serial| { + serial.enable_dma_rx(); + }); + info!("Transfer started"); + + // while !transfer.get_transfer_complete_flag() {} + // info!("Transfer complete"); + // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); + + let mut sbg: sbg::SBG = sbg::SBG::new( + |data| { + sbg_handle_data::spawn(data).ok(); + }, + |data| { + sbg_write_data::spawn(data).ok(); + }, + || sbg_get_time(), + || { + sbg_flush::spawn().ok(); + }, + ); + // sbg.setup(); + // sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + + SBGManager { + sbg_device: sbg, + xfer: Some(transfer), + sbg_tx, + } + } +} + +pub async fn sbg_flush(cx: sbg_flush::Context<'_>) { + // cx.shared.sbg_manager.lock(|sbg| { + // sbg.sbg_tx + // }); +} +pub async fn sbg_write_data(mut cx: sbg_write_data::Context<'_>, data: Vec) { + cx.shared.sbg_manager.lock(|sbg| { + sbg.sbg_tx.write_all(data.as_slice()); + }); +} + +pub fn sbg_get_time() -> u32 { + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }) +} + +pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { + CallbackData::UtcTime(x) => manager.utc_time = Some(x), + CallbackData::Air(x) => manager.air = Some(x), + CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), + CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), + CallbackData::Imu(x) => manager.imu = Some(x), + CallbackData::GpsVel(x) => manager.gps_vel = Some(x), + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + }); +} + +pub async fn sbg_sd_task( + mut cx: crate::app::sbg_sd_task::Context<'_>, + data: [u8; SBG_BUFFER_SIZE], +) { + cx.shared.sd_manager.lock(|manager| { + if let Some(mut file) = manager.file.take() { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); // give the file back after use + } else if let Ok(mut file) = manager.open_file("lc24.txt") { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); + } + }); +} +/** + * Handles the DMA interrupt. + * Handles the SBG data. + */ +pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + cx.shared.sbg_manager.lock(|sbg| { + match &mut sbg.xfer { + Some(xfer) => { + if xfer.get_transfer_complete_flag() { + let data = unsafe { SBG_BUFFER.assume_init_read() }.clone(); + xfer.next_transfer( + unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory + ); + // info!("{}", data); + xfer.clear_transfer_complete_interrupt(); + sbg.sbg_device.read_data(&data); + crate::app::sbg_sd_task::spawn(data).ok(); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } + } + }); +} + +/// Stored right before an allocation. Stores information that is needed to deallocate memory. +#[derive(Copy, Clone)] +struct AllocInfo { + layout: Layout, + ptr: *mut u8, +} + +/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the +/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, +/// which makes it possible to implement [`free`] without any other data structures. +#[no_mangle] +pub extern "C" fn malloc(size: usize) -> *mut c_void { + if size == 0 { + return ptr::null_mut(); + } + + // Get a layout for both the requested size + let header_layout = Layout::new::(); + let requested_layout = Layout::from_size_align(size, 8).unwrap(); + let (layout, offset) = header_layout.extend(requested_layout).unwrap(); + + // Ask the allocator for memory + let orig_ptr = unsafe { HEAP.alloc(layout) }; + if orig_ptr.is_null() { + return orig_ptr as *mut c_void; + } + + // Compute the pointer that we will return + let result_ptr = unsafe { orig_ptr.add(offset) }; + + // Store the allocation information right before the returned pointer + let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; + unsafe { + info_ptr.write_unaligned(AllocInfo { + layout, + ptr: orig_ptr, + }); + } + + result_ptr as *mut c_void +} + +/// Custom free implementation for the SBG library. This uses the stored allocation information +/// right before the pointer to free up the resources. +/// +/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. +#[no_mangle] +pub unsafe extern "C" fn free(ptr: *mut c_void) { + assert!(!ptr.is_null()); + + let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; + let info = unsafe { info_ptr.read_unaligned() }; + unsafe { + HEAP.dealloc(info.ptr, info.layout); + } +} diff --git a/boards/nav/src/types.rs b/boards/nav/src/types.rs new file mode 100644 index 00000000..48e3e9a1 --- /dev/null +++ b/boards/nav/src/types.rs @@ -0,0 +1,10 @@ +use hal::pac; +use hal::serial::Serial; +use messages::sender::{Sender, Sender::SensorBoard}; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use stm32h7xx_hal as hal; + +pub static COM_ID: Sender = SensorBoard; + +pub type SBGSerial = Serial; +pub type SBGBuffer = &'static mut [u8; SBG_BUFFER_SIZE]; diff --git a/boards/power/Cargo.toml b/boards/power/Cargo.toml deleted file mode 100644 index 56ae7a12..00000000 --- a/boards/power/Cargo.toml +++ /dev/null @@ -1,21 +0,0 @@ -[package] -name = "power" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -cortex-m-rtic = "1.1.3" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -typenum = "1.16.0" -enum_dispatch = "0.3.11" \ No newline at end of file diff --git a/boards/power/src/communication.rs b/boards/power/src/communication.rs deleted file mode 100644 index 59ca7b66..00000000 --- a/boards/power/src/communication.rs +++ /dev/null @@ -1,174 +0,0 @@ -use crate::data_manager::DataManager; -use atsamd_hal as hal; -use common_arm::mcan; -use common_arm::HydraError; -use defmt::info; -use hal::can::Dependencies; -use hal::clock::v2::ahb::AhbClk; -use hal::clock::v2::gclk::Gclk0Id; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::types::Can0; -use hal::clock::v2::Source; -use hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use hal::pac::CAN0; -use hal::typelevel::Increment; -use heapless::Vec; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; // line 0 and 1 are connected -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::message::{rx, Raw}; -use mcan::messageram::SharedMemory; -use mcan::tx_buffers::DynTx; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; -use typenum::{U0, U128, U32, U64}; -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::node::Node::CommunicationBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn _send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn _process_data(&mut self, data_manager: &mut DataManager) { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data); - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - _ => (), - } - } - } -} diff --git a/boards/power/src/data_manager.rs b/boards/power/src/data_manager.rs deleted file mode 100644 index 0b20b216..00000000 --- a/boards/power/src/data_manager.rs +++ /dev/null @@ -1,19 +0,0 @@ -// import messages -use messages::Message; - -pub struct DataManager {} - -impl DataManager { - pub fn new() -> Self { - Self {} - } - pub fn handle_data(&mut self, _data: Message) { - // to do - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/power/src/main.rs b/boards/power/src/main.rs deleted file mode 100644 index 405b1df4..00000000 --- a/boards/power/src/main.rs +++ /dev/null @@ -1,151 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod types; - -use crate::data_manager::DataManager; -use atsamd_hal as hal; -use common_arm::mcan; -use common_arm::*; -use communication::CanDevice0; -use communication::Capacities; -use hal::clock::v2::pclk::Pclk; -use hal::clock::v2::Source; -use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; -use hal::prelude::*; -use mcan::messageram::SharedMemory; -use systick_monotonic::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - can0: CanDevice0, - } - - #[local] - struct Local { - led_green: Pin, - led_red: Pin, - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - /* Clock setup */ - let (_, clocks, tokens) = hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - let gclk0 = clocks.gclk0; - - /* CAN config */ - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can0, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - // let (pclk_adc0, gclk0) = Pclk::enable(tokens.pclks.adc0, gclk0); - - // // SAFETY: Misusing the PAC API can break the system. - // // This is safe because we only steal the MCLK. - // let (_, _, gclk, mut mclk) = unsafe { clocks.pac.steal() }; - - // // setup ADC - // let mut adc_test = hal::adc::Adc::adc0(peripherals.ADC0, &mut mclk); - - // LEDs - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); - - blink::spawn().ok(); - // adc::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can0, - }, - Local { led_green, led_red }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - // #[task(local = [adc_test, adc_pin], shared = [&em])] - // fn adc(cx: adc::Context) { - // // test adc for 5v PWR sense - // info!("try adc"); - // cx.shared.em.run(||{ - // let val = cx.local.adc_test.read(cx.local.adc_pin); - // let x: u16 = match val { - // Ok(v) => { - // v - // } - // Err(_) => { - // 0 - // } - // }; - // info!("{}", x); - // Ok(()) - // }); - // spawn_after!(adc, ExtU64::millis(500)).ok(); - // } - - /// Simple blink task to test the system. - /// Acts as a heartbeat for the system. - #[task(local = [led_green, led_red], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle()?; - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle()?; - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } -} diff --git a/boards/power/src/types.rs b/boards/power/src/types.rs deleted file mode 100644 index f8bae4ef..00000000 --- a/boards/power/src/types.rs +++ /dev/null @@ -1,5 +0,0 @@ -use messages::node::{Node, Node::PowerBoard}; -// ------- -// Node ID -// ------- -pub static _COM_ID: Node = PowerBoard; diff --git a/boards/recovery/Cargo.toml b/boards/recovery/Cargo.toml index e5460103..64888ce9 100644 --- a/boards/recovery/Cargo.toml +++ b/boards/recovery/Cargo.toml @@ -18,4 +18,8 @@ common-arm-atsame = { path = "../../libraries/common-arm-atsame" } atsamd-hal = { workspace = true } messages = { path = "../../libraries/messages" } typenum = "1.16.0" -enum_dispatch = "0.3.11" \ No newline at end of file +enum_dispatch = "0.3.11" +ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } +defmt-rtt = "0.4" +panic-probe = { version = "0.3", features = ["print-defmt"] } +embedded-hal = "0.2.7" diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index ee914d38..3ebd0082 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -3,13 +3,14 @@ use atsamd_hal::can::Dependencies; use atsamd_hal::clock::v2::ahb::AhbClk; use atsamd_hal::clock::v2::gclk::Gclk0Id; use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::types::Can0; +use atsamd_hal::clock::v2::types::{Can0, Can1}; use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use atsamd_hal::pac::CAN0; +use atsamd_hal::gpio::{Alternate, AlternateH, AlternateI, Pin, H, I, PA22, PA23, PB14, PB15}; +use atsamd_hal::pac::{CAN0, CAN1}; use atsamd_hal::typelevel::Increment; -use common_arm::mcan; use common_arm::HydraError; +use common_arm_atsame::mcan; +use defmt::error; use defmt::info; use heapless::Vec; use mcan::bus::Can; @@ -142,7 +143,7 @@ impl CanDevice0 { id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), frame_type: tx::FrameType::FlexibleDatarate { payload: &payload[..], - bit_rate_switching: false, + bit_rate_switching: true, force_error_state_indicator: false, }, store_tx_event: None, @@ -159,6 +160,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_0 { match from_bytes::(message.data()) { Ok(data) => { + // info!("Sender: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -171,6 +173,7 @@ impl CanDevice0 { for message in &mut self.can.rx_fifo_1 { match from_bytes::(message.data()) { Ok(data) => { + // info!("CAN Command: {:?}", data.clone()); data_manager.handle_data(data)?; } Err(e) => { @@ -185,3 +188,158 @@ impl CanDevice0 { Ok(()) } } + +pub struct CanCommandManager { + pub can: Can< + 'static, + Can1, + Dependencies>, Pin>, CAN1>, + Capacities, + >, + line_interrupts: OwnedInterruptSet, +} + +impl CanCommandManager { + pub fn new( + can_rx: Pin, + can_tx: Pin, + pclk_can: Pclk, + ahb_clock: AhbClk, + peripheral: CAN1, + gclk0: S, + can_memory: &'static mut SharedMemory, + loopback: bool, + ) -> (Self, S::Inc) + where + S: Source + Increment, + { + let (can_dependencies, gclk0) = + Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); + + let mut can = + mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); + can.config().mode = Mode::Fd { + allow_bit_rate_switching: true, + data_phase_timing: BitTiming::new(500.kHz()), + }; + + if loopback { + can.config().loopback = true; + } + + let interrupts_to_be_enabled = can + .interrupts() + .split( + [ + Interrupt::RxFifo0NewMessage, + Interrupt::RxFifo0Full, + // Interrupt::RxFifo0MessageLost, + Interrupt::RxFifo1NewMessage, + Interrupt::RxFifo1Full, + // Interrupt::RxFifo1MessageLost, + ] + .into_iter() + .collect(), + ) + .unwrap(); + + // Line 0 and 1 are connected to the same interrupt line + let line_interrupts = can + .interrupt_configuration() + .enable_line_0(interrupts_to_be_enabled); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::RecoveryBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Recovery filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo1, + filter: ecan::StandardId::new(messages::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::PowerBoard.into()).unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Power filter")); + + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Ground Station filter")); + + let can = can.finalize().unwrap(); + ( + CanCommandManager { + can, + line_interrupts, + }, + gclk0, + ) + } + pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + let payload: Vec = postcard::to_vec(&m)?; + self.can.tx.transmit_queued( + tx::MessageBuilder { + id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), + frame_type: tx::FrameType::FlexibleDatarate { + payload: &payload[..], + bit_rate_switching: true, + force_error_state_indicator: false, + }, + store_tx_event: None, + } + .build()?, + )?; + Ok(()) + } + pub fn process_data(&mut self, data_manager: &mut DataManager) { + let line_interrupts = &self.line_interrupts; + for interrupt in line_interrupts.iter_flagged() { + match interrupt { + Interrupt::RxFifo0NewMessage => { + for message in &mut self.can.rx_fifo_0 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("CAN Command: {:?}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + Interrupt::RxFifo1NewMessage => { + for message in &mut self.can.rx_fifo_1 { + match from_bytes::(message.data()) { + Ok(data) => { + info!("CAN Command: {:?}", data.clone()); + data_manager.handle_command(data); + } + Err(_) => { + error!("Error, ErrorContext::UnkownCanMessage"); + } + } + } + } + _ => (), + } + } + } +} diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index acceff1e..7378d97e 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -6,11 +6,13 @@ use heapless::HistoryBuffer; use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime}; use messages::Message; -const MAIN_HEIGHT: f32 = 876.0; // meters ASL -const HEIGHT_MIN: f32 = 600.0; // meters ASL -const RECOVERY_DATA_POINTS: u8 = 8; // number of barometric altitude readings held by the recovery - // algorithm -const RECOVERY_TIMER_TIMEOUT: u8 = 15; // minutes +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 TICK_RATE: f32 = 0.002; // seconds +const ASCENT_LOCKOUT: f32 = 100.0; +const DATA_POINTS: usize = 8; +const VALID_DESCENT_RATE: f32 = -1.0; // meters per second pub struct DataManager { pub air: Option, @@ -19,8 +21,7 @@ pub struct DataManager { pub imu: (Option, Option), pub utc_time: Option, pub gps_vel: Option, - pub historical_barometer_altitude: HistoryBuffer<(f32, u32), 8>, // RECOVERY_DATA_POINTS (issue - // when putting as const) + pub historical_barometer_altitude: HistoryBuffer<(f32, u32), DATA_POINTS>, // (alt, timestamp) pub current_state: Option, // each tick represents a minute that passed pub recovery_counter: u8, @@ -42,8 +43,45 @@ impl DataManager { } } /// Returns true if the rocket is descending + // pub fn is_falling(&self) -> bool { + // if self.historical_barometer_altitude.len() < 8 { + // info!("not enough data points"); + // return false; + // } + // let mut buf = self.historical_barometer_altitude.oldest_ordered(); + // match buf.next() { + // Some(last) => { + // let mut avg_sum: f32 = 0.0; + // let mut prev = last; + // for i in buf { + + // let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; // Each tick is 2ms, so multiply by 0.002 to get seconds + // info!("prev alt: {:?}, new alt: {}, time diff {}", prev.0, i.0, time_diff); + + // if time_diff == 0.0 { + // continue; + // } + // let slope = (i.0 - prev.0) / time_diff; + // if slope > ASCENT_LOCKOUT { + // return false; + // } + // avg_sum += slope; + // prev = i; + // } + // if avg_sum / (DATA_POINTS as f32 - 1.0) > VALID_DESCENT_RATE { + // return false; + // } + // info!("avg_sum: {}", avg_sum / (DATA_POINTS as f32 - 1.0)); + // } + // None => { + // return false; + // } + // } + // true + // } pub fn is_falling(&self) -> bool { - if (self.historical_barometer_altitude.len() as u8) < RECOVERY_DATA_POINTS { + if self.historical_barometer_altitude.len() < 8 { + info!("not enough data points"); return false; } let mut buf = self.historical_barometer_altitude.oldest_ordered(); @@ -52,25 +90,23 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 / 1_000_000.0; + let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; // Each tick is 2ms, so multiply by 0.002 to get seconds + info!("prev alt: {:?}, new alt: {}, time diff {}", prev.0, i.0, time_diff); + if time_diff == 0.0 { continue; } let slope = (i.0 - prev.0) / time_diff; - if slope < -100.0 { + if slope > ASCENT_LOCKOUT { return false; } avg_sum += slope; prev = i; - } - match avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.0) { - // 7 because we have 8 points. - // exclusive range - x if !(-100.0..=-5.0).contains(&x) => { - return false; - } - _ => { - info!("avg: {}", avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.0)); + + // Check if the average descent rate is valid + if avg_sum / (DATA_POINTS as f32 - 1.0) <= VALID_DESCENT_RATE { + info!("avg_sum: {}", avg_sum / (DATA_POINTS as f32 - 1.0)); + return true; } } } @@ -78,8 +114,9 @@ impl DataManager { return false; } } - true + false } + pub fn is_launched(&self) -> bool { match self.air.as_ref() { Some(air) => match air.altitude { @@ -99,7 +136,7 @@ impl DataManager { let mut avg_sum: f32 = 0.0; let mut prev = last; for i in buf { - let time_diff: f32 = (i.1 - prev.1) as f32 / 1_000_000.0; + let time_diff: f32 = (i.1 - prev.1) as f32 * TICK_RATE; if time_diff == 0.0 { continue; } @@ -108,10 +145,8 @@ impl DataManager { } match avg_sum / (RECOVERY_DATA_POINTS as f32 - 1.0) { // inclusive range - x if (-0.25..=0.25).contains(&x) => { - if self.recovery_counter >= RECOVERY_TIMER_TIMEOUT { - return true; - } + x if (-0.5..=0.5).contains(&x) => { + return true; } _ => { self.recovery_counter = 0; @@ -146,9 +181,8 @@ impl DataManager { the alt is dropped, if the number is high switch to the on board barometer. */ - if let Some(alt) = air_data.altitude { - let tup_data: (f32, u32) = (alt, air_data.time_stamp); + let tup_data: (f32, u32) = (alt, data.timestamp); self.air = Some(air_data); if let Some(recent) = self.historical_barometer_altitude.recent() { if recent.1 != tup_data.1 { @@ -182,9 +216,16 @@ impl DataManager { } _ => {} }, + + _ => {} + } + Ok(()) + } + pub fn handle_command(&mut self, command: Message) -> Result<(), HydraError> { + match command.data { messages::Data::Command(command) => match command.data { messages::command::CommandData::DeployDrogue(_) => { - spawn!(fire_drogue)?; // need someway to capture this error. + spawn!(fire_drogue)?; } messages::command::CommandData::DeployMain(_) => { spawn!(fire_main)?; diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs index 2857fbdd..6e8cb61a 100644 --- a/boards/recovery/src/gpio_manager.rs +++ b/boards/recovery/src/gpio_manager.rs @@ -1,17 +1,23 @@ -use atsamd_hal::gpio::{Pin, PushPullOutput, PA06, PA09}; +use atsamd_hal::gpio::{Pin, PushPullOutput, PB11, PB12}; use atsamd_hal::prelude::*; - +use defmt::info; pub struct GPIOManager { - main_ematch: Pin, - drogue_ematch: Pin, + main_ematch: Pin, + drogue_ematch: Pin, } impl GPIOManager { pub fn new( - mut main_ematch: Pin, - mut drogue_ematch: Pin, + mut main_ematch: Pin, + mut drogue_ematch: Pin, ) -> Self { drogue_ematch.set_low().ok(); + match drogue_ematch.set_low() { + Ok(_) => {} + Err(_) => { + info!("Cannot set low"); + } + } main_ematch.set_low().ok(); Self { main_ematch, diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index c96fbbc7..a10f886c 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -12,29 +12,52 @@ use atsamd_hal::clock::v2::pclk::Pclk; use atsamd_hal::clock::v2::Source; use atsamd_hal::dmac::DmaController; use common_arm::hinfo; -use common_arm::mcan; use common_arm::*; +use common_arm_atsame::mcan; +use communication::CanCommandManager; use communication::Capacities; use data_manager::DataManager; +use defmt::info; +use defmt_rtt as _; +use embedded_hal::spi::FullDuplex; use gpio_manager::GPIOManager; -use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; +use hal::gpio::{Alternate, Pin, Pins, PushPullOutput, D, PA04, PA05, PA07}; +use panic_probe as _; // global logger + +use core::cell::RefCell; +use cortex_m::interrupt::Mutex; use hal::prelude::*; -use hal::timer::TimerCounter2; +use hal::rtc::Count32Mode; +use hal::sercom::{spi, spi::Config, spi::Duplex, spi::Pads, spi::Spi, IoSet3, Sercom0}; use mcan::messageram::SharedMemory; use messages::*; +use ms5611_01ba::{calibration::OversamplingRatio, MS5611_01BA}; use state_machine::{StateMachine, StateMachineContext}; use systick_monotonic::*; use types::COM_ID; +pub static RTC: Mutex>>> = Mutex::new(RefCell::new(None)); +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); +/// Hardfault handler. +/// +/// Terminates the application and makes a semihosting-capable debug tool exit +/// with an error. This seems better than the default, which is to spin in a +/// loop. +#[cortex_m_rt::exception] +unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! { + loop {} } #[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] mod app { + + use atsamd_hal::gpio::{B, PA03, PB04, PB06, PB07, PB08, PB09}; + use embedded_hal::digital::v2::StatefulOutputPin; + use super::*; #[shared] @@ -43,14 +66,33 @@ mod app { data_manager: DataManager, can0: communication::CanDevice0, gpio: GPIOManager, - recovery_timer: TimerCounter2, + can_command_manager: CanCommandManager, } #[local] struct Local { - led_green: Pin, - led_red: Pin, + led_green: Pin, + led_red: Pin, + drogue_current_sense: Pin>, + main_current_sense: Pin>, + drogue_sense: Pin>, + main_sense: Pin>, + adc1: hal::adc::Adc, state_machine: StateMachine, + barometer: MS5611_01BA< + Spi< + Config< + Pads< + hal::pac::SERCOM0, + IoSet3, + Pin>, + Pin>, + Pin>, + >, + >, + Duplex, + >, + >, } #[monotonic(binds = SysTick, default = true)] @@ -58,13 +100,17 @@ mod app { #[init(local = [ #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() + can_memory: SharedMemory = SharedMemory::new(), + #[link_section = ".can_command"] + can_command_memory: SharedMemory = SharedMemory::new() ])] fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { let mut peripherals = cx.device; let core = cx.core; let pins = Pins::new(peripherals.PORT); + HydraLogging::set_ground_station_callback(queue_gs_message); + let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); let _dmaChannels = dmac.split(); @@ -95,28 +141,88 @@ mod app { false, ); - /* GPIO config */ - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); - let gpio = GPIOManager::new( - // pins switched from schematic - pins.pa09.into_push_pull_output(), - pins.pa06.into_push_pull_output(), + let (pclk_can_command, gclk0) = Pclk::enable(tokens.pclks.can1, gclk0); + let (can_command_manager, gclk0) = CanCommandManager::new( + pins.pb15.into_mode(), + pins.pb14.into_mode(), + pclk_can_command, + clocks.ahbs.can1, + peripherals.CAN1, + gclk0, + cx.local.can_command_memory, + false, ); + + /* GPIO config */ + let led_green = pins.pa03.into_push_pull_output(); + let led_red = pins.pb04.into_push_pull_output(); + let mut main_ematch = pins.pb12.into_push_pull_output(); + let mut drogue_ematch = pins.pb11.into_push_pull_output(); + main_ematch.set_low().ok(); + drogue_ematch.set_low().ok(); + + + let gpio = GPIOManager::new(main_ematch, drogue_ematch); /* State Machine config */ let state_machine = StateMachine::new(); - /* Recovery Timer config */ - let (pclk_tc2tc3, gclk0) = Pclk::enable(tokens.pclks.tc2_tc3, gclk0); - let timerclk: hal::clock::v1::Tc2Tc3Clock = pclk_tc2tc3.into(); - let recovery_timer = hal::timer::TimerCounter2::tc2_(&timerclk, peripherals.TC2, &mut mclk); + //type pads = hal::sercom::spi::PadsFromIds; + //let pads_spi = pads::Pads::new(pins.pa07, pins.pa04, pins.pa05, pins.pa06); + + let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); + let pads_spi = spi::Pads::::default() + .sclk(pins.pa05) + .data_in(pins.pa07) + .data_out(pins.pa04); + + let baro_spi = spi::Config::new(&mclk, peripherals.SERCOM0, pads_spi, pclk_sd.freq()) + .length::() + .bit_order(spi::BitOrder::MsbFirst) + .spi_mode(spi::MODE_0) + .enable(); + + let barometer = MS5611_01BA::new(baro_spi, OversamplingRatio::OSR2048); + + info!("RTC"); + let rtc = hal::rtc::Rtc::clock_mode(peripherals.RTC, 1024.Hz(), &mut mclk); + let mut rtc = rtc.into_count32_mode(); // hal bug this must be done + rtc.set_count32(0); + cortex_m::interrupt::free(|cs| { + RTC.borrow(cs).replace(Some(rtc)); + }); + // info!("RTC done"); + + // ADC sensing setup + // let (pclk_adc, gclk0) = Pclk::enable(tokens.pclks.adc1, gclk0); + // let adc1 = hal::adc::Adc::adc1(peripherals.ADC1, &mut mclk); + // let drogue_current_sense = pins.pb06.into_alternate(); + // let main_current_sense = pins.pb07.into_alternate(); + // let drogue_sense = pins.pb08.into_alternate(); + // let main_sense = pins.pb09.into_alternate(); + // let data = adc1.read(&mut drogue_sense); + + let mut drogue_current_sense = pins.pb06.into_push_pull_output(); + let mut main_current_sense = pins.pb07.into_push_pull_output(); + drogue_current_sense.set_low().ok(); + main_current_sense.set_low().ok(); /* Spawn tasks */ run_sm::spawn().ok(); + // read_barometer::spawn().ok(); state_send::spawn().ok(); + ejection_sense::spawn().ok(); blink::spawn().ok(); - // fire_main::spawn_after(ExtU64::secs(15)).ok(); - // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); + // send an online message to the com board. + let message = Message::new( + 0,// technically true time is not known yet. + COM_ID, + messages::command::Command { + data: messages::command::CommandData::Online(messages::command::Online { online: true }), + }, + ); + send_command::spawn(message).ok(); + // fire_main::spawn_after(ExtU64::secs(60)).ok(); + // fire_drogue::spawn_after(ExtU64::secs(60)).ok(); /* Monotonic clock */ let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); @@ -126,6 +232,7 @@ mod app { em: ErrorManager::new(), data_manager: DataManager::new(), can0, + can_command_manager, gpio, recovery_timer, }, @@ -133,6 +240,12 @@ mod app { led_green, led_red, state_machine, + adc1, + drogue_current_sense, + main_current_sense, + drogue_sense, + main_sense, + barometer, }, init::Monotonics(mono), ) @@ -144,27 +257,94 @@ mod app { loop {} } - // interrupt handler for recovery counter - #[task(binds=TC2, shared=[data_manager, recovery_timer])] - fn recovery_counter_tick(mut cx: recovery_counter_tick::Context) { - cx.shared.recovery_timer.lock(|timer| { - if timer.wait().is_ok() { - cx.shared.data_manager.lock(|data| { - data.recovery_counter += 1; - }); - // restart timer after interrupt - let duration_mins = atsamd_hal::fugit::MinutesDurationU32::minutes(1); - // timer requires specific duration format - let timer_duration: atsamd_hal::fugit::Duration = - duration_mins.convert(); - timer.start(timer_duration); - } - timer.enable_interrupt(); // clear interrupt + #[task(local = [adc1, main_sense, drogue_sense, main_current_sense, drogue_current_sense], shared = [&em])] + fn ejection_sense(mut cx: ejection_sense::Context) { + cx.shared.em.run(|| { + let data_drogue_current: u16 = + cx.local.adc1.read(cx.local.drogue_current_sense).unwrap(); + let data_main_current: u16 = cx.local.adc1.read(cx.local.main_current_sense).unwrap(); + let data_drogue_sense: u16 = cx.local.adc1.read(cx.local.drogue_sense).unwrap(); + let data_main_sense: u16 = cx.local.adc1.read(cx.local.main_sense).unwrap(); + let sensing = messages::sensor::Sensor { + data: messages::sensor::SensorData::RecoverySensing( + messages::sensor::RecoverySensing { + drogue_current: data_drogue_current, + main_current: data_main_current, + drogue_voltage: data_drogue_sense, + main_voltage: data_main_sense, + }, + ), + }; + + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + sensing, + ); + // info!("Drogue current: {} Main current: {} Drogue sense: {} Main sense: {}", data_drogue_current, data_main_current, data_drogue_sense, data_main_sense); + spawn!(send_internal, message).ok(); + spawn_after!(ejection_sense, ExtU64::secs(1)).ok(); + Ok(()) + }); + } + + /// Handles the CAN0 interrupt. + #[task(priority = 3, binds = CAN1, shared = [can_command_manager, data_manager])] + fn can_command(mut cx: can_command::Context) { + info!("CAN1 interrupt"); + cx.shared.can_command_manager.lock(|can| { + cx.shared + .data_manager + .lock(|data_manager| can.process_data(data_manager)); + }); + } + + /// Receives a log message from the custom logger so that it can be sent over the radio. + pub fn queue_gs_message(d: impl Into) { + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + d.into(), + ); + + send_internal::spawn(message).ok(); + } + + /** + * Sends a message over CAN. + */ + #[task(capacity = 5, shared = [can_command_manager, &em])] + fn send_command(mut cx: send_command::Context, m: Message) { + cx.shared.em.run(|| { + cx.shared + .can_command_manager + .lock(|can| can.send_message(m))?; + Ok(()) + }); + } + + #[task(local = [barometer], shared = [&em])] + fn read_barometer(cx: read_barometer::Context) { + cx.shared.em.run(|| { + let barometer = cx.local.barometer; + let (p, t) = barometer.get_data()?; + info!("pressure {} temperature {}", p, t); + Ok(()) }); + spawn_after!(read_barometer, ExtU64::secs(1)).ok(); } #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_drogue(mut cx: fire_drogue::Context) { + info!("Firing drogue"); cx.shared.em.run(|| { if !(*cx.local.fired) { cx.shared.gpio.lock(|gpio| { @@ -183,6 +363,7 @@ mod app { #[task(priority = 3, local = [fired: bool = false], shared=[gpio, &em])] fn fire_main(mut cx: fire_main::Context) { + info!("Firing main"); cx.shared.em.run(|| { if !(*cx.local.fired) { cx.shared.gpio.lock(|gpio| { @@ -236,7 +417,7 @@ mod app { } /// Sends info about the current state of the system. - #[task(shared = [data_manager, &em])] + #[task(priority = 3, shared = [data_manager, &em])] fn state_send(mut cx: state_send::Context) { cx.shared.em.run(|| { let rocket_state = cx @@ -250,9 +431,17 @@ mod app { return Ok(()); }; let board_state = messages::state::State { data: state.into() }; - let message = Message::new(&monotonics::now(), COM_ID, board_state); - spawn!(send_internal, message)?; - spawn_after!(state_send, ExtU64::secs(2))?; // I think this is fine here. + let message = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + board_state, + ); + spawn!(send_command, message)?; + spawn_after!(state_send, ExtU64::secs(5))?; // I think this is fine here. Ok(()) }); } diff --git a/boards/recovery/src/state_machine/states/ascent.rs b/boards/recovery/src/state_machine/states/ascent.rs index 5caf9ec1..c1a84012 100644 --- a/boards/recovery/src/state_machine/states/ascent.rs +++ b/boards/recovery/src/state_machine/states/ascent.rs @@ -1,8 +1,8 @@ -use crate::app::monotonics; use crate::state_machine::states::descent::Descent; use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; use crate::types::COM_ID; +use crate::RTC; use crate::{no_transition, transition}; use defmt::{write, Format, Formatter}; use messages::command::{Command, RadioRate, RadioRateChange}; @@ -14,19 +14,24 @@ pub struct Ascent {} impl State for Ascent { fn enter(&self, context: &mut StateMachineContext) { - for _i in 0..10 { let radio_rate_change = RadioRateChange { rate: RadioRate::Fast, }; - let message_com = - Message::new(&monotonics::now(), COM_ID, Command::new(radio_rate_change)); + let message_com = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + Command::new(radio_rate_change), + ); context.shared_resources.can0.lock(|can| { context.shared_resources.em.run(|| { can.send_message(message_com)?; Ok(()) }) }); - } } fn step(&mut self, context: &mut StateMachineContext) -> Option { context.shared_resources.data_manager.lock(|data| { diff --git a/boards/recovery/src/state_machine/states/wait_for_recovery.rs b/boards/recovery/src/state_machine/states/wait_for_recovery.rs index ddcef0b3..b28f698e 100644 --- a/boards/recovery/src/state_machine/states/wait_for_recovery.rs +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -1,41 +1,56 @@ use super::TerminalDescent; -use crate::app::monotonics; +use crate::app::send_command; use crate::no_transition; use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; use crate::types::COM_ID; -use atsamd_hal::timer_traits::InterruptDrivenTimer; +use crate::RTC; +use common_arm::spawn; use defmt::{write, Format, Formatter}; use messages::command::{Command, PowerDown, RadioRate, RadioRateChange}; use messages::node::Node::SensorBoard; use messages::Message; -use rtic::mutex::Mutex; #[derive(Debug, Clone)] pub struct WaitForRecovery {} impl State for WaitForRecovery { fn enter(&self, context: &mut StateMachineContext) { - // This should change to a ack and not be sent 10 times - // send a command over CAN to shut down non-critical systems for recovery. - for _i in 0..10 { - let sensor_power_down = PowerDown { board: SensorBoard }; + // let sensor_power_down = PowerDown { board: SensorBoard }; let radio_rate_change = RadioRateChange { rate: RadioRate::Slow, }; - let message = Message::new(&monotonics::now(), COM_ID, Command::new(sensor_power_down)); - let message_com = - Message::new(&monotonics::now(), COM_ID, Command::new(radio_rate_change)); - context.shared_resources.can0.lock(|can| { - context.shared_resources.em.run(|| { - can.send_message(message)?; - can.send_message(message_com)?; - Ok(()) - }) + // let message = Message::new( + // cortex_m::interrupt::free(|cs| { + // let mut rc = RTC.borrow(cs).borrow_mut(); + // let rtc = rc.as_mut().unwrap(); + // rtc.count32() + // }), + // COM_ID, + // Command::new(sensor_power_down), + // ); + let message_com = Message::new( + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.count32() + }), + COM_ID, + Command::new(radio_rate_change), + ); + context.shared_resources.em.run(|| { + // spawn!(send_command, message)?; + spawn!(send_command, message_com)?; + Ok(()) }); - } - context.shared_resources.recovery_timer.lock(|timer| { - timer.disable_interrupt(); - }) + + // context.shared_resources.can0.lock(|can| { + // context.shared_resources.em.run(|| { + // can.send_message(message)?; + // can.send_message(message_com)?; + // Ok(()) + // }) + // }); + // } } fn step(&mut self, _context: &mut StateMachineContext) -> Option { no_transition!() // this is our final resting place. We should also powerdown this board. @@ -50,6 +65,6 @@ impl TransitionInto for TerminalDescent { impl Format for WaitForRecovery { fn format(&self, f: Formatter) { - write!(f, "Descent") + write!(f, "Wait For Recovery") } } diff --git a/boards/sensor/src/communication.rs b/boards/sensor/src/communication.rs deleted file mode 100644 index 097adcbb..00000000 --- a/boards/sensor/src/communication.rs +++ /dev/null @@ -1,195 +0,0 @@ -use atsamd_hal::can::Dependencies; -use atsamd_hal::clock::v2::ahb::AhbClk; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; - -use atsamd_hal::clock::v2::types::Can0; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::gpio::{Alternate, AlternateI, Pin, I, PA22, PA23}; -use atsamd_hal::pac::CAN0; - -use atsamd_hal::typelevel::Increment; -use common_arm::mcan; -use common_arm::HydraError; -use defmt::info; -use heapless::Vec; -use mcan::bus::Can; -use mcan::embedded_can as ecan; -use mcan::interrupt::state::EnabledLine0; -use mcan::interrupt::{Interrupt, OwnedInterruptSet}; -use mcan::message::tx; -use mcan::message::{rx, Raw}; -use mcan::messageram::SharedMemory; -use mcan::tx_buffers::DynTx; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; - -use messages::Message; -use postcard::from_bytes; -use systick_monotonic::fugit::RateExtU32; -use typenum::{U0, U128, U32, U64}; - -use crate::data_manager::DataManager; - -pub struct Capacities; - -impl mcan::messageram::Capacities for Capacities { - type StandardFilters = U128; - type ExtendedFilters = U64; - type RxBufferMessage = rx::Message<64>; - type DedicatedRxBuffers = U64; - type RxFifo0Message = rx::Message<64>; - type RxFifo0 = U64; - type RxFifo1Message = rx::Message<64>; - type RxFifo1 = U64; - type TxMessage = tx::Message<64>; - type TxBuffers = U32; - type DedicatedTxBuffers = U0; - type TxEventFifo = U32; -} - -pub struct CanDevice0 { - pub can: Can< - 'static, - Can0, - Dependencies>, Pin>, CAN0>, - Capacities, - >, - line_interrupts: OwnedInterruptSet, -} - -impl CanDevice0 { - pub fn new( - can_rx: Pin, - can_tx: Pin, - pclk_can: Pclk, - ahb_clock: AhbClk, - peripheral: CAN0, - gclk0: S, - can_memory: &'static mut SharedMemory, - loopback: bool, - ) -> (Self, S::Inc) - where - S: Source + Increment, - { - let (can_dependencies, gclk0) = - Dependencies::new(gclk0, pclk_can, ahb_clock, can_rx, can_tx, peripheral); - - let mut can = - mcan::bus::CanConfigurable::new(200.kHz(), can_dependencies, can_memory).unwrap(); - can.config().mode = Mode::Fd { - allow_bit_rate_switching: false, - data_phase_timing: BitTiming::new(500.kHz()), - }; - - if loopback { - can.config().loopback = true; - } - - let interrupts_to_be_enabled = can - .interrupts() - .split( - [ - Interrupt::RxFifo0NewMessage, - Interrupt::RxFifo0Full, - Interrupt::RxFifo0MessageLost, - Interrupt::RxFifo1NewMessage, - Interrupt::RxFifo1Full, - Interrupt::RxFifo1MessageLost, - ] - .into_iter() - .collect(), - ) - .unwrap(); - - // Line 0 and 1 are connected to the same interrupt line - let line_interrupts = can - .interrupt_configuration() - .enable_line_0(interrupts_to_be_enabled); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::CommunicationBoard.into()) - .unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Communication filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::node::Node::RecoveryBoard.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Recovery filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::node::Node::GroundStation.into()).unwrap(), - mask: ecan::StandardId::ZERO, - }) - .unwrap_or_else(|_| panic!("Ground Station filter")); - - let can = can.finalize().unwrap(); - ( - CanDevice0 { - can, - line_interrupts, - }, - gclk0, - ) - } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { - let payload: Vec = postcard::to_vec(&m)?; - self.can.tx.transmit_queued( - tx::MessageBuilder { - id: ecan::Id::Standard(ecan::StandardId::new(m.sender.into()).unwrap()), - frame_type: tx::FrameType::FlexibleDatarate { - payload: &payload[..], - bit_rate_switching: false, - force_error_state_indicator: false, - }, - store_tx_event: None, - } - .build()?, - )?; - Ok(()) - } - pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { - let line_interrupts = &self.line_interrupts; - for interrupt in line_interrupts.iter_flagged() { - match interrupt { - Interrupt::RxFifo0NewMessage => { - for message in &mut self.can.rx_fifo_0 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data)?; - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - data_manager.handle_data(data)?; - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - _ => (), - } - } - Ok(()) - } -} diff --git a/boards/sensor/src/main.rs b/boards/sensor/src/main.rs deleted file mode 100644 index 96d1eea2..00000000 --- a/boards/sensor/src/main.rs +++ /dev/null @@ -1,261 +0,0 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod sbg_manager; -mod types; - -use atsamd_hal as hal; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::clock::v2::Source; -use atsamd_hal::dmac::DmaController; -use common_arm::mcan; -use common_arm::SdManager; -use common_arm::*; -use communication::Capacities; -use data_manager::DataManager; -use defmt::info; -use hal::dmac; -use hal::gpio::Output; -use hal::gpio::Pins; -use hal::gpio::{Pin, PushPullOutput}; -use hal::gpio::{PushPull, PB01, PB10, PB16, PB17}; -use hal::prelude::*; -use hal::sercom::{spi, IoSet2, Sercom4}; -use mcan::messageram::SharedMemory; -use messages::sensor::Sensor; -use messages::*; -use sbg_manager::{sbg_dma, sbg_handle_data, sbg_sd_task, SBGManager}; -//use sbg_manager::{sbg_dma, sbg_handle_data, SBGManager}; -use sbg_rs::sbg::{CallbackData, SBG_BUFFER_SIZE}; -use systick_monotonic::*; -use types::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(_info: &core::panic::PanicInfo) -> ! { - atsamd_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] -mod app { - use super::*; - - #[shared] - struct Shared { - em: ErrorManager, - data_manager: DataManager, - can: communication::CanDevice0, - sd_manager: SdManager>>, - } - - #[local] - struct Local { - led_green: Pin, - led_red: Pin, - sbg_manager: SBGManager, - sbg_power_pin: Pin, // this is here so we do not need to lock sbg_manager.! put into a gpio controller with leds. - } - - #[monotonic(binds = SysTick, default = true)] - type SysMono = Systick<100>; // 100 Hz / 10 ms granularity - - #[init(local = [ - #[link_section = ".can"] - can_memory: SharedMemory = SharedMemory::new() - ])] - fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) { - let mut peripherals = cx.device; - let core = cx.core; - let pins = Pins::new(peripherals.PORT); - - let mut sbg_power_pin = pins.pb01.into_push_pull_output(); - sbg_power_pin.set_high().unwrap(); - - let mut dmac = DmaController::init(peripherals.DMAC, &mut peripherals.PM); - let dmaChannels = dmac.split(); - - /* Clock setup */ - let (_, clocks, tokens) = atsamd_hal::clock::v2::clock_system_at_reset( - peripherals.OSCCTRL, - peripherals.OSC32KCTRL, - peripherals.GCLK, - peripherals.MCLK, - &mut peripherals.NVMCTRL, - ); - let gclk0 = clocks.gclk0; - - // SAFETY: Misusing the PAC API can break the system. - // This is safe because we only steal the MCLK. - let (_, _, _, mut mclk) = unsafe { clocks.pac.steal() }; - - /* CAN config */ - let (pclk_can, gclk0) = Pclk::enable(tokens.pclks.can0, gclk0); - let (can, gclk0) = communication::CanDevice0::new( - pins.pa23.into_mode(), - pins.pa22.into_mode(), - pclk_can, - clocks.ahbs.can0, - peripherals.CAN0, - gclk0, - cx.local.can_memory, - false, - ); - - /* SD config */ - let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom4, gclk0); - let pads_spi = spi::Pads::::default() - .sclk(pins.pb09) - .data_in(pins.pb11) - .data_out(pins.pb08); - - let sdmmc_spi = spi::Config::new(&mclk, peripherals.SERCOM4, pads_spi, pclk_sd.freq()) - .length::() - .bit_order(spi::BitOrder::MsbFirst) - .spi_mode(spi::MODE_0) - .enable(); - let sd_manager = SdManager::new(sdmmc_spi, pins.pb10.into_push_pull_output()); - - /* SBG config */ - let (pclk_sbg, gclk0) = Pclk::enable(tokens.pclks.sercom5, gclk0); - let dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); - let sbg_manager = SBGManager::new( - pins.pb03, - pins.pb02, - pclk_sbg, - &mut mclk, - peripherals.SERCOM5, - peripherals.RTC, - dmaCh0, - ); - - // Buzzer should go here. There is complexity using the new clock system with the atsamdhal pwm implementation. - - /* Status LED */ - let led_green = pins.pb16.into_push_pull_output(); - let led_red = pins.pb17.into_push_pull_output(); - - /* Spawn tasks */ - sensor_send::spawn().ok(); - blink::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can, - sd_manager, - }, - Local { - led_green, - led_red, - sbg_manager, - sbg_power_pin, - }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - #[task(local = [sbg_power_pin], shared = [sd_manager, &em])] - fn sleep_system(mut cx: sleep_system::Context) { - info!("Power Down"); - // close out sd files. - cx.shared.sd_manager.lock(|sd| { - cx.shared.em.run(|| { - sd.close_current_file()?; - // sd.close(); // we can also close the root directory and volume. - // power down sbg - cx.local.sbg_power_pin.set_low()?; // define hydra error for this error type. - // Call core.SCB.set_deepsleep for even less power consumption. - Ok(()) - }); - }); - } - - #[task(priority = 3, binds = CAN0, shared = [can, data_manager, &em])] - fn can0(mut cx: can0::Context) { - cx.shared.can.lock(|can| { - cx.shared.data_manager.lock(|manager| { - cx.shared.em.run(|| { - can.process_data(manager)?; - Ok(()) - }); - }); - }); - } - - /** - * Sends a message over CAN. - */ - #[task(capacity = 10, local = [counter: u16 = 0], shared = [can, &em])] - fn send_internal(mut cx: send_internal::Context, m: Message) { - cx.shared.em.run(|| { - cx.shared.can.lock(|can| can.send_message(m))?; - Ok(()) - }); - } - - /** - * Sends information about the sensors. - */ - #[task(shared = [data_manager, &em])] - fn sensor_send(mut cx: sensor_send::Context) { - let sensor_data = cx - .shared - .data_manager - .lock(|data_manager| data_manager.clone_sensors()); - - let messages = sensor_data - .into_iter() - .flatten() - .map(|x| Message::new(&monotonics::now(), COM_ID, Sensor::new(x))); - - cx.shared.em.run(|| { - for msg in messages { - spawn!(send_internal, msg)?; - } - Ok(()) - }); - spawn_after!(sensor_send, ExtU64::millis(100)).ok(); - } - - /** - * Simple blink task to test the system. - * Acts as a heartbeat for the system. - */ - #[task(local = [led_green, led_red], shared = [&em])] - fn blink(cx: blink::Context) { - cx.shared.em.run(|| { - if cx.shared.em.has_error() { - cx.local.led_red.toggle()?; - spawn_after!(blink, ExtU64::millis(200))?; - } else { - cx.local.led_green.toggle()?; - spawn_after!(blink, ExtU64::secs(1))?; - } - Ok(()) - }); - } - - extern "Rust" { - #[task(capacity = 3, shared = [&em, sd_manager])] - fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); - - #[task(binds = DMAC_0, shared = [&em], local = [sbg_manager])] - fn sbg_dma(context: sbg_dma::Context); - - #[task(capacity = 20, shared = [data_manager])] - fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); - } -} diff --git a/boards/sensor/src/sbg_manager.rs b/boards/sensor/src/sbg_manager.rs deleted file mode 100644 index 25b8e259..00000000 --- a/boards/sensor/src/sbg_manager.rs +++ /dev/null @@ -1,207 +0,0 @@ -use crate::types::{ConfigSBG, SBGBuffer, SBGTransfer}; -use atsamd_hal::clock::v2::gclk::Gclk0Id; -use atsamd_hal::clock::v2::pclk::Pclk; -use atsamd_hal::dmac; -use atsamd_hal::dmac::Transfer; -use atsamd_hal::gpio::{Pin, Reset, PB02, PB03}; -use atsamd_hal::pac::{MCLK, RTC}; -use atsamd_hal::sercom::IoSet6; -// use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext; -use atsamd_hal::rtc::Rtc; -use core::alloc::{GlobalAlloc, Layout}; -use core::ffi::c_void; -use core::mem::size_of; -use core::ptr; -// use atsamd_hal::time::*; -use crate::app::sbg_handle_data; -use crate::app::sbg_sd_task as sbg_sd; -use atsamd_hal::prelude::*; -use atsamd_hal::sercom::{uart, Sercom, Sercom5}; -use common_arm::spawn; -use defmt::info; -use embedded_alloc::Heap; -use rtic::Mutex; -use sbg_rs::sbg; -use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; - -pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE]; - -// Simple heap required by the SBG library -static HEAP: Heap = Heap::empty(); - -pub struct SBGManager { - sbg_device: SBG, - xfer: Option, -} - -impl SBGManager { - pub fn new( - rx: Pin, - tx: Pin, - pclk_sercom5: Pclk, - mclk: &mut MCLK, - sercom5: Sercom5, - rtc: RTC, - mut dma_channel: dmac::Channel, - ) -> Self { - /* Initialize the Heap */ - { - use core::mem::MaybeUninit; - const HEAP_SIZE: usize = 1024; - static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } - } - - let pads_sbg = uart::Pads::::default().rx(rx).tx(tx); - let uart_sbg = ConfigSBG::new(mclk, sercom5, pads_sbg, pclk_sercom5.freq()) - .baud( - 115200.Hz(), - uart::BaudMode::Fractional(uart::Oversampling::Bits8), - ) - .enable(); - - let (sbg_rx, sbg_tx) = uart_sbg.split(); - - /* DMAC config */ - dma_channel - .as_mut() - .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); - let xfer = Transfer::new(dma_channel, sbg_rx, unsafe { &mut *BUF_DST }, false) - .expect("DMA err") - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - - // There is a bug within the HAL that improperly configures the RTC - // in count32 mode. This is circumvented by first using clock mode then - // converting to count32 mode. - let rtc_temp = Rtc::clock_mode(rtc, 1024.Hz(), mclk); - let mut rtc = rtc_temp.into_count32_mode(); - rtc.set_count32(0); - - let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| { - sbg_handle_data::spawn(data).ok(); - }); - - SBGManager { - sbg_device: sbg, - xfer: Some(xfer), - } - } -} - -pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) { - cx.shared.data_manager.lock(|manager| match data { - CallbackData::UtcTime(x) => manager.utc_time = Some(x), - CallbackData::Air(x) => manager.air = Some(x), - CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), - CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), - CallbackData::Imu(x) => manager.imu = Some(x), - CallbackData::GpsVel(x) => manager.gps_vel = Some(x), - CallbackData::GpsPos(x) => manager.gps_pos = Some(x), - }); -} - -pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) { - cx.shared.sd_manager.lock(|manager| { - if let Some(mut file) = manager.file.take() { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); // give the file back after use - } else if let Ok(mut file) = manager.open_file("sbg.txt") { - cx.shared.em.run(|| { - manager.write(&mut file, &data)?; - Ok(()) - }); - manager.file = Some(file); - } - }); -} -/** - * Handles the DMA interrupt. - * Handles the SBG data. - */ -pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { - let sbg = cx.local.sbg_manager; - - match &mut sbg.xfer { - Some(xfer) => { - if xfer.complete() { - let (chan0, source, buf) = sbg.xfer.take().unwrap().stop(); - let mut xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false) - .unwrap() - .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); - let buf_clone = buf.clone(); - sbg.sbg_device.read_data(buf); - unsafe { BUF_DST.copy_from_slice(&[0; SBG_BUFFER_SIZE]) }; - xfer.block_transfer_interrupt(); - sbg.xfer = Some(xfer); - cx.shared.em.run(|| { - spawn!(sbg_sd, buf_clone)?; // this warning isn't right but it's fine - Ok(()) - }); - } - } - None => { - // it should be impossible to reach here. - info!("None"); - } - } -} - -/// Stored right before an allocation. Stores information that is needed to deallocate memory. -#[derive(Copy, Clone)] -struct AllocInfo { - layout: Layout, - ptr: *mut u8, -} - -/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the -/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, -/// which makes it possible to implement [`free`] without any other data structures. -#[no_mangle] -pub extern "C" fn malloc(size: usize) -> *mut c_void { - if size == 0 { - return ptr::null_mut(); - } - - // Get a layout for both the requested size - let header_layout = Layout::new::(); - let requested_layout = Layout::from_size_align(size, 8).unwrap(); - let (layout, offset) = header_layout.extend(requested_layout).unwrap(); - - // Ask the allocator for memory - let orig_ptr = unsafe { HEAP.alloc(layout) }; - if orig_ptr.is_null() { - return orig_ptr as *mut c_void; - } - - // Compute the pointer that we will return - let result_ptr = unsafe { orig_ptr.add(offset) }; - - // Store the allocation information right before the returned pointer - let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; - unsafe { - info_ptr.write_unaligned(AllocInfo { - layout, - ptr: orig_ptr, - }); - } - - result_ptr as *mut c_void -} - -/// Custom free implementation for the SBG library. This uses the stored allocation information -/// right before the pointer to free up the resources. -/// -/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. -#[no_mangle] -pub unsafe extern "C" fn free(ptr: *mut c_void) { - assert!(!ptr.is_null()); - - let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; - let info = unsafe { info_ptr.read_unaligned() }; - unsafe { - HEAP.dealloc(info.ptr, info.layout); - } -} diff --git a/boards/sensor/src/types.rs b/boards/sensor/src/types.rs deleted file mode 100644 index 7c063a71..00000000 --- a/boards/sensor/src/types.rs +++ /dev/null @@ -1,42 +0,0 @@ -use atsamd_hal as hal; -use atsamd_hal::gpio::*; -use atsamd_hal::sercom::uart::EightBit; -use atsamd_hal::sercom::uart::Uart; -use atsamd_hal::sercom::{spi, uart, IoSet6, Sercom5}; -use hal::dmac; -use hal::dmac::BufferPair; -use hal::sercom::IoSet2; - -use hal::sercom::Sercom4; -use messages::node::Node; -use messages::node::Node::SensorBoard; -use sbg_rs::sbg::SBG_BUFFER_SIZE; - -// ------- -// Node ID -// ------- -pub static COM_ID: Node = SensorBoard; - -// ------- -// SBG -// ------- -pub type PadsSBG = uart::PadsFromIds; -pub type ConfigSBG = uart::Config; -pub type SBGTransfer = dmac::Transfer< - dmac::Channel, - BufferPair, SBGBuffer>, ->; -pub type SBGBuffer = &'static mut [u8; SBG_BUFFER_SIZE]; - -// ------- -// SD Card -// ------- -pub type SdPads = spi::Pads< - Sercom4, - IoSet2, - Pin>, - Pin>, - Pin>, ->; - -pub type SdSpi = spi::Spi, spi::Duplex>; diff --git a/boards/sensor_v2/Cargo.toml b/boards/sensor_v2/Cargo.toml deleted file mode 100644 index 4c0710e9..00000000 --- a/boards/sensor_v2/Cargo.toml +++ /dev/null @@ -1,16 +0,0 @@ -[package] -name = "sensor_v2" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -cortex-m-rtic = "1.1.3" -common-arm-stm32h7 = {path = "../../libraries/common-arm-stm32h7"} -common-arm = {path = "../../libraries/common-arm"} -stm32h7xx-hal = { workspace = true } -postcard = "1.0.2" -messages = { path = "../../libraries/messages" } diff --git a/boards/sensor_v2/src/communication.rs b/boards/sensor_v2/src/communication.rs deleted file mode 100644 index e69de29b..00000000 diff --git a/boards/sensor_v2/src/data_manager.rs b/boards/sensor_v2/src/data_manager.rs deleted file mode 100644 index f3e7d695..00000000 --- a/boards/sensor_v2/src/data_manager.rs +++ /dev/null @@ -1,69 +0,0 @@ -use crate::app::sleep_system; -use common_arm::{spawn, HydraError}; -use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, -}; -use messages::Message; - -#[derive(Clone)] -pub struct DataManager { - pub air: Option, - pub ekf_nav: Option<(EkfNav1, EkfNav2)>, - pub ekf_quat: Option, - pub imu: Option<(Imu1, Imu2)>, - pub utc_time: Option, - pub gps_vel: Option, - pub gps_pos: Option<(GpsPos1, GpsPos2)>, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav: None, - ekf_quat: None, - imu: None, - utc_time: None, - gps_vel: None, - gps_pos: None, - } - } - - pub fn clone_sensors(&self) -> [Option; 10] { - [ - self.air.clone().map(|x| x.into()), - self.ekf_nav.clone().map(|x| x.0.into()), - self.ekf_nav.clone().map(|x| x.1.into()), - self.ekf_quat.clone().map(|x| x.into()), - self.imu.clone().map(|x| x.0.into()), - self.imu.clone().map(|x| x.1.into()), - self.utc_time.clone().map(|x| x.into()), - self.gps_vel.clone().map(|x| x.into()), - self.gps_pos.clone().map(|x| x.0.into()), - self.gps_pos.clone().map(|x| x.1.into()), - ] - } - - pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { - match data.data { - messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - spawn!(sleep_system)?; // need proper error handling. could just expect, but that is mal practice. - } - _ => { - // We don't care atm about these other commands. - } - }, - _ => { - // we can disregard all other messages for now. - } - } - Ok(()) - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} diff --git a/boards/sensor_v2/src/main.rs b/boards/sensor_v2/src/main.rs deleted file mode 100644 index c1e00f58..00000000 --- a/boards/sensor_v2/src/main.rs +++ /dev/null @@ -1,71 +0,0 @@ -#![no_std] -#![no_main] - -mod data_manager; - -use data_manager::DataManager; - -use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; -use stm32h7xx_hal::gpio::Input; -use stm32h7xx_hal::gpio::{Output, PushPull}; -use stm32h7xx_hal::prelude::*; - -/// Custom panic handler. -/// Reset the system if a panic occurs. -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - stm32h7xx_hal::pac::SCB::sys_reset(); -} - -#[rtic::app(device = stm32h7xx_hal::stm32, dispatchers = [EXTI0, EXTI1])] -mod app { - use super::*; - - #[shared] - struct SharedResources { - data_manager: DataManager, - } - #[local] - struct LocalResources { - button: PC13, - led: PC3>, - } - - #[init] - fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { - let pwr = ctx.device.PWR.constrain(); - // We could use smps, but the board is not designed for it - // let pwrcfg = example_power!(pwr).freeze(); - let pwrcfg = pwr.freeze(); - - // RCC - let rcc = ctx.device.RCC.constrain(); - let ccdr = rcc.sys_ck(100.MHz()).freeze(pwrcfg, &ctx.device.SYSCFG); - - // GPIO - let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); - - // Button - let mut button = gpioc.pc13.into_floating_input(); - ( - SharedResources { - data_manager: DataManager::new(), - }, - LocalResources { - button, - led: gpioc.pc3.into_push_pull_output(), - }, - init::Monotonics(), - ) - } - - #[idle] - fn idle(mut ctx: idle::Context) -> ! { - loop {} - } - - #[task(local = [button, led])] - fn sleep_system(mut cx: sleep_system::Context) { - // Turn off the SBG and CAN - } -} diff --git a/boards/sensor_v2/src/sbg_manager.rs b/boards/sensor_v2/src/sbg_manager.rs deleted file mode 100644 index e69de29b..00000000 diff --git a/boards/sensor_v2/src/types.rs b/boards/sensor_v2/src/types.rs deleted file mode 100644 index e69de29b..00000000 diff --git a/dump.txt b/dump.txt new file mode 100644 index 00000000..9931b1b1 --- /dev/null +++ b/dump.txt @@ -0,0 +1,400 @@ +us: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 7950000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 7950000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8225000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8150000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7819969, 8.476829, -4.8654366]), gyroscopes: Some([0.2668274, -0.20375271, 0.2170755]) })), Some(Imu2(Imu2 { temperature: Some(34.880306), delta_velocity: Some([-0.7815182, 8.47428, -4.8654504]), delta_angle: Some([0.24994917, -0.172889, 0.19049446]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 7750000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8425000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.77643555, 8.477079, -4.8792014]), gyroscopes: Some([-0.0000019043451, 0.0004975038, 0.0011869399]) })), Some(Imu2(Imu2 { temperature: Some(34.689964), delta_velocity: Some([-0.77573836, 8.473462, -4.8768487]), delta_angle: Some([-0.00028944333, 0.00046295888, 0.0011444035]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8325000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8030138, status: AirStatus { status: 6 }, pressure_abs: Some(100996.0), altitude: Some(27.411463), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8500000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8600000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7864047, 8.46844, -4.8724856]), gyroscopes: Some([-0.17968997, 0.17372671, -0.1757427]) })), Some(Imu2(Imu2 { temperature: Some(37.269176), delta_velocity: Some([-0.78470856, 8.468112, -4.871467]), delta_angle: Some([-0.22557966, 0.199629, -0.20625234]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 8700000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 8875000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:249:None:4:invalid page information : %hu/%hu +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9150000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 8975000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7728235, 8.471409, -4.8829265]), gyroscopes: Some([-0.0004018101, 0.00058577274, 0.0007932187]) })), Some(Imu2(Imu2 { temperature: Some(34.493572), delta_velocity: Some([-0.77614504, 8.469466, -4.8794026]), delta_angle: Some([-0.0005839597, 0.0003447587, 0.0006327608]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9350000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7754529, 8.46911, -4.8705688]), gyroscopes: Some([0.05026379, -0.04602995, 0.047635462]) })), Some(Imu2(Imu2 { temperature: Some(35.50069), delta_velocity: Some([-0.7782066, 8.469136, -4.8717275]), delta_angle: Some([0.1043067, -0.09052475, 0.09423562]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9250000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9450000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7897834, 8.473315, -4.864551]), gyroscopes: Some([0.0011712027, 0.0018978603, 0.00020830943]) })), Some(Imu2(Imu2 { temperature: Some(36.96805), delta_velocity: Some([-0.78915995, 8.475783, -4.8641205]), delta_angle: Some([0.0011904711, 0.0016827679, 0.00017058027]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 8790144, status: AirStatus { status: 6 }, pressure_abs: Some(100993.0), altitude: Some(27.661749), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO DMA +└─ simple_example::sbg_manager::sbg_dma @ examples/simple/src/sbg_manager.rs:176 +INFO Reading SBG Data +└─ sbg_rs::sbg::{impl#0}::read_data @ libraries/sbg-rs/src/sbg.rs:149 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO /home/ns/Rocketry/hydra/libraries/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c:sbgEComProtocolParseFrame:300:None:4:invalid end-of-frame: byte:%#hhx +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:470 +ERROR SBG Error +└─ sbg_rs::sbg::sbgPlatformDebugLogMsg @ libraries/sbg-rs/src/sbg.rs:474 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 +INFO Data [Some(Air(Air { time_stamp: 9710143, status: AirStatus { status: 6 }, pressure_abs: Some(100994.0), altitude: Some(27.57832), pressure_diff: None, true_airspeed: None, air_temperature: None })), Some(EkfNav1(EkfNav1 { time_stamp: 9525000, velocity: None })), Some(EkfNav2(EkfNav2 { position: None, undulation: None })), Some(EkfQuat(EkfQuat { time_stamp: 9075000, quaternion: None, euler_std_dev: None, status: EkfStatus { status: 258 } })), Some(Imu1(Imu1 { time_stamp: 9625000, status: ImuStatus { status: 799 }, accelerometers: Some([-0.7815678, 8.472983, -4.879673]), gyroscopes: Some([0.00068770646, 0.00042975583, 0.00083899405]) })), Some(Imu2(Imu2 { temperature: Some(35.12805), delta_velocity: Some([-0.7833217, 8.470738, -4.8809614]), delta_angle: Some([0.0005976452, 0.00045764886, 0.0010904415]) })), None, None, None, None, None] +└─ simple_example::app::idle @ examples/simple/src/main.rs:120 \ No newline at end of file diff --git a/examples/rtic/src/main.rs b/examples/rtic/src/main.rs index 3419586f..5fb6ece8 100644 --- a/examples/rtic/src/main.rs +++ b/examples/rtic/src/main.rs @@ -46,9 +46,11 @@ mod app { let led = pins.pa14.into_push_pull_output(); // Tell the MCU to sleep deeply for maximum power savings - core.SCB.set_sleepdeep(); + // core.SCB.set_sleepdeep(); + core.SCB.clear_sleepdeep(); // Spawn the LED blink task right after init + info!("Spawned blink task"); blink::spawn().ok(); // Use the system's Systick for RTIC to keep track of the time diff --git a/examples/simple/Cargo.toml b/examples/simple/Cargo.toml index 7d3b1ca4..5c413866 100644 --- a/examples/simple/Cargo.toml +++ b/examples/simple/Cargo.toml @@ -8,8 +8,16 @@ edition = "2021" [dependencies] cortex-m = { version = "^0.7.0", features = ["critical-section-single-core"] } cortex-m-rt = "^0.7.0" -panic-halt = "0.2.0" +cortex-m-rtic = "1.1.3" + defmt-rtt = "0.4.0" defmt = "0.3.2" -atsamd-hal = { workspace = true } -common-arm-atsame = { path = "../../libraries/common-arm-atsame" } +stm32h7xx-hal = { workspace = true } +common-arm-stm32h7 = { path = "../../libraries/common-arm-stm32h7" } +panic-probe = { version = "0.3", features = ["print-defmt"] } +chrono = {version = "0.4.0", default-features = false} +sbg-rs = {path = "../../libraries/sbg-rs"} +messages = { path = "../../libraries/messages" } +heapless = "0.7.16" +embedded-alloc = "0.5.0" +common-arm = { path = "../../libraries/common-arm" } diff --git a/boards/sensor/src/data_manager.rs b/examples/simple/src/data_manager.rs similarity index 83% rename from boards/sensor/src/data_manager.rs rename to examples/simple/src/data_manager.rs index 5691b5cd..29868be6 100644 --- a/boards/sensor/src/data_manager.rs +++ b/examples/simple/src/data_manager.rs @@ -1,5 +1,4 @@ -use crate::app::sleep_system; -use common_arm::{spawn, HydraError}; +use common_arm::HydraError; use messages::sensor::{ Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, Imu1, Imu2, SensorData, UtcTime, @@ -30,12 +29,12 @@ impl DataManager { } } - pub fn clone_sensors(&self) -> [Option; 13] { + // TODO: stop cloning so much this is a waste of resources. + pub fn clone_sensors(&self) -> [Option; 11] { [ self.air.clone().map(|x| x.into()), self.ekf_nav.clone().map(|x| x.0.into()), self.ekf_nav.clone().map(|x| x.1.into()), - self.ekf_nav.clone().map(|x| x.2.into()), self.ekf_quat.clone().map(|x| x.into()), self.imu.clone().map(|x| x.0.into()), self.imu.clone().map(|x| x.1.into()), @@ -44,16 +43,13 @@ impl DataManager { self.gps_vel.clone().map(|x| x.1.into()), self.gps_pos.clone().map(|x| x.0.into()), self.gps_pos.clone().map(|x| x.1.into()), - self.gps_pos.clone().map(|x| x.2.into()), ] } pub fn handle_data(&mut self, data: Message) -> Result<(), HydraError> { match data.data { messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { - spawn!(sleep_system)?; // need proper error handling. could just expect, but that is mal practice. - } + messages::command::CommandData::PowerDown(_) => {} _ => { // We don't care atm about these other commands. } diff --git a/examples/simple/src/main.rs b/examples/simple/src/main.rs index 1ef5e4c4..bdabab4c 100644 --- a/examples/simple/src/main.rs +++ b/examples/simple/src/main.rs @@ -1,47 +1,144 @@ -#![no_std] #![no_main] +#![no_std] + +use panic_probe as _; -use atsamd_hal as hal; -use atsamd_hal::prelude::*; +use defmt::Format; +use defmt_rtt as _; // global logger + +#[inline(never)] +#[defmt::panic_handler] +fn panic() -> ! { + cortex_m::asm::udf() +} + +use core::cell::RefCell; +mod data_manager; +use stm32h7xx_hal::gpio::{Pin, Alternate, PA3}; +use heapless::Vec; + +use data_manager::DataManager; + +use chrono::prelude::*; +use cortex_m::{asm, interrupt::Mutex}; use cortex_m_rt::entry; use defmt::info; -use defmt_rtt as _; -use hal::gpio::Pins; -use hal::pac; -use pac::Peripherals; -use panic_halt as _; - -#[entry] -fn main() -> ! { - let mut peripherals = Peripherals::take().unwrap(); - let p2 = cortex_m::peripheral::Peripherals::take().unwrap(); - - let pins = Pins::new(peripherals.PORT); - let mut led = pins.pa14.into_push_pull_output(); - - // External 32KHz clock for stability - let mut clock = hal::clock::GenericClockController::with_external_32kosc( - peripherals.GCLK, - &mut peripherals.MCLK, - &mut peripherals.OSC32KCTRL, - &mut peripherals.OSCCTRL, - &mut peripherals.NVMCTRL, - ); - - clock.configure_gclk_divider_and_source( - pac::gclk::pchctrl::GENSELECT_A::GCLK2, - 1, - pac::gclk::genctrl::SRCSELECT_A::DFLL, - false, - ); - - let mut delay = hal::delay::Delay::new(p2.SYST, &mut clock); - - loop { - led.set_high().unwrap(); - delay.delay_ms(1000_u16); - info!("Test"); - led.set_low().unwrap(); - delay.delay_ms(1000_u16); + +use pac::interrupt; +use stm32h7xx_hal::{pac, prelude::*, rtc}; +mod sbg_manager; +use sbg_manager::SBGManager; +use sbg_manager::{sbg_dma, sbg_flush, sbg_handle_data, sbg_write_data}; +use sbg_rs::sbg::CallbackData; +use sbg_rs::sbg::SBG_BUFFER_SIZE; +use stm32h7xx_hal::dma::dma::StreamsTuple; +use common_arm::ErrorManager; +static RTC: Mutex>> = Mutex::new(RefCell::new(None)); + +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI2])] +mod app { + use stm32h7xx_hal::gpio::gpioc::{PC13, PC3}; + use stm32h7xx_hal::gpio::{Edge, ExtiPin, Input, PA2}; + use stm32h7xx_hal::gpio::{Output, PushPull}; + use stm32h7xx_hal::prelude::*; + + use super::*; + + #[shared] + struct SharedResources { + sbg_manager: SBGManager, + em: ErrorManager, + data_manager: DataManager, + } + #[local] + struct LocalResources { + button: PC13, + led: PC3>, + green_led: PA3>, + } + + #[init] + fn init(mut ctx: init::Context) -> (SharedResources, LocalResources, init::Monotonics) { + let pwr = ctx.device.PWR.constrain(); + let pwrcfg = pwr.freeze(); + + // RCC + let rcc = ctx.device.RCC.constrain(); + let ccdr = rcc.sys_ck(100.MHz()).freeze(pwrcfg, &ctx.device.SYSCFG); + + // GPIO + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + + // Button + let mut button = gpioc.pc13.into_floating_input(); + button.make_interrupt_source(&mut ctx.device.SYSCFG); + button.trigger_on_edge(&mut ctx.device.EXTI, Edge::Rising); + button.enable_interrupt(&mut ctx.device.EXTI); + + // GPIO + let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); + let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); + let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); + // leds + let led_red = gpioa.pa2.into_push_pull_output(); + let green_led = gpioa.pa3.into_push_pull_output(); + + // sbg power pin + let mut sbg_power = gpiob.pb4.into_push_pull_output(); + sbg_power.set_high(); + + // UART for sbg + let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); + let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); + + let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + let uart_sbg = ctx + .device + .UART4 + .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + + ( + SharedResources { + sbg_manager, + em: ErrorManager::new(), + data_manager: DataManager::new(), + }, + LocalResources { + button, + led: gpioc.pc3.into_push_pull_output(), + green_led, + }, + init::Monotonics(), + ) + } + + #[idle(shared = [data_manager])] + fn idle(mut cx: idle::Context) -> ! { + loop { + info!("Data {}", cx.shared.data_manager.lock(|data| data.clone_sensors())) + } + } + + #[task(binds = EXTI15_10, local = [button, led])] + fn button_click(ctx: button_click::Context) { + ctx.local.button.clear_interrupt_pending_bit(); + ctx.local.led.toggle(); + } + + extern "Rust" { + + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + fn sbg_dma(mut context: sbg_dma::Context); + + #[task(priority = 1, shared = [data_manager])] + fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + + #[task(priority = 1, shared = [&em, sbg_manager])] + fn sbg_flush(context: sbg_flush::Context); + + #[task(priority = 1, shared = [&em, sbg_manager])] + fn sbg_write_data(context: sbg_write_data::Context, data: Vec); } } diff --git a/examples/simple/src/sbg_manager.rs b/examples/simple/src/sbg_manager.rs new file mode 100644 index 00000000..bdfeecad --- /dev/null +++ b/examples/simple/src/sbg_manager.rs @@ -0,0 +1,252 @@ +use core::alloc::{GlobalAlloc, Layout}; +use core::ffi::c_void; +use core::mem::size_of; +use core::ptr; +// use atsamd_hal::time::*; +use crate::app::sbg_flush; +use crate::app::sbg_handle_data; +// use crate::app::sbg_sd_task as sbg_sd; +use crate::app::sbg_write_data; +use crate::RTC; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use core::mem::MaybeUninit; +use defmt::{info, panic}; +use embedded_alloc::Heap; +use heapless::Vec; +use messages::mavlink::embedded::{Read, Write}; +use sbg_rs::sbg; +use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamX; +use stm32h7xx_hal::dma::{ + dma::{DmaConfig, StreamsTuple}, + PeripheralToMemory, Transfer, +}; +use stm32h7xx_hal::pac::UART4; +use stm32h7xx_hal::serial::{Rx, Tx}; +// use cortex_m::{asm}; +use rtic::Mutex; + +//#[link_section = ".axisram.buffers"] +//static mut SBG_BUFFER: MayberUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +#[link_section = ".axisram.buffers"] +pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +// Simple heap required by the SBG library +static HEAP: Heap = Heap::empty(); + +pub struct SBGManager { + sbg_device: SBG, + xfer: Option< + Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + &'static mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + >, + >, + sbg_tx: Tx, +} + +impl SBGManager { + pub fn new( + mut serial: stm32h7xx_hal::serial::Serial, + stream_tuple: StreamsTuple, + //mut dma_channel: dmac::Channel, + ) -> Self { + /* Initialize the Heap */ + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + // TODO: Could add a link section here to memory. + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + + let (sbg_tx, mut sbg_rx) = serial.split(); + + // TODO: This could be wrong. It's a bit of a guess. + // let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; + unsafe { + // Convert an uninitialised array into an array of uninitialised + let buf: &mut [core::mem::MaybeUninit; SBG_BUFFER_SIZE] = + &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _); + buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); + } + + let config = DmaConfig::default().memory_increment(true).transfer_complete_interrupt(true); + let mut transfer: Transfer< + StreamX, + Rx, + PeripheralToMemory, + &mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + > = Transfer::init( + stream_tuple.1, + sbg_rx, + unsafe { SBG_BUFFER.assume_init_mut() }, // Uninitialised memory + None, + config, + ); + + + + info!("Starting transfer"); + transfer.start(|serial| { + serial.enable_dma_rx(); + + }); + info!("Transfer started"); + + while !transfer.get_transfer_complete_flag() { + // info!("Transfer not complete"); + } + + let mut sbg: sbg::SBG = sbg::SBG::new( + |data| { + sbg_handle_data::spawn(data).ok(); + }, + |data| { + sbg_write_data::spawn(data).ok(); + }, + || sbg_get_time(), + || { + sbg_flush::spawn().ok(); + }, + ); + sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + SBGManager { + sbg_device: sbg, + xfer: Some(transfer), + sbg_tx, + } + } +} + +pub fn sbg_flush(cx: sbg_flush::Context<'_>) { + // cx.shared.sbg_manager.lock(|sbg| { + // sbg.sbg_tx + // }); +} +pub fn sbg_write_data(mut cx: sbg_write_data::Context<'_>, data: Vec) { + cx.shared.sbg_manager.lock(|sbg| { + sbg.sbg_tx.write_all(data.as_slice()); + }); +} + +pub fn sbg_get_time() -> u32 { + cortex_m::interrupt::free(|cs| { + let mut rc = RTC.borrow(cs).borrow_mut(); + let rtc = rc.as_mut().unwrap(); + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }) +} + +pub fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { + CallbackData::UtcTime(x) => manager.utc_time = Some(x), + CallbackData::Air(x) => manager.air = Some(x), + CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), + CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), + CallbackData::Imu(x) => manager.imu = Some(x), + CallbackData::GpsVel(x) => manager.gps_vel = Some(x), + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + }); +} + +/** + * Handles the DMA interrupt. + * Handles the SBG data. + */ +pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + info!("DMA"); + cx.shared.sbg_manager.lock(|sbg| { + match &mut sbg.xfer { + Some(xfer) => { + if xfer.get_transfer_complete_flag() { + let data = unsafe { SBG_BUFFER.assume_init_read() }; + xfer.clear_transfer_complete_interrupt(); + xfer.next_transfer( + unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory + ); + sbg.sbg_device.read_data(&data); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } + } + }); +} + +/// Stored right before an allocation. Stores information that is needed to deallocate memory. +#[derive(Copy, Clone)] +struct AllocInfo { + layout: Layout, + ptr: *mut u8, +} + +/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the +/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, +/// which makes it possible to implement [`free`] without any other data structures. +#[no_mangle] +pub extern "C" fn malloc(size: usize) -> *mut c_void { + if size == 0 { + return ptr::null_mut(); + } + + // Get a layout for both the requested size + let header_layout = Layout::new::(); + let requested_layout = Layout::from_size_align(size, 8).unwrap(); + let (layout, offset) = header_layout.extend(requested_layout).unwrap(); + + // Ask the allocator for memory + let orig_ptr = unsafe { HEAP.alloc(layout) }; + if orig_ptr.is_null() { + return orig_ptr as *mut c_void; + } + + // Compute the pointer that we will return + let result_ptr = unsafe { orig_ptr.add(offset) }; + + // Store the allocation information right before the returned pointer + let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; + unsafe { + info_ptr.write_unaligned(AllocInfo { + layout, + ptr: orig_ptr, + }); + } + + result_ptr as *mut c_void +} + +/// Custom free implementation for the SBG library. This uses the stored allocation information +/// right before the pointer to free up the resources. +/// +/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. +#[no_mangle] +pub unsafe extern "C" fn free(ptr: *mut c_void) { + assert!(!ptr.is_null()); + + let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; + let info = unsafe { info_ptr.read_unaligned() }; + unsafe { + HEAP.dealloc(info.ptr, info.layout); + } +} diff --git a/libraries/common-arm-atsame/Cargo.toml b/libraries/common-arm-atsame/Cargo.toml index eb199ecf..1b98028f 100644 --- a/libraries/common-arm-atsame/Cargo.toml +++ b/libraries/common-arm-atsame/Cargo.toml @@ -8,4 +8,6 @@ edition = "2021" [dependencies] common-arm = { path = "../common-arm" } atsamd-hal = { workspace = true } -embedded-hal = {workspace = true } \ No newline at end of file +embedded-hal = { workspace = true } +mcan = "0.3" + diff --git a/libraries/common-arm-atsame/memory.x b/libraries/common-arm-atsame/memory.x index 690b7767..fd693582 100644 --- a/libraries/common-arm-atsame/memory.x +++ b/libraries/common-arm-atsame/memory.x @@ -2,7 +2,8 @@ MEMORY { /* NOTE K = KiBi = 1024 bytes */ FLASH : ORIGIN = 0x00000000, LENGTH = 256K - CAN : ORIGIN = 0x20000000, LENGTH = 64K + CAN : ORIGIN = 0x20000000, LENGTH = 32K + CAN_COMMAND : ORIGIN = 0x20008000, LENGTH = 32K RAM : ORIGIN = 0x20010000, LENGTH = 64K } SECTIONS @@ -11,5 +12,9 @@ SECTIONS { *(.can .can.*); } > CAN + .can_command (NOLOAD) : + { + *(.can_command .can_command.*); + } > CAN_COMMAND } /* _stack_start is optional and we can define this later */ \ No newline at end of file diff --git a/libraries/common-arm-atsame/src/lib.rs b/libraries/common-arm-atsame/src/lib.rs index 701862fe..b2649251 100644 --- a/libraries/common-arm-atsame/src/lib.rs +++ b/libraries/common-arm-atsame/src/lib.rs @@ -4,3 +4,5 @@ //! This crate contains common code for HYDRA. Any code that is not board specific but is ATSAME specific should be put in //! here. //! + +pub use mcan; diff --git a/libraries/common-arm-stm32h7/memory.x b/libraries/common-arm-stm32h7/memory.x index eb063383..428106a9 100644 --- a/libraries/common-arm-stm32h7/memory.x +++ b/libraries/common-arm-stm32h7/memory.x @@ -22,12 +22,11 @@ MEMORY RAM : ORIGIN = 0x20000000, LENGTH = 128K /* AXISRAM */ - AXISRAM : ORIGIN = 0x24000000, LENGTH = 512K + AXISRAM : ORIGIN = 0x24000000, LENGTH = 128K /* SRAM */ - SRAM1 : ORIGIN = 0x30000000, LENGTH = 128K - SRAM2 : ORIGIN = 0x30020000, LENGTH = 128K - SRAM3 : ORIGIN = 0x30040000, LENGTH = 32K + SRAM1 : ORIGIN = 0x30000000, LENGTH = 64K + SRAM2 : ORIGIN = 0x30004000, LENGTH = 64K SRAM4 : ORIGIN = 0x38000000, LENGTH = 64K /* Backup SRAM */ @@ -53,10 +52,14 @@ SECTIONS { } > AXISRAM /* The SRAM1 and SRAM2 section are commonly used as the stack and heap for the CM4 core in dualcore versions and should thus not be used in examples*/ - .sram3 (NOLOAD) : ALIGN(4) { - *(.sram3 .sram3.*); + .sram1 (NOLOAD) : ALIGN(4) { + *(.sram1 .sram1.*); . = ALIGN(4); - } > SRAM3 + } > SRAM1 + .sram2 (NOLOAD) : ALIGN(4) { + *(.sram2 .sram2.*); + . = ALIGN(4); + } > SRAM2 .sram4 (NOLOAD) : ALIGN(4) { *(.sram4 .sram4.*); . = ALIGN(4); diff --git a/libraries/common-arm/Cargo.toml b/libraries/common-arm/Cargo.toml index a44ef2f6..f56a825a 100644 --- a/libraries/common-arm/Cargo.toml +++ b/libraries/common-arm/Cargo.toml @@ -17,6 +17,7 @@ derive_more = "0.99.17" cortex-m-rt = "^0.7.0" embedded-sdmmc = "0.3.0" embedded-hal = "0.2.7" -mcan = "0.3" nb = "1.1.0" +mcan = "0.3.0" messages = { path = "../messages" } +ms5611-01ba = { git = "https://github.com/NoahSprenger/ms5611-01ba", branch = "embedded-hal-02" } diff --git a/libraries/common-arm/src/error/error_manager.rs b/libraries/common-arm/src/error/error_manager.rs index e5a03931..a462c028 100644 --- a/libraries/common-arm/src/error/error_manager.rs +++ b/libraries/common-arm/src/error/error_manager.rs @@ -42,8 +42,6 @@ impl ErrorManager { if let Err(e) = result { self.has_error.store(true, Relaxed); - error!("{}", e); - if let Some(c) = e.get_context() { error!("{}", e); herror!(Error, c); diff --git a/libraries/common-arm/src/error/hydra_error.rs b/libraries/common-arm/src/error/hydra_error.rs index a6bcbd2f..d085cc22 100644 --- a/libraries/common-arm/src/error/hydra_error.rs +++ b/libraries/common-arm/src/error/hydra_error.rs @@ -4,7 +4,8 @@ use defmt::{write, Format}; use derive_more::From; use embedded_sdmmc as sd; use messages::ErrorContext; - +use ms5611_01ba::error::DeviceError; +use nb::Error as NbError; /// Open up atsamd hal errors without including the whole crate. /// Contains all the various error types that can be encountered in the Hydra codebase. Extra errors @@ -28,6 +29,9 @@ pub enum HydraErrorType { CanError(nb::Error), /// CAN message build error. CanMessageError(mcan::message::TooMuchData), + /// Error from the MS5611 barometer library. + BarometerError(DeviceError), + NbError(NbError), } impl defmt::Format for HydraErrorType { @@ -51,6 +55,9 @@ impl defmt::Format for HydraErrorType { HydraErrorType::MavlinkReadError(_) => { write!(f, "Mavlink read error!"); } + HydraErrorType::BarometerError(_) => { + write!(f, "Barometer error!"); + } // HydraErrorType::DmaError(_) => { // write!(f, "DMA error!"); // } @@ -60,6 +67,9 @@ impl defmt::Format for HydraErrorType { HydraErrorType::CanMessageError(_) => { write!(f, "CAN message error!"); } + HydraErrorType::NbError(_) => { + write!(f, "Nb error!"); + } } } } diff --git a/libraries/common-arm/src/health/health_manager.rs b/libraries/common-arm/src/health/health_manager.rs deleted file mode 100644 index 7b059ae4..00000000 --- a/libraries/common-arm/src/health/health_manager.rs +++ /dev/null @@ -1,77 +0,0 @@ -use crate::health::health_monitor::{HealthMonitor, HealthMonitorChannels}; -use core::ops::RangeInclusive; -use defmt::warn; -use messages::health::HealthState; - -/// Manages and reports the health of the system. -/// I think OutputPin trait is the right one that should be good for atsame and stm32. -pub struct HealthManager { - pub monitor: HealthMonitor, -} - -impl HealthManager -where - T: HealthMonitorChannels, -{ - /// Create a new health manager. - /// Divider is the ohmage of the resistor and resolution is the resolution of the ADC. - pub fn new(monitor: HealthMonitor) -> Self { - Self { monitor } - } - /// !! Values are not correct !! - /// They need to be updated with the proper dividers - /// Evaluate the health of the system. - /// All values must be nominal for the system to be in a nominal state. - /// could be nice to send out a log message if something fails, tell the system what failed. - pub fn evaluate(&mut self) -> HealthState { - self.monitor.update(); // We need new values before we can evaluate the health status. - let data = self.monitor.data.clone(); - // This is ugly... - let v5_status = get_status(data.v5, &self.monitor.range_5v); - let v3_3_status = get_status(data.v3_3, &self.monitor.range_3v3); - let ext_3v3 = get_status(data.ext_3v3, &self.monitor.range_ext_3v3); - let ext_v5_status = get_status(data.ext_v5, &self.monitor.range_ext_5v); - let int_v3_3_status = get_status(data.int_v3_3, &self.monitor.range_int_3v3); - let int_5v_status = get_status(data.int_v5, &self.monitor.range_int_5v); - let pyro_status = get_status(data.pyro_sense, &self.monitor.range_pyro); - let vcc_status = get_status(data.vcc_sense, &self.monitor.range_vcc); - let failover_status = get_status(data.failover_sense, &self.monitor.range_failover); - - for status in [ - v5_status, - v3_3_status, - ext_3v3, - ext_v5_status, - int_v3_3_status, - int_5v_status, - pyro_status, - vcc_status, - failover_status, - ] { - match status { - HealthState::Error => return HealthState::Error, - HealthState::Warning => return HealthState::Warning, - _ => continue, - } - } - - HealthState::Nominal - } -} - -fn get_status(data: Option, nominal: &RangeInclusive) -> HealthState { - match data { - Some(x) => { - if nominal.contains(&x) { - return HealthState::Nominal; - } else { - // warn!("Unsafe Voltage"); - HealthState::Error - } - } - None => { - warn!("No data"); - return HealthState::Warning; - } - } -} diff --git a/libraries/common-arm/src/health/health_monitor.rs b/libraries/common-arm/src/health/health_monitor.rs deleted file mode 100644 index 29d61afe..00000000 --- a/libraries/common-arm/src/health/health_monitor.rs +++ /dev/null @@ -1,110 +0,0 @@ -use core::ops::RangeInclusive; -/// Health monitor module -/// This may not actually be feasible. -/// So, we have a health monitor module that is responsible for monitoring the health of the system. -/// But if we want this to be generic to the point where we can use it on any board, then we need to -/// ensure that the ADC channels will accept the pins that we want to use. -use messages::health::HealthStatus; - -/// Add a note about what each means and what the range is. -pub trait HealthMonitorChannels { - fn get_5v(&mut self) -> Option; - fn get_3v3(&mut self) -> Option; - fn get_pyro(&mut self) -> Option; - fn get_vcc(&mut self) -> Option; - fn get_int_5v(&mut self) -> Option; - fn get_int_3v3(&mut self) -> Option; - fn get_ext_5v(&mut self) -> Option; - fn get_ext_3v3(&mut self) -> Option; - fn get_failover(&mut self) -> Option; -} - -pub struct HealthMonitor { - // we will store a set of readings here that can be propagated up to the manager - // we also - channels: T, - pub data: HealthStatus, - pub range_5v: RangeInclusive, - pub range_3v3: RangeInclusive, - pub range_pyro: RangeInclusive, - pub range_vcc: RangeInclusive, - pub range_int_5v: RangeInclusive, - pub range_int_3v3: RangeInclusive, - pub range_ext_5v: RangeInclusive, - pub range_ext_3v3: RangeInclusive, - pub range_failover: RangeInclusive, -} - -impl HealthMonitor -where - T: HealthMonitorChannels, -{ - pub fn new(channels: T, divider1: u16, divider2: u16, resolution: u16) -> Self { - let range_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - let range_pyro = ((resolution as f32 / calculate_voltage(divider1, divider2, 8.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 9.1)) as u16); - let range_vcc = ((resolution as f32 / calculate_voltage(divider1, divider2, 11.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 12.1)) as u16); - let range_int_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_int_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - let range_ext_5v = ((resolution as f32 / calculate_voltage(divider1, divider2, 4.9)) as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 5.1)) as u16); - let range_ext_3v3 = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - // I'm not certain that failover is actually 3.3v - let range_failover = ((resolution as f32 / calculate_voltage(divider1, divider2, 3.2)) - as u16) - ..=((resolution as f32 / calculate_voltage(divider1, divider2, 3.4)) as u16); - Self { - channels, - data: HealthStatus { - v5: None, - v3_3: None, - pyro_sense: None, - vcc_sense: None, - int_v5: None, - int_v3_3: None, - ext_v5: None, - ext_3v3: None, - failover_sense: None, - }, - range_5v, - range_3v3, - range_pyro, - range_vcc, - range_int_5v, - range_int_3v3, - range_ext_5v, - range_ext_3v3, - range_failover, - } - } - - /// Should this really be unwrap? - /// Or should we return this to the manager and let it decide what to do with it? - /// For now we will just unwrap it. - /// Also update will be called by the manager. - /// This could be intensive if we are reading all of the channels at once. - pub fn update(&mut self) { - self.data.v5 = self.channels.get_5v(); - self.data.v3_3 = self.channels.get_3v3(); - self.data.pyro_sense = self.channels.get_pyro(); - self.data.vcc_sense = self.channels.get_vcc(); - self.data.int_v5 = self.channels.get_int_5v(); - self.data.int_v3_3 = self.channels.get_int_3v3(); - self.data.ext_v5 = self.channels.get_ext_5v(); - self.data.ext_3v3 = self.channels.get_ext_3v3(); - self.data.failover_sense = self.channels.get_failover(); - } -} - -fn calculate_voltage(r1: u16, r2: u16, v_source: f32) -> f32 { - v_source * (r2 as f32 / (r1 as f32 + r2 as f32)) -} diff --git a/libraries/common-arm/src/health/mod.rs b/libraries/common-arm/src/health/mod.rs deleted file mode 100644 index e5de5966..00000000 --- a/libraries/common-arm/src/health/mod.rs +++ /dev/null @@ -1,2 +0,0 @@ -pub mod health_manager; -pub mod health_monitor; diff --git a/libraries/common-arm/src/lib.rs b/libraries/common-arm/src/lib.rs index 83adaaf1..643477b7 100644 --- a/libraries/common-arm/src/lib.rs +++ b/libraries/common-arm/src/lib.rs @@ -6,17 +6,12 @@ //! here. //! -pub use mcan; - mod error; -mod health; mod logging; mod sd_manager; pub use crate::error::error_manager::ErrorManager; pub use crate::error::hydra_error::{ErrorContextTrait, HydraError, SpawnError}; -pub use crate::health::health_manager::HealthManager; -pub use crate::health::health_monitor::{HealthMonitor, HealthMonitorChannels}; pub use crate::logging::HydraLogging; pub use crate::sd_manager::SdManager; diff --git a/libraries/common-arm/src/sd_manager.rs b/libraries/common-arm/src/sd_manager.rs index d4abe100..9e087b7e 100644 --- a/libraries/common-arm/src/sd_manager.rs +++ b/libraries/common-arm/src/sd_manager.rs @@ -1,5 +1,6 @@ use core::{fmt::Debug, marker::PhantomData}; use defmt::info; +use defmt::panic; use embedded_hal as hal; use embedded_sdmmc as sd; use hal::spi::FullDuplex; @@ -52,6 +53,7 @@ where { pub fn new(spi: SPI, cs: CS) -> Self { let time_sink: TimeSink = TimeSink::new(); // Need to give this a DateTime object for actual timing. + info!("Initializing SD card"); let mut sd_cont = sd::Controller::new(sd::SdMmcSpi::new(spi, cs), time_sink); match sd_cont.device().init() { Ok(_) => match sd_cont.device().card_size_bytes() { @@ -79,7 +81,7 @@ where let file = sd_cont.open_file_in_dir( &mut volume, &root_directory, - "log.txt", + "lc24.txt", sd::Mode::ReadWriteCreateOrTruncate, ); let file = match file { diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml index e6f02764..684730a4 100644 --- a/libraries/messages/Cargo.toml +++ b/libraries/messages/Cargo.toml @@ -9,7 +9,10 @@ serde = { workspace = true } defmt = "0.3.2" fugit = "0.3.6" heapless = "0.7.16" -mavlink = { git = "https://github.com/uorocketry/rust-mavlink", default-features = false } +ts-rs = { version = "6.2.1", optional = true } +mavlink = { git = "https://github.com/uorocketry/rust-mavlink.git", features = [ + "uorocketry", +], default-features = false } bitflags = { version = "2.3.1", features = ["serde"] } proptest = { version = "1.2.0", optional = true } proptest-derive = { version = "0.3.0", optional = true } @@ -21,5 +24,6 @@ proptest-derive = "0.3.0" postcard = { version = "1.0.4", features = ["alloc"] } [features] -default = ["mavlink/embedded", "mavlink/uorocketry"] -std = ["mavlink/default", "dep:proptest", "dep:proptest-derive"] +default = ["mavlink/embedded-hal-02", "mavlink/uorocketry"] +std = ["mavlink/std", "mavlink/tcp", "mavlink/udp", "mavlink/direct-serial", "mavlink/serde", "dep:proptest", "dep:proptest-derive"] +ts = ["std", "dep:ts-rs"] diff --git a/libraries/messages/src/command.rs b/libraries/messages/src/command.rs index 8e2f0b40..d321fbae 100644 --- a/libraries/messages/src/command.rs +++ b/libraries/messages/src/command.rs @@ -7,6 +7,7 @@ pub struct Command { pub data: CommandData, } + #[common_derives] #[derive(From)] pub enum CommandData { @@ -14,6 +15,14 @@ pub enum CommandData { DeployMain(DeployMain), PowerDown(PowerDown), RadioRateChange(RadioRateChange), + Online(Online), +} + + +// not really a command but this decreases the complexity of the change on ground station. +#[common_derives] +pub struct Online { + pub online: bool, } #[common_derives] diff --git a/libraries/messages/src/health.rs b/libraries/messages/src/health.rs deleted file mode 100644 index 3ff228ef..00000000 --- a/libraries/messages/src/health.rs +++ /dev/null @@ -1,53 +0,0 @@ -use derive_more::From; -use messages_proc_macros_lib::common_derives; - -#[common_derives] -#[derive(From)] -pub struct Health { - pub data: HealthData, - pub status: HealthState, -} - -/// The health status of a component. -/// Nomial: everything is fine -/// Warning: something is wrong, but it's not critical -/// Error: something is wrong, and it's critical -/// We need some way to quantify this concept of good and bad health. -/// Could be current range or voltage range. If failover happens on the regulators that is warning. -#[common_derives] -#[derive(From)] -pub enum HealthState { - Nominal, - Warning, - Error, -} - -#[common_derives] -#[derive(From)] -pub enum HealthData { - // RegulatorStatus(RegulatorStatus), - HealthStatus(HealthStatus), -} - -#[common_derives] -#[derive(From)] -pub struct HealthStatus { - pub v5: Option, - pub v3_3: Option, - pub pyro_sense: Option, - pub vcc_sense: Option, - pub int_v5: Option, - pub int_v3_3: Option, - pub ext_v5: Option, - pub ext_3v3: Option, - pub failover_sense: Option, -} - -impl Health { - pub fn new(data: impl Into, status: impl Into) -> Self { - Health { - data: data.into(), - status: status.into(), - } - } -} diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index 9541c85c..c4a90ad5 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -6,18 +6,15 @@ //! and ground-station communication. use crate::command::Command; -use crate::health::Health; -use crate::node::Node; +use crate::sender::Sender; use crate::sensor::Sensor; use crate::state::State; use derive_more::From; -use fugit::Instant; /// This is to help control versions. pub use mavlink; use messages_proc_macros_lib::common_derives; pub mod command; -pub mod health; mod logging; pub mod node; pub mod sensor; @@ -39,7 +36,7 @@ pub use logging::{ErrorContext, Event, Log, LogLevel}; pub struct Message { /// Time in milliseconds since epoch. Note that the epoch here can be arbitrary and is not the /// Unix epoch. - pub timestamp: u64, + pub timestamp: u32, /// The original sender of this message. pub sender: Node, @@ -56,17 +53,12 @@ pub enum Data { Sensor(Sensor), Log(Log), Command(Command), - Health(Health), } impl Message { - pub fn new( - timestamp: &Instant, - sender: Node, - data: impl Into, - ) -> Self { + pub fn new(timestamp: u32, sender: Sender, data: impl Into) -> Self { Message { - timestamp: timestamp.duration_since_epoch().to_millis(), + timestamp: timestamp, sender, data: data.into(), } diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index 580bb1c3..3fc844f1 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -22,40 +22,56 @@ pub enum SensorData { EkfNavAcc(EkfNavAcc), Imu1(Imu1), Imu2(Imu2), + NavPosLlh(NavPosLlh), GpsVel(GpsVel), GpsVelAcc(GpsVelAcc), GpsPos1(GpsPos1), GpsPos2(GpsPos2), GpsPosAcc(GpsPosAcc), -} - -/* Replace with new health monitor */ - -#[common_derives] -pub struct Regulator { - pub status: bool, + ResetReason(ResetReason), + RecoverySensing(RecoverySensing), } #[common_derives] -pub struct Voltage { - pub voltage: f32, - pub rolling_avg: f32, +pub struct NavPosLlh { + pub height_msl: f64, + pub longitude: f64, + pub latitude: f64, } -#[common_derives] -pub struct Current { - pub current: f32, - pub rolling_avg: f32, -} +/* Replace with new health monitor */ #[common_derives] -pub struct Temperature { - pub temperature: f32, - pub rolling_avg: f32, +pub enum ResetReason { + /// The mcu went from not having power to having power and resetting + PowerOnReset, + /// The reset pin was asserted + PinReset, + /// The brownout detector triggered + BrownoutReset, + /// The software did a soft reset through the SCB peripheral + SystemReset, + /// The software did a soft reset through the RCC periperal + CpuReset, + /// The window watchdog triggered + WindowWatchdogReset, + /// The independent watchdog triggered + IndependentWatchdogReset, + /// Either of the two watchdogs triggered (but we don't know which one) + GenericWatchdogReset, + /// The DStandby mode was exited + D1ExitsDStandbyMode, + /// The DStandby mode was exited + D2ExitsDStandbyMode, + /// A state has been entered erroneously + D1EntersDStandbyErroneouslyOrCpuEntersCStopErroneously, + /// The reason could not be determined + Unknown { + /// The raw register value + rcc_rsr: u32, + }, } -/* Replace with new health monitor */ - #[common_derives] pub struct GpsPos1 { #[doc = "< Latitude in degrees, positive north."] @@ -64,6 +80,8 @@ pub struct GpsPos1 { pub longitude: Option, } + + #[common_derives] pub struct GpsPos2 { #[doc = "< GPS time of week in ms."] @@ -218,6 +236,14 @@ pub struct GpsVelAcc { pub velocity_acc: Option<[f32; 3usize]>, } +#[common_derives] +pub struct RecoverySensing { + pub drogue_current: u16, + pub main_current: u16, + pub drogue_voltage: u16, + pub main_voltage: u16, +} + impl Sensor { pub fn new(data: impl Into) -> Self { Sensor { @@ -225,4 +251,4 @@ impl Sensor { data: data.into(), } } -} +} \ No newline at end of file diff --git a/libraries/sbg-rs/Cargo.toml b/libraries/sbg-rs/Cargo.toml index cb2bd91e..669cc3e0 100644 --- a/libraries/sbg-rs/Cargo.toml +++ b/libraries/sbg-rs/Cargo.toml @@ -9,7 +9,6 @@ edition = "2021" embedded-hal = "0.2.7" nb = "1.0.0" defmt = "0.3.2" -atsamd-hal = { workspace = true } cortex-m = { workspace = true } serde = { workspace = true } diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index f54c8c6d..e82e0031 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -1,15 +1,23 @@ use crate::bindings::{ SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsPos, SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, + SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsPos, SbgLogGpsVel, SbgLogImuData, + SbgLogUtcData, }; use bitflags::Flags; use messages::sensor::{ Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, Imu1, Imu2, UtcTime, }; +use messages::sensor::{ + Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsPos1, GpsPos2, GpsPosAcc, GpsVel, GpsVelAcc, + Imu1, Imu2, UtcTime, +}; use messages::sensor_status::{ AirFlags, AirStatus, EkfFlags, EkfStatus, GpsPositionStatus, GpsPositionStatusE, GpsVelStatus, GpsVelStatusE, ImuFlags, ImuStatus, UtcStatus, UtcTimeStatus, + AirFlags, AirStatus, EkfFlags, EkfStatus, GpsPositionStatus, GpsPositionStatusE, GpsVelStatus, + GpsVelStatusE, ImuFlags, ImuStatus, UtcStatus, UtcTimeStatus, }; /// Simple helper function to work with the flags structure and set the fields as needed. @@ -28,8 +36,9 @@ impl From for (GpsPos1, GpsPos2, GpsPosAcc) { fn from(value: SbgLogGpsPos) -> Self { let status = GpsPositionStatus::new(value.status); - let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); + let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); + ( GpsPos1 { latitude: if valid { Some(value.latitude) } else { None }, diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 45922492..ec23c213 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -1,34 +1,22 @@ use crate::bindings::{ - self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, - _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, - _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, _SbgErrorCode_SBG_NO_ERROR, - _SbgErrorCode_SBG_NULL_POINTER, _SbgErrorCode_SBG_READ_ERROR, _SbgErrorCode_SBG_WRITE_ERROR, - sbgEComCmdOutputSetConf, sbgEComHandle, + self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, _SbgEComLog_SBG_ECOM_LOG_GPS1_RAW, _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, _SbgErrorCode_SBG_NO_ERROR, _SbgErrorCode_SBG_NULL_POINTER, _SbgErrorCode_SBG_READ_ERROR, _SbgErrorCode_SBG_WRITE_ERROR, sbgEComCmdOutputSetConf, sbgEComHandle }; use crate::bindings::{ _SbgBinaryLogData, _SbgDebugLogType, _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, _SbgEComHandle, _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT, _SbgEComLog_SBG_ECOM_LOG_IMU_DATA, _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, _SbgEComProtocol, _SbgErrorCode, _SbgInterface, }; -use atsamd_hal as hal; use core::ffi::c_void; use core::ptr::null_mut; use core::slice::{from_raw_parts, from_raw_parts_mut}; use core::sync::atomic::AtomicUsize; -use defmt::{flush, warn}; +use defmt::{flush, debug, info, warn, error}; use embedded_hal::serial::Write; -use hal::gpio::{PB02, PB03, PB16, PB17}; -use hal::sercom::uart::Duplex; -use hal::sercom::uart::{self, EightBit, Uart}; -use hal::sercom::{IoSet1, IoSet6, Sercom5}; use heapless::Deque; +use heapless::Vec; +use core::ffi::CStr; use messages::sensor::*; -type Pads = uart::PadsFromIds; -type PadsCDC = uart::PadsFromIds; -type Config = uart::Config; - /** * Max buffer size for SBG messages. */ @@ -43,15 +31,16 @@ static mut BUF_INDEX: AtomicUsize = AtomicUsize::new(0); */ static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE]; -static mut DEQ: Deque = Deque::new(); - -/** - * Holds the RTC instance. This is used to get the current time. - */ -static mut RTC: Option> = None; +static mut DEQ: Deque = Deque::new(); static mut DATA_CALLBACK: Option = None; +static mut SERIAL_WRITE_CALLBACK: Option)> = None; + +static mut RTC_GET_TIME: Option u32> = None; + +static mut SERIAL_FLUSH_CALLBACK: Option = None; + pub enum CallbackData { UtcTime(UtcTime), Air(Air), @@ -68,7 +57,6 @@ struct UARTSBGInterface { pub struct SBG { UARTSBGInterface: UARTSBGInterface, - serial_device: Uart, handle: _SbgEComHandle, isInitialized: bool, } @@ -79,23 +67,22 @@ impl SBG { * Takes ownership of the serial device and RTC instance. */ pub fn new( - mut serial_device: Uart, - rtc: hal::rtc::Rtc, callback: fn(CallbackData), + serial_write_callback: fn(Vec), + rtc_get_time: fn() -> u32, + serial_flush_callback: fn(), ) -> Self { - // SAFETY: We are accessing a static variable. - // This is safe because we are the only ones who have access to it. - // Panic if the RTC instance is already taken, this - // only can happen if the SBG instance is created twice. - if unsafe { RTC.is_some() } { - panic!("RTC instance is already taken!"); + unsafe { + DATA_CALLBACK = Some(callback); + SERIAL_WRITE_CALLBACK = Some(serial_write_callback); + RTC_GET_TIME = Some(rtc_get_time); + SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback); } // SAFETY: We are assigning the RTC instance to a static variable. // This is safe because we are the only ones who have access to it. - unsafe { RTC = Some(rtc) }; let interface = UARTSBGInterface { interface: &mut _SbgInterface { - handle: &mut serial_device as *mut Uart as *mut c_void, + handle: null_mut(), // SAFEY: No idea what I just did. type_: 0, name: [0; 48], pDestroyFunc: Some(SBG::SbgDestroyFunc), @@ -130,17 +117,17 @@ impl SBG { cmdDefaultTimeOut: 500, }; - unsafe { DATA_CALLBACK = Some(callback) } + let isInitialized = false; SBG { UARTSBGInterface: interface, - serial_device, handle: handle, isInitialized, } } + /** * Returns true if the SBG is initialized. */ @@ -150,7 +137,7 @@ impl SBG { /** * Reads SBG data frames for a buffer and returns the most recent data. */ - pub fn read_data(&mut self, buffer: &'static [u8; SBG_BUFFER_SIZE]) { + pub fn read_data(&mut self, buffer: &[u8; SBG_BUFFER_SIZE]) { // SAFETY: We are assigning a static mut variable. // Buf can only be accessed from functions called by sbgEComHandle after this assignment. // unsafe { BUF = buffer }; @@ -198,6 +185,19 @@ impl SBG { warn!("Unable to configure UTC Time logs to 40 cycles"); } + let errorCode: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure UTC Time logs to 40 cycles"); + } + // SAFETY: We are calling a C function. // This is safe because it is assumed the SBG library is safe. let errorCode: _SbgErrorCode = unsafe { @@ -323,28 +323,28 @@ impl SBG { if pBuffer.is_null() { return _SbgErrorCode_SBG_NULL_POINTER; } - // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer. - // This is safe because we only have one sbg object and we ensure that - // the peripheral pointer is not accessed during the lifetime of this function. - let serial: *mut Uart = - unsafe { (*pInterface).handle as *mut Uart }; + // SAFETY: We are casting a c_void pointer to a u8 pointer and then creating a slice from it. // This is safe because we ensure pBuffer is valid, pBuffer is not accessed during the lifetime of this function, // and the SBGECom library ensures the buffer given is of the correct size. let array: &[u8] = unsafe { from_raw_parts(pBuffer as *const u8, bytesToWrite) }; - let mut counter: usize = 0; - loop { - if bytesToWrite == counter { - break; - } - // SAFETY: We are accessing a Uart Peripheral pointer. - // This is safe because we ensure that the pointer is not accessed during the lifetime of this function. - let result = unsafe { nb::block!(serial.as_mut().unwrap().write(array[counter])) }; - match result { - Ok(_) => counter += 1, - Err(_) => return _SbgErrorCode_SBG_WRITE_ERROR, - } + let vec = array.iter().copied().collect::>(); + match unsafe { SERIAL_WRITE_CALLBACK } { + Some(callback) => callback(vec), + None => return _SbgErrorCode_SBG_WRITE_ERROR, } + // let mut counter: usize = 0; + // loop { + // if bytesToWrite == counter { + // break; + // } + // // SAFETY: We are accessing a Uart Peripheral pointer. + // // This is safe because we ensure that the pointer is not accessed during the lifetime of this function. + // match unsafe { SERIAL_WRITE_CALLBACK } { + // Some(callback) => callback(&array[counter..counter + 1]), + // None => return _SbgErrorCode_SBG_WRITE_ERROR, + // } + // } _SbgErrorCode_SBG_NO_ERROR } @@ -384,7 +384,13 @@ impl SBG { _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) } - _ => (), + _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { + callback(CallbackData::GpsVel((*pLogData).gpsVelData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => { + } + _ => { + }, } } } @@ -413,13 +419,11 @@ impl SBG { // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer. // This is safe because we only have one sbg object and we ensure that // the peripheral pointer is not accessed during the lifetime of this function. - let serial: *mut Uart = - unsafe { (*pInterface).handle as *mut Uart }; - let result = unsafe { serial.as_mut().unwrap().flush() }; - match result { - Ok(_) => return _SbgErrorCode_SBG_NO_ERROR, - Err(_) => return _SbgErrorCode_SBG_READ_ERROR, + match unsafe { SERIAL_FLUSH_CALLBACK } { + Some(callback) => callback(), + None => return _SbgErrorCode_SBG_WRITE_ERROR, } + _SbgErrorCode_SBG_NO_ERROR } /** @@ -457,31 +461,33 @@ unsafe impl Send for SBG {} */ #[no_mangle] pub unsafe extern "C" fn sbgPlatformDebugLogMsg( - _pFileName: *const ::core::ffi::c_char, - _pFunctionName: *const ::core::ffi::c_char, - _line: u32, - _pCategory: *const ::core::ffi::c_char, + pFileName: *const ::core::ffi::c_char, + pFunctionName: *const ::core::ffi::c_char, + line: u32, + pCategory: *const ::core::ffi::c_char, logType: _SbgDebugLogType, - _errorCode: _SbgErrorCode, - _pFormat: *const ::core::ffi::c_char, + errorCode: _SbgErrorCode, + pFormat: *const ::core::ffi::c_char, ) { - // if pFileName.is_null() || pFunctionName.is_null() || pCategory.is_null() || pFormat.is_null() { - // return; - // } + if pFileName.is_null() || pFunctionName.is_null() || pCategory.is_null() || pFormat.is_null() { + return; + } // // SAFETY: We are converting a raw pointer to a CStr and then to a str. // // This is safe because we check if the pointers are null and // // the pointers can only be accessed during the lifetime of this function. - // let file = unsafe { CStr::from_ptr(pFileName).to_str().unwrap() }; - // let function = unsafe { CStr::from_ptr(pFunctionName).to_str().unwrap() }; - // let _category = unsafe { CStr::from_ptr(pCategory).to_str().unwrap() }; - // let _format = unsafe { CStr::from_ptr(pFormat).to_str().unwrap() }; + let file = unsafe { CStr::from_ptr(pFileName).to_str().unwrap() }; + let function = unsafe { CStr::from_ptr(pFunctionName).to_str().unwrap() }; + let category = unsafe { CStr::from_ptr(pCategory).to_str().unwrap() }; + let format = unsafe { CStr::from_ptr(pFormat).to_str().unwrap() }; + + // info!("{}:{}:{}:{}:{}:{}", file, function, line, category, errorCode, format); match logType { // silently handle errors // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info {} {}", file, function), - // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug {} {}", file, function), + // _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), _ => (), }; flush(); @@ -494,11 +500,11 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg( pub extern "C" fn sbgGetTime() -> u32 { // SAFETY: We are accessing a static mut variable. // This is safe because this is the only place where we access the RTC. - unsafe { - match &RTC { - Some(x) => x.count32(), - None => 0, // bad error handling but we can't panic, maybe we should force the timeout to be zero in the event there is no RTC. + match unsafe { RTC_GET_TIME } { + Some(get_time) => { + get_time() } + None => 0, } }