diff --git a/.cargo/config b/.cargo/config index 209886d8..a8dbc043 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,12 +1,12 @@ -[target.'cfg(all(target_arch = "arm", target_os = "none"))'] -runner = "probe-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"))'] +runner = "probe-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) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cc297233..10545784 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,63 +1,64 @@ -on: - push: - branches: [ master ] - pull_request: - -name: Build - -jobs: - build: - name: All - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - name: Install packages - run: sudo apt update && sudo apt install -y cmake - - name: Install Arm GNU Toolchain (arm-none-eabi-gcc) - uses: carlosperate/arm-none-eabi-gcc-action@v1 - with: - release: '12.2.Rel1' - - uses: actions-rs/toolchain@v1 - with: - toolchain: stable - target: thumbv7em-none-eabihf - - uses: actions/cache@v3 - with: - path: | - ~/.cargo/bin/ - ~/.cargo/registry/index/ - ~/.cargo/registry/cache/ - ~/.cargo/git/db/ - target/ - key: ${{ runner.os }}-cargo-${{ hashFiles('**/Cargo.lock') }} - - uses: actions-rs/cargo@v1 - name: "Install cargo-make" - with: - command: install - args: --force cargo-make - - uses: actions-rs/cargo@v1 - name: "Build all binaries" - with: - command: build -# TODO: Broken, need to fix them -# - uses: actions-rs/cargo@v1 -# name: "Compile Device Tests (no running)" -# with: -# command: make -# args: test-device --no-run - - uses: actions-rs/cargo@v1 - name: "Run Host Tests" - with: - command: make - args: test-host - - uses: actions-rs/cargo@v1 - name: "Generate Typescript bindings" - with: - command: make - args: generate-ts-bindings - - uses: actions/upload-artifact@v3 - name: "Upload Binding Artifacts" - with: - name: bindings - path: ./libraries/messages/bindings - +on: + push: + branches: [ master ] + pull_request: + +name: Build + +jobs: + build: + name: All + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: Install packages + run: sudo apt update && sudo apt install -y cmake + - name: Install Arm GNU Toolchain (arm-none-eabi-gcc) + uses: carlosperate/arm-none-eabi-gcc-action@v1 + with: + release: '12.2.Rel1' + - uses: actions-rs/toolchain@v1 + with: + toolchain: stable + target: thumbv7em-none-eabihf + - uses: actions/cache@v3 + with: + path: | + ~/.cargo/bin/ + ~/.cargo/registry/index/ + ~/.cargo/registry/cache/ + ~/.cargo/git/db/ + target/ + key: ${{ runner.os }}-cargo-${{ hashFiles('**/Cargo.lock') }} + - uses: actions-rs/cargo@v1 + name: "Install cargo-make" + with: + command: install + args: --force cargo-make + - uses: actions-rs/cargo@v1 + name: "Build all binaries" + with: + command: build + args: --release +# TODO: Broken, need to fix them +# - uses: actions-rs/cargo@v1 +# name: "Compile Device Tests (no running)" +# with: +# command: make +# args: test-device --no-run + - uses: actions-rs/cargo@v1 + name: "Run Host Tests" + with: + command: make + args: test-host + - uses: actions-rs/cargo@v1 + name: "Generate Typescript bindings" + with: + command: make + args: generate-ts-bindings + - uses: actions/upload-artifact@v3 + name: "Upload Binding Artifacts" + with: + name: bindings + path: ./libraries/messages/bindings + diff --git a/Cargo.lock b/Cargo.lock index 678522f6..f838443b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -31,16 +31,17 @@ dependencies = [ [[package]] name = "atsamd-hal" -version = "0.15.1" -source = "git+https://github.com/atsamd-rs/atsamd#52073e88d2940101a161340decfcde527e41d279" +version = "0.16.0" +source = "git+https://github.com/atsamd-rs/atsamd#0820f0df58eb8705ddfa6533ed76953d18e6b992" dependencies = [ "aes", "atsame51j", "bitfield 0.13.2", - "bitflags", + "bitflags 1.3.2", "cipher", "cortex-m", "embedded-hal", + "fugit", "mcan-core", "modular-bitfield", "nb 1.1.0", @@ -57,7 +58,7 @@ dependencies = [ [[package]] name = "atsame51j" version = "0.12.0" -source = "git+https://github.com/atsamd-rs/atsamd#52073e88d2940101a161340decfcde527e41d279" +source = "git+https://github.com/atsamd-rs/atsamd#0820f0df58eb8705ddfa6533ed76953d18e6b992" dependencies = [ "cortex-m", "cortex-m-rt", @@ -118,17 +119,45 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" +[[package]] +name = "bitflags" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4682ae6287fcf752ecaabbfcc7b6f9b72aa33933dc23a554d853aea8eea8635" + [[package]] name = "byteorder" version = "1.4.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" +[[package]] +name = "camera" +version = "0.1.0" +dependencies = [ + "atsamd-hal", + "common-arm", + "cortex-m", + "cortex-m-rt", + "cortex-m-rtic", + "defmt", + "enum_dispatch", + "heapless", + "messages", + "panic-halt", + "postcard", + "systick-monotonic", + "typenum", +] + [[package]] name = "cc" -version = "1.0.79" +version = "1.0.82" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "50d30906286121d95be3d479533b458f87493b30a4b5f79a607db8f5d11aa91f" +checksum = "305fe645edc1442a0fa8b6726ba61d422798d37a52e12eaecf4b022ebbb88f01" +dependencies = [ + "libc", +] [[package]] name = "cfg-if" @@ -203,6 +232,7 @@ dependencies = [ "cortex-m-rt", "cortex-m-rtic", "defmt", + "embedded-sdmmc", "heapless", "messages", "panic-halt", @@ -245,8 +275,8 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", ] @@ -272,17 +302,17 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eefb40b1ca901c759d29526e5c8a0a1b246c20caaa5b4cc5d0f0b94debecd4c7" dependencies = [ "proc-macro-error", - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "rtic-syntax", "syn 1.0.109", ] [[package]] name = "cpufeatures" -version = "0.2.7" +version = "0.2.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3e4c1eaa2012c47becbbad2ab175484c2a84d1185b566fb2cc5b8707343dfe58" +checksum = "a17b76ff3a4162b0b27f354a0c87015ddad39d35f9c0c36607a3bdd175dde1f1" dependencies = [ "libc", ] @@ -295,31 +325,31 @@ checksum = "774646b687f63643eb0f4bf13dc263cb581c8c9e57973b6ddf78bda3994d88df" [[package]] name = "critical-section" -version = "1.1.1" +version = "1.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6548a0ad5d2549e111e1f6a11a6c2e2d00ce6a3dafe22948d67c2b443f775e52" +checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "defmt" -version = "0.3.4" +version = "0.3.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "956673bd3cb347512bf988d1e8d89ac9a82b64f6eec54d3c01c3529dac019882" +checksum = "a8a2d011b2fee29fb7d659b83c43fce9a2cb4df453e16d441a51448e448f3f98" dependencies = [ - "bitflags", + "bitflags 1.3.2", "defmt-macros", ] [[package]] name = "defmt-macros" -version = "0.3.5" +version = "0.3.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4abc4821bd84d3d8f49945ddb24d029be9385ed9b77c99bf2f6296847a6a9f0" +checksum = "54f0216f6c5acb5ae1a47050a6645024e6edafc2ee32d421955eccfef12ef92e" dependencies = [ "defmt-parser", "proc-macro-error", - "proc-macro2 1.0.59", - "quote 1.0.28", - "syn 1.0.109", + "proc-macro2 1.0.66", + "quote 1.0.32", + "syn 2.0.28", ] [[package]] @@ -359,8 +389,8 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "94a0dfea4063d72e1ba20494dfbc4667f67420869328cf3670b5824a38a22dc1" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", ] @@ -371,8 +401,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4fb810d30a7c1953f91334de7244731fc3f3c10d7fe163338a35b9f640960321" dependencies = [ "convert_case", - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "rustc_version 0.4.0", "syn 1.0.109", ] @@ -420,25 +450,25 @@ dependencies = [ [[package]] name = "enum_dispatch" -version = "0.3.11" +version = "0.3.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "11f36e95862220b211a6e2aa5eca09b4fa391b13cd52ceb8035a24bf65a79de2" +checksum = "8f33313078bb8d4d05a2733a94ac4c2d8a0df9a2b84424ebf4f33bfc224a890e" dependencies = [ "once_cell", - "proc-macro2 1.0.59", - "quote 1.0.28", - "syn 1.0.109", + "proc-macro2 1.0.66", + "quote 1.0.32", + "syn 2.0.28", ] [[package]] name = "errno" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4bcfec3a70f97c962c307b2d2c56e358cf1d00b558d74262b5f929ee8cc7e73a" +checksum = "6b30f669a7961ef1631673d2766cc92f52d64f7ef354d4fe0ddfd30ed52f0f4f" dependencies = [ "errno-dragonfly", "libc", - "windows-sys 0.48.0", + "windows-sys", ] [[package]] @@ -453,12 +483,9 @@ dependencies = [ [[package]] name = "fastrand" -version = "1.9.0" +version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e51093e27b0797c359783294ca4f0a911c270184cb10f85783b118614a1501be" -dependencies = [ - "instant", -] +checksum = "6999dc1837253364c2ebb0704ba97994bd874e8f195d665c50b7548f6ea92764" [[package]] name = "fnv" @@ -468,9 +495,9 @@ checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" [[package]] name = "fugit" -version = "0.3.6" +version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ab17bb279def6720d058cb6c052249938e7f99260ab534879281a95367a87e5" +checksum = "17186ad64927d5ac8f02c1e77ccefa08ccd9eaa314d5a4772278aa204a22f7e7" dependencies = [ "gcd", ] @@ -493,9 +520,9 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.9" +version = "0.2.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c85e1d9ab2eadba7e5040d4e09cbd6d072b76a557ad64e797c2cb9d4da21d7e4" +checksum = "be4136b2a15dd319360be1c07d9933517ccf0be8f16bf62a3bee4f0d618df427" dependencies = [ "cfg-if", "libc", @@ -531,12 +558,6 @@ dependencies = [ "stable_deref_trait", ] -[[package]] -name = "hermit-abi" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fed44880c466736ef9a5c5b5facefb5ed0785676d0c02d612db14e54f0d84286" - [[package]] name = "indexmap" version = "1.9.3" @@ -547,26 +568,6 @@ dependencies = [ "hashbrown", ] -[[package]] -name = "instant" -version = "0.1.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a5bbe824c507c5da5956355e86a746d82e0e1464f65d862cc5e71da70e94b2c" -dependencies = [ - "cfg-if", -] - -[[package]] -name = "io-lifetimes" -version = "1.0.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eae7b9aee968036d54dce06cebaefd919e4472e753296daccd6d344e3e2df0c2" -dependencies = [ - "hermit-abi", - "libc", - "windows-sys 0.48.0", -] - [[package]] name = "ioctl-rs" version = "0.1.6" @@ -584,9 +585,9 @@ checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" [[package]] name = "libc" -version = "0.2.144" +version = "0.2.147" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b00cc1c228a6782d0f076e7b232802e0c5689d41bb5df366f2a6b6621cfdfe1" +checksum = "b4668fb0ea861c1df094127ac5f1da3409a82116a4ba74fca2e58ef927159bb3" [[package]] name = "libm" @@ -602,15 +603,15 @@ checksum = "9afa463f5405ee81cdb9cc2baf37e08ec7e4c8209442b5d72c04cfb2cd6e6286" [[package]] name = "linux-raw-sys" -version = "0.3.8" +version = "0.4.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519" +checksum = "57bcfdad1b858c2db7c38303a6d2ad4dfaf5eb53dfeb0910128b2c26d6158503" [[package]] name = "lock_api" -version = "0.4.9" +version = "0.4.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "435011366fe56583b16cf956f9df0095b405b82d76425bc8981c0e22e60ec4df" +checksum = "c1cc9717a20b1bb222f333e6a92fd32f7d8a18ddc5a3191a11af45dcbf4dcd16" dependencies = [ "autocfg", "scopeguard", @@ -618,19 +619,16 @@ dependencies = [ [[package]] name = "log" -version = "0.4.17" +version = "0.4.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" -dependencies = [ - "cfg-if", -] +checksum = "b5e6163cb8c49088c2c36f57875e58ccd8c87c7427f7fbd50ea6710b2f3f2e8f" [[package]] name = "mavlink" version = "0.11.1" -source = "git+https://github.com/uorocketry/rust-mavlink?branch=hydra_dialect#26623dfbc6b32b2f48f03c1206b815fce0d1f1d3" +source = "git+https://github.com/uorocketry/rust-mavlink?branch=hydra_dialect#0e0bedd0f37ccad490ea693ad80c2ecffb3d6351" dependencies = [ - "bitflags", + "bitflags 1.3.2", "byteorder", "crc-any", "embedded-hal", @@ -639,9 +637,9 @@ dependencies = [ "nb 1.1.0", "num-derive", "num-traits", - "proc-macro2 1.0.59", + "proc-macro2 1.0.66", "quick-xml", - "quote 1.0.28", + "quote 1.0.32", "serde", "serial", ] @@ -708,8 +706,8 @@ version = "0.11.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a7d5f7076603ebc68de2dc6a650ec331a062a13abaa346975be747bbfa4b789" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", ] @@ -734,16 +732,16 @@ version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "876a53fff98e03a936a674b29568b0e605f06b29372c2489ff4de23f1949743d" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", ] [[package]] name = "num-traits" -version = "0.2.15" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" +checksum = "f30b0abd723be7e2ffca1272140fac1a2f084c77ec3e123c192b66af1ee9e6c2" dependencies = [ "autocfg", "libm", @@ -779,15 +777,15 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.12" +version = "1.0.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f746c4065a8fa3fe23974dd82f15431cc8d40779821001404d10d2e79ca7d79" +checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" [[package]] name = "postcard" -version = "1.0.4" +version = "1.0.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cfa512cd0d087cc9f99ad30a1bf64795b67871edbead083ffc3a4dfafa59aa00" +checksum = "c9ee729232311d3cd113749948b689627618133b1c5012b77342c1950b25eaeb" dependencies = [ "cobs", "defmt", @@ -795,6 +793,25 @@ dependencies = [ "serde", ] +[[package]] +name = "power" +version = "0.1.0" +dependencies = [ + "atsamd-hal", + "common-arm", + "cortex-m", + "cortex-m-rt", + "cortex-m-rtic", + "defmt", + "enum_dispatch", + "heapless", + "messages", + "panic-halt", + "postcard", + "systick-monotonic", + "typenum", +] + [[package]] name = "ppv-lite86" version = "0.2.17" @@ -808,8 +825,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" dependencies = [ "proc-macro-error-attr", - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", "version_check", ] @@ -820,8 +837,8 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "version_check", ] @@ -836,9 +853,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.59" +version = "1.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6aeca18b86b413c660b781aa319e4e2648a3e6f9eadc9b47e9038e6fe9f3451b" +checksum = "18fb31db3f9bddb2ea821cde30a9f70117e3f119938b5ee630b7403aa6e2ead9" dependencies = [ "unicode-ident", ] @@ -850,7 +867,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4e35c06b98bf36aba164cc17cb25f7e232f5c4aeea73baa14b8a9f0d92dbfa65" dependencies = [ "bit-set", - "bitflags", + "bitflags 1.3.2", "byteorder", "lazy_static", "num-traits", @@ -900,11 +917,11 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.28" +version = "1.0.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b9ab9c7eadfd8df19006f1cf1a4aed13540ed5cbc047010ece5826e10825488" +checksum = "50f3b39ccfb720540debaa0164757101c08ecb8d326b15358ce76a62c7e85965" dependencies = [ - "proc-macro2 1.0.59", + "proc-macro2 1.0.66", ] [[package]] @@ -971,7 +988,7 @@ version = "0.3.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "567664f262709473930a4bf9e51bf2ebf3348f2e748ccc50dea20646858f8f29" dependencies = [ - "bitflags", + "bitflags 1.3.2", ] [[package]] @@ -999,8 +1016,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5f5e215601dc467752c2bddc6284a622c6f3d2bab569d992adcd5ab7e4cb9478" dependencies = [ "indexmap", - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", ] @@ -1039,21 +1056,20 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.17", + "semver 1.0.18", ] [[package]] name = "rustix" -version = "0.37.19" +version = "0.38.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "acf8729d8542766f1b2cf77eb034d52f40d375bb8b615d0b147089946e16613d" +checksum = "19ed4fa021d81c8392ce04db050a3da9a60299050b7ae1cf482d862b54a7218f" dependencies = [ - "bitflags", + "bitflags 2.4.0", "errno", - "io-lifetimes", "libc", "linux-raw-sys", - "windows-sys 0.48.0", + "windows-sys", ] [[package]] @@ -1079,15 +1095,16 @@ dependencies = [ "cortex-m-rt", "defmt", "embedded-hal", + "heapless", "messages", "nb 1.1.0", ] [[package]] name = "scopeguard" -version = "1.1.0" +version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "semver" @@ -1100,9 +1117,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.17" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bebd363326d05ec3e2f532ab7660680f3b02130d780c299bca73469d521bc0ed" +checksum = "b0293b4b29daaf487284529cc2f5675b8e57c61f70167ba415a463651fd6a918" [[package]] name = "semver-parser" @@ -1133,28 +1150,28 @@ dependencies = [ [[package]] name = "seq-macro" -version = "0.3.3" +version = "0.3.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6b44e8fc93a14e66336d230954dda83d18b4605ccace8fe09bc7514a71ad0bc" +checksum = "a3f0bf26fd526d2a95683cd0f87bf103b8539e2ca1ef48ce002d67aad59aa0b4" [[package]] name = "serde" -version = "1.0.163" +version = "1.0.183" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2113ab51b87a539ae008b5c6c02dc020ffa39afd2d83cffcb3f4eb2722cebec2" +checksum = "32ac8da02677876d532745a130fc9d8e6edfa81a269b107c5b00829b91d8eb3c" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.163" +version = "1.0.183" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8c805777e3930c8883389c602315a24224bcc738b63905ef87cd1420353ea93e" +checksum = "aafe972d60b0b9bee71a91b92fee2d4fb3c9d7e8f6b179aa99f27203d99a4816" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", - "syn 2.0.18", + "proc-macro2 1.0.66", + "quote 1.0.32", + "syn 2.0.28", ] [[package]] @@ -1250,19 +1267,19 @@ version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "unicode-ident", ] [[package]] name = "syn" -version = "2.0.18" +version = "2.0.28" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "32d41677bcbe24c20c52e7c70b0d8db04134c5d1066bf98662e2871ad200ea3e" +checksum = "04361975b3f5e348b2189d8dc55bc942f278b2d482a6a0365de5bdd62d351567" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "unicode-ident", ] @@ -1279,15 +1296,15 @@ dependencies = [ [[package]] name = "tempfile" -version = "3.5.0" +version = "3.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b9fbec84f381d5795b08656e4912bec604d162bff9291d6189a78f4c8ab87998" +checksum = "dc02fddf48964c42031a0b3fe0428320ecf3a73c401040fc0096f97794310651" dependencies = [ "cfg-if", "fastrand", "redox_syscall", "rustix", - "windows-sys 0.45.0", + "windows-sys", ] [[package]] @@ -1310,22 +1327,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.40" +version = "1.0.44" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "978c9a314bd8dc99be594bc3c175faaa9794be04a5a5e153caba6915336cebac" +checksum = "611040a08a0439f8248d1990b111c95baa9c704c805fa1f62104b39655fd7f90" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.40" +version = "1.0.44" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f" +checksum = "090198534930841fab3a5d1bb637cde49e339654e606195f8d9c76eeb081dc96" dependencies = [ - "proc-macro2 1.0.59", - "quote 1.0.28", - "syn 2.0.18", + "proc-macro2 1.0.66", + "quote 1.0.32", + "syn 2.0.28", ] [[package]] @@ -1345,8 +1362,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9f807fdb3151fee75df7485b901a89624358cd07a67a8fb1a5831bf5a07681ff" dependencies = [ "Inflector", - "proc-macro2 1.0.59", - "quote 1.0.28", + "proc-macro2 1.0.66", + "quote 1.0.32", "syn 1.0.109", "termcolor", ] @@ -1365,9 +1382,9 @@ checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" [[package]] name = "unicode-ident" -version = "1.0.9" +version = "1.0.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b15811caf2415fb889178633e7724bad2509101cde276048e013b9def5e51fa0" +checksum = "301abaae475aa91687eb82514b328ab47a211a533026cb25fc3e519b86adfc3c" [[package]] name = "unicode-xid" @@ -1448,132 +1465,66 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" -[[package]] -name = "windows-sys" -version = "0.45.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "75283be5efb2831d37ea142365f009c02ec203cd29a3ebecbc093d52315b66d0" -dependencies = [ - "windows-targets 0.42.2", -] - [[package]] name = "windows-sys" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" dependencies = [ - "windows-targets 0.48.0", -] - -[[package]] -name = "windows-targets" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e5180c00cd44c9b1c88adb3693291f1cd93605ded80c250a75d472756b4d071" -dependencies = [ - "windows_aarch64_gnullvm 0.42.2", - "windows_aarch64_msvc 0.42.2", - "windows_i686_gnu 0.42.2", - "windows_i686_msvc 0.42.2", - "windows_x86_64_gnu 0.42.2", - "windows_x86_64_gnullvm 0.42.2", - "windows_x86_64_msvc 0.42.2", + "windows-targets", ] [[package]] name = "windows-targets" -version = "0.48.0" +version = "0.48.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b1eb6f0cd7c80c79759c929114ef071b87354ce476d9d94271031c0497adfd5" +checksum = "05d4b17490f70499f20b9e791dcf6a299785ce8af4d709018206dc5b4953e95f" dependencies = [ - "windows_aarch64_gnullvm 0.48.0", - "windows_aarch64_msvc 0.48.0", - "windows_i686_gnu 0.48.0", - "windows_i686_msvc 0.48.0", - "windows_x86_64_gnu 0.48.0", - "windows_x86_64_gnullvm 0.48.0", - "windows_x86_64_msvc 0.48.0", + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", ] -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "597a5118570b68bc08d8d59125332c54f1ba9d9adeedeef5b99b02ba2b0698f8" - [[package]] name = "windows_aarch64_gnullvm" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "91ae572e1b79dba883e0d315474df7305d12f569b400fcf90581b06062f7e1bc" -[[package]] -name = "windows_aarch64_msvc" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e08e8864a60f06ef0d0ff4ba04124db8b0fb3be5776a5cd47641e942e58c4d43" - [[package]] name = "windows_aarch64_msvc" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b2ef27e0d7bdfcfc7b868b317c1d32c641a6fe4629c171b8928c7b08d98d7cf3" -[[package]] -name = "windows_i686_gnu" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c61d927d8da41da96a81f029489353e68739737d3beca43145c8afec9a31a84f" - [[package]] name = "windows_i686_gnu" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "622a1962a7db830d6fd0a69683c80a18fda201879f0f447f065a3b7467daa241" -[[package]] -name = "windows_i686_msvc" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "44d840b6ec649f480a41c8d80f9c65108b92d89345dd94027bfe06ac444d1060" - [[package]] name = "windows_i686_msvc" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4542c6e364ce21bf45d69fdd2a8e455fa38d316158cfd43b3ac1c5b1b19f8e00" -[[package]] -name = "windows_x86_64_gnu" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8de912b8b8feb55c064867cf047dda097f92d51efad5b491dfb98f6bbb70cb36" - [[package]] name = "windows_x86_64_gnu" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ca2b8a661f7628cbd23440e50b05d705db3686f894fc9580820623656af974b1" -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26d41b46a36d453748aedef1486d5c7a85db22e56aff34643984ea85514e94a3" - [[package]] name = "windows_x86_64_gnullvm" version = "0.48.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7896dbc1f41e08872e9d5e8f8baa8fdd2677f29468c4e156210174edc7f7b953" -[[package]] -name = "windows_x86_64_msvc" -version = "0.42.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9aec5da331524158c6d1a4ac0ab1541149c0b9505fde06423b02f5ef0106b9f0" - [[package]] name = "windows_x86_64_msvc" version = "0.48.0" diff --git a/Cargo.toml b/Cargo.toml index fcc1f27c..7ccf1783 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,57 +1,57 @@ -[workspace] - -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/*" -] - -[workspace.dependencies.atsamd-hal] -git = "https://github.com/atsamd-rs/atsamd" -features = ["same51j", "same51j-rt", "dma", "can"] - -[workspace.dependencies.serde] -version = "1.0.150" -default-features = false -features = ["derive"] - -[workspace.dependencies.cortex-m] -version = "0.7.6" -features = ["critical-section-single-core"] - -[profile.dev] -# 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" - -[profile.dev.package.sbg-rs] -opt-level = 0 -debug = true - -[profile.dev.build-override] -opt-level = 0 -debug = true - -[profile.release] -# symbols are nice and they don't increase the size on Flash -debug = true -lto = true -opt-level = 1 - -[profile.release.package.sbg-rs] -debug = true -opt-level = 0 - -[profile.release.build-override] -opt-level = 0 -debug = true +[workspace] + +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/*" +] + +[workspace.dependencies.atsamd-hal] +git = "https://github.com/atsamd-rs/atsamd" +features = ["same51j", "same51j-rt", "dma", "can"] + +[workspace.dependencies.serde] +version = "1.0.150" +default-features = false +features = ["derive"] + +[workspace.dependencies.cortex-m] +version = "0.7.6" +features = ["critical-section-single-core"] + +[profile.dev] +# 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" + +[profile.dev.package.sbg-rs] +opt-level = 0 +debug = true + +[profile.dev.build-override] +opt-level = 0 +debug = true + +[profile.release] +# symbols are nice and they don't increase the size on Flash +debug = true +lto = true +opt-level = 1 + +[profile.release.package.sbg-rs] +debug = true +opt-level = 0 + +[profile.release.build-override] +opt-level = 0 +debug = true diff --git a/README.md b/README.md index ef0a65e5..cde47cf4 100644 --- a/README.md +++ b/README.md @@ -1,44 +1,44 @@ -# HYDRA   [![Build Status]][actions] [![docs-badge]][docs] -*HYper Dynamic Rocketry Avionics* - -[Build Status]: https://github.com/uorocketry/hydra/actions/workflows/build.yml/badge.svg -[actions]: https://github.com/uorocketry/hydra/actions?query=branch%3Amaster -[docs-badge]: https://img.shields.io/github/actions/workflow/status/uorocketry/hydra/docs.yml?label=docs -[docs]: http://hydra-docs.uorocketry.ca/common_arm - -**uORocketry's next-generation avionics system** - ---- - -## Getting Started - -1. Install Rust: https://www.rust-lang.org/tools/install -2. Build: `cargo build` -3. Install probe-run: `cargo install --git https://github.com/uorocketry/probe-run` - - `probe-run` currently requires a patch to flash our chip, so please use the above version while the patch is upstreamed -4, Install cargo-make: `cargo install cargo-make` -4. Flash: `cargo run --bin sensor` or one of the other boards [communication, recovery, power] -5. Run tests: `cargo make test-host` or `cargo make test-device` - -For more detailed instructions on flashing, debugging, and more, please see [the wiki](https://avwiki.uorocketry.ca/en/Avionics/HYDRA/Software). - -## Documentation - -Run `cargo doc --open` to build and open the documentation. Most documentation for this repo is contained in the `common-arm` crate. - -Documentation is also automatically built and deployed to https://hydra-docs.uorocketry.ca/common_arm - -## Project Structure -The project is structured in a way to allow reuse of the code across various boards and other projects. - -- `boards`: Folder containing each individual board's binary crates. Any code in those crates should only contain logic specific to the board. -- `debug`: Useful files for debugging, such as GDB and OpenOCD configuration. -- `examples`: Example projects that can be used to quickly start a new board. Simply copy and paste one of those creates to the `boards` folder, and rename as needed. -- `libraries`: - - `common-arm`: Common code that depends on embedded-specific logic or crates. - -## License - -This project is licensed under GPLv3-only. - -We please ask for any derivatives of this work to be kept open-source, even if such derivative is only for internal use. +# HYDRA   [![Build Status]][actions] [![docs-badge]][docs] +*HYper Dynamic Rocketry Avionics* + +[Build Status]: https://github.com/uorocketry/hydra/actions/workflows/build.yml/badge.svg +[actions]: https://github.com/uorocketry/hydra/actions?query=branch%3Amaster +[docs-badge]: https://img.shields.io/github/actions/workflow/status/uorocketry/hydra/docs.yml?label=docs +[docs]: http://hydra-docs.uorocketry.ca/common_arm + +**uORocketry's next-generation avionics system** + +--- + +## Getting Started + +1. Install Rust: https://www.rust-lang.org/tools/install +2. Build: `cargo build` +3. Install probe-run: `cargo install --git https://github.com/uorocketry/probe-run` + - `probe-run` currently requires a patch to flash our chip, so please use the above version while the patch is upstreamed +4, Install cargo-make: `cargo install cargo-make` +4. Flash: `cargo run --bin main` +5. Run tests: `cargo make test-host` or `cargo make test-device` + +For more detailed instructions on flashing, debugging, and more, please see [the wiki](https://avwiki.uorocketry.ca/en/Avionics/HYDRA/Software). + +## Documentation + +Run `cargo doc --open` to build and open the documentation. Most documentation for this repo is contained in the `common-arm` crate. + +Documentation is also automatically built and deployed to https://hydra-docs.uorocketry.ca/common_arm + +## Project Structure +The project is structured in a way to allow reuse of the code across various boards and other projects. + +- `boards`: Folder containing each individual board's binary crates. Any code in those crates should only contain logic specific to the board. +- `debug`: Useful files for debugging, such as GDB and OpenOCD configuration. +- `examples`: Example projects that can be used to quickly start a new board. Simply copy and paste one of those creates to the `boards` folder, and rename as needed. +- `libraries`: + - `common-arm`: Common code that depends on embedded-specific logic or crates. + +## License + +This project is licensed under GPLv3-only. + +We please ask for any derivatives of this work to be kept open-source, even if such derivative is only for internal use. \ No newline at end of file diff --git a/boards/camera/Cargo.toml b/boards/camera/Cargo.toml new file mode 100644 index 00000000..371fdbc6 --- /dev/null +++ b/boards/camera/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "camera" +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" +panic-halt = "0.2.0" +systick-monotonic = "1.0.1" +defmt = "0.3.2" +postcard = "1.0.2" +heapless = "0.7.16" +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/camera/src/communication.rs b/boards/camera/src/communication.rs new file mode 100644 index 00000000..e30a7d54 --- /dev/null +++ b/boards/camera/src/communication.rs @@ -0,0 +1,171 @@ +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::dmac; +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::sercom::Sercom; +use atsamd_hal::sercom::{uart, Sercom5}; +use atsamd_hal::sercom; +use atsamd_hal::sercom::uart::{ TxDuplex, RxDuplex}; +use atsamd_hal::sercom::uart::{Duplex, Uart}; +use atsamd_hal::time::*; +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::HydraError; +use defmt::flush; +use defmt::info; +use common_arm::spawn; +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; +use messages::Message; +use postcard::from_bytes; +use systick_monotonic::fugit::RateExtU32; +use typenum::{U0, U128, U32, U64}; + +use atsamd_hal::dmac::Transfer; + +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, + ] + .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::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor filter")); + + let can = can.finalize().unwrap(); + ( + CanDevice0 { + can, + line_interrupts, + }, + gclk0, + ) + } + 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/camera/src/data_manager.rs b/boards/camera/src/data_manager.rs new file mode 100644 index 00000000..5349b9c5 --- /dev/null +++ b/boards/camera/src/data_manager.rs @@ -0,0 +1,125 @@ +use messages::sensor::{Air}; +use messages::Message; +use messages::state::{State, StateData}; +use defmt::info; +use heapless::HistoryBuffer; + +const MAIN_HEIGHT: f32 = 876.0; // meters +const HEIGHT_MIN: f32 = 600.0; // meters + +pub struct DataManager { + pub air: Option, + pub historical_barometer_altitude: HistoryBuffer<(f32, u32), 8>, +} + +impl DataManager { + pub fn new() -> Self { + let historical_barometer_altitude = HistoryBuffer::new(); + Self { + air: None, + historical_barometer_altitude, + } + } + /// Returns true if the rocket is descending + pub fn is_falling(&self) -> bool { + if self.historical_barometer_altitude.len() < 8 { + 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 / 1_000_000.0; + if time_diff == 0.0 { + continue; + } + let slope = (i.0 - prev.0)/time_diff; + if slope < -100.0 { + return false; + } + avg_sum += slope; + prev = i; + } + match avg_sum / 7.0 { // 7 because we have 8 points. + // exclusive range + x if !(-100.0..=-5.0).contains(&x) => { + return false; + } + _ => { + info!("avg: {}", avg_sum / 7.0); + } + } + } + None => { + return false; + } + } + true + } + pub fn is_launched(&self) -> bool { + match self.air.as_ref() { + Some(air) => air.altitude > HEIGHT_MIN, + None => false, + } + } + pub fn is_landed(&self) -> bool { + if self.historical_barometer_altitude.len() < 8 { + 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 / 1_000_000.0; + if time_diff == 0.0 { + continue; + } + avg_sum += (i.0 - prev.0)/time_diff; + prev = i; + } + match avg_sum / 7.0 { + // inclusive range + x if (-4.0..=4.0).contains(&x) => { + return true; + } + _ => { + // continue + } + } + } + None => { + return false; + } + } + false + } + pub fn is_below_main(&self) -> bool { + match self.air.as_ref() { + Some(air) => air.altitude < MAIN_HEIGHT, + None => false, + } + } + pub fn handle_data(&mut self, data: Message) { + match data.data { + messages::Data::Sensor(sensor) => match sensor.data { + messages::sensor::SensorData::Air(air_data) => { + self.air = Some(air_data); + } + _ => { + } + }, + _ => { + } + } + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/camera/src/gpio_manager.rs b/boards/camera/src/gpio_manager.rs new file mode 100644 index 00000000..5ab28d08 --- /dev/null +++ b/boards/camera/src/gpio_manager.rs @@ -0,0 +1,22 @@ +use atsamd_hal::gpio::{Pin, PushPullOutput, PA09, PA06}; +use atsamd_hal::prelude::*; + +pub struct GPIOManager { + cam1: Pin, + cam2: Pin, +} + +impl GPIOManager { + pub fn new(mut cam1: Pin, mut cam2: Pin) -> Self { + Self { + cam1, + cam2 + } + } + pub fn toggle_cam1(&mut self) { + self.cam1.toggle().ok(); + } + pub fn toggle_cam2(&mut self) { + self.cam2.toggle().ok(); + } +} \ No newline at end of file diff --git a/boards/camera/src/main.rs b/boards/camera/src/main.rs new file mode 100644 index 00000000..66dcf2b3 --- /dev/null +++ b/boards/camera/src/main.rs @@ -0,0 +1,163 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod gpio_manager; +mod types; +mod state_machine; + +use communication::Capacities; +use crate::state_machine::{StateMachine, StateMachineContext}; +use crate::data_manager::DataManager; +use crate::gpio_manager::GPIOManager; +use common_arm::ErrorManager; +use atsamd_hal as hal; +use atsamd_hal::clock::v2::pclk::Pclk; +use atsamd_hal::clock::v2::Source; +use common_arm::mcan; +use hal::gpio::Pins; +use mcan::messageram::SharedMemory; +use panic_halt as _; +use systick_monotonic::*; +use common_arm::*; +use hal::gpio::{PB17, PushPullOutput, Pin, PB16}; +use hal::prelude::*; + + +#[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: communication::CanDevice0, + gpio_manager: GPIOManager, + } + + #[local] + struct Local { + state_machine: StateMachine, + led_red: Pin, + led_green: 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) = 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 (_, _, _, 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, + ); + + /* Status LED */ + let led_red = pins.pb17.into_push_pull_output(); + let led_green = pins.pb16.into_push_pull_output(); + let gpio = GPIOManager::new( + pins.pa09.into_push_pull_output(), + pins.pa06.into_push_pull_output(), + ); + + let state_machine = StateMachine::new(); + + /* Spawn tasks */ + run_sm::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(), can0, gpio_manager: gpio}, Local {state_machine, led_red , led_green}, init::Monotonics(mono)) + } + + /// Idle task for when no other tasks are running. + #[idle] + fn idle(_: idle::Context) -> ! { + loop {} + } + + /// Handles the CAN0 interrupt. + #[task(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)); + }); + } + + #[task(priority = 3, local = [num: u8 = 0], shared = [gpio_manager, &em])] + fn toggle_cams(mut cx: toggle_cams::Context) { + cx.shared.em.run(|| { + cx.shared.gpio_manager.lock(|gpio| { + gpio.toggle_cam1(); + gpio.toggle_cam2(); + }); + Ok(()) + }); + if (*cx.local.num < 3) { + *cx.local.num += 1; + spawn_after!(toggle_cams, ExtU64::millis(150)); + } + } + + /// Runs the state machine. + /// This takes control of the shared resources. + #[task(priority = 3, local = [state_machine], shared = [can0, gpio_manager, data_manager, &em])] + fn run_sm(mut cx: run_sm::Context) { + cx.local.state_machine.run(&mut StateMachineContext { + shared_resources: &mut cx.shared, + }); + spawn_after!(run_sm, 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/camera/src/state_machine/black_magic.rs b/boards/camera/src/state_machine/black_magic.rs new file mode 100644 index 00000000..1211a839 --- /dev/null +++ b/boards/camera/src/state_machine/black_magic.rs @@ -0,0 +1,44 @@ +use crate::state_machine::{RocketEvents, RocketStates, StateMachineContext}; +use core::fmt::Debug; +use defmt::{info, Format}; +use enum_dispatch::enum_dispatch; + +/// Trait that all states implement. Ignore this, not super important +#[enum_dispatch] +pub trait State: Debug { + fn enter(&self, _context: &mut StateMachineContext) + where + Self: Format, + { + info!("Enter {:?}", self) + } + fn exit(&self) + where + Self: Format, + { + info!("Exit {:?}", self) + } + fn event(&mut self, _event: RocketEvents) -> Option { + None + } + fn step(&mut self, context: &mut StateMachineContext) -> Option; +} + +/// Transition Trait +pub trait TransitionInto { + fn transition(&self) -> T; +} + +#[macro_export] +macro_rules! transition { + ($self:ident, $i:ident) => { + Some(TransitionInto::<$i>::transition($self).into()) + }; +} + +#[macro_export] +macro_rules! no_transition { + () => { + None + }; +} diff --git a/boards/camera/src/state_machine/mod.rs b/boards/camera/src/state_machine/mod.rs new file mode 100644 index 00000000..fd2ee6c1 --- /dev/null +++ b/boards/camera/src/state_machine/mod.rs @@ -0,0 +1,118 @@ +mod black_magic; +mod states; + +use messages::state; +use crate::communication::CanDevice0; +use crate::data_manager::DataManager; +use crate::state_machine::states::*; +use crate::gpio_manager::GPIOManager; +pub use black_magic::*; +pub use states::Initializing; +use core::fmt::Debug; +use defmt::Format; +use enum_dispatch::enum_dispatch; +use rtic::Mutex; + +pub trait StateMachineSharedResources { + fn lock_can(&mut self, f: &dyn Fn(&mut CanDevice0)); + fn lock_data_manager(&mut self, f: &dyn Fn(&mut DataManager)); + fn lock_gpio(&mut self, f: &dyn Fn(&mut GPIOManager)); +} + +impl<'a> StateMachineSharedResources for crate::app::__rtic_internal_run_smSharedResources<'a> { + fn lock_can(&mut self, fun: &dyn Fn(&mut CanDevice0)) { + self.can0.lock(fun) + } + fn lock_data_manager(&mut self, fun: &dyn Fn(&mut DataManager)) { + self.data_manager.lock(fun) + } + fn lock_gpio(&mut self, fun: &dyn Fn(&mut GPIOManager)) { + self.gpio_manager.lock(fun) + } +} + +pub struct StateMachineContext<'a, 'b> { + pub shared_resources: &'b mut crate::app::__rtic_internal_run_smSharedResources<'a> +} +pub struct StateMachine { + state: RocketStates, +} + +// Define some functions to interact with the state machine +impl StateMachine { + pub fn new() -> Self { + let state = Initializing {}; + + StateMachine { + state: state.into(), + } + } + + pub fn run(&mut self, context: &mut StateMachineContext) { + if let Some(new_state) = self.state.step(context) { + self.state.exit(); + new_state.enter(context); + self.state = new_state; + } + } + + pub fn get_state(&self) -> RocketStates { + self.state.clone() + } +} + +impl Default for StateMachine { + fn default() -> Self { + Self::new() + } +} + +// All events are found here +pub enum RocketEvents { + DeployDrogue, + DeployMain, +} + +// All states are defined here. Another struct must be defined for the actual state, and that struct +// must implement the State trait +#[enum_dispatch(State)] +#[derive(Debug, Format, Clone)] +pub enum RocketStates { + Initializing, + WaitForTakeoff, + Ascent, + Descent, + TerminalDescent, + WaitForRecovery, + Abort, +} + +// Not a fan of this. +// Should be able to put this is a shared library. +impl From for RocketStates { + fn from(state: state::StateData) -> Self { + match state { + state::StateData::Initializing => RocketStates::Initializing(Initializing {}), + state::StateData::WaitForTakeoff => RocketStates::WaitForTakeoff(WaitForTakeoff {}), + state::StateData::Ascent => RocketStates::Ascent(Ascent {}), + state::StateData::Descent => RocketStates::Descent(Descent {}), + state::StateData::TerminalDescent => RocketStates::TerminalDescent(TerminalDescent { } ), + state::StateData::WaitForRecovery => RocketStates::WaitForTakeoff(WaitForTakeoff { }), + state::StateData::Abort => RocketStates::Abort(Abort {}), + } + } +} +// Linter: an implementation of From is preferred since it gives you Into<_> for free where the reverse isn't true +impl Into for RocketStates { + fn into(self) -> state::StateData { + match self { + RocketStates::Initializing(_) => state::StateData::Initializing, + RocketStates::WaitForTakeoff(_) => state::StateData::WaitForTakeoff, + RocketStates::Ascent(_) => state::StateData::Ascent, + RocketStates::Descent(_) => state::StateData::Descent, + RocketStates::TerminalDescent(_) => state::StateData::TerminalDescent, + RocketStates::WaitForRecovery(_) => state::StateData::WaitForRecovery, + RocketStates::Abort(_) => state::StateData::Abort, + } + } +} \ No newline at end of file diff --git a/boards/camera/src/state_machine/states/abort.rs b/boards/camera/src/state_machine/states/abort.rs new file mode 100644 index 00000000..92cc71ca --- /dev/null +++ b/boards/camera/src/state_machine/states/abort.rs @@ -0,0 +1,24 @@ +use crate::state_machine::states::initializing::Initializing; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use defmt::{write, Format, Formatter}; + +#[derive(Debug, Clone)] +pub struct Abort {} + +impl State for Abort { + fn step(&mut self, _context: &mut StateMachineContext) -> Option { + todo!() + } +} + +impl TransitionInto for Initializing { + fn transition(&self) -> Abort { + Abort {} + } +} + +impl Format for Abort { + fn format(&self, f: Formatter) { + write!(f, "Abort") + } +} diff --git a/boards/camera/src/state_machine/states/ascent.rs b/boards/camera/src/state_machine/states/ascent.rs new file mode 100644 index 00000000..c86df4d5 --- /dev/null +++ b/boards/camera/src/state_machine/states/ascent.rs @@ -0,0 +1,46 @@ +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::{no_transition, transition}; +use messages::command::{Command, CommandData, PowerDown, RadioRateChange, RadioRate}; +use messages::Message; +use defmt::{write, Format, Formatter}; +use rtic::mutex::Mutex; +use crate::types::COM_ID; +use crate::app::monotonics; +use systick_monotonic::ExtU64; +use crate::app::toggle_cams; +use common_arm::spawn; + +#[derive(Debug, Clone)] +pub struct Ascent {} + +impl State for Ascent { + fn enter(&self, context: &mut StateMachineContext) { + spawn!(toggle_cams); + } + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context + .shared_resources + .data_manager + .lock(|data| { + if data.is_falling() { + transition!(self, Descent) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for WaitForTakeoff { + fn transition(&self) -> Ascent { + Ascent {} + } +} + +impl Format for Ascent { + fn format(&self, f: Formatter) { + write!(f, "Ascent") + } +} diff --git a/boards/camera/src/state_machine/states/descent.rs b/boards/camera/src/state_machine/states/descent.rs new file mode 100644 index 00000000..18596061 --- /dev/null +++ b/boards/camera/src/state_machine/states/descent.rs @@ -0,0 +1,32 @@ +use super::Ascent; +use crate::state_machine::{TerminalDescent, RocketStates, State, StateMachineContext, TransitionInto}; +use crate::{no_transition, transition}; +use rtic::mutex::Mutex; +use defmt::{write, Format, Formatter, info}; + +#[derive(Debug, Clone)] +pub struct Descent {} + +impl State for Descent { + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context.shared_resources.data_manager.lock(|data| { + if data.is_below_main() { + transition!(self, TerminalDescent) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for Ascent { + fn transition(&self) -> Descent { + Descent {} + } +} + +impl Format for Descent { + fn format(&self, f: Formatter) { + write!(f, "Descent") + } +} diff --git a/boards/camera/src/state_machine/states/initializing.rs b/boards/camera/src/state_machine/states/initializing.rs new file mode 100644 index 00000000..b9adab77 --- /dev/null +++ b/boards/camera/src/state_machine/states/initializing.rs @@ -0,0 +1,20 @@ +use defmt::{write, Format, Formatter}; + +use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use crate::transition; + +#[derive(Debug, Clone)] +pub struct Initializing {} + +impl State for Initializing { + fn step(&mut self, _context: &mut StateMachineContext) -> Option { + transition!(self, WaitForTakeoff) + } +} + +impl Format for Initializing { + fn format(&self, f: Formatter) { + write!(f, "Initializing") + } +} diff --git a/boards/camera/src/state_machine/states/mod.rs b/boards/camera/src/state_machine/states/mod.rs new file mode 100644 index 00000000..f4d4c3fe --- /dev/null +++ b/boards/camera/src/state_machine/states/mod.rs @@ -0,0 +1,15 @@ +mod initializing; +mod terminal_descent; +mod ascent; +mod descent; +mod wait_for_takeoff; +mod wait_for_recovery; +mod abort; + +pub use crate::state_machine::states::descent::Descent; +pub use crate::state_machine::states::ascent::Ascent; +pub use crate::state_machine::states::initializing::Initializing; +pub use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; +pub use crate::state_machine::states::terminal_descent::TerminalDescent; +pub use crate::state_machine::states::wait_for_recovery::WaitForRecovery; +pub use crate::state_machine::states::abort::Abort; \ No newline at end of file diff --git a/boards/camera/src/state_machine/states/terminal_descent.rs b/boards/camera/src/state_machine/states/terminal_descent.rs new file mode 100644 index 00000000..47394287 --- /dev/null +++ b/boards/camera/src/state_machine/states/terminal_descent.rs @@ -0,0 +1,36 @@ +use defmt::{write, Format, Formatter, info}; + +use crate::{no_transition, transition}; +use crate::state_machine::{WaitForRecovery, RocketStates, State, StateMachineContext, TransitionInto}; +use rtic::mutex::Mutex; +use super::Descent; + +#[derive(Debug, Clone)] +pub struct TerminalDescent {} + +impl State for TerminalDescent { + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context + .shared_resources + .data_manager + .lock(|data| { + if data.is_landed() { + transition!(self, WaitForRecovery) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for Descent { + fn transition(&self) -> TerminalDescent { + TerminalDescent {} + } +} + +impl Format for TerminalDescent { + fn format(&self, f: Formatter) { + write!(f, "Terminal Descent") + } +} diff --git a/boards/camera/src/state_machine/states/wait_for_recovery.rs b/boards/camera/src/state_machine/states/wait_for_recovery.rs new file mode 100644 index 00000000..5ced98af --- /dev/null +++ b/boards/camera/src/state_machine/states/wait_for_recovery.rs @@ -0,0 +1,31 @@ +use super::TerminalDescent; +use crate::app::monotonics; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use crate::types::COM_ID; +use crate::{no_transition, transition}; +use rtic::mutex::Mutex; +use defmt::{write, Format, Formatter, info}; +use messages::command::{Command, CommandData, PowerDown, RadioRateChange, RadioRate}; +use messages::Message; +use messages::sender::Sender::SensorBoard; + +#[derive(Debug, Clone)] +pub struct WaitForRecovery {} + +impl State for WaitForRecovery { + fn step(&mut self, context: &mut StateMachineContext) -> Option { + no_transition!() // this is our final resting place. We should also powerdown this board. + } +} + +impl TransitionInto for TerminalDescent { + fn transition(&self) -> WaitForRecovery { + WaitForRecovery {} + } +} + +impl Format for WaitForRecovery { + fn format(&self, f: Formatter) { + write!(f, "Descent") + } +} diff --git a/boards/camera/src/state_machine/states/wait_for_takeoff.rs b/boards/camera/src/state_machine/states/wait_for_takeoff.rs new file mode 100644 index 00000000..a63e8743 --- /dev/null +++ b/boards/camera/src/state_machine/states/wait_for_takeoff.rs @@ -0,0 +1,36 @@ +use crate::state_machine::states::ascent::Ascent; +use crate::state_machine::states::initializing::Initializing; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use crate::{no_transition, transition}; +use rtic::mutex::Mutex; +use defmt::{write, Format, Formatter}; + +#[derive(Debug, Clone)] +pub struct WaitForTakeoff {} + +impl State for WaitForTakeoff { + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context + .shared_resources + .data_manager + .lock(|data| { + if data.is_launched() { + transition!(self, Ascent) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for Initializing { + fn transition(&self) -> WaitForTakeoff { + WaitForTakeoff {} + } +} + +impl Format for WaitForTakeoff { + fn format(&self, f: Formatter) { + write!(f, "WaitForTakeoff") + } +} diff --git a/boards/camera/src/types.rs b/boards/camera/src/types.rs new file mode 100644 index 00000000..72b4b3ca --- /dev/null +++ b/boards/camera/src/types.rs @@ -0,0 +1,5 @@ +use messages::sender::{Sender, Sender::CameraBoard}; +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = CameraBoard; \ No newline at end of file diff --git a/boards/communication/Cargo.toml b/boards/communication/Cargo.toml index 8a298049..0d45d894 100644 --- a/boards/communication/Cargo.toml +++ b/boards/communication/Cargo.toml @@ -1,20 +1,21 @@ -[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" -panic-halt = "0.2.0" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -common-arm = { path = "../../libraries/common-arm" } -atsamd-hal = { workspace = true } -messages = { path = "../../libraries/messages" } -typenum = "1.16.0" \ No newline at end of file +[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" +panic-halt = "0.2.0" +systick-monotonic = "1.0.1" +defmt = "0.3.2" +postcard = "1.0.2" +heapless = "0.7.16" +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 index 606cb4b3..41ae89aa 100644 --- a/boards/communication/src/communication.rs +++ b/boards/communication/src/communication.rs @@ -1,268 +1,334 @@ -/// Encapsulates all communication logic. -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::sercom; -use atsamd_hal::sercom::uart; -use atsamd_hal::sercom::uart::{Duplex, Uart}; -use atsamd_hal::time::U32Ext; -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::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::messageram::SharedMemory; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; -use messages::mavlink; -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::sender::Sender::RecoveryBoard.into()) - .unwrap(), - mask: ecan::StandardId::MAX, - }) - .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::MAX, - }) - .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::MAX, - }) - .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::MAX, - }) - .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) - } - } - } - } - _ => (), - } - } - } -} - -pub struct RadioDevice { - uart: Uart, - mav_sequence: u8, -} - -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(); - ( - RadioDevice { - uart, - mav_sequence: 0, - }, - gclk0, - ) - } - 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.mav_sequence.wrapping_add(1), - }; - - let mav_message = mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE( - mavlink::uorocketry::POSTCARD_MESSAGE_DATA { message: payload }, - ); - - mavlink::write_versioned_msg( - &mut self.uart, - mavlink::MavlinkVersion::V2, - mav_header, - &mav_message, - )?; - Ok(()) - } -} +use crate::app::send_internal; +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::dmac; +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::sercom; +use atsamd_hal::sercom::uart::{Duplex, Uart}; +use atsamd_hal::sercom::uart::{RxDuplex, TxDuplex}; +use atsamd_hal::sercom::Sercom; +use atsamd_hal::sercom::{uart, Sercom5}; +use atsamd_hal::time::*; +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::spawn; +use common_arm::HydraError; +use defmt::flush; +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; +use messages::Message; +use postcard::from_bytes; +use systick_monotonic::fugit::RateExtU32; +use typenum::{U0, U128, U32, U64}; + +use atsamd_hal::dmac::Transfer; + +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::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: 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) + } + } + } + } + _ => (), + } + } + } +} + +pub static mut BUF_DST: RadioBuffer = &mut [0; 255]; + +pub struct RadioDevice { + uart: Uart, + mav_sequence: u8, +} + +impl RadioDevice { + pub fn new( + radio_token: PclkToken, + mclk: &MCLK, + sercom: SERCOM5, + rx_pin: Pin>, + tx_pin: Pin>, + gclk0: S, + mut dma_channel: dmac::Channel, + ) -> (Self, S::Inc, RadioTransfer) + 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 (rx, tx) = uart.split(); // tx sends rx uses dma to receive so we need to setup dma here. + dma_channel + .as_mut() + .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true)); + let xfer = Transfer::new(dma_channel, rx, unsafe { &mut *BUF_DST }, false) + .expect("DMA Radio RX") + .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); + ( + RadioDevice { + uart: tx, + mav_sequence: 0, + }, + gclk0, + xfer, + ) + } + 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.uart, + 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 struct RadioManager { + xfer: Option, + buf_select: bool, +} + +impl RadioManager { + pub fn new(xfer: RadioTransfer) -> Self { + RadioManager { + xfer: Some(xfer), + buf_select: false, + } + } + pub fn process_message(&mut self, buf: RadioBuffer) { + match from_bytes::(buf) { + Ok(msg) => { + info!("Radio: {}", msg); + // spawn!(send_internal, msg); // dump to the Can Bus + } + Err(_) => { + info!("Radio unknown msg"); + return; + } + } + } +} + +// pub fn radio_dma(cx: crate::app::radio_dma::Context) { +// let manager = cx.local.radio_manager; +// match &mut manager.xfer { +// Some(xfer) => { +// if xfer.complete() { +// let (chan0, source, buf) = manager.xfer.take().unwrap().stop(); +// let mut xfer = dmac::Transfer::new(chan0, source, unsafe{&mut *BUF_DST}, false).expect("f").begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST); +// manager.process_message(buf); +// unsafe{BUF_DST.copy_from_slice(&[0;255])}; +// xfer.block_transfer_interrupt(); +// manager.xfer = Some(xfer); +// } +// } +// None => { +// info!("none"); +// } +// } +// } diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs index f95a3e75..cfe92b33 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -1,75 +1,121 @@ -use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, SensorData, UtcTime}; -use messages::Message; - -#[derive(Clone)] -pub struct DataManager { - pub air: Option, - pub ekf_nav: (Option, Option), - pub ekf_quat: Option, - pub imu: (Option, Option), - pub utc_time: Option, - pub gps_vel: Option, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav: (None, None), - ekf_quat: None, - imu: (None, None), - utc_time: None, - gps_vel: None, - } - } - - pub fn clone_sensors(&self) -> [Option; 8] { - [ - self.air.clone().map(|x| x.into()), - self.ekf_nav.0.clone().map(|x| x.into()), - self.ekf_nav.1.clone().map(|x| x.into()), - self.ekf_quat.clone().map(|x| x.into()), - self.imu.0.clone().map(|x| x.into()), - self.imu.1.clone().map(|x| x.into()), - self.utc_time.clone().map(|x| x.into()), - self.gps_vel.clone().map(|x| x.into()), - ] - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(sensor) => match sensor.data { - messages::sensor::SensorData::Air(air_data) => { - self.air = Some(air_data); - } - messages::sensor::SensorData::EkfNav1(nav1_data) => { - self.ekf_nav.0 = Some(nav1_data); - } - messages::sensor::SensorData::EkfNav2(nav2_data) => { - self.ekf_nav.1 = Some(nav2_data); - } - messages::sensor::SensorData::EkfQuat(quat_data) => { - self.ekf_quat = Some(quat_data); - } - messages::sensor::SensorData::GpsVel(gps_vel_data) => { - self.gps_vel = Some(gps_vel_data); - } - messages::sensor::SensorData::Imu1(imu1_data) => { - self.imu.0 = Some(imu1_data); - } - messages::sensor::SensorData::Imu2(imu2_data) => { - self.imu.1 = Some(imu2_data); - } - messages::sensor::SensorData::UtcTime(utc_time_data) => { - self.utc_time = Some(utc_time_data); - } - }, - _ => {} - } - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} +use defmt::info; +use messages::command::{Command, CommandData, RadioRate, RadioRateChange}; +use messages::sensor::{ + Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, +}; +use messages::state::{State, StateData}; +use messages::Message; + +#[derive(Clone)] +pub struct DataManager { + pub air: Option, + pub ekf_nav: (Option, Option), + pub ekf_quat: Option, + pub imu: (Option, Option), + pub utc_time: Option, + pub gps_vel: Option, + pub gps_pos: (Option, Option), + pub state: Option, + pub logging_rate: Option, +} + +impl DataManager { + pub fn new() -> Self { + Self { + air: None, + ekf_nav: (None, None), + ekf_quat: None, + imu: (None, None), + utc_time: None, + gps_vel: None, + gps_pos: (None, 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; + } + + pub fn clone_sensors(&self) -> [Option; 10] { + [ + self.air.clone().map(|x| x.into()), + self.ekf_nav.0.clone().map(|x| x.into()), + self.ekf_nav.1.clone().map(|x| x.into()), + self.ekf_quat.clone().map(|x| x.into()), + self.imu.0.clone().map(|x| x.into()), + self.imu.1.clone().map(|x| x.into()), + self.utc_time.clone().map(|x| x.into()), + self.gps_vel.clone().map(|x| x.into()), + self.gps_pos.0.clone().map(|x| x.into()), + self.gps_pos.1.clone().map(|x| x.into()), + ] + } + pub fn clone_states(&self) -> [Option; 1] { + [self.state.clone()] + } + pub fn handle_data(&mut self, data: Message) { + match data.data { + messages::Data::Sensor(sensor) => match sensor.data { + messages::sensor::SensorData::Air(air_data) => { + self.air = Some(air_data); + } + messages::sensor::SensorData::EkfNav1(nav1_data) => { + self.ekf_nav.0 = Some(nav1_data); + } + messages::sensor::SensorData::EkfNav2(nav2_data) => { + self.ekf_nav.1 = Some(nav2_data); + } + messages::sensor::SensorData::EkfQuat(quat_data) => { + self.ekf_quat = Some(quat_data); + } + messages::sensor::SensorData::GpsVel(gps_vel_data) => { + self.gps_vel = Some(gps_vel_data); + } + messages::sensor::SensorData::Imu1(imu1_data) => { + self.imu.0 = Some(imu1_data); + } + messages::sensor::SensorData::Imu2(imu2_data) => { + self.imu.1 = Some(imu2_data); + } + messages::sensor::SensorData::UtcTime(utc_time_data) => { + self.utc_time = Some(utc_time_data); + } + messages::sensor::SensorData::GpsPos1(gps) => { + self.gps_pos.0 = Some(gps); + } + messages::sensor::SensorData::GpsPos2(gps) => { + self.gps_pos.1 = Some(gps); + } + _ => { + info!("impl power related"); + } + }, + 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); + } + _ => {} + }, + _ => { + info!("unkown"); + } + } + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/communication/src/main.rs b/boards/communication/src/main.rs index ec91c344..897aebd5 100644 --- a/boards/communication/src/main.rs +++ b/boards/communication/src/main.rs @@ -1,205 +1,302 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_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::*; -use communication::Capacities; -use communication::RadioDevice; -use data_manager::DataManager; - -use hal::gpio::Pins; -use hal::gpio::PA14; -use hal::gpio::{Pin, PushPullOutput}; -use hal::prelude::*; -use mcan::messageram::SharedMemory; -use messages::sensor::Sensor; -use messages::*; -use panic_halt as _; -use systick_monotonic::*; -use types::*; - -#[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: communication::CanDevice0, - } - - #[local] - struct Local { - led: Pin, - radio: RadioDevice, - } - - #[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 (_, _, _, 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, - ); - - /* Status LED */ - let led = pins.pa14.into_push_pull_output(); - - /* Spawn tasks */ - sensor_send::spawn().ok(); - blink::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().0); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can0, - }, - Local { led, radio }, - 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(()) - }); - } - - 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, local = [radio], shared = [&em])] - fn send_gs(cx: send_gs::Context, m: Message) { - cx.shared.em.run(|| { - cx.local.radio.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_gs, msg.clone())?; - } - Ok(()) - }); - spawn_after!(sensor_send, 250.millis()).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, 200.millis())?; - } else { - spawn_after!(blink, 1.secs())?; - } - Ok(()) - }); - } -} +#![no_std] +#![no_main] + +mod communication; +mod data_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; +use atsamd_hal::dmac::DmaController; +use common_arm::mcan; +use common_arm::SdManager; +use common_arm::*; +use communication::Capacities; +use communication::{RadioDevice, RadioManager}; +use data_manager::DataManager; +// use communication::radio_dma; + +use defmt::{flush, info}; +use hal::gpio::Pins; +use hal::gpio::{Alternate, 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 heapless::Vec; +use mcan::messageram::SharedMemory; +use messages::command::RadioRate; +use messages::sensor::Sensor; +use messages::state::State; +use messages::*; +use panic_halt as _; +use systick_monotonic::*; +use types::*; + +#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] +mod app { + use hal::gpio::Output; + + use super::*; + + #[shared] + struct Shared { + em: ErrorManager, + data_manager: DataManager, + can0: communication::CanDevice0, + } + + #[local] + struct Local { + led: Pin, + radio: RadioDevice, + sd_manager: SdManager< + Spi< + Config< + Pads< + hal::pac::SERCOM4, + IoSet1, + Pin>, + Pin>, + Pin>, + >, + >, + Duplex, + >, + Pin>, + >, + radio_manager: RadioManager, + } + + #[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 (_, _, _, 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 dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); + let (radio, gclk0, xfer) = RadioDevice::new( + tokens.pclks.sercom5, + &mclk, + peripherals.SERCOM5, + pins.pb17, + pins.pb16, + gclk0, + dmaCh0, + ); + + let radio_manager = RadioManager::new(xfer); + + /* 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()); + + /* Status LED */ + let led = pins.pa05.into_push_pull_output(); + + /* Spawn tasks */ + sensor_send::spawn().ok(); + state_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(), + can0, + }, + Local { + led, + radio, + sd_manager, + radio_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(()) + }); + } + + /// Probably should use this ( ﾉ^ω^)ﾉ + 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, local = [radio], shared = [&em])] + fn send_gs(cx: send_gs::Context, m: Message) { + cx.shared.em.run(|| { + cx.local.radio.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 (sensor_data, logging_rate) = cx.shared.data_manager.lock(|data_manager| { + ( + data_manager.clone_sensors(), + data_manager.get_logging_rate(), + ) + }); + + 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_gs, msg.clone())?; + spawn!(sd_dump, msg)?; + } + 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 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 index 1a7b0f0a..4cdaf702 100644 --- a/boards/communication/src/types.rs +++ b/boards/communication/src/types.rs @@ -1,19 +1,26 @@ -use atsamd_hal as hal; -use atsamd_hal::gpio::*; -use atsamd_hal::sercom::uart::EightBit; - -use atsamd_hal::sercom::{uart, IoSet1}; -use hal::sercom::Sercom5; -use messages::sender::Sender; -use messages::sender::Sender::CommunicationBoard; - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = CommunicationBoard; - -// ------- -// Ground Station -// ------- -pub type GroundStationPads = uart::PadsFromIds; -pub type GroundStationUartConfig = uart::Config; +use atsamd_hal::{gpio::*, dmac}; +use atsamd_hal::sercom::uart::EightBit; +use atsamd_hal::sercom::{spi, Sercom4}; +use atsamd_hal::sercom::{Sercom5, Sercom1, uart, IoSet1}; +use messages::sender::Sender; +use messages::sender::Sender::CommunicationBoard; +use embedded_sdmmc as sd; +use atsamd_hal::dmac::BufferPair; +use atsamd_hal::sercom::uart::Uart; + +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = CommunicationBoard; + +// ------- +// Ground Station +// ------- +pub type GroundStationPads = uart::PadsFromIds; +pub type GroundStationUartConfig = uart::Config; + +pub type RadioTransfer = dmac::Transfer< + dmac::Channel, + BufferPair, RadioBuffer>, +>; +pub type RadioBuffer = &'static mut [u8; 255]; \ No newline at end of file diff --git a/boards/power/Cargo.toml b/boards/power/Cargo.toml new file mode 100644 index 00000000..73764080 --- /dev/null +++ b/boards/power/Cargo.toml @@ -0,0 +1,21 @@ +[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" +panic-halt = "0.2.0" +systick-monotonic = "1.0.1" +defmt = "0.3.2" +postcard = "1.0.2" +heapless = "0.7.16" +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 new file mode 100644 index 00000000..3e11ed29 --- /dev/null +++ b/boards/power/src/communication.rs @@ -0,0 +1,174 @@ +use atsamd_hal as hal; +use hal::can::Dependencies; +use hal::pac::CAN0; +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::typelevel::Increment; +use common_arm::mcan; +use mcan::message::{rx, Raw}; +use mcan::tx_buffers::DynTx; +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::messageram::SharedMemory; +use mcan::{ + config::{BitTiming, Mode}, + filter::{Action, Filter}, +}; +use common_arm::HydraError; +use messages::Message; +use heapless::Vec; +use defmt::info; +use crate::data_manager::DataManager; +use systick_monotonic::fugit::RateExtU32; +use typenum::{U0, U128, U32, U64}; +use postcard::from_bytes; +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::sender::Sender::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) + } + } + } + } + _ => (), + } + } + } +} \ No newline at end of file diff --git a/boards/power/src/data_manager.rs b/boards/power/src/data_manager.rs new file mode 100644 index 00000000..b7cdb6d3 --- /dev/null +++ b/boards/power/src/data_manager.rs @@ -0,0 +1,47 @@ +// import messages +use messages::sensor::{Current, Voltage, Regulator, Temperature, SensorData}; +use messages::Message; + +pub struct DataManager { + pub regulator: [Option; 4], + pub voltage: [Option; 4], + pub current: Option, + pub temperature: [Option; 4], +} + +impl DataManager { + pub fn new() -> Self { + Self { + regulator: [None, None, None, None], + voltage: [None, None, None, None], + current: None, + temperature: [None, None, None, None], + } + } + pub fn clone_power(&self) -> [Option; 13] { + [ + self.regulator[0].clone().map(|x| x.into()), + self.regulator[1].clone().map(|x| x.into()), + self.regulator[2].clone().map(|x| x.into()), + self.regulator[3].clone().map(|x| x.into()), + self.voltage[0].clone().map(|x| x.into()), + self.voltage[1].clone().map(|x| x.into()), + self.voltage[2].clone().map(|x| x.into()), + self.voltage[3].clone().map(|x| x.into()), + self.current.clone().map(|x| x.into()), + self.temperature[0].clone().map(|x| x.into()), + self.temperature[1].clone().map(|x| x.into()), + self.temperature[2].clone().map(|x| x.into()), + self.temperature[3].clone().map(|x| x.into()), + ] + } + pub fn handle_data(&mut self, data: Message) { + // to do + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} \ No newline at end of file diff --git a/boards/power/src/main.rs b/boards/power/src/main.rs new file mode 100644 index 00000000..c2573639 --- /dev/null +++ b/boards/power/src/main.rs @@ -0,0 +1,152 @@ +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod types; + +use atsamd_hal as hal; +use hal::clock::v2::pclk::Pclk; +use hal::clock::v2::Source; +use hal::prelude::*; +use hal::gpio::{PB16, PushPullOutput, Pin, PB17, Pins}; +use common_arm::*; +use common_arm::mcan; +use mcan::messageram::SharedMemory; +use communication::Capacities; +use communication::CanDevice0; +use systick_monotonic::*; +use panic_halt as _; +use crate::data_manager::DataManager; + +#[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(()) + }); + } +} \ No newline at end of file diff --git a/boards/power/src/types.rs b/boards/power/src/types.rs new file mode 100644 index 00000000..cf6f8db0 --- /dev/null +++ b/boards/power/src/types.rs @@ -0,0 +1,5 @@ +use messages::sender::{Sender, Sender::PowerBoard}; +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = PowerBoard; \ No newline at end of file diff --git a/boards/recovery/src/communication.rs b/boards/recovery/src/communication.rs index 8a6f2486..e30867da 100644 --- a/boards/recovery/src/communication.rs +++ b/boards/recovery/src/communication.rs @@ -1,187 +1,187 @@ -/// Encapsulates all communication logic. -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::mcan::message::{rx, Raw}; -use common_arm::mcan::tx_buffers::DynTx; -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::messageram::SharedMemory; -use mcan::{ - config::{BitTiming, Mode}, - filter::{Action, Filter}, -}; - -use crate::data_manager::DataManager; -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); - - // We accept messages from the sensor board as we need data from the sensors. - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::SensorBoard.into()) - .unwrap(), - mask: ecan::StandardId::MAX, - }) - .unwrap_or_else(|_| panic!("Sensor Board filter")); - - // We accept messages from the communication board as we may need to force the deployment of the parachute. - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::sender::Sender::CommunicationBoard.into()) - .unwrap(), - mask: ecan::StandardId::MAX, - }) - .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) - } - } - } - } - _ => (), - } - } - } -} +/// Encapsulates all communication logic. +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::mcan::message::{rx, Raw}; +use common_arm::mcan::tx_buffers::DynTx; +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::messageram::SharedMemory; +use mcan::{ + config::{BitTiming, Mode}, + filter::{Action, Filter}, +}; + +use crate::data_manager::DataManager; +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); + + // We accept messages from the sensor board as we need data from the sensors. + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo0, + filter: ecan::StandardId::new(messages::sender::Sender::SensorBoard.into()) + .unwrap(), + mask: ecan::StandardId::ZERO, + }) + .unwrap_or_else(|_| panic!("Sensor Board filter")); + + // We accept messages from the communication board as we may need to force the deployment of the parachute. + can.filters_standard() + .push(Filter::Classic { + action: Action::StoreFifo1, + filter: ecan::StandardId::new(messages::sender::Sender::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/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs index 3fc9c018..3125ed42 100644 --- a/boards/recovery/src/data_manager.rs +++ b/boards/recovery/src/data_manager.rs @@ -1,113 +1,185 @@ -use crate::state_machine::RocketStates; -use heapless::HistoryBuffer; -use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime}; -use messages::Message; - -const MAIN_HEIGHT: f32 = 450.0; -const VELOCITY_MIN: f32 = 20.0; -const HEIGHT_MIN: f32 = 300.0; - -pub struct DataManager { - pub air: Option, - pub ekf_nav: (Option, Option), - pub ekf_quat: Option, - pub imu: (Option, Option), - pub utc_time: Option, - pub gps_vel: Option, - pub historical_pressure: HistoryBuffer, - pub current_state: Option, -} - -impl DataManager { - pub fn new() -> Self { - let historical_pressure = HistoryBuffer::new(); - Self { - air: None, - ekf_nav: (None, None), - ekf_quat: None, - imu: (None, None), - utc_time: None, - gps_vel: None, - historical_pressure, - current_state: None, - } - } - /// Returns true if the rocket has passed apogee. - /// This is determined by looking at the historical pressure data. - /// Furthermore, we only start checking pressure data when velocity is less than 20m/s - /// because we want to avoid the complexities of pressure during transonic flight. - pub fn is_falling(&self) -> bool { - let ekf_nav1 = self.ekf_nav.0.as_ref(); - if let Some(ekf_nav1) = ekf_nav1 { - if ekf_nav1.velocity[2] > VELOCITY_MIN { - return false; - } - } - match self.historical_pressure.recent() { - Some(mut point_previous) => { - for i in self.historical_pressure.oldest_ordered() { - let slope = i - point_previous; - if slope > 0.0 { - return false; - } - point_previous = i; - } - } - None => { - return false; - } - } - true - } - pub fn is_launched(&self) -> bool { - match self.air.as_ref() { - Some(air) => air.altitude > HEIGHT_MIN, - None => false, - } - } - pub fn is_below_main(&self) -> bool { - match self.air.as_ref() { - Some(air) => air.altitude < MAIN_HEIGHT, - None => false, - } - } - pub fn handle_data(&mut self, data: Message) { - match data.data { - messages::Data::Sensor(sensor) => match sensor.data { - messages::sensor::SensorData::Air(air_data) => { - let pressure = air_data.pressure_abs; - self.air = Some(air_data); - self.historical_pressure.write(pressure); - } - messages::sensor::SensorData::EkfNav1(nav1_data) => { - self.ekf_nav.0 = Some(nav1_data); - } - messages::sensor::SensorData::EkfNav2(nav2_data) => { - self.ekf_nav.1 = Some(nav2_data); - } - messages::sensor::SensorData::EkfQuat(quat_data) => { - self.ekf_quat = Some(quat_data); - } - messages::sensor::SensorData::GpsVel(gps_vel_data) => { - self.gps_vel = Some(gps_vel_data); - } - messages::sensor::SensorData::Imu1(imu1_data) => { - self.imu.0 = Some(imu1_data); - } - messages::sensor::SensorData::Imu2(imu2_data) => { - self.imu.1 = Some(imu2_data); - } - messages::sensor::SensorData::UtcTime(utc_time_data) => { - self.utc_time = Some(utc_time_data); - } - }, - _ => {} - } - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} +use crate::state_machine::RocketStates; +use crate::app::{fire_drogue, fire_main}; +use common_arm::spawn; +use heapless::HistoryBuffer; +use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime}; +use messages::Message; +use defmt::{info}; + +const MAIN_HEIGHT: f32 = 876.0; // meters ASL +const HEIGHT_MIN: f32 = 600.0; // meters ASL + +pub struct DataManager { + pub air: Option, + pub ekf_nav: (Option, Option), + pub ekf_quat: Option, + pub imu: (Option, Option), + pub utc_time: Option, + pub gps_vel: Option, + pub historical_barometer_altitude: HistoryBuffer<(f32, u32), 8>, + pub current_state: Option, +} + +impl DataManager { + pub fn new() -> Self { + let historical_barometer_altitude = HistoryBuffer::new(); + Self { + air: None, + ekf_nav: (None, None), + ekf_quat: None, + imu: (None, None), + utc_time: None, + gps_vel: None, + historical_barometer_altitude, + current_state: None, + } + } + /// Returns true if the rocket is descending + pub fn is_falling(&self) -> bool { + if self.historical_barometer_altitude.len() < 8 { + 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 / 1_000_000.0; + if time_diff == 0.0 { + continue; + } + let slope = (i.0 - prev.0)/time_diff; + if slope < -100.0 { + return false; + } + avg_sum += slope; + prev = i; + } + match avg_sum / 7.0 { // 7 because we have 8 points. + // exclusive range + x if !(-100.0..=-5.0).contains(&x) => { + return false; + } + _ => { + info!("avg: {}", avg_sum / 7.0); + } + } + } + None => { + return false; + } + } + true + } + pub fn is_launched(&self) -> bool { + match self.air.as_ref() { + Some(air) => air.altitude > HEIGHT_MIN, + None => false, + } + } + pub fn is_landed(&self) -> bool { + if self.historical_barometer_altitude.len() < 8 { + 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 / 1_000_000.0; + if time_diff == 0.0 { + continue; + } + avg_sum += (i.0 - prev.0)/time_diff; + prev = i; + } + match avg_sum / 7.0 { + // inclusive range + x if (-4.0..=4.0).contains(&x) => { + return true; + } + _ => { + // continue + } + } + } + None => { + return false; + } + } + false + } + pub fn is_below_main(&self) -> bool { + match self.air.as_ref() { + Some(air) => air.altitude < MAIN_HEIGHT, + None => false, + } + } + pub fn set_state(&mut self, state: RocketStates) { + self.current_state = Some(state); + } + pub fn handle_data(&mut self, data: Message) { + match data.data { + messages::Data::Sensor(sensor) => match sensor.data { + messages::sensor::SensorData::Air(air_data) => { + let tup_data = (air_data.altitude, air_data.time_stamp); + self.air = Some(air_data); + if let Some(recent) = self.historical_barometer_altitude.recent() { + if recent.1 != tup_data.1 { + self.historical_barometer_altitude.write(tup_data); + } + } else { + self.historical_barometer_altitude.write(tup_data); + } + } + messages::sensor::SensorData::EkfNav1(nav1_data) => { + self.ekf_nav.0 = Some(nav1_data); + } + messages::sensor::SensorData::EkfNav2(nav2_data) => { + self.ekf_nav.1 = Some(nav2_data); + } + messages::sensor::SensorData::EkfQuat(quat_data) => { + self.ekf_quat = Some(quat_data); + } + messages::sensor::SensorData::GpsVel(gps_vel_data) => { + self.gps_vel = Some(gps_vel_data); + } + messages::sensor::SensorData::Imu1(imu1_data) => { + self.imu.0 = Some(imu1_data); + } + messages::sensor::SensorData::Imu2(imu2_data) => { + self.imu.1 = Some(imu2_data); + } + messages::sensor::SensorData::UtcTime(utc_time_data) => { + self.utc_time = Some(utc_time_data); + }, + _ => { + } + }, + messages::Data::Command(command) => match command.data { + messages::command::CommandData::DeployDrogue(drogue) => { + spawn!(fire_drogue); + }, + messages::command::CommandData::DeployMain(main) => { + spawn!(fire_main); + }, + messages::command::CommandData::PowerDown(_) => { + // don't handle for now. + }, + _ => { + + } + } + _ => { + } + } + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/recovery/src/gpio_manager.rs b/boards/recovery/src/gpio_manager.rs new file mode 100644 index 00000000..a26897ec --- /dev/null +++ b/boards/recovery/src/gpio_manager.rs @@ -0,0 +1,30 @@ +use atsamd_hal::gpio::{Pin, PushPullOutput, PA09, PA06}; +use atsamd_hal::prelude::*; + +pub struct GPIOManager { + main_ematch: Pin, + drogue_ematch: Pin, +} + +impl GPIOManager { + pub fn new(mut main_ematch: Pin, mut drogue_ematch: Pin) -> Self { + drogue_ematch.set_low().ok(); + main_ematch.set_low().ok(); + Self { + main_ematch, + drogue_ematch, + } + } + pub fn fire_drogue(&mut self) { + self.drogue_ematch.set_high().ok(); + } + pub fn fire_main(&mut self) { + self.main_ematch.set_high().ok(); + } + pub fn close_drogue(&mut self) { + self.drogue_ematch.set_low().ok(); + } + pub fn close_main(&mut self) { + self.main_ematch.set_low().ok(); + } +} \ No newline at end of file diff --git a/boards/recovery/src/main.rs b/boards/recovery/src/main.rs index df90d922..9cecc3d3 100644 --- a/boards/recovery/src/main.rs +++ b/boards/recovery/src/main.rs @@ -1,190 +1,229 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod state_machine; -mod types; - -use crate::state_machine::RocketStates; -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::*; -use communication::Capacities; -use data_manager::DataManager; -use hal::gpio::{Pin, Pins, PushPullOutput, PA14}; -use hal::prelude::*; -use mcan::messageram::SharedMemory; -use messages::*; -use panic_halt as _; -use state_machine::{StateMachine, StateMachineContext}; -use systick_monotonic::*; -use types::GPIOController; -use types::COM_ID; - -#[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: communication::CanDevice0, - gpio: GPIOController, - } - - #[local] - struct Local { - led: Pin, - state_machine: StateMachine, - } - - #[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(); - - /* 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 (_, _, _, _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, - ); - - /* GPIO config */ - let led = pins.pa14.into_push_pull_output(); - let gpio = GPIOController::new( - pins.pa18.into_push_pull_output(), - pins.pa19.into_push_pull_output(), - ); - /* State Machine config */ - let state_machine = StateMachine::new(); - - /* Spawn tasks */ - run_sm::spawn().ok(); - state_send::spawn().ok(); - blink::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().0); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can0, - gpio, - }, - Local { led, state_machine }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - /// Runs the state machine. - /// This takes control of the shared resources. - #[task(priority = 3, local = [state_machine], shared = [can0, gpio, data_manager])] - fn run_sm(mut cx: run_sm::Context) { - cx.local.state_machine.run(&mut StateMachineContext { - shared_resources: &mut cx.shared, - }) - } - - /// 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, 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(()) - }); - } - - /// Sends info about the current state of the system. - #[task(shared = [data_manager, &em])] - fn state_send(mut cx: state_send::Context) { - let em_error = cx.shared.em.has_error(); - cx.shared.em.run(|| { - let rocket_state = cx - .shared - .data_manager - .lock(|data| data.current_state.clone()); - let state = if let Some(rocket_state) = rocket_state { - rocket_state - } else { - RocketStates::Initializing(state_machine::Initializing {}) - }; - let board_state = messages::State { - status: state.into(), - has_error: em_error, - }; - let message = Message::new(&monotonics::now(), COM_ID, board_state); - spawn!(send_internal, message)?; - spawn_after!(state_send, 5.secs())?; - 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, 200.millis())?; - } else { - spawn_after!(blink, 1.secs())?; - } - Ok(()) - }); - } -} +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod gpio_manager; +mod state_machine; +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::*; +use communication::Capacities; +use data_manager::DataManager; +use gpio_manager::GPIOManager; +use hal::gpio::{Pin, Pins, PushPullOutput, PB16, PB17}; +use hal::prelude::*; +use mcan::messageram::SharedMemory; +use messages::*; +use panic_halt as _; +use state_machine::{StateMachine, StateMachineContext}; +use systick_monotonic::*; +use types::COM_ID; +use defmt::info; + +#[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: communication::CanDevice0, + gpio: GPIOManager, + } + + #[local] + struct Local { + led_green: Pin, + led_red: Pin, + state_machine: StateMachine, + } + + #[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(); + + /* 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 (_, _, _, _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, + ); + + /* 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(), + ); + /* State Machine config */ + let state_machine = StateMachine::new(); + + /* Spawn tasks */ + run_sm::spawn().ok(); + state_send::spawn().ok(); + blink::spawn().ok(); + // fire_main::spawn_after(ExtU64::secs(15)).ok(); + // fire_drogue::spawn_after(ExtU64::secs(15)).ok(); + + /* Monotonic clock */ + let mono = Systick::new(core.SYST, gclk0.freq().to_Hz()); + + ( + Shared { + em: ErrorManager::new(), + data_manager: DataManager::new(), + can0, + gpio, + }, + Local {led_green, led_red, state_machine}, + init::Monotonics(mono), + ) + } + + /// Idle task for when no other tasks are running. + #[idle] + fn idle(_: idle::Context) -> ! { + loop {} + } + + #[task(priority = 3, local = [fired: bool = false], shared=[gpio])] + fn fire_drogue(mut cx: fire_drogue::Context) { + if !(*cx.local.fired) { + cx.shared.gpio.lock(|gpio| { + gpio.fire_drogue(); + *cx.local.fired = true; + }); + spawn_after!(fire_drogue, ExtU64::secs(5)); + } else { + cx.shared.gpio.lock(|gpio| { + gpio.close_drogue(); + }); + } + } + + #[task(priority = 3, local = [fired: bool = false], shared=[gpio])] + fn fire_main(mut cx: fire_main::Context) { + if !(*cx.local.fired) { + cx.shared.gpio.lock(|gpio| { + gpio.fire_main(); + *cx.local.fired = true; + }); + spawn_after!(fire_main, ExtU64::secs(5)); + } else { + cx.shared.gpio.lock(|gpio| { + gpio.close_main(); + }); + } + } + + /// Runs the state machine. + /// This takes control of the shared resources. + #[task(priority = 3, local = [state_machine], shared = [can0, gpio, data_manager, &em])] + fn run_sm(mut cx: run_sm::Context) { + cx.local.state_machine.run(&mut StateMachineContext { + shared_resources: &mut cx.shared, + }); + cx.shared.data_manager.lock(|data| { + data.set_state(cx.local.state_machine.get_state()); + }); + spawn_after!(run_sm, ExtU64::millis(500)).ok(); + } + + /// Handles the CAN0 interrupt. + #[task(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, 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(()) + }); + } + + /// Sends info about the current state of the system. + #[task(shared = [data_manager, &em])] + fn state_send(mut cx: state_send::Context) { + cx.shared.em.run(|| { + let rocket_state = cx + .shared + .data_manager + .lock(|data| data.current_state.clone()); + let state = if let Some(rocket_state) = rocket_state { + rocket_state + } else { + // This isn't really an error, we just don't have data yet. + 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)?; + Ok(()) + }); + spawn_after!(state_send, ExtU64::secs(2)); + } + + /// 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/recovery/src/state_machine/mod.rs b/boards/recovery/src/state_machine/mod.rs index c86f3c7a..5adef76e 100644 --- a/boards/recovery/src/state_machine/mod.rs +++ b/boards/recovery/src/state_machine/mod.rs @@ -1,105 +1,118 @@ -mod black_magic; -mod states; - -use messages::Status; -use crate::communication::CanDevice0; -use crate::data_manager::DataManager; -use crate::state_machine::states::*; -use crate::GPIOController; -pub use black_magic::*; -pub use states::Initializing; -use core::fmt::Debug; -use defmt::Format; -use enum_dispatch::enum_dispatch; -use rtic::Mutex; - -pub trait StateMachineSharedResources { - fn lock_can(&mut self, f: &dyn Fn(&mut CanDevice0)); - fn lock_data_manager(&mut self, f: &dyn Fn(&mut DataManager)); - fn lock_gpio(&mut self, f: &dyn Fn(&mut GPIOController)); -} - -impl<'a> StateMachineSharedResources for crate::app::__rtic_internal_run_smSharedResources<'a> { - fn lock_can(&mut self, fun: &dyn Fn(&mut CanDevice0)) { - self.can0.lock(fun) - } - fn lock_data_manager(&mut self, fun: &dyn Fn(&mut DataManager)) { - self.data_manager.lock(fun) - } - fn lock_gpio(&mut self, fun: &dyn Fn(&mut GPIOController)) { - self.gpio.lock(fun) - } -} - -pub struct StateMachineContext<'a, 'b> { - pub shared_resources: &'b mut crate::app::__rtic_internal_run_smSharedResources<'a> -} -pub struct StateMachine { - state: RocketStates, -} - -// Define some functions to interact with the state machine -impl StateMachine { - pub fn new() -> Self { - let state = Initializing {}; - - StateMachine { - state: state.into(), - } - } - - pub fn run(&mut self, context: &mut StateMachineContext) { - if let Some(new_state) = self.state.step(context) { - self.state.exit(); - new_state.enter(context); - self.state = new_state; - } - } -} - -// All events are found here -pub enum RocketEvents { - DeployDrogue, - DeployMain, -} - -// All states are defined here. Another struct must be defined for the actual state, and that struct -// must implement the State trait -#[enum_dispatch(State)] -#[derive(Debug, Format, Clone)] -pub enum RocketStates { - Initializing, - WaitForTakeoff, - Ascent, - Apogee, - Landed, - Abort, -} - -// Not ideal, but it works for now. -// Should be able to put this is a shared library, but as of now, I can't figure out how to do that. -impl From for RocketStates { - fn from(state: messages::Status) -> Self { - match state { - Status::Initializing => RocketStates::Initializing(Initializing {}), - Status::WaitForTakeoff => RocketStates::WaitForTakeoff(WaitForTakeoff {}), - Status::Ascent => RocketStates::Ascent(Ascent {}), - Status::Apogee => RocketStates::Apogee(Apogee {}), - Status::Landed => RocketStates::Landed(Landed {}), - Status::Abort => RocketStates::Abort(Abort {}), - } - } -} - -impl Into for RocketStates { - fn into(self) -> Status { - match self { - RocketStates::Initializing(_) => Status::Initializing, - RocketStates::WaitForTakeoff(_) => Status::WaitForTakeoff, - RocketStates::Ascent(_) => Status::Ascent, - RocketStates::Apogee(_) => Status::Apogee, - RocketStates::Landed(_) => Status::Landed, - RocketStates::Abort(_) => Status::Abort, - } - } +mod black_magic; +mod states; + +use messages::state; +use crate::communication::CanDevice0; +use crate::data_manager::DataManager; +use crate::state_machine::states::*; +use crate::gpio_manager::GPIOManager; +pub use black_magic::*; +pub use states::Initializing; +use core::fmt::Debug; +use defmt::Format; +use enum_dispatch::enum_dispatch; +use rtic::Mutex; + +pub trait StateMachineSharedResources { + fn lock_can(&mut self, f: &dyn Fn(&mut CanDevice0)); + fn lock_data_manager(&mut self, f: &dyn Fn(&mut DataManager)); + fn lock_gpio(&mut self, f: &dyn Fn(&mut GPIOManager)); +} + +impl<'a> StateMachineSharedResources for crate::app::__rtic_internal_run_smSharedResources<'a> { + fn lock_can(&mut self, fun: &dyn Fn(&mut CanDevice0)) { + self.can0.lock(fun) + } + fn lock_data_manager(&mut self, fun: &dyn Fn(&mut DataManager)) { + self.data_manager.lock(fun) + } + fn lock_gpio(&mut self, fun: &dyn Fn(&mut GPIOManager)) { + self.gpio.lock(fun) + } +} + +pub struct StateMachineContext<'a, 'b> { + pub shared_resources: &'b mut crate::app::__rtic_internal_run_smSharedResources<'a> +} +pub struct StateMachine { + state: RocketStates, +} + +// Define some functions to interact with the state machine +impl StateMachine { + pub fn new() -> Self { + let state = Initializing {}; + + StateMachine { + state: state.into(), + } + } + + pub fn run(&mut self, context: &mut StateMachineContext) { + if let Some(new_state) = self.state.step(context) { + self.state.exit(); + new_state.enter(context); + self.state = new_state; + } + } + + pub fn get_state(&self) -> RocketStates { + self.state.clone() + } +} + +impl Default for StateMachine { + fn default() -> Self { + Self::new() + } +} + +// All events are found here +pub enum RocketEvents { + DeployDrogue, + DeployMain, +} + +// All states are defined here. Another struct must be defined for the actual state, and that struct +// must implement the State trait +#[enum_dispatch(State)] +#[derive(Debug, Format, Clone)] +pub enum RocketStates { + Initializing, + WaitForTakeoff, + Ascent, + Descent, + TerminalDescent, + WaitForRecovery, + Abort, +} + +// Not a fan of this. +// Should be able to put this is a shared library. +impl From for RocketStates { + fn from(state: state::StateData) -> Self { + match state { + state::StateData::Initializing => RocketStates::Initializing(Initializing {}), + state::StateData::WaitForTakeoff => RocketStates::WaitForTakeoff(WaitForTakeoff {}), + state::StateData::Ascent => RocketStates::Ascent(Ascent {}), + state::StateData::Descent => RocketStates::Descent(Descent {}), + state::StateData::TerminalDescent => RocketStates::TerminalDescent(TerminalDescent { } ), + state::StateData::WaitForRecovery => RocketStates::WaitForTakeoff(WaitForTakeoff { }), + state::StateData::Abort => RocketStates::Abort(Abort {}), + } + } +} +// Linter: an implementation of From is preferred since it gives you Into<_> for free where the reverse isn't true +impl Into for RocketStates { + fn into(self) -> state::StateData { + match self { + RocketStates::Initializing(_) => state::StateData::Initializing, + RocketStates::WaitForTakeoff(_) => state::StateData::WaitForTakeoff, + RocketStates::Ascent(_) => state::StateData::Ascent, + RocketStates::Descent(_) => state::StateData::Descent, + RocketStates::TerminalDescent(_) => state::StateData::TerminalDescent, + RocketStates::WaitForRecovery(_) => state::StateData::WaitForRecovery, + RocketStates::Abort(_) => state::StateData::Abort, + } + } } \ No newline at end of file diff --git a/boards/recovery/src/state_machine/states/apogee.rs b/boards/recovery/src/state_machine/states/apogee.rs deleted file mode 100644 index 59d2981d..00000000 --- a/boards/recovery/src/state_machine/states/apogee.rs +++ /dev/null @@ -1,38 +0,0 @@ -use super::Ascent; -use crate::state_machine::{Landed, RocketStates, State, StateMachineContext, TransitionInto}; -use crate::{no_transition, transition}; -use rtic::mutex::Mutex; -use defmt::{write, Format, Formatter}; - -#[derive(Debug, Clone)] -pub struct Apogee {} - -impl State for Apogee { - fn enter(&self, context: &mut StateMachineContext) { - context.shared_resources.gpio.lock(|gpio| gpio.fire_drogue()); - } - fn step(&mut self, context: &mut StateMachineContext) -> Option { - // is this 450 AGL? I could put these types of values in a top file like - // types.rs to make it easier to change. - context.shared_resources.data_manager.lock(|data| { - // Handle the case where we don't have any data yet - if data.is_below_main() { - transition!(self, Landed) - } else { - no_transition!() - } - }) - } -} - -impl TransitionInto for Ascent { - fn transition(&self) -> Apogee { - Apogee {} - } -} - -impl Format for Apogee { - fn format(&self, f: Formatter) { - write!(f, "Apogee") - } -} diff --git a/boards/recovery/src/state_machine/states/ascent.rs b/boards/recovery/src/state_machine/states/ascent.rs index 73138d8c..fdcb6f9b 100644 --- a/boards/recovery/src/state_machine/states/ascent.rs +++ b/boards/recovery/src/state_machine/states/ascent.rs @@ -1,36 +1,54 @@ -use crate::state_machine::states::apogee::Apogee; -use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; -use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; -use crate::{no_transition, transition}; -use defmt::{write, Format, Formatter}; -use rtic::mutex::Mutex; - -#[derive(Debug, Clone)] -pub struct Ascent {} - -impl State for Ascent { - fn step(&mut self, context: &mut StateMachineContext) -> Option { - context - .shared_resources - .data_manager - .lock(|data| { - if data.is_falling() { - transition!(self, Apogee) - } else { - no_transition!() - } - }) - } -} - -impl TransitionInto for WaitForTakeoff { - fn transition(&self) -> Ascent { - Ascent {} - } -} - -impl Format for Ascent { - fn format(&self, f: Formatter) { - write!(f, "Ascent") - } -} +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::{no_transition, transition}; +use messages::command::{Command, CommandData, PowerDown, RadioRateChange, RadioRate}; +use messages::Message; +use defmt::{write, Format, Formatter}; +use rtic::mutex::Mutex; +use crate::types::COM_ID; +use crate::app::monotonics; + +#[derive(Debug, Clone)] +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)); + 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| { + if data.is_falling() { + transition!(self, Descent) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for WaitForTakeoff { + fn transition(&self) -> Ascent { + Ascent {} + } +} + +impl Format for Ascent { + fn format(&self, f: Formatter) { + write!(f, "Ascent") + } +} diff --git a/boards/recovery/src/state_machine/states/descent.rs b/boards/recovery/src/state_machine/states/descent.rs new file mode 100644 index 00000000..61d7f7e6 --- /dev/null +++ b/boards/recovery/src/state_machine/states/descent.rs @@ -0,0 +1,37 @@ +use super::Ascent; +use crate::app::fire_drogue; +use crate::state_machine::{TerminalDescent, RocketStates, State, StateMachineContext, TransitionInto}; +use crate::{no_transition, transition}; +use rtic::mutex::Mutex; +use defmt::{write, Format, Formatter, info}; +use common_arm::spawn; + +#[derive(Debug, Clone)] +pub struct Descent {} + +impl State for Descent { + fn enter(&self, context: &mut StateMachineContext) { + spawn!(fire_drogue); + } + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context.shared_resources.data_manager.lock(|data| { + if data.is_below_main() { + transition!(self, TerminalDescent) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for Ascent { + fn transition(&self) -> Descent { + Descent {} + } +} + +impl Format for Descent { + fn format(&self, f: Formatter) { + write!(f, "Descent") + } +} diff --git a/boards/recovery/src/state_machine/states/initializing.rs b/boards/recovery/src/state_machine/states/initializing.rs index ce88f32a..b9adab77 100644 --- a/boards/recovery/src/state_machine/states/initializing.rs +++ b/boards/recovery/src/state_machine/states/initializing.rs @@ -1,21 +1,20 @@ -use defmt::{write, Format, Formatter}; - -use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; -use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; -use crate::transition; - -#[derive(Debug, Clone)] -pub struct Initializing {} - -impl State for Initializing { - fn step(&mut self, _context: &mut StateMachineContext) -> Option { - // Check that all systems are ok, then transition - transition!(self, WaitForTakeoff) - } -} - -impl Format for Initializing { - fn format(&self, f: Formatter) { - write!(f, "Initializing") - } -} +use defmt::{write, Format, Formatter}; + +use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use crate::transition; + +#[derive(Debug, Clone)] +pub struct Initializing {} + +impl State for Initializing { + fn step(&mut self, _context: &mut StateMachineContext) -> Option { + transition!(self, WaitForTakeoff) + } +} + +impl Format for Initializing { + fn format(&self, f: Formatter) { + write!(f, "Initializing") + } +} diff --git a/boards/recovery/src/state_machine/states/landed.rs b/boards/recovery/src/state_machine/states/landed.rs deleted file mode 100644 index d6e756ac..00000000 --- a/boards/recovery/src/state_machine/states/landed.rs +++ /dev/null @@ -1,30 +0,0 @@ -use defmt::{write, Format, Formatter}; - -use crate::no_transition; -use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; -use rtic::mutex::Mutex; -use super::Apogee; - -#[derive(Debug, Clone)] -pub struct Landed {} - -impl State for Landed { - fn enter(&self,context: &mut StateMachineContext) { - context.shared_resources.gpio.lock(|gpio| gpio.fire_main()); - } - fn step(&mut self, _context: &mut StateMachineContext) -> Option { - no_transition!() - } -} - -impl TransitionInto for Apogee { - fn transition(&self) -> Landed { - Landed {} - } -} - -impl Format for Landed { - fn format(&self, f: Formatter) { - write!(f, "Landed") - } -} diff --git a/boards/recovery/src/state_machine/states/mod.rs b/boards/recovery/src/state_machine/states/mod.rs index 14cf4411..f4d4c3fe 100644 --- a/boards/recovery/src/state_machine/states/mod.rs +++ b/boards/recovery/src/state_machine/states/mod.rs @@ -1,13 +1,15 @@ -mod initializing; -mod landed; -mod ascent; -mod apogee; -mod wait_for_takeoff; -mod abort; - -pub use crate::state_machine::states::apogee::Apogee; -pub use crate::state_machine::states::ascent::Ascent; -pub use crate::state_machine::states::initializing::Initializing; -pub use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; -pub use crate::state_machine::states::landed::Landed; +mod initializing; +mod terminal_descent; +mod ascent; +mod descent; +mod wait_for_takeoff; +mod wait_for_recovery; +mod abort; + +pub use crate::state_machine::states::descent::Descent; +pub use crate::state_machine::states::ascent::Ascent; +pub use crate::state_machine::states::initializing::Initializing; +pub use crate::state_machine::states::wait_for_takeoff::WaitForTakeoff; +pub use crate::state_machine::states::terminal_descent::TerminalDescent; +pub use crate::state_machine::states::wait_for_recovery::WaitForRecovery; pub use crate::state_machine::states::abort::Abort; \ No newline at end of file diff --git a/boards/recovery/src/state_machine/states/terminal_descent.rs b/boards/recovery/src/state_machine/states/terminal_descent.rs new file mode 100644 index 00000000..f5f02cda --- /dev/null +++ b/boards/recovery/src/state_machine/states/terminal_descent.rs @@ -0,0 +1,40 @@ +use defmt::{write, Format, Formatter, info}; +use crate::app::fire_main; +use crate::{no_transition, transition}; +use crate::state_machine::{WaitForRecovery, RocketStates, State, StateMachineContext, TransitionInto}; +use rtic::mutex::Mutex; +use super::Descent; +use common_arm::spawn; + +#[derive(Debug, Clone)] +pub struct TerminalDescent {} + +impl State for TerminalDescent { + fn enter(&self,context: &mut StateMachineContext) { + spawn!(fire_main); + } + fn step(&mut self, context: &mut StateMachineContext) -> Option { + context + .shared_resources + .data_manager + .lock(|data| { + if data.is_landed() { + transition!(self, WaitForRecovery) + } else { + no_transition!() + } + }) + } +} + +impl TransitionInto for Descent { + fn transition(&self) -> TerminalDescent { + TerminalDescent {} + } +} + +impl Format for TerminalDescent { + fn format(&self, f: Formatter) { + write!(f, "Terminal Descent") + } +} diff --git a/boards/recovery/src/state_machine/states/wait_for_recovery.rs b/boards/recovery/src/state_machine/states/wait_for_recovery.rs new file mode 100644 index 00000000..4f944a63 --- /dev/null +++ b/boards/recovery/src/state_machine/states/wait_for_recovery.rs @@ -0,0 +1,51 @@ +use super::TerminalDescent; +use crate::app::monotonics; +use crate::state_machine::{RocketStates, State, StateMachineContext, TransitionInto}; +use crate::types::COM_ID; +use crate::{no_transition, transition}; +use rtic::mutex::Mutex; +use defmt::{write, Format, Formatter, info}; +use messages::command::{Command, CommandData, PowerDown, RadioRateChange, RadioRate}; +use messages::Message; +use messages::sender::Sender::SensorBoard; + +#[derive(Debug, Clone)] +pub struct WaitForRecovery {} + +impl State for WaitForRecovery { + fn enter(&self, context: &mut StateMachineContext) { + // 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 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(()) + }) + }); + } + } + fn step(&mut self, context: &mut StateMachineContext) -> Option { + no_transition!() // this is our final resting place. We should also powerdown this board. + } +} + +impl TransitionInto for TerminalDescent { + fn transition(&self) -> WaitForRecovery { + WaitForRecovery {} + } +} + +impl Format for WaitForRecovery { + fn format(&self, f: Formatter) { + write!(f, "Descent") + } +} diff --git a/boards/recovery/src/types.rs b/boards/recovery/src/types.rs index 7097bcc6..bc4e098d 100644 --- a/boards/recovery/src/types.rs +++ b/boards/recovery/src/types.rs @@ -1,30 +1,9 @@ -use atsamd_hal::gpio::{PA18, PA19, Pin, PushPullOutput}; -use atsamd_hal::prelude::*; -use messages::sender::Sender; -use messages::sender::Sender::RecoveryBoard; - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = RecoveryBoard; - - -pub struct GPIOController { - drogue_ematch: Pin, - main_ematch: Pin, -} - -impl GPIOController { - pub fn new(drogue_ematch: Pin, main_ematch: Pin) -> Self { - Self { - drogue_ematch, - main_ematch, - } - } - pub fn fire_drogue(&mut self) { - self.drogue_ematch.set_high().ok(); - } - pub fn fire_main(&mut self) { - self.main_ematch.set_high().ok(); - } -} \ No newline at end of file +use atsamd_hal::gpio::{Pin, PushPullOutput, PA09, PA06}; +use atsamd_hal::prelude::*; +use messages::sender::Sender; +use messages::sender::Sender::RecoveryBoard; + +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = RecoveryBoard; \ No newline at end of file diff --git a/boards/sensor/Cargo.toml b/boards/sensor/Cargo.toml index 1948612b..943de5c5 100644 --- a/boards/sensor/Cargo.toml +++ b/boards/sensor/Cargo.toml @@ -1,23 +1,23 @@ -[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" -panic-halt = "0.2.0" -systick-monotonic = "1.0.1" -defmt = "0.3.2" -postcard = "1.0.2" -heapless = "0.7.16" -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" -typenum = "1.16.0" +[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" +panic-halt = "0.2.0" +systick-monotonic = "1.0.1" +defmt = "0.3.2" +postcard = "1.0.2" +heapless = "0.7.16" +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" \ No newline at end of file diff --git a/boards/sensor/src/communication.rs b/boards/sensor/src/communication.rs index f9ebeca6..59af1120 100644 --- a/boards/sensor/src/communication.rs +++ b/boards/sensor/src/communication.rs @@ -1,185 +1,197 @@ -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::mcan::message::{rx, Raw}; -use common_arm::mcan::tx_buffers::DynTx; -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::messageram::SharedMemory; -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::StoreFifo0, - filter: ecan::StandardId::new(messages::sender::Sender::RecoveryBoard.into()) - .unwrap(), - mask: ecan::StandardId::MAX, - }) - .unwrap_or_else(|_| panic!("Recovery filter")); - - can.filters_standard() - .push(Filter::Classic { - action: Action::StoreFifo1, - filter: ecan::StandardId::new(messages::sender::Sender::GroundStation.into()) - .unwrap(), - mask: ecan::StandardId::MAX, - }) - .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) { - 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!("Message: {:?}", data) - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - Interrupt::RxFifo1NewMessage => { - for message in &mut self.can.rx_fifo_1 { - match from_bytes::(message.data()) { - Ok(data) => { - info!("Message: {:?}", data) - } - Err(e) => { - info!("Error: {:?}", e) - } - } - } - } - _ => (), - } - } - } -} +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, AlternateH, Pin, I, H, PA22, PA23, PB15, PB14}; +use atsamd_hal::pac::{CAN0, CAN1}; + +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::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::messageram::SharedMemory; +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, self}; + +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::sender::Sender::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::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::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(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) + } + } + } + } + _ => (), + } + } + } +} \ No newline at end of file diff --git a/boards/sensor/src/data_manager.rs b/boards/sensor/src/data_manager.rs index 61c9977f..053c5cb0 100644 --- a/boards/sensor/src/data_manager.rs +++ b/boards/sensor/src/data_manager.rs @@ -1,43 +1,68 @@ -use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, SensorData, UtcTime}; - -#[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, -} - -impl DataManager { - pub fn new() -> Self { - Self { - air: None, - ekf_nav: None, - ekf_quat: None, - imu: None, - utc_time: None, - gps_vel: None, - } - } - - pub fn clone_sensors(&self) -> [Option; 8] { - [ - 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()), - ] - } -} - -impl Default for DataManager { - fn default() -> Self { - Self::new() - } -} +use common_arm::spawn; +use messages::sender::Sender; +use messages::sensor::{GpsPos1, GpsPos2, Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, SensorData, UtcTime}; +use messages::Message; +use crate::app::sleep_system; +use defmt::info; + +#[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) { + match data.data { + messages::Data::Command(command) => match command.data { + messages::command::CommandData::PowerDown(info) => { + spawn!(sleep_system); + } + _ => { + // We don't care atm about these other commands. + } + } + _ => { + // we can disregard all other messages for now. + } + } + } +} + +impl Default for DataManager { + fn default() -> Self { + Self::new() + } +} diff --git a/boards/sensor/src/main.rs b/boards/sensor/src/main.rs index 0e719a44..53a6d0eb 100644 --- a/boards/sensor/src/main.rs +++ b/boards/sensor/src/main.rs @@ -1,216 +1,250 @@ -#![no_std] -#![no_main] - -mod communication; -mod data_manager; -mod sbg_manager; -mod sd_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::*; -use communication::Capacities; -use data_manager::DataManager; -use hal::dmac; -use hal::gpio::Pins; -use hal::gpio::PA14; -use hal::gpio::{Pin, PushPullOutput}; -use hal::prelude::*; -use mcan::messageram::SharedMemory; -use messages::sensor::Sensor; -use messages::*; -use panic_halt as _; -use sbg_manager::{sbg_dma, sbg_handle_data, SBGManager}; -use sbg_rs::sbg::CallbackData; -use sd_manager::SdManager; -use systick_monotonic::*; -use types::*; - -#[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: communication::CanDevice0, - } - - #[local] - struct Local { - led: Pin, - sd_manager: SdManager, - sbg_manager: SBGManager, - } - - #[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(); - - /* 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, - ); - - /* SD config */ - let (pclk_sd, gclk0) = Pclk::enable(tokens.pclks.sercom1, gclk0); - let sd_manager = SdManager::new( - &mclk, - peripherals.SERCOM1, - pclk_sd.freq(), - pins.pa18.into_push_pull_output(), - pins.pa17.into_push_pull_output(), - pins.pa19.into_push_pull_output(), - pins.pa16.into_push_pull_output(), - ); - - /* SBG config */ - let (pclk_sbg, gclk0) = Pclk::enable(tokens.pclks.sercom0, gclk0); - let dmaCh0 = dmaChannels.0.init(dmac::PriorityLevel::LVL3); - let sbg_manager = SBGManager::new( - pins.pa09, - pins.pa08, - pclk_sbg, - &mut mclk, - peripherals.SERCOM0, - peripherals.RTC, - dmaCh0, - ); - - /* Status LED */ - let led = pins.pa14.into_push_pull_output(); - - /* Spawn tasks */ - sensor_send::spawn().ok(); - blink::spawn().ok(); - - /* Monotonic clock */ - let mono = Systick::new(core.SYST, gclk0.freq().0); - - ( - Shared { - em: ErrorManager::new(), - data_manager: DataManager::new(), - can0, - }, - Local { - led, - sd_manager, - sbg_manager, - }, - init::Monotonics(mono), - ) - } - - /// Idle task for when no other tasks are running. - #[idle] - fn idle(_: idle::Context) -> ! { - loop {} - } - - #[task(priority = 3, binds = CAN0, shared = [can0])] - fn can0(mut cx: can0::Context) { - cx.shared.can0.lock(|can| { - can.process_data(); - }); - } - - /** - * 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(()) - }); - } - - /** - * 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, 250.millis()).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, 200.millis())?; - } else { - spawn_after!(blink, 1.secs())?; - } - Ok(()) - }); - } - - extern "Rust" { - #[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); - } -} +#![no_std] +#![no_main] + +mod communication; +mod data_manager; +mod sbg_manager; +mod sd_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::*; +use communication::Capacities; +use data_manager::DataManager; +use hal::dmac; +use defmt::info; +use hal::gpio::Pins; +use hal::gpio::{PB16, PB17, PB01}; +use hal::gpio::{Pin, PushPullOutput}; +use hal::prelude::*; +use mcan::messageram::SharedMemory; +use messages::sensor::Sensor; +use messages::*; +use panic_halt as _; +use sd_manager::SdManager; +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::*; + +#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0, EVSYS_1, EVSYS_2])] +mod app { + use defmt::flush; + + 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 sd_manager = SdManager::new( + &mclk, + peripherals.SERCOM4, + pclk_sd.freq(), + pins.pb10.into_push_pull_output(), + pins.pb09.into_push_pull_output(), + pins.pb11.into_push_pull_output(), + pins.pb08.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(); + Ok(()) + }); + }); + // 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. + } + + #[task(priority = 3, binds = CAN0, shared = [can, data_manager])] + fn can0(mut cx: can0::Context) { + cx.shared.can.lock(|can| { + cx.shared.data_manager.lock(|manager| can.process_data(manager)) + }); + } + + /** + * 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 index 4cc952c7..9208ade9 100644 --- a/boards/sensor/src/sbg_manager.rs +++ b/boards/sensor/src/sbg_manager.rs @@ -1,183 +1,205 @@ -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, PA08, PA09}; -use atsamd_hal::pac::{MCLK, RTC}; -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 crate::app::sbg_handle_data; -use atsamd_hal::sercom::{uart, Sercom, Sercom0}; -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]; -pub static mut BUF_DST2: 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: SBGTransfer, - buf_select: bool, -} - -impl SBGManager { - pub fn new( - rx: Pin, - tx: Pin, - pclk_sercom0: Pclk, - mclk: &mut MCLK, - sercom0: Sercom0, - 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, sercom0, pads_sbg, pclk_sercom0.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(Sercom0::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, - buf_select: false, - 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), - }); -} - -/** - * Handles the DMA interrupt. - * Handles the SBG data. - * Logs data to the SD card. - */ -pub fn sbg_dma(cx: crate::app::sbg_dma::Context) { - let sbg = cx.local.sbg_manager; - - if sbg.xfer.complete() { - cx.shared.em.run(|| { - let buf = match sbg.buf_select { - false => { - sbg.buf_select = true; - sbg.xfer.recycle_source(unsafe { &mut *BUF_DST })? - } - true => { - sbg.buf_select = false; - sbg.xfer.recycle_source(unsafe { &mut *BUF_DST2 })? - } - }; - - sbg.sbg_device.read_data(buf); - - Ok(()) - }); - } -} - -/// 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); - } -} +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::sercom::{IoSet6}; +use atsamd_hal::gpio::{Pin, Reset, PB03, PB02, PB01, PushPullOutput}; +use atsamd_hal::pac::{MCLK, RTC}; +// 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 atsamd_hal::prelude::*; +use defmt::info; +use common_arm::spawn; +use crate::app::sbg_sd_task as sbg_sd; +use crate::app::{sbg_handle_data}; +use atsamd_hal::sercom::{uart, Sercom, Sercom5}; +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)); + 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/sd_manager.rs b/boards/sensor/src/sd_manager.rs index ad1793a1..6289ac1f 100644 --- a/boards/sensor/src/sd_manager.rs +++ b/boards/sensor/src/sd_manager.rs @@ -1,146 +1,148 @@ -use core::marker::PhantomData; - -use crate::types::SdController; -use atsamd_hal::gpio::{Output, Pin, PushPull, PA16, PA17, PA18, PA19}; -use atsamd_hal::pac; -use atsamd_hal::sercom::{spi, IoSet1, Sercom1}; -use atsamd_hal::time::Hertz; -use defmt::{info, warn}; -use embedded_sdmmc as sd; - -/// Time source for `[SdInterface]`. It doesn't return any useful information for now, and will -/// always return an arbitrary time. -pub struct TimeSink { - _marker: PhantomData<*const ()>, -} - -impl TimeSink { - fn new() -> Self { - TimeSink { - _marker: PhantomData, - } - } -} - -impl sd::TimeSource for TimeSink { - fn get_timestamp(&self) -> sd::Timestamp { - sd::Timestamp { - year_since_1970: 0, - zero_indexed_month: 0, - zero_indexed_day: 0, - hours: 0, - minutes: 0, - seconds: 0, - } - } -} - -/// Wrapper for the SD Card. For now, the pins are hard-coded. -pub struct SdManager { - pub sd_controller: SdController, - pub volume: sd::Volume, - pub root_directory: sd::Directory, - pub file: sd::File, -} - -impl SdManager { - pub fn new( - mclk: &pac::MCLK, - sercom: pac::SERCOM1, - freq: Hertz, - cs: Pin>, - sck: Pin>, - miso: Pin>, - mosi: Pin>, - ) -> Self { - let pads_spi = spi::Pads::::default() - .sclk(sck) - .data_in(miso) - .data_out(mosi); - let sdmmc_spi = spi::Config::new(mclk, sercom, pads_spi, freq) - .length::() - .bit_order(spi::BitOrder::MsbFirst) - .spi_mode(spi::MODE_0) - .enable(); - let time_sink: TimeSink = TimeSink::new(); // Need to give this a DateTime object for actual timing. - let mut sd_cont = sd::Controller::new(sd::SdMmcSpi::new(sdmmc_spi, cs), time_sink); - match sd_cont.device().init() { - Ok(_) => match sd_cont.device().card_size_bytes() { - Ok(size) => info!("Card is {} bytes", size), - Err(_) => warn!("Cannot get card size"), - }, - Err(_) => { - warn!("Cannot get SD card"); - panic!("Cannot get SD card."); - } - } - - let mut volume = match sd_cont.get_volume(sd::VolumeIdx(0)) { - Ok(volume) => volume, - Err(_) => { - warn!("Cannot get volume 0"); - panic!("Cannot get volume 0"); - } - }; - - let root_directory = match sd_cont.open_root_dir(&volume) { - Ok(root_directory) => root_directory, - Err(_) => { - warn!("Cannot get root"); - panic!("Cannot get root"); - } - }; - let file = sd_cont.open_file_in_dir( - &mut volume, - &root_directory, - "test2.txt", - sd::Mode::ReadWriteCreateOrTruncate, - ); - let file = match file { - Ok(file) => file, - Err(_) => { - warn!("Cannot create file."); - panic!("Cannot create file."); - } - }; - - SdManager { - sd_controller: sd_cont, - volume, - root_directory, - file, - } - } - pub fn write( - &mut self, - file: &mut sd::File, - buffer: &[u8], - ) -> Result> { - self.sd_controller.write(&mut self.volume, file, buffer) - } - pub fn write_str( - &mut self, - file: &mut sd::File, - msg: &str, - ) -> Result> { - let buffer: &[u8] = msg.as_bytes(); - self.sd_controller.write(&mut self.volume, file, buffer) - } - pub fn open_file(&mut self, file_name: &str) -> Result> { - self.sd_controller.open_file_in_dir( - &mut self.volume, - &self.root_directory, - file_name, - sd::Mode::ReadWriteCreateOrTruncate, - ) - } - pub fn close_file(&mut self, file: sd::File) -> Result<(), sd::Error> { - self.sd_controller.close_file(&self.volume, file) - } - pub fn close(mut self) { - self.sd_controller - .close_dir(&self.volume, self.root_directory); - } -} - -unsafe impl Send for SdManager {} +use core::marker::PhantomData; + +use crate::types::SdController; +use atsamd_hal::gpio::{Output, Pin, PushPull, PB10, PB09, PB11, PB08}; +use atsamd_hal::pac; +use atsamd_hal::sercom::{spi, Sercom4, IoSet2}; +use atsamd_hal::time::Hertz; +use defmt::{info, warn}; +use embedded_sdmmc as sd; + +/// Time source for `[SdInterface]`. It doesn't return any useful information for now, and will +/// always return an arbitrary time. +pub struct TimeSink { + _marker: PhantomData<*const ()>, +} + +impl TimeSink { + fn new() -> Self { + TimeSink { + _marker: PhantomData, + } + } +} + +impl sd::TimeSource for TimeSink { + fn get_timestamp(&self) -> sd::Timestamp { + sd::Timestamp { + year_since_1970: 0, + zero_indexed_month: 0, + zero_indexed_day: 0, + hours: 0, + minutes: 0, + seconds: 0, + } + } +} + +/// Wrapper for the SD Card. For now, the pins are hard-coded. +pub struct SdManager { + pub sd_controller: SdController, + pub volume: sd::Volume, + pub root_directory: sd::Directory, + pub file: Option, +} + +impl SdManager { + pub fn new( + mclk: &pac::MCLK, + sercom: pac::SERCOM4, + freq: Hertz, + cs: Pin>, + sck: Pin>, + miso: Pin>, + mosi: Pin>, + ) -> Self { + let pads_spi = spi::Pads::::default() + .sclk(sck) + .data_in(miso) + .data_out(mosi); + let sdmmc_spi = spi::Config::new(mclk, sercom, pads_spi, freq) + .length::() + .bit_order(spi::BitOrder::MsbFirst) + .spi_mode(spi::MODE_0) + .enable(); + let time_sink: TimeSink = TimeSink::new(); // Need to give this a DateTime object for actual timing. + let mut sd_cont = sd::Controller::new(sd::SdMmcSpi::new(sdmmc_spi, cs), time_sink); + match sd_cont.device().init() { + Ok(_) => match sd_cont.device().card_size_bytes() { + Ok(size) => info!("Card is {} bytes", size), + Err(_) => panic!("Cannot get card size"), + }, + Err(_) => { + panic!("Cannot get SD card."); + } + } + + let mut volume = match sd_cont.get_volume(sd::VolumeIdx(0)) { + Ok(volume) => volume, + Err(_) => { + panic!("Cannot get volume 0"); + } + }; + + let root_directory = match sd_cont.open_root_dir(&volume) { + Ok(root_directory) => root_directory, + Err(_) => { + panic!("Cannot get root"); + } + }; + let file = sd_cont.open_file_in_dir( + &mut volume, + &root_directory, + "sbg.txt", + sd::Mode::ReadWriteCreateOrTruncate, + ); + let file = match file { + Ok(file) => file, + Err(_) => { + panic!("Cannot create file."); + } + }; + + SdManager { + sd_controller: sd_cont, + volume, + root_directory, + file: Some(file), + } + } + pub fn write( + &mut self, + file: &mut sd::File, // this should be an option + buffer: &[u8], + ) -> Result> { + self.sd_controller.write(&mut self.volume, file, buffer) + } + pub fn write_str( + &mut self, + file: &mut sd::File, + msg: &str, + ) -> Result> { + let buffer: &[u8] = msg.as_bytes(); + self.sd_controller.write(&mut self.volume, file, buffer) + } + pub fn open_file(&mut self, file_name: &str) -> Result> { + self.sd_controller.open_file_in_dir( + &mut self.volume, + &self.root_directory, + file_name, + sd::Mode::ReadWriteCreateOrTruncate, + ) + } + pub fn close_current_file(&mut self) -> Result<(), sd::Error> { + if let Some(file) = self.file.take() { + return self.close_file(file); + } + Ok(()) // no file to close + } + pub fn close_file(&mut self, file: sd::File) -> Result<(), sd::Error> { + self.sd_controller.close_file(&self.volume, file) + } + pub fn close(mut self) { + self.sd_controller + .close_dir(&self.volume, self.root_directory); + } +} + +unsafe impl Send for SdManager {} \ No newline at end of file diff --git a/boards/sensor/src/types.rs b/boards/sensor/src/types.rs index 644ccdee..c2a8cbd5 100644 --- a/boards/sensor/src/types.rs +++ b/boards/sensor/src/types.rs @@ -1,51 +1,47 @@ -use crate::sd_manager::TimeSink; -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, IoSet1, Sercom1}; -use embedded_sdmmc as sd; -use hal::dmac; -use hal::dmac::BufferPair; -use hal::sercom::Sercom0; -use hal::sercom::Sercom5; -use messages::sender::Sender; -use messages::sender::Sender::SensorBoard; -use sbg_rs::sbg::SBG_BUFFER_SIZE; - -// ------- -// Sender ID -// ------- -pub static COM_ID: Sender = SensorBoard; - -// ------- -// Ground Station -// ------- -pub type GroundStationPads = uart::PadsFromIds; -pub type GroundStationUartConfig = uart::Config; - -// ------- -// 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< - Sercom1, - IoSet1, - Pin>, - Pin>, - Pin>, ->; -pub type SdController = sd::Controller< - sd::SdMmcSpi, spi::Duplex>, Pin>>, - TimeSink, ->; +use crate::sd_manager::TimeSink; +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, Sercom5, IoSet6}; +use embedded_sdmmc as sd; +use hal::dmac; +use hal::dmac::BufferPair; +use hal::sercom::IoSet2; + + +use hal::sercom::Sercom4; +use messages::sender::Sender; +use messages::sender::Sender::SensorBoard; +use sbg_rs::sbg::SBG_BUFFER_SIZE; + +// ------- +// Sender ID +// ------- +pub static COM_ID: Sender = 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 SdController = sd::Controller< + sd::SdMmcSpi, spi::Duplex>, Pin>>, + TimeSink, +>; \ No newline at end of file diff --git a/examples/rtic/src/main.rs b/examples/rtic/src/main.rs index 5a3a9d1c..3419586f 100644 --- a/examples/rtic/src/main.rs +++ b/examples/rtic/src/main.rs @@ -1,78 +1,78 @@ -#![no_std] -#![no_main] - -use atsamd_hal as hal; -use defmt::info; -use defmt_rtt as _; -use hal::gpio::Pins; -use hal::gpio::PA14; -use hal::gpio::{Pin, PushPullOutput}; -use hal::prelude::*; -use hal::time::Hertz; -use panic_halt as _; -use systick_monotonic::*; - -// "dispatchers" here can be any unused interrupts -#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0])] -mod app { - use super::*; - - #[shared] - struct Shared {} - - #[local] - struct Local { - led: Pin, - } - - #[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 mut peripherals = cx.device; - let mut core = cx.core; - - // External 32KHz clock for stability - let mut clocks = hal::clock::GenericClockController::with_external_32kosc( - peripherals.GCLK, - &mut peripherals.MCLK, - &mut peripherals.OSC32KCTRL, - &mut peripherals.OSCCTRL, - &mut peripherals.NVMCTRL, - ); - - let pins = Pins::new(peripherals.PORT); - let led = pins.pa14.into_push_pull_output(); - - // Tell the MCU to sleep deeply for maximum power savings - core.SCB.set_sleepdeep(); - - // Spawn the LED blink task right after init - blink::spawn().ok(); - - // Use the system's Systick for RTIC to keep track of the time - let sysclk: Hertz = clocks.gclk0().into(); - let mono = Systick::new(core.SYST, sysclk.0); - - (Shared {}, Local { led }, init::Monotonics(mono)) - } - - #[idle] - fn idle(_cx: idle::Context) -> ! { - loop { - // Put the MCU to sleep until an interrupt happens - rtic::export::wfi(); - } - } - - #[task(local = [led])] - fn blink(cx: blink::Context) { - cx.local.led.toggle().unwrap(); - - let time = monotonics::now().duration_since_epoch().to_secs(); - info!("Seconds since epoch: {}", time); - - blink::spawn_after(1.secs()).ok(); - } -} +#![no_std] +#![no_main] + +use atsamd_hal as hal; +use defmt::info; +use defmt_rtt as _; +use hal::gpio::Pins; +use hal::gpio::PA14; +use hal::gpio::{Pin, PushPullOutput}; +use hal::prelude::*; +use hal::time::Hertz; +use panic_halt as _; +use systick_monotonic::*; + +// "dispatchers" here can be any unused interrupts +#[rtic::app(device = hal::pac, peripherals = true, dispatchers = [EVSYS_0])] +mod app { + use super::*; + + #[shared] + struct Shared {} + + #[local] + struct Local { + led: Pin, + } + + #[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 mut peripherals = cx.device; + let mut core = cx.core; + + // External 32KHz clock for stability + let mut clocks = hal::clock::GenericClockController::with_external_32kosc( + peripherals.GCLK, + &mut peripherals.MCLK, + &mut peripherals.OSC32KCTRL, + &mut peripherals.OSCCTRL, + &mut peripherals.NVMCTRL, + ); + + let pins = Pins::new(peripherals.PORT); + let led = pins.pa14.into_push_pull_output(); + + // Tell the MCU to sleep deeply for maximum power savings + core.SCB.set_sleepdeep(); + + // Spawn the LED blink task right after init + blink::spawn().ok(); + + // Use the system's Systick for RTIC to keep track of the time + let sysclk: Hertz = clocks.gclk0().into(); + let mono = Systick::new(core.SYST, sysclk.to_Hz()); + + (Shared {}, Local { led }, init::Monotonics(mono)) + } + + #[idle] + fn idle(_cx: idle::Context) -> ! { + loop { + // Put the MCU to sleep until an interrupt happens + rtic::export::wfi(); + } + } + + #[task(local = [led])] + fn blink(cx: blink::Context) { + cx.local.led.toggle().unwrap(); + + let time = monotonics::now().duration_since_epoch().to_secs(); + info!("Seconds since epoch: {}", time); + + blink::spawn_after(ExtU64::secs(1)).ok(); + } +} diff --git a/libraries/common-arm/Cargo.toml b/libraries/common-arm/Cargo.toml index bc6f7035..ac4b5d05 100644 --- a/libraries/common-arm/Cargo.toml +++ b/libraries/common-arm/Cargo.toml @@ -19,4 +19,4 @@ embedded-sdmmc = "0.3.0" embedded-hal = "0.2.7" mcan = "0.3" nb = "1.1.0" -messages = { path = "../messages" } +messages = { path = "../messages" } \ No newline at end of file diff --git a/libraries/common-arm/memory.x b/libraries/common-arm/memory.x index 506f899d..690b7767 100644 --- a/libraries/common-arm/memory.x +++ b/libraries/common-arm/memory.x @@ -1,15 +1,15 @@ -MEMORY -{ - /* NOTE K = KiBi = 1024 bytes */ - FLASH : ORIGIN = 0x00000000, LENGTH = 1024K - CAN : ORIGIN = 0x20000000, LENGTH = 64K - RAM : ORIGIN = 0x20010000, LENGTH = 64K -} -SECTIONS -{ - .can (NOLOAD) : - { - *(.can .can.*); - } > CAN -} +MEMORY +{ + /* NOTE K = KiBi = 1024 bytes */ + FLASH : ORIGIN = 0x00000000, LENGTH = 256K + CAN : ORIGIN = 0x20000000, LENGTH = 64K + RAM : ORIGIN = 0x20010000, LENGTH = 64K +} +SECTIONS +{ + .can (NOLOAD) : + { + *(.can .can.*); + } > CAN +} /* _stack_start is optional and we can define this later */ \ No newline at end of file diff --git a/libraries/common-arm/src/data_manager.rs b/libraries/common-arm/src/data_manager.rs new file mode 100644 index 00000000..e69de29b diff --git a/libraries/common-arm/src/lib.rs b/libraries/common-arm/src/lib.rs index a8fb9a51..c91bbaf5 100644 --- a/libraries/common-arm/src/lib.rs +++ b/libraries/common-arm/src/lib.rs @@ -10,9 +10,11 @@ pub use mcan; mod error; mod logging; +mod sd_manager; pub use crate::error::error_manager::ErrorManager; pub use crate::error::hydra_error::{ErrorContextTrait, HydraError, SpawnError}; pub use crate::logging::HydraLogging; +pub use crate::sd_manager::SdManager; use defmt_rtt as _; // global logger diff --git a/libraries/common-arm/src/sd_manager.rs b/libraries/common-arm/src/sd_manager.rs new file mode 100644 index 00000000..bd6bab0c --- /dev/null +++ b/libraries/common-arm/src/sd_manager.rs @@ -0,0 +1,145 @@ +use core::{marker::PhantomData, fmt::Debug}; +use defmt::{info, warn}; +use atsamd_hal::gpio::{Output, Pin, PushPull, PB14}; +use embedded_sdmmc as sd; +use embedded_hal as hal; +use cortex_m; +use hal::spi::FullDuplex; + +/// Time source for `[SdInterface]`. It doesn't return any useful information for now, and will +/// always return an arbitrary time. +pub struct TimeSink { + _marker: PhantomData<*const ()>, +} + +impl TimeSink { + fn new() -> Self { + TimeSink { + _marker: PhantomData, + } + } +} + +impl sd::TimeSource for TimeSink { + fn get_timestamp(&self) -> sd::Timestamp { + sd::Timestamp { + year_since_1970: 0, + zero_indexed_month: 0, + zero_indexed_day: 0, + hours: 0, + minutes: 0, + seconds: 0, + } + } +} + +/// Wrapper for the SD Card. For now, the pins are hard-coded. +pub struct SdManager +where + SPI: hal::spi::FullDuplex, >::Error: Debug, + CS: hal::digital::v2::OutputPin, +{ + pub sd_controller: sd::Controller, TimeSink>, + pub volume: sd::Volume, + pub root_directory: sd::Directory, + pub file: Option, +} + +impl SdManager +where + SPI: hal::spi::FullDuplex, >::Error: Debug, + CS: hal::digital::v2::OutputPin, +{ + pub fn new( + spi: SPI, + cs: CS + ) -> Self + { + let time_sink: TimeSink = TimeSink::new(); // Need to give this a DateTime object for actual timing. + 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() { + Ok(size) => info!("Card is {} bytes", size), + Err(_) => panic!("Cannot get card size"), + }, + Err(_) => { + panic!("Cannot get SD card."); + } + } + + let mut volume = match sd_cont.get_volume(sd::VolumeIdx(0)) { + Ok(volume) => volume, + Err(_) => { + panic!("Cannot get volume 0"); + } + }; + + let root_directory = match sd_cont.open_root_dir(&volume) { + Ok(root_directory) => root_directory, + Err(_) => { + panic!("Cannot get root"); + } + }; + let file = sd_cont.open_file_in_dir( + &mut volume, + &root_directory, + "log.txt", + sd::Mode::ReadWriteCreateOrTruncate, + ); + let file = match file { + Ok(file) => file, + Err(_) => { + panic!("Cannot create file."); + } + }; + + SdManager { + sd_controller: sd_cont, + volume, + root_directory, + file: Some(file), + } + } + pub fn write( + &mut self, + file: &mut sd::File, + buffer: &[u8], + ) -> Result> { + self.sd_controller.write(&mut self.volume, file, buffer) + } + pub fn write_str( + &mut self, + file: &mut sd::File, + msg: &str, + ) -> Result> { + let buffer: &[u8] = msg.as_bytes(); + self.sd_controller.write(&mut self.volume, file, buffer) + } + pub fn open_file(&mut self, file_name: &str) -> Result> { + self.sd_controller.open_file_in_dir( + &mut self.volume, + &self.root_directory, + file_name, + sd::Mode::ReadWriteCreateOrTruncate, + ) + } + pub fn close_current_file(&mut self) -> Result<(), sd::Error> { + if let Some(file) = self.file.take() { + return self.close_file(file); + } + Ok(()) + } + pub fn close_file(&mut self, file: sd::File) -> Result<(), sd::Error> { + self.sd_controller.close_file(&self.volume, file) + } + pub fn close(mut self) { + self.sd_controller + .close_dir(&self.volume, self.root_directory); + } +} + +unsafe impl Send for SdManager +where + SPI: hal::spi::FullDuplex, >::Error: Debug, + CS: hal::digital::v2::OutputPin, +{} \ No newline at end of file diff --git a/libraries/messages/src/command.rs b/libraries/messages/src/command.rs new file mode 100644 index 00000000..e1b731be --- /dev/null +++ b/libraries/messages/src/command.rs @@ -0,0 +1,79 @@ +use derive_more::From; +use defmt::Format; +use serde::{Deserialize, Serialize}; +use crate::sender::Sender; + +#[cfg(test)] +use proptest_derive::Arbitrary; + +#[cfg(feature = "ts")] +use ts_rs::TS; + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Command { + pub data: CommandData, +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub enum CommandData { + DeployDrogue(DeployDrogue), + DeployMain(DeployMain), + PowerDown(PowerDown), + RadioRateChange(RadioRateChange), +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct DeployDrogue { + pub val: bool, +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct DeployMain { + pub val: bool, + // Auth? +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct PowerDown { + pub board: Sender, // This isn't proper naming !! This is the board to be powered down. Changes name of sender.rs to board.rs. +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct RadioRateChange { + pub rate: RadioRate, +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub enum RadioRate { + Fast, + Slow, +} + +impl Command { + pub fn new(data: impl Into) -> Self { + Command { + data: data.into(), + } + } +} \ No newline at end of file diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index c2000e99..45e5cacc 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -1,121 +1,93 @@ -#![cfg_attr(all(not(feature = "std"), not(test)), no_std)] - -//! # HYDRA Messages -//! -//! This crate contains all the message definitions that will be used for inter-board communication -//! and ground-station communication. - -use crate::sender::Sender; -use crate::sensor::Sensor; -use defmt::Format; -use derive_more::From; -use fugit::Instant; -use serde::{Deserialize, Serialize}; - -/// This is to help control versions. -pub use mavlink; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -mod logging; -pub mod sender; -pub mod sensor; - -pub const MAX_SIZE: usize = 64; - -pub use logging::{ErrorContext, Event, Log, LogLevel}; - -/// Topmost message. Encloses all the other possible messages, and is the only thing that should -/// be sent over the wire. -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] -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, - - /// The original sender of this message. - pub sender: Sender, - - /// The data contained in this message. - pub data: Data, -} - -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] -#[serde(rename_all = "lowercase")] -pub enum Data { - State(State), - Sensor(Sensor), - Log(Log), -} - -// #[derive(Serialize, Deserialize, Clone, Debug, Format)] -// #[cfg_attr(test, derive(Arbitrary))] -// #[cfg_attr(feature = "ts", derive(TS))] -// #[cfg_attr(feature = "ts", ts(export))] -// pub enum Status { -// Uninitialized, -// Initializing, -// Running, -// } - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub enum Status { - Initializing, - WaitForTakeoff, - Ascent, - Apogee, - Landed, - Abort, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct State { - pub status: Status, - pub has_error: bool, -} - -impl Message { - pub fn new( - timestamp: &Instant, - sender: Sender, - data: impl Into, - ) -> Self { - Message { - timestamp: timestamp.duration_since_epoch().to_millis(), - sender, - data: data.into(), - } - } -} - -#[cfg(test)] -mod test { - use crate::{Message, MAX_SIZE}; - use proptest::prelude::*; - - proptest! { - #[test] - fn message_size(msg: Message) { - let bytes = postcard::to_allocvec(&msg).unwrap(); - - dbg!(msg); - assert!(dbg!(bytes.len()) <= MAX_SIZE); - } - } -} +#![cfg_attr(all(not(feature = "std"), not(test)), no_std)] + +//! # HYDRA Messages +//! +//! This crate contains all the message definitions that will be used for inter-board communication +//! and ground-station communication. + +use crate::sender::Sender; +use crate::sensor::Sensor; +use crate::state::State; +use crate::command::Command; +use defmt::Format; +use derive_more::From; +use fugit::Instant; +use serde::{Deserialize, Serialize}; +/// This is to help control versions. +pub use mavlink; + +#[cfg(test)] +use proptest_derive::Arbitrary; + +#[cfg(feature = "ts")] +use ts_rs::TS; + +mod logging; +pub mod sender; +pub mod sensor; +pub mod command; +pub mod state; + +pub const MAX_SIZE: usize = 64; + +pub use logging::{ErrorContext, Event, Log, LogLevel}; + +/// Topmost message. Encloses all the other possible messages, and is the only thing that should +/// be sent over the wire. +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +#[cfg_attr(test, derive(Arbitrary))] +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, + + /// The original sender of this message. + pub sender: Sender, + + /// The data contained in this message. + pub data: Data, +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +#[cfg_attr(test, derive(Arbitrary))] +#[serde(rename_all = "lowercase")] +pub enum Data { + State(State), + Sensor(Sensor), + Log(Log), + Command(Command), +} + +impl Message { + pub fn new( + timestamp: &Instant, + sender: Sender, + data: impl Into, + ) -> Self { + Message { + timestamp: timestamp.duration_since_epoch().to_millis(), + sender, + data: data.into(), + } + } +} + +#[cfg(test)] +mod test { + use crate::{Message, MAX_SIZE}; + use proptest::prelude::*; + + proptest! { + #[test] + fn message_size(msg: Message) { + let bytes = postcard::to_allocvec(&msg).unwrap(); + + dbg!(msg); + assert!(dbg!(bytes.len()) <= MAX_SIZE); + } + } +} diff --git a/libraries/messages/src/sender.rs b/libraries/messages/src/sender.rs index 7d34cf32..27ea91c6 100644 --- a/libraries/messages/src/sender.rs +++ b/libraries/messages/src/sender.rs @@ -1,32 +1,35 @@ -use defmt::Format; -use serde::{Deserialize, Serialize}; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format, Copy)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub enum Sender { - GroundStation, - SensorBoard, - RecoveryBoard, - CommunicationBoard, - PowerBoard -} - -impl From for u16 { - fn from(sender: Sender) -> Self { - match sender { - Sender::GroundStation => 0, - Sender::SensorBoard => 1, - Sender::RecoveryBoard => 2, - Sender::CommunicationBoard => 3, - Sender::PowerBoard => 4 - } - } -} +use defmt::Format; +use serde::{Deserialize, Serialize}; + +#[cfg(test)] +use proptest_derive::Arbitrary; + +#[cfg(feature = "ts")] +use ts_rs::TS; + +// I don't agree with the naming, We can use these as Ids to sent commands to that specific board. +#[derive(Serialize, Deserialize, Clone, Debug, Format, Copy)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub enum Sender { + GroundStation, + SensorBoard, + RecoveryBoard, + CommunicationBoard, + PowerBoard, + CameraBoard, +} + +impl From for u16 { + fn from(sender: Sender) -> Self { + match sender { + Sender::GroundStation => 0, + Sender::SensorBoard => 1, + Sender::RecoveryBoard => 2, + Sender::CommunicationBoard => 3, + Sender::PowerBoard => 4, + Sender::CameraBoard => 5, + } + } +} diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index df8badfb..63ad0522 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -1,183 +1,265 @@ -use defmt::Format; -use derive_more::From; -use serde::{Deserialize, Serialize}; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct Sensor { - /// Used to differentiate between multiple components on the same sender. Unused right now. - pub component_id: u8, - pub data: SensorData, -} - -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub enum SensorData { - UtcTime(UtcTime), - Air(Air), - EkfQuat(EkfQuat), - EkfNav1(EkfNav1), - EkfNav2(EkfNav2), - Imu1(Imu1), - Imu2(Imu2), - GpsVel(GpsVel), -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct UtcTime { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< UTC time and clock status information"] - pub status: u16, - #[doc = "< Year for example: 2013."] - pub year: u16, - #[doc = "< Month in year [1 .. 12]."] - pub month: i8, - #[doc = "< Day in month [1 .. 31]."] - pub day: i8, - #[doc = "< Hour in day [0 .. 23]."] - pub hour: i8, - #[doc = "< Minute in hour [0 .. 59]."] - pub minute: i8, - #[doc = "< Second in minute [0 .. 60]. (60 is used only when a leap second is added)"] - pub second: i8, - #[doc = "< Nanosecond of current second in ns."] - pub nano_second: i32, - #[doc = "< GPS time of week in ms."] - pub gps_time_of_week: u32, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct Air { - #[doc = "< Time in us since the sensor power up OR measurement delay in us."] - pub time_stamp: u32, - #[doc = "< Airdata sensor status bitmask."] - pub status: u16, - #[doc = "< Raw absolute pressure measured by the barometer sensor in Pascals."] - pub pressure_abs: f32, - #[doc = "< Altitude computed from barometric altimeter in meters and positive upward."] - pub altitude: f32, - #[doc = "< Raw differential pressure measured by the pitot tube in Pascal."] - pub pressure_diff: f32, - #[doc = "< True airspeed measured by a pitot tube in m.s^-1 and positive forward."] - pub true_airspeed: f32, - #[doc = "< Outside air temperature in °C that could be used to compute true airspeed from differential pressure."] - pub air_temperature: f32, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct EkfQuat { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< Orientation quaternion stored in W, X, Y, Z form."] - pub quaternion: [f32; 4usize], - #[doc = "< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad."] - pub euler_std_dev: [f32; 3usize], - #[doc = "< EKF solution status bitmask and enum."] - pub status: u32, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct EkfNav1 { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< North, East, Down velocity in m.s^-1."] - pub velocity: [f32; 3usize], - #[doc = "< North, East, Down velocity 1 sigma standard deviation in m.s^-1."] - pub velocity_std_dev: [f32; 3usize], -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct EkfNav2 { - #[doc = "< Latitude, Longitude in degrees positive North and East.\nAltitude above Mean Sea Level in meters."] - pub position: [f64; 3usize], - #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] - pub undulation: f32, - #[doc = "< Latitude, longitude and altitude 1 sigma standard deviation in meters."] - pub position_std_dev: [f32; 3usize], - #[doc = "< EKF solution status bitmask and enum."] - pub status: u32, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct Imu1 { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< IMU status bitmask."] - pub status: u16, - #[doc = "< X, Y, Z accelerometers in m.s^-2."] - pub accelerometers: [f32; 3usize], - #[doc = "< X, Y, Z gyroscopes in rad.s^-1."] - pub gyroscopes: [f32; 3usize], -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct Imu2 { - #[doc = "< Internal temperature in °C."] - pub temperature: f32, - #[doc = "< X, Y, Z delta velocity in m.s^-2."] - pub delta_velocity: [f32; 3usize], - #[doc = "< X, Y, Z delta angle in rad.s^-1."] - pub delta_angle: [f32; 3usize], -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct GpsVel { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< GPS velocity status, type and bitmask."] - pub status: u32, - #[doc = "< GPS time of week in ms."] - pub time_of_week: u32, - #[doc = "< GPS North, East, Down velocity in m.s^-1."] - pub velocity: [f32; 3usize], - #[doc = "< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1."] - pub velocity_acc: [f32; 3usize], - #[doc = "< Track ground course in degrees."] - pub course: f32, - #[doc = "< Course accuracy in degrees."] - pub course_acc: f32, -} - -impl Sensor { - pub fn new(data: impl Into) -> Self { - Sensor { - component_id: 0, - data: data.into(), - } - } -} +use defmt::Format; +use derive_more::From; +use serde::{Deserialize, Serialize}; + +#[cfg(test)] +use proptest_derive::Arbitrary; + +#[cfg(feature = "ts")] +use ts_rs::TS; + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Sensor { + /// Used to differentiate between multiple components on the same sender. Unused right now. + pub component_id: u8, + pub data: SensorData, +} + +#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub enum SensorData { + UtcTime(UtcTime), + Air(Air), + EkfQuat(EkfQuat), + EkfNav1(EkfNav1), + EkfNav2(EkfNav2), + Imu1(Imu1), + Imu2(Imu2), + GpsVel(GpsVel), + GpsPos1(GpsPos1), + GpsPos2(GpsPos2), + Current(Current), + Voltage(Voltage), + Regulator(Regulator), + Temperature(Temperature), +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct GpsPos1 { + #[doc = "< Time in us since the sensor power up."] + pub timeStamp: u32, + #[doc = "< GPS position status, type and bitmask."] + pub status: u32, + #[doc = "< GPS time of week in ms."] + pub timeOfWeek: u32, + #[doc = "< Latitude in degrees, positive north."] + pub latitude: f64, + #[doc = "< Longitude in degrees, positive east."] + pub longitude: f64, + #[doc = "< Altitude above Mean Sea Level in meters."] + pub altitude: f64, + #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] + pub undulation: f32, +} + + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct GpsPos2 { + #[doc = "< 1 sigma latitude accuracy in meters."] + pub latitudeAccuracy: f32, + #[doc = "< 1 sigma longitude accuracy in meters."] + pub longitudeAccuracy: f32, + #[doc = "< 1 sigma altitude accuracy in meters."] + pub altitudeAccuracy: f32, + #[doc = "< Number of space vehicles used to compute the solution (since version 1.4)."] + pub numSvUsed: u8, + #[doc = "< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4)."] + pub baseStationId: u16, + #[doc = "< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4)."] + pub differentialAge: u16, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Regulator { + pub status: bool, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Voltage { + pub voltage: f32, + pub rolling_avg: f32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Current { + pub current: f32, + pub rolling_avg: f32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Temperature { + pub temperature: f32, + pub rolling_avg: f32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct UtcTime { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< UTC time and clock status information"] + pub status: u16, + #[doc = "< Year for example: 2013."] + pub year: u16, + #[doc = "< Month in year [1 .. 12]."] + pub month: i8, + #[doc = "< Day in month [1 .. 31]."] + pub day: i8, + #[doc = "< Hour in day [0 .. 23]."] + pub hour: i8, + #[doc = "< Minute in hour [0 .. 59]."] + pub minute: i8, + #[doc = "< Second in minute [0 .. 60]. (60 is used only when a leap second is added)"] + pub second: i8, + #[doc = "< Nanosecond of current second in ns."] + pub nano_second: i32, + #[doc = "< GPS time of week in ms."] + pub gps_time_of_week: u32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Air { + #[doc = "< Time in us since the sensor power up OR measurement delay in us."] + pub time_stamp: u32, + #[doc = "< Airdata sensor status bitmask."] + pub status: u16, + #[doc = "< Raw absolute pressure measured by the barometer sensor in Pascals."] + pub pressure_abs: f32, + #[doc = "< Altitude computed from barometric altimeter in meters and positive upward."] + pub altitude: f32, + #[doc = "< Raw differential pressure measured by the pitot tube in Pascal."] + pub pressure_diff: f32, + #[doc = "< True airspeed measured by a pitot tube in m.s^-1 and positive forward."] + pub true_airspeed: f32, + #[doc = "< Outside air temperature in °C that could be used to compute true airspeed from differential pressure."] + pub air_temperature: f32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct EkfQuat { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< Orientation quaternion stored in W, X, Y, Z form."] + pub quaternion: [f32; 4usize], + #[doc = "< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad."] + pub euler_std_dev: [f32; 3usize], + #[doc = "< EKF solution status bitmask and enum."] + pub status: u32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct EkfNav1 { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< North, East, Down velocity in m.s^-1."] + pub velocity: [f32; 3usize], + #[doc = "< North, East, Down velocity 1 sigma standard deviation in m.s^-1."] + pub velocity_std_dev: [f32; 3usize], +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct EkfNav2 { + #[doc = "< Latitude, Longitude in degrees positive North and East.\nAltitude above Mean Sea Level in meters."] + pub position: [f64; 3usize], + #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] + pub undulation: f32, + #[doc = "< Latitude, longitude and altitude 1 sigma standard deviation in meters."] + pub position_std_dev: [f32; 3usize], + #[doc = "< EKF solution status bitmask and enum."] + pub status: u32, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Imu1 { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< IMU status bitmask."] + pub status: u16, + #[doc = "< X, Y, Z accelerometers in m.s^-2."] + pub accelerometers: [f32; 3usize], + #[doc = "< X, Y, Z gyroscopes in rad.s^-1."] + pub gyroscopes: [f32; 3usize], +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct Imu2 { + #[doc = "< Internal temperature in °C."] + pub temperature: f32, + #[doc = "< X, Y, Z delta velocity in m.s^-2."] + pub delta_velocity: [f32; 3usize], + #[doc = "< X, Y, Z delta angle in rad.s^-1."] + pub delta_angle: [f32; 3usize], +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct GpsVel { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< GPS velocity status, type and bitmask."] + pub status: u32, + #[doc = "< GPS time of week in ms."] + pub time_of_week: u32, + #[doc = "< GPS North, East, Down velocity in m.s^-1."] + pub velocity: [f32; 3usize], + #[doc = "< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1."] + pub velocity_acc: [f32; 3usize], + #[doc = "< Track ground course in degrees."] + pub course: f32, + #[doc = "< Course accuracy in degrees."] + pub course_acc: f32, +} + +impl Sensor { + pub fn new(data: impl Into) -> Self { + Sensor { + component_id: 0, + data: data.into(), + } + } +} diff --git a/libraries/messages/src/state.rs b/libraries/messages/src/state.rs new file mode 100644 index 00000000..9a2c8c5f --- /dev/null +++ b/libraries/messages/src/state.rs @@ -0,0 +1,38 @@ +use defmt::Format; +use serde::{Deserialize, Serialize}; + +#[cfg(test)] +use proptest_derive::Arbitrary; + +#[cfg(feature = "ts")] +use ts_rs::TS; + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub struct State { + pub data: StateData, +} + +#[derive(Serialize, Deserialize, Clone, Debug, Format)] +#[cfg_attr(test, derive(Arbitrary))] +#[cfg_attr(feature = "ts", derive(TS))] +#[cfg_attr(feature = "ts", ts(export))] +pub enum StateData { + Initializing, + WaitForTakeoff, + Ascent, + Descent, + TerminalDescent, + WaitForRecovery, + Abort, +} + +impl State { + pub fn new(data: impl Into) -> Self { + State { + data: data.into(), + } + } +} \ No newline at end of file diff --git a/libraries/sbg-rs/Cargo.toml b/libraries/sbg-rs/Cargo.toml index efd546bc..8e3e60a1 100644 --- a/libraries/sbg-rs/Cargo.toml +++ b/libraries/sbg-rs/Cargo.toml @@ -1,19 +1,20 @@ -[package] -name = "sbg-rs" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -embedded-hal = "0.2.7" -nb = "1.0.0" -defmt = "0.3.2" -atsamd-hal = { workspace = true } -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -messages = { path = "../../libraries/messages" } -common-arm = { path = "../../libraries/common-arm" } - -[build-dependencies] +[package] +name = "sbg-rs" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +embedded-hal = "0.2.7" +nb = "1.0.0" +defmt = "0.3.2" +atsamd-hal = { workspace = true } +cortex-m = { workspace = true } +cortex-m-rt = "^0.7.0" +messages = { path = "../../libraries/messages" } +common-arm = { path = "../../libraries/common-arm" } +heapless = "0.7.16" + +[build-dependencies] cmake = "0.1" \ No newline at end of file diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index 1e0007e5..84dc95d0 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -1,7 +1,31 @@ use crate::bindings::{ - SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, + SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, SbgLogGpsPos, }; -use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime}; +use messages::sensor::{Air, EkfNav1, EkfNav2, EkfQuat, GpsVel, Imu1, Imu2, UtcTime, GpsPos1, GpsPos2}; + +impl From for (GpsPos1, GpsPos2) { + fn from(value: SbgLogGpsPos) -> Self { + ( + GpsPos1 { + timeStamp: value.timeStamp, + status: value.status, + timeOfWeek: value.timeOfWeek, + latitude: value.latitude, + longitude: value.longitude, + altitude: value.altitude, + undulation: value.undulation, + }, + GpsPos2 { + latitudeAccuracy: value.latitudeAccuracy, + longitudeAccuracy: value.longitudeAccuracy, + altitudeAccuracy: value.altitudeAccuracy, + numSvUsed: value.numSvUsed, + baseStationId: value.baseStationId, + differentialAge: value.differentialAge, + }, + ) + } +} impl From for UtcTime { fn from(value: SbgLogUtcData) -> Self { diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index b6817e95..cef06c56 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -1,530 +1,553 @@ -use crate::bindings::{ - self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR, - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, - _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, - _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, CStr}; -use core::ptr::null_mut; -use core::slice::{from_raw_parts, from_raw_parts_mut}; -use core::sync::atomic::AtomicUsize; -use defmt::{debug, error, flush, info, warn}; -use embedded_hal::serial::Write; -use hal::gpio::{PA08, PA09, PB16, PB17}; -use hal::sercom::uart::Duplex; -use hal::sercom::uart::{self, EightBit, Uart}; -use hal::sercom::{IoSet1, Sercom0, Sercom5}; -use messages::sensor::*; - -type Pads = uart::PadsFromIds; -type PadsCDC = uart::PadsFromIds; -type Config = uart::Config; - -/** - * Max buffer size for SBG messages. - */ -pub const SBG_BUFFER_SIZE: usize = 1024; - -/** - * Represents the index of the buffer that is currently being used. - */ -static mut BUF_INDEX: AtomicUsize = AtomicUsize::new(0); -/** - * Points to the buffer that is currently being used. - */ -static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE]; - -/** - * Holds the RTC instance. This is used to get the current time. - */ -static mut RTC: Option> = None; - -static mut DATA_CALLBACK: Option = None; - -pub enum CallbackData { - UtcTime(UtcTime), - Air(Air), - EkfQuat(EkfQuat), - EkfNav((EkfNav1, EkfNav2)), - Imu((Imu1, Imu2)), - GpsVel(GpsVel), -} - -struct UARTSBGInterface { - interface: *mut bindings::SbgInterface, -} - -pub struct SBG { - UARTSBGInterface: UARTSBGInterface, - serial_device: Uart, - handle: _SbgEComHandle, - isInitialized: bool, -} - -impl SBG { - /** - * Creates a new SBG instance. - * Takes ownership of the serial device and RTC instance. - */ - pub fn new( - mut serial_device: Uart, - rtc: hal::rtc::Rtc, - callback: fn(CallbackData), - ) -> 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!"); - } - // 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, - type_: 0, - name: [0; 48], - pDestroyFunc: Some(SBG::SbgDestroyFunc), - pWriteFunc: Some(SBG::SbgInterfaceWriteFunc), - pReadFunc: Some(SBG::SbgInterfaceReadFunc), - pFlushFunc: Some(SBG::SbgFlushFunc), - pSetSpeedFunc: Some(SBG::SbgSetSpeedFunc), - pGetSpeedFunc: Some(SBG::SbgGetSpeedFunc), - pDelayFunc: Some(SBG::SbgDelayFunc), - }, - }; - let pLargeBuffer: *mut u8 = null_mut(); - let protocol: _SbgEComProtocol = _SbgEComProtocol { - pLinkedInterface: interface.interface, - rxBuffer: [0; 4096usize], - rxBufferSize: 0, - discardSize: 0, - nextLargeTxId: 0, - pLargeBuffer, - largeBufferSize: 0, - msgClass: 0, - msgId: 0, - transferId: 0, - pageIndex: 0, - nrPages: 0, - }; - let handle: _SbgEComHandle = _SbgEComHandle { - protocolHandle: protocol, - pReceiveLogCallback: Some(SBG::SbgEComReceiveLogFunc), - pUserArg: null_mut(), - numTrials: 3, - 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. - */ - pub fn isInitialized(&self) -> bool { - self.isInitialized - } - /** - * 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]) { - // SAFETY: We are assigning a static mut variable. - // Buf can only be accessed from functions called by sbgEComHandle after this assignment. - unsafe { BUF = buffer }; - // SAFETY: We are assigning a static variable. - // This is safe because are the only thread reading since SBG is locked. - unsafe { - *BUF_INDEX.get_mut() = 0; - } - // SAFETY: We are calling a C function. - // This is safe because it is assumed the SBG library is safe. - unsafe { - sbgEComHandle(&mut self.handle); - } - } - - /** - * Configures the SBG to output the following data - * Air data - * IMU data - * Extended Kalman Filter Euler data - * Extended Kalman Filter Quaternions - * Extended Kalman Filter Navigation data - */ - pub fn setup(&mut self) -> u32 { - // SAFETY: We are calling a C function. - // This is safe because it is assumed the SBG library is safe. - 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_VEL, - _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 { - sbgEComCmdOutputSetConf( - &mut self.handle, - _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, - _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, - _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, - _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 { - sbgEComCmdOutputSetConf( - &mut self.handle, - _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, - _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, - _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, - ) - }; - if errorCode != _SbgErrorCode_SBG_NO_ERROR { - warn!("Unable to configure Air Data 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 = unsafe { - sbgEComCmdOutputSetConf( - &mut self.handle, - _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, - _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, - _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, - ) - }; - if errorCode != _SbgErrorCode_SBG_NO_ERROR { - warn!("Unable to configure EKF Quat 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 = unsafe { - sbgEComCmdOutputSetConf( - &mut self.handle, - _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, - _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, - _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, - ) - }; - if errorCode != _SbgErrorCode_SBG_NO_ERROR { - warn!("Unable to configure EKF Nav 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 = unsafe { - sbgEComCmdOutputSetConf( - &mut self.handle, - _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, - _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, - _SbgEComLog_SBG_ECOM_LOG_IMU_DATA, - _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, - ) - }; - if errorCode != _SbgErrorCode_SBG_NO_ERROR { - warn!("Unable to configure IMU logs to 40 cycles"); - } else { - self.isInitialized = true; - }; - errorCode - } - - /** - * Allows the SBG interface to read data from the serial ports. - */ - pub unsafe extern "C" fn SbgInterfaceReadFunc( - _pInterface: *mut _SbgInterface, - pBuffer: *mut c_void, - pBytesRead: *mut usize, - mut bytesToRead: usize, - ) -> _SbgErrorCode { - if pBuffer.is_null() { - return _SbgErrorCode_SBG_NULL_POINTER; - } - if pBytesRead.is_null() { - return _SbgErrorCode_SBG_NULL_POINTER; - } - // 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: &mut [u8] = unsafe { from_raw_parts_mut(pBuffer as *mut u8, bytesToRead) }; - // SAFETY: We are accessing a static mut variable. - // This is safe because we ensure that the variable is only accessed in this function. - let index = unsafe { *BUF_INDEX.get_mut() }; - - if index + bytesToRead > SBG_BUFFER_SIZE { - // Read what we can. - bytesToRead = SBG_BUFFER_SIZE - index; - if bytesToRead == 0 { - // SAFETY: We are accessing a mutable pointer. - // This is safe because the pointer cannot be null - // and the SBGECom library ensures that the pointer - // is not accessed during the lifetime of this function. - unsafe { *pBytesRead = 0 }; - return _SbgErrorCode_SBG_READ_ERROR; // no data - } - let end = bytesToRead + index; - // SAFETY: We are accessing a static mut variable. - // This is safe because we ensure that the variable is only accessed in this function. - array[0..bytesToRead - 1].copy_from_slice(unsafe { &BUF[index..end - 1] }); - // SAFETY: We are accessing a static mut variable. - // This is safe because we ensure that the variable is only accessed in this function. - unsafe { *BUF_INDEX.get_mut() = index + bytesToRead }; - // SAFETY: We are accessing a mutable pointer. - // This is safe because the pointer cannot be null - // and the SBGECom library ensures that the pointer - // is not accessed during the lifetime of this function. - unsafe { *pBytesRead = bytesToRead }; - return _SbgErrorCode_SBG_NO_ERROR; - } - let end = bytesToRead + index; - // SAFETY: We are accessing a static mut variable. - // This is safe because we ensure that the variable is only accessed in this function. - array[0..bytesToRead - 1].copy_from_slice(unsafe { &BUF[index..end - 1] }); - // SAFETY: We are accessing a static mut variable. - // This is safe because we ensure that the variable is only accessed in this function. - unsafe { *BUF_INDEX.get_mut() = index + bytesToRead }; - // SAFETY: We are accessing a mutable pointer. - // This is safe because the pointer cannot be null - // and the SBGECom library ensures that the pointer - // is not accessed during the lifetime of this function. - unsafe { *pBytesRead = bytesToRead }; - - _SbgErrorCode_SBG_NO_ERROR - } - - /** - * Allows the SBG interface to write to the UART peripheral - */ - pub unsafe extern "C" fn SbgInterfaceWriteFunc( - pInterface: *mut _SbgInterface, - pBuffer: *const c_void, - bytesToWrite: usize, - ) -> _SbgErrorCode { - if pInterface.is_null() { - return _SbgErrorCode_SBG_NULL_POINTER; - } - 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, - } - } - _SbgErrorCode_SBG_NO_ERROR - } - - /** - * Callback function for handling logs. - */ - pub unsafe extern "C" fn SbgEComReceiveLogFunc( - _pHandle: *mut _SbgEComHandle, - msgClass: u32, - msg: u32, - pLogData: *const _SbgBinaryLogData, - _pUserArg: *mut c_void, - ) -> _SbgErrorCode { - if pLogData.is_null() { - return _SbgErrorCode_SBG_NULL_POINTER; - } - - // SAFETY: DATA_CALLBACK is set once, before this function is called, - // so no race conditions can happen. - if let Some(callback) = unsafe { DATA_CALLBACK } { - if msgClass == _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0 { - // SAFETY: pLogData is not null, and we are checking the union flag before accessing it - unsafe { - match msg { - _SbgEComLog_SBG_ECOM_LOG_AIR_DATA => { - callback(CallbackData::Air((*pLogData).airData.into())) - } - _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT => { - callback(CallbackData::EkfQuat((*pLogData).ekfQuatData.into())) - } - _SbgEComLog_SBG_ECOM_LOG_IMU_DATA => { - callback(CallbackData::Imu((*pLogData).imuData.into())) - } - _SbgEComLog_SBG_ECOM_LOG_EKF_NAV => { - callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) - } - _ => (), - } - } - } - } - - _SbgErrorCode_SBG_NO_ERROR - } - - /** - * The SBG interface does not need to be destroyed. - */ - pub extern "C" fn SbgDestroyFunc(_pInterface: *mut _SbgInterface) -> _SbgErrorCode { - _SbgErrorCode_SBG_NO_ERROR - } - - /** - * Flushes the UART peripheral. - */ - pub unsafe extern "C" fn SbgFlushFunc( - pInterface: *mut _SbgInterface, - _flags: u32, - ) -> _SbgErrorCode { - if pInterface.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 }; - let result = unsafe { serial.as_mut().unwrap().flush() }; - match result { - Ok(_) => return _SbgErrorCode_SBG_NO_ERROR, - Err(_) => return _SbgErrorCode_SBG_READ_ERROR, - } - } - - /** - * The baud rate is fixed to 115200 and hence this function does nothing. - */ - pub extern "C" fn SbgSetSpeedFunc( - _pInterface: *mut _SbgInterface, - _speed: u32, - ) -> _SbgErrorCode { - _SbgErrorCode_SBG_NO_ERROR - } - - /** - * The baud rate is fixed to 115200 - */ - pub extern "C" fn SbgGetSpeedFunc(_pInterface: *const _SbgInterface) -> u32 { - 115200 - } - - /** - * Optional method used to compute an expected delay to transmit/receive X bytes - */ - pub extern "C" fn SbgDelayFunc(_pInterface: *const _SbgInterface, _numBytes: usize) -> u32 { - 501 - } -} - -// SAFETY: No one besides us has the raw pointer to the SBG struct. -// We can safely transfer the SBG struct between threads. -unsafe impl Send for SBG {} - -/** - * Logs the message to the console. - * Needs to be updated to handle the Variadic arguments. - */ -#[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, - logType: _SbgDebugLogType, - _errorCode: _SbgErrorCode, - pFormat: *const ::core::ffi::c_char, -) { - 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() }; - - match logType { - // silently handle errors - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error {} {}", file, function), - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning {} {}", file, function), - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info {} {}", file, function), - _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug {} {}", file, function), - _ => (), - }; - flush(); -} - -/** - * Returns the number of milliseconds that have passed. - */ -#[no_mangle] -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. - } - } -} - -/** - * Sleeps the sbg execution - */ -#[no_mangle] -pub extern "C" fn sbgSleep(ms: u32) { - let start_time = sbgGetTime(); - while (sbgGetTime() - start_time) < ms { - // do nothing - } -} +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_VEL, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, + _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, error, info}; +use embedded_hal::serial::Write; +use hal::gpio::{PB16, PB17, PB03, PB02}; +use hal::sercom::uart::Duplex; +use hal::sercom::uart::{self, EightBit, Uart}; +use hal::sercom::{IoSet1, IoSet6, Sercom5}; +use messages::sensor::*; +use heapless::Deque; + +type Pads = uart::PadsFromIds; +type PadsCDC = uart::PadsFromIds; +type Config = uart::Config; + +/** + * Max buffer size for SBG messages. + */ +pub const SBG_BUFFER_SIZE: usize = 1024; + +/** + * Represents the index of the buffer that is currently being used. + */ +static mut BUF_INDEX: AtomicUsize = AtomicUsize::new(0); +/** + * Points to the buffer that is currently being used. + */ +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 DATA_CALLBACK: Option = None; + +pub enum CallbackData { + UtcTime(UtcTime), + Air(Air), + EkfQuat(EkfQuat), + EkfNav((EkfNav1, EkfNav2)), + Imu((Imu1, Imu2)), + GpsVel(GpsVel), + GpsPos((GpsPos1, GpsPos2)), +} + +struct UARTSBGInterface { + interface: *mut bindings::SbgInterface, +} + +pub struct SBG { + UARTSBGInterface: UARTSBGInterface, + serial_device: Uart, + handle: _SbgEComHandle, + isInitialized: bool, +} + +impl SBG { + /** + * Creates a new SBG instance. + * Takes ownership of the serial device and RTC instance. + */ + pub fn new( + mut serial_device: Uart, + rtc: hal::rtc::Rtc, + callback: fn(CallbackData), + ) -> 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!"); + } + // 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, + type_: 0, + name: [0; 48], + pDestroyFunc: Some(SBG::SbgDestroyFunc), + pWriteFunc: Some(SBG::SbgInterfaceWriteFunc), + pReadFunc: Some(SBG::SbgInterfaceReadFunc), + pFlushFunc: Some(SBG::SbgFlushFunc), + pSetSpeedFunc: Some(SBG::SbgSetSpeedFunc), + pGetSpeedFunc: Some(SBG::SbgGetSpeedFunc), + pDelayFunc: Some(SBG::SbgDelayFunc), + }, + }; + let pLargeBuffer: *mut u8 = null_mut(); + let protocol: _SbgEComProtocol = _SbgEComProtocol { + pLinkedInterface: interface.interface, + rxBuffer: [0; 4096usize], + rxBufferSize: 0, + discardSize: 0, + nextLargeTxId: 0, + pLargeBuffer, + largeBufferSize: 0, + msgClass: 0, + msgId: 0, + transferId: 0, + pageIndex: 0, + nrPages: 0, + }; + let handle: _SbgEComHandle = _SbgEComHandle { + protocolHandle: protocol, + pReceiveLogCallback: Some(SBG::SbgEComReceiveLogFunc), + pUserArg: null_mut(), + numTrials: 3, + 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. + */ + pub fn isInitialized(&self) -> bool { + self.isInitialized + } + /** + * 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]) { + // SAFETY: We are assigning a static mut variable. + // Buf can only be accessed from functions called by sbgEComHandle after this assignment. + // unsafe { BUF = buffer }; + for i in buffer { + unsafe{DEQ.push_back(*i)}; + } + // SAFETY: We are assigning a static variable. + // This is safe because are the only thread reading since SBG is locked. + unsafe { + *BUF_INDEX.get_mut() = 0; + } + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + unsafe { + sbgEComHandle(&mut self.handle); + } + } + + /** + * Configures the SBG to output the following data + * Air data + * IMU data + * Extended Kalman Filter Euler data + * Extended Kalman Filter Quaternions + * Extended Kalman Filter Navigation data + */ + pub fn setup(&mut self) -> u32 { + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + 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_VEL, + _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 { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, + _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 { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure Air Data 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 = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure EKF Quat 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 = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure EKF Nav 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 = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_IMU_DATA, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if errorCode != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure IMU logs to 40 cycles"); + } else { + self.isInitialized = true; + }; + errorCode + } + + /** + * Allows the SBG interface to read data from the serial ports. + */ + pub unsafe extern "C" fn SbgInterfaceReadFunc( + _pInterface: *mut _SbgInterface, + pBuffer: *mut c_void, + pBytesRead: *mut usize, + mut bytesToRead: usize, + ) -> _SbgErrorCode { + if pBuffer.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + if pBytesRead.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + // 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: &mut [u8] = unsafe { from_raw_parts_mut(pBuffer as *mut u8, bytesToRead) }; + // SAFETY: We are accessing a static mut variable. + // This is safe because we ensure that the variable is only accessed in this function. + let index = unsafe { *BUF_INDEX.get_mut() }; + let mut readBytes = 0; + for i in 0..(bytesToRead) { + if let Some(front) = DEQ.pop_front() { + readBytes += 1; + array[i] = front; + } else { + // info!("No item in dequeue"); + break; + } + } + // info!("Bytes Read {}", readBytes); + unsafe {*pBytesRead = readBytes}; + return _SbgErrorCode_SBG_NO_ERROR; + + // if index + bytesToRead > SBG_BUFFER_SIZE { + // // Read what we can. + // bytesToRead = SBG_BUFFER_SIZE - index; + // if bytesToRead == 0 { + // // SAFETY: We are accessing a mutable pointer. + // // This is safe because the pointer cannot be null + // // and the SBGECom library ensures that the pointer + // // is not accessed during the lifetime of this function. + // unsafe { *pBytesRead = 0 }; + // return _SbgErrorCode_SBG_READ_ERROR; // no data + // } + // let end = bytesToRead + index; + // // SAFETY: We are accessing a static mut variable. + // // This is safe because we ensure that the variable is only accessed in this function. + // array[0..bytesToRead - 1].copy_from_slice(unsafe { &BUF[index..end - 1] }); + // // SAFETY: We are accessing a static mut variable. + // // This is safe because we ensure that the variable is only accessed in this function. + // unsafe { *BUF_INDEX.get_mut() = index + bytesToRead }; + // // SAFETY: We are accessing a mutable pointer. + // // This is safe because the pointer cannot be null + // // and the SBGECom library ensures that the pointer + // // is not accessed during the lifetime of this function. + // unsafe { *pBytesRead = bytesToRead }; + // return _SbgErrorCode_SBG_NO_ERROR; + // } + // let end = bytesToRead + index; + // // SAFETY: We are accessing a static mut variable. + // // This is safe because we ensure that the variable is only accessed in this function. + // array[0..bytesToRead - 1].copy_from_slice(unsafe { &BUF[index..end - 1] }); + // // SAFETY: We are accessing a static mut variable. + // // This is safe because we ensure that the variable is only accessed in this function. + // unsafe { *BUF_INDEX.get_mut() = index + bytesToRead }; + // // SAFETY: We are accessing a mutable pointer. + // // This is safe because the pointer cannot be null + // // and the SBGECom library ensures that the pointer + // // is not accessed during the lifetime of this function. + // unsafe { *pBytesRead = bytesToRead }; + + // _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Allows the SBG interface to write to the UART peripheral + */ + pub unsafe extern "C" fn SbgInterfaceWriteFunc( + pInterface: *mut _SbgInterface, + pBuffer: *const c_void, + bytesToWrite: usize, + ) -> _SbgErrorCode { + if pInterface.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + 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, + } + } + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Callback function for handling logs. + */ + pub unsafe extern "C" fn SbgEComReceiveLogFunc( + _pHandle: *mut _SbgEComHandle, + msgClass: u32, + msg: u32, + pLogData: *const _SbgBinaryLogData, + _pUserArg: *mut c_void, + ) -> _SbgErrorCode { + if pLogData.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + + // SAFETY: DATA_CALLBACK is set once, before this function is called, + // so no race conditions can happen. + if let Some(callback) = unsafe { DATA_CALLBACK } { + if msgClass == _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0 { + // SAFETY: pLogData is not null, and we are checking the union flag before accessing it + unsafe { + match msg { + _SbgEComLog_SBG_ECOM_LOG_AIR_DATA => { + callback(CallbackData::Air((*pLogData).airData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT => { + callback(CallbackData::EkfQuat((*pLogData).ekfQuatData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_IMU_DATA => { + callback(CallbackData::Imu((*pLogData).imuData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_EKF_NAV => { + callback(CallbackData::EkfNav((*pLogData).ekfNavData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { + callback(CallbackData::GpsPos((*pLogData).gpsPosData.into())) + } + _ => (), + } + } + } + } + + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * The SBG interface does not need to be destroyed. + */ + pub extern "C" fn SbgDestroyFunc(_pInterface: *mut _SbgInterface) -> _SbgErrorCode { + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Flushes the UART peripheral. + */ + pub unsafe extern "C" fn SbgFlushFunc( + pInterface: *mut _SbgInterface, + _flags: u32, + ) -> _SbgErrorCode { + if pInterface.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 }; + let result = unsafe { serial.as_mut().unwrap().flush() }; + match result { + Ok(_) => return _SbgErrorCode_SBG_NO_ERROR, + Err(_) => return _SbgErrorCode_SBG_READ_ERROR, + } + } + + /** + * The baud rate is fixed to 115200 and hence this function does nothing. + */ + pub extern "C" fn SbgSetSpeedFunc( + _pInterface: *mut _SbgInterface, + _speed: u32, + ) -> _SbgErrorCode { + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * The baud rate is fixed to 115200 + */ + pub extern "C" fn SbgGetSpeedFunc(_pInterface: *const _SbgInterface) -> u32 { + 115200 + } + + /** + * Optional method used to compute an expected delay to transmit/receive X bytes + */ + pub extern "C" fn SbgDelayFunc(_pInterface: *const _SbgInterface, _numBytes: usize) -> u32 { + 501 + } +} + +// SAFETY: No one besides us has the raw pointer to the SBG struct. +// We can safely transfer the SBG struct between threads. +unsafe impl Send for SBG {} + +/** + * Logs the message to the console. + * Needs to be updated to handle the Variadic arguments. + */ +#[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, + logType: _SbgDebugLogType, + _errorCode: _SbgErrorCode, + _pFormat: *const ::core::ffi::c_char, +) { + // 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() }; + + 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), + _ => (), + }; + flush(); +} + +/** + * Returns the number of milliseconds that have passed. + */ +#[no_mangle] +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. + } + } +} + +/** + * Sleeps the sbg execution + */ +#[no_mangle] +pub extern "C" fn sbgSleep(ms: u32) { + let start_time = sbgGetTime(); + while (sbgGetTime() - start_time) < ms { + // do nothing + } +}