From 9b8b847e7b681438ae70e54e0470e7a8968ead44 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 16 Apr 2024 22:42:43 -0400 Subject: [PATCH 001/157] LEDs, ADC working --- flake.lock | 66 +- kicker-board-v3/.cargo/config.toml | 8 + kicker-board-v3/.gitignore | 1 + kicker-board-v3/.vscode/settings.json | 4 + kicker-board-v3/Cargo.lock | 1398 +++++++++++++++++ kicker-board-v3/Cargo.toml | 101 ++ kicker-board-v3/build.rs | 6 + kicker-board-v3/memory.x | 12 + kicker-board-v3/rust-toolchain.toml | 3 + kicker-board-v3/src/bin/hwtest-blinky/main.rs | 103 ++ .../src/bin/hwtest-breakbeam/main.rs | 84 + kicker-board-v3/src/bin/hwtest-charge/main.rs | 191 +++ kicker-board-v3/src/bin/hwtest-coms/main.rs | 278 ++++ kicker-board-v3/src/bin/hwtest-kick/main.rs | 227 +++ kicker-board-v3/src/bin/kicker/main.rs | 468 ++++++ kicker-board-v3/src/drivers/breakbeam.rs | 32 + kicker-board-v3/src/drivers/breakbeam_pwm.rs | 37 + kicker-board-v3/src/drivers/mod.rs | 1 + kicker-board-v3/src/kick_manager.rs | 167 ++ kicker-board-v3/src/lib.rs | 44 + kicker-board-v3/src/pins.rs | 29 + kicker-board-v3/src/queue.rs | 220 +++ kicker-board-v3/src/uart_queue.rs | 206 +++ 23 files changed, 3671 insertions(+), 15 deletions(-) create mode 100644 kicker-board-v3/.cargo/config.toml create mode 100644 kicker-board-v3/.gitignore create mode 100644 kicker-board-v3/.vscode/settings.json create mode 100644 kicker-board-v3/Cargo.lock create mode 100644 kicker-board-v3/Cargo.toml create mode 100644 kicker-board-v3/build.rs create mode 100644 kicker-board-v3/memory.x create mode 100644 kicker-board-v3/rust-toolchain.toml create mode 100644 kicker-board-v3/src/bin/hwtest-blinky/main.rs create mode 100644 kicker-board-v3/src/bin/hwtest-breakbeam/main.rs create mode 100644 kicker-board-v3/src/bin/hwtest-charge/main.rs create mode 100644 kicker-board-v3/src/bin/hwtest-coms/main.rs create mode 100644 kicker-board-v3/src/bin/hwtest-kick/main.rs create mode 100644 kicker-board-v3/src/bin/kicker/main.rs create mode 100644 kicker-board-v3/src/drivers/breakbeam.rs create mode 100644 kicker-board-v3/src/drivers/breakbeam_pwm.rs create mode 100644 kicker-board-v3/src/drivers/mod.rs create mode 100644 kicker-board-v3/src/kick_manager.rs create mode 100644 kicker-board-v3/src/lib.rs create mode 100644 kicker-board-v3/src/pins.rs create mode 100644 kicker-board-v3/src/queue.rs create mode 100644 kicker-board-v3/src/uart_queue.rs diff --git a/flake.lock b/flake.lock index a15ad287..6858befd 100644 --- a/flake.lock +++ b/flake.lock @@ -1,12 +1,15 @@ { "nodes": { "flake-utils": { + "inputs": { + "systems": "systems" + }, "locked": { - "lastModified": 1676283394, - "narHash": "sha256-XX2f9c3iySLCw54rJ/CZs+ZK6IQy7GXNY4nSOyu2QG4=", + "lastModified": 1710146030, + "narHash": "sha256-SZ5L6eA7HJ/nmkzGG7/ISclqe6oZdOZTNoesiInkXPQ=", "owner": "numtide", "repo": "flake-utils", - "rev": "3db36a8b464d0c4532ba1c7dda728f4576d6d073", + "rev": "b1d9ab70662946ef0850d488da1c9019f3a9752a", "type": "github" }, "original": { @@ -16,12 +19,15 @@ } }, "flake-utils_2": { + "inputs": { + "systems": "systems_2" + }, "locked": { - "lastModified": 1659877975, - "narHash": "sha256-zllb8aq3YO3h8B/U0/J1WBgAL8EX5yWf5pMj3G0NAmc=", + "lastModified": 1705309234, + "narHash": "sha256-uNRRNRKmJyCRC/8y1RqBkqWBLM034y4qN7EprSdmgyA=", "owner": "numtide", "repo": "flake-utils", - "rev": "c0e246b9b83f637f4681389ecabcb2681b4f3af0", + "rev": "1ef2e671c3b0c19053962c07dbda38332dcebf26", "type": "github" }, "original": { @@ -32,11 +38,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1678314140, - "narHash": "sha256-B91P+zQbxc0vi72cexRbcaYak00qVGtEbiRnmfG/mdI=", + "lastModified": 1713315850, + "narHash": "sha256-7ARGHtepRSxFJntlBuaJAwPJx3qY7B8856TnJ+Nl5FY=", "owner": "nixos", "repo": "nixpkgs", - "rev": "94c379180a50d0e915bb49a9d567ecc4d3fb548b", + "rev": "518ebacf79b49361332d540ea91d85e2e573871b", "type": "github" }, "original": { @@ -47,11 +53,11 @@ }, "nixpkgs_2": { "locked": { - "lastModified": 1665296151, - "narHash": "sha256-uOB0oxqxN9K7XGF1hcnY+PQnlQJ+3bP2vCn/+Ru/bbc=", + "lastModified": 1706487304, + "narHash": "sha256-LE8lVX28MV2jWJsidW13D2qrHU/RUUONendL2Q/WlJg=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "14ccaaedd95a488dd7ae142757884d8e125b3363", + "rev": "90f456026d284c22b3e3497be980b2e47d0b28ac", "type": "github" }, "original": { @@ -74,11 +80,11 @@ "nixpkgs": "nixpkgs_2" }, "locked": { - "lastModified": 1678242776, - "narHash": "sha256-36K1Rg2vM+NLqORSBL4e3aZHmgkb6aS9upHsuG4Akns=", + "lastModified": 1713233539, + "narHash": "sha256-dPGrCy5ttx6E3bUOmDynY/cAotRqvoIAimZlbv+Zr1w=", "owner": "oxalica", "repo": "rust-overlay", - "rev": "ea311f10a5d51e7588799281bab0556b4e978d00", + "rev": "847bc25ebab8dc72a86d2b1f0c088740eebbb1b8", "type": "github" }, "original": { @@ -86,6 +92,36 @@ "repo": "rust-overlay", "type": "github" } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } } }, "root": "root", diff --git a/kicker-board-v3/.cargo/config.toml b/kicker-board-v3/.cargo/config.toml new file mode 100644 index 00000000..bc8dfc3e --- /dev/null +++ b/kicker-board-v3/.cargo/config.toml @@ -0,0 +1,8 @@ +[build] +target = "thumbv7em-none-eabihf" + +[target.thumbv7em-none-eabihf] +runner = 'probe-run --chip STM32F407VETx --connect-under-reset' + +[env] +DEFMT_LOG = "trace" diff --git a/kicker-board-v3/.gitignore b/kicker-board-v3/.gitignore new file mode 100644 index 00000000..2f7896d1 --- /dev/null +++ b/kicker-board-v3/.gitignore @@ -0,0 +1 @@ +target/ diff --git a/kicker-board-v3/.vscode/settings.json b/kicker-board-v3/.vscode/settings.json new file mode 100644 index 00000000..e9de4f95 --- /dev/null +++ b/kicker-board-v3/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + // tell rust-analyzer which ABI this crate targets + "rust-analyzer.cargo.target": "thumbv6m-none-eabi" +} \ No newline at end of file diff --git a/kicker-board-v3/Cargo.lock b/kicker-board-v3/Cargo.lock new file mode 100644 index 00000000..d4c89d32 --- /dev/null +++ b/kicker-board-v3/Cargo.lock @@ -0,0 +1,1398 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "aho-corasick" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" +dependencies = [ + "memchr", +] + +[[package]] +name = "ateam-common-packets" +version = "1.0.0" +dependencies = [ + "bindgen", + "which", +] + +[[package]] +name = "ateam-kicker-board-v3" +version = "0.1.0" +dependencies = [ + "ateam-common-packets", + "const_format", + "cortex-m", + "cortex-m-rt", + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "defmt-test", + "embassy-executor", + "embassy-futures", + "embassy-stm32", + "embassy-sync", + "embassy-time", + "futures-util", + "heapless 0.7.17", + "libm", + "panic-probe", + "static_cell", +] + +[[package]] +name = "atomic-polyfill" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" +dependencies = [ + "critical-section 1.1.2", +] + +[[package]] +name = "atty" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8" +dependencies = [ + "hermit-abi", + "libc", + "winapi", +] + +[[package]] +name = "autocfg" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" + +[[package]] +name = "bare-metal" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" +dependencies = [ + "rustc_version 0.2.3", +] + +[[package]] +name = "bare-metal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" + +[[package]] +name = "bindgen" +version = "0.60.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" +dependencies = [ + "bitflags 1.3.2", + "cexpr", + "clang-sys", + "clap", + "env_logger", + "lazy_static", + "lazycell", + "log", + "peeking_take_while", + "proc-macro2", + "quote", + "regex", + "rustc-hash", + "shlex", + "which", +] + +[[package]] +name = "bit_field" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" + +[[package]] +name = "bitfield" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46afbd2983a5d5a7bd740ccb198caf5b82f45c40c09c0eed36052d91cb92e719" + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cexpr" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" +dependencies = [ + "nom", +] + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" +dependencies = [ + "num-traits", +] + +[[package]] +name = "clang-sys" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" +dependencies = [ + "glob", + "libc", + "libloading", +] + +[[package]] +name = "clap" +version = "3.2.25" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" +dependencies = [ + "atty", + "bitflags 1.3.2", + "clap_lex", + "indexmap", + "strsim", + "termcolor", + "textwrap", +] + +[[package]] +name = "clap_lex" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2850f2f5a82cbf437dd5af4d49848fbdfc27c157c3d010345776f952765261c5" +dependencies = [ + "os_str_bytes", +] + +[[package]] +name = "const_format" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3a214c7af3d04997541b18d432afaff4c455e79e2029079647e72fc2bd27673" +dependencies = [ + "const_format_proc_macros", +] + +[[package]] +name = "const_format_proc_macros" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7f6ff08fd20f4f299298a28e2dfa8a8ba1036e6cd2460ac1de7b425d76f2500" +dependencies = [ + "proc-macro2", + "quote", + "unicode-xid", +] + +[[package]] +name = "cortex-m" +version = "0.7.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" +dependencies = [ + "bare-metal 0.2.5", + "bitfield", + "critical-section 1.1.2", + "embedded-hal 0.2.7", + "volatile-register", +] + +[[package]] +name = "cortex-m-rt" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +dependencies = [ + "cortex-m-rt-macros", +] + +[[package]] +name = "cortex-m-rt-macros" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "cortex-m-semihosting" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" +dependencies = [ + "cortex-m", +] + +[[package]] +name = "critical-section" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" +dependencies = [ + "bare-metal 1.0.0", + "critical-section 1.1.2", +] + +[[package]] +name = "critical-section" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" + +[[package]] +name = "darling" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.59", +] + +[[package]] +name = "darling_macro" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "defmt" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +dependencies = [ + "defmt-parser", + "proc-macro-error", + "proc-macro2", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "defmt-parser" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" +dependencies = [ + "thiserror", +] + +[[package]] +name = "defmt-rtt" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1" +dependencies = [ + "critical-section 0.2.8", + "defmt", +] + +[[package]] +name = "defmt-test" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +dependencies = [ + "cortex-m-rt", + "cortex-m-semihosting", + "defmt", + "defmt-test-macros", +] + +[[package]] +name = "defmt-test-macros" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "document-features" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" +dependencies = [ + "litrs", +] + +[[package]] +name = "either" +version = "1.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" + +[[package]] +name = "embassy-embedded-hal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "defmt", + "embassy-futures", + "embassy-sync", + "embassy-time", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-storage", + "embedded-storage-async", + "nb 1.1.0", +] + +[[package]] +name = "embassy-executor" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "cortex-m", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-executor-macros", + "embassy-time-driver", + "embassy-time-queue-driver", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.4.1" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "embassy-futures" +version = "0.1.1" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" + +[[package]] +name = "embassy-hal-internal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "cortex-m", + "critical-section 1.1.2", + "defmt", + "num-traits", +] + +[[package]] +name = "embassy-net-driver" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "defmt", +] + +[[package]] +name = "embassy-stm32" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "bit_field", + "bitflags 2.5.0", + "cfg-if", + "chrono", + "cortex-m", + "cortex-m-rt", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-embedded-hal", + "embassy-futures", + "embassy-hal-internal", + "embassy-net-driver", + "embassy-sync", + "embassy-time", + "embassy-time-driver", + "embassy-usb-driver", + "embedded-can", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "embedded-storage", + "embedded-storage-async", + "futures", + "nb 1.1.0", + "proc-macro2", + "quote", + "rand_core", + "sdio-host", + "static_assertions", + "stm32-fmc", + "stm32-metapac", + "vcell", + "volatile-register", +] + +[[package]] +name = "embassy-sync" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "defmt", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-time-driver", + "embassy-time-queue-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "document-features", +] + +[[package]] +name = "embassy-time-queue-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" + +[[package]] +name = "embassy-usb-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "defmt", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" +dependencies = [ + "defmt", +] + +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "defmt", + "embedded-io", +] + +[[package]] +name = "embedded-storage" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" + +[[package]] +name = "embedded-storage-async" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" +dependencies = [ + "embedded-storage", +] + +[[package]] +name = "env_logger" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" +dependencies = [ + "atty", + "humantime", + "log", + "regex", + "termcolor", +] + +[[package]] +name = "errno" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +dependencies = [ + "libc", + "windows-sys", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "futures" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" +dependencies = [ + "futures-core", + "futures-sink", +] + +[[package]] +name = "futures-core" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" + +[[package]] +name = "futures-io" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" + +[[package]] +name = "futures-macro" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "futures-sink" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" + +[[package]] +name = "futures-task" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" + +[[package]] +name = "futures-util" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +dependencies = [ + "futures-core", + "futures-macro", + "futures-sink", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "glob" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" + +[[package]] +name = "hash32" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hashbrown" +version = "0.12.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" + +[[package]] +name = "heapless" +version = "0.7.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" +dependencies = [ + "atomic-polyfill", + "hash32 0.2.1", + "rustc_version 0.4.0", + "spin", + "stable_deref_trait", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] + +[[package]] +name = "hermit-abi" +version = "0.1.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33" +dependencies = [ + "libc", +] + +[[package]] +name = "home" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "humantime" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "indexmap" +version = "1.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" +dependencies = [ + "autocfg", + "hashbrown", +] + +[[package]] +name = "lazy_static" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" + +[[package]] +name = "lazycell" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" + +[[package]] +name = "libc" +version = "0.2.153" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" + +[[package]] +name = "libloading" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" +dependencies = [ + "cfg-if", + "windows-targets", +] + +[[package]] +name = "libm" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + +[[package]] +name = "linux-raw-sys" +version = "0.4.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" + +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" + +[[package]] +name = "lock_api" +version = "0.4.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" +dependencies = [ + "autocfg", + "scopeguard", +] + +[[package]] +name = "log" +version = "0.4.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" + +[[package]] +name = "memchr" +version = "2.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" + +[[package]] +name = "minimal-lexical" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "nom" +version = "7.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" +dependencies = [ + "memchr", + "minimal-lexical", +] + +[[package]] +name = "num-traits" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "once_cell" +version = "1.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" + +[[package]] +name = "os_str_bytes" +version = "6.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" + +[[package]] +name = "panic-probe" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +dependencies = [ + "cortex-m", + "defmt", +] + +[[package]] +name = "peeking_take_while" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" + +[[package]] +name = "pin-project-lite" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "portable-atomic" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" + +[[package]] +name = "proc-macro-error" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +dependencies = [ + "proc-macro-error-attr", + "proc-macro2", + "quote", + "syn 1.0.109", + "version_check", +] + +[[package]] +name = "proc-macro-error-attr" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +dependencies = [ + "proc-macro2", + "quote", + "version_check", +] + +[[package]] +name = "proc-macro2" +version = "1.0.80" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a56dea16b0a29e94408b9aa5e2940a4eedbd128a1ba20e8f7ae60fd3d465af0e" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" + +[[package]] +name = "regex" +version = "1.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" + +[[package]] +name = "rustc-hash" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" + +[[package]] +name = "rustc_version" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" +dependencies = [ + "semver 0.9.0", +] + +[[package]] +name = "rustc_version" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" +dependencies = [ + "semver 1.0.22", +] + +[[package]] +name = "rustix" +version = "0.38.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "65e04861e65f21776e67888bfbea442b3642beaa0138fdb1dd7a84a52dffdb89" +dependencies = [ + "bitflags 2.5.0", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", +] + +[[package]] +name = "scopeguard" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" + +[[package]] +name = "sdio-host" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" + +[[package]] +name = "semver" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" +dependencies = [ + "semver-parser", +] + +[[package]] +name = "semver" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" + +[[package]] +name = "semver-parser" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" + +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + +[[package]] +name = "spin" +version = "0.9.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" +dependencies = [ + "lock_api", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + +[[package]] +name = "static_cell" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa6ba4cf83bf80d3eb25f098ea5e790a0a1fcb5e357442259b231e412c2d3ca0" +dependencies = [ + "portable-atomic", +] + +[[package]] +name = "stm32-fmc" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" +dependencies = [ + "embedded-hal 0.2.7", +] + +[[package]] +name = "stm32-metapac" +version = "15.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-01ac9bfd035961dc75f32dcd6080501538246d5c#a5e8ddc393950d780eb95fba15833d8a68b4e216" +dependencies = [ + "cortex-m", + "cortex-m-rt", +] + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.59" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4a6531ffc7b071655e4ce2e04bd464c4830bb585a61cabb96cf808f05172615a" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "termcolor" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "textwrap" +version = "0.16.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" + +[[package]] +name = "thiserror" +version = "1.0.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.59", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "unicode-xid" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" + +[[package]] +name = "vcell" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "volatile-register" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" +dependencies = [ + "vcell", +] + +[[package]] +name = "which" +version = "4.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" +dependencies = [ + "either", + "home", + "once_cell", + "rustix", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" +dependencies = [ + "winapi", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" diff --git a/kicker-board-v3/Cargo.toml b/kicker-board-v3/Cargo.toml new file mode 100644 index 00000000..b7796107 --- /dev/null +++ b/kicker-board-v3/Cargo.toml @@ -0,0 +1,101 @@ +[package] +name = "ateam-kicker-board-v3" +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 = { version = "0.7.6", features = ["critical-section-single-core"] } +cortex-m-rt = "0.7.1" +defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "0.3" +embassy-executor = { version = "0.5.0", features = [ + "arch-cortex-m", + "executor-thread", + "executor-interrupt", + "defmt", + "integrated-timers", + "task-arena-size-32768", +] } +embassy-time = { version = "0.3.0", features = [ + "defmt", + "defmt-timestamp-uptime", + "tick-hz-32_768", +] } +embassy-stm32 = { version = "0.1.0", features = [ + "defmt", + "stm32f407ve", + "unstable-pac", + "time-driver-tim1", + "exti", + "chrono" +] } +embassy-futures = { version = "0.1.0" } +futures-util = { version = "0.3.17", default-features = false } +embassy-sync = { version = "0.5.0" } +panic-probe = { version = "0.3", features = ["print-defmt"] } +static_cell = "2.0.0" +critical-section = "1.1.1" +const_format = "0.2.30" +heapless = "0.7.16" +libm = "0.2.6" +ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } + +[dev-dependencies] +defmt-test = "0.3.0" + +[profile.dev] +opt-level = 3 +lto = 'fat' +debug-assertions = false + +[profile.release] +debug = true +lto = 'fat' + +[lib] +test = false +harness = false + +[[bin]] +name = "hwtest-blinky" +test = false +harness = false + +[[bin]] +name = "hwtest-breakbeam" +test = false +harness = false + +[[bin]] +name = "hwtest-charge" +test = false +harness = false + +[[bin]] +name = "hwtest-coms" +test = false +harness = false + +[[bin]] +name = "hwtest-kick" +test = false +harness = false + +[[bin]] +name = "kicker" +test = false +harness = false + +[patch.crates-io] +# embassy-executor = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } +# embassy-sync = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } +# embassy-time = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } +# embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } +# embassy-futures = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } +embassy-executor = { git = "https://github.com/embassy-rs/embassy"} +embassy-sync = { git = "https://github.com/embassy-rs/embassy"} +embassy-time = { git = "https://github.com/embassy-rs/embassy"} +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} +embassy-futures = { git = "https://github.com/embassy-rs/embassy"} \ No newline at end of file diff --git a/kicker-board-v3/build.rs b/kicker-board-v3/build.rs new file mode 100644 index 00000000..786ce10f --- /dev/null +++ b/kicker-board-v3/build.rs @@ -0,0 +1,6 @@ +fn main() { + // TODO: flip-link? + println!("cargo:rustc-link-arg=--nmagic"); + println!("cargo:rustc-link-arg=-Tlink.x"); + println!("cargo:rustc-link-arg=-Tdefmt.x"); +} diff --git a/kicker-board-v3/memory.x b/kicker-board-v3/memory.x new file mode 100644 index 00000000..5ad3e630 --- /dev/null +++ b/kicker-board-v3/memory.x @@ -0,0 +1,12 @@ +MEMORY +{ + /* NOTE K = KiBi = 1024 bytes */ + FLASH : ORIGIN = 0x08000000, LENGTH = 512K + RAM : ORIGIN = 0x20000000, LENGTH = 128K + /* CCMRAM : ORIGIN = 0x10000000, LENGTH = 64K */ +} + +/* This is where the call stack will be allocated. */ +/* The stack is of the full descending type. */ +/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */ +_stack_start = ORIGIN(RAM) + LENGTH(RAM); \ No newline at end of file diff --git a/kicker-board-v3/rust-toolchain.toml b/kicker-board-v3/rust-toolchain.toml new file mode 100644 index 00000000..42b54e8d --- /dev/null +++ b/kicker-board-v3/rust-toolchain.toml @@ -0,0 +1,3 @@ +[toolchain] +channel = "nightly" +targets = [ "thumbv7em-none-eabihf" ] diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs new file mode 100644 index 00000000..25ca923a --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -0,0 +1,103 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use cortex_m_rt::entry; + +use embassy_executor::Spawner; +use embassy_executor::Executor; +use embassy_stm32::{ + adc::SampleTime, gpio::{Level, Output, Speed}, peripherals::ADC1, time::mhz +}; +use embassy_time::{Delay, Duration, Timer}; +use embassy_stm32::adc::{Adc, Temperature, VrefInt}; + +use static_cell::StaticCell; + +use ateam_kicker_board_v3::*; +use ateam_kicker_board_v3::pins::*; + +use panic_probe as _; +// use panic_halt as _; + +#[embassy_executor::task] +async fn blink( + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + status_led_blue1: BlueStatusLed1Pin, + status_led_blue2: BlueStatusLed2Pin, + mut adc: Adc<'static, PowerRailAdc>, + mut rail_200v_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + mut rail_6v2_pin: PowerRail6v2ReadPin, + mut rail_5v0_pin: PowerRail5v0ReadPin) -> ! { + + let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); + let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); + + // let mut temp = adc.enable_temperature(); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); + + loop { + reg_charge.set_low(); + + status_led_green.set_high(); + status_led_blue1.set_high(); + status_led_blue2.set_high(); + status_led_red.set_low(); + Timer::after(Duration::from_millis(500)).await; + + status_led_green.set_low(); + status_led_blue1.set_low(); + status_led_blue2.set_low(); + status_led_red.set_high(); + Timer::after(Duration::from_millis(500)).await; + + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + let raw_200v = adc.read(&mut rail_200v_pin) as f32; + let raw_12v = adc.read(&mut rail_12v0_pin) as f32; + let raw_6v2 = adc.read(&mut rail_6v2_pin) as f32; + let raw_5v0 = adc.read(&mut rail_5v0_pin) as f32; + let raw_int = adc.read(&mut vrefint) as f32; + + defmt::info!("voltages - 200v ({}), 12v0 ({}), 6v2 ({}), 5v0 ({}), 3v3 ({})", + adc_200v_to_rail_voltage(adc_raw_to_v(raw_200v, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(raw_12v, vrefint_sample)), + adc_6v2_to_rail_voltage(adc_raw_to_v(raw_6v2, vrefint_sample)), + adc_5v0_to_rail_voltage(adc_raw_to_v(raw_5v0, vrefint_sample)), + adc_3v3_to_rail_voltage(adc_raw_to_v(raw_int, vrefint_sample))); + } +} +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + let mut stm32_config: embassy_stm32::Config = Default::default(); + // stm32_config.rcc.sys_ck = Some(mhz(168)); + // stm32_config.rcc.hclk = Some(mhz(96)); + + let p = embassy_stm32::init(stm32_config); + + info!("kicker startup!"); + + let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); + let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); + + let mut adc = Adc::new(p.ADC1); + + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, adc, p.PC0, p.PC1, p.PC3, p.PC2))); + }); +} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs b/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs new file mode 100644 index 00000000..a1d5207c --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs @@ -0,0 +1,84 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use cortex_m::prelude::_embedded_hal_blocking_delay_DelayUs; +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; +use embassy_executor::Spawner; +use embassy_stm32::{ + exti::ExtiInput, + gpio::{Input, Level, Output, Pull, Speed}, + interrupt::{self, InterruptExt}, + time::mhz, + usart::{self, Uart}, +}; +use embassy_time::{Duration, Ticker, Timer}; +use ateam_kicker_board::{ + drivers::{breakbeam::Breakbeam} +}; +use ateam_kicker_board::pins::{BlueStatusLedPin, GreenStatusLedPin, BreakbeamTxPin, BreakbeamRxPin}; + + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let mut stm32_config: embassy_stm32::Config = Default::default(); + stm32_config.rcc.sys_ck = Some(mhz(48)); + stm32_config.rcc.hclk = Some(mhz(48)); + stm32_config.rcc.pclk = Some(mhz(48)); + + let p = embassy_stm32::init(stm32_config); + + let _charge_pin = Output::new(p.PB3, Level::Low, Speed::Medium); + let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); + let _chip_pin = Output::new(p.PB1, Level::Low, Speed::Medium); + + info!("breakbeam startup!"); + + let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); + let mut status_led_blue = Output::new(p.PA8, Level::Low, Speed::Medium); + + // Breakbeam + // nets on schematic are inverted to silkscreen, sorry :/ -Will + let mut breakbeam = Breakbeam::new(p.PA3, p.PA2); + + status_led_green.set_high(); + Timer::after(Duration::from_millis(250)).await; + status_led_green.set_low(); + Timer::after(Duration::from_millis(250)).await; + status_led_green.set_high(); + Timer::after(Duration::from_millis(250)).await; + status_led_green.set_low(); + Timer::after(Duration::from_millis(250)).await; + + breakbeam.enable_tx(); + loop + { + // Enable transmitter, wait 100ms, drive blue status LED if receiving, wait 1 sec + // Timer::after(Duration::from_millis(100)).await; + if breakbeam.read() + { + status_led_blue.set_high(); + } + else + { + status_led_blue.set_low(); + } + // Timer::after(Duration::from_millis(1000)).await; + + // Disable transmitter, wait 100ms, drive blue status LED if receiving, wait 1 sec + // breakbeam.disable_tx(); + // if breakbeam.read() + // { + // status_led_blue.set_high(); + // } + // else + // { + // status_led_blue.set_low(); + // } + // Timer::after(Duration::from_millis(1000)).await; + + Timer::after(Duration::from_millis(10)).await; + + } +} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-charge/main.rs b/kicker-board-v3/src/bin/hwtest-charge/main.rs new file mode 100644 index 00000000..4597d3c8 --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-charge/main.rs @@ -0,0 +1,191 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use core::mem; + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +// use cortex_m::{peripheral::NVIC, delay::Delay}; +use cortex_m::{peripheral::NVIC}; +use cortex_m_rt::entry; + +use embassy_executor::{Executor, InterruptExecutor}; +use embassy_stm32::{ + adc::{Adc, AdcPin, InternalChannel, Temperature}, + pac::Interrupt, + Peripherals, + gpio::{Input, Level, Output, Pull, Speed}, + gpio::low_level::Pin, + interrupt, adc::SampleTime, +}; +use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; +use embassy_time::{Delay, Duration, Timer, Ticker}; + +use static_cell::StaticCell; + +use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; + +// #[embassy_executor::task] +// async fn run_critical_section_task(p: Peripherals) { +// let reg_done = Input::new(p.PB4, Pull::None); +// let reg_fault = Input::new(p.PB5, Pull::None); + +// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); +// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); +// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); + +// status_led_green.set_high(); +// Timer::after(Duration::from_millis(500)).await; +// status_led_green.set_low(); +// Timer::after(Duration::from_millis(500)).await; + +// reg_charge.set_high(); +// Timer::after(Duration::from_millis(100)).await; +// reg_charge.set_low(); + +// loop { +// reg_charge.set_low(); + +// if reg_done.is_low() { +// status_led_green.set_high(); +// } else { +// status_led_green.set_low(); +// } + +// if reg_fault.is_low() { +// status_led_red.set_high(); +// } else { +// status_led_red.set_low(); +// } +// } +// } + +// #[embassy_executor::task] +// async fn read_adc_samples(aps: PubSubChannel::) -> ! { +// let sub = aps.subscriber().unwrap(); +// loop { +// info!(sub.) +// } +// } + +#[embassy_executor::task] +async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + mut hv_pin: HighVoltageReadPin, + mut batt_pin: BatteryVoltageReadPin, + mut reg_charge: ChargePin, + mut reg_done: RegulatorDonePin, + mut reg_fault: RegulatorFaultPin, + mut status_led_red: RedStatusLedPin, + mut status_led_green: GreenStatusLedPin) -> ! { + + let mut ticker = Ticker::every(Duration::from_millis(1)); + + let reg_done = Input::new(reg_done, Pull::None); + let reg_fault = Input::new(reg_fault, Pull::None); + + let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + + status_led_green.set_high(); + Timer::after(Duration::from_millis(500)).await; + status_led_green.set_low(); + Timer::after(Duration::from_millis(500)).await; + + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + let mut hv = adc.read(&mut hv_pin) as f32; + let mut bv = adc.read(&mut batt_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); + if start_up_battery_voltage < 18.0 { + status_led_red.set_high(); + warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("refusing to continue"); + loop { + reg_charge.set_low(); + } + } + + reg_charge.set_high(); + Timer::after(Duration::from_millis(400)).await; + let reg_done_stat = reg_done.is_low(); + let reg_fault_stat = reg_fault.is_low(); + reg_charge.set_low(); + + loop { + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + bv = adc.read(&mut batt_pin) as f32; + + info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + + reg_charge.set_low(); + + if reg_done_stat { + status_led_green.set_high(); + } else { + status_led_green.set_low(); + } + + if reg_fault_stat { + status_led_red.set_high(); + } else { + status_led_red.set_low(); + } + + ticker.next().await; + } +} + +// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[derive(Clone, Copy)] +struct AnalogValues { + high_voltage: u16, + batt_voltage: u16, + temp: u16 +} + +// #[interrupt] +// unsafe fn I2C1() { +// EXECUTOR_HIGH.on_interrupt(); +// } + + +#[entry] +fn main() -> ! { + let p = embassy_stm32::init(Default::default()); + info!("kicker startup!"); + let mut nvic: NVIC = unsafe { mem::transmute(()) }; + + // let aps = PubSubChannel::::new(); + + + // highest priorty, energy management + // medium priority, ADC publisher + // low priority uart handler + // lowest prioiryt main update loop + + // High-priority executor: I2C1, priority level 6 + // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; + // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); + // unwrap!(spawner.spawn(run_critical_section_task(p))); + + let mut adc = Adc::new(p.ADC, &mut Delay); + adc.set_sample_time(SampleTime::Cycles71_5); + // let mut temp_ch = adc.enable_temperature(&mut Delay); + + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11))); + }); +} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-coms/main.rs b/kicker-board-v3/src/bin/hwtest-coms/main.rs new file mode 100644 index 00000000..d7c0aa25 --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-coms/main.rs @@ -0,0 +1,278 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(const_mut_refs)] + +use core::mem; +use static_cell::StaticCell; + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use cortex_m::peripheral::NVIC; +use cortex_m_rt::entry; + +use embassy_executor::{Executor, InterruptExecutor}; +use embassy_stm32::{ + adc::{Adc, SampleTime}, + pac::Interrupt, + gpio::{Level, Output, Speed}, + interrupt, + time::mhz, + usart::{Uart, Parity, StopBits, Config} +}; +use embassy_stm32::{bind_interrupts, peripherals, usart}; + +use embassy_time::{Delay, Duration, Instant, Ticker}; + +use ateam_kicker_board::{ + adc_raw_to_v, + adc_v_to_battery_voltage, + adc_v_to_rail_voltage, + kick_manager::{ + KickManager, + KickType}, + pins::{ + HighVoltageReadPin, BatteryVoltageReadPin, + ChargePin, KickPin, ChipPin, + ComsUartModule, ComsUartRxDma, ComsUartTxDma, RedStatusLedPin, BlueStatusLedPin}, + queue::Buffer, + uart_queue::{ + UartReadQueue, + UartWriteQueue, + } +}; + +use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; + +const MAX_TX_PACKET_SIZE: usize = 16; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 16; +const RX_BUF_DEPTH: usize = 3; + +// control communications tx buffer +#[link_section = ".bss"] +static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = + [Buffer::EMPTY; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); + +// control communications rx buffer +#[link_section = ".bss"] +static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = + [Buffer::EMPTY; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); + +fn get_empty_control_packet() -> KickerControl { + KickerControl { + _bitfield_align_1: [], + _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), + kick_request: KickRequest::KR_DISABLE, + kick_speed: 0.0, + } +} + +fn get_empty_telem_packet() -> KickerTelemetry { + KickerTelemetry { + _bitfield_align_1: [], + _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), + rail_voltage: 0.0, + battery_voltage: 0.0, + } +} + +#[embassy_executor::task] +async fn high_pri_kick_task( + coms_reader: &'static UartReadQueue<'static, ComsUartModule, ComsUartRxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH>, + coms_writer: &'static UartWriteQueue<'static, ComsUartModule, ComsUartTxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH>, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + charge_pin: ChargePin, + kick_pin: KickPin, + chip_pin: ChipPin, + mut rail_pin: HighVoltageReadPin, + mut battery_voltage_pin: BatteryVoltageReadPin, + err_led_pin: RedStatusLedPin, + ball_detected_led_pin: BlueStatusLedPin) -> ! { + + // pins/safety management + let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); + let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); + let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); + let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); + + // debug LEDs + let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + // TODO dotstars + + // coms buffers + let mut telemetry_enabled: bool = false; + let mut kicker_control_packet: KickerControl = get_empty_control_packet(); + let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); + + // loop rate control + let mut ticker = Ticker::every(Duration::from_millis(1)); + let mut last_packet_sent_time = Instant::now(); + + loop { + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); + let battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample)); + // optionally pre-flag errors? + + ///////////////////////////////////// + // process any available packets // + ///////////////////////////////////// + + while let Ok(res) = coms_reader.try_dequeue() { + let buf = res.data(); + + if buf.len() != core::mem::size_of::() { + defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + continue; + } + + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe + unsafe { + // copy receieved uart bytes into packet + let state = &mut kicker_control_packet as *mut _ as *mut u8; + for i in 0..core::mem::size_of::() { + *state.offset(i as isize) = buf[i]; + } + } + } + + // TODO: read breakbeam + let ball_detected = false; + + /////////////////////////////////////////////// + // manage repetitive kick commands + state // + /////////////////////////////////////////////// + + // update telemetry requests + telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; + + // no charge/kick in coms test + let res = kick_manager.command(battery_voltage, rail_voltage, false, KickType::None, 0.0).await; + + // send telemetry packet + if telemetry_enabled { + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > 20 { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1(0, 0, ball_detected as u32, res.is_err() as u32); + kicker_telemetry_packet.rail_voltage = rail_voltage; + kicker_telemetry_packet.battery_voltage = battery_voltage; + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), + ); + + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } + + if ball_detected_led.is_set_high() { + ball_detected_led.set_low(); + } else { + ball_detected_led.set_high(); + } + + last_packet_sent_time = cur_time; + } + } + + // LEDs + if res.is_err() { + err_led.set_high(); + } else { + err_led.set_low(); + } + + if ball_detected { + ball_detected_led.set_high(); + } else { + ball_detected_led.set_low(); + } + // TODO Dotstar + + // loop rate control @1KHz + ticker.next().await; + } + +} + +static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[interrupt] +unsafe fn I2C1() { + EXECUTOR_HIGH.on_interrupt(); +} + +bind_interrupts!(struct Irqs { + USART1 => usart::InterruptHandler; +}); + +#[entry] +fn main() -> ! { + let mut stm32_config: embassy_stm32::Config = Default::default(); + stm32_config.rcc.sys_ck = Some(mhz(48)); + stm32_config.rcc.hclk = Some(mhz(48)); + stm32_config.rcc.pclk = Some(mhz(48)); + + let p = embassy_stm32::init(stm32_config); + + info!("kicker startup!"); + + let mut nvic: NVIC = unsafe { mem::transmute(()) }; + + let _status_led = Output::new(p.PA11, Level::High, Speed::Low); + + let mut adc = Adc::new(p.ADC, &mut Delay); + adc.set_sample_time(SampleTime::Cycles71_5); + + // high priority executor handles kicking system + // High-priority executor: I2C1, priority level 6 + // TODO CHECK THIS IS THE HIGHEST PRIORITY + unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; + let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PB3, p.PB0, p.PB1, p.PA0, p.PA1, p.PA12, p.PA8))); + // unwrap!(spawner.spawn(high_pri_tx_test(&COMS_QUEUE_TX, p.PB3, p.PB0, p.PB1))); + + + ////////////////////////////////// + // COMMUNICATIONS TASKS SETUP // + ////////////////////////////////// + + let mut coms_uart_config = Config::default(); + coms_uart_config.baudrate = 2_000_000; // 2 Mbaud + coms_uart_config.parity = Parity::ParityEven; + coms_uart_config.stop_bits = StopBits::STOP1; + + let coms_usart = Uart::new( + p.USART1, + p.PA10, + p.PA9, + Irqs, + p.DMA1_CH2, + p.DMA1_CH3, + coms_uart_config, + ); + + let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + + // low priority executor handles coms and user IO + // Low priority executor: runs in thread mode, using WFE/SEV + let lp_executor = EXECUTOR_LOW.init(Executor::new()); + lp_executor.run(|spawner| { + unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + }); +} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-kick/main.rs b/kicker-board-v3/src/bin/hwtest-kick/main.rs new file mode 100644 index 00000000..fe285d9e --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-kick/main.rs @@ -0,0 +1,227 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use core::mem; + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +// use core::hint::black_box; + +// use cortex_m::{peripheral::NVIC, delay::Delay}; +use cortex_m::{peripheral::NVIC}; +use cortex_m_rt::entry; + +use embassy_executor::{Executor, InterruptExecutor}; +use embassy_stm32::{ + adc::{Adc, AdcPin, InternalChannel, Temperature}, + pac::Interrupt, + Peripherals, + gpio::{Input, Level, Output, Pull, Speed}, + gpio::low_level::Pin, + interrupt, adc::SampleTime, +}; +use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; +use embassy_time::{Delay, Duration, Timer, Ticker}; + +use static_cell::StaticCell; + +use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin, KickPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; + +// #[embassy_executor::task] +// async fn run_critical_section_task(p: Peripherals) { +// let reg_done = Input::new(p.PB4, Pull::None); +// let reg_fault = Input::new(p.PB5, Pull::None); + +// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); +// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); +// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); + +// status_led_green.set_high(); +// Timer::after(Duration::from_millis(500)).await; +// status_led_green.set_low(); +// Timer::after(Duration::from_millis(500)).await; + +// reg_charge.set_high(); +// Timer::after(Duration::from_millis(100)).await; +// reg_charge.set_low(); + +// loop { +// reg_charge.set_low(); + +// if reg_done.is_low() { +// status_led_green.set_high(); +// } else { +// status_led_green.set_low(); +// } + +// if reg_fault.is_low() { +// status_led_red.set_high(); +// } else { +// status_led_red.set_low(); +// } +// } +// } + +// #[embassy_executor::task] +// async fn read_adc_samples(aps: PubSubChannel::) -> ! { +// let sub = aps.subscriber().unwrap(); +// loop { +// info!(sub.) +// } +// } + +#[embassy_executor::task] +async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + mut hv_pin: HighVoltageReadPin, + mut batt_pin: BatteryVoltageReadPin, + mut reg_charge: ChargePin, + mut reg_done: RegulatorDonePin, + mut reg_fault: RegulatorFaultPin, + mut status_led_red: RedStatusLedPin, + mut status_led_green: GreenStatusLedPin, + mut kick_pin: KickPin) -> ! { + + let mut ticker = Ticker::every(Duration::from_millis(1)); + + let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); + + let reg_done = Input::new(reg_done, Pull::None); + let reg_fault = Input::new(reg_fault, Pull::None); + + let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + + status_led_green.set_high(); + Timer::after(Duration::from_millis(500)).await; + status_led_green.set_low(); + Timer::after(Duration::from_millis(500)).await; + + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + let mut hv = adc.read(&mut hv_pin) as f32; + let mut bv = adc.read(&mut batt_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); + if start_up_battery_voltage < 18.0 { + status_led_red.set_high(); + warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("refusing to continue"); + loop { + reg_charge.set_low(); + + kick.set_high(); + Timer::after(Duration::from_micros(500)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; + } + } + + reg_charge.set_high(); + Timer::after(Duration::from_millis(600)).await; + let reg_done_stat = reg_done.is_low(); + let reg_fault_stat = reg_fault.is_low(); + reg_charge.set_low(); + + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + bv = adc.read(&mut batt_pin) as f32; + // info!("hv V: {}, batt mv: {}", ((hv * 200) * 8 / 10), (bv * 25000 / 1650 * (8 / 10))); // 8 / 10 for adc offset + info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + + Timer::after(Duration::from_millis(1000)).await; + + kick.set_high(); + Timer::after(Duration::from_micros(10000)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(2000)).await; + + kick.set_high(); + Timer::after(Duration::from_micros(500000)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; + + + loop { + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + bv = adc.read(&mut batt_pin) as f32; + + info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + + + reg_charge.set_low(); + kick.set_low(); + + // if reg_done_stat { + // status_led_green.set_high(); + // } else { + // status_led_green.set_low(); + // } + + // if reg_fault_stat { + // status_led_red.set_high(); + // } else { + // status_led_red.set_low(); + // } + + ticker.next().await; + } +} + +// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[derive(Clone, Copy)] +struct AnalogValues { + high_voltage: u16, + batt_voltage: u16, + temp: u16 +} + +// #[interrupt] +// unsafe fn I2C1() { +// EXECUTOR_HIGH.on_interrupt(); +// } + + +#[entry] +fn main() -> ! { + let p = embassy_stm32::init(Default::default()); + info!("kicker startup!"); + let mut nvic: NVIC = unsafe { mem::transmute(()) }; + + // let aps = PubSubChannel::::new(); + + + // highest priorty, energy management + // medium priority, ADC publisher + // low priority uart handler + // lowest prioiryt main update loop + + // High-priority executor: I2C1, priority level 6 + // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; + // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); + // unwrap!(spawner.spawn(run_critical_section_task(p))); + + let mut adc = Adc::new(p.ADC, &mut Delay); + adc.set_sample_time(SampleTime::Cycles71_5); + // let mut temp_ch = adc.enable_temperature(&mut Delay); + + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11, p.PB0))); + }); +} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/kicker/main.rs b/kicker-board-v3/src/bin/kicker/main.rs new file mode 100644 index 00000000..768e617b --- /dev/null +++ b/kicker-board-v3/src/bin/kicker/main.rs @@ -0,0 +1,468 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(const_mut_refs)] + +use core::mem; +use static_cell::StaticCell; + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use cortex_m::peripheral::NVIC; +use cortex_m_rt::entry; + +use libm::{fmaxf, fminf}; + +use embassy_executor::{Executor, InterruptExecutor}; +use embassy_stm32::{ + adc::{Adc, SampleTime}, + bind_interrupts, + gpio::{Level, Output, Speed}, + interrupt, + pac::Interrupt, + peripherals, + time::mhz, + usart, + usart::{Config, Parity, StopBits, Uart}, +}; +use embassy_time::{Delay, Duration, Instant, Ticker}; + +use ateam_kicker_board::{ + adc_raw_to_v, adc_v_to_battery_voltage, adc_v_to_rail_voltage, + kick_manager::{KickManager, KickType}, + pins::{ + BatteryVoltageReadPin, BlueStatusLedPin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, + ComsUartTxDma, HighVoltageReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, + }, + queue::Buffer, + uart_queue::{UartReadQueue, UartWriteQueue}, drivers::breakbeam::Breakbeam, +}; + +use ateam_common_packets::bindings_kicker::{ + KickRequest::{self, KR_ARM, KR_DISABLE}, + KickerControl, KickerTelemetry, +}; + +const MAX_TX_PACKET_SIZE: usize = 16; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 16; +const RX_BUF_DEPTH: usize = 3; + +const MAX_KICK_SPEED: f32 = 5.5; +const SHUTDOWN_KICK_DUTY: f32 = 0.20; + +pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; +pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; +pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; +pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; + +// control communications tx buffer +// #[link_section = ".axisram.buffers"] +static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = + [Buffer::EMPTY; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue< + ComsUartModule, + ComsUartTxDma, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, +> = UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); + +// control communications rx buffer +// #[link_section = ".axisram.buffers"] +static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = + [Buffer::EMPTY; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue< + ComsUartModule, + ComsUartRxDma, + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, +> = UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); + +fn get_empty_control_packet() -> KickerControl { + KickerControl { + _bitfield_align_1: [], + _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), + kick_request: KickRequest::KR_DISABLE, + kick_speed: 0.0, + } +} + +fn get_empty_telem_packet() -> KickerTelemetry { + KickerTelemetry { + _bitfield_align_1: [], + _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), + rail_voltage: 0.0, + battery_voltage: 0.0, + } +} + +#[embassy_executor::task] +async fn high_pri_kick_task( + coms_reader: &'static UartReadQueue< + 'static, + ComsUartModule, + ComsUartRxDma, + MAX_RX_PACKET_SIZE, + RX_BUF_DEPTH, + >, + coms_writer: &'static UartWriteQueue< + 'static, + ComsUartModule, + ComsUartTxDma, + MAX_TX_PACKET_SIZE, + TX_BUF_DEPTH, + >, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + charge_pin: ChargePin, + kick_pin: KickPin, + chip_pin: ChipPin, + breakbeam_tx: BreakbeamTxPin, + breakbeam_rx: BreakbeamRxPin, + mut rail_pin: HighVoltageReadPin, + mut battery_voltage_pin: BatteryVoltageReadPin, + err_led_pin: RedStatusLedPin, + ball_detected_led_pin: BlueStatusLedPin, +) -> ! { + // pins/safety management + let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); + let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); + let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); + let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); + + // debug LEDs + let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + // TODO dotstars + + let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); + + // coms buffers + let mut telemetry_enabled: bool; // = false; + let mut kicker_control_packet: KickerControl = get_empty_control_packet(); + let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); + + // bookkeeping for latched state + let mut kick_command_cleared: bool = false; + let mut latched_command = KickRequest::KR_DISABLE; + let mut error_latched: bool = false; + let mut charge_hv_rail: bool = false; + + // power down status + let mut shutdown_requested: bool = false; + let mut shutdown_completed: bool = false; + + // loop rate control + let mut ticker = Ticker::every(Duration::from_millis(1)); + let mut last_packet_sent_time = Instant::now(); + + breakbeam.enable_tx(); + loop { + let mut vrefint = adc.enable_vref(&mut Delay); + let vrefint_sample = adc.read_internal(&mut vrefint); + + let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + let battery_voltage = + adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); + // optionally pre-flag errors? + + ///////////////////////////////////// + // process any available packets // + ///////////////////////////////////// + + while let Ok(res) = coms_reader.try_dequeue() { + let buf = res.data(); + + if buf.len() != core::mem::size_of::() { + defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + continue; + } + + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe + unsafe { + // copy receieved uart bytes into packet + let state = &mut kicker_control_packet as *mut _ as *mut u8; + for i in 0..core::mem::size_of::() { + *state.offset(i as isize) = buf[i]; + } + } + } + + // TODO: read breakbeam + let ball_detected = breakbeam.read(); + + /////////////////////////////////////////////// + // manage repetitive kick commands + state // + /////////////////////////////////////////////// + + // update telemetry requests + telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; + + // for now shutdown requests will be latched and a reboot is required to re-power + if kicker_control_packet.request_power_down() != 0 { + shutdown_requested = true; + } + + // check if we've met the criteria for completed shutdown + if shutdown_requested && rail_voltage < CHARGE_SAFE_VOLTAGE { + shutdown_completed = true; + } + + if rail_voltage > CHARGE_OVERVOLT_THRESH_VOLTAGE { + error_latched = true; + } + + // charge if were not in shutdown mode AND the control board has requested any state but "DISABLE" + charge_hv_rail = if shutdown_requested || shutdown_completed { + false + } else { + match kicker_control_packet.kick_request { + KickRequest::KR_ARM => true, + KickRequest::KR_DISABLE => false, + KickRequest::KR_KICK_NOW + | KickRequest::KR_KICK_CAPTURED + | KickRequest::KR_KICK_TOUCH + | KickRequest::KR_CHIP_NOW + | KickRequest::KR_CHIP_CAPTURED + | KickRequest::KR_CHIP_TOUCH => charge_hv_rail, + _ => false, + } + }; + + // scale kick strength from m/s to duty for the critical section + // if shutdown is requested and not complete, set kick discharge kick strength to 5% + let kick_strength = if shutdown_completed { + 0.0 + } else if shutdown_requested { + SHUTDOWN_KICK_DUTY + } else { + fmaxf(0.0, fminf(MAX_KICK_SPEED, kicker_control_packet.kick_speed)) / MAX_KICK_SPEED + }; + + // if control requests only an ARM or DISABLE, clear the active command + // software/joystick is not asserting a kick event, so any future kick event is a unique request + if kicker_control_packet.kick_request == KR_ARM + || kicker_control_packet.kick_request == KR_DISABLE + { + kick_command_cleared = true; + } + + // if the previous command has cleared acknowledgement, then any new request is a unique event and can be latched + if kick_command_cleared { + latched_command = kicker_control_packet.kick_request; + } + + // determine if the latched command is actionable + // if a shutdown is requested and not completed, always request a kick + let kick_command = if shutdown_completed { + KickType::None + } else if shutdown_requested { + KickType::Kick + } else { + match latched_command { + KickRequest::KR_DISABLE => KickType::None, + KickRequest::KR_ARM => KickType::None, + KickRequest::KR_KICK_NOW => KickType::Kick, + KickRequest::KR_KICK_TOUCH => { + if ball_detected { + KickType::Kick + } else { + KickType::None + } + } + KickRequest::KR_KICK_CAPTURED => { + if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { + KickType::Kick + } else { + KickType::None + } + } + KickRequest::KR_CHIP_NOW => KickType::Chip, + KickRequest::KR_CHIP_TOUCH => { + if ball_detected { + KickType::Chip + } else { + KickType::None + } + } + KickRequest::KR_CHIP_CAPTURED => { + if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { + KickType::Chip + } else { + KickType::None + } + } + // possible if packet decoding has corrupted enum val + _ => KickType::None, + } + }; + + // the latched command is actionable, and the critical section will be instructed to carry it out in the next segment + // set clear the command cleared flag, indicating the control board will need to deassert any kick command showing it + // wants a new unique event + if kick_command != KickType::None { + kick_command_cleared = false; + } + + // perform charge/kick actions. this function handles elec/mechanical safety + // if telemetry isn't enabled, the control board doesn't want to talk to us, don't permit any actions + let res = if !telemetry_enabled || error_latched { + kick_manager + .command(battery_voltage, rail_voltage, false, KickType::None, 0.0) + .await + } else { + if kick_command == KickType::Kick || kick_command == KickType::Chip { + kicker_control_packet.kick_request = match charge_hv_rail { + true => KickRequest::KR_ARM, + false => KickRequest::KR_DISABLE, + } + } + kick_manager + .command( + battery_voltage, + rail_voltage, + charge_hv_rail, + kick_command, + kick_strength, + ) + .await + }; + + // this will permanently latch an error if the rail voltages are low + // which we probably don't want on boot up? + // maybe this error should be clearable and the HV rail OV should not be + // if res.is_err() { + // error_latched = true; + // } + + // send telemetry packet + if telemetry_enabled { + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, last_packet_sent_time) + .unwrap() + .as_millis() + > 20 + { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( + shutdown_requested as u32, + shutdown_completed as u32, + ball_detected as u32, + res.is_err() as u32, + ); + kicker_telemetry_packet.rail_voltage = rail_voltage; + kicker_telemetry_packet.battery_voltage = battery_voltage; + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), + ); + + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } + + last_packet_sent_time = cur_time; + } + } + + // LEDs + if error_latched { + err_led.set_high(); + } else { + err_led.set_low(); + } + + if ball_detected { + ball_detected_led.set_high(); + } else { + ball_detected_led.set_low(); + } + // TODO Dotstar + + // loop rate control @1KHz + ticker.next().await; + } +} + +static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[interrupt] +unsafe fn I2C1() { + EXECUTOR_HIGH.on_interrupt(); +} + +bind_interrupts!(struct Irqs { + USART1 => usart::InterruptHandler; +}); + +#[entry] +fn main() -> ! { + let mut stm32_config: embassy_stm32::Config = Default::default(); + stm32_config.rcc.sys_ck = Some(mhz(48)); + stm32_config.rcc.hclk = Some(mhz(48)); + stm32_config.rcc.pclk = Some(mhz(48)); + + let p = embassy_stm32::init(stm32_config); + + info!("kicker startup!"); + + let mut nvic: NVIC = unsafe { mem::transmute(()) }; + + let _status_led = Output::new(p.PA11, Level::High, Speed::Low); + + let mut adc = Adc::new(p.ADC, &mut Delay); + adc.set_sample_time(SampleTime::Cycles71_5); + + // high priority executor handles kicking system + // High-priority executor: I2C1, priority level 6 + // TODO CHECK THIS IS THE HIGHEST PRIORITY + unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; + let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); + unwrap!(spawner.spawn(high_pri_kick_task( + &COMS_QUEUE_RX, + &COMS_QUEUE_TX, + adc, + p.PB3, + p.PB0, + p.PB1, + p.PA3, + p.PA2, + p.PA0, + p.PA1, + p.PA12, + p.PA8 + ))); + + ////////////////////////////////// + // COMMUNICATIONS TASKS SETUP // + ////////////////////////////////// + + let mut coms_uart_config = Config::default(); + coms_uart_config.baudrate = 2_000_000; // 2 Mbaud + coms_uart_config.parity = Parity::ParityEven; + coms_uart_config.stop_bits = StopBits::STOP1; + + let coms_usart = Uart::new( + p.USART1, + p.PA10, + p.PA9, + Irqs, + p.DMA1_CH2, + p.DMA1_CH3, + coms_uart_config, + ); + + let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + + // low priority executor handles coms and user IO + // Low priority executor: runs in thread mode, using WFE/SEV + let lp_executor = EXECUTOR_LOW.init(Executor::new()); + lp_executor.run(|spawner| { + unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + }); +} diff --git a/kicker-board-v3/src/drivers/breakbeam.rs b/kicker-board-v3/src/drivers/breakbeam.rs new file mode 100644 index 00000000..ae58ce90 --- /dev/null +++ b/kicker-board-v3/src/drivers/breakbeam.rs @@ -0,0 +1,32 @@ +use embassy_stm32::gpio::{Output, Input, Level, Speed, Pin, Pull}; + +pub struct Breakbeam<'a, PinTx: Pin, PinRx: Pin> { + pin_tx: Output<'a, PinTx>, + pin_rx: Input<'a, PinRx>, +} + +impl<'a, PinTx: Pin, PinRx: Pin> Breakbeam<'a, PinTx, PinRx> { + pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { + let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); + let pin_rx = Input::new(pin_rx, Pull::Down); + Self { + pin_tx, + pin_rx + } + } + + #[inline] + pub fn enable_tx(&mut self) { + self.pin_tx.set_high(); + } + + #[inline] + pub fn disable_tx(&mut self) { + self.pin_tx.set_low(); + } + + #[inline] + pub fn read(&self) -> bool { + self.pin_rx.is_low() + } +} diff --git a/kicker-board-v3/src/drivers/breakbeam_pwm.rs b/kicker-board-v3/src/drivers/breakbeam_pwm.rs new file mode 100644 index 00000000..63d9ceb0 --- /dev/null +++ b/kicker-board-v3/src/drivers/breakbeam_pwm.rs @@ -0,0 +1,37 @@ +use embassy_stm32::gpio::{Output, Input, Pin, Pull}; +use embassy_stm32::pwm::simple_pwm::{PwmPin, SimplePwm}; +use embassy_stm32::pwm::Channel; +use embassy_stm32::time::khz; +use embassy_time::{Duration, Timer}; + +pub struct Breakbeam<'a, PwmTx: Pin, PinRx: Pin> { + pin_tx: SimplePwm<'a, PwmTx>, + pin_rx: Input<'a, PinRx>, +} + +impl<'a, PinTx: Pin, PinRx: Pin, Timer: Peripheral> Breakbeam<'a, PinTx, PinRx, > { + pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { + let ch_1 = PwmPin::new_ch1(pin_tx); + let mut pwm_tx = SimplePwm::new(pin_tx, Level::High, Speed::Low); + let pin_rx = Input::new(pin_rx, Pull::Down); + Self { + pwm_tx, + pin_rx + } + } + + #[inline] + pub fn enable_tx(&mut self) { + self.pin_tx.set_high(); + } + + #[inline] + pub fn disable_tx(&mut self) { + self.pin_tx.set_low(); + } + + #[inline] + pub fn read(&self) -> bool { + self.pin_rx.is_high() + } +} diff --git a/kicker-board-v3/src/drivers/mod.rs b/kicker-board-v3/src/drivers/mod.rs new file mode 100644 index 00000000..c69230cd --- /dev/null +++ b/kicker-board-v3/src/drivers/mod.rs @@ -0,0 +1 @@ +pub mod breakbeam; \ No newline at end of file diff --git a/kicker-board-v3/src/kick_manager.rs b/kicker-board-v3/src/kick_manager.rs new file mode 100644 index 00000000..02c0bb8f --- /dev/null +++ b/kicker-board-v3/src/kick_manager.rs @@ -0,0 +1,167 @@ +/* + * This file is responsible for managing the mechanically and + * electrically critical sections of operation. Foundational + * assumptions and requirements are listed below. + * + * ASSUMPTIONS: + * 1. The init code hands off to the manager in a default intert state + * (no charge commanded, no discharge commanded on any channel). + * 2. There is no unsafe use of the relevant PAC wrappers outside of + * this file. + * 3. The Rust type ownership system prevents "safe" external access of + * the critical hardware interface. + * 4. Charging (active or in soft stop) during any discharge event is + * unsafe behavior. + * 5. Discharging on multiple channels at once is unsafe. + * 6. All async call backs run at the highest priority. + * + * HYPOTHESES: + * 1. The charge pin is not in an active state when kick discharge pin + * is in an active state. + * 2. The charge pin is not in an active state when chip discharge pin + * is in an active state. + */ + +use crate::pins::{ChargePin, KickPin, ChipPin}; +use embassy_stm32::gpio::Output; +use embassy_time::{Duration, Timer}; +use libm::{fmaxf, fminf}; + +const MAX_KICK_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick +const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick + +const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off +const KICK_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it +const CHIP_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it + +const MAX_SAFE_RAIL_VOLTAGE: f32 = 190.0; // rail is rated for 200V, and should stop charging around 180V +const VBATT_OVERVOLTAGE_LOCKOUT: f32 = 27.2; +const VBATT_UNDERVOLTAGE_LOCKOUT: f32 = 17.2; + +#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord)] +pub enum KickType { + Kick, + Chip, + None +} + +pub struct KickManager<'a> { + // external interface + charge_pin: Output<'a, ChargePin>, + kick_pin: Output<'a, KickPin>, + chip_pin: Output<'a, ChipPin>, + + // record keeping + error_latched: bool, +} + +impl<'a> KickManager<'a> { + pub fn new( + charge_pin: Output<'a, ChargePin>, + kick_pin: Output<'a, KickPin>, + chip_pin: Output<'a, ChipPin>, + ) -> KickManager<'a> { + KickManager { + charge_pin, + kick_pin, + chip_pin, + error_latched: false + } + } + + pub async fn command(&mut self, battery_voltage: f32, rail_voltage:f32, charge: bool, kick_type: KickType, kick_strength: f32) -> Result<(), ()> { + // latch an error for invalid battery voltage + if battery_voltage > VBATT_OVERVOLTAGE_LOCKOUT || battery_voltage < VBATT_UNDERVOLTAGE_LOCKOUT { + self.error_latched = true; + } + + // latch and error for unsafe rail voltage + if rail_voltage > MAX_SAFE_RAIL_VOLTAGE { + self.error_latched = true; + } + + // if an error is latched, disable systems + if self.error_latched { + self.charge_pin.set_low(); + self.kick_pin.set_low(); + self.chip_pin.set_low(); + + return Err(()); + } + + // bound kick strength + let kick_strength = fmaxf(fminf(kick_strength, 1.0), 0.0); + + // handle charge, kick, and chip + match kick_type { + KickType::Kick => { + // disable kick and chip + self.kick_pin.set_low(); + self.chip_pin.set_low(); + + // disable charging and wait the cooldown + self.charge_pin.set_low(); + Timer::after(CHARGE_COOLDOWN).await; + + // beign kick, wait time to determine power + self.kick_pin.set_high(); + Timer::after(Duration::from_micros((MAX_KICK_DURATION_US * kick_strength) as u64)).await; + + // end kick + self.kick_pin.set_low(); + Timer::after(Duration::from_micros(10)).await; + + // reenable charging if requested + if charge { + self.charge_pin.set_high(); + } + + // cooldown for mechanical return of the subsystem + Timer::after(KICK_COOLDOWN).await; + }, + KickType::Chip => { + // disable kick and chip + self.kick_pin.set_low(); + self.chip_pin.set_low(); + + // disable charging and wait the cooldown + self.charge_pin.set_low(); + Timer::after(CHARGE_COOLDOWN).await; + + // begin chip, wait time to determine power + self.chip_pin.set_high(); + Timer::after(Duration::from_micros((MAX_CHIP_DURATION_US * kick_strength) as u64)).await; + + // end chip + self.chip_pin.set_low(); + Timer::after(Duration::from_micros(10)).await; + + // reenable charging if requested + if charge { + self.charge_pin.set_high(); + } + + // cooldown for mechanical return of the subsystem + Timer::after(CHIP_COOLDOWN).await; + } + KickType::None => { + // disable kick and chip + self.kick_pin.set_low(); + self.chip_pin.set_low(); + + // enable charging if requested + if charge { + self.charge_pin.set_high(); + } else { + self.charge_pin.set_low(); + } + } + } + + return Ok(()); + } + + pub fn clear_error(&mut self) { + self.error_latched = false; + } +} \ No newline at end of file diff --git a/kicker-board-v3/src/lib.rs b/kicker-board-v3/src/lib.rs new file mode 100644 index 00000000..608caad8 --- /dev/null +++ b/kicker-board-v3/src/lib.rs @@ -0,0 +1,44 @@ +#![no_std] +#![feature(const_mut_refs)] +#![feature(const_fn_floating_point_arithmetic)] +#![feature(async_fn_in_trait)] +#![feature(type_alias_impl_trait)] +#![feature(maybe_uninit_slice)] +#![feature(maybe_uninit_uninit_array)] +#![feature(const_maybe_uninit_uninit_array)] + +pub mod pins; +// pub mod kick_manager; +// pub mod queue; +// pub mod uart_queue; +// pub mod drivers; + +pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV + +pub const fn adc_raw_to_v(adc_raw: f32, vrefint_sample: f32) -> f32 { + adc_raw * ADC_VREFINT_NOMINAL / vrefint_sample / 1000. +} + +pub const fn adc_v_to_battery_voltage(adc_mv: f32) -> f32 { + (adc_mv / 1.65) * 25.2 +} + +pub const fn adc_200v_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 200.0 +} + +pub const fn adc_12v_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 12.0 +} + +pub const fn adc_6v2_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 6.2 +} + +pub const fn adc_5v0_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 5.0 +} + +pub const fn adc_3v3_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv / 1.25 * 3.3 +} \ No newline at end of file diff --git a/kicker-board-v3/src/pins.rs b/kicker-board-v3/src/pins.rs new file mode 100644 index 00000000..c17f84b6 --- /dev/null +++ b/kicker-board-v3/src/pins.rs @@ -0,0 +1,29 @@ +use embassy_stm32::peripherals::*; + +pub type KickPin = PE5; +pub type ChipPin = PE6; +pub type ChargePin = PE4; + +pub type PowerRailAdc = ADC1; +pub type PowerRail200vReadPin = PC0; +pub type PowerRail12vReadPin = PC1; +pub type PowerRail6v2ReadPin = PC3; +pub type PowerRail5v0ReadPin = PC2; + +pub type BreakbeamTxPin = PA3; +pub type BreakbeamRxPin = PA2; + +pub type GreenStatusLedPin = PE0; +pub type RedStatusLedPin = PE1; +pub type BlueStatusLed1Pin = PE2; +pub type BlueStatusLed2Pin = PE3; + +pub type UserBtn = PD4; +pub type PowerBtnInt = PD5; +pub type PowerKill = PD6; + +pub type ComsUartModule = USART1; +pub type ComsUartTxPin = PA9; +pub type ComsUartRxPin = PA10; +pub type ComsUartTxDma = DMA1_CH2; +pub type ComsUartRxDma = DMA1_CH3; \ No newline at end of file diff --git a/kicker-board-v3/src/queue.rs b/kicker-board-v3/src/queue.rs new file mode 100644 index 00000000..8328e5ac --- /dev/null +++ b/kicker-board-v3/src/queue.rs @@ -0,0 +1,220 @@ +use core::{ + cell::UnsafeCell, + future::poll_fn, + mem::MaybeUninit, + task::{Poll, Waker}, +}; +use critical_section; + +pub struct Buffer { + pub data: [MaybeUninit; LENGTH], + pub len: MaybeUninit, +} + +impl Buffer { + pub const EMPTY: Buffer = Buffer { + data: MaybeUninit::uninit_array(), + len: MaybeUninit::uninit(), + }; +} + +pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { + queue: &'a Queue<'a, LENGTH, DEPTH>, + data: &'a [u8], +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> DequeueRef<'a, LENGTH, DEPTH> { + pub fn data(&self) -> &[u8] { + self.data + } + pub fn cancel(self) { + self.queue.cancel_dequeue(); + } +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH, DEPTH> { + fn drop(&mut self) { + self.queue.finish_dequeue(); + } +} + +pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { + queue: &'a Queue<'a, LENGTH, DEPTH>, + data: &'a mut [u8], + len: &'a mut usize, +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> { + pub fn data(&mut self) -> &mut [u8] { + self.data + } + + pub fn len(&mut self) -> &mut usize { + self.len + } + pub fn cancel(self) { + self.queue.cancel_enqueue(); + } +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for EnqueueRef<'a, LENGTH, DEPTH> { + fn drop(&mut self) { + self.queue.finish_enqueue(); + } +} + +#[derive(PartialEq, Eq, Clone, Copy, Debug, defmt::Format)] +pub enum Error { + QueueFullEmpty, + InProgress, +} + +pub struct Queue<'a, const LENGTH: usize, const DEPTH: usize> { + buffers: &'a [Buffer; DEPTH], + read_index: UnsafeCell, + read_in_progress: UnsafeCell, + write_index: UnsafeCell, + write_in_progress: UnsafeCell, + size: UnsafeCell, + enqueue_waker: UnsafeCell>, + dequeue_waker: UnsafeCell>, +} + +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue<'a, LENGTH, DEPTH> {} +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue<'a, LENGTH, DEPTH> {} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { + pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { + Self { + buffers, + read_index: UnsafeCell::new(0), + read_in_progress: UnsafeCell::new(false), + write_index: UnsafeCell::new(0), + write_in_progress: UnsafeCell::new(false), + size: UnsafeCell::new(0), + enqueue_waker: UnsafeCell::new(None), + dequeue_waker: UnsafeCell::new(None), + } + } + + pub fn try_dequeue(&self) -> Result, Error> { + critical_section::with(|_| unsafe { + if *self.read_in_progress.get() { + return Err(Error::InProgress); + } + + if *self.size.get() > 0 { + *self.read_in_progress.get() = true; + let buf = &self.buffers[*self.read_index.get()]; + let len = buf.len.assume_init(); + let data = &MaybeUninit::slice_assume_init_ref(&buf.data)[..len]; + Ok(DequeueRef { queue: self, data }) + } else { + Err(Error::QueueFullEmpty) + } + }) + } + + pub async fn dequeue(&self) -> Result, Error> { + // TODO: look at this (when uncommented causes issue cancelling dequeue) + // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { + // return Err(Error::InProgress); + // } + + poll_fn(|cx| { + critical_section::with(|_| unsafe { + match self.try_dequeue() { + Err(Error::QueueFullEmpty) => { + *self.dequeue_waker.get() = Some(cx.waker().clone()); + Poll::Pending + } + r => Poll::Ready(r), + } + }) + }) + .await + } + + fn cancel_dequeue(&self) { + critical_section::with(|_| unsafe { + *self.read_in_progress.get() = false; + }); + } + + fn finish_dequeue(&self) { + critical_section::with(|_| unsafe { + let read_in_progress = self.read_in_progress.get(); + if *read_in_progress { + *read_in_progress = false; + *self.read_index.get() = (*self.read_index.get() + 1) % DEPTH; + *self.size.get() -= 1; + } + if let Some(w) = (*self.enqueue_waker.get()).take() { + w.wake(); + } + }); + } + + pub fn try_enqueue(&self) -> Result, Error> { + critical_section::with(|_| unsafe { + if *self.write_in_progress.get() { + return Err(Error::InProgress); + } + + if *self.size.get() < DEPTH { + *self.write_in_progress.get() = true; + let buf = &self.buffers[*self.write_index.get()]; + let buf = &mut *(buf as *const _ as *mut Buffer); + let data = MaybeUninit::slice_assume_init_mut(&mut buf.data); + let len = buf.len.write(0); + + Ok(EnqueueRef { + queue: self, + data, + len: len, + }) + } else { + Err(Error::QueueFullEmpty) + } + }) + } + + pub async fn enqueue(&self) -> Result, Error> { + if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { + return Err(Error::InProgress); + } + + poll_fn(|cx| { + critical_section::with(|_| unsafe { + match self.try_enqueue() { + Err(Error::QueueFullEmpty) => { + *self.enqueue_waker.get() = Some(cx.waker().clone()); + Poll::Pending + } + r => Poll::Ready(r), + } + }) + }) + .await + } + + fn cancel_enqueue(&self) { + critical_section::with(|_| unsafe { + *self.write_in_progress.get() = false; + }); + } + + fn finish_enqueue(&self) { + critical_section::with(|_| unsafe { + let write_in_progress = self.write_in_progress.get(); + if *write_in_progress { + *write_in_progress = false; + *self.write_index.get() = (*self.write_index.get() + 1) % DEPTH; + *self.size.get() += 1; + } + if let Some(w) = (*self.dequeue_waker.get()).take() { + w.wake(); + } + }); + } +} diff --git a/kicker-board-v3/src/uart_queue.rs b/kicker-board-v3/src/uart_queue.rs new file mode 100644 index 00000000..86c5c254 --- /dev/null +++ b/kicker-board-v3/src/uart_queue.rs @@ -0,0 +1,206 @@ +use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; + +use core::future::Future; +use defmt::info; +use embassy_executor::{raw::TaskStorage, SpawnToken}; +use embassy_stm32::{ + usart::{self, UartRx, UartTx}, +}; + +pub struct UartReadQueue< + 'a, + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, +> { + queue_rx: Queue<'a, LENGTH, DEPTH>, + task: TaskStorage>, +} + +// TODO: pretty sure shouldn't do this +unsafe impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, + > Send for UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> +{ +} + +pub type ReadTaskFuture< + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, +> = impl Future; + +impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, + > UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> +{ + pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { + Self { + queue_rx: Queue::new(buffers), + task: TaskStorage::new(), + } + } + + fn read_task( + queue_rx: &'static Queue<'a, LENGTH, DEPTH>, + mut rx: UartRx<'a, UART, DMA>, + // mut int: UART::Interrupt, + ) -> ReadTaskFuture { + async move { + loop { + let mut buf = queue_rx.enqueue().await.unwrap(); + let len = rx + .read_until_idle(buf.data()) + .await; + // .unwrap(); + // TODO: this + if let Ok(len) = len { + if len == 0 { + info!("uart zero"); + buf.cancel(); + } else { + *buf.len() = len; + } + } else { + info!("{}", len); + buf.cancel(); + } + } + } + } + + pub fn spawn_task( + &'static self, + rx: UartRx<'a, UART, DMA>, + ) -> SpawnToken { + self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) + } + + pub fn try_dequeue(&self) -> Result, Error> { + return self.queue_rx.try_dequeue(); + } + + pub async fn dequeue(&self, fn_write: impl FnOnce(&[u8]) -> RET) -> RET { + let buf = self.queue_rx.dequeue().await.unwrap(); + fn_write(buf.data()) + } +} + +pub struct UartWriteQueue< + 'a, + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, +> { + queue_tx: Queue<'a, LENGTH, DEPTH>, + task: TaskStorage>, +} + +type WriteTaskFuture< + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, +> = impl Future; + +impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, + > UartWriteQueue<'a, UART, DMA, LENGTH, DEPTH> +{ + pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { + Self { + queue_tx: Queue::new(buffers), + task: TaskStorage::new(), + } + } + + fn write_task( + queue_tx: &'static Queue<'a, LENGTH, DEPTH>, + mut tx: UartTx<'a, UART, DMA>, + ) -> WriteTaskFuture { + async move { + loop { + let buf = queue_tx.dequeue().await.unwrap(); + // defmt::info!("invoking API write"); + tx.write(buf.data()).await.unwrap(); // we are blocked here! + // defmt::info!("passed API write"); + + drop(buf); + // unsafe { + // // TODO: what does this do again? + // while !UART::regs().isr().read().tc() {} + // UART::regs().cr1().modify(|w| w.set_te(false)); + // while UART::regs().isr().read().teack() {} + // UART::regs().cr1().modify(|w| w.set_te(true)); + // } + } + } + } + + pub fn spawn_task(&'static self, tx: UartTx<'a, UART, DMA>) -> SpawnToken { + self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) + } + + pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { + let mut buf = self.queue_tx.try_enqueue()?; + let len = fn_write(buf.data()); + *buf.len() = len; + Ok(()) + } + + pub fn enqueue_copy(&self, source: &[u8]) -> Result<(), queue::Error> { + self.enqueue(|dest| { + dest[..source.len()].copy_from_slice(source); + source.len() + }) + } +} + +pub trait Reader { + async fn read RET>(&self, fn_read: FN) -> Result; +} + +pub trait Writer { + async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; +} + +impl< + 'a, + UART: usart::BasicInstance, + Dma: usart::RxDma, + const LEN: usize, + const DEPTH: usize, + > Reader for crate::uart_queue::UartReadQueue<'a, UART, Dma, LEN, DEPTH> +{ + async fn read RET>(&self, fn_read: FN) -> Result { + Ok(self.dequeue(|buf| fn_read(buf)).await) + } +} + +impl< + 'a, + UART: usart::BasicInstance, + Dma: usart::TxDma, + const LEN: usize, + const DEPTH: usize, + > Writer for crate::uart_queue::UartWriteQueue<'a, UART, Dma, LEN, DEPTH> +{ + async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { + self.enqueue(|buf| fn_write(buf)).or(Err(())) + } +} From 17153b9dad094be12aa4d54a0893c3ea971651e3 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 16 Apr 2024 23:32:09 -0400 Subject: [PATCH 002/157] updated kick image --- kicker-board-v3/src/bin/hwtest-kick/main.rs | 170 ++++---------------- 1 file changed, 35 insertions(+), 135 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-kick/main.rs b/kicker-board-v3/src/bin/hwtest-kick/main.rs index fe285d9e..0bac7402 100644 --- a/kicker-board-v3/src/bin/hwtest-kick/main.rs +++ b/kicker-board-v3/src/bin/hwtest-kick/main.rs @@ -2,94 +2,37 @@ #![no_main] #![feature(type_alias_impl_trait)] -use core::mem; - use defmt::*; use {defmt_rtt as _, panic_probe as _}; -// use core::hint::black_box; - -// use cortex_m::{peripheral::NVIC, delay::Delay}; -use cortex_m::{peripheral::NVIC}; use cortex_m_rt::entry; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, AdcPin, InternalChannel, Temperature}, - pac::Interrupt, - Peripherals, - gpio::{Input, Level, Output, Pull, Speed}, - gpio::low_level::Pin, - interrupt, adc::SampleTime, + adc::Adc, + gpio::{Level, Output, Speed}, + adc::SampleTime, }; -use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; -use embassy_time::{Delay, Duration, Timer, Ticker}; +use embassy_time::{Duration, Timer, Ticker}; use static_cell::StaticCell; -use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin, KickPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; - -// #[embassy_executor::task] -// async fn run_critical_section_task(p: Peripherals) { -// let reg_done = Input::new(p.PB4, Pull::None); -// let reg_fault = Input::new(p.PB5, Pull::None); - -// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); -// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); -// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); - -// status_led_green.set_high(); -// Timer::after(Duration::from_millis(500)).await; -// status_led_green.set_low(); -// Timer::after(Duration::from_millis(500)).await; - -// reg_charge.set_high(); -// Timer::after(Duration::from_millis(100)).await; -// reg_charge.set_low(); - -// loop { -// reg_charge.set_low(); - -// if reg_done.is_low() { -// status_led_green.set_high(); -// } else { -// status_led_green.set_low(); -// } - -// if reg_fault.is_low() { -// status_led_red.set_high(); -// } else { -// status_led_red.set_low(); -// } -// } -// } - -// #[embassy_executor::task] -// async fn read_adc_samples(aps: PubSubChannel::) -> ! { -// let sub = aps.subscriber().unwrap(); -// loop { -// info!(sub.) -// } -// } +use ateam_kicker_board_v3::*; +use ateam_kicker_board_v3::pins::*; #[embassy_executor::task] -async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, - mut hv_pin: HighVoltageReadPin, - mut batt_pin: BatteryVoltageReadPin, - mut reg_charge: ChargePin, - mut reg_done: RegulatorDonePin, - mut reg_fault: RegulatorFaultPin, - mut status_led_red: RedStatusLedPin, - mut status_led_green: GreenStatusLedPin, - mut kick_pin: KickPin) -> ! { +async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + kick_pin: KickPin) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); - let reg_done = Input::new(reg_done, Pull::None); - let reg_fault = Input::new(reg_fault, Pull::None); - let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); @@ -99,17 +42,17 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, status_led_green.set_low(); Timer::after(Duration::from_millis(500)).await; - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; let mut hv = adc.read(&mut hv_pin) as f32; - let mut bv = adc.read(&mut batt_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + let mut regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); - if start_up_battery_voltage < 18.0 { + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); + if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); warn!("refusing to continue"); loop { reg_charge.set_low(); @@ -123,18 +66,15 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, } reg_charge.set_high(); - Timer::after(Duration::from_millis(600)).await; - let reg_done_stat = reg_done.is_low(); - let reg_fault_stat = reg_fault.is_low(); + Timer::after(Duration::from_millis(50)).await; reg_charge.set_low(); - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; - // info!("hv V: {}, batt mv: {}", ((hv * 200) * 8 / 10), (bv * 25000 / 1650 * (8 / 10))); // 8 / 10 for adc offset - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); Timer::after(Duration::from_millis(1000)).await; @@ -152,76 +92,36 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; - - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); reg_charge.set_low(); kick.set_low(); - // if reg_done_stat { - // status_led_green.set_high(); - // } else { - // status_led_green.set_low(); - // } - - // if reg_fault_stat { - // status_led_red.set_high(); - // } else { - // status_led_red.set_low(); - // } - ticker.next().await; } } -// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); -#[derive(Clone, Copy)] -struct AnalogValues { - high_voltage: u16, - batt_voltage: u16, - temp: u16 -} - -// #[interrupt] -// unsafe fn I2C1() { -// EXECUTOR_HIGH.on_interrupt(); -// } - - #[entry] fn main() -> ! { let p = embassy_stm32::init(Default::default()); - info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - - // let aps = PubSubChannel::::new(); + info!("kicker startup!"); - // highest priorty, energy management - // medium priority, ADC publisher - // low priority uart handler - // lowest prioiryt main update loop - - // High-priority executor: I2C1, priority level 6 - // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - // unwrap!(spawner.spawn(run_critical_section_task(p))); - - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); - // let mut temp_ch = adc.enable_temperature(&mut Delay); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11, p.PB0))); + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PE5))); }); } \ No newline at end of file From ddcc1de05adc1f2473d28f650e428d4ffb69ba87 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 17 Apr 2024 15:53:08 -0400 Subject: [PATCH 003/157] migration work --- kicker-board-v3/src/bin/hwtest-coms/main.rs | 123 +++++++++++-------- kicker-board-v3/src/drivers/breakbeam.rs | 12 +- kicker-board-v3/src/drivers/breakbeam_pwm.rs | 27 ++-- kicker-board-v3/src/drivers/mod.rs | 1 + kicker-board-v3/src/kick_manager.rs | 12 +- kicker-board-v3/src/lib.rs | 9 +- kicker-board-v3/src/pins.rs | 6 +- kicker-board-v3/src/queue.rs | 94 +++++++------- kicker-board-v3/src/uart_queue.rs | 41 +++---- 9 files changed, 174 insertions(+), 151 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-coms/main.rs b/kicker-board-v3/src/bin/hwtest-coms/main.rs index d7c0aa25..a516ed0c 100644 --- a/kicker-board-v3/src/bin/hwtest-coms/main.rs +++ b/kicker-board-v3/src/bin/hwtest-coms/main.rs @@ -3,39 +3,35 @@ #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] -use core::mem; +use core::cell::UnsafeCell; use static_cell::StaticCell; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m::peripheral::NVIC; use cortex_m_rt::entry; use embassy_executor::{Executor, InterruptExecutor}; use embassy_stm32::{ - adc::{Adc, SampleTime}, - pac::Interrupt, + adc::{Adc, Resolution, SampleTime}, gpio::{Level, Output, Speed}, - interrupt, - time::mhz, - usart::{Uart, Parity, StopBits, Config} + interrupt::{self, InterruptExt}, + pac::Interrupt, + rcc::{AHBPrescaler, APBPrescaler, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllSource, Sysclk}, + usart::{Config, Parity, StopBits, Uart} }; use embassy_stm32::{bind_interrupts, peripherals, usart}; use embassy_time::{Delay, Duration, Instant, Ticker}; -use ateam_kicker_board::{ +use ateam_kicker_board_v3::{ adc_raw_to_v, adc_v_to_battery_voltage, - adc_v_to_rail_voltage, kick_manager::{ KickManager, KickType}, pins::{ - HighVoltageReadPin, BatteryVoltageReadPin, - ChargePin, KickPin, ChipPin, - ComsUartModule, ComsUartRxDma, ComsUartTxDma, RedStatusLedPin, BlueStatusLedPin}, + BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, ComsUartTxDma, KickPin, PowerRail200vReadPin, RedStatusLedPin}, queue::Buffer, uart_queue::{ UartReadQueue, @@ -52,17 +48,19 @@ const RX_BUF_DEPTH: usize = 3; // control communications tx buffer #[link_section = ".bss"] -static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); +static mut COMS_BUFFERS_TX: StaticCell; TX_BUF_DEPTH]>> = StaticCell::new(); +static COMS_QUEUE_TX: StaticCell> = StaticCell::new(); // control communications rx buffer #[link_section = ".bss"] -static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); +static mut COMS_BUFFERS_RX: StaticCell; RX_BUF_DEPTH]>> = StaticCell::new(); +static COMS_QUEUE_RX: StaticCell> = StaticCell::new(); + +// #[link_section = ".bss"] +// static mut COMS_BUFFERS_RX: UnsafeCell<[Buffer; RX_BUF_DEPTH]> = +// UnsafeCell::new([Buffer::EMPTY; RX_BUF_DEPTH]); +// static COMS_QUEUE_RX: UartReadQueue = +// UartReadQueue::new(unsafe { COMS_BUFFERS_RX }); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -84,16 +82,16 @@ fn get_empty_telem_packet() -> KickerTelemetry { #[embassy_executor::task] async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue<'static, ComsUartModule, ComsUartRxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH>, - coms_writer: &'static UartWriteQueue<'static, ComsUartModule, ComsUartTxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH>, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + coms_reader: &'static UartReadQueue, + coms_writer: &'static UartWriteQueue, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, charge_pin: ChargePin, kick_pin: KickPin, chip_pin: ChipPin, - mut rail_pin: HighVoltageReadPin, - mut battery_voltage_pin: BatteryVoltageReadPin, + mut rail_pin: PowerRail200vReadPin, err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin) -> ! { + ball_detected_led1_pin: BlueStatusLed1Pin, + ball_detected_led2_pin: BlueStatusLed2Pin) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -103,7 +101,9 @@ async fn high_pri_kick_task( // debug LEDs let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); + // TODO dotstars // coms buffers @@ -178,10 +178,16 @@ async fn high_pri_kick_task( let _res = coms_writer.enqueue_copy(struct_bytes); } - if ball_detected_led.is_set_high() { - ball_detected_led.set_low(); + if ball_detected_led1.is_set_high() { + ball_detected_led1.set_low(); + } else { + ball_detected_led1.set_high(); + } + + if ball_detected_led2.is_set_high() { + ball_detected_led2.set_low(); } else { - ball_detected_led.set_high(); + ball_detected_led2.set_high(); } last_packet_sent_time = cur_time; @@ -196,9 +202,12 @@ async fn high_pri_kick_task( } if ball_detected { - ball_detected_led.set_high(); + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); } else { - ball_detected_led.set_low(); + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); + } // TODO Dotstar @@ -212,7 +221,7 @@ static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[interrupt] -unsafe fn I2C1() { +unsafe fn TIM2() { EXECUTOR_HIGH.on_interrupt(); } @@ -223,29 +232,45 @@ bind_interrupts!(struct Irqs { #[entry] fn main() -> ! { let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); + stm32_config.rcc.hsi = true; + stm32_config.rcc.pll_src = PllSource::HSI; // internal 16Mhz source + stm32_config.rcc.pll = Some(Pll { + prediv: PllPreDiv::DIV8, // base 2MHz + mul: PllMul::MUL168, // mul up to 336 MHz + divp: Some(PllPDiv::DIV2), // div down to 168 MHz, AHB (max) + divq: Some(PllQDiv::DIV7), // div down to 48 Mhz for dedicated 48MHz clk (exact) + divr: None, // turn off I2S clk + }); + stm32_config.rcc.ahb_pre = AHBPrescaler::DIV1; // AHB 168 MHz (max) + stm32_config.rcc.apb1_pre = APBPrescaler::DIV4; // APB1 42 MHz (max) + stm32_config.rcc.apb2_pre = APBPrescaler::DIV2; // APB2 84 MHz (max) + stm32_config.rcc.sys = Sysclk::PLL1_P; // enable the system let p = embassy_stm32::init(stm32_config); info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; + let coms_buffers_tx = unsafe { COMS_BUFFERS_TX.init(UnsafeCell::new([Buffer::EMPTY; TX_BUF_DEPTH])) }; + // I don't think the queue actually needs to be static or linked into a specific section of memory + let coms_queue_tx = unsafe { COMS_QUEUE_TX.init(UartWriteQueue::new(*coms_buffers_tx)) }; + + let coms_buffers_rx = unsafe { COMS_BUFFERS_RX.init(UnsafeCell::new([Buffer::EMPTY; RX_BUF_DEPTH])) }; + // I don't think the queue actually needs to be static or linked into a specific section of memory + let coms_queue_rx = unsafe { COMS_QUEUE_RX.init(UartReadQueue::new(*coms_buffers_rx)) }; let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PB3, p.PB0, p.PB1, p.PA0, p.PA1, p.PA12, p.PA8))); - // unwrap!(spawner.spawn(high_pri_tx_test(&COMS_QUEUE_TX, p.PB3, p.PB0, p.PB1))); + interrupt::TIM2.set_priority(interrupt::Priority::P6); + let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + unwrap!(spawner.spawn(high_pri_kick_task(&coms_queue_rx, &coms_queue_tx, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -261,18 +286,18 @@ fn main() -> ! { p.PA10, p.PA9, Irqs, - p.DMA1_CH2, - p.DMA1_CH3, + p.DMA2_CH7, + p.DMA2_CH2, coms_uart_config, - ); + ).unwrap(); - let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); // low priority executor handles coms and user IO // Low priority executor: runs in thread mode, using WFE/SEV let lp_executor = EXECUTOR_LOW.init(Executor::new()); lp_executor.run(|spawner| { - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + unwrap!(spawner.spawn(coms_queue_tx.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(coms_queue_rx.spawn_task(coms_uart_rx))); }); } \ No newline at end of file diff --git a/kicker-board-v3/src/drivers/breakbeam.rs b/kicker-board-v3/src/drivers/breakbeam.rs index ae58ce90..a109ace3 100644 --- a/kicker-board-v3/src/drivers/breakbeam.rs +++ b/kicker-board-v3/src/drivers/breakbeam.rs @@ -1,12 +1,12 @@ -use embassy_stm32::gpio::{Output, Input, Level, Speed, Pin, Pull}; +use embassy_stm32::{gpio::{Input, Level, Output, Pin, Pull, Speed}, Peripheral}; -pub struct Breakbeam<'a, PinTx: Pin, PinRx: Pin> { - pin_tx: Output<'a, PinTx>, - pin_rx: Input<'a, PinRx>, +pub struct Breakbeam<'a> { + pin_tx: Output<'a>, + pin_rx: Input<'a>, } -impl<'a, PinTx: Pin, PinRx: Pin> Breakbeam<'a, PinTx, PinRx> { - pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { +impl<'a> Breakbeam<'a> { + pub fn new(pin_tx: impl Peripheral

+ 'a, pin_rx: impl Peripheral

+ 'a) -> Self { let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); let pin_rx = Input::new(pin_rx, Pull::Down); Self { diff --git a/kicker-board-v3/src/drivers/breakbeam_pwm.rs b/kicker-board-v3/src/drivers/breakbeam_pwm.rs index 63d9ceb0..c6179c2d 100644 --- a/kicker-board-v3/src/drivers/breakbeam_pwm.rs +++ b/kicker-board-v3/src/drivers/breakbeam_pwm.rs @@ -1,21 +1,28 @@ -use embassy_stm32::gpio::{Output, Input, Pin, Pull}; -use embassy_stm32::pwm::simple_pwm::{PwmPin, SimplePwm}; -use embassy_stm32::pwm::Channel; +use embassy_stm32::gpio::{Input, Output, OutputType, Pin, Pull}; +use embassy_stm32::timer::{ + GeneralInstance4Channel, + Channel, + simple_pwm::{ + PwmPin, + SimplePwm}}; use embassy_stm32::time::khz; +use embassy_stm32::Peripheral; use embassy_time::{Duration, Timer}; -pub struct Breakbeam<'a, PwmTx: Pin, PinRx: Pin> { - pin_tx: SimplePwm<'a, PwmTx>, - pin_rx: Input<'a, PinRx>, +pub struct Breakbeam<'a, Timer: GeneralInstance4Channel> { + pin_tx: SimplePwm<'a, Timer>, + pin_rx: Input<'a>, } -impl<'a, PinTx: Pin, PinRx: Pin, Timer: Peripheral> Breakbeam<'a, PinTx, PinRx, > { - pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { - let ch_1 = PwmPin::new_ch1(pin_tx); +impl<'a, Timer: GeneralInstance4Channel> Breakbeam<'a, Timer> { + pub fn new(pin_tx: PwmPin<'a, Timer, Channel>, pin_rx: impl Peripheral

+ 'a) -> Self { + let ch_1 = PwmPin::new_ch1(pin_tx, OutputType::PushPull); let mut pwm_tx = SimplePwm::new(pin_tx, Level::High, Speed::Low); + let pin_rx = Input::new(pin_rx, Pull::Down); + Self { - pwm_tx, + pin_tx: pwm_tx, pin_rx } } diff --git a/kicker-board-v3/src/drivers/mod.rs b/kicker-board-v3/src/drivers/mod.rs index c69230cd..c5f5cce8 100644 --- a/kicker-board-v3/src/drivers/mod.rs +++ b/kicker-board-v3/src/drivers/mod.rs @@ -1 +1,2 @@ +// pub mod breakbeam_pwm; pub mod breakbeam; \ No newline at end of file diff --git a/kicker-board-v3/src/kick_manager.rs b/kicker-board-v3/src/kick_manager.rs index 02c0bb8f..60aecf35 100644 --- a/kicker-board-v3/src/kick_manager.rs +++ b/kicker-board-v3/src/kick_manager.rs @@ -47,9 +47,9 @@ pub enum KickType { pub struct KickManager<'a> { // external interface - charge_pin: Output<'a, ChargePin>, - kick_pin: Output<'a, KickPin>, - chip_pin: Output<'a, ChipPin>, + charge_pin: Output<'a>, + kick_pin: Output<'a>, + chip_pin: Output<'a>, // record keeping error_latched: bool, @@ -57,9 +57,9 @@ pub struct KickManager<'a> { impl<'a> KickManager<'a> { pub fn new( - charge_pin: Output<'a, ChargePin>, - kick_pin: Output<'a, KickPin>, - chip_pin: Output<'a, ChipPin>, + charge_pin: Output<'a>, + kick_pin: Output<'a>, + chip_pin: Output<'a>, ) -> KickManager<'a> { KickManager { charge_pin, diff --git a/kicker-board-v3/src/lib.rs b/kicker-board-v3/src/lib.rs index 608caad8..a28d03be 100644 --- a/kicker-board-v3/src/lib.rs +++ b/kicker-board-v3/src/lib.rs @@ -1,17 +1,16 @@ #![no_std] #![feature(const_mut_refs)] #![feature(const_fn_floating_point_arithmetic)] -#![feature(async_fn_in_trait)] #![feature(type_alias_impl_trait)] #![feature(maybe_uninit_slice)] #![feature(maybe_uninit_uninit_array)] #![feature(const_maybe_uninit_uninit_array)] pub mod pins; -// pub mod kick_manager; -// pub mod queue; -// pub mod uart_queue; -// pub mod drivers; +pub mod kick_manager; +pub mod queue; +pub mod uart_queue; +pub mod drivers; pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV diff --git a/kicker-board-v3/src/pins.rs b/kicker-board-v3/src/pins.rs index c17f84b6..7a842254 100644 --- a/kicker-board-v3/src/pins.rs +++ b/kicker-board-v3/src/pins.rs @@ -1,8 +1,8 @@ use embassy_stm32::peripherals::*; +pub type ChargePin = PE4; pub type KickPin = PE5; pub type ChipPin = PE6; -pub type ChargePin = PE4; pub type PowerRailAdc = ADC1; pub type PowerRail200vReadPin = PC0; @@ -25,5 +25,5 @@ pub type PowerKill = PD6; pub type ComsUartModule = USART1; pub type ComsUartTxPin = PA9; pub type ComsUartRxPin = PA10; -pub type ComsUartTxDma = DMA1_CH2; -pub type ComsUartRxDma = DMA1_CH3; \ No newline at end of file +pub type ComsUartTxDma = DMA2_CH7; +pub type ComsUartRxDma = DMA2_CH2; \ No newline at end of file diff --git a/kicker-board-v3/src/queue.rs b/kicker-board-v3/src/queue.rs index 8328e5ac..b5ce34a6 100644 --- a/kicker-board-v3/src/queue.rs +++ b/kicker-board-v3/src/queue.rs @@ -1,8 +1,5 @@ use core::{ - cell::UnsafeCell, - future::poll_fn, - mem::MaybeUninit, - task::{Poll, Waker}, + cell::UnsafeCell, future::poll_fn, mem::MaybeUninit, sync::atomic::{AtomicBool, AtomicUsize, Ordering}, task::{Poll, Waker} }; use critical_section; @@ -19,7 +16,7 @@ impl Buffer { } pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, + queue: &'a Queue, data: &'a [u8], } @@ -39,7 +36,7 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH } pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, + queue: &'a Queue, data: &'a mut [u8], len: &'a mut usize, } @@ -69,29 +66,29 @@ pub enum Error { InProgress, } -pub struct Queue<'a, const LENGTH: usize, const DEPTH: usize> { - buffers: &'a [Buffer; DEPTH], - read_index: UnsafeCell, - read_in_progress: UnsafeCell, - write_index: UnsafeCell, - write_in_progress: UnsafeCell, - size: UnsafeCell, +pub struct Queue { + buffers: UnsafeCell<[Buffer; DEPTH]>, + read_index: AtomicUsize, + read_in_progress: AtomicBool, + write_index: AtomicUsize, + write_in_progress: AtomicBool, + size: AtomicUsize, enqueue_waker: UnsafeCell>, dequeue_waker: UnsafeCell>, } -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue<'a, LENGTH, DEPTH> {} -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue<'a, LENGTH, DEPTH> {} +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue {} +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue {} -impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { +impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { + pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { Self { - buffers, - read_index: UnsafeCell::new(0), - read_in_progress: UnsafeCell::new(false), - write_index: UnsafeCell::new(0), - write_in_progress: UnsafeCell::new(false), - size: UnsafeCell::new(0), + buffers: buffers, + read_index: AtomicUsize::new(0), + read_in_progress: AtomicBool::new(false), + write_index: AtomicUsize::new(0), + write_in_progress: AtomicBool::new(false), + size: AtomicUsize::new(0), enqueue_waker: UnsafeCell::new(None), dequeue_waker: UnsafeCell::new(None), } @@ -99,15 +96,18 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { pub fn try_dequeue(&self) -> Result, Error> { critical_section::with(|_| unsafe { - if *self.read_in_progress.get() { + if self.read_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); } - if *self.size.get() > 0 { - *self.read_in_progress.get() = true; - let buf = &self.buffers[*self.read_index.get()]; + if self.size.load(Ordering::SeqCst) > 0 { + self.read_in_progress.store(true, Ordering::SeqCst); + + let bufs = &*self.buffers.get(); + let buf = &bufs[self.read_index.load(Ordering::SeqCst)]; let len = buf.len.assume_init(); let data = &MaybeUninit::slice_assume_init_ref(&buf.data)[..len]; + Ok(DequeueRef { queue: self, data }) } else { Err(Error::QueueFullEmpty) @@ -136,19 +136,18 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { } fn cancel_dequeue(&self) { - critical_section::with(|_| unsafe { - *self.read_in_progress.get() = false; - }); + self.read_in_progress.store(false, Ordering::SeqCst); } fn finish_dequeue(&self) { critical_section::with(|_| unsafe { - let read_in_progress = self.read_in_progress.get(); - if *read_in_progress { - *read_in_progress = false; - *self.read_index.get() = (*self.read_index.get() + 1) % DEPTH; - *self.size.get() -= 1; + if self.read_in_progress.load(Ordering::SeqCst) { + self.read_in_progress.store(false, Ordering::SeqCst); + + self.read_index.store((self.read_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.size.fetch_sub(1, Ordering::SeqCst); } + if let Some(w) = (*self.enqueue_waker.get()).take() { w.wake(); } @@ -157,14 +156,14 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { pub fn try_enqueue(&self) -> Result, Error> { critical_section::with(|_| unsafe { - if *self.write_in_progress.get() { + if self.write_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); } - if *self.size.get() < DEPTH { - *self.write_in_progress.get() = true; - let buf = &self.buffers[*self.write_index.get()]; - let buf = &mut *(buf as *const _ as *mut Buffer); + if self.size.load(Ordering::SeqCst) < DEPTH { + self.write_in_progress.store(true, Ordering::SeqCst); + let bufs = &mut *self.buffers.get(); + let buf = &mut bufs[self.write_index.load(Ordering::SeqCst)]; let data = MaybeUninit::slice_assume_init_mut(&mut buf.data); let len = buf.len.write(0); @@ -199,19 +198,18 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { } fn cancel_enqueue(&self) { - critical_section::with(|_| unsafe { - *self.write_in_progress.get() = false; - }); + self.write_in_progress.store(false, Ordering::SeqCst); } fn finish_enqueue(&self) { critical_section::with(|_| unsafe { - let write_in_progress = self.write_in_progress.get(); - if *write_in_progress { - *write_in_progress = false; - *self.write_index.get() = (*self.write_index.get() + 1) % DEPTH; - *self.size.get() += 1; + if self.write_in_progress.load(Ordering::SeqCst) { + self.write_in_progress.store(false, Ordering::SeqCst); + + self.write_index.store((self.write_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.size.fetch_add(1, Ordering::SeqCst); } + if let Some(w) = (*self.dequeue_waker.get()).take() { w.wake(); } diff --git a/kicker-board-v3/src/uart_queue.rs b/kicker-board-v3/src/uart_queue.rs index 86c5c254..a1047d7b 100644 --- a/kicker-board-v3/src/uart_queue.rs +++ b/kicker-board-v3/src/uart_queue.rs @@ -1,20 +1,17 @@ use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; -use core::future::Future; +use core::{cell::UnsafeCell, future::Future}; use defmt::info; use embassy_executor::{raw::TaskStorage, SpawnToken}; -use embassy_stm32::{ - usart::{self, UartRx, UartTx}, -}; +use embassy_stm32::{mode::Async, usart::{self, UartRx, UartTx}}; pub struct UartReadQueue< - 'a, UART: usart::BasicInstance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, > { - queue_rx: Queue<'a, LENGTH, DEPTH>, + queue_rx: Queue, task: TaskStorage>, } @@ -25,7 +22,7 @@ unsafe impl< DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, - > Send for UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> + > Send for UartReadQueue { } @@ -42,9 +39,9 @@ impl< DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, - > UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> + > UartReadQueue { - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { + pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { Self { queue_rx: Queue::new(buffers), task: TaskStorage::new(), @@ -52,9 +49,8 @@ impl< } fn read_task( - queue_rx: &'static Queue<'a, LENGTH, DEPTH>, - mut rx: UartRx<'a, UART, DMA>, - // mut int: UART::Interrupt, + queue_rx: &'static Queue, + mut rx: UartRx<'static, UART, Async>, ) -> ReadTaskFuture { async move { loop { @@ -81,7 +77,7 @@ impl< pub fn spawn_task( &'static self, - rx: UartRx<'a, UART, DMA>, + rx: UartRx<'static, UART, Async>, ) -> SpawnToken { self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) } @@ -97,13 +93,12 @@ impl< } pub struct UartWriteQueue< - 'a, UART: usart::BasicInstance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, > { - queue_tx: Queue<'a, LENGTH, DEPTH>, + queue_tx: Queue, task: TaskStorage>, } @@ -120,9 +115,9 @@ impl< DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, - > UartWriteQueue<'a, UART, DMA, LENGTH, DEPTH> + > UartWriteQueue { - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { + pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { Self { queue_tx: Queue::new(buffers), task: TaskStorage::new(), @@ -130,8 +125,8 @@ impl< } fn write_task( - queue_tx: &'static Queue<'a, LENGTH, DEPTH>, - mut tx: UartTx<'a, UART, DMA>, + queue_tx: &'static Queue, + mut tx: UartTx<'static, UART, Async>, ) -> WriteTaskFuture { async move { loop { @@ -152,7 +147,7 @@ impl< } } - pub fn spawn_task(&'static self, tx: UartTx<'a, UART, DMA>) -> SpawnToken { + pub fn spawn_task(&'static self, tx: UartTx<'static, UART, Async>) -> SpawnToken { self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) } @@ -180,12 +175,11 @@ pub trait Writer { } impl< - 'a, UART: usart::BasicInstance, Dma: usart::RxDma, const LEN: usize, const DEPTH: usize, - > Reader for crate::uart_queue::UartReadQueue<'a, UART, Dma, LEN, DEPTH> + > Reader for crate::uart_queue::UartReadQueue { async fn read RET>(&self, fn_read: FN) -> Result { Ok(self.dequeue(|buf| fn_read(buf)).await) @@ -193,12 +187,11 @@ impl< } impl< - 'a, UART: usart::BasicInstance, Dma: usart::TxDma, const LEN: usize, const DEPTH: usize, - > Writer for crate::uart_queue::UartWriteQueue<'a, UART, Dma, LEN, DEPTH> + > Writer for crate::uart_queue::UartWriteQueue { async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { self.enqueue(|buf| fn_write(buf)).or(Err(())) From 619596d4793ef5abaaa681f68a6542e67db3d19f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 20 Apr 2024 16:58:06 -0400 Subject: [PATCH 004/157] migration work for kicker v3 --- flake.nix | 1 + kicker-board-v3/src/bin/hwtest-coms/main.rs | 69 +++++++--------- kicker-board-v3/src/kick_manager.rs | 1 - kicker-board-v3/src/lib.rs | 1 + kicker-board-v3/src/queue.rs | 89 +++++++++++++++------ kicker-board-v3/src/uart_queue.rs | 10 ++- 6 files changed, 104 insertions(+), 67 deletions(-) diff --git a/flake.nix b/flake.nix index 26851bce..d7bb25eb 100644 --- a/flake.nix +++ b/flake.nix @@ -53,6 +53,7 @@ })) rust-analyzer probe-run + probe-rs # Python (pkgs.${python}.withPackages diff --git a/kicker-board-v3/src/bin/hwtest-coms/main.rs b/kicker-board-v3/src/bin/hwtest-coms/main.rs index a516ed0c..493bea7a 100644 --- a/kicker-board-v3/src/bin/hwtest-coms/main.rs +++ b/kicker-board-v3/src/bin/hwtest-coms/main.rs @@ -2,8 +2,9 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] -use core::cell::UnsafeCell; +use core::cell::SyncUnsafeCell; use static_cell::StaticCell; use defmt::*; @@ -15,25 +16,22 @@ use embassy_executor::{Executor, InterruptExecutor}; use embassy_stm32::{ adc::{Adc, Resolution, SampleTime}, gpio::{Level, Output, Speed}, - interrupt::{self, InterruptExt}, + interrupt, + interrupt::InterruptExt, pac::Interrupt, rcc::{AHBPrescaler, APBPrescaler, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllSource, Sysclk}, usart::{Config, Parity, StopBits, Uart} }; use embassy_stm32::{bind_interrupts, peripherals, usart}; -use embassy_time::{Delay, Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker}; use ateam_kicker_board_v3::{ - adc_raw_to_v, - adc_v_to_battery_voltage, + adc_200v_to_rail_voltage, adc_raw_to_v, kick_manager::{ KickManager, - KickType}, - pins::{ - BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, ComsUartTxDma, KickPin, PowerRail200vReadPin, RedStatusLedPin}, - queue::Buffer, - uart_queue::{ + KickType}, pins::{ + BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, ComsUartTxDma, KickPin, PowerRail200vReadPin, RedStatusLedPin}, queue::Buffer, uart_queue::{ UartReadQueue, UartWriteQueue, } @@ -47,20 +45,22 @@ const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; // control communications tx buffer +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".bss"] -static mut COMS_BUFFERS_TX: StaticCell; TX_BUF_DEPTH]>> = StaticCell::new(); -static COMS_QUEUE_TX: StaticCell> = StaticCell::new(); +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX); // control communications rx buffer +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".bss"] -static mut COMS_BUFFERS_RX: StaticCell; RX_BUF_DEPTH]>> = StaticCell::new(); -static COMS_QUEUE_RX: StaticCell> = StaticCell::new(); - -// #[link_section = ".bss"] -// static mut COMS_BUFFERS_RX: UnsafeCell<[Buffer; RX_BUF_DEPTH]> = -// UnsafeCell::new([Buffer::EMPTY; RX_BUF_DEPTH]); -// static COMS_QUEUE_RX: UartReadQueue = -// UartReadQueue::new(unsafe { COMS_BUFFERS_RX }); +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -107,7 +107,7 @@ async fn high_pri_kick_task( // TODO dotstars // coms buffers - let mut telemetry_enabled: bool = false; + let mut telemetry_enabled: bool; let mut kicker_control_packet: KickerControl = get_empty_control_packet(); let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); @@ -116,11 +116,10 @@ async fn high_pri_kick_task( let mut last_packet_sent_time = Instant::now(); loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; - let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); - let battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample)); + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); // optionally pre-flag errors? ///////////////////////////////////// @@ -156,7 +155,7 @@ async fn high_pri_kick_task( telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; // no charge/kick in coms test - let res = kick_manager.command(battery_voltage, rail_voltage, false, KickType::None, 0.0).await; + let res = kick_manager.command(22.5, rail_voltage, false, KickType::None, 0.0).await; // send telemetry packet if telemetry_enabled { @@ -164,7 +163,7 @@ async fn high_pri_kick_task( if Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > 20 { kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1(0, 0, ball_detected as u32, res.is_err() as u32); kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = battery_voltage; + kicker_telemetry_packet.battery_voltage = 22.5; // raw interpretaion of a struct for wire transmission is unsafe unsafe { @@ -249,14 +248,6 @@ fn main() -> ! { let p = embassy_stm32::init(stm32_config); info!("kicker startup!"); - - let coms_buffers_tx = unsafe { COMS_BUFFERS_TX.init(UnsafeCell::new([Buffer::EMPTY; TX_BUF_DEPTH])) }; - // I don't think the queue actually needs to be static or linked into a specific section of memory - let coms_queue_tx = unsafe { COMS_QUEUE_TX.init(UartWriteQueue::new(*coms_buffers_tx)) }; - - let coms_buffers_rx = unsafe { COMS_BUFFERS_RX.init(UnsafeCell::new([Buffer::EMPTY; RX_BUF_DEPTH])) }; - // I don't think the queue actually needs to be static or linked into a specific section of memory - let coms_queue_rx = unsafe { COMS_QUEUE_RX.init(UartReadQueue::new(*coms_buffers_rx)) }; let _status_led = Output::new(p.PA11, Level::High, Speed::Low); @@ -267,10 +258,10 @@ fn main() -> ! { // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - interrupt::TIM2.set_priority(interrupt::Priority::P6); + embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(spawner.spawn(high_pri_kick_task(&coms_queue_rx, &coms_queue_tx, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -297,7 +288,7 @@ fn main() -> ! { // Low priority executor: runs in thread mode, using WFE/SEV let lp_executor = EXECUTOR_LOW.init(Executor::new()); lp_executor.run(|spawner| { - unwrap!(spawner.spawn(coms_queue_tx.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(coms_queue_rx.spawn_task(coms_uart_rx))); + unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); }); } \ No newline at end of file diff --git a/kicker-board-v3/src/kick_manager.rs b/kicker-board-v3/src/kick_manager.rs index 60aecf35..b5a88894 100644 --- a/kicker-board-v3/src/kick_manager.rs +++ b/kicker-board-v3/src/kick_manager.rs @@ -22,7 +22,6 @@ * is in an active state. */ -use crate::pins::{ChargePin, KickPin, ChipPin}; use embassy_stm32::gpio::Output; use embassy_time::{Duration, Timer}; use libm::{fmaxf, fminf}; diff --git a/kicker-board-v3/src/lib.rs b/kicker-board-v3/src/lib.rs index a28d03be..304db521 100644 --- a/kicker-board-v3/src/lib.rs +++ b/kicker-board-v3/src/lib.rs @@ -5,6 +5,7 @@ #![feature(maybe_uninit_slice)] #![feature(maybe_uninit_uninit_array)] #![feature(const_maybe_uninit_uninit_array)] +#![feature(sync_unsafe_cell)] pub mod pins; pub mod kick_manager; diff --git a/kicker-board-v3/src/queue.rs b/kicker-board-v3/src/queue.rs index b5ce34a6..ee8fdb51 100644 --- a/kicker-board-v3/src/queue.rs +++ b/kicker-board-v3/src/queue.rs @@ -1,17 +1,23 @@ use core::{ - cell::UnsafeCell, future::poll_fn, mem::MaybeUninit, sync::atomic::{AtomicBool, AtomicUsize, Ordering}, task::{Poll, Waker} + cell::{SyncUnsafeCell, UnsafeCell}, + future::poll_fn, + mem::MaybeUninit, + sync::atomic::{AtomicBool, AtomicUsize, Ordering}, + task::{Poll, Waker} }; use critical_section; pub struct Buffer { pub data: [MaybeUninit; LENGTH], - pub len: MaybeUninit, + // pub len: MaybeUninit, + len: usize, } impl Buffer { pub const EMPTY: Buffer = Buffer { data: MaybeUninit::uninit_array(), - len: MaybeUninit::uninit(), + // len: MaybeUninit::uninit(), + len: 0 }; } @@ -67,7 +73,7 @@ pub enum Error { } pub struct Queue { - buffers: UnsafeCell<[Buffer; DEPTH]>, + buffers: &'static [SyncUnsafeCell>; DEPTH], read_index: AtomicUsize, read_in_progress: AtomicBool, write_index: AtomicUsize, @@ -81,7 +87,7 @@ unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue Sync for Queue {} impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { - pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { Self { buffers: buffers, read_index: AtomicUsize::new(0), @@ -95,7 +101,7 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { } pub fn try_dequeue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { + critical_section::with(|_| { if self.read_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); } @@ -103,10 +109,18 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { if self.size.load(Ordering::SeqCst) > 0 { self.read_in_progress.store(true, Ordering::SeqCst); - let bufs = &*self.buffers.get(); - let buf = &bufs[self.read_index.load(Ordering::SeqCst)]; - let len = buf.len.assume_init(); - let data = &MaybeUninit::slice_assume_init_ref(&buf.data)[..len]; + /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. + * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but + * not both at once. + */ + let buf = unsafe { &*self.buffers[self.read_index.load(Ordering::SeqCst)].get() }; + // let len = unsafe { buf.len.assume_init() } ; + /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file + * (where a user can specify the link section) and so the compiler knows the type and satisfied + * defined behavior constraints w.r.t data alignment and init values, and therefore referencing + * the buffer means the internal data is valid. + */ + let data = unsafe { &MaybeUninit::slice_assume_init_ref(&buf.data)[..buf.len] }; Ok(DequeueRef { queue: self, data }) } else { @@ -122,10 +136,13 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { // } poll_fn(|cx| { - critical_section::with(|_| unsafe { + critical_section::with(|_| { match self.try_dequeue() { Err(Error::QueueFullEmpty) => { - *self.dequeue_waker.get() = Some(cx.waker().clone()); + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + unsafe { *self.dequeue_waker.get() = Some(cx.waker().clone()) }; Poll::Pending } r => Poll::Ready(r), @@ -140,7 +157,7 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { } fn finish_dequeue(&self) { - critical_section::with(|_| unsafe { + critical_section::with(|_| { if self.read_in_progress.load(Ordering::SeqCst) { self.read_in_progress.store(false, Ordering::SeqCst); @@ -148,29 +165,44 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { self.size.fetch_sub(1, Ordering::SeqCst); } - if let Some(w) = (*self.enqueue_waker.get()).take() { + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + if let Some(w) = unsafe { (*self.enqueue_waker.get()).take() } { w.wake(); } }); } pub fn try_enqueue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { + critical_section::with(|_| { if self.write_in_progress.load(Ordering::SeqCst) { return Err(Error::InProgress); } if self.size.load(Ordering::SeqCst) < DEPTH { self.write_in_progress.store(true, Ordering::SeqCst); - let bufs = &mut *self.buffers.get(); - let buf = &mut bufs[self.write_index.load(Ordering::SeqCst)]; - let data = MaybeUninit::slice_assume_init_mut(&mut buf.data); - let len = buf.len.write(0); + /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. + * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but + * not both at once. + */ + let buf = unsafe { &mut *self.buffers[self.write_index.load(Ordering::SeqCst)].get() }; + /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file + * (where a user can specify the link section) and so the compiler knows the type and satisfied + * defined behavior constraints w.r.t data alignment and init values, and therefore referencing + * the buffer means the internal data is valid. + */ + let data = unsafe { MaybeUninit::slice_assume_init_mut(&mut buf.data) }; + + // TODO CHCEK: https://doc.rust-lang.org/std/mem/union.MaybeUninit.html#method.write-1 this should overwrite the value and + // return a mut ref to the new value + // let len = buf.len.write(0); + buf.len = 0; Ok(EnqueueRef { queue: self, data, - len: len, + len: &mut buf.len, }) } else { Err(Error::QueueFullEmpty) @@ -179,15 +211,21 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { } pub async fn enqueue(&self) -> Result, Error> { + /* Safety: this raw pointer access is safe because the underlying memory is statically allocated + * and the total read operation is atomic across tasks because of the critical_section closure + */ if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { return Err(Error::InProgress); } poll_fn(|cx| { - critical_section::with(|_| unsafe { + critical_section::with(|_| { match self.try_enqueue() { Err(Error::QueueFullEmpty) => { - *self.enqueue_waker.get() = Some(cx.waker().clone()); + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + unsafe { *self.enqueue_waker.get() = Some(cx.waker().clone()) }; Poll::Pending } r => Poll::Ready(r), @@ -202,7 +240,7 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { } fn finish_enqueue(&self) { - critical_section::with(|_| unsafe { + critical_section::with(|_| { if self.write_in_progress.load(Ordering::SeqCst) { self.write_in_progress.store(false, Ordering::SeqCst); @@ -210,7 +248,10 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { self.size.fetch_add(1, Ordering::SeqCst); } - if let Some(w) = (*self.dequeue_waker.get()).take() { + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + if let Some(w) = unsafe { (*self.dequeue_waker.get()).take() } { w.wake(); } }); diff --git a/kicker-board-v3/src/uart_queue.rs b/kicker-board-v3/src/uart_queue.rs index a1047d7b..d35faa1a 100644 --- a/kicker-board-v3/src/uart_queue.rs +++ b/kicker-board-v3/src/uart_queue.rs @@ -1,6 +1,10 @@ +#![warn(async_fn_in_trait)] + use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; -use core::{cell::UnsafeCell, future::Future}; +use core::{ + cell::SyncUnsafeCell, + future::Future}; use defmt::info; use embassy_executor::{raw::TaskStorage, SpawnToken}; use embassy_stm32::{mode::Async, usart::{self, UartRx, UartTx}}; @@ -41,7 +45,7 @@ impl< const DEPTH: usize, > UartReadQueue { - pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { + pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH]) -> Self { Self { queue_rx: Queue::new(buffers), task: TaskStorage::new(), @@ -117,7 +121,7 @@ impl< const DEPTH: usize, > UartWriteQueue { - pub const fn new(buffers: UnsafeCell<[Buffer; DEPTH]>) -> Self { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { Self { queue_tx: Queue::new(buffers), task: TaskStorage::new(), From 540f02f039e2042bbd03d3581a956982ce6074bb Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 20 Apr 2024 17:46:36 -0400 Subject: [PATCH 005/157] migration work of binary images --- kicker-board-v3/.cargo/config.toml | 2 +- kicker-board-v3/src/bin/hwtest-blinky/main.rs | 17 +++---- .../src/bin/hwtest-breakbeam/main.rs | 27 +++------- kicker-board-v3/src/lib.rs | 6 ++- kicker-board-v3/src/tasks/mod.rs | 49 +++++++++++++++++++ 5 files changed, 69 insertions(+), 32 deletions(-) create mode 100644 kicker-board-v3/src/tasks/mod.rs diff --git a/kicker-board-v3/.cargo/config.toml b/kicker-board-v3/.cargo/config.toml index bc8dfc3e..e2d18980 100644 --- a/kicker-board-v3/.cargo/config.toml +++ b/kicker-board-v3/.cargo/config.toml @@ -2,7 +2,7 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] -runner = 'probe-run --chip STM32F407VETx --connect-under-reset' +runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' [env] DEFMT_LOG = "trace" diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs index 25ca923a..1a5249dd 100644 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -10,14 +10,14 @@ use cortex_m_rt::entry; use embassy_executor::Spawner; use embassy_executor::Executor; use embassy_stm32::{ - adc::SampleTime, gpio::{Level, Output, Speed}, peripherals::ADC1, time::mhz + adc::{Adc, SampleTime}, + gpio::{Level, Output, Speed} }; -use embassy_time::{Delay, Duration, Timer}; -use embassy_stm32::adc::{Adc, Temperature, VrefInt}; +use embassy_time::{Duration, Timer}; use static_cell::StaticCell; -use ateam_kicker_board_v3::*; +use ateam_kicker_board_v3::{tasks::get_system_config, *}; use ateam_kicker_board_v3::pins::*; use panic_probe as _; @@ -82,18 +82,15 @@ static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[embassy_executor::main] async fn main(_spawner: Spawner) -> ! { - let mut stm32_config: embassy_stm32::Config = Default::default(); - // stm32_config.rcc.sys_ck = Some(mhz(168)); - // stm32_config.rcc.hclk = Some(mhz(96)); - - let p = embassy_stm32::init(stm32_config); + let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_config); info!("kicker startup!"); let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); - let mut adc = Adc::new(p.ADC1); + let adc = Adc::new(p.ADC1); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); diff --git a/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs b/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs index a1d5207c..d47d5742 100644 --- a/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs +++ b/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs @@ -2,32 +2,21 @@ #![no_main] #![feature(type_alias_impl_trait)] -use cortex_m::prelude::_embedded_hal_blocking_delay_DelayUs; use defmt::*; + +use ateam_kicker_board_v3::tasks::get_system_config; +use embassy_stm32::gpio::{Level, Output, Speed}; use {defmt_rtt as _, panic_probe as _}; use embassy_executor::Spawner; -use embassy_stm32::{ - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, - time::mhz, - usart::{self, Uart}, -}; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_kicker_board::{ - drivers::{breakbeam::Breakbeam} -}; -use ateam_kicker_board::pins::{BlueStatusLedPin, GreenStatusLedPin, BreakbeamTxPin, BreakbeamRxPin}; + +use embassy_time::{Duration, Timer}; +use ateam_kicker_board_v3::drivers::breakbeam::Breakbeam; #[embassy_executor::main] async fn main(_spawner: Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); - - let p = embassy_stm32::init(stm32_config); + let sys_cfg = get_system_config(ateam_kicker_board_v3::tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_cfg); let _charge_pin = Output::new(p.PB3, Level::Low, Speed::Medium); let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); diff --git a/kicker-board-v3/src/lib.rs b/kicker-board-v3/src/lib.rs index 304db521..492dacc2 100644 --- a/kicker-board-v3/src/lib.rs +++ b/kicker-board-v3/src/lib.rs @@ -7,11 +7,13 @@ #![feature(const_maybe_uninit_uninit_array)] #![feature(sync_unsafe_cell)] -pub mod pins; +pub mod drivers; +pub mod tasks; + pub mod kick_manager; +pub mod pins; pub mod queue; pub mod uart_queue; -pub mod drivers; pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV diff --git a/kicker-board-v3/src/tasks/mod.rs b/kicker-board-v3/src/tasks/mod.rs new file mode 100644 index 00000000..159fe6d1 --- /dev/null +++ b/kicker-board-v3/src/tasks/mod.rs @@ -0,0 +1,49 @@ +use embassy_stm32::{rcc::{AHBPrescaler, APBPrescaler, Hse, HseMode, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllSource, Sysclk}, time::Hertz, Config}; + +#[derive(Debug, PartialEq, Eq, Clone, Copy)] +pub enum ClkSource { + InternalOscillator, + External8MHzOscillator, +} + +pub fn get_system_config(clk_src: ClkSource) -> Config { + let mut config = Config::default(); + + let pre_div = if clk_src == ClkSource::External8MHzOscillator { + // configure the external clock mode + config.rcc.hse = Some(Hse { + freq: Hertz(8_000_000), + mode: HseMode::Bypass, + }); + // set the pll source to be the high speed external oscillator + config.rcc.pll_src = PllSource::HSE; + + // 8MHz ext osc means we divide by 4 to get 2MHz root clock + PllPreDiv::DIV4 + } else { + // set the pll source to be the high speed intenal oscillator + config.rcc.pll_src = PllSource::HSI; + + // 16MHz internal osc means we divide by 8 to get a 2MHz root clock + PllPreDiv::DIV8 + }; + + // configure the main PLL + config.rcc.pll = Some(Pll { + prediv: pre_div, // root frequency to PLL will be 2MHz after pre_div regardless of source + mul: PllMul::MUL168, // multiply up by 168 to get 336 MHz + divp: Some(PllPDiv::DIV2), // 336 MHz / 2 = 168 MHZ p which is feeds sysclk + divq: Some(PllQDiv::DIV7), // 336 MHz / 7 = 48Mhz which feeds the 48MHz bus exactly + divr: None, // not using the I2S clock + }); + + // configure the busses + config.rcc.ahb_pre = AHBPrescaler::DIV1; // don't scale the AHB bus, it has a max frequency of 168 MHz, so it can match the sysclk + config.rcc.apb1_pre = APBPrescaler::DIV4; // 168 / 4 = 42 MHz which is the max frequency of APB1 + config.rcc.apb2_pre = APBPrescaler::DIV2; // 168 / 2 = 84 MHz which is the max frequency of APB2 + + // all configs should be good now, switch the system root clock from the raw HSI (16 MHz) to the PLL (168 MHz) + config.rcc.sys = Sysclk::PLL1_P; + + return config; +} \ No newline at end of file From 45184683c013c264dea6109805c8c953879b10d8 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 20 Apr 2024 17:49:04 -0400 Subject: [PATCH 006/157] binary migration work --- kicker-board-v3/src/bin/hwtest-blinky/main.rs | 2 - kicker-board-v3/src/bin/hwtest-charge/main.rs | 173 +++++------------- 2 files changed, 48 insertions(+), 127 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs index 1a5249dd..3d59bd47 100644 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -5,8 +5,6 @@ use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m_rt::entry; - use embassy_executor::Spawner; use embassy_executor::Executor; use embassy_stm32::{ diff --git a/kicker-board-v3/src/bin/hwtest-charge/main.rs b/kicker-board-v3/src/bin/hwtest-charge/main.rs index 4597d3c8..96307778 100644 --- a/kicker-board-v3/src/bin/hwtest-charge/main.rs +++ b/kicker-board-v3/src/bin/hwtest-charge/main.rs @@ -2,88 +2,36 @@ #![no_main] #![feature(type_alias_impl_trait)] -use core::mem; - use defmt::*; use {defmt_rtt as _, panic_probe as _}; -// use cortex_m::{peripheral::NVIC, delay::Delay}; -use cortex_m::{peripheral::NVIC}; use cortex_m_rt::entry; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, AdcPin, InternalChannel, Temperature}, - pac::Interrupt, - Peripherals, - gpio::{Input, Level, Output, Pull, Speed}, - gpio::low_level::Pin, - interrupt, adc::SampleTime, + adc::Adc, + gpio::{Level, Output, Speed}, + adc::SampleTime, }; -use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; -use embassy_time::{Delay, Duration, Timer, Ticker}; +use embassy_time::{Duration, Timer, Ticker}; use static_cell::StaticCell; -use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; - -// #[embassy_executor::task] -// async fn run_critical_section_task(p: Peripherals) { -// let reg_done = Input::new(p.PB4, Pull::None); -// let reg_fault = Input::new(p.PB5, Pull::None); - -// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); -// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); -// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); - -// status_led_green.set_high(); -// Timer::after(Duration::from_millis(500)).await; -// status_led_green.set_low(); -// Timer::after(Duration::from_millis(500)).await; - -// reg_charge.set_high(); -// Timer::after(Duration::from_millis(100)).await; -// reg_charge.set_low(); - -// loop { -// reg_charge.set_low(); - -// if reg_done.is_low() { -// status_led_green.set_high(); -// } else { -// status_led_green.set_low(); -// } - -// if reg_fault.is_low() { -// status_led_red.set_high(); -// } else { -// status_led_red.set_low(); -// } -// } -// } - -// #[embassy_executor::task] -// async fn read_adc_samples(aps: PubSubChannel::) -> ! { -// let sub = aps.subscriber().unwrap(); -// loop { -// info!(sub.) -// } -// } +use ateam_kicker_board_v3::*; +use ateam_kicker_board_v3::pins::*; #[embassy_executor::task] -async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, - mut hv_pin: HighVoltageReadPin, - mut batt_pin: BatteryVoltageReadPin, - mut reg_charge: ChargePin, - mut reg_done: RegulatorDonePin, - mut reg_fault: RegulatorFaultPin, - mut status_led_red: RedStatusLedPin, - mut status_led_green: GreenStatusLedPin) -> ! { +async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + kick_pin: KickPin) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); - let reg_done = Input::new(reg_done, Pull::None); - let reg_fault = Input::new(reg_fault, Pull::None); + let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); @@ -94,98 +42,73 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, status_led_green.set_low(); Timer::after(Duration::from_millis(500)).await; - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; let mut hv = adc.read(&mut hv_pin) as f32; - let mut bv = adc.read(&mut batt_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + let mut regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); - if start_up_battery_voltage < 18.0 { + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); + if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); warn!("refusing to continue"); loop { reg_charge.set_low(); + + kick.set_high(); + Timer::after(Duration::from_micros(500)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; } } reg_charge.set_high(); - Timer::after(Duration::from_millis(400)).await; - let reg_done_stat = reg_done.is_low(); - let reg_fault_stat = reg_fault.is_low(); + Timer::after(Duration::from_millis(50)).await; reg_charge.set_low(); + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + + Timer::after(Duration::from_millis(1000)).await; + loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); reg_charge.set_low(); - - if reg_done_stat { - status_led_green.set_high(); - } else { - status_led_green.set_low(); - } - - if reg_fault_stat { - status_led_red.set_high(); - } else { - status_led_red.set_low(); - } + kick.set_low(); ticker.next().await; } } -// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); -#[derive(Clone, Copy)] -struct AnalogValues { - high_voltage: u16, - batt_voltage: u16, - temp: u16 -} - -// #[interrupt] -// unsafe fn I2C1() { -// EXECUTOR_HIGH.on_interrupt(); -// } - - #[entry] fn main() -> ! { let p = embassy_stm32::init(Default::default()); - info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - // let aps = PubSubChannel::::new(); - - - // highest priorty, energy management - // medium priority, ADC publisher - // low priority uart handler - // lowest prioiryt main update loop - - // High-priority executor: I2C1, priority level 6 - // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - // unwrap!(spawner.spawn(run_critical_section_task(p))); + info!("kicker startup!"); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); - // let mut temp_ch = adc.enable_temperature(&mut Delay); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11))); + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PE5))); }); } \ No newline at end of file From 3bbdb07374d0729f84303403de1a59def9fc45a6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 20 Apr 2024 17:58:47 -0400 Subject: [PATCH 007/157] all binaries compiling on new dep versions --- kicker-board-v3/src/bin/kicker/main.rs | 127 +++++++++++-------------- 1 file changed, 54 insertions(+), 73 deletions(-) diff --git a/kicker-board-v3/src/bin/kicker/main.rs b/kicker-board-v3/src/bin/kicker/main.rs index 768e617b..08e29d84 100644 --- a/kicker-board-v3/src/bin/kicker/main.rs +++ b/kicker-board-v3/src/bin/kicker/main.rs @@ -2,14 +2,15 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] -use core::mem; +use core::cell::SyncUnsafeCell; +use ateam_kicker_board_v3::tasks::get_system_config; use static_cell::StaticCell; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m::peripheral::NVIC; use cortex_m_rt::entry; use libm::{fmaxf, fminf}; @@ -20,20 +21,19 @@ use embassy_stm32::{ bind_interrupts, gpio::{Level, Output, Speed}, interrupt, + interrupt::InterruptExt, pac::Interrupt, peripherals, - time::mhz, - usart, - usart::{Config, Parity, StopBits, Uart}, + usart::{self, Config, Parity, StopBits, Uart}, }; -use embassy_time::{Delay, Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker}; -use ateam_kicker_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, adc_v_to_rail_voltage, +use ateam_kicker_board_v3::{ + adc_raw_to_v, adc_200v_to_rail_voltage, kick_manager::{KickManager, KickType}, pins::{ - BatteryVoltageReadPin, BlueStatusLedPin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, - ComsUartTxDma, HighVoltageReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, + BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, + ComsUartTxDma, PowerRail200vReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, }, queue::Buffer, uart_queue::{UartReadQueue, UartWriteQueue}, drivers::breakbeam::Breakbeam, @@ -58,26 +58,20 @@ pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; // control communications tx buffer -// #[link_section = ".axisram.buffers"] -static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue< - ComsUartModule, - ComsUartTxDma, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX); // control communications rx buffer -// #[link_section = ".axisram.buffers"] -static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue< - ComsUartModule, - ComsUartRxDma, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -100,29 +94,27 @@ fn get_empty_telem_packet() -> KickerTelemetry { #[embassy_executor::task] async fn high_pri_kick_task( coms_reader: &'static UartReadQueue< - 'static, ComsUartModule, ComsUartRxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, >, coms_writer: &'static UartWriteQueue< - 'static, ComsUartModule, ComsUartTxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, >, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, charge_pin: ChargePin, kick_pin: KickPin, chip_pin: ChipPin, breakbeam_tx: BreakbeamTxPin, breakbeam_rx: BreakbeamRxPin, - mut rail_pin: HighVoltageReadPin, - mut battery_voltage_pin: BatteryVoltageReadPin, + mut rail_pin: PowerRail200vReadPin, err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin, + ball_detected_led1_pin: BlueStatusLed1Pin, + ball_detected_led2_pin: BlueStatusLed2Pin, ) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -132,7 +124,9 @@ async fn high_pri_kick_task( // debug LEDs let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); + // TODO dotstars let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); @@ -158,13 +152,14 @@ async fn high_pri_kick_task( breakbeam.enable_tx(); loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint); + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint); - let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); - let battery_voltage = - adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + // let battery_voltage = + // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); // optionally pre-flag errors? + let battery_voltage = 22.5; ///////////////////////////////////// // process any available packets // @@ -376,9 +371,12 @@ async fn high_pri_kick_task( } if ball_detected { - ball_detected_led.set_high(); + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); + } else { - ball_detected_led.set_low(); + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); } // TODO Dotstar @@ -391,7 +389,7 @@ static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[interrupt] -unsafe fn I2C1() { +unsafe fn TIM2() { EXECUTOR_HIGH.on_interrupt(); } @@ -401,41 +399,24 @@ bind_interrupts!(struct Irqs { #[entry] fn main() -> ! { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); - - let p = embassy_stm32::init(stm32_config); + let sys_cfg = get_system_config(ateam_kicker_board_v3::tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_cfg); info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - unwrap!(spawner.spawn(high_pri_kick_task( - &COMS_QUEUE_RX, - &COMS_QUEUE_TX, - adc, - p.PB3, - p.PB0, - p.PB1, - p.PA3, - p.PA2, - p.PA0, - p.PA1, - p.PA12, - p.PA8 - ))); + embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); + let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA3, p.PA2, p.PC0, p.PE1, p.PE2, p.PE3))); + ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -451,12 +432,12 @@ fn main() -> ! { p.PA10, p.PA9, Irqs, - p.DMA1_CH2, - p.DMA1_CH3, + p.DMA2_CH7, + p.DMA2_CH2, coms_uart_config, - ); + ).unwrap(); - let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); // low priority executor handles coms and user IO // Low priority executor: runs in thread mode, using WFE/SEV From 44bb4b45f2d5402d30b88e122a45bf5b5421079a Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 21 Apr 2024 18:03:21 -0400 Subject: [PATCH 008/157] add commented debug cfg --- kicker-board-v3/.cargo/config.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/kicker-board-v3/.cargo/config.toml b/kicker-board-v3/.cargo/config.toml index e2d18980..2c1b4189 100644 --- a/kicker-board-v3/.cargo/config.toml +++ b/kicker-board-v3/.cargo/config.toml @@ -3,6 +3,7 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' +# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' [env] DEFMT_LOG = "trace" From 8bdaebd5133f562b4697648c347226a8136dd0da Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 23 Apr 2024 21:32:02 -0400 Subject: [PATCH 009/157] WIP wip WIP --- kicker-board-v3/src/bin/hwtest-blinky/main.rs | 34 ++++++++++++++++--- kicker-board-v3/src/pins.rs | 7 ++-- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs index 3d59bd47..7f776e5f 100644 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -8,8 +8,7 @@ use {defmt_rtt as _, panic_probe as _}; use embassy_executor::Spawner; use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, SampleTime}, - gpio::{Level, Output, Speed} + adc::{Adc, SampleTime}, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt::InterruptExt }; use embassy_time::{Duration, Timer}; @@ -40,6 +39,8 @@ async fn blink( let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); + // let usr_btn = Input::new(); + // let mut temp = adc.enable_temperature(); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); adc.set_sample_time(SampleTime::CYCLES480); @@ -74,8 +75,32 @@ async fn blink( adc_6v2_to_rail_voltage(adc_raw_to_v(raw_6v2, vrefint_sample)), adc_5v0_to_rail_voltage(adc_raw_to_v(raw_5v0, vrefint_sample)), adc_3v3_to_rail_voltage(adc_raw_to_v(raw_int, vrefint_sample))); + + } } + +#[embassy_executor::task] +async fn shutdown_int(pwr_btn_int_pin: PowerBtnIntPin, + pwr_btn_int_exti: PowerBtnIntExti, + pwr_kill_pin: PowerKillPin) { + + let mut pwr_btn_int = ExtiInput::new(pwr_btn_int_pin, pwr_btn_int_exti, Pull::Down); + let mut pwr_kill = Output::new(pwr_kill_pin, Level::High, Speed::Medium); + + defmt::info!("task waiting for btn int from power front end..."); + + pwr_btn_int.wait_for_rising_edge().await; + + defmt::warn!("received request to power down."); + + // TODO do anything you need to do + + pwr_kill.set_low(); + + loop {} +} + static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[embassy_executor::main] @@ -85,14 +110,15 @@ async fn main(_spawner: Spawner) -> ! { info!("kicker startup!"); - let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); - let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); + // let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); + // let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); let adc = Adc::new(p.ADC1); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { + unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, adc, p.PC0, p.PC1, p.PC3, p.PC2))); }); } \ No newline at end of file diff --git a/kicker-board-v3/src/pins.rs b/kicker-board-v3/src/pins.rs index 7a842254..62aca567 100644 --- a/kicker-board-v3/src/pins.rs +++ b/kicker-board-v3/src/pins.rs @@ -18,9 +18,10 @@ pub type RedStatusLedPin = PE1; pub type BlueStatusLed1Pin = PE2; pub type BlueStatusLed2Pin = PE3; -pub type UserBtn = PD4; -pub type PowerBtnInt = PD5; -pub type PowerKill = PD6; +pub type UserBtnPin = PD4; +pub type PowerBtnIntPin = PD5; +pub type PowerBtnIntExti = EXTI5; +pub type PowerKillPin = PD6; pub type ComsUartModule = USART1; pub type ComsUartTxPin = PA9; From c1611ef4087e73d5cda497b845cc144d72c57000 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 23 Apr 2024 22:15:21 -0400 Subject: [PATCH 010/157] working power btn --- kicker-board-v3/src/bin/hwtest-blinky/main.rs | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs index 7f776e5f..67d6cbbe 100644 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -8,7 +8,9 @@ use {defmt_rtt as _, panic_probe as _}; use embassy_executor::Spawner; use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, SampleTime}, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt::InterruptExt + adc::{Adc, SampleTime}, + exti::ExtiInput, + gpio::{Input, Level, Output, Pull, Speed}, }; use embassy_time::{Duration, Timer}; @@ -27,6 +29,7 @@ async fn blink( status_led_green: GreenStatusLedPin, status_led_blue1: BlueStatusLed1Pin, status_led_blue2: BlueStatusLed2Pin, + usr_btn_pin: UserBtnPin, mut adc: Adc<'static, PowerRailAdc>, mut rail_200v_pin: PowerRail200vReadPin, mut rail_12v0_pin: PowerRail12vReadPin, @@ -39,12 +42,21 @@ async fn blink( let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); - // let usr_btn = Input::new(); + let usr_btn = Input::new(usr_btn_pin, Pull::None); // let mut temp = adc.enable_temperature(); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); adc.set_sample_time(SampleTime::CYCLES480); + 'outer: while usr_btn.is_low() { + while usr_btn.is_high() { + defmt::info!("btn pressed! - initiating kick cycle"); + break 'outer; + } + } + + Timer::after(Duration::from_millis(1000)).await; + loop { reg_charge.set_low(); @@ -94,6 +106,8 @@ async fn shutdown_int(pwr_btn_int_pin: PowerBtnIntPin, defmt::warn!("received request to power down."); + Timer::after(Duration::from_millis(1000)).await; + // TODO do anything you need to do pwr_kill.set_low(); @@ -110,8 +124,8 @@ async fn main(_spawner: Spawner) -> ! { info!("kicker startup!"); - // let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); - // let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); + let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); + let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); let adc = Adc::new(p.ADC1); @@ -119,6 +133,6 @@ async fn main(_spawner: Spawner) -> ! { let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); - unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, adc, p.PC0, p.PC1, p.PC3, p.PC2))); + unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, p.PD4, adc, p.PC0, p.PC1, p.PC3, p.PC2))); }); } \ No newline at end of file From 26afe299f87ea32c7f38fcdc53a3c833b619faa2 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 23 Apr 2024 23:41:40 -0400 Subject: [PATCH 011/157] working charge/kick firmware --- kicker-board-v3/src/bin/hwtest-blinky/main.rs | 4 ++-- kicker-board-v3/src/bin/hwtest-kick/main.rs | 21 ++++++++++++++----- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs index 67d6cbbe..6e6b97aa 100644 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ b/kicker-board-v3/src/bin/hwtest-blinky/main.rs @@ -37,7 +37,7 @@ async fn blink( mut rail_5v0_pin: PowerRail5v0ReadPin) -> ! { let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); - let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::High, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); @@ -50,7 +50,7 @@ async fn blink( 'outer: while usr_btn.is_low() { while usr_btn.is_high() { - defmt::info!("btn pressed! - initiating kick cycle"); + defmt::info!("btn pressed! - cycle"); break 'outer; } } diff --git a/kicker-board-v3/src/bin/hwtest-kick/main.rs b/kicker-board-v3/src/bin/hwtest-kick/main.rs index 0bac7402..f3e9cb8a 100644 --- a/kicker-board-v3/src/bin/hwtest-kick/main.rs +++ b/kicker-board-v3/src/bin/hwtest-kick/main.rs @@ -9,9 +9,8 @@ use cortex_m_rt::entry; use embassy_executor::Executor; use embassy_stm32::{ - adc::Adc, - gpio::{Level, Output, Speed}, - adc::SampleTime, + adc::{Adc, SampleTime}, + gpio::{Input, Level, Output, Pull, Speed}, }; use embassy_time::{Duration, Timer, Ticker}; @@ -27,6 +26,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, reg_charge: ChargePin, status_led_red: RedStatusLedPin, status_led_green: GreenStatusLedPin, + usr_btn_pin: UserBtnPin, kick_pin: KickPin) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); @@ -37,6 +37,8 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + let usr_btn = Input::new(usr_btn_pin, Pull::None); + status_led_green.set_high(); Timer::after(Duration::from_millis(500)).await; status_led_green.set_low(); @@ -65,8 +67,17 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, } } + 'outer: while usr_btn.is_low() { + while usr_btn.is_high() { + defmt::info!("btn pressed! - initiating kick cycle"); + break 'outer; + } + } + + Timer::after(Duration::from_millis(1000)).await; + reg_charge.set_high(); - Timer::after(Duration::from_millis(50)).await; + Timer::after(Duration::from_millis(450)).await; reg_charge.set_low(); let mut vrefint = adc.enable_vrefint(); @@ -122,6 +133,6 @@ fn main() -> ! { // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PE5))); + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PD4, p.PE5))); }); } \ No newline at end of file From 8a482a6149d0747424b341092bec632d5ad6e3f0 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 28 Apr 2024 22:02:35 -0400 Subject: [PATCH 012/157] initial commit for firmware lib --- flake.nix | 4 +- kicker-board-v3/Cargo.toml | 5 - lib-stm32-test/.cargo/config.toml | 9 + lib-stm32-test/.gitignore | 1 + lib-stm32-test/.vscode/settings.json | 4 + lib-stm32-test/Cargo.lock | 936 ++++++++++++++++++ lib-stm32-test/Cargo.toml | 73 ++ lib-stm32-test/rust-toolchain.toml | 2 + .../src/bin/hwtest-uart-queue/main.rs | 199 ++++ lib-stm32-test/src/lib.rs | 6 + lib-stm32/.cargo/config.toml | 9 + lib-stm32/.gitignore | 1 + lib-stm32/.vscode/settings.json | 4 + lib-stm32/Cargo.lock | 719 ++++++++++++++ lib-stm32/Cargo.toml | 21 + lib-stm32/rust-toolchain.toml | 2 + lib-stm32/src/lib.rs | 9 + lib-stm32/src/queue.rs | 266 +++++ lib-stm32/src/uart/mod.rs | 1 + lib-stm32/src/uart/queue.rs | 215 ++++ 20 files changed, 2479 insertions(+), 7 deletions(-) create mode 100644 lib-stm32-test/.cargo/config.toml create mode 100644 lib-stm32-test/.gitignore create mode 100644 lib-stm32-test/.vscode/settings.json create mode 100644 lib-stm32-test/Cargo.lock create mode 100644 lib-stm32-test/Cargo.toml create mode 100644 lib-stm32-test/rust-toolchain.toml create mode 100644 lib-stm32-test/src/bin/hwtest-uart-queue/main.rs create mode 100644 lib-stm32-test/src/lib.rs create mode 100644 lib-stm32/.cargo/config.toml create mode 100644 lib-stm32/.gitignore create mode 100644 lib-stm32/.vscode/settings.json create mode 100644 lib-stm32/Cargo.lock create mode 100644 lib-stm32/Cargo.toml create mode 100644 lib-stm32/rust-toolchain.toml create mode 100644 lib-stm32/src/lib.rs create mode 100644 lib-stm32/src/queue.rs create mode 100644 lib-stm32/src/uart/mod.rs create mode 100644 lib-stm32/src/uart/queue.rs diff --git a/flake.nix b/flake.nix index d7bb25eb..3eaa0adb 100644 --- a/flake.nix +++ b/flake.nix @@ -48,10 +48,10 @@ # Rust Embedded (rust-bin.selectLatestNightlyWith (toolchain: toolchain.default.override { - extensions = [ "rust-src" ]; + extensions = [ "rust-src" "rust-analyzer" ]; targets = [ "thumbv7em-none-eabihf" "thumbv6m-none-eabi" ]; })) - rust-analyzer + # rust-analyzer probe-run probe-rs diff --git a/kicker-board-v3/Cargo.toml b/kicker-board-v3/Cargo.toml index b7796107..06c4f8db 100644 --- a/kicker-board-v3/Cargo.toml +++ b/kicker-board-v3/Cargo.toml @@ -89,11 +89,6 @@ test = false harness = false [patch.crates-io] -# embassy-executor = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-sync = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-time = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-futures = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} embassy-time = { git = "https://github.com/embassy-rs/embassy"} diff --git a/lib-stm32-test/.cargo/config.toml b/lib-stm32-test/.cargo/config.toml new file mode 100644 index 00000000..2c1b4189 --- /dev/null +++ b/lib-stm32-test/.cargo/config.toml @@ -0,0 +1,9 @@ +[build] +target = "thumbv7em-none-eabihf" + +[target.thumbv7em-none-eabihf] +runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' +# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' + +[env] +DEFMT_LOG = "trace" diff --git a/lib-stm32-test/.gitignore b/lib-stm32-test/.gitignore new file mode 100644 index 00000000..c41cc9e3 --- /dev/null +++ b/lib-stm32-test/.gitignore @@ -0,0 +1 @@ +/target \ No newline at end of file diff --git a/lib-stm32-test/.vscode/settings.json b/lib-stm32-test/.vscode/settings.json new file mode 100644 index 00000000..95d7634e --- /dev/null +++ b/lib-stm32-test/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + // tell rust-analyzer which ABI this crate targets + "rust-analyzer.cargo.target": "thumbv7em-none-eabihf" +} \ No newline at end of file diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock new file mode 100644 index 00000000..b11011fc --- /dev/null +++ b/lib-stm32-test/Cargo.lock @@ -0,0 +1,936 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "ateam-lib-stm32" +version = "0.1.0" +dependencies = [ + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "embassy-executor", + "embassy-stm32", +] + +[[package]] +name = "ateam-lib-stm32-test" +version = "0.1.0" +dependencies = [ + "ateam-lib-stm32", + "const_format", + "cortex-m", + "cortex-m-rt", + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "defmt-test", + "embassy-executor", + "embassy-futures", + "embassy-stm32", + "embassy-sync", + "embassy-time", + "futures-util", + "heapless 0.7.17", + "libm", + "panic-probe", + "static_cell", +] + +[[package]] +name = "atomic-polyfill" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" +dependencies = [ + "critical-section 1.1.2", +] + +[[package]] +name = "autocfg" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" + +[[package]] +name = "bare-metal" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" +dependencies = [ + "rustc_version 0.2.3", +] + +[[package]] +name = "bare-metal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" + +[[package]] +name = "bit_field" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" + +[[package]] +name = "bitfield" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46afbd2983a5d5a7bd740ccb198caf5b82f45c40c09c0eed36052d91cb92e719" + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" +dependencies = [ + "num-traits", +] + +[[package]] +name = "const_format" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3a214c7af3d04997541b18d432afaff4c455e79e2029079647e72fc2bd27673" +dependencies = [ + "const_format_proc_macros", +] + +[[package]] +name = "const_format_proc_macros" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7f6ff08fd20f4f299298a28e2dfa8a8ba1036e6cd2460ac1de7b425d76f2500" +dependencies = [ + "proc-macro2", + "quote", + "unicode-xid", +] + +[[package]] +name = "cortex-m" +version = "0.7.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" +dependencies = [ + "bare-metal 0.2.5", + "bitfield", + "critical-section 1.1.2", + "embedded-hal 0.2.7", + "volatile-register", +] + +[[package]] +name = "cortex-m-rt" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +dependencies = [ + "cortex-m-rt-macros", +] + +[[package]] +name = "cortex-m-rt-macros" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "cortex-m-semihosting" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" +dependencies = [ + "cortex-m", +] + +[[package]] +name = "critical-section" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" +dependencies = [ + "bare-metal 1.0.0", + "critical-section 1.1.2", +] + +[[package]] +name = "critical-section" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" + +[[package]] +name = "darling" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.60", +] + +[[package]] +name = "darling_macro" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "defmt" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +dependencies = [ + "defmt-parser", + "proc-macro-error", + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "defmt-parser" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" +dependencies = [ + "thiserror", +] + +[[package]] +name = "defmt-rtt" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1" +dependencies = [ + "critical-section 0.2.8", + "defmt", +] + +[[package]] +name = "defmt-test" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +dependencies = [ + "cortex-m-rt", + "cortex-m-semihosting", + "defmt", + "defmt-test-macros", +] + +[[package]] +name = "defmt-test-macros" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "document-features" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" +dependencies = [ + "litrs", +] + +[[package]] +name = "embassy-embedded-hal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "defmt", + "embassy-futures", + "embassy-sync", + "embassy-time", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-storage", + "embedded-storage-async", + "nb 1.1.0", +] + +[[package]] +name = "embassy-executor" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cortex-m", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-executor-macros", + "embassy-time-driver", + "embassy-time-queue-driver", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.4.1" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "embassy-futures" +version = "0.1.1" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-hal-internal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cortex-m", + "critical-section 1.1.2", + "defmt", + "num-traits", +] + +[[package]] +name = "embassy-net-driver" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "defmt", +] + +[[package]] +name = "embassy-stm32" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "bit_field", + "bitflags 2.5.0", + "cfg-if", + "chrono", + "cortex-m", + "cortex-m-rt", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-embedded-hal", + "embassy-futures", + "embassy-hal-internal", + "embassy-net-driver", + "embassy-sync", + "embassy-time", + "embassy-time-driver", + "embassy-usb-driver", + "embassy-usb-synopsys-otg", + "embedded-can", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "embedded-storage", + "embedded-storage-async", + "futures-util", + "nb 1.1.0", + "proc-macro2", + "quote", + "rand_core", + "sdio-host", + "static_assertions", + "stm32-fmc", + "stm32-metapac", + "vcell", + "volatile-register", +] + +[[package]] +name = "embassy-sync" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "defmt", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "defmt", + "document-features", + "embassy-time-driver", + "embassy-time-queue-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "document-features", +] + +[[package]] +name = "embassy-time-queue-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-usb-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "defmt", +] + +[[package]] +name = "embassy-usb-synopsys-otg" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "critical-section 1.1.2", + "embassy-sync", + "embassy-usb-driver", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" +dependencies = [ + "defmt", +] + +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "defmt", + "embedded-io", +] + +[[package]] +name = "embedded-storage" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" + +[[package]] +name = "embedded-storage-async" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" +dependencies = [ + "embedded-storage", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "futures-core" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" + +[[package]] +name = "futures-task" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" + +[[package]] +name = "futures-util" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "hash32" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" +dependencies = [ + "byteorder", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "heapless" +version = "0.7.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" +dependencies = [ + "atomic-polyfill", + "hash32 0.2.1", + "rustc_version 0.4.0", + "spin", + "stable_deref_trait", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "libm" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" + +[[package]] +name = "lock_api" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" +dependencies = [ + "autocfg", + "scopeguard", +] + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "num-traits" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "panic-probe" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +dependencies = [ + "cortex-m", + "defmt", +] + +[[package]] +name = "pin-project-lite" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "portable-atomic" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" + +[[package]] +name = "proc-macro-error" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +dependencies = [ + "proc-macro-error-attr", + "proc-macro2", + "quote", + "syn 1.0.109", + "version_check", +] + +[[package]] +name = "proc-macro-error-attr" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +dependencies = [ + "proc-macro2", + "quote", + "version_check", +] + +[[package]] +name = "proc-macro2" +version = "1.0.81" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" + +[[package]] +name = "rustc_version" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" +dependencies = [ + "semver 0.9.0", +] + +[[package]] +name = "rustc_version" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" +dependencies = [ + "semver 1.0.22", +] + +[[package]] +name = "scopeguard" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" + +[[package]] +name = "sdio-host" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" + +[[package]] +name = "semver" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" +dependencies = [ + "semver-parser", +] + +[[package]] +name = "semver" +version = "1.0.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" + +[[package]] +name = "semver-parser" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" + +[[package]] +name = "spin" +version = "0.9.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" +dependencies = [ + "lock_api", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + +[[package]] +name = "static_cell" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" +dependencies = [ + "portable-atomic", +] + +[[package]] +name = "stm32-fmc" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" +dependencies = [ + "embedded-hal 0.2.7", +] + +[[package]] +name = "stm32-metapac" +version = "15.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-823168933f3860770111f7bde2a82b912eac58c0#617678145a87435d1b19628b0cc03c1f89cf7bfe" +dependencies = [ + "cortex-m", + "cortex-m-rt", +] + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.60" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "thiserror" +version = "1.0.59" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.59" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "unicode-xid" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" + +[[package]] +name = "vcell" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "volatile-register" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" +dependencies = [ + "vcell", +] diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml new file mode 100644 index 00000000..42842024 --- /dev/null +++ b/lib-stm32-test/Cargo.toml @@ -0,0 +1,73 @@ +[package] +name = "ateam-lib-stm32-test" +version = "0.1.0" +edition = "2021" +license = "MIT" +repository = "https://github.com/SSL-A-Team/firmware" + +[dependencies] +cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } +cortex-m-rt = "0.7.1" +defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "0.3" +embassy-executor = { version = "0.5.0", features = [ + "arch-cortex-m", + "executor-thread", + "executor-interrupt", + "defmt", + "integrated-timers", + "task-arena-size-32768", +] } +embassy-time = { version = "0.3.0", features = [ + "defmt", + "defmt-timestamp-uptime", + "tick-hz-32_768", +] } +embassy-stm32 = { version = "0.1.0", features = [ + "defmt", + "stm32h723zg", + "unstable-pac", + "time-driver-tim1", + "exti", + "chrono" +] } +embassy-futures = { version = "0.1.0" } +futures-util = { version = "0.3.17", default-features = false } +embassy-sync = { version = "0.5.0" } +panic-probe = { version = "0.3", features = ["print-defmt"] } +static_cell = "2.0.0" +critical-section = "1.1.1" +const_format = "0.2.30" +heapless = "0.7.16" +libm = "0.2.6" +# ateam-lib-stm32 = { path = "../lib-stm32", default-featues = false, feature = "stm32h723zg" } +ateam-lib-stm32 = { path = "../lib-stm32" } + + +[dev-dependencies] +defmt-test = "0.3.0" + +[profile.dev] +opt-level = 3 +lto = 'fat' +debug-assertions = false + +[profile.release] +debug = true +lto = 'fat' + +[lib] +test = false +harness = false + +[[bin]] +name = "hwtest-uart-queue" +test = false +harness = false + +[patch.crates-io] +embassy-executor = { git = "https://github.com/embassy-rs/embassy"} +embassy-sync = { git = "https://github.com/embassy-rs/embassy"} +embassy-time = { git = "https://github.com/embassy-rs/embassy"} +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} +embassy-futures = { git = "https://github.com/embassy-rs/embassy"} \ No newline at end of file diff --git a/lib-stm32-test/rust-toolchain.toml b/lib-stm32-test/rust-toolchain.toml new file mode 100644 index 00000000..271800cb --- /dev/null +++ b/lib-stm32-test/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "nightly" \ No newline at end of file diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs new file mode 100644 index 00000000..a7215043 --- /dev/null +++ b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs @@ -0,0 +1,199 @@ +#![no_std] +#![no_main] +#![feature(sync_unsafe_cell)] + +use core::{ + cell::SyncUnsafeCell, + sync::atomic::AtomicU16 +}; + +use embassy_stm32::{ + bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, peripherals::{self, *}, usart::{self, *} +}; +use embassy_executor::Executor; +use embassy_time::Timer; + +use defmt::*; +use defmt_rtt as _; +use panic_probe as _; + +use static_cell::StaticCell; + +use ateam_lib_stm32::{queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; + +type ComsUartModule = USART2; +type ComsUartTxDma = DMA1_CH0; +type ComsUartRxDma = DMA1_CH1; + +type LedGreenPin = PB0; +type LedYellowPin = PE1; +type LedRedPin = PB14; +type UserBtnPin = PC13; +type UserBtnExti = EXTI13; + +const MAX_TX_PACKET_SIZE: usize = 64; +const TX_BUF_DEPTH: usize = 10; +const MAX_RX_PACKET_SIZE: usize = 64; +const RX_BUF_DEPTH: usize = 10; + +// control communications tx buffer +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +#[link_section = ".axisram.buffers"] +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX); + +// control communications rx buffer +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +#[link_section = ".axisram.buffers"] +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX); + +static LOOP_RATE_MS: AtomicU16 = AtomicU16::new(100); + +struct StupidPacket { + fields_of_minimal_intelligence: [usize; 16], +} + +#[embassy_executor::task] +async fn rx_task(coms_reader: &'static UartReadQueue) { + let mut rx_packet: StupidPacket = StupidPacket { + fields_of_minimal_intelligence: [0x55AA55AA; 16] + }; + + loop { + while let Ok(res) = coms_reader.try_dequeue() { + let buf = res.data(); + + if buf.len() != core::mem::size_of::() { + defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + continue; + } + + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe + unsafe { + // copy receieved uart bytes into packet + let state = &mut rx_packet as *mut _ as *mut u8; + for i in 0..core::mem::size_of::() { + *state.offset(i as isize) = buf[i]; + } + } + } + + let millis = LOOP_RATE_MS.load(core::sync::atomic::Ordering::SeqCst); + Timer::after_millis(millis as u64).await; + } +} + +#[embassy_executor::task] +async fn tx_task(coms_writer: &'static UartWriteQueue) { + let tx_packet: StupidPacket = StupidPacket { + fields_of_minimal_intelligence: [0x55AA55AA; 16] + }; + + loop { + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&tx_packet as *const StupidPacket) as *const u8, + core::mem::size_of::(), + ); + + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } + + let millis = LOOP_RATE_MS.load(core::sync::atomic::Ordering::SeqCst); + Timer::after_millis(millis as u64).await; + } +} + +#[embassy_executor::task] +async fn handle_btn_press(usr_btn_pin: UserBtnPin, + usr_btn_exti: UserBtnExti, + led_green_pin: LedGreenPin, + led_yellow_pin: LedYellowPin, + led_red_pin: LedRedPin) { + + let mut usr_btn = ExtiInput::new(usr_btn_pin, usr_btn_exti, Pull::Down); + + let mut green_led = Output::new(led_green_pin, Level::Low, Speed::Medium); + let mut yellow_led = Output::new(led_yellow_pin, Level::Low, Speed::Medium); + let mut red_led = Output::new(led_red_pin, Level::Low, Speed::Medium); + + + loop { + usr_btn.wait_for_rising_edge().await; + + green_led.set_low(); + yellow_led.set_low(); + red_led.set_low(); + + defmt::info!("updating loop rates"); + + let cur_loop_rate = LOOP_RATE_MS.load(core::sync::atomic::Ordering::SeqCst); + let mut new_loop_rate = 100; + if cur_loop_rate == 100 { + new_loop_rate = 10; + + red_led.set_high(); + } else if cur_loop_rate == 10 { + new_loop_rate = 1; + + yellow_led.set_high(); + } else { + green_led.set_high(); + } + LOOP_RATE_MS.store(new_loop_rate, core::sync::atomic::Ordering::SeqCst); + } +} + +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +bind_interrupts!(struct Irqs { + USART2 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: embassy_executor::Spawner) -> !{ + // this actually gets us 64MHz peripheral bus clock + let stm32_config: embassy_stm32::Config = Default::default(); + let p = embassy_stm32::init(stm32_config); + + ////////////////////////////////// + // COMMUNICATIONS TASKS SETUP // + ////////////////////////////////// + + let mut coms_uart_config = Config::default(); + coms_uart_config.baudrate = 2_000_000; // 2 Mbaud + coms_uart_config.parity = Parity::ParityEven; + coms_uart_config.stop_bits = StopBits::STOP1; + + let coms_usart = Uart::new( + p.USART2, + p.PD6, + p.PD5, + Irqs, + p.DMA1_CH1, + p.DMA1_CH2, + coms_uart_config, + ).unwrap(); + + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14))); + unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(rx_task(&COMS_QUEUE_RX))); + unwrap!(spawner.spawn(tx_task(&COMS_QUEUE_TX))); + }); +} \ No newline at end of file diff --git a/lib-stm32-test/src/lib.rs b/lib-stm32-test/src/lib.rs new file mode 100644 index 00000000..5ec76710 --- /dev/null +++ b/lib-stm32-test/src/lib.rs @@ -0,0 +1,6 @@ +#![no_std] +// #![feature(maybe_uninit_uninit_array)] +// #![feature(sync_unsafe_cell)] +// #![feature(type_alias_impl_trait)] +// #![feature(const_maybe_uninit_uninit_array)] +// #![feature(maybe_uninit_slice)] diff --git a/lib-stm32/.cargo/config.toml b/lib-stm32/.cargo/config.toml new file mode 100644 index 00000000..2c1b4189 --- /dev/null +++ b/lib-stm32/.cargo/config.toml @@ -0,0 +1,9 @@ +[build] +target = "thumbv7em-none-eabihf" + +[target.thumbv7em-none-eabihf] +runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' +# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' + +[env] +DEFMT_LOG = "trace" diff --git a/lib-stm32/.gitignore b/lib-stm32/.gitignore new file mode 100644 index 00000000..c41cc9e3 --- /dev/null +++ b/lib-stm32/.gitignore @@ -0,0 +1 @@ +/target \ No newline at end of file diff --git a/lib-stm32/.vscode/settings.json b/lib-stm32/.vscode/settings.json new file mode 100644 index 00000000..95d7634e --- /dev/null +++ b/lib-stm32/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + // tell rust-analyzer which ABI this crate targets + "rust-analyzer.cargo.target": "thumbv7em-none-eabihf" +} \ No newline at end of file diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock new file mode 100644 index 00000000..abd75488 --- /dev/null +++ b/lib-stm32/Cargo.lock @@ -0,0 +1,719 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "ateam-lib-stm32" +version = "0.1.0" +dependencies = [ + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "embassy-executor", + "embassy-stm32", +] + +[[package]] +name = "autocfg" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" + +[[package]] +name = "bare-metal" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" +dependencies = [ + "rustc_version", +] + +[[package]] +name = "bare-metal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" + +[[package]] +name = "bit_field" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" + +[[package]] +name = "bitfield" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46afbd2983a5d5a7bd740ccb198caf5b82f45c40c09c0eed36052d91cb92e719" + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "cortex-m" +version = "0.7.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" +dependencies = [ + "bare-metal 0.2.5", + "bitfield", + "embedded-hal 0.2.7", + "volatile-register", +] + +[[package]] +name = "cortex-m-rt" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +dependencies = [ + "cortex-m-rt-macros", +] + +[[package]] +name = "cortex-m-rt-macros" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "critical-section" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" +dependencies = [ + "bare-metal 1.0.0", + "critical-section 1.1.2", +] + +[[package]] +name = "critical-section" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" + +[[package]] +name = "darling" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.60", +] + +[[package]] +name = "darling_macro" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "defmt" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +dependencies = [ + "defmt-parser", + "proc-macro-error", + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "defmt-parser" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" +dependencies = [ + "thiserror", +] + +[[package]] +name = "defmt-rtt" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1" +dependencies = [ + "critical-section 0.2.8", + "defmt", +] + +[[package]] +name = "document-features" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" +dependencies = [ + "litrs", +] + +[[package]] +name = "embassy-embedded-hal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "embassy-futures", + "embassy-sync", + "embassy-time", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-storage", + "embedded-storage-async", + "nb 1.1.0", +] + +[[package]] +name = "embassy-executor" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "critical-section 1.1.2", + "document-features", + "embassy-executor-macros", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.4.1" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "embassy-futures" +version = "0.1.1" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-hal-internal" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cortex-m", + "critical-section 1.1.2", + "num-traits", +] + +[[package]] +name = "embassy-net-driver" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-stm32" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "bit_field", + "bitflags 2.5.0", + "cfg-if", + "cortex-m", + "cortex-m-rt", + "critical-section 1.1.2", + "document-features", + "embassy-embedded-hal", + "embassy-futures", + "embassy-hal-internal", + "embassy-net-driver", + "embassy-sync", + "embassy-usb-driver", + "embassy-usb-synopsys-otg", + "embedded-can", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "embedded-storage", + "embedded-storage-async", + "futures-util", + "nb 1.1.0", + "proc-macro2", + "quote", + "rand_core", + "sdio-host", + "static_assertions", + "stm32-fmc", + "stm32-metapac", + "vcell", + "volatile-register", +] + +[[package]] +name = "embassy-sync" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-time" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "document-features", + "embassy-time-driver", + "embassy-time-queue-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-time-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "document-features", +] + +[[package]] +name = "embassy-time-queue-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-usb-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" + +[[package]] +name = "embassy-usb-synopsys-otg" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +dependencies = [ + "critical-section 1.1.2", + "embassy-sync", + "embassy-usb-driver", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "embedded-hal" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" +dependencies = [ + "nb 0.1.3", + "void", +] + +[[package]] +name = "embedded-hal" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" + +[[package]] +name = "embedded-hal-async" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "embedded-hal-nb" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" +dependencies = [ + "embedded-hal 1.0.0", + "nb 1.1.0", +] + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "embedded-io", +] + +[[package]] +name = "embedded-storage" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" + +[[package]] +name = "embedded-storage-async" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" +dependencies = [ + "embedded-storage", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "futures-core" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" + +[[package]] +name = "futures-task" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" + +[[package]] +name = "futures-util" +version = "0.3.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" + +[[package]] +name = "nb" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" +dependencies = [ + "nb 1.1.0", +] + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "num-traits" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "pin-project-lite" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "proc-macro-error" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +dependencies = [ + "proc-macro-error-attr", + "proc-macro2", + "quote", + "syn 1.0.109", + "version_check", +] + +[[package]] +name = "proc-macro-error-attr" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +dependencies = [ + "proc-macro2", + "quote", + "version_check", +] + +[[package]] +name = "proc-macro2" +version = "1.0.81" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" + +[[package]] +name = "rustc_version" +version = "0.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" +dependencies = [ + "semver", +] + +[[package]] +name = "sdio-host" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" + +[[package]] +name = "semver" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" +dependencies = [ + "semver-parser", +] + +[[package]] +name = "semver-parser" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + +[[package]] +name = "stm32-fmc" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" +dependencies = [ + "embedded-hal 0.2.7", +] + +[[package]] +name = "stm32-metapac" +version = "15.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-823168933f3860770111f7bde2a82b912eac58c0#617678145a87435d1b19628b0cc03c1f89cf7bfe" +dependencies = [ + "cortex-m", +] + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.60" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "thiserror" +version = "1.0.59" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.59" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.60", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" + +[[package]] +name = "vcell" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "volatile-register" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" +dependencies = [ + "vcell", +] diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml new file mode 100644 index 00000000..30d4b099 --- /dev/null +++ b/lib-stm32/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "ateam-lib-stm32" +version = "0.1.0" +edition = "2021" +license = "MIT" +description = "A common library for ateam stm32 targets" +repository = "https://github.com/SSL-A-Team/firmware" + +[dependencies] +embassy-stm32 = { version = "0.1.0", default-features = false } +embassy-executor = { version = "0.5.0", default-features = false } +defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "0.3" +critical-section = "1.1.1" + +[patch.crates-io] +embassy-executor = { git = "https://github.com/embassy-rs/embassy"} +embassy-sync = { git = "https://github.com/embassy-rs/embassy"} +embassy-time = { git = "https://github.com/embassy-rs/embassy"} +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} +embassy-futures = { git = "https://github.com/embassy-rs/embassy"} \ No newline at end of file diff --git a/lib-stm32/rust-toolchain.toml b/lib-stm32/rust-toolchain.toml new file mode 100644 index 00000000..271800cb --- /dev/null +++ b/lib-stm32/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "nightly" \ No newline at end of file diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs new file mode 100644 index 00000000..359f250b --- /dev/null +++ b/lib-stm32/src/lib.rs @@ -0,0 +1,9 @@ +#![no_std] +#![feature(maybe_uninit_uninit_array)] +#![feature(sync_unsafe_cell)] +#![feature(type_alias_impl_trait)] +#![feature(const_maybe_uninit_uninit_array)] +#![feature(maybe_uninit_slice)] + +pub mod queue; +pub mod uart; diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs new file mode 100644 index 00000000..787c4755 --- /dev/null +++ b/lib-stm32/src/queue.rs @@ -0,0 +1,266 @@ +use core::{ + cell::{SyncUnsafeCell, UnsafeCell}, + future::poll_fn, + mem::MaybeUninit, + sync::atomic::{AtomicBool, AtomicUsize, Ordering}, + task::{Poll, Waker} +}; +use critical_section; + +// #[macro_export] +// macro_rules! create_queue_buffer { +// () => { + +// }; +// } + +pub struct Buffer { + pub data: [MaybeUninit; LENGTH], + // pub len: MaybeUninit, + len: usize, +} + +impl Buffer { + pub const EMPTY: Buffer = Buffer { + data: MaybeUninit::uninit_array(), + // len: MaybeUninit::uninit(), + len: 0 + }; +} + +pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { + queue: &'a Queue, + data: &'a [u8], +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> DequeueRef<'a, LENGTH, DEPTH> { + pub fn data(&self) -> &[u8] { + self.data + } + pub fn cancel(self) { + self.queue.cancel_dequeue(); + } +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH, DEPTH> { + fn drop(&mut self) { + self.queue.finish_dequeue(); + } +} + +pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { + queue: &'a Queue, + data: &'a mut [u8], + len: &'a mut usize, +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> { + pub fn data(&mut self) -> &mut [u8] { + self.data + } + + pub fn len(&mut self) -> &mut usize { + self.len + } + pub fn cancel(self) { + self.queue.cancel_enqueue(); + } +} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for EnqueueRef<'a, LENGTH, DEPTH> { + fn drop(&mut self) { + self.queue.finish_enqueue(); + } +} + +#[derive(PartialEq, Eq, Clone, Copy, Debug, defmt::Format)] +pub enum Error { + QueueFullEmpty, + InProgress, +} + +pub struct Queue { + buffers: &'static [SyncUnsafeCell>; DEPTH], + read_index: AtomicUsize, + read_in_progress: AtomicBool, + write_index: AtomicUsize, + write_in_progress: AtomicBool, + size: AtomicUsize, + enqueue_waker: UnsafeCell>, + dequeue_waker: UnsafeCell>, +} + +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue {} +unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue {} + +impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { + Self { + buffers: buffers, + read_index: AtomicUsize::new(0), + read_in_progress: AtomicBool::new(false), + write_index: AtomicUsize::new(0), + write_in_progress: AtomicBool::new(false), + size: AtomicUsize::new(0), + enqueue_waker: UnsafeCell::new(None), + dequeue_waker: UnsafeCell::new(None), + } + } + + pub fn try_dequeue(&self) -> Result, Error> { + critical_section::with(|_| { + if self.read_in_progress.load(Ordering::SeqCst) { + return Err(Error::InProgress); + } + + if self.size.load(Ordering::SeqCst) > 0 { + self.read_in_progress.store(true, Ordering::SeqCst); + + /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. + * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but + * not both at once. + */ + let buf = unsafe { &*self.buffers[self.read_index.load(Ordering::SeqCst)].get() }; + // let len = unsafe { buf.len.assume_init() } ; + /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file + * (where a user can specify the link section) and so the compiler knows the type and satisfied + * defined behavior constraints w.r.t data alignment and init values, and therefore referencing + * the buffer means the internal data is valid. + */ + let data = unsafe { &MaybeUninit::slice_assume_init_ref(&buf.data)[..buf.len] }; + + Ok(DequeueRef { queue: self, data }) + } else { + Err(Error::QueueFullEmpty) + } + }) + } + + pub async fn dequeue(&self) -> Result, Error> { + // TODO: look at this (when uncommented causes issue cancelling dequeue) + // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { + // return Err(Error::InProgress); + // } + + poll_fn(|cx| { + critical_section::with(|_| { + match self.try_dequeue() { + Err(Error::QueueFullEmpty) => { + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + unsafe { *self.dequeue_waker.get() = Some(cx.waker().clone()) }; + Poll::Pending + } + r => Poll::Ready(r), + } + }) + }) + .await + } + + fn cancel_dequeue(&self) { + self.read_in_progress.store(false, Ordering::SeqCst); + } + + fn finish_dequeue(&self) { + critical_section::with(|_| { + if self.read_in_progress.load(Ordering::SeqCst) { + self.read_in_progress.store(false, Ordering::SeqCst); + + self.read_index.store((self.read_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.size.fetch_sub(1, Ordering::SeqCst); + } + + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + if let Some(w) = unsafe { (*self.enqueue_waker.get()).take() } { + w.wake(); + } + }); + } + + pub fn try_enqueue(&self) -> Result, Error> { + critical_section::with(|_| { + if self.write_in_progress.load(Ordering::SeqCst) { + return Err(Error::InProgress); + } + + if self.size.load(Ordering::SeqCst) < DEPTH { + self.write_in_progress.store(true, Ordering::SeqCst); + /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. + * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but + * not both at once. + */ + let buf = unsafe { &mut *self.buffers[self.write_index.load(Ordering::SeqCst)].get() }; + /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file + * (where a user can specify the link section) and so the compiler knows the type and satisfied + * defined behavior constraints w.r.t data alignment and init values, and therefore referencing + * the buffer means the internal data is valid. + */ + let data = unsafe { MaybeUninit::slice_assume_init_mut(&mut buf.data) }; + + // TODO CHCEK: https://doc.rust-lang.org/std/mem/union.MaybeUninit.html#method.write-1 this should overwrite the value and + // return a mut ref to the new value + // let len = buf.len.write(0); + buf.len = 0; + + Ok(EnqueueRef { + queue: self, + data, + len: &mut buf.len, + }) + } else { + Err(Error::QueueFullEmpty) + } + }) + } + + pub async fn enqueue(&self) -> Result, Error> { + /* Safety: this raw pointer access is safe because the underlying memory is statically allocated + * and the total read operation is atomic across tasks because of the critical_section closure + */ + if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { + return Err(Error::InProgress); + } + + poll_fn(|cx| { + critical_section::with(|_| { + match self.try_enqueue() { + Err(Error::QueueFullEmpty) => { + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + unsafe { *self.enqueue_waker.get() = Some(cx.waker().clone()) }; + Poll::Pending + } + r => Poll::Ready(r), + } + }) + }) + .await + } + + fn cancel_enqueue(&self) { + self.write_in_progress.store(false, Ordering::SeqCst); + } + + fn finish_enqueue(&self) { + critical_section::with(|_| { + if self.write_in_progress.load(Ordering::SeqCst) { + self.write_in_progress.store(false, Ordering::SeqCst); + + self.write_index.store((self.write_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); + self.size.fetch_add(1, Ordering::SeqCst); + } + + /* Safety: this raw pointer write is safe because the underlying memory is statically allocated + * and the total write operation is atomic across tasks because of the critical_section closure + */ + if let Some(w) = unsafe { (*self.dequeue_waker.get()).take() } { + w.wake(); + } + }); + } +} diff --git a/lib-stm32/src/uart/mod.rs b/lib-stm32/src/uart/mod.rs new file mode 100644 index 00000000..837fbf44 --- /dev/null +++ b/lib-stm32/src/uart/mod.rs @@ -0,0 +1 @@ +pub mod queue; \ No newline at end of file diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs new file mode 100644 index 00000000..6103baca --- /dev/null +++ b/lib-stm32/src/uart/queue.rs @@ -0,0 +1,215 @@ +#![warn(async_fn_in_trait)] + +use core::{ + cell::SyncUnsafeCell, + future::Future}; + +use embassy_executor::{ + raw::TaskStorage, + SpawnToken}; +use embassy_stm32::{ + mode::Async, + usart::{self, UartRx, UartTx} +}; + +use defmt::info; + +use crate::queue::{ + self, + Buffer, + DequeueRef, + Error, + Queue}; + +pub struct UartReadQueue< + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, +> { + queue_rx: Queue, + task: TaskStorage>, +} + +// TODO: pretty sure shouldn't do this +unsafe impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, + > Send for UartReadQueue +{ +} + +pub type ReadTaskFuture< + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, +> = impl Future; + +impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::RxDma, + const LENGTH: usize, + const DEPTH: usize, + > UartReadQueue +{ + pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH]) -> Self { + Self { + queue_rx: Queue::new(buffers), + task: TaskStorage::new(), + } + } + + fn read_task( + queue_rx: &'static Queue, + mut rx: UartRx<'static, UART, Async>, + ) -> ReadTaskFuture { + async move { + loop { + let mut buf = queue_rx.enqueue().await.unwrap(); + let len = rx + .read_until_idle(buf.data()) + .await; + // .unwrap(); + // TODO: this + if let Ok(len) = len { + if len == 0 { + info!("uart zero"); + buf.cancel(); + } else { + *buf.len() = len; + } + } else { + info!("{}", len); + buf.cancel(); + } + } + } + } + + pub fn spawn_task( + &'static self, + rx: UartRx<'static, UART, Async>, + ) -> SpawnToken { + self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) + } + + pub fn try_dequeue(&self) -> Result, Error> { + return self.queue_rx.try_dequeue(); + } + + pub async fn dequeue(&self, fn_write: impl FnOnce(&[u8]) -> RET) -> RET { + let buf = self.queue_rx.dequeue().await.unwrap(); + fn_write(buf.data()) + } +} + +pub struct UartWriteQueue< + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, +> { + queue_tx: Queue, + task: TaskStorage>, +} + +type WriteTaskFuture< + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, +> = impl Future; + +impl< + 'a, + UART: usart::BasicInstance, + DMA: usart::TxDma, + const LENGTH: usize, + const DEPTH: usize, + > UartWriteQueue +{ + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { + Self { + queue_tx: Queue::new(buffers), + task: TaskStorage::new(), + } + } + + fn write_task( + queue_tx: &'static Queue, + mut tx: UartTx<'static, UART, Async>, + ) -> WriteTaskFuture { + async move { + loop { + let buf = queue_tx.dequeue().await.unwrap(); + // defmt::info!("invoking API write"); + tx.write(buf.data()).await.unwrap(); // we are blocked here! + // defmt::info!("passed API write"); + + drop(buf); + // unsafe { + // // TODO: what does this do again? + // while !UART::regs().isr().read().tc() {} + // UART::regs().cr1().modify(|w| w.set_te(false)); + // while UART::regs().isr().read().teack() {} + // UART::regs().cr1().modify(|w| w.set_te(true)); + // } + } + } + } + + pub fn spawn_task(&'static self, tx: UartTx<'static, UART, Async>) -> SpawnToken { + self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) + } + + pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { + let mut buf = self.queue_tx.try_enqueue()?; + let len = fn_write(buf.data()); + *buf.len() = len; + Ok(()) + } + + pub fn enqueue_copy(&self, source: &[u8]) -> Result<(), queue::Error> { + self.enqueue(|dest| { + dest[..source.len()].copy_from_slice(source); + source.len() + }) + } +} + +pub trait Reader { + async fn read RET>(&self, fn_read: FN) -> Result; +} + +pub trait Writer { + async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; +} + +impl< + UART: usart::BasicInstance, + Dma: usart::RxDma, + const LEN: usize, + const DEPTH: usize, + > Reader for UartReadQueue +{ + async fn read RET>(&self, fn_read: FN) -> Result { + Ok(self.dequeue(|buf| fn_read(buf)).await) + } +} + +impl< + UART: usart::BasicInstance, + Dma: usart::TxDma, + const LEN: usize, + const DEPTH: usize, + > Writer for UartWriteQueue +{ + async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { + self.enqueue(|buf| fn_write(buf)).or(Err(())) + } +} From 5eff85144d03f89836bf976440ccefd75a81fb8f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 30 Apr 2024 15:06:52 -0400 Subject: [PATCH 013/157] more lib work --- kicker-board-v3/Cargo.lock | 12 ++++ kicker-board-v3/Cargo.toml | 2 + kicker-board-v3/src/bin/hwtest-flash/main.rs | 37 ++++++++++++ lib-stm32/src/drivers/flash/at25df041b.rs | 63 ++++++++++++++++++++ lib-stm32/src/drivers/flash/mod.rs | 2 + lib-stm32/src/drivers/mod.rs | 2 + lib-stm32/src/lib.rs | 5 +- 7 files changed, 122 insertions(+), 1 deletion(-) create mode 100644 kicker-board-v3/src/bin/hwtest-flash/main.rs create mode 100644 lib-stm32/src/drivers/flash/at25df041b.rs create mode 100644 lib-stm32/src/drivers/flash/mod.rs create mode 100644 lib-stm32/src/drivers/mod.rs diff --git a/kicker-board-v3/Cargo.lock b/kicker-board-v3/Cargo.lock index d4c89d32..2fd5a450 100644 --- a/kicker-board-v3/Cargo.lock +++ b/kicker-board-v3/Cargo.lock @@ -24,6 +24,7 @@ name = "ateam-kicker-board-v3" version = "0.1.0" dependencies = [ "ateam-common-packets", + "ateam-lib-stm32", "const_format", "cortex-m", "cortex-m-rt", @@ -43,6 +44,17 @@ dependencies = [ "static_cell", ] +[[package]] +name = "ateam-lib-stm32" +version = "0.1.0" +dependencies = [ + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "embassy-executor", + "embassy-stm32", +] + [[package]] name = "atomic-polyfill" version = "1.0.3" diff --git a/kicker-board-v3/Cargo.toml b/kicker-board-v3/Cargo.toml index 06c4f8db..efb073e4 100644 --- a/kicker-board-v3/Cargo.toml +++ b/kicker-board-v3/Cargo.toml @@ -40,6 +40,8 @@ critical-section = "1.1.1" const_format = "0.2.30" heapless = "0.7.16" libm = "0.2.6" + +ateam-lib-stm32 = { path = "../lib-stm32" } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } [dev-dependencies] diff --git a/kicker-board-v3/src/bin/hwtest-flash/main.rs b/kicker-board-v3/src/bin/hwtest-flash/main.rs new file mode 100644 index 00000000..53175a5e --- /dev/null +++ b/kicker-board-v3/src/bin/hwtest-flash/main.rs @@ -0,0 +1,37 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use embassy_executor::Spawner; +use embassy_stm32::{ + gpio::{Input, Level, Output, Pull, Speed}, spi::{Config, Spi}, time::Hertz, +}; +use embassy_time::{Duration, Timer}; + +use ateam_lib_stm32::drivers::flash::at25df041b::AT25DF041B; + +use ateam_kicker_board_v3::{tasks::get_system_config, *}; +use ateam_kicker_board_v3::pins::*; + +use panic_probe as _; +// use panic_halt as _; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_config); + + info!("kicker startup!"); + + let mut spi_config = Config::default(); + spi_config.frequency = Hertz(1_000_000); + + let mut spi = Spi::new(p.SPI1, p.PB3, p.PB5, p.PB4, p.DMA2_CH3, p.DMA2_CH2, spi_config); + + let flash = AT25DF041B::new(spi); + + loop {} +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs new file mode 100644 index 00000000..3929e355 --- /dev/null +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -0,0 +1,63 @@ +use embassy_stm32::{ + gpio::{Level, Output, Pin, Speed}, + mode::Async, + spi::{self, Error} +}; + +pub struct AT25DF041B<'buf, T: spi::Instance, const CS_POL_N: bool> { + spi: spi::Spi<'static, T, Async>, + chip_select: Output<'static>, + tx_buf: &'buf mut [u8; 256], + rx_buf: &'buf mut [u8; 256], +} + +impl<'buf, T: spi::Instance, const CS_POL_N: bool> AT25DF041B<'buf, T, CS_POL_N> { + pub fn new(spi: spi::Spi<'static, T, Async>, + chip_select: impl Pin, + tx_buf: &'buf mut [u8; 256], + rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, T, CS_POL_N> { + AT25DF041B { + spi, + chip_select: Output::new(chip_select, Level::High, Speed::High), + tx_buf, + rx_buf, + } + } + + fn select(&mut self) { + if CS_POL_N { + self.chip_select.set_low(); + } else { + self.chip_select.set_high(); + } + } + + fn deselect(&mut self) { + if CS_POL_N { + self.chip_select.set_high(); + } else { + self.chip_select.set_low(); + } + } + + async fn transfer(&mut self, len: usize) -> Result<(), Error> { + let len = if len > 256 { 256 } else { len }; + + let res = self.spi.transfer(&mut self.rx_buf[0..len], self.tx_buf).await; + return res; + } + + pub async fn verify_chip_id(&mut self) -> Result<(), ()> { + self.select(); + + self.rx_buf[0] = 0x9F; + let res = self.transfer(5).await; // send cmd byte, get 4 back + if res.is_err() { + return Err(()); + } + + + + return Err(()) + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/flash/mod.rs b/lib-stm32/src/drivers/flash/mod.rs new file mode 100644 index 00000000..2b0d9cc0 --- /dev/null +++ b/lib-stm32/src/drivers/flash/mod.rs @@ -0,0 +1,2 @@ + +pub mod at25df041b; \ No newline at end of file diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs new file mode 100644 index 00000000..faa0d38e --- /dev/null +++ b/lib-stm32/src/drivers/mod.rs @@ -0,0 +1,2 @@ + +pub mod flash; \ No newline at end of file diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 359f250b..78ebd182 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -5,5 +5,8 @@ #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] -pub mod queue; +pub mod drivers; pub mod uart; + +pub mod queue; + From 18ae6624de34da213fc8ea657d096cce941bef93 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 30 Apr 2024 15:08:25 -0400 Subject: [PATCH 014/157] uart test work --- lib-stm32-test/src/bin/hwtest-uart-queue/main.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs index a7215043..bf24adef 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs @@ -177,8 +177,8 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let coms_usart = Uart::new( p.USART2, - p.PD6, - p.PD5, + p.PD6, // rx + p.PD5, // tx Irqs, p.DMA1_CH1, p.DMA1_CH2, @@ -187,6 +187,7 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + // MIGHT should put queues in mix prio, this could elicit the bug // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { From 720d6173dcefdbef188829fb40e18805f8880052 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 4 May 2024 12:08:59 -0400 Subject: [PATCH 015/157] initial structure of generation 2 firmware --- kicker-board-v3/.cargo/config.toml | 9 - kicker-board-v3/.gitignore | 1 - kicker-board-v3/.vscode/settings.json | 4 - kicker-board-v3/Cargo.lock | 1410 ----------------- kicker-board-v3/Cargo.toml | 98 -- kicker-board-v3/memory.x | 12 - kicker-board-v3/rust-toolchain.toml | 3 - kicker-board-v3/src/bin/hwtest-blinky/main.rs | 138 -- .../src/bin/hwtest-breakbeam/main.rs | 73 - kicker-board-v3/src/bin/hwtest-charge/main.rs | 114 -- kicker-board-v3/src/bin/hwtest-coms/main.rs | 294 ---- kicker-board-v3/src/bin/hwtest-flash/main.rs | 37 - kicker-board-v3/src/bin/hwtest-kick/main.rs | 138 -- kicker-board-v3/src/bin/kicker/main.rs | 449 ------ kicker-board-v3/src/drivers/breakbeam.rs | 32 - kicker-board-v3/src/drivers/breakbeam_pwm.rs | 44 - kicker-board-v3/src/drivers/mod.rs | 2 - kicker-board-v3/src/kick_manager.rs | 166 -- kicker-board-v3/src/lib.rs | 46 - kicker-board-v3/src/pins.rs | 30 - kicker-board-v3/src/queue.rs | 259 --- kicker-board-v3/src/uart_queue.rs | 203 --- kicker-board/.cargo/config.toml | 7 +- kicker-board/Cargo.lock | 690 +++++--- kicker-board/Cargo.toml | 38 +- kicker-board/memory.x | 5 +- kicker-board/rust-toolchain.toml | 2 +- kicker-board/src/bin/hwtest-blinky/main.rs | 146 +- kicker-board/src/bin/hwtest-breakbeam/main.rs | 27 +- kicker-board/src/bin/hwtest-charge/main.rs | 173 +- kicker-board/src/bin/hwtest-coms/main.rs | 142 +- kicker-board/src/bin/hwtest-flash/main.rs | 53 + kicker-board/src/bin/hwtest-kick/main.rs | 179 +-- kicker-board/src/bin/kicker/main.rs | 130 +- kicker-board/src/drivers/breakbeam.rs | 12 +- kicker-board/src/drivers/breakbeam_pwm.rs | 27 +- kicker-board/src/drivers/mod.rs | 1 + kicker-board/src/kick_manager.rs | 13 +- kicker-board/src/lib.rs | 30 +- kicker-board/src/pins.rs | 44 +- kicker-board/src/queue.rs | 220 --- .../src/tasks/mod.rs | 0 kicker-board/src/uart_queue.rs | 206 --- lib-stm32-test/.cargo/config.toml | 2 +- {kicker-board-v3 => lib-stm32-test}/build.rs | 0 lib-stm32-test/memory.x | 45 + .../src/bin/hwtest-uart-queue/main.rs | 34 +- lib-stm32/src/drivers/flash/at25df041b.rs | 19 +- lib-stm32/src/lib.rs | 2 + 49 files changed, 1089 insertions(+), 4720 deletions(-) delete mode 100644 kicker-board-v3/.cargo/config.toml delete mode 100644 kicker-board-v3/.gitignore delete mode 100644 kicker-board-v3/.vscode/settings.json delete mode 100644 kicker-board-v3/Cargo.lock delete mode 100644 kicker-board-v3/Cargo.toml delete mode 100644 kicker-board-v3/memory.x delete mode 100644 kicker-board-v3/rust-toolchain.toml delete mode 100644 kicker-board-v3/src/bin/hwtest-blinky/main.rs delete mode 100644 kicker-board-v3/src/bin/hwtest-breakbeam/main.rs delete mode 100644 kicker-board-v3/src/bin/hwtest-charge/main.rs delete mode 100644 kicker-board-v3/src/bin/hwtest-coms/main.rs delete mode 100644 kicker-board-v3/src/bin/hwtest-flash/main.rs delete mode 100644 kicker-board-v3/src/bin/hwtest-kick/main.rs delete mode 100644 kicker-board-v3/src/bin/kicker/main.rs delete mode 100644 kicker-board-v3/src/drivers/breakbeam.rs delete mode 100644 kicker-board-v3/src/drivers/breakbeam_pwm.rs delete mode 100644 kicker-board-v3/src/drivers/mod.rs delete mode 100644 kicker-board-v3/src/kick_manager.rs delete mode 100644 kicker-board-v3/src/lib.rs delete mode 100644 kicker-board-v3/src/pins.rs delete mode 100644 kicker-board-v3/src/queue.rs delete mode 100644 kicker-board-v3/src/uart_queue.rs create mode 100644 kicker-board/src/bin/hwtest-flash/main.rs delete mode 100644 kicker-board/src/queue.rs rename {kicker-board-v3 => kicker-board}/src/tasks/mod.rs (100%) delete mode 100644 kicker-board/src/uart_queue.rs rename {kicker-board-v3 => lib-stm32-test}/build.rs (100%) create mode 100644 lib-stm32-test/memory.x diff --git a/kicker-board-v3/.cargo/config.toml b/kicker-board-v3/.cargo/config.toml deleted file mode 100644 index 2c1b4189..00000000 --- a/kicker-board-v3/.cargo/config.toml +++ /dev/null @@ -1,9 +0,0 @@ -[build] -target = "thumbv7em-none-eabihf" - -[target.thumbv7em-none-eabihf] -runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' -# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' - -[env] -DEFMT_LOG = "trace" diff --git a/kicker-board-v3/.gitignore b/kicker-board-v3/.gitignore deleted file mode 100644 index 2f7896d1..00000000 --- a/kicker-board-v3/.gitignore +++ /dev/null @@ -1 +0,0 @@ -target/ diff --git a/kicker-board-v3/.vscode/settings.json b/kicker-board-v3/.vscode/settings.json deleted file mode 100644 index e9de4f95..00000000 --- a/kicker-board-v3/.vscode/settings.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - // tell rust-analyzer which ABI this crate targets - "rust-analyzer.cargo.target": "thumbv6m-none-eabi" -} \ No newline at end of file diff --git a/kicker-board-v3/Cargo.lock b/kicker-board-v3/Cargo.lock deleted file mode 100644 index 2fd5a450..00000000 --- a/kicker-board-v3/Cargo.lock +++ /dev/null @@ -1,1410 +0,0 @@ -# This file is automatically @generated by Cargo. -# It is not intended for manual editing. -version = 3 - -[[package]] -name = "aho-corasick" -version = "1.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" -dependencies = [ - "memchr", -] - -[[package]] -name = "ateam-common-packets" -version = "1.0.0" -dependencies = [ - "bindgen", - "which", -] - -[[package]] -name = "ateam-kicker-board-v3" -version = "0.1.0" -dependencies = [ - "ateam-common-packets", - "ateam-lib-stm32", - "const_format", - "cortex-m", - "cortex-m-rt", - "critical-section 1.1.2", - "defmt", - "defmt-rtt", - "defmt-test", - "embassy-executor", - "embassy-futures", - "embassy-stm32", - "embassy-sync", - "embassy-time", - "futures-util", - "heapless 0.7.17", - "libm", - "panic-probe", - "static_cell", -] - -[[package]] -name = "ateam-lib-stm32" -version = "0.1.0" -dependencies = [ - "critical-section 1.1.2", - "defmt", - "defmt-rtt", - "embassy-executor", - "embassy-stm32", -] - -[[package]] -name = "atomic-polyfill" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" -dependencies = [ - "critical-section 1.1.2", -] - -[[package]] -name = "atty" -version = "0.2.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8" -dependencies = [ - "hermit-abi", - "libc", - "winapi", -] - -[[package]] -name = "autocfg" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" - -[[package]] -name = "bare-metal" -version = "0.2.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" -dependencies = [ - "rustc_version 0.2.3", -] - -[[package]] -name = "bare-metal" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" - -[[package]] -name = "bindgen" -version = "0.60.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" -dependencies = [ - "bitflags 1.3.2", - "cexpr", - "clang-sys", - "clap", - "env_logger", - "lazy_static", - "lazycell", - "log", - "peeking_take_while", - "proc-macro2", - "quote", - "regex", - "rustc-hash", - "shlex", - "which", -] - -[[package]] -name = "bit_field" -version = "0.10.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" - -[[package]] -name = "bitfield" -version = "0.13.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46afbd2983a5d5a7bd740ccb198caf5b82f45c40c09c0eed36052d91cb92e719" - -[[package]] -name = "bitflags" -version = "1.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" - -[[package]] -name = "bitflags" -version = "2.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" - -[[package]] -name = "byteorder" -version = "1.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" - -[[package]] -name = "cexpr" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" -dependencies = [ - "nom", -] - -[[package]] -name = "cfg-if" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" - -[[package]] -name = "chrono" -version = "0.4.38" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" -dependencies = [ - "num-traits", -] - -[[package]] -name = "clang-sys" -version = "1.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" -dependencies = [ - "glob", - "libc", - "libloading", -] - -[[package]] -name = "clap" -version = "3.2.25" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" -dependencies = [ - "atty", - "bitflags 1.3.2", - "clap_lex", - "indexmap", - "strsim", - "termcolor", - "textwrap", -] - -[[package]] -name = "clap_lex" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2850f2f5a82cbf437dd5af4d49848fbdfc27c157c3d010345776f952765261c5" -dependencies = [ - "os_str_bytes", -] - -[[package]] -name = "const_format" -version = "0.2.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3a214c7af3d04997541b18d432afaff4c455e79e2029079647e72fc2bd27673" -dependencies = [ - "const_format_proc_macros", -] - -[[package]] -name = "const_format_proc_macros" -version = "0.2.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7f6ff08fd20f4f299298a28e2dfa8a8ba1036e6cd2460ac1de7b425d76f2500" -dependencies = [ - "proc-macro2", - "quote", - "unicode-xid", -] - -[[package]] -name = "cortex-m" -version = "0.7.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" -dependencies = [ - "bare-metal 0.2.5", - "bitfield", - "critical-section 1.1.2", - "embedded-hal 0.2.7", - "volatile-register", -] - -[[package]] -name = "cortex-m-rt" -version = "0.7.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" -dependencies = [ - "cortex-m-rt-macros", -] - -[[package]] -name = "cortex-m-rt-macros" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" -dependencies = [ - "proc-macro2", - "quote", - "syn 1.0.109", -] - -[[package]] -name = "cortex-m-semihosting" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" -dependencies = [ - "cortex-m", -] - -[[package]] -name = "critical-section" -version = "0.2.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" -dependencies = [ - "bare-metal 1.0.0", - "critical-section 1.1.2", -] - -[[package]] -name = "critical-section" -version = "1.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" - -[[package]] -name = "darling" -version = "0.20.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" -dependencies = [ - "darling_core", - "darling_macro", -] - -[[package]] -name = "darling_core" -version = "0.20.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" -dependencies = [ - "fnv", - "ident_case", - "proc-macro2", - "quote", - "strsim", - "syn 2.0.59", -] - -[[package]] -name = "darling_macro" -version = "0.20.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" -dependencies = [ - "darling_core", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "defmt" -version = "0.3.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" -dependencies = [ - "bitflags 1.3.2", - "defmt-macros", -] - -[[package]] -name = "defmt-macros" -version = "0.3.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" -dependencies = [ - "defmt-parser", - "proc-macro-error", - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "defmt-parser" -version = "0.3.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" -dependencies = [ - "thiserror", -] - -[[package]] -name = "defmt-rtt" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1" -dependencies = [ - "critical-section 0.2.8", - "defmt", -] - -[[package]] -name = "defmt-test" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" -dependencies = [ - "cortex-m-rt", - "cortex-m-semihosting", - "defmt", - "defmt-test-macros", -] - -[[package]] -name = "defmt-test-macros" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "document-features" -version = "0.2.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" -dependencies = [ - "litrs", -] - -[[package]] -name = "either" -version = "1.11.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" - -[[package]] -name = "embassy-embedded-hal" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "defmt", - "embassy-futures", - "embassy-sync", - "embassy-time", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-async", - "embedded-storage", - "embedded-storage-async", - "nb 1.1.0", -] - -[[package]] -name = "embassy-executor" -version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "cortex-m", - "critical-section 1.1.2", - "defmt", - "document-features", - "embassy-executor-macros", - "embassy-time-driver", - "embassy-time-queue-driver", -] - -[[package]] -name = "embassy-executor-macros" -version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "darling", - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "embassy-futures" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" - -[[package]] -name = "embassy-hal-internal" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "cortex-m", - "critical-section 1.1.2", - "defmt", - "num-traits", -] - -[[package]] -name = "embassy-net-driver" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "defmt", -] - -[[package]] -name = "embassy-stm32" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "bit_field", - "bitflags 2.5.0", - "cfg-if", - "chrono", - "cortex-m", - "cortex-m-rt", - "critical-section 1.1.2", - "defmt", - "document-features", - "embassy-embedded-hal", - "embassy-futures", - "embassy-hal-internal", - "embassy-net-driver", - "embassy-sync", - "embassy-time", - "embassy-time-driver", - "embassy-usb-driver", - "embedded-can", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-async", - "embedded-hal-nb", - "embedded-io", - "embedded-io-async", - "embedded-storage", - "embedded-storage-async", - "futures", - "nb 1.1.0", - "proc-macro2", - "quote", - "rand_core", - "sdio-host", - "static_assertions", - "stm32-fmc", - "stm32-metapac", - "vcell", - "volatile-register", -] - -[[package]] -name = "embassy-sync" -version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "cfg-if", - "critical-section 1.1.2", - "defmt", - "embedded-io-async", - "futures-util", - "heapless 0.8.0", -] - -[[package]] -name = "embassy-time" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "cfg-if", - "critical-section 1.1.2", - "defmt", - "document-features", - "embassy-time-driver", - "embassy-time-queue-driver", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-async", - "futures-util", - "heapless 0.8.0", -] - -[[package]] -name = "embassy-time-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "document-features", -] - -[[package]] -name = "embassy-time-queue-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" - -[[package]] -name = "embassy-usb-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" -dependencies = [ - "defmt", -] - -[[package]] -name = "embedded-can" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" -dependencies = [ - "nb 1.1.0", -] - -[[package]] -name = "embedded-hal" -version = "0.2.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" -dependencies = [ - "nb 0.1.3", - "void", -] - -[[package]] -name = "embedded-hal" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" - -[[package]] -name = "embedded-hal-async" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" -dependencies = [ - "embedded-hal 1.0.0", -] - -[[package]] -name = "embedded-hal-nb" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" -dependencies = [ - "embedded-hal 1.0.0", - "nb 1.1.0", -] - -[[package]] -name = "embedded-io" -version = "0.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" -dependencies = [ - "defmt", -] - -[[package]] -name = "embedded-io-async" -version = "0.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" -dependencies = [ - "defmt", - "embedded-io", -] - -[[package]] -name = "embedded-storage" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" - -[[package]] -name = "embedded-storage-async" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" -dependencies = [ - "embedded-storage", -] - -[[package]] -name = "env_logger" -version = "0.9.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" -dependencies = [ - "atty", - "humantime", - "log", - "regex", - "termcolor", -] - -[[package]] -name = "errno" -version = "0.3.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" -dependencies = [ - "libc", - "windows-sys", -] - -[[package]] -name = "fnv" -version = "1.0.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" - -[[package]] -name = "futures" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" -dependencies = [ - "futures-channel", - "futures-core", - "futures-io", - "futures-sink", - "futures-task", - "futures-util", -] - -[[package]] -name = "futures-channel" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" -dependencies = [ - "futures-core", - "futures-sink", -] - -[[package]] -name = "futures-core" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" - -[[package]] -name = "futures-io" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" - -[[package]] -name = "futures-macro" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "futures-sink" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" - -[[package]] -name = "futures-task" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" - -[[package]] -name = "futures-util" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" -dependencies = [ - "futures-core", - "futures-macro", - "futures-sink", - "futures-task", - "pin-project-lite", - "pin-utils", -] - -[[package]] -name = "glob" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" - -[[package]] -name = "hash32" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" -dependencies = [ - "byteorder", -] - -[[package]] -name = "hash32" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" -dependencies = [ - "byteorder", -] - -[[package]] -name = "hashbrown" -version = "0.12.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" - -[[package]] -name = "heapless" -version = "0.7.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" -dependencies = [ - "atomic-polyfill", - "hash32 0.2.1", - "rustc_version 0.4.0", - "spin", - "stable_deref_trait", -] - -[[package]] -name = "heapless" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" -dependencies = [ - "hash32 0.3.1", - "stable_deref_trait", -] - -[[package]] -name = "hermit-abi" -version = "0.1.19" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33" -dependencies = [ - "libc", -] - -[[package]] -name = "home" -version = "0.5.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" -dependencies = [ - "windows-sys", -] - -[[package]] -name = "humantime" -version = "2.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" - -[[package]] -name = "ident_case" -version = "1.0.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" - -[[package]] -name = "indexmap" -version = "1.9.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" -dependencies = [ - "autocfg", - "hashbrown", -] - -[[package]] -name = "lazy_static" -version = "1.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" - -[[package]] -name = "lazycell" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" - -[[package]] -name = "libc" -version = "0.2.153" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" - -[[package]] -name = "libloading" -version = "0.8.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" -dependencies = [ - "cfg-if", - "windows-targets", -] - -[[package]] -name = "libm" -version = "0.2.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" - -[[package]] -name = "linux-raw-sys" -version = "0.4.13" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" - -[[package]] -name = "litrs" -version = "0.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" - -[[package]] -name = "lock_api" -version = "0.4.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" -dependencies = [ - "autocfg", - "scopeguard", -] - -[[package]] -name = "log" -version = "0.4.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" - -[[package]] -name = "memchr" -version = "2.7.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" - -[[package]] -name = "minimal-lexical" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" - -[[package]] -name = "nb" -version = "0.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" -dependencies = [ - "nb 1.1.0", -] - -[[package]] -name = "nb" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" - -[[package]] -name = "nom" -version = "7.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" -dependencies = [ - "memchr", - "minimal-lexical", -] - -[[package]] -name = "num-traits" -version = "0.2.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" -dependencies = [ - "autocfg", -] - -[[package]] -name = "once_cell" -version = "1.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" - -[[package]] -name = "os_str_bytes" -version = "6.6.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" - -[[package]] -name = "panic-probe" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" -dependencies = [ - "cortex-m", - "defmt", -] - -[[package]] -name = "peeking_take_while" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" - -[[package]] -name = "pin-project-lite" -version = "0.2.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" - -[[package]] -name = "pin-utils" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" - -[[package]] -name = "portable-atomic" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" - -[[package]] -name = "proc-macro-error" -version = "1.0.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" -dependencies = [ - "proc-macro-error-attr", - "proc-macro2", - "quote", - "syn 1.0.109", - "version_check", -] - -[[package]] -name = "proc-macro-error-attr" -version = "1.0.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" -dependencies = [ - "proc-macro2", - "quote", - "version_check", -] - -[[package]] -name = "proc-macro2" -version = "1.0.80" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a56dea16b0a29e94408b9aa5e2940a4eedbd128a1ba20e8f7ae60fd3d465af0e" -dependencies = [ - "unicode-ident", -] - -[[package]] -name = "quote" -version = "1.0.36" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" -dependencies = [ - "proc-macro2", -] - -[[package]] -name = "rand_core" -version = "0.6.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" - -[[package]] -name = "regex" -version = "1.10.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" -dependencies = [ - "aho-corasick", - "memchr", - "regex-automata", - "regex-syntax", -] - -[[package]] -name = "regex-automata" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" -dependencies = [ - "aho-corasick", - "memchr", - "regex-syntax", -] - -[[package]] -name = "regex-syntax" -version = "0.8.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" - -[[package]] -name = "rustc-hash" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" - -[[package]] -name = "rustc_version" -version = "0.2.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" -dependencies = [ - "semver 0.9.0", -] - -[[package]] -name = "rustc_version" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" -dependencies = [ - "semver 1.0.22", -] - -[[package]] -name = "rustix" -version = "0.38.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "65e04861e65f21776e67888bfbea442b3642beaa0138fdb1dd7a84a52dffdb89" -dependencies = [ - "bitflags 2.5.0", - "errno", - "libc", - "linux-raw-sys", - "windows-sys", -] - -[[package]] -name = "scopeguard" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" - -[[package]] -name = "sdio-host" -version = "0.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" - -[[package]] -name = "semver" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" -dependencies = [ - "semver-parser", -] - -[[package]] -name = "semver" -version = "1.0.22" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" - -[[package]] -name = "semver-parser" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" - -[[package]] -name = "shlex" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" - -[[package]] -name = "spin" -version = "0.9.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" -dependencies = [ - "lock_api", -] - -[[package]] -name = "stable_deref_trait" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" - -[[package]] -name = "static_assertions" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" - -[[package]] -name = "static_cell" -version = "2.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa6ba4cf83bf80d3eb25f098ea5e790a0a1fcb5e357442259b231e412c2d3ca0" -dependencies = [ - "portable-atomic", -] - -[[package]] -name = "stm32-fmc" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" -dependencies = [ - "embedded-hal 0.2.7", -] - -[[package]] -name = "stm32-metapac" -version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-01ac9bfd035961dc75f32dcd6080501538246d5c#a5e8ddc393950d780eb95fba15833d8a68b4e216" -dependencies = [ - "cortex-m", - "cortex-m-rt", -] - -[[package]] -name = "strsim" -version = "0.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" - -[[package]] -name = "syn" -version = "1.0.109" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "syn" -version = "2.0.59" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4a6531ffc7b071655e4ce2e04bd464c4830bb585a61cabb96cf808f05172615a" -dependencies = [ - "proc-macro2", - "quote", - "unicode-ident", -] - -[[package]] -name = "termcolor" -version = "1.4.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" -dependencies = [ - "winapi-util", -] - -[[package]] -name = "textwrap" -version = "0.16.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" - -[[package]] -name = "thiserror" -version = "1.0.58" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" -dependencies = [ - "thiserror-impl", -] - -[[package]] -name = "thiserror-impl" -version = "1.0.58" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "unicode-ident" -version = "1.0.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" - -[[package]] -name = "unicode-xid" -version = "0.2.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f962df74c8c05a667b5ee8bcf162993134c104e96440b663c8daa176dc772d8c" - -[[package]] -name = "vcell" -version = "0.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" - -[[package]] -name = "version_check" -version = "0.9.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" - -[[package]] -name = "void" -version = "1.0.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" - -[[package]] -name = "volatile-register" -version = "0.2.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" -dependencies = [ - "vcell", -] - -[[package]] -name = "which" -version = "4.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" -dependencies = [ - "either", - "home", - "once_cell", - "rustix", -] - -[[package]] -name = "winapi" -version = "0.3.9" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" -dependencies = [ - "winapi-i686-pc-windows-gnu", - "winapi-x86_64-pc-windows-gnu", -] - -[[package]] -name = "winapi-i686-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" - -[[package]] -name = "winapi-util" -version = "0.1.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" -dependencies = [ - "winapi", -] - -[[package]] -name = "winapi-x86_64-pc-windows-gnu" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" - -[[package]] -name = "windows-sys" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" -dependencies = [ - "windows-targets", -] - -[[package]] -name = "windows-targets" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" -dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", -] - -[[package]] -name = "windows_aarch64_gnullvm" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" - -[[package]] -name = "windows_aarch64_msvc" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" - -[[package]] -name = "windows_i686_gnu" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" - -[[package]] -name = "windows_i686_gnullvm" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" - -[[package]] -name = "windows_i686_msvc" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" - -[[package]] -name = "windows_x86_64_gnu" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" - -[[package]] -name = "windows_x86_64_gnullvm" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" - -[[package]] -name = "windows_x86_64_msvc" -version = "0.52.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" diff --git a/kicker-board-v3/Cargo.toml b/kicker-board-v3/Cargo.toml deleted file mode 100644 index efb073e4..00000000 --- a/kicker-board-v3/Cargo.toml +++ /dev/null @@ -1,98 +0,0 @@ -[package] -name = "ateam-kicker-board-v3" -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 = { version = "0.7.6", features = ["critical-section-single-core"] } -cortex-m-rt = "0.7.1" -defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) -defmt-rtt = "0.3" -embassy-executor = { version = "0.5.0", features = [ - "arch-cortex-m", - "executor-thread", - "executor-interrupt", - "defmt", - "integrated-timers", - "task-arena-size-32768", -] } -embassy-time = { version = "0.3.0", features = [ - "defmt", - "defmt-timestamp-uptime", - "tick-hz-32_768", -] } -embassy-stm32 = { version = "0.1.0", features = [ - "defmt", - "stm32f407ve", - "unstable-pac", - "time-driver-tim1", - "exti", - "chrono" -] } -embassy-futures = { version = "0.1.0" } -futures-util = { version = "0.3.17", default-features = false } -embassy-sync = { version = "0.5.0" } -panic-probe = { version = "0.3", features = ["print-defmt"] } -static_cell = "2.0.0" -critical-section = "1.1.1" -const_format = "0.2.30" -heapless = "0.7.16" -libm = "0.2.6" - -ateam-lib-stm32 = { path = "../lib-stm32" } -ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } - -[dev-dependencies] -defmt-test = "0.3.0" - -[profile.dev] -opt-level = 3 -lto = 'fat' -debug-assertions = false - -[profile.release] -debug = true -lto = 'fat' - -[lib] -test = false -harness = false - -[[bin]] -name = "hwtest-blinky" -test = false -harness = false - -[[bin]] -name = "hwtest-breakbeam" -test = false -harness = false - -[[bin]] -name = "hwtest-charge" -test = false -harness = false - -[[bin]] -name = "hwtest-coms" -test = false -harness = false - -[[bin]] -name = "hwtest-kick" -test = false -harness = false - -[[bin]] -name = "kicker" -test = false -harness = false - -[patch.crates-io] -embassy-executor = { git = "https://github.com/embassy-rs/embassy"} -embassy-sync = { git = "https://github.com/embassy-rs/embassy"} -embassy-time = { git = "https://github.com/embassy-rs/embassy"} -embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} -embassy-futures = { git = "https://github.com/embassy-rs/embassy"} \ No newline at end of file diff --git a/kicker-board-v3/memory.x b/kicker-board-v3/memory.x deleted file mode 100644 index 5ad3e630..00000000 --- a/kicker-board-v3/memory.x +++ /dev/null @@ -1,12 +0,0 @@ -MEMORY -{ - /* NOTE K = KiBi = 1024 bytes */ - FLASH : ORIGIN = 0x08000000, LENGTH = 512K - RAM : ORIGIN = 0x20000000, LENGTH = 128K - /* CCMRAM : ORIGIN = 0x10000000, LENGTH = 64K */ -} - -/* This is where the call stack will be allocated. */ -/* The stack is of the full descending type. */ -/* NOTE Do NOT modify `_stack_start` unless you know what you are doing */ -_stack_start = ORIGIN(RAM) + LENGTH(RAM); \ No newline at end of file diff --git a/kicker-board-v3/rust-toolchain.toml b/kicker-board-v3/rust-toolchain.toml deleted file mode 100644 index 42b54e8d..00000000 --- a/kicker-board-v3/rust-toolchain.toml +++ /dev/null @@ -1,3 +0,0 @@ -[toolchain] -channel = "nightly" -targets = [ "thumbv7em-none-eabihf" ] diff --git a/kicker-board-v3/src/bin/hwtest-blinky/main.rs b/kicker-board-v3/src/bin/hwtest-blinky/main.rs deleted file mode 100644 index 6e6b97aa..00000000 --- a/kicker-board-v3/src/bin/hwtest-blinky/main.rs +++ /dev/null @@ -1,138 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use embassy_executor::Spawner; -use embassy_executor::Executor; -use embassy_stm32::{ - adc::{Adc, SampleTime}, - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, -}; -use embassy_time::{Duration, Timer}; - -use static_cell::StaticCell; - -use ateam_kicker_board_v3::{tasks::get_system_config, *}; -use ateam_kicker_board_v3::pins::*; - -use panic_probe as _; -// use panic_halt as _; - -#[embassy_executor::task] -async fn blink( - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - status_led_blue1: BlueStatusLed1Pin, - status_led_blue2: BlueStatusLed2Pin, - usr_btn_pin: UserBtnPin, - mut adc: Adc<'static, PowerRailAdc>, - mut rail_200v_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRail12vReadPin, - mut rail_6v2_pin: PowerRail6v2ReadPin, - mut rail_5v0_pin: PowerRail5v0ReadPin) -> ! { - - let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); - let mut status_led_green = Output::new(status_led_green, Level::High, Speed::Medium); - let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); - let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); - let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); - - let usr_btn = Input::new(usr_btn_pin, Pull::None); - - // let mut temp = adc.enable_temperature(); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - 'outer: while usr_btn.is_low() { - while usr_btn.is_high() { - defmt::info!("btn pressed! - cycle"); - break 'outer; - } - } - - Timer::after(Duration::from_millis(1000)).await; - - loop { - reg_charge.set_low(); - - status_led_green.set_high(); - status_led_blue1.set_high(); - status_led_blue2.set_high(); - status_led_red.set_low(); - Timer::after(Duration::from_millis(500)).await; - - status_led_green.set_low(); - status_led_blue1.set_low(); - status_led_blue2.set_low(); - status_led_red.set_high(); - Timer::after(Duration::from_millis(500)).await; - - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - let raw_200v = adc.read(&mut rail_200v_pin) as f32; - let raw_12v = adc.read(&mut rail_12v0_pin) as f32; - let raw_6v2 = adc.read(&mut rail_6v2_pin) as f32; - let raw_5v0 = adc.read(&mut rail_5v0_pin) as f32; - let raw_int = adc.read(&mut vrefint) as f32; - - defmt::info!("voltages - 200v ({}), 12v0 ({}), 6v2 ({}), 5v0 ({}), 3v3 ({})", - adc_200v_to_rail_voltage(adc_raw_to_v(raw_200v, vrefint_sample)), - adc_12v_to_rail_voltage(adc_raw_to_v(raw_12v, vrefint_sample)), - adc_6v2_to_rail_voltage(adc_raw_to_v(raw_6v2, vrefint_sample)), - adc_5v0_to_rail_voltage(adc_raw_to_v(raw_5v0, vrefint_sample)), - adc_3v3_to_rail_voltage(adc_raw_to_v(raw_int, vrefint_sample))); - - - } -} - -#[embassy_executor::task] -async fn shutdown_int(pwr_btn_int_pin: PowerBtnIntPin, - pwr_btn_int_exti: PowerBtnIntExti, - pwr_kill_pin: PowerKillPin) { - - let mut pwr_btn_int = ExtiInput::new(pwr_btn_int_pin, pwr_btn_int_exti, Pull::Down); - let mut pwr_kill = Output::new(pwr_kill_pin, Level::High, Speed::Medium); - - defmt::info!("task waiting for btn int from power front end..."); - - pwr_btn_int.wait_for_rising_edge().await; - - defmt::warn!("received request to power down."); - - Timer::after(Duration::from_millis(1000)).await; - - // TODO do anything you need to do - - pwr_kill.set_low(); - - loop {} -} - -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[embassy_executor::main] -async fn main(_spawner: Spawner) -> ! { - let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_config); - - info!("kicker startup!"); - - let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); - let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); - - let adc = Adc::new(p.ADC1); - - // Low priority executor: runs in thread mode, using WFE/SEV - let executor = EXECUTOR_LOW.init(Executor::new()); - executor.run(|spawner| { - unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); - unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, p.PD4, adc, p.PC0, p.PC1, p.PC3, p.PC2))); - }); -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs b/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs deleted file mode 100644 index d47d5742..00000000 --- a/kicker-board-v3/src/bin/hwtest-breakbeam/main.rs +++ /dev/null @@ -1,73 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use defmt::*; - -use ateam_kicker_board_v3::tasks::get_system_config; -use embassy_stm32::gpio::{Level, Output, Speed}; -use {defmt_rtt as _, panic_probe as _}; -use embassy_executor::Spawner; - -use embassy_time::{Duration, Timer}; -use ateam_kicker_board_v3::drivers::breakbeam::Breakbeam; - - -#[embassy_executor::main] -async fn main(_spawner: Spawner) { - let sys_cfg = get_system_config(ateam_kicker_board_v3::tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_cfg); - - let _charge_pin = Output::new(p.PB3, Level::Low, Speed::Medium); - let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); - let _chip_pin = Output::new(p.PB1, Level::Low, Speed::Medium); - - info!("breakbeam startup!"); - - let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); - let mut status_led_blue = Output::new(p.PA8, Level::Low, Speed::Medium); - - // Breakbeam - // nets on schematic are inverted to silkscreen, sorry :/ -Will - let mut breakbeam = Breakbeam::new(p.PA3, p.PA2); - - status_led_green.set_high(); - Timer::after(Duration::from_millis(250)).await; - status_led_green.set_low(); - Timer::after(Duration::from_millis(250)).await; - status_led_green.set_high(); - Timer::after(Duration::from_millis(250)).await; - status_led_green.set_low(); - Timer::after(Duration::from_millis(250)).await; - - breakbeam.enable_tx(); - loop - { - // Enable transmitter, wait 100ms, drive blue status LED if receiving, wait 1 sec - // Timer::after(Duration::from_millis(100)).await; - if breakbeam.read() - { - status_led_blue.set_high(); - } - else - { - status_led_blue.set_low(); - } - // Timer::after(Duration::from_millis(1000)).await; - - // Disable transmitter, wait 100ms, drive blue status LED if receiving, wait 1 sec - // breakbeam.disable_tx(); - // if breakbeam.read() - // { - // status_led_blue.set_high(); - // } - // else - // { - // status_led_blue.set_low(); - // } - // Timer::after(Duration::from_millis(1000)).await; - - Timer::after(Duration::from_millis(10)).await; - - } -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-charge/main.rs b/kicker-board-v3/src/bin/hwtest-charge/main.rs deleted file mode 100644 index 96307778..00000000 --- a/kicker-board-v3/src/bin/hwtest-charge/main.rs +++ /dev/null @@ -1,114 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use cortex_m_rt::entry; - -use embassy_executor::Executor; -use embassy_stm32::{ - adc::Adc, - gpio::{Level, Output, Speed}, - adc::SampleTime, -}; -use embassy_time::{Duration, Timer, Ticker}; - -use static_cell::StaticCell; - -use ateam_kicker_board_v3::*; -use ateam_kicker_board_v3::pins::*; - -#[embassy_executor::task] -async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRail12vReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - kick_pin: KickPin) -> ! { - - let mut ticker = Ticker::every(Duration::from_millis(1)); - - let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); - - let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); - let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); - let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); - - status_led_green.set_high(); - Timer::after(Duration::from_millis(500)).await; - status_led_green.set_low(); - Timer::after(Duration::from_millis(500)).await; - - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - let mut hv = adc.read(&mut hv_pin) as f32; - let mut regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); - if start_up_battery_voltage < 11.5 { - status_led_red.set_high(); - warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); - warn!("refusing to continue"); - loop { - reg_charge.set_low(); - - kick.set_high(); - Timer::after(Duration::from_micros(500)).await; - kick.set_low(); - - Timer::after(Duration::from_millis(1000)).await; - } - } - - reg_charge.set_high(); - Timer::after(Duration::from_millis(50)).await; - reg_charge.set_low(); - - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - hv = adc.read(&mut hv_pin) as f32; - regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - Timer::after(Duration::from_millis(1000)).await; - - loop { - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - hv = adc.read(&mut hv_pin) as f32; - regv = adc.read(&mut rail_12v0_pin) as f32; - - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - reg_charge.set_low(); - kick.set_low(); - - ticker.next().await; - } -} - -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[entry] -fn main() -> ! { - let p = embassy_stm32::init(Default::default()); - - info!("kicker startup!"); - - let mut adc = Adc::new(p.ADC1); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - // Low priority executor: runs in thread mode, using WFE/SEV - let executor = EXECUTOR_LOW.init(Executor::new()); - executor.run(|spawner| { - unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PE5))); - }); -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-coms/main.rs b/kicker-board-v3/src/bin/hwtest-coms/main.rs deleted file mode 100644 index 493bea7a..00000000 --- a/kicker-board-v3/src/bin/hwtest-coms/main.rs +++ /dev/null @@ -1,294 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(sync_unsafe_cell)] - -use core::cell::SyncUnsafeCell; -use static_cell::StaticCell; - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use cortex_m_rt::entry; - -use embassy_executor::{Executor, InterruptExecutor}; -use embassy_stm32::{ - adc::{Adc, Resolution, SampleTime}, - gpio::{Level, Output, Speed}, - interrupt, - interrupt::InterruptExt, - pac::Interrupt, - rcc::{AHBPrescaler, APBPrescaler, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllSource, Sysclk}, - usart::{Config, Parity, StopBits, Uart} -}; -use embassy_stm32::{bind_interrupts, peripherals, usart}; - -use embassy_time::{Duration, Instant, Ticker}; - -use ateam_kicker_board_v3::{ - adc_200v_to_rail_voltage, adc_raw_to_v, - kick_manager::{ - KickManager, - KickType}, pins::{ - BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, ComsUartTxDma, KickPin, PowerRail200vReadPin, RedStatusLedPin}, queue::Buffer, uart_queue::{ - UartReadQueue, - UartWriteQueue, - } -}; - -use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; - -const MAX_TX_PACKET_SIZE: usize = 16; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 16; -const RX_BUF_DEPTH: usize = 3; - -// control communications tx buffer -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".bss"] -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".bss"] -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX); - -fn get_empty_control_packet() -> KickerControl { - KickerControl { - _bitfield_align_1: [], - _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), - kick_request: KickRequest::KR_DISABLE, - kick_speed: 0.0, - } -} - -fn get_empty_telem_packet() -> KickerTelemetry { - KickerTelemetry { - _bitfield_align_1: [], - _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), - rail_voltage: 0.0, - battery_voltage: 0.0, - } -} - -#[embassy_executor::task] -async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue, - coms_writer: &'static UartWriteQueue, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - mut rail_pin: PowerRail200vReadPin, - err_led_pin: RedStatusLedPin, - ball_detected_led1_pin: BlueStatusLed1Pin, - ball_detected_led2_pin: BlueStatusLed2Pin) -> ! { - - // pins/safety management - let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); - let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); - let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); - let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); - - // debug LEDs - let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); - let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); - - // TODO dotstars - - // coms buffers - let mut telemetry_enabled: bool; - let mut kicker_control_packet: KickerControl = get_empty_control_packet(); - let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); - - // loop rate control - let mut ticker = Ticker::every(Duration::from_millis(1)); - let mut last_packet_sent_time = Instant::now(); - - loop { - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); - // optionally pre-flag errors? - - ///////////////////////////////////// - // process any available packets // - ///////////////////////////////////// - - while let Ok(res) = coms_reader.try_dequeue() { - let buf = res.data(); - - if buf.len() != core::mem::size_of::() { - defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); - continue; - } - - // reinterpreting/initializing packed ffi structs is nearly entirely unsafe - unsafe { - // copy receieved uart bytes into packet - let state = &mut kicker_control_packet as *mut _ as *mut u8; - for i in 0..core::mem::size_of::() { - *state.offset(i as isize) = buf[i]; - } - } - } - - // TODO: read breakbeam - let ball_detected = false; - - /////////////////////////////////////////////// - // manage repetitive kick commands + state // - /////////////////////////////////////////////// - - // update telemetry requests - telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; - - // no charge/kick in coms test - let res = kick_manager.command(22.5, rail_voltage, false, KickType::None, 0.0).await; - - // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > 20 { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1(0, 0, ball_detected as u32, res.is_err() as u32); - kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = 22.5; - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - - if ball_detected_led1.is_set_high() { - ball_detected_led1.set_low(); - } else { - ball_detected_led1.set_high(); - } - - if ball_detected_led2.is_set_high() { - ball_detected_led2.set_low(); - } else { - ball_detected_led2.set_high(); - } - - last_packet_sent_time = cur_time; - } - } - - // LEDs - if res.is_err() { - err_led.set_high(); - } else { - err_led.set_low(); - } - - if ball_detected { - ball_detected_led1.set_high(); - ball_detected_led2.set_high(); - } else { - ball_detected_led1.set_low(); - ball_detected_led2.set_low(); - - } - // TODO Dotstar - - // loop rate control @1KHz - ticker.next().await; - } - -} - -static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[interrupt] -unsafe fn TIM2() { - EXECUTOR_HIGH.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART1 => usart::InterruptHandler; -}); - -#[entry] -fn main() -> ! { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hsi = true; - stm32_config.rcc.pll_src = PllSource::HSI; // internal 16Mhz source - stm32_config.rcc.pll = Some(Pll { - prediv: PllPreDiv::DIV8, // base 2MHz - mul: PllMul::MUL168, // mul up to 336 MHz - divp: Some(PllPDiv::DIV2), // div down to 168 MHz, AHB (max) - divq: Some(PllQDiv::DIV7), // div down to 48 Mhz for dedicated 48MHz clk (exact) - divr: None, // turn off I2S clk - }); - stm32_config.rcc.ahb_pre = AHBPrescaler::DIV1; // AHB 168 MHz (max) - stm32_config.rcc.apb1_pre = APBPrescaler::DIV4; // APB1 42 MHz (max) - stm32_config.rcc.apb2_pre = APBPrescaler::DIV2; // APB2 84 MHz (max) - stm32_config.rcc.sys = Sysclk::PLL1_P; // enable the system - - let p = embassy_stm32::init(stm32_config); - - info!("kicker startup!"); - - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - - let mut adc = Adc::new(p.ADC1); - adc.set_resolution(Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - // high priority executor handles kicking system - // High-priority executor: I2C1, priority level 6 - // TODO CHECK THIS IS THE HIGHEST PRIORITY - embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); - let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); - - ////////////////////////////////// - // COMMUNICATIONS TASKS SETUP // - ////////////////////////////////// - - let mut coms_uart_config = Config::default(); - coms_uart_config.baudrate = 2_000_000; // 2 Mbaud - coms_uart_config.parity = Parity::ParityEven; - coms_uart_config.stop_bits = StopBits::STOP1; - - let coms_usart = Uart::new( - p.USART1, - p.PA10, - p.PA9, - Irqs, - p.DMA2_CH7, - p.DMA2_CH2, - coms_uart_config, - ).unwrap(); - - let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - - // low priority executor handles coms and user IO - // Low priority executor: runs in thread mode, using WFE/SEV - let lp_executor = EXECUTOR_LOW.init(Executor::new()); - lp_executor.run(|spawner| { - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - }); -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-flash/main.rs b/kicker-board-v3/src/bin/hwtest-flash/main.rs deleted file mode 100644 index 53175a5e..00000000 --- a/kicker-board-v3/src/bin/hwtest-flash/main.rs +++ /dev/null @@ -1,37 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use embassy_executor::Spawner; -use embassy_stm32::{ - gpio::{Input, Level, Output, Pull, Speed}, spi::{Config, Spi}, time::Hertz, -}; -use embassy_time::{Duration, Timer}; - -use ateam_lib_stm32::drivers::flash::at25df041b::AT25DF041B; - -use ateam_kicker_board_v3::{tasks::get_system_config, *}; -use ateam_kicker_board_v3::pins::*; - -use panic_probe as _; -// use panic_halt as _; - -#[embassy_executor::main] -async fn main(_spawner: Spawner) -> ! { - let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_config); - - info!("kicker startup!"); - - let mut spi_config = Config::default(); - spi_config.frequency = Hertz(1_000_000); - - let mut spi = Spi::new(p.SPI1, p.PB3, p.PB5, p.PB4, p.DMA2_CH3, p.DMA2_CH2, spi_config); - - let flash = AT25DF041B::new(spi); - - loop {} -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/hwtest-kick/main.rs b/kicker-board-v3/src/bin/hwtest-kick/main.rs deleted file mode 100644 index f3e9cb8a..00000000 --- a/kicker-board-v3/src/bin/hwtest-kick/main.rs +++ /dev/null @@ -1,138 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use cortex_m_rt::entry; - -use embassy_executor::Executor; -use embassy_stm32::{ - adc::{Adc, SampleTime}, - gpio::{Input, Level, Output, Pull, Speed}, -}; -use embassy_time::{Duration, Timer, Ticker}; - -use static_cell::StaticCell; - -use ateam_kicker_board_v3::*; -use ateam_kicker_board_v3::pins::*; - -#[embassy_executor::task] -async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, - mut hv_pin: PowerRail200vReadPin, - mut rail_12v0_pin: PowerRail12vReadPin, - reg_charge: ChargePin, - status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin, - usr_btn_pin: UserBtnPin, - kick_pin: KickPin) -> ! { - - let mut ticker = Ticker::every(Duration::from_millis(1)); - - let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); - - let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); - let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); - let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); - - let usr_btn = Input::new(usr_btn_pin, Pull::None); - - status_led_green.set_high(); - Timer::after(Duration::from_millis(500)).await; - status_led_green.set_low(); - Timer::after(Duration::from_millis(500)).await; - - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - let mut hv = adc.read(&mut hv_pin) as f32; - let mut regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); - if start_up_battery_voltage < 11.5 { - status_led_red.set_high(); - warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); - warn!("refusing to continue"); - loop { - reg_charge.set_low(); - - kick.set_high(); - Timer::after(Duration::from_micros(500)).await; - kick.set_low(); - - Timer::after(Duration::from_millis(1000)).await; - } - } - - 'outer: while usr_btn.is_low() { - while usr_btn.is_high() { - defmt::info!("btn pressed! - initiating kick cycle"); - break 'outer; - } - } - - Timer::after(Duration::from_millis(1000)).await; - - reg_charge.set_high(); - Timer::after(Duration::from_millis(450)).await; - reg_charge.set_low(); - - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - hv = adc.read(&mut hv_pin) as f32; - regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - Timer::after(Duration::from_millis(1000)).await; - - kick.set_high(); - Timer::after(Duration::from_micros(10000)).await; - kick.set_low(); - - Timer::after(Duration::from_millis(2000)).await; - - kick.set_high(); - Timer::after(Duration::from_micros(500000)).await; - kick.set_low(); - - Timer::after(Duration::from_millis(1000)).await; - - - loop { - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint) as f32; - - hv = adc.read(&mut hv_pin) as f32; - regv = adc.read(&mut rail_12v0_pin) as f32; - - info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - - reg_charge.set_low(); - kick.set_low(); - - ticker.next().await; - } -} - -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[entry] -fn main() -> ! { - let p = embassy_stm32::init(Default::default()); - - info!("kicker startup!"); - - let mut adc = Adc::new(p.ADC1); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - // Low priority executor: runs in thread mode, using WFE/SEV - let executor = EXECUTOR_LOW.init(Executor::new()); - executor.run(|spawner| { - unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PD4, p.PE5))); - }); -} \ No newline at end of file diff --git a/kicker-board-v3/src/bin/kicker/main.rs b/kicker-board-v3/src/bin/kicker/main.rs deleted file mode 100644 index 08e29d84..00000000 --- a/kicker-board-v3/src/bin/kicker/main.rs +++ /dev/null @@ -1,449 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(sync_unsafe_cell)] - -use core::cell::SyncUnsafeCell; -use ateam_kicker_board_v3::tasks::get_system_config; -use static_cell::StaticCell; - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use cortex_m_rt::entry; - -use libm::{fmaxf, fminf}; - -use embassy_executor::{Executor, InterruptExecutor}; -use embassy_stm32::{ - adc::{Adc, SampleTime}, - bind_interrupts, - gpio::{Level, Output, Speed}, - interrupt, - interrupt::InterruptExt, - pac::Interrupt, - peripherals, - usart::{self, Config, Parity, StopBits, Uart}, -}; -use embassy_time::{Duration, Instant, Ticker}; - -use ateam_kicker_board_v3::{ - adc_raw_to_v, adc_200v_to_rail_voltage, - kick_manager::{KickManager, KickType}, - pins::{ - BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, - ComsUartTxDma, PowerRail200vReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, - }, - queue::Buffer, - uart_queue::{UartReadQueue, UartWriteQueue}, drivers::breakbeam::Breakbeam, -}; - -use ateam_common_packets::bindings_kicker::{ - KickRequest::{self, KR_ARM, KR_DISABLE}, - KickerControl, KickerTelemetry, -}; - -const MAX_TX_PACKET_SIZE: usize = 16; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 16; -const RX_BUF_DEPTH: usize = 3; - -const MAX_KICK_SPEED: f32 = 5.5; -const SHUTDOWN_KICK_DUTY: f32 = 0.20; - -pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; -pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; -pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; -pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; - -// control communications tx buffer -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX); - -fn get_empty_control_packet() -> KickerControl { - KickerControl { - _bitfield_align_1: [], - _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), - kick_request: KickRequest::KR_DISABLE, - kick_speed: 0.0, - } -} - -fn get_empty_telem_packet() -> KickerTelemetry { - KickerTelemetry { - _bitfield_align_1: [], - _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), - rail_voltage: 0.0, - battery_voltage: 0.0, - } -} - -#[embassy_executor::task] -async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue< - ComsUartModule, - ComsUartRxDma, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, - >, - coms_writer: &'static UartWriteQueue< - ComsUartModule, - ComsUartTxDma, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, - >, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - breakbeam_tx: BreakbeamTxPin, - breakbeam_rx: BreakbeamRxPin, - mut rail_pin: PowerRail200vReadPin, - err_led_pin: RedStatusLedPin, - ball_detected_led1_pin: BlueStatusLed1Pin, - ball_detected_led2_pin: BlueStatusLed2Pin, -) -> ! { - // pins/safety management - let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); - let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); - let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); - let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); - - // debug LEDs - let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); - let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); - - // TODO dotstars - - let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); - - // coms buffers - let mut telemetry_enabled: bool; // = false; - let mut kicker_control_packet: KickerControl = get_empty_control_packet(); - let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); - - // bookkeeping for latched state - let mut kick_command_cleared: bool = false; - let mut latched_command = KickRequest::KR_DISABLE; - let mut error_latched: bool = false; - let mut charge_hv_rail: bool = false; - - // power down status - let mut shutdown_requested: bool = false; - let mut shutdown_completed: bool = false; - - // loop rate control - let mut ticker = Ticker::every(Duration::from_millis(1)); - let mut last_packet_sent_time = Instant::now(); - - breakbeam.enable_tx(); - loop { - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint); - - let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); - // let battery_voltage = - // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); - // optionally pre-flag errors? - let battery_voltage = 22.5; - - ///////////////////////////////////// - // process any available packets // - ///////////////////////////////////// - - while let Ok(res) = coms_reader.try_dequeue() { - let buf = res.data(); - - if buf.len() != core::mem::size_of::() { - defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); - continue; - } - - // reinterpreting/initializing packed ffi structs is nearly entirely unsafe - unsafe { - // copy receieved uart bytes into packet - let state = &mut kicker_control_packet as *mut _ as *mut u8; - for i in 0..core::mem::size_of::() { - *state.offset(i as isize) = buf[i]; - } - } - } - - // TODO: read breakbeam - let ball_detected = breakbeam.read(); - - /////////////////////////////////////////////// - // manage repetitive kick commands + state // - /////////////////////////////////////////////// - - // update telemetry requests - telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; - - // for now shutdown requests will be latched and a reboot is required to re-power - if kicker_control_packet.request_power_down() != 0 { - shutdown_requested = true; - } - - // check if we've met the criteria for completed shutdown - if shutdown_requested && rail_voltage < CHARGE_SAFE_VOLTAGE { - shutdown_completed = true; - } - - if rail_voltage > CHARGE_OVERVOLT_THRESH_VOLTAGE { - error_latched = true; - } - - // charge if were not in shutdown mode AND the control board has requested any state but "DISABLE" - charge_hv_rail = if shutdown_requested || shutdown_completed { - false - } else { - match kicker_control_packet.kick_request { - KickRequest::KR_ARM => true, - KickRequest::KR_DISABLE => false, - KickRequest::KR_KICK_NOW - | KickRequest::KR_KICK_CAPTURED - | KickRequest::KR_KICK_TOUCH - | KickRequest::KR_CHIP_NOW - | KickRequest::KR_CHIP_CAPTURED - | KickRequest::KR_CHIP_TOUCH => charge_hv_rail, - _ => false, - } - }; - - // scale kick strength from m/s to duty for the critical section - // if shutdown is requested and not complete, set kick discharge kick strength to 5% - let kick_strength = if shutdown_completed { - 0.0 - } else if shutdown_requested { - SHUTDOWN_KICK_DUTY - } else { - fmaxf(0.0, fminf(MAX_KICK_SPEED, kicker_control_packet.kick_speed)) / MAX_KICK_SPEED - }; - - // if control requests only an ARM or DISABLE, clear the active command - // software/joystick is not asserting a kick event, so any future kick event is a unique request - if kicker_control_packet.kick_request == KR_ARM - || kicker_control_packet.kick_request == KR_DISABLE - { - kick_command_cleared = true; - } - - // if the previous command has cleared acknowledgement, then any new request is a unique event and can be latched - if kick_command_cleared { - latched_command = kicker_control_packet.kick_request; - } - - // determine if the latched command is actionable - // if a shutdown is requested and not completed, always request a kick - let kick_command = if shutdown_completed { - KickType::None - } else if shutdown_requested { - KickType::Kick - } else { - match latched_command { - KickRequest::KR_DISABLE => KickType::None, - KickRequest::KR_ARM => KickType::None, - KickRequest::KR_KICK_NOW => KickType::Kick, - KickRequest::KR_KICK_TOUCH => { - if ball_detected { - KickType::Kick - } else { - KickType::None - } - } - KickRequest::KR_KICK_CAPTURED => { - if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { - KickType::Kick - } else { - KickType::None - } - } - KickRequest::KR_CHIP_NOW => KickType::Chip, - KickRequest::KR_CHIP_TOUCH => { - if ball_detected { - KickType::Chip - } else { - KickType::None - } - } - KickRequest::KR_CHIP_CAPTURED => { - if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { - KickType::Chip - } else { - KickType::None - } - } - // possible if packet decoding has corrupted enum val - _ => KickType::None, - } - }; - - // the latched command is actionable, and the critical section will be instructed to carry it out in the next segment - // set clear the command cleared flag, indicating the control board will need to deassert any kick command showing it - // wants a new unique event - if kick_command != KickType::None { - kick_command_cleared = false; - } - - // perform charge/kick actions. this function handles elec/mechanical safety - // if telemetry isn't enabled, the control board doesn't want to talk to us, don't permit any actions - let res = if !telemetry_enabled || error_latched { - kick_manager - .command(battery_voltage, rail_voltage, false, KickType::None, 0.0) - .await - } else { - if kick_command == KickType::Kick || kick_command == KickType::Chip { - kicker_control_packet.kick_request = match charge_hv_rail { - true => KickRequest::KR_ARM, - false => KickRequest::KR_DISABLE, - } - } - kick_manager - .command( - battery_voltage, - rail_voltage, - charge_hv_rail, - kick_command, - kick_strength, - ) - .await - }; - - // this will permanently latch an error if the rail voltages are low - // which we probably don't want on boot up? - // maybe this error should be clearable and the HV rail OV should not be - // if res.is_err() { - // error_latched = true; - // } - - // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() - .as_millis() - > 20 - { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( - shutdown_requested as u32, - shutdown_completed as u32, - ball_detected as u32, - res.is_err() as u32, - ); - kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = battery_voltage; - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - - last_packet_sent_time = cur_time; - } - } - - // LEDs - if error_latched { - err_led.set_high(); - } else { - err_led.set_low(); - } - - if ball_detected { - ball_detected_led1.set_high(); - ball_detected_led2.set_high(); - - } else { - ball_detected_led1.set_low(); - ball_detected_led2.set_low(); - } - // TODO Dotstar - - // loop rate control @1KHz - ticker.next().await; - } -} - -static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[interrupt] -unsafe fn TIM2() { - EXECUTOR_HIGH.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART1 => usart::InterruptHandler; -}); - -#[entry] -fn main() -> ! { - let sys_cfg = get_system_config(ateam_kicker_board_v3::tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_cfg); - - info!("kicker startup!"); - - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - - let mut adc = Adc::new(p.ADC1); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - // high priority executor handles kicking system - // High-priority executor: I2C1, priority level 6 - // TODO CHECK THIS IS THE HIGHEST PRIORITY - embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); - let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA3, p.PA2, p.PC0, p.PE1, p.PE2, p.PE3))); - - - ////////////////////////////////// - // COMMUNICATIONS TASKS SETUP // - ////////////////////////////////// - - let mut coms_uart_config = Config::default(); - coms_uart_config.baudrate = 2_000_000; // 2 Mbaud - coms_uart_config.parity = Parity::ParityEven; - coms_uart_config.stop_bits = StopBits::STOP1; - - let coms_usart = Uart::new( - p.USART1, - p.PA10, - p.PA9, - Irqs, - p.DMA2_CH7, - p.DMA2_CH2, - coms_uart_config, - ).unwrap(); - - let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - - // low priority executor handles coms and user IO - // Low priority executor: runs in thread mode, using WFE/SEV - let lp_executor = EXECUTOR_LOW.init(Executor::new()); - lp_executor.run(|spawner| { - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - }); -} diff --git a/kicker-board-v3/src/drivers/breakbeam.rs b/kicker-board-v3/src/drivers/breakbeam.rs deleted file mode 100644 index a109ace3..00000000 --- a/kicker-board-v3/src/drivers/breakbeam.rs +++ /dev/null @@ -1,32 +0,0 @@ -use embassy_stm32::{gpio::{Input, Level, Output, Pin, Pull, Speed}, Peripheral}; - -pub struct Breakbeam<'a> { - pin_tx: Output<'a>, - pin_rx: Input<'a>, -} - -impl<'a> Breakbeam<'a> { - pub fn new(pin_tx: impl Peripheral

+ 'a, pin_rx: impl Peripheral

+ 'a) -> Self { - let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); - let pin_rx = Input::new(pin_rx, Pull::Down); - Self { - pin_tx, - pin_rx - } - } - - #[inline] - pub fn enable_tx(&mut self) { - self.pin_tx.set_high(); - } - - #[inline] - pub fn disable_tx(&mut self) { - self.pin_tx.set_low(); - } - - #[inline] - pub fn read(&self) -> bool { - self.pin_rx.is_low() - } -} diff --git a/kicker-board-v3/src/drivers/breakbeam_pwm.rs b/kicker-board-v3/src/drivers/breakbeam_pwm.rs deleted file mode 100644 index c6179c2d..00000000 --- a/kicker-board-v3/src/drivers/breakbeam_pwm.rs +++ /dev/null @@ -1,44 +0,0 @@ -use embassy_stm32::gpio::{Input, Output, OutputType, Pin, Pull}; -use embassy_stm32::timer::{ - GeneralInstance4Channel, - Channel, - simple_pwm::{ - PwmPin, - SimplePwm}}; -use embassy_stm32::time::khz; -use embassy_stm32::Peripheral; -use embassy_time::{Duration, Timer}; - -pub struct Breakbeam<'a, Timer: GeneralInstance4Channel> { - pin_tx: SimplePwm<'a, Timer>, - pin_rx: Input<'a>, -} - -impl<'a, Timer: GeneralInstance4Channel> Breakbeam<'a, Timer> { - pub fn new(pin_tx: PwmPin<'a, Timer, Channel>, pin_rx: impl Peripheral

+ 'a) -> Self { - let ch_1 = PwmPin::new_ch1(pin_tx, OutputType::PushPull); - let mut pwm_tx = SimplePwm::new(pin_tx, Level::High, Speed::Low); - - let pin_rx = Input::new(pin_rx, Pull::Down); - - Self { - pin_tx: pwm_tx, - pin_rx - } - } - - #[inline] - pub fn enable_tx(&mut self) { - self.pin_tx.set_high(); - } - - #[inline] - pub fn disable_tx(&mut self) { - self.pin_tx.set_low(); - } - - #[inline] - pub fn read(&self) -> bool { - self.pin_rx.is_high() - } -} diff --git a/kicker-board-v3/src/drivers/mod.rs b/kicker-board-v3/src/drivers/mod.rs deleted file mode 100644 index c5f5cce8..00000000 --- a/kicker-board-v3/src/drivers/mod.rs +++ /dev/null @@ -1,2 +0,0 @@ -// pub mod breakbeam_pwm; -pub mod breakbeam; \ No newline at end of file diff --git a/kicker-board-v3/src/kick_manager.rs b/kicker-board-v3/src/kick_manager.rs deleted file mode 100644 index b5a88894..00000000 --- a/kicker-board-v3/src/kick_manager.rs +++ /dev/null @@ -1,166 +0,0 @@ -/* - * This file is responsible for managing the mechanically and - * electrically critical sections of operation. Foundational - * assumptions and requirements are listed below. - * - * ASSUMPTIONS: - * 1. The init code hands off to the manager in a default intert state - * (no charge commanded, no discharge commanded on any channel). - * 2. There is no unsafe use of the relevant PAC wrappers outside of - * this file. - * 3. The Rust type ownership system prevents "safe" external access of - * the critical hardware interface. - * 4. Charging (active or in soft stop) during any discharge event is - * unsafe behavior. - * 5. Discharging on multiple channels at once is unsafe. - * 6. All async call backs run at the highest priority. - * - * HYPOTHESES: - * 1. The charge pin is not in an active state when kick discharge pin - * is in an active state. - * 2. The charge pin is not in an active state when chip discharge pin - * is in an active state. - */ - -use embassy_stm32::gpio::Output; -use embassy_time::{Duration, Timer}; -use libm::{fmaxf, fminf}; - -const MAX_KICK_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick -const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick - -const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off -const KICK_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it -const CHIP_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it - -const MAX_SAFE_RAIL_VOLTAGE: f32 = 190.0; // rail is rated for 200V, and should stop charging around 180V -const VBATT_OVERVOLTAGE_LOCKOUT: f32 = 27.2; -const VBATT_UNDERVOLTAGE_LOCKOUT: f32 = 17.2; - -#[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord)] -pub enum KickType { - Kick, - Chip, - None -} - -pub struct KickManager<'a> { - // external interface - charge_pin: Output<'a>, - kick_pin: Output<'a>, - chip_pin: Output<'a>, - - // record keeping - error_latched: bool, -} - -impl<'a> KickManager<'a> { - pub fn new( - charge_pin: Output<'a>, - kick_pin: Output<'a>, - chip_pin: Output<'a>, - ) -> KickManager<'a> { - KickManager { - charge_pin, - kick_pin, - chip_pin, - error_latched: false - } - } - - pub async fn command(&mut self, battery_voltage: f32, rail_voltage:f32, charge: bool, kick_type: KickType, kick_strength: f32) -> Result<(), ()> { - // latch an error for invalid battery voltage - if battery_voltage > VBATT_OVERVOLTAGE_LOCKOUT || battery_voltage < VBATT_UNDERVOLTAGE_LOCKOUT { - self.error_latched = true; - } - - // latch and error for unsafe rail voltage - if rail_voltage > MAX_SAFE_RAIL_VOLTAGE { - self.error_latched = true; - } - - // if an error is latched, disable systems - if self.error_latched { - self.charge_pin.set_low(); - self.kick_pin.set_low(); - self.chip_pin.set_low(); - - return Err(()); - } - - // bound kick strength - let kick_strength = fmaxf(fminf(kick_strength, 1.0), 0.0); - - // handle charge, kick, and chip - match kick_type { - KickType::Kick => { - // disable kick and chip - self.kick_pin.set_low(); - self.chip_pin.set_low(); - - // disable charging and wait the cooldown - self.charge_pin.set_low(); - Timer::after(CHARGE_COOLDOWN).await; - - // beign kick, wait time to determine power - self.kick_pin.set_high(); - Timer::after(Duration::from_micros((MAX_KICK_DURATION_US * kick_strength) as u64)).await; - - // end kick - self.kick_pin.set_low(); - Timer::after(Duration::from_micros(10)).await; - - // reenable charging if requested - if charge { - self.charge_pin.set_high(); - } - - // cooldown for mechanical return of the subsystem - Timer::after(KICK_COOLDOWN).await; - }, - KickType::Chip => { - // disable kick and chip - self.kick_pin.set_low(); - self.chip_pin.set_low(); - - // disable charging and wait the cooldown - self.charge_pin.set_low(); - Timer::after(CHARGE_COOLDOWN).await; - - // begin chip, wait time to determine power - self.chip_pin.set_high(); - Timer::after(Duration::from_micros((MAX_CHIP_DURATION_US * kick_strength) as u64)).await; - - // end chip - self.chip_pin.set_low(); - Timer::after(Duration::from_micros(10)).await; - - // reenable charging if requested - if charge { - self.charge_pin.set_high(); - } - - // cooldown for mechanical return of the subsystem - Timer::after(CHIP_COOLDOWN).await; - } - KickType::None => { - // disable kick and chip - self.kick_pin.set_low(); - self.chip_pin.set_low(); - - // enable charging if requested - if charge { - self.charge_pin.set_high(); - } else { - self.charge_pin.set_low(); - } - } - } - - return Ok(()); - } - - pub fn clear_error(&mut self) { - self.error_latched = false; - } -} \ No newline at end of file diff --git a/kicker-board-v3/src/lib.rs b/kicker-board-v3/src/lib.rs deleted file mode 100644 index 492dacc2..00000000 --- a/kicker-board-v3/src/lib.rs +++ /dev/null @@ -1,46 +0,0 @@ -#![no_std] -#![feature(const_mut_refs)] -#![feature(const_fn_floating_point_arithmetic)] -#![feature(type_alias_impl_trait)] -#![feature(maybe_uninit_slice)] -#![feature(maybe_uninit_uninit_array)] -#![feature(const_maybe_uninit_uninit_array)] -#![feature(sync_unsafe_cell)] - -pub mod drivers; -pub mod tasks; - -pub mod kick_manager; -pub mod pins; -pub mod queue; -pub mod uart_queue; - -pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV - -pub const fn adc_raw_to_v(adc_raw: f32, vrefint_sample: f32) -> f32 { - adc_raw * ADC_VREFINT_NOMINAL / vrefint_sample / 1000. -} - -pub const fn adc_v_to_battery_voltage(adc_mv: f32) -> f32 { - (adc_mv / 1.65) * 25.2 -} - -pub const fn adc_200v_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv * 200.0 -} - -pub const fn adc_12v_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv * 12.0 -} - -pub const fn adc_6v2_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv * 6.2 -} - -pub const fn adc_5v0_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv * 5.0 -} - -pub const fn adc_3v3_to_rail_voltage(adc_mv: f32) -> f32 { - adc_mv / 1.25 * 3.3 -} \ No newline at end of file diff --git a/kicker-board-v3/src/pins.rs b/kicker-board-v3/src/pins.rs deleted file mode 100644 index 62aca567..00000000 --- a/kicker-board-v3/src/pins.rs +++ /dev/null @@ -1,30 +0,0 @@ -use embassy_stm32::peripherals::*; - -pub type ChargePin = PE4; -pub type KickPin = PE5; -pub type ChipPin = PE6; - -pub type PowerRailAdc = ADC1; -pub type PowerRail200vReadPin = PC0; -pub type PowerRail12vReadPin = PC1; -pub type PowerRail6v2ReadPin = PC3; -pub type PowerRail5v0ReadPin = PC2; - -pub type BreakbeamTxPin = PA3; -pub type BreakbeamRxPin = PA2; - -pub type GreenStatusLedPin = PE0; -pub type RedStatusLedPin = PE1; -pub type BlueStatusLed1Pin = PE2; -pub type BlueStatusLed2Pin = PE3; - -pub type UserBtnPin = PD4; -pub type PowerBtnIntPin = PD5; -pub type PowerBtnIntExti = EXTI5; -pub type PowerKillPin = PD6; - -pub type ComsUartModule = USART1; -pub type ComsUartTxPin = PA9; -pub type ComsUartRxPin = PA10; -pub type ComsUartTxDma = DMA2_CH7; -pub type ComsUartRxDma = DMA2_CH2; \ No newline at end of file diff --git a/kicker-board-v3/src/queue.rs b/kicker-board-v3/src/queue.rs deleted file mode 100644 index ee8fdb51..00000000 --- a/kicker-board-v3/src/queue.rs +++ /dev/null @@ -1,259 +0,0 @@ -use core::{ - cell::{SyncUnsafeCell, UnsafeCell}, - future::poll_fn, - mem::MaybeUninit, - sync::atomic::{AtomicBool, AtomicUsize, Ordering}, - task::{Poll, Waker} -}; -use critical_section; - -pub struct Buffer { - pub data: [MaybeUninit; LENGTH], - // pub len: MaybeUninit, - len: usize, -} - -impl Buffer { - pub const EMPTY: Buffer = Buffer { - data: MaybeUninit::uninit_array(), - // len: MaybeUninit::uninit(), - len: 0 - }; -} - -pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue, - data: &'a [u8], -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> DequeueRef<'a, LENGTH, DEPTH> { - pub fn data(&self) -> &[u8] { - self.data - } - pub fn cancel(self) { - self.queue.cancel_dequeue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_dequeue(); - } -} - -pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue, - data: &'a mut [u8], - len: &'a mut usize, -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> { - pub fn data(&mut self) -> &mut [u8] { - self.data - } - - pub fn len(&mut self) -> &mut usize { - self.len - } - pub fn cancel(self) { - self.queue.cancel_enqueue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for EnqueueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_enqueue(); - } -} - -#[derive(PartialEq, Eq, Clone, Copy, Debug, defmt::Format)] -pub enum Error { - QueueFullEmpty, - InProgress, -} - -pub struct Queue { - buffers: &'static [SyncUnsafeCell>; DEPTH], - read_index: AtomicUsize, - read_in_progress: AtomicBool, - write_index: AtomicUsize, - write_in_progress: AtomicBool, - size: AtomicUsize, - enqueue_waker: UnsafeCell>, - dequeue_waker: UnsafeCell>, -} - -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue {} -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue {} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { - pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { - Self { - buffers: buffers, - read_index: AtomicUsize::new(0), - read_in_progress: AtomicBool::new(false), - write_index: AtomicUsize::new(0), - write_in_progress: AtomicBool::new(false), - size: AtomicUsize::new(0), - enqueue_waker: UnsafeCell::new(None), - dequeue_waker: UnsafeCell::new(None), - } - } - - pub fn try_dequeue(&self) -> Result, Error> { - critical_section::with(|_| { - if self.read_in_progress.load(Ordering::SeqCst) { - return Err(Error::InProgress); - } - - if self.size.load(Ordering::SeqCst) > 0 { - self.read_in_progress.store(true, Ordering::SeqCst); - - /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. - * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but - * not both at once. - */ - let buf = unsafe { &*self.buffers[self.read_index.load(Ordering::SeqCst)].get() }; - // let len = unsafe { buf.len.assume_init() } ; - /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file - * (where a user can specify the link section) and so the compiler knows the type and satisfied - * defined behavior constraints w.r.t data alignment and init values, and therefore referencing - * the buffer means the internal data is valid. - */ - let data = unsafe { &MaybeUninit::slice_assume_init_ref(&buf.data)[..buf.len] }; - - Ok(DequeueRef { queue: self, data }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn dequeue(&self) -> Result, Error> { - // TODO: look at this (when uncommented causes issue cancelling dequeue) - // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { - // return Err(Error::InProgress); - // } - - poll_fn(|cx| { - critical_section::with(|_| { - match self.try_dequeue() { - Err(Error::QueueFullEmpty) => { - /* Safety: this raw pointer write is safe because the underlying memory is statically allocated - * and the total write operation is atomic across tasks because of the critical_section closure - */ - unsafe { *self.dequeue_waker.get() = Some(cx.waker().clone()) }; - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_dequeue(&self) { - self.read_in_progress.store(false, Ordering::SeqCst); - } - - fn finish_dequeue(&self) { - critical_section::with(|_| { - if self.read_in_progress.load(Ordering::SeqCst) { - self.read_in_progress.store(false, Ordering::SeqCst); - - self.read_index.store((self.read_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); - self.size.fetch_sub(1, Ordering::SeqCst); - } - - /* Safety: this raw pointer write is safe because the underlying memory is statically allocated - * and the total write operation is atomic across tasks because of the critical_section closure - */ - if let Some(w) = unsafe { (*self.enqueue_waker.get()).take() } { - w.wake(); - } - }); - } - - pub fn try_enqueue(&self) -> Result, Error> { - critical_section::with(|_| { - if self.write_in_progress.load(Ordering::SeqCst) { - return Err(Error::InProgress); - } - - if self.size.load(Ordering::SeqCst) < DEPTH { - self.write_in_progress.store(true, Ordering::SeqCst); - /* Safety: the async access to buffer data is guarded by atomic read/write and queue size flags. - * The flagging logic should ensure a buffer can only be referenced be a user or a DMA engine but - * not both at once. - */ - let buf = unsafe { &mut *self.buffers[self.write_index.load(Ordering::SeqCst)].get() }; - /* Saftey: this is safe because &buf.data is const/static allocated legally in the main.rs file - * (where a user can specify the link section) and so the compiler knows the type and satisfied - * defined behavior constraints w.r.t data alignment and init values, and therefore referencing - * the buffer means the internal data is valid. - */ - let data = unsafe { MaybeUninit::slice_assume_init_mut(&mut buf.data) }; - - // TODO CHCEK: https://doc.rust-lang.org/std/mem/union.MaybeUninit.html#method.write-1 this should overwrite the value and - // return a mut ref to the new value - // let len = buf.len.write(0); - buf.len = 0; - - Ok(EnqueueRef { - queue: self, - data, - len: &mut buf.len, - }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn enqueue(&self) -> Result, Error> { - /* Safety: this raw pointer access is safe because the underlying memory is statically allocated - * and the total read operation is atomic across tasks because of the critical_section closure - */ - if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { - return Err(Error::InProgress); - } - - poll_fn(|cx| { - critical_section::with(|_| { - match self.try_enqueue() { - Err(Error::QueueFullEmpty) => { - /* Safety: this raw pointer write is safe because the underlying memory is statically allocated - * and the total write operation is atomic across tasks because of the critical_section closure - */ - unsafe { *self.enqueue_waker.get() = Some(cx.waker().clone()) }; - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_enqueue(&self) { - self.write_in_progress.store(false, Ordering::SeqCst); - } - - fn finish_enqueue(&self) { - critical_section::with(|_| { - if self.write_in_progress.load(Ordering::SeqCst) { - self.write_in_progress.store(false, Ordering::SeqCst); - - self.write_index.store((self.write_index.load(Ordering::SeqCst) + 1) % DEPTH, Ordering::SeqCst); - self.size.fetch_add(1, Ordering::SeqCst); - } - - /* Safety: this raw pointer write is safe because the underlying memory is statically allocated - * and the total write operation is atomic across tasks because of the critical_section closure - */ - if let Some(w) = unsafe { (*self.dequeue_waker.get()).take() } { - w.wake(); - } - }); - } -} diff --git a/kicker-board-v3/src/uart_queue.rs b/kicker-board-v3/src/uart_queue.rs deleted file mode 100644 index d35faa1a..00000000 --- a/kicker-board-v3/src/uart_queue.rs +++ /dev/null @@ -1,203 +0,0 @@ -#![warn(async_fn_in_trait)] - -use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; - -use core::{ - cell::SyncUnsafeCell, - future::Future}; -use defmt::info; -use embassy_executor::{raw::TaskStorage, SpawnToken}; -use embassy_stm32::{mode::Async, usart::{self, UartRx, UartTx}}; - -pub struct UartReadQueue< - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_rx: Queue, - task: TaskStorage>, -} - -// TODO: pretty sure shouldn't do this -unsafe impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > Send for UartReadQueue -{ -} - -pub type ReadTaskFuture< - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartReadQueue -{ - pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH]) -> Self { - Self { - queue_rx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn read_task( - queue_rx: &'static Queue, - mut rx: UartRx<'static, UART, Async>, - ) -> ReadTaskFuture { - async move { - loop { - let mut buf = queue_rx.enqueue().await.unwrap(); - let len = rx - .read_until_idle(buf.data()) - .await; - // .unwrap(); - // TODO: this - if let Ok(len) = len { - if len == 0 { - info!("uart zero"); - buf.cancel(); - } else { - *buf.len() = len; - } - } else { - info!("{}", len); - buf.cancel(); - } - } - } - } - - pub fn spawn_task( - &'static self, - rx: UartRx<'static, UART, Async>, - ) -> SpawnToken { - self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) - } - - pub fn try_dequeue(&self) -> Result, Error> { - return self.queue_rx.try_dequeue(); - } - - pub async fn dequeue(&self, fn_write: impl FnOnce(&[u8]) -> RET) -> RET { - let buf = self.queue_rx.dequeue().await.unwrap(); - fn_write(buf.data()) - } -} - -pub struct UartWriteQueue< - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_tx: Queue, - task: TaskStorage>, -} - -type WriteTaskFuture< - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartWriteQueue -{ - pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { - Self { - queue_tx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn write_task( - queue_tx: &'static Queue, - mut tx: UartTx<'static, UART, Async>, - ) -> WriteTaskFuture { - async move { - loop { - let buf = queue_tx.dequeue().await.unwrap(); - // defmt::info!("invoking API write"); - tx.write(buf.data()).await.unwrap(); // we are blocked here! - // defmt::info!("passed API write"); - - drop(buf); - // unsafe { - // // TODO: what does this do again? - // while !UART::regs().isr().read().tc() {} - // UART::regs().cr1().modify(|w| w.set_te(false)); - // while UART::regs().isr().read().teack() {} - // UART::regs().cr1().modify(|w| w.set_te(true)); - // } - } - } - } - - pub fn spawn_task(&'static self, tx: UartTx<'static, UART, Async>) -> SpawnToken { - self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) - } - - pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { - let mut buf = self.queue_tx.try_enqueue()?; - let len = fn_write(buf.data()); - *buf.len() = len; - Ok(()) - } - - pub fn enqueue_copy(&self, source: &[u8]) -> Result<(), queue::Error> { - self.enqueue(|dest| { - dest[..source.len()].copy_from_slice(source); - source.len() - }) - } -} - -pub trait Reader { - async fn read RET>(&self, fn_read: FN) -> Result; -} - -pub trait Writer { - async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; -} - -impl< - UART: usart::BasicInstance, - Dma: usart::RxDma, - const LEN: usize, - const DEPTH: usize, - > Reader for crate::uart_queue::UartReadQueue -{ - async fn read RET>(&self, fn_read: FN) -> Result { - Ok(self.dequeue(|buf| fn_read(buf)).await) - } -} - -impl< - UART: usart::BasicInstance, - Dma: usart::TxDma, - const LEN: usize, - const DEPTH: usize, - > Writer for crate::uart_queue::UartWriteQueue -{ - async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { - self.enqueue(|buf| fn_write(buf)).or(Err(())) - } -} diff --git a/kicker-board/.cargo/config.toml b/kicker-board/.cargo/config.toml index cbb7ef87..2c1b4189 100644 --- a/kicker-board/.cargo/config.toml +++ b/kicker-board/.cargo/config.toml @@ -1,8 +1,9 @@ [build] -target = "thumbv6m-none-eabi" +target = "thumbv7em-none-eabihf" -[target.thumbv6m-none-eabi] -runner = 'probe-run --chip STM32F030K6Tx --connect-under-reset' +[target.thumbv7em-none-eabihf] +runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' +# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' [env] DEFMT_LOG = "trace" diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 2623add9..39a19b0c 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -4,13 +4,23 @@ version = 3 [[package]] name = "aho-corasick" -version = "1.0.2" +version = "1.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43f6cb1bf222025340178f382c426f13757b2960e89779dfcb319c32542a5a41" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" dependencies = [ "memchr", ] +[[package]] +name = "apa102-spi" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "747bbdb12e3b413febf7f888dbc43d3b59facf55ea21b9d38a54617643638b12" +dependencies = [ + "embedded-hal 1.0.0", + "smart-leds-trait", +] + [[package]] name = "ateam-common-packets" version = "1.0.0" @@ -23,11 +33,13 @@ dependencies = [ name = "ateam-kicker-board" version = "0.1.0" dependencies = [ + "apa102-spi", "ateam-common-packets", + "ateam-lib-stm32", "const_format", "cortex-m", "cortex-m-rt", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", "defmt-rtt", "defmt-test", @@ -37,28 +49,31 @@ dependencies = [ "embassy-sync", "embassy-time", "futures-util", - "heapless", + "heapless 0.7.17", "libm", "panic-probe", + "smart-leds", "static_cell", ] [[package]] -name = "atomic-polyfill" -version = "0.1.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3ff7eb3f316534d83a8a2c3d1674ace8a5a71198eba31e2e2b597833f699b28" +name = "ateam-lib-stm32" +version = "0.1.0" dependencies = [ - "critical-section 1.1.1", + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "embassy-executor", + "embassy-stm32", ] [[package]] name = "atomic-polyfill" -version = "1.0.2" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c314e70d181aa6053b26e3f7fbf86d1dfff84f816a6175b967666b3506ef7289" +checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" dependencies = [ - "critical-section 1.1.1", + "critical-section 1.1.2", ] [[package]] @@ -74,9 +89,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" [[package]] name = "bare-metal" @@ -99,7 +114,7 @@ version = "0.60.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" dependencies = [ - "bitflags", + "bitflags 1.3.2", "cexpr", "clang-sys", "clap", @@ -135,23 +150,22 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] -name = "bxcan" -version = "0.7.0" +name = "bitflags" +version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "40ac3d0c0a542d0ab5521211f873f62706a7136df415676f676d347e5a41dd80" -dependencies = [ - "bitflags", - "defmt", - "embedded-hal 0.2.7", - "nb 1.1.0", - "vcell", -] +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" + +[[package]] +name = "bytemuck" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" [[package]] name = "byteorder" -version = "1.4.3" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cexpr" @@ -168,11 +182,20 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "chrono" +version = "0.4.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" +dependencies = [ + "num-traits", +] + [[package]] name = "clang-sys" -version = "1.6.1" +version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c688fc74432808e3eb684cae8830a86be1d66a2bd58e1f248ed0960a590baf6f" +checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" dependencies = [ "glob", "libc", @@ -186,7 +209,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" dependencies = [ "atty", - "bitflags", + "bitflags 1.3.2", "clap_lex", "indexmap", "strsim", @@ -205,18 +228,18 @@ dependencies = [ [[package]] name = "const_format" -version = "0.2.31" +version = "0.2.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c990efc7a285731f9a4378d81aff2f0e85a2c8781a05ef0f8baa8dac54d0ff48" +checksum = "e3a214c7af3d04997541b18d432afaff4c455e79e2029079647e72fc2bd27673" dependencies = [ "const_format_proc_macros", ] [[package]] name = "const_format_proc_macros" -version = "0.2.31" +version = "0.2.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e026b6ce194a874cb9cf32cd5772d1ef9767cc8fcb5765948d74f37a9d8b2bf6" +checksum = "c7f6ff08fd20f4f299298a28e2dfa8a8ba1036e6cd2460ac1de7b425d76f2500" dependencies = [ "proc-macro2", "quote", @@ -231,16 +254,16 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" dependencies = [ "bare-metal 0.2.5", "bitfield", - "critical-section 1.1.1", + "critical-section 1.1.2", "embedded-hal 0.2.7", "volatile-register", ] [[package]] name = "cortex-m-rt" -version = "0.7.3" +version = "0.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" +checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" dependencies = [ "cortex-m-rt-macros", ] @@ -256,6 +279,15 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "cortex-m-semihosting" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" +dependencies = [ + "cortex-m", +] + [[package]] name = "critical-section" version = "0.2.8" @@ -263,20 +295,20 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" dependencies = [ "bare-metal 1.0.0", - "critical-section 1.1.1", + "critical-section 1.1.2", ] [[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 = "darling" -version = "0.20.1" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0558d22a7b463ed0241e993f76f09f30b126687447751a8638587b864e4b3944" +checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" dependencies = [ "darling_core", "darling_macro", @@ -284,57 +316,57 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.1" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ab8bfa2e259f8ee1ce5e97824a3c55ec4404a0d772ca7fa96bf19f0752a046eb" +checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn 2.0.23", + "syn 2.0.59", ] [[package]] name = "darling_macro" -version = "0.20.1" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "29a358ff9f12ec09c3e61fef9b5a9902623a695a46a917b07f269bff1445611a" +checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" dependencies = [ "darling_core", "quote", - "syn 2.0.23", + "syn 2.0.59", ] [[package]] name = "defmt" -version = "0.3.2" +version = "0.3.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d3a0ae7494d9bff013d7b89471f4c424356a71e9752e0c78abe7e6c608a16bb3" +checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" dependencies = [ - "bitflags", + "bitflags 1.3.2", "defmt-macros", ] [[package]] name = "defmt-macros" -version = "0.3.6" +version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54f0216f6c5acb5ae1a47050a6645024e6edafc2ee32d421955eccfef12ef92e" +checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.23", + "syn 2.0.59", ] [[package]] name = "defmt-parser" -version = "0.3.3" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "269924c02afd7f94bc4cecbfa5c379f6ffcf9766b3408fe63d22c728654eccd0" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" dependencies = [ "thiserror", ] @@ -351,43 +383,53 @@ dependencies = [ [[package]] name = "defmt-test" -version = "0.3.0" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4df24f1ca104a0c1bce2047d8a21aa9fa29695e5d662118eb48daedb97edca88" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" dependencies = [ - "cortex-m", "cortex-m-rt", + "cortex-m-semihosting", "defmt", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94a0dfea4063d72e1ba20494dfbc4667f67420869328cf3670b5824a38a22dc1" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 1.0.109", + "syn 2.0.59", +] + +[[package]] +name = "document-features" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" +dependencies = [ + "litrs", ] [[package]] name = "either" -version = "1.8.1" +version = "1.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" +checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ "defmt", "embassy-futures", "embassy-sync", + "embassy-time", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.10", + "embedded-hal 1.0.0", "embedded-hal-async", "embedded-storage", "embedded-storage-async", @@ -396,50 +438,49 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ - "atomic-polyfill 1.0.2", "cortex-m", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", - "embassy-macros", - "embassy-time", - "futures-util", - "static_cell", + "document-features", + "embassy-executor-macros", + "embassy-time-driver", + "embassy-time-queue-driver", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.4.1" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.59", ] [[package]] name = "embassy-futures" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +version = "0.1.1" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" [[package]] -name = "embassy-hal-common" +name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ "cortex-m", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", "num-traits", ] -[[package]] -name = "embassy-macros" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" -dependencies = [ - "darling", - "proc-macro2", - "quote", - "syn 2.0.23", -] - [[package]] name = "embassy-net-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ "defmt", ] @@ -447,28 +488,32 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ - "atomic-polyfill 1.0.2", "bit_field", - "bxcan", + "bitflags 2.5.0", "cfg-if", + "chrono", "cortex-m", "cortex-m-rt", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", + "document-features", "embassy-embedded-hal", "embassy-futures", - "embassy-hal-common", + "embassy-hal-internal", "embassy-net-driver", "embassy-sync", "embassy-time", + "embassy-time-driver", "embassy-usb-driver", + "embedded-can", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.10", + "embedded-hal 1.0.0", "embedded-hal-async", "embedded-hal-nb", "embedded-io", + "embedded-io-async", "embedded-storage", "embedded-storage-async", "futures", @@ -477,48 +522,74 @@ dependencies = [ "quote", "rand_core", "sdio-host", - "seq-macro", + "static_assertions", "stm32-fmc", "stm32-metapac", "vcell", + "volatile-register", ] [[package]] name = "embassy-sync" -version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ "cfg-if", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", - "embedded-io", + "embedded-io-async", "futures-util", - "heapless", + "heapless 0.8.0", ] [[package]] name = "embassy-time" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ - "atomic-polyfill 1.0.2", "cfg-if", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", + "document-features", + "embassy-time-driver", + "embassy-time-queue-driver", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.10", + "embedded-hal 1.0.0", + "embedded-hal-async", "futures-util", - "heapless", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +dependencies = [ + "document-features", ] +[[package]] +name = "embassy-time-queue-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" + [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#99b4ea7c1dd5d2d10020ae569b878231c6c00524" +source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" dependencies = [ "defmt", ] +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + [[package]] name = "embedded-hal" version = "0.2.7" @@ -531,49 +602,59 @@ dependencies = [ [[package]] name = "embedded-hal" -version = "1.0.0-alpha.10" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f65c4d073f5d91c66e629b216818a4c9747eeda0debedf2deda9a0a947e4e93b" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" [[package]] name = "embedded-hal-async" -version = "0.2.0-alpha.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8042370aa7af48de36d5312cda14c18ed8ca6b7ce64f5a07832fedc9dc83063f" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" dependencies = [ - "embedded-hal 1.0.0-alpha.10", + "embedded-hal 1.0.0", ] [[package]] name = "embedded-hal-nb" -version = "1.0.0-alpha.2" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1465fffd56a95bbc105c17965bca1c1d5815027b1cc6bb183bc05d04563d065c" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" dependencies = [ - "embedded-hal 1.0.0-alpha.10", + "embedded-hal 1.0.0", "nb 1.1.0", ] [[package]] name = "embedded-io" -version = "0.4.0" +version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ "defmt", ] +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "defmt", + "embedded-io", +] + [[package]] name = "embedded-storage" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "156d7a2fdd98ebbf9ae579cbceca3058cff946e13f8e17b90e3511db0508c723" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" [[package]] name = "embedded-storage-async" -version = "0.4.0" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "052997a894670d0cde873faa7405bc98e2fd29f569d2acd568561bc1c396b35a" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" dependencies = [ "embedded-storage", ] @@ -591,6 +672,16 @@ dependencies = [ "termcolor", ] +[[package]] +name = "errno" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +dependencies = [ + "libc", + "windows-sys", +] + [[package]] name = "fnv" version = "1.0.7" @@ -599,9 +690,9 @@ checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" [[package]] name = "futures" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23342abe12aba583913b2e62f22225ff9c950774065e4bfb61a19cd9770fec40" +checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" dependencies = [ "futures-channel", "futures-core", @@ -613,9 +704,9 @@ dependencies = [ [[package]] name = "futures-channel" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "955518d47e09b25bbebc7a18df10b81f0c766eaf4c4f1cccef2fca5f2a4fb5f2" +checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" dependencies = [ "futures-core", "futures-sink", @@ -623,44 +714,44 @@ dependencies = [ [[package]] name = "futures-core" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4bca583b7e26f571124fe5b7561d49cb2868d79116cfa0eefce955557c6fee8c" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" [[package]] name = "futures-io" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fff74096e71ed47f8e023204cfd0aa1289cd54ae5430a9523be060cdb849964" +checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" [[package]] name = "futures-macro" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72" +checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" dependencies = [ "proc-macro2", "quote", - "syn 2.0.23", + "syn 2.0.59", ] [[package]] name = "futures-sink" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f43be4fe21a13b9781a69afa4985b0f6ee0e1afab2c6f454a8cf30e2b2237b6e" +checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" [[package]] name = "futures-task" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "76d3d132be6c0e6aa1534069c705a74a5997a356c0dc2f86a47765e5617c5b65" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" [[package]] name = "futures-util" -version = "0.3.28" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "26b01e40b772d54cf6c6d721c1d1abd0647a0106a12ecaa1c186273392a69533" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" dependencies = [ "futures-core", "futures-macro", @@ -685,6 +776,15 @@ dependencies = [ "byteorder", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -693,17 +793,27 @@ checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" [[package]] name = "heapless" -version = "0.7.16" +version = "0.7.17" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743" +checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" dependencies = [ - "atomic-polyfill 0.1.11", - "hash32", + "atomic-polyfill", + "hash32 0.2.1", "rustc_version 0.4.0", "spin", "stable_deref_trait", ] +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] + [[package]] name = "hermit-abi" version = "0.1.19" @@ -713,6 +823,15 @@ dependencies = [ "libc", ] +[[package]] +name = "home" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +dependencies = [ + "windows-sys", +] + [[package]] name = "humantime" version = "2.1.0" @@ -749,31 +868,43 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.147" +version = "0.2.153" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4668fb0ea861c1df094127ac5f1da3409a82116a4ba74fca2e58ef927159bb3" +checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" [[package]] name = "libloading" -version = "0.7.4" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" dependencies = [ "cfg-if", - "winapi", + "windows-targets", ] [[package]] name = "libm" -version = "0.2.7" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + +[[package]] +name = "linux-raw-sys" +version = "0.4.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" + +[[package]] +name = "litrs" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f7012b1bbb0719e1097c47611d3898568c546d597c2e74d66f6087edd5233ff4" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" [[package]] name = "lock_api" -version = "0.4.10" +version = "0.4.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c1cc9717a20b1bb222f333e6a92fd32f7d8a18ddc5a3191a11af45dcbf4dcd16" +checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" dependencies = [ "autocfg", "scopeguard", @@ -781,15 +912,15 @@ dependencies = [ [[package]] name = "log" -version = "0.4.19" +version = "0.4.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b06a4cde4c0f271a446782e3eff8de789548ce57dbc8eca9292c27f4a42004b4" +checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" [[package]] name = "memchr" -version = "2.5.0" +version = "2.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" [[package]] name = "minimal-lexical" @@ -824,24 +955,24 @@ dependencies = [ [[package]] name = "num-traits" -version = "0.2.15" +version = "0.2.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" +checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" dependencies = [ "autocfg", ] [[package]] name = "once_cell" -version = "1.18.0" +version = "1.19.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "os_str_bytes" -version = "6.5.1" +version = "6.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4d5d9eb14b174ee9aa2ef96dc2b94637a2d4b6e7cb873c7e171f0c20c6cf3eac" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" @@ -861,9 +992,9 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" [[package]] name = "pin-project-lite" -version = "0.2.10" +version = "0.2.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c40d25201921e5ff0c862a505c6557ea88568a4e3ace775ab55e93f2f4f9d57" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" [[package]] name = "pin-utils" @@ -871,6 +1002,12 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" +[[package]] +name = "portable-atomic" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" + [[package]] name = "proc-macro-error" version = "1.0.4" @@ -897,18 +1034,18 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.63" +version = "1.0.80" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b368fba921b0dce7e60f5e04ec15e565b3303972b42bcfde1d0713b881959eb" +checksum = "a56dea16b0a29e94408b9aa5e2940a4eedbd128a1ba20e8f7ae60fd3d465af0e" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.29" +version = "1.0.36" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "573015e8ab27661678357f27dc26460738fd2b6c86e46f386fde94cb5d913105" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" dependencies = [ "proc-macro2", ] @@ -921,9 +1058,21 @@ checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" [[package]] name = "regex" -version = "1.8.4" +version = "1.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d0ab3ca65655bb1e41f2a8c8cd662eb4fb035e67c3f78da1d61dffe89d07300f" +checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" dependencies = [ "aho-corasick", "memchr", @@ -932,9 +1081,18 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.7.2" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" + +[[package]] +name = "rgb" +version = "0.8.37" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "436b050e76ed2903236f032a59761c1eb99e1b0aead2c257922771dab1fc8c78" +checksum = "05aaa8004b64fd573fc9d002f4e632d51ad4f026c2b5ba95fcb6c2f32c2c47d8" +dependencies = [ + "bytemuck", +] [[package]] name = "rustc-hash" @@ -957,14 +1115,27 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.17", + "semver 1.0.22", +] + +[[package]] +name = "rustix" +version = "0.38.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "65e04861e65f21776e67888bfbea442b3642beaa0138fdb1dd7a84a52dffdb89" +dependencies = [ + "bitflags 2.5.0", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", ] [[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 = "sdio-host" @@ -983,9 +1154,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.17" +version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bebd363326d05ec3e2f532ab7660680f3b02130d780c299bca73469d521bc0ed" +checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" [[package]] name = "semver-parser" @@ -994,16 +1165,28 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] -name = "seq-macro" -version = "0.3.4" +name = "shlex" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "63134939175b3131fe4d2c131b103fd42f25ccca89423d43b5e4f267920ccf03" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" [[package]] -name = "shlex" -version = "1.1.0" +name = "smart-leds" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3" +checksum = "66df34e571fa9993fa6f99131a374d58ca3d694b75f9baac93458fe0d6057bf0" +dependencies = [ + "smart-leds-trait", +] + +[[package]] +name = "smart-leds-trait" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +dependencies = [ + "rgb", +] [[package]] name = "spin" @@ -1020,29 +1203,34 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + [[package]] name = "static_cell" -version = "1.2.0" +version = "2.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49cd323fc21eb534f903ee78d781d622099f9716c5b408ed23bcf39f8f1651c0" +checksum = "fa6ba4cf83bf80d3eb25f098ea5e790a0a1fcb5e357442259b231e412c2d3ca0" dependencies = [ - "atomic-polyfill 1.0.2", + "portable-atomic", ] [[package]] name = "stm32-fmc" -version = "0.2.4" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf16ee9bd5de754482883cf3eac9a49eb862baf1420f55ce408e001705e9ae74" +checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" dependencies = [ "embedded-hal 0.2.7", ] [[package]] name = "stm32-metapac" -version = "12.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a361cae600f71ad0f1ba2163eb7b7681767fa86ffac685dc5b5f14c4a93aee1" +version = "15.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-01ac9bfd035961dc75f32dcd6080501538246d5c#a5e8ddc393950d780eb95fba15833d8a68b4e216" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1067,9 +1255,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.23" +version = "2.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "59fb7d6d8281a51045d62b8eb3a7d1ce347b76f312af50cd3dc0af39c87c1737" +checksum = "4a6531ffc7b071655e4ce2e04bd464c4830bb585a61cabb96cf808f05172615a" dependencies = [ "proc-macro2", "quote", @@ -1078,44 +1266,44 @@ dependencies = [ [[package]] name = "termcolor" -version = "1.2.0" +version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "be55cf8942feac5c765c2c993422806843c9a9a45d4d5c407ad6dd2ea95eb9b6" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" dependencies = [ "winapi-util", ] [[package]] name = "textwrap" -version = "0.16.0" +version = "0.16.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "222a222a5bfe1bba4a77b45ec488a741b3cb8872e5e499451fd7d0129c9c7c3d" +checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.40" +version = "1.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "978c9a314bd8dc99be594bc3c175faaa9794be04a5a5e153caba6915336cebac" +checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.40" +version = "1.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f" +checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.23", + "syn 2.0.59", ] [[package]] name = "unicode-ident" -version = "1.0.10" +version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "22049a19f4a68748a168c0fc439f9516686aa045927ff767eca0a85101fb6e73" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" [[package]] name = "unicode-xid" @@ -1143,22 +1331,23 @@ checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" [[package]] name = "volatile-register" -version = "0.2.1" +version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6" +checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" dependencies = [ "vcell", ] [[package]] name = "which" -version = "4.4.0" +version = "4.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2441c784c52b289a054b7201fc93253e288f094e2f4be9058343127c4226a269" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" dependencies = [ "either", - "libc", + "home", "once_cell", + "rustix", ] [[package]] @@ -1179,9 +1368,9 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.5" +version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" dependencies = [ "winapi", ] @@ -1191,3 +1380,76 @@ name = "winapi-x86_64-pc-windows-gnu" version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 3e409eb2..e35216dd 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -8,40 +8,45 @@ edition = "2021" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.1" -defmt = "=0.3.2" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) -defmt-rtt = "0.3" -embassy-executor = { version = "0.2.0", features = [ - "nightly", +embassy-executor = { version = "0.5.0", features = [ "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", "integrated-timers", + "task-arena-size-32768", ] } -embassy-time = { version = "0.1.0", features = [ +embassy-time = { version = "0.3.0", features = [ "defmt", "defmt-timestamp-uptime", - "unstable-traits", "tick-hz-32_768", ] } embassy-stm32 = { version = "0.1.0", features = [ - "nightly", "defmt", - "stm32f030k6", - "time-driver-any", + "stm32f407ve", + "unstable-pac", + "time-driver-tim1", "exti", - "unstable-pac", - "unstable-traits", + "chrono" ] } embassy-futures = { version = "0.1.0" } -futures-util = { version = "0.3.17", default-features = false } -embassy-sync = { version = "0.2.0" } +embassy-sync = { version = "0.5.0" } + +defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) +defmt-rtt = "0.3" panic-probe = { version = "0.3", features = ["print-defmt"] } -static_cell = "1.0" + +futures-util = { version = "0.3.17", default-features = false } +static_cell = "2.0.0" critical-section = "1.1.1" const_format = "0.2.30" heapless = "0.7.16" libm = "0.2.6" + +smart-leds = "0.4.0" +apa102-spi = "0.4.0" + +ateam-lib-stm32 = { path = "../lib-stm32" } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } [dev-dependencies] @@ -91,11 +96,6 @@ test = false harness = false [patch.crates-io] -# embassy-executor = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-sync = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-time = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } -# embassy-futures = { git = "https://github.com/embassy-rs/embassy", rev = "047ea9066f0d946fd4d706577b21df38fd3b1647" } embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} embassy-time = { git = "https://github.com/embassy-rs/embassy"} diff --git a/kicker-board/memory.x b/kicker-board/memory.x index db6af4f3..5ad3e630 100644 --- a/kicker-board/memory.x +++ b/kicker-board/memory.x @@ -1,8 +1,9 @@ MEMORY { /* NOTE K = KiBi = 1024 bytes */ - FLASH : ORIGIN = 0x08000000, LENGTH = 32K - RAM : ORIGIN = 0x20000000, LENGTH = 4K + FLASH : ORIGIN = 0x08000000, LENGTH = 512K + RAM : ORIGIN = 0x20000000, LENGTH = 128K + /* CCMRAM : ORIGIN = 0x10000000, LENGTH = 64K */ } /* This is where the call stack will be allocated. */ diff --git a/kicker-board/rust-toolchain.toml b/kicker-board/rust-toolchain.toml index ffb24c49..42b54e8d 100644 --- a/kicker-board/rust-toolchain.toml +++ b/kicker-board/rust-toolchain.toml @@ -1,3 +1,3 @@ [toolchain] channel = "nightly" -targets = [ "thumbv6m-none-eabi" ] \ No newline at end of file +targets = [ "thumbv7em-none-eabihf" ] diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 4646c083..9d8b1c67 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -5,62 +5,170 @@ use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m_rt::entry; - +use embassy_executor::Spawner; use embassy_executor::Executor; use embassy_stm32::{ - gpio::{Level, Output, Speed}, - time::mhz, + adc::{Adc, SampleTime}, + exti::ExtiInput, + gpio::{Input, Level, Output, Pull, Speed}, + spi::{Config, Spi}, time::mhz, }; use embassy_time::{Duration, Timer}; use static_cell::StaticCell; -use ateam_kicker_board::pins::{ChargePin, RedStatusLedPin, GreenStatusLedPin}; +use smart_leds::{SmartLedsWrite, RGB8}; +use apa102_spi::Apa102; + +use ateam_kicker_board::{tasks::get_system_config, *}; +use ateam_kicker_board::pins::*; + +use panic_probe as _; +// use panic_halt as _; #[embassy_executor::task] async fn blink( reg_charge: ChargePin, status_led_red: RedStatusLedPin, - status_led_green: GreenStatusLedPin) -> ! { + status_led_green: GreenStatusLedPin, + status_led_blue1: BlueStatusLed1Pin, + status_led_blue2: BlueStatusLed2Pin, + usr_btn_pin: UserBtnPin, + mut adc: Adc<'static, PowerRailAdc>, + mut rail_200v_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + mut rail_6v2_pin: PowerRail6v2ReadPin, + mut rail_5v0_pin: PowerRail5v0ReadPin) -> ! { let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); - let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::High, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + let mut status_led_blue1 = Output::new(status_led_blue1, Level::Low, Speed::Medium); + let mut status_led_blue2 = Output::new(status_led_blue2, Level::Low, Speed::Medium); + + let usr_btn = Input::new(usr_btn_pin, Pull::None); + + // let mut temp = adc.enable_temperature(); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); + + 'outer: while usr_btn.is_low() { + while usr_btn.is_high() { + defmt::info!("btn pressed! - cycle"); + break 'outer; + } + } + + Timer::after(Duration::from_millis(1000)).await; loop { reg_charge.set_low(); status_led_green.set_high(); + status_led_blue1.set_high(); + status_led_blue2.set_high(); status_led_red.set_low(); Timer::after(Duration::from_millis(500)).await; + status_led_green.set_low(); + status_led_blue1.set_low(); + status_led_blue2.set_low(); status_led_red.set_high(); Timer::after(Duration::from_millis(500)).await; + + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + let raw_200v = adc.read(&mut rail_200v_pin) as f32; + let raw_12v = adc.read(&mut rail_12v0_pin) as f32; + let raw_6v2 = adc.read(&mut rail_6v2_pin) as f32; + let raw_5v0 = adc.read(&mut rail_5v0_pin) as f32; + let raw_int = adc.read(&mut vrefint) as f32; + + defmt::info!("voltages - 200v ({}), 12v0 ({}), 6v2 ({}), 5v0 ({}), 3v3 ({})", + adc_200v_to_rail_voltage(adc_raw_to_v(raw_200v, vrefint_sample)), + adc_12v_to_rail_voltage(adc_raw_to_v(raw_12v, vrefint_sample)), + adc_6v2_to_rail_voltage(adc_raw_to_v(raw_6v2, vrefint_sample)), + adc_5v0_to_rail_voltage(adc_raw_to_v(raw_5v0, vrefint_sample)), + adc_3v3_to_rail_voltage(adc_raw_to_v(raw_int, vrefint_sample))); + + } } -static EXECUTOR_LOW: StaticCell = StaticCell::new(); +#[embassy_executor::task] +async fn dotstar_lerp_task(dotstar_spi: DotstarSpi, + dotstar_sck_pin: DotstarSpiSckPin, + dotstar_mosi_pin: DotstarSpiMosiPin, + dotstar_tx_dma: DotstarSpiTxDma) { + + let mut dotstar_spi_config = Config::default(); + dotstar_spi_config.frequency = mhz(1); -#[entry] -fn main() -> ! { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); + let dotstar_spi = Spi::new_txonly( + dotstar_spi, + dotstar_sck_pin, + dotstar_mosi_pin, + dotstar_tx_dma, + dotstar_spi_config); - let p = embassy_stm32::init(stm32_config); + let mut dotstar = Apa102::new(dotstar_spi); + + let mut val = 0; + loop { + let _ = dotstar.write([RGB8 { r: val, g: val, b: val }, RGB8 { r: val / 2, g: val / 2, b: val / 2 }].iter().cloned()); + val += 1; - let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); - let _chip_pin = Output::new(p.PB1, Level::Low, Speed::Medium); + if val == 255 { + val = 0; + } + + Timer::after_millis(10).await; + } +} + +#[embassy_executor::task] +async fn shutdown_int(pwr_btn_int_pin: PowerBtnIntPin, + pwr_btn_int_exti: PowerBtnIntExti, + pwr_kill_pin: PowerKillPin) { + + let mut pwr_btn_int = ExtiInput::new(pwr_btn_int_pin, pwr_btn_int_exti, Pull::Down); + let mut pwr_kill = Output::new(pwr_kill_pin, Level::High, Speed::Medium); + + defmt::info!("task waiting for btn int from power front end..."); + + pwr_btn_int.wait_for_rising_edge().await; + + defmt::warn!("received request to power down."); + + Timer::after(Duration::from_millis(1000)).await; + + // TODO do anything you need to do + + pwr_kill.set_low(); + + loop {} +} + +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_config); info!("kicker startup!"); + + let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); + let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); - // let mut nvic: NVIC = unsafe { mem::transmute(()) }; + let adc = Adc::new(p.ADC1); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(blink(p.PB3, p.PA12, p.PA11))); + unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); + unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, p.PD4, adc, p.PC0, p.PC1, p.PC3, p.PC2))); + unwrap!(spawner.spawn(dotstar_lerp_task(p.SPI1, p.PA5, p.PA7, p.DMA2_CH3))); }); } \ No newline at end of file diff --git a/kicker-board/src/bin/hwtest-breakbeam/main.rs b/kicker-board/src/bin/hwtest-breakbeam/main.rs index a1d5207c..a030a868 100644 --- a/kicker-board/src/bin/hwtest-breakbeam/main.rs +++ b/kicker-board/src/bin/hwtest-breakbeam/main.rs @@ -2,32 +2,21 @@ #![no_main] #![feature(type_alias_impl_trait)] -use cortex_m::prelude::_embedded_hal_blocking_delay_DelayUs; use defmt::*; + +use ateam_kicker_board::tasks::get_system_config; +use embassy_stm32::gpio::{Level, Output, Speed}; use {defmt_rtt as _, panic_probe as _}; use embassy_executor::Spawner; -use embassy_stm32::{ - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, - time::mhz, - usart::{self, Uart}, -}; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_kicker_board::{ - drivers::{breakbeam::Breakbeam} -}; -use ateam_kicker_board::pins::{BlueStatusLedPin, GreenStatusLedPin, BreakbeamTxPin, BreakbeamRxPin}; + +use embassy_time::{Duration, Timer}; +use ateam_kicker_board::drivers::breakbeam::Breakbeam; #[embassy_executor::main] async fn main(_spawner: Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); - - let p = embassy_stm32::init(stm32_config); + let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_cfg); let _charge_pin = Output::new(p.PB3, Level::Low, Speed::Medium); let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); diff --git a/kicker-board/src/bin/hwtest-charge/main.rs b/kicker-board/src/bin/hwtest-charge/main.rs index 4597d3c8..1fb1f90c 100644 --- a/kicker-board/src/bin/hwtest-charge/main.rs +++ b/kicker-board/src/bin/hwtest-charge/main.rs @@ -2,88 +2,36 @@ #![no_main] #![feature(type_alias_impl_trait)] -use core::mem; - use defmt::*; use {defmt_rtt as _, panic_probe as _}; -// use cortex_m::{peripheral::NVIC, delay::Delay}; -use cortex_m::{peripheral::NVIC}; use cortex_m_rt::entry; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, AdcPin, InternalChannel, Temperature}, - pac::Interrupt, - Peripherals, - gpio::{Input, Level, Output, Pull, Speed}, - gpio::low_level::Pin, - interrupt, adc::SampleTime, + adc::Adc, + gpio::{Level, Output, Speed}, + adc::SampleTime, }; -use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; -use embassy_time::{Delay, Duration, Timer, Ticker}; +use embassy_time::{Duration, Timer, Ticker}; use static_cell::StaticCell; -use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; - -// #[embassy_executor::task] -// async fn run_critical_section_task(p: Peripherals) { -// let reg_done = Input::new(p.PB4, Pull::None); -// let reg_fault = Input::new(p.PB5, Pull::None); - -// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); -// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); -// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); - -// status_led_green.set_high(); -// Timer::after(Duration::from_millis(500)).await; -// status_led_green.set_low(); -// Timer::after(Duration::from_millis(500)).await; - -// reg_charge.set_high(); -// Timer::after(Duration::from_millis(100)).await; -// reg_charge.set_low(); - -// loop { -// reg_charge.set_low(); - -// if reg_done.is_low() { -// status_led_green.set_high(); -// } else { -// status_led_green.set_low(); -// } - -// if reg_fault.is_low() { -// status_led_red.set_high(); -// } else { -// status_led_red.set_low(); -// } -// } -// } - -// #[embassy_executor::task] -// async fn read_adc_samples(aps: PubSubChannel::) -> ! { -// let sub = aps.subscriber().unwrap(); -// loop { -// info!(sub.) -// } -// } +use ateam_kicker_board::*; +use ateam_kicker_board::pins::*; #[embassy_executor::task] -async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, - mut hv_pin: HighVoltageReadPin, - mut batt_pin: BatteryVoltageReadPin, - mut reg_charge: ChargePin, - mut reg_done: RegulatorDonePin, - mut reg_fault: RegulatorFaultPin, - mut status_led_red: RedStatusLedPin, - mut status_led_green: GreenStatusLedPin) -> ! { +async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + kick_pin: KickPin) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); - let reg_done = Input::new(reg_done, Pull::None); - let reg_fault = Input::new(reg_fault, Pull::None); + let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); @@ -94,98 +42,73 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, status_led_green.set_low(); Timer::after(Duration::from_millis(500)).await; - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; let mut hv = adc.read(&mut hv_pin) as f32; - let mut bv = adc.read(&mut batt_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + let mut regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); - if start_up_battery_voltage < 18.0 { + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); + if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); warn!("refusing to continue"); loop { reg_charge.set_low(); + + kick.set_high(); + Timer::after(Duration::from_micros(500)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; } } reg_charge.set_high(); - Timer::after(Duration::from_millis(400)).await; - let reg_done_stat = reg_done.is_low(); - let reg_fault_stat = reg_fault.is_low(); + Timer::after(Duration::from_millis(50)).await; reg_charge.set_low(); + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + + Timer::after(Duration::from_millis(1000)).await; + loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); reg_charge.set_low(); - - if reg_done_stat { - status_led_green.set_high(); - } else { - status_led_green.set_low(); - } - - if reg_fault_stat { - status_led_red.set_high(); - } else { - status_led_red.set_low(); - } + kick.set_low(); ticker.next().await; } } -// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); -#[derive(Clone, Copy)] -struct AnalogValues { - high_voltage: u16, - batt_voltage: u16, - temp: u16 -} - -// #[interrupt] -// unsafe fn I2C1() { -// EXECUTOR_HIGH.on_interrupt(); -// } - - #[entry] fn main() -> ! { let p = embassy_stm32::init(Default::default()); - info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - // let aps = PubSubChannel::::new(); - - - // highest priorty, energy management - // medium priority, ADC publisher - // low priority uart handler - // lowest prioiryt main update loop - - // High-priority executor: I2C1, priority level 6 - // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - // unwrap!(spawner.spawn(run_critical_section_task(p))); + info!("kicker startup!"); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); - // let mut temp_ch = adc.enable_temperature(&mut Delay); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11))); + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PE5))); }); } \ No newline at end of file diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index d7c0aa25..5e810073 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -2,47 +2,41 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] -use core::mem; +use core::cell::SyncUnsafeCell; use static_cell::StaticCell; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m::peripheral::NVIC; use cortex_m_rt::entry; use embassy_executor::{Executor, InterruptExecutor}; use embassy_stm32::{ - adc::{Adc, SampleTime}, - pac::Interrupt, + adc::{Adc, Resolution, SampleTime}, gpio::{Level, Output, Speed}, interrupt, - time::mhz, - usart::{Uart, Parity, StopBits, Config} + interrupt::InterruptExt, + pac::Interrupt, + rcc::{AHBPrescaler, APBPrescaler, Pll, PllMul, PllPDiv, PllPreDiv, PllQDiv, PllSource, Sysclk}, + usart::{Config, Parity, StopBits, Uart} }; use embassy_stm32::{bind_interrupts, peripherals, usart}; -use embassy_time::{Delay, Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker}; use ateam_kicker_board::{ - adc_raw_to_v, - adc_v_to_battery_voltage, - adc_v_to_rail_voltage, + adc_200v_to_rail_voltage, adc_raw_to_v, kick_manager::{ KickManager, - KickType}, - pins::{ - HighVoltageReadPin, BatteryVoltageReadPin, - ChargePin, KickPin, ChipPin, - ComsUartModule, ComsUartRxDma, ComsUartTxDma, RedStatusLedPin, BlueStatusLedPin}, - queue::Buffer, - uart_queue::{ - UartReadQueue, - UartWriteQueue, - } + KickType}, + pins::* }; +use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; + use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; const MAX_TX_PACKET_SIZE: usize = 16; @@ -51,18 +45,22 @@ const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; // control communications tx buffer +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".bss"] -static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX); // control communications rx buffer +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".bss"] -static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -84,16 +82,16 @@ fn get_empty_telem_packet() -> KickerTelemetry { #[embassy_executor::task] async fn high_pri_kick_task( - coms_reader: &'static UartReadQueue<'static, ComsUartModule, ComsUartRxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH>, - coms_writer: &'static UartWriteQueue<'static, ComsUartModule, ComsUartTxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH>, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + coms_reader: &'static UartReadQueue, + coms_writer: &'static UartWriteQueue, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, charge_pin: ChargePin, kick_pin: KickPin, chip_pin: ChipPin, - mut rail_pin: HighVoltageReadPin, - mut battery_voltage_pin: BatteryVoltageReadPin, + mut rail_pin: PowerRail200vReadPin, err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin) -> ! { + ball_detected_led1_pin: BlueStatusLed1Pin, + ball_detected_led2_pin: BlueStatusLed2Pin) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -103,11 +101,13 @@ async fn high_pri_kick_task( // debug LEDs let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); + // TODO dotstars // coms buffers - let mut telemetry_enabled: bool = false; + let mut telemetry_enabled: bool; let mut kicker_control_packet: KickerControl = get_empty_control_packet(); let mut kicker_telemetry_packet: KickerTelemetry = get_empty_telem_packet(); @@ -116,11 +116,10 @@ async fn high_pri_kick_task( let mut last_packet_sent_time = Instant::now(); loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; - let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); - let battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample)); + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample)); // optionally pre-flag errors? ///////////////////////////////////// @@ -156,7 +155,7 @@ async fn high_pri_kick_task( telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; // no charge/kick in coms test - let res = kick_manager.command(battery_voltage, rail_voltage, false, KickType::None, 0.0).await; + let res = kick_manager.command(22.5, rail_voltage, false, KickType::None, 0.0).await; // send telemetry packet if telemetry_enabled { @@ -164,7 +163,7 @@ async fn high_pri_kick_task( if Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > 20 { kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1(0, 0, ball_detected as u32, res.is_err() as u32); kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = battery_voltage; + kicker_telemetry_packet.battery_voltage = 22.5; // raw interpretaion of a struct for wire transmission is unsafe unsafe { @@ -178,10 +177,16 @@ async fn high_pri_kick_task( let _res = coms_writer.enqueue_copy(struct_bytes); } - if ball_detected_led.is_set_high() { - ball_detected_led.set_low(); + if ball_detected_led1.is_set_high() { + ball_detected_led1.set_low(); } else { - ball_detected_led.set_high(); + ball_detected_led1.set_high(); + } + + if ball_detected_led2.is_set_high() { + ball_detected_led2.set_low(); + } else { + ball_detected_led2.set_high(); } last_packet_sent_time = cur_time; @@ -196,9 +201,12 @@ async fn high_pri_kick_task( } if ball_detected { - ball_detected_led.set_high(); + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); } else { - ball_detected_led.set_low(); + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); + } // TODO Dotstar @@ -212,7 +220,7 @@ static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[interrupt] -unsafe fn I2C1() { +unsafe fn TIM2() { EXECUTOR_HIGH.on_interrupt(); } @@ -223,29 +231,37 @@ bind_interrupts!(struct Irqs { #[entry] fn main() -> ! { let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); + stm32_config.rcc.hsi = true; + stm32_config.rcc.pll_src = PllSource::HSI; // internal 16Mhz source + stm32_config.rcc.pll = Some(Pll { + prediv: PllPreDiv::DIV8, // base 2MHz + mul: PllMul::MUL168, // mul up to 336 MHz + divp: Some(PllPDiv::DIV2), // div down to 168 MHz, AHB (max) + divq: Some(PllQDiv::DIV7), // div down to 48 Mhz for dedicated 48MHz clk (exact) + divr: None, // turn off I2S clk + }); + stm32_config.rcc.ahb_pre = AHBPrescaler::DIV1; // AHB 168 MHz (max) + stm32_config.rcc.apb1_pre = APBPrescaler::DIV4; // APB1 42 MHz (max) + stm32_config.rcc.apb2_pre = APBPrescaler::DIV2; // APB2 84 MHz (max) + stm32_config.rcc.sys = Sysclk::PLL1_P; // enable the system let p = embassy_stm32::init(stm32_config); info!("kicker startup!"); - - let mut nvic: NVIC = unsafe { mem::transmute(()) }; let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PB3, p.PB0, p.PB1, p.PA0, p.PA1, p.PA12, p.PA8))); - // unwrap!(spawner.spawn(high_pri_tx_test(&COMS_QUEUE_TX, p.PB3, p.PB0, p.PB1))); + embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); + let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -261,12 +277,12 @@ fn main() -> ! { p.PA10, p.PA9, Irqs, - p.DMA1_CH2, - p.DMA1_CH3, + p.DMA2_CH7, + p.DMA2_CH2, coms_uart_config, - ); + ).unwrap(); - let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); // low priority executor handles coms and user IO // Low priority executor: runs in thread mode, using WFE/SEV diff --git a/kicker-board/src/bin/hwtest-flash/main.rs b/kicker-board/src/bin/hwtest-flash/main.rs new file mode 100644 index 00000000..73e200af --- /dev/null +++ b/kicker-board/src/bin/hwtest-flash/main.rs @@ -0,0 +1,53 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::*; +use static_cell::StaticCell; +use {defmt_rtt as _, panic_probe as _}; + +use embassy_executor::Spawner; +use embassy_stm32::{ + spi::{Config, Spi}, + time::Hertz, +}; + +use ateam_lib_stm32::drivers::flash::at25df041b::AT25DF041B; + +use ateam_kicker_board::{tasks::get_system_config, *}; + +use panic_probe as _; +// use panic_halt as _; + +#[link_section = ".bss"] +static FLASH_RX_BUF: StaticCell<[u8; 256]> = StaticCell::new(); +#[link_section = ".bss"] +static FLASH_TX_BUF: StaticCell<[u8; 256]> = StaticCell::new(); + + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + let sys_config = get_system_config(tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_config); + + info!("kicker startup!"); + + let mut spi_config = Config::default(); + spi_config.frequency = Hertz(1_000_000); + + let spi = Spi::new(p.SPI2, p.PB13, p.PB15, p.PB14, p.DMA1_CH4, p.DMA1_CH3, spi_config); + + let rx_buf = FLASH_RX_BUF.init([0; 256]); + let tx_buf = FLASH_TX_BUF.init([0; 256]); + + + let mut flash: AT25DF041B<'static, embassy_stm32::peripherals::SPI2, true> = AT25DF041B::new(spi, p.PB12, rx_buf, tx_buf); + let res = flash.verify_chip_id().await; + if res.is_err() { + defmt::error!("failed to verify flash chip ID"); + } else { + defmt::info!("verified flash chip ID"); + } + + loop {} +} \ No newline at end of file diff --git a/kicker-board/src/bin/hwtest-kick/main.rs b/kicker-board/src/bin/hwtest-kick/main.rs index fe285d9e..caaf0340 100644 --- a/kicker-board/src/bin/hwtest-kick/main.rs +++ b/kicker-board/src/bin/hwtest-kick/main.rs @@ -2,114 +2,59 @@ #![no_main] #![feature(type_alias_impl_trait)] -use core::mem; - use defmt::*; use {defmt_rtt as _, panic_probe as _}; -// use core::hint::black_box; - -// use cortex_m::{peripheral::NVIC, delay::Delay}; -use cortex_m::{peripheral::NVIC}; use cortex_m_rt::entry; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::Executor; use embassy_stm32::{ - adc::{Adc, AdcPin, InternalChannel, Temperature}, - pac::Interrupt, - Peripherals, + adc::{Adc, SampleTime}, gpio::{Input, Level, Output, Pull, Speed}, - gpio::low_level::Pin, - interrupt, adc::SampleTime, }; -use embassy_sync::{pubsub::{PubSubChannel, Publisher}, blocking_mutex::raw::NoopRawMutex}; -use embassy_time::{Delay, Duration, Timer, Ticker}; +use embassy_time::{Duration, Timer, Ticker}; use static_cell::StaticCell; -use ateam_kicker_board::{pins::{HighVoltageReadPin, BatteryVoltageReadPin, ChargePin, RegulatorDonePin, RegulatorFaultPin, RedStatusLedPin, GreenStatusLedPin, KickPin}, adc_v_to_rail_voltage, adc_raw_to_v, adc_v_to_battery_voltage}; - -// #[embassy_executor::task] -// async fn run_critical_section_task(p: Peripherals) { -// let reg_done = Input::new(p.PB4, Pull::None); -// let reg_fault = Input::new(p.PB5, Pull::None); - -// let mut reg_charge = Output::new(p.PB3, Level::Low, Speed::Medium); -// let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); -// let mut status_led_red = Output::new(p.PA12, Level::Low, Speed::Medium); - -// status_led_green.set_high(); -// Timer::after(Duration::from_millis(500)).await; -// status_led_green.set_low(); -// Timer::after(Duration::from_millis(500)).await; - -// reg_charge.set_high(); -// Timer::after(Duration::from_millis(100)).await; -// reg_charge.set_low(); - -// loop { -// reg_charge.set_low(); - -// if reg_done.is_low() { -// status_led_green.set_high(); -// } else { -// status_led_green.set_low(); -// } - -// if reg_fault.is_low() { -// status_led_red.set_high(); -// } else { -// status_led_red.set_low(); -// } -// } -// } - -// #[embassy_executor::task] -// async fn read_adc_samples(aps: PubSubChannel::) -> ! { -// let sub = aps.subscriber().unwrap(); -// loop { -// info!(sub.) -// } -// } +use ateam_kicker_board::*; +use ateam_kicker_board::pins::*; #[embassy_executor::task] -async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, - mut hv_pin: HighVoltageReadPin, - mut batt_pin: BatteryVoltageReadPin, - mut reg_charge: ChargePin, - mut reg_done: RegulatorDonePin, - mut reg_fault: RegulatorFaultPin, - mut status_led_red: RedStatusLedPin, - mut status_led_green: GreenStatusLedPin, - mut kick_pin: KickPin) -> ! { +async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + usr_btn_pin: UserBtnPin, + kick_pin: KickPin) -> ! { let mut ticker = Ticker::every(Duration::from_millis(1)); let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); - let reg_done = Input::new(reg_done, Pull::None); - let reg_fault = Input::new(reg_fault, Pull::None); - let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + let usr_btn = Input::new(usr_btn_pin, Pull::None); + status_led_green.set_high(); Timer::after(Duration::from_millis(500)).await; status_led_green.set_low(); Timer::after(Duration::from_millis(500)).await; - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; let mut hv = adc.read(&mut hv_pin) as f32; - let mut bv = adc.read(&mut batt_pin) as f32; - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + let mut regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); - let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample)); - if start_up_battery_voltage < 18.0 { + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); + if start_up_battery_voltage < 11.5 { status_led_red.set_high(); - warn!("battery voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); warn!("refusing to continue"); loop { reg_charge.set_low(); @@ -122,19 +67,25 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, } } + 'outer: while usr_btn.is_low() { + while usr_btn.is_high() { + defmt::info!("btn pressed! - initiating kick cycle"); + break 'outer; + } + } + + Timer::after(Duration::from_millis(1000)).await; + reg_charge.set_high(); - Timer::after(Duration::from_millis(600)).await; - let reg_done_stat = reg_done.is_low(); - let reg_fault_stat = reg_fault.is_low(); + Timer::after(Duration::from_millis(450)).await; reg_charge.set_low(); - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; - // info!("hv V: {}, batt mv: {}", ((hv * 200) * 8 / 10), (bv * 25000 / 1650 * (8 / 10))); // 8 / 10 for adc offset - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); Timer::after(Duration::from_millis(1000)).await; @@ -152,76 +103,36 @@ async fn sample_adc(mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint) as f32; + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; hv = adc.read(&mut hv_pin) as f32; - bv = adc.read(&mut batt_pin) as f32; - - info!("hv V: {}, batt mv: {}", adc_v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_v_to_battery_voltage(adc_raw_to_v(bv, vrefint_sample))); + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); reg_charge.set_low(); kick.set_low(); - // if reg_done_stat { - // status_led_green.set_high(); - // } else { - // status_led_green.set_low(); - // } - - // if reg_fault_stat { - // status_led_red.set_high(); - // } else { - // status_led_red.set_low(); - // } - ticker.next().await; } } -// static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); -#[derive(Clone, Copy)] -struct AnalogValues { - high_voltage: u16, - batt_voltage: u16, - temp: u16 -} - -// #[interrupt] -// unsafe fn I2C1() { -// EXECUTOR_HIGH.on_interrupt(); -// } - - #[entry] fn main() -> ! { let p = embassy_stm32::init(Default::default()); - info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - - // let aps = PubSubChannel::::new(); - - // highest priorty, energy management - // medium priority, ADC publisher - // low priority uart handler - // lowest prioiryt main update loop - - // High-priority executor: I2C1, priority level 6 - // unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - // let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - // unwrap!(spawner.spawn(run_critical_section_task(p))); + info!("kicker startup!"); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); - // let mut temp_ch = adc.enable_temperature(&mut Delay); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(sample_adc(adc, p.PA0, p.PA1, p.PB3, p.PB4, p.PB5, p.PA12, p.PA11, p.PB0))); + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PD4, p.PE5))); }); } \ No newline at end of file diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 768e617b..1f79715c 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -2,14 +2,15 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] -use core::mem; +use core::cell::SyncUnsafeCell; +use ateam_kicker_board::{drivers::breakbeam::Breakbeam, tasks::get_system_config}; use static_cell::StaticCell; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m::peripheral::NVIC; use cortex_m_rt::entry; use libm::{fmaxf, fminf}; @@ -20,25 +21,25 @@ use embassy_stm32::{ bind_interrupts, gpio::{Level, Output, Speed}, interrupt, + interrupt::InterruptExt, pac::Interrupt, peripherals, - time::mhz, - usart, - usart::{Config, Parity, StopBits, Uart}, + usart::{self, Config, Parity, StopBits, Uart}, }; -use embassy_time::{Delay, Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker}; use ateam_kicker_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, adc_v_to_rail_voltage, + adc_raw_to_v, adc_200v_to_rail_voltage, kick_manager::{KickManager, KickType}, pins::{ - BatteryVoltageReadPin, BlueStatusLedPin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, - ComsUartTxDma, HighVoltageReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, + BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, + ComsUartTxDma, PowerRail200vReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, }, - queue::Buffer, - uart_queue::{UartReadQueue, UartWriteQueue}, drivers::breakbeam::Breakbeam, }; +use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; + use ateam_common_packets::bindings_kicker::{ KickRequest::{self, KR_ARM, KR_DISABLE}, KickerControl, KickerTelemetry, @@ -58,26 +59,20 @@ pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; // control communications tx buffer -// #[link_section = ".axisram.buffers"] -static mut COMS_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue< - ComsUartModule, - ComsUartTxDma, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut COMS_BUFFERS_TX }); +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX); // control communications rx buffer -// #[link_section = ".axisram.buffers"] -static mut COMS_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue< - ComsUartModule, - ComsUartRxDma, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut COMS_BUFFERS_RX }); +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -100,29 +95,27 @@ fn get_empty_telem_packet() -> KickerTelemetry { #[embassy_executor::task] async fn high_pri_kick_task( coms_reader: &'static UartReadQueue< - 'static, ComsUartModule, ComsUartRxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, >, coms_writer: &'static UartWriteQueue< - 'static, ComsUartModule, ComsUartTxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, >, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC>, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, charge_pin: ChargePin, kick_pin: KickPin, chip_pin: ChipPin, breakbeam_tx: BreakbeamTxPin, breakbeam_rx: BreakbeamRxPin, - mut rail_pin: HighVoltageReadPin, - mut battery_voltage_pin: BatteryVoltageReadPin, + mut rail_pin: PowerRail200vReadPin, err_led_pin: RedStatusLedPin, - ball_detected_led_pin: BlueStatusLedPin, + ball_detected_led1_pin: BlueStatusLed1Pin, + ball_detected_led2_pin: BlueStatusLed2Pin, ) -> ! { // pins/safety management let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); @@ -132,7 +125,9 @@ async fn high_pri_kick_task( // debug LEDs let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); + // TODO dotstars let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); @@ -158,13 +153,14 @@ async fn high_pri_kick_task( breakbeam.enable_tx(); loop { - let mut vrefint = adc.enable_vref(&mut Delay); - let vrefint_sample = adc.read_internal(&mut vrefint); + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint); - let rail_voltage = adc_v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); - let battery_voltage = - adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + // let battery_voltage = + // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); // optionally pre-flag errors? + let battery_voltage = 22.5; ///////////////////////////////////// // process any available packets // @@ -376,9 +372,12 @@ async fn high_pri_kick_task( } if ball_detected { - ball_detected_led.set_high(); + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); + } else { - ball_detected_led.set_low(); + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); } // TODO Dotstar @@ -391,7 +390,7 @@ static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[interrupt] -unsafe fn I2C1() { +unsafe fn TIM2() { EXECUTOR_HIGH.on_interrupt(); } @@ -401,41 +400,24 @@ bind_interrupts!(struct Irqs { #[entry] fn main() -> ! { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.sys_ck = Some(mhz(48)); - stm32_config.rcc.hclk = Some(mhz(48)); - stm32_config.rcc.pclk = Some(mhz(48)); - - let p = embassy_stm32::init(stm32_config); + let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_cfg); info!("kicker startup!"); - let mut nvic: NVIC = unsafe { mem::transmute(()) }; - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - let mut adc = Adc::new(p.ADC, &mut Delay); - adc.set_sample_time(SampleTime::Cycles71_5); + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); // high priority executor handles kicking system // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY - unsafe { nvic.set_priority(Interrupt::I2C1, 6 << 4) }; - let spawner = EXECUTOR_HIGH.start(Interrupt::I2C1); - unwrap!(spawner.spawn(high_pri_kick_task( - &COMS_QUEUE_RX, - &COMS_QUEUE_TX, - adc, - p.PB3, - p.PB0, - p.PB1, - p.PA3, - p.PA2, - p.PA0, - p.PA1, - p.PA12, - p.PA8 - ))); + embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); + let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA3, p.PA2, p.PC0, p.PE1, p.PE2, p.PE3))); + ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -451,12 +433,12 @@ fn main() -> ! { p.PA10, p.PA9, Irqs, - p.DMA1_CH2, - p.DMA1_CH3, + p.DMA2_CH7, + p.DMA2_CH2, coms_uart_config, - ); + ).unwrap(); - let (coms_uart_tx, coms_uart_rx) = coms_usart.split(); + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); // low priority executor handles coms and user IO // Low priority executor: runs in thread mode, using WFE/SEV diff --git a/kicker-board/src/drivers/breakbeam.rs b/kicker-board/src/drivers/breakbeam.rs index ae58ce90..a109ace3 100644 --- a/kicker-board/src/drivers/breakbeam.rs +++ b/kicker-board/src/drivers/breakbeam.rs @@ -1,12 +1,12 @@ -use embassy_stm32::gpio::{Output, Input, Level, Speed, Pin, Pull}; +use embassy_stm32::{gpio::{Input, Level, Output, Pin, Pull, Speed}, Peripheral}; -pub struct Breakbeam<'a, PinTx: Pin, PinRx: Pin> { - pin_tx: Output<'a, PinTx>, - pin_rx: Input<'a, PinRx>, +pub struct Breakbeam<'a> { + pin_tx: Output<'a>, + pin_rx: Input<'a>, } -impl<'a, PinTx: Pin, PinRx: Pin> Breakbeam<'a, PinTx, PinRx> { - pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { +impl<'a> Breakbeam<'a> { + pub fn new(pin_tx: impl Peripheral

+ 'a, pin_rx: impl Peripheral

+ 'a) -> Self { let pin_tx = Output::new(pin_tx, Level::High, Speed::Low); let pin_rx = Input::new(pin_rx, Pull::Down); Self { diff --git a/kicker-board/src/drivers/breakbeam_pwm.rs b/kicker-board/src/drivers/breakbeam_pwm.rs index 63d9ceb0..c6179c2d 100644 --- a/kicker-board/src/drivers/breakbeam_pwm.rs +++ b/kicker-board/src/drivers/breakbeam_pwm.rs @@ -1,21 +1,28 @@ -use embassy_stm32::gpio::{Output, Input, Pin, Pull}; -use embassy_stm32::pwm::simple_pwm::{PwmPin, SimplePwm}; -use embassy_stm32::pwm::Channel; +use embassy_stm32::gpio::{Input, Output, OutputType, Pin, Pull}; +use embassy_stm32::timer::{ + GeneralInstance4Channel, + Channel, + simple_pwm::{ + PwmPin, + SimplePwm}}; use embassy_stm32::time::khz; +use embassy_stm32::Peripheral; use embassy_time::{Duration, Timer}; -pub struct Breakbeam<'a, PwmTx: Pin, PinRx: Pin> { - pin_tx: SimplePwm<'a, PwmTx>, - pin_rx: Input<'a, PinRx>, +pub struct Breakbeam<'a, Timer: GeneralInstance4Channel> { + pin_tx: SimplePwm<'a, Timer>, + pin_rx: Input<'a>, } -impl<'a, PinTx: Pin, PinRx: Pin, Timer: Peripheral> Breakbeam<'a, PinTx, PinRx, > { - pub fn new(pin_tx: PinTx, pin_rx: PinRx) -> Self { - let ch_1 = PwmPin::new_ch1(pin_tx); +impl<'a, Timer: GeneralInstance4Channel> Breakbeam<'a, Timer> { + pub fn new(pin_tx: PwmPin<'a, Timer, Channel>, pin_rx: impl Peripheral

+ 'a) -> Self { + let ch_1 = PwmPin::new_ch1(pin_tx, OutputType::PushPull); let mut pwm_tx = SimplePwm::new(pin_tx, Level::High, Speed::Low); + let pin_rx = Input::new(pin_rx, Pull::Down); + Self { - pwm_tx, + pin_tx: pwm_tx, pin_rx } } diff --git a/kicker-board/src/drivers/mod.rs b/kicker-board/src/drivers/mod.rs index c69230cd..c5f5cce8 100644 --- a/kicker-board/src/drivers/mod.rs +++ b/kicker-board/src/drivers/mod.rs @@ -1 +1,2 @@ +// pub mod breakbeam_pwm; pub mod breakbeam; \ No newline at end of file diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index 02c0bb8f..b5a88894 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -22,7 +22,6 @@ * is in an active state. */ -use crate::pins::{ChargePin, KickPin, ChipPin}; use embassy_stm32::gpio::Output; use embassy_time::{Duration, Timer}; use libm::{fmaxf, fminf}; @@ -47,9 +46,9 @@ pub enum KickType { pub struct KickManager<'a> { // external interface - charge_pin: Output<'a, ChargePin>, - kick_pin: Output<'a, KickPin>, - chip_pin: Output<'a, ChipPin>, + charge_pin: Output<'a>, + kick_pin: Output<'a>, + chip_pin: Output<'a>, // record keeping error_latched: bool, @@ -57,9 +56,9 @@ pub struct KickManager<'a> { impl<'a> KickManager<'a> { pub fn new( - charge_pin: Output<'a, ChargePin>, - kick_pin: Output<'a, KickPin>, - chip_pin: Output<'a, ChipPin>, + charge_pin: Output<'a>, + kick_pin: Output<'a>, + chip_pin: Output<'a>, ) -> KickManager<'a> { KickManager { charge_pin, diff --git a/kicker-board/src/lib.rs b/kicker-board/src/lib.rs index a50b10ae..7eebe820 100644 --- a/kicker-board/src/lib.rs +++ b/kicker-board/src/lib.rs @@ -1,17 +1,19 @@ #![no_std] #![feature(const_mut_refs)] #![feature(const_fn_floating_point_arithmetic)] -#![feature(async_fn_in_trait)] #![feature(type_alias_impl_trait)] #![feature(maybe_uninit_slice)] #![feature(maybe_uninit_uninit_array)] #![feature(const_maybe_uninit_uninit_array)] +#![feature(sync_unsafe_cell)] -pub mod pins; -pub mod kick_manager; -pub mod queue; -pub mod uart_queue; pub mod drivers; +pub mod tasks; + +pub mod kick_manager; +pub mod pins; +// pub mod queue; +// pub mod uart_queue; pub const ADC_VREFINT_NOMINAL: f32 = 1230.0; // mV @@ -23,6 +25,22 @@ pub const fn adc_v_to_battery_voltage(adc_mv: f32) -> f32 { (adc_mv / 1.65) * 25.2 } -pub const fn adc_v_to_rail_voltage(adc_mv: f32) -> f32 { +pub const fn adc_200v_to_rail_voltage(adc_mv: f32) -> f32 { adc_mv * 200.0 } + +pub const fn adc_12v_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 12.0 +} + +pub const fn adc_6v2_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 6.2 +} + +pub const fn adc_5v0_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv * 5.0 +} + +pub const fn adc_3v3_to_rail_voltage(adc_mv: f32) -> f32 { + adc_mv / 1.25 * 3.3 +} \ No newline at end of file diff --git a/kicker-board/src/pins.rs b/kicker-board/src/pins.rs index 050aceff..e4c79be6 100644 --- a/kicker-board/src/pins.rs +++ b/kicker-board/src/pins.rs @@ -1,27 +1,35 @@ -use embassy_stm32::peripherals::{ - PA0, PA1, PA2, PA3, PA8, PA9, PA10, PA11, PA12, - PB0, PB1, PB3, PB4, PB5, - USART1, - DMA1_CH2, DMA1_CH3, -}; +use embassy_stm32::peripherals::*; -pub type KickPin = PB0; -pub type ChipPin = PB1; -pub type ChargePin = PB3; -pub type RegulatorDonePin = PB4; -pub type RegulatorFaultPin = PB5; +pub type ChargePin = PE4; +pub type KickPin = PE5; +pub type ChipPin = PE6; + +pub type PowerRailAdc = ADC1; +pub type PowerRail200vReadPin = PC0; +pub type PowerRail12vReadPin = PC1; +pub type PowerRail6v2ReadPin = PC3; +pub type PowerRail5v0ReadPin = PC2; -pub type HighVoltageReadPin = PA0; -pub type BatteryVoltageReadPin = PA1; pub type BreakbeamTxPin = PA3; pub type BreakbeamRxPin = PA2; -pub type BlueStatusLedPin = PA8; -pub type GreenStatusLedPin = PA11; -pub type RedStatusLedPin = PA12; +pub type GreenStatusLedPin = PE0; +pub type RedStatusLedPin = PE1; +pub type BlueStatusLed1Pin = PE2; +pub type BlueStatusLed2Pin = PE3; + +pub type DotstarSpi = SPI1; +pub type DotstarSpiSckPin = PA5; +pub type DotstarSpiMosiPin = PA7; +pub type DotstarSpiTxDma = DMA2_CH3; + +pub type UserBtnPin = PD4; +pub type PowerBtnIntPin = PD5; +pub type PowerBtnIntExti = EXTI5; +pub type PowerKillPin = PD6; pub type ComsUartModule = USART1; pub type ComsUartTxPin = PA9; pub type ComsUartRxPin = PA10; -pub type ComsUartTxDma = DMA1_CH2; -pub type ComsUartRxDma = DMA1_CH3; \ No newline at end of file +pub type ComsUartTxDma = DMA2_CH7; +pub type ComsUartRxDma = DMA2_CH2; \ No newline at end of file diff --git a/kicker-board/src/queue.rs b/kicker-board/src/queue.rs deleted file mode 100644 index 8328e5ac..00000000 --- a/kicker-board/src/queue.rs +++ /dev/null @@ -1,220 +0,0 @@ -use core::{ - cell::UnsafeCell, - future::poll_fn, - mem::MaybeUninit, - task::{Poll, Waker}, -}; -use critical_section; - -pub struct Buffer { - pub data: [MaybeUninit; LENGTH], - pub len: MaybeUninit, -} - -impl Buffer { - pub const EMPTY: Buffer = Buffer { - data: MaybeUninit::uninit_array(), - len: MaybeUninit::uninit(), - }; -} - -pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, - data: &'a [u8], -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> DequeueRef<'a, LENGTH, DEPTH> { - pub fn data(&self) -> &[u8] { - self.data - } - pub fn cancel(self) { - self.queue.cancel_dequeue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_dequeue(); - } -} - -pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, - data: &'a mut [u8], - len: &'a mut usize, -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> { - pub fn data(&mut self) -> &mut [u8] { - self.data - } - - pub fn len(&mut self) -> &mut usize { - self.len - } - pub fn cancel(self) { - self.queue.cancel_enqueue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for EnqueueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_enqueue(); - } -} - -#[derive(PartialEq, Eq, Clone, Copy, Debug, defmt::Format)] -pub enum Error { - QueueFullEmpty, - InProgress, -} - -pub struct Queue<'a, const LENGTH: usize, const DEPTH: usize> { - buffers: &'a [Buffer; DEPTH], - read_index: UnsafeCell, - read_in_progress: UnsafeCell, - write_index: UnsafeCell, - write_in_progress: UnsafeCell, - size: UnsafeCell, - enqueue_waker: UnsafeCell>, - dequeue_waker: UnsafeCell>, -} - -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue<'a, LENGTH, DEPTH> {} -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue<'a, LENGTH, DEPTH> {} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - buffers, - read_index: UnsafeCell::new(0), - read_in_progress: UnsafeCell::new(false), - write_index: UnsafeCell::new(0), - write_in_progress: UnsafeCell::new(false), - size: UnsafeCell::new(0), - enqueue_waker: UnsafeCell::new(None), - dequeue_waker: UnsafeCell::new(None), - } - } - - pub fn try_dequeue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { - if *self.read_in_progress.get() { - return Err(Error::InProgress); - } - - if *self.size.get() > 0 { - *self.read_in_progress.get() = true; - let buf = &self.buffers[*self.read_index.get()]; - let len = buf.len.assume_init(); - let data = &MaybeUninit::slice_assume_init_ref(&buf.data)[..len]; - Ok(DequeueRef { queue: self, data }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn dequeue(&self) -> Result, Error> { - // TODO: look at this (when uncommented causes issue cancelling dequeue) - // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { - // return Err(Error::InProgress); - // } - - poll_fn(|cx| { - critical_section::with(|_| unsafe { - match self.try_dequeue() { - Err(Error::QueueFullEmpty) => { - *self.dequeue_waker.get() = Some(cx.waker().clone()); - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_dequeue(&self) { - critical_section::with(|_| unsafe { - *self.read_in_progress.get() = false; - }); - } - - fn finish_dequeue(&self) { - critical_section::with(|_| unsafe { - let read_in_progress = self.read_in_progress.get(); - if *read_in_progress { - *read_in_progress = false; - *self.read_index.get() = (*self.read_index.get() + 1) % DEPTH; - *self.size.get() -= 1; - } - if let Some(w) = (*self.enqueue_waker.get()).take() { - w.wake(); - } - }); - } - - pub fn try_enqueue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { - if *self.write_in_progress.get() { - return Err(Error::InProgress); - } - - if *self.size.get() < DEPTH { - *self.write_in_progress.get() = true; - let buf = &self.buffers[*self.write_index.get()]; - let buf = &mut *(buf as *const _ as *mut Buffer); - let data = MaybeUninit::slice_assume_init_mut(&mut buf.data); - let len = buf.len.write(0); - - Ok(EnqueueRef { - queue: self, - data, - len: len, - }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn enqueue(&self) -> Result, Error> { - if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { - return Err(Error::InProgress); - } - - poll_fn(|cx| { - critical_section::with(|_| unsafe { - match self.try_enqueue() { - Err(Error::QueueFullEmpty) => { - *self.enqueue_waker.get() = Some(cx.waker().clone()); - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_enqueue(&self) { - critical_section::with(|_| unsafe { - *self.write_in_progress.get() = false; - }); - } - - fn finish_enqueue(&self) { - critical_section::with(|_| unsafe { - let write_in_progress = self.write_in_progress.get(); - if *write_in_progress { - *write_in_progress = false; - *self.write_index.get() = (*self.write_index.get() + 1) % DEPTH; - *self.size.get() += 1; - } - if let Some(w) = (*self.dequeue_waker.get()).take() { - w.wake(); - } - }); - } -} diff --git a/kicker-board-v3/src/tasks/mod.rs b/kicker-board/src/tasks/mod.rs similarity index 100% rename from kicker-board-v3/src/tasks/mod.rs rename to kicker-board/src/tasks/mod.rs diff --git a/kicker-board/src/uart_queue.rs b/kicker-board/src/uart_queue.rs deleted file mode 100644 index 86c5c254..00000000 --- a/kicker-board/src/uart_queue.rs +++ /dev/null @@ -1,206 +0,0 @@ -use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; - -use core::future::Future; -use defmt::info; -use embassy_executor::{raw::TaskStorage, SpawnToken}; -use embassy_stm32::{ - usart::{self, UartRx, UartTx}, -}; - -pub struct UartReadQueue< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_rx: Queue<'a, LENGTH, DEPTH>, - task: TaskStorage>, -} - -// TODO: pretty sure shouldn't do this -unsafe impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > Send for UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> -{ -} - -pub type ReadTaskFuture< - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> -{ - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - queue_rx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn read_task( - queue_rx: &'static Queue<'a, LENGTH, DEPTH>, - mut rx: UartRx<'a, UART, DMA>, - // mut int: UART::Interrupt, - ) -> ReadTaskFuture { - async move { - loop { - let mut buf = queue_rx.enqueue().await.unwrap(); - let len = rx - .read_until_idle(buf.data()) - .await; - // .unwrap(); - // TODO: this - if let Ok(len) = len { - if len == 0 { - info!("uart zero"); - buf.cancel(); - } else { - *buf.len() = len; - } - } else { - info!("{}", len); - buf.cancel(); - } - } - } - } - - pub fn spawn_task( - &'static self, - rx: UartRx<'a, UART, DMA>, - ) -> SpawnToken { - self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) - } - - pub fn try_dequeue(&self) -> Result, Error> { - return self.queue_rx.try_dequeue(); - } - - pub async fn dequeue(&self, fn_write: impl FnOnce(&[u8]) -> RET) -> RET { - let buf = self.queue_rx.dequeue().await.unwrap(); - fn_write(buf.data()) - } -} - -pub struct UartWriteQueue< - 'a, - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_tx: Queue<'a, LENGTH, DEPTH>, - task: TaskStorage>, -} - -type WriteTaskFuture< - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartWriteQueue<'a, UART, DMA, LENGTH, DEPTH> -{ - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - queue_tx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn write_task( - queue_tx: &'static Queue<'a, LENGTH, DEPTH>, - mut tx: UartTx<'a, UART, DMA>, - ) -> WriteTaskFuture { - async move { - loop { - let buf = queue_tx.dequeue().await.unwrap(); - // defmt::info!("invoking API write"); - tx.write(buf.data()).await.unwrap(); // we are blocked here! - // defmt::info!("passed API write"); - - drop(buf); - // unsafe { - // // TODO: what does this do again? - // while !UART::regs().isr().read().tc() {} - // UART::regs().cr1().modify(|w| w.set_te(false)); - // while UART::regs().isr().read().teack() {} - // UART::regs().cr1().modify(|w| w.set_te(true)); - // } - } - } - } - - pub fn spawn_task(&'static self, tx: UartTx<'a, UART, DMA>) -> SpawnToken { - self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) - } - - pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { - let mut buf = self.queue_tx.try_enqueue()?; - let len = fn_write(buf.data()); - *buf.len() = len; - Ok(()) - } - - pub fn enqueue_copy(&self, source: &[u8]) -> Result<(), queue::Error> { - self.enqueue(|dest| { - dest[..source.len()].copy_from_slice(source); - source.len() - }) - } -} - -pub trait Reader { - async fn read RET>(&self, fn_read: FN) -> Result; -} - -pub trait Writer { - async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; -} - -impl< - 'a, - UART: usart::BasicInstance, - Dma: usart::RxDma, - const LEN: usize, - const DEPTH: usize, - > Reader for crate::uart_queue::UartReadQueue<'a, UART, Dma, LEN, DEPTH> -{ - async fn read RET>(&self, fn_read: FN) -> Result { - Ok(self.dequeue(|buf| fn_read(buf)).await) - } -} - -impl< - 'a, - UART: usart::BasicInstance, - Dma: usart::TxDma, - const LEN: usize, - const DEPTH: usize, - > Writer for crate::uart_queue::UartWriteQueue<'a, UART, Dma, LEN, DEPTH> -{ - async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { - self.enqueue(|buf| fn_write(buf)).or(Err(())) - } -} diff --git a/lib-stm32-test/.cargo/config.toml b/lib-stm32-test/.cargo/config.toml index 2c1b4189..b56b74be 100644 --- a/lib-stm32-test/.cargo/config.toml +++ b/lib-stm32-test/.cargo/config.toml @@ -2,7 +2,7 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] -runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' +runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' # runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' [env] diff --git a/kicker-board-v3/build.rs b/lib-stm32-test/build.rs similarity index 100% rename from kicker-board-v3/build.rs rename to lib-stm32-test/build.rs diff --git a/lib-stm32-test/memory.x b/lib-stm32-test/memory.x new file mode 100644 index 00000000..37a5dd85 --- /dev/null +++ b/lib-stm32-test/memory.x @@ -0,0 +1,45 @@ +/* Linker for STM32H743ZI */ + +MEMORY +{ + FLASH : ORIGIN = 0x08000000, LENGTH = 2M + + /* DTCM */ + RAM : ORIGIN = 0x20000000, LENGTH = 128K + + /* AXISRAM */ + AXISRAM : ORIGIN = 0x24000000, LENGTH = 512K + + /* SRAM */ + SRAM1 : ORIGIN = 0x30000000, LENGTH = 128K + SRAM2 : ORIGIN = 0x30020000, LENGTH = 128K + SRAM3 : ORIGIN = 0x30040000, LENGTH = 32K + SRAM4 : ORIGIN = 0x38000000, LENGTH = 64K + + /* Backup SRAM */ + BSRAM : ORIGIN = 0x38800000, LENGTH = 4K + + /* Instruction TCM */ + ITCM : ORIGIN = 0x00000000, LENGTH = 64K +} + +_stack_start = ORIGIN(RAM) + LENGTH(RAM); + +SECTIONS { + .axisram (NOLOAD) : ALIGN(8) { + *(.axisram .axisram.*); + . = ALIGN(8); + } > AXISRAM + .axisram_prog : ALIGN(8) { + *(.axisram_prog.*); + . = ALIGN(8); + } > AXISRAM + .sram3 (NOLOAD) : ALIGN(4) { + *(.sram3 .sram3.*); + . = ALIGN(4); + } > SRAM3 + .sram4 (NOLOAD) : ALIGN(4) { + *(.sram4 .sram4.*); + . = ALIGN(4); + } > SRAM4 +}; \ No newline at end of file diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs index bf24adef..93a98d4d 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs @@ -8,9 +8,9 @@ use core::{ }; use embassy_stm32::{ - bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, peripherals::{self, *}, usart::{self, *} + bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, *}, usart::{self, *} }; -use embassy_executor::Executor; +use embassy_executor::{Executor, InterruptExecutor}; use embassy_time::Timer; use defmt::*; @@ -32,9 +32,9 @@ type UserBtnPin = PC13; type UserBtnExti = EXTI13; const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 10; +const TX_BUF_DEPTH: usize = 5; const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 10; +const RX_BUF_DEPTH: usize = 5; // control communications tx buffer const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = @@ -123,7 +123,7 @@ async fn handle_btn_press(usr_btn_pin: UserBtnPin, let mut usr_btn = ExtiInput::new(usr_btn_pin, usr_btn_exti, Pull::Down); - let mut green_led = Output::new(led_green_pin, Level::Low, Speed::Medium); + let mut green_led = Output::new(led_green_pin, Level::High, Speed::Medium); let mut yellow_led = Output::new(led_yellow_pin, Level::Low, Speed::Medium); let mut red_led = Output::new(led_red_pin, Level::Low, Speed::Medium); @@ -142,11 +142,11 @@ async fn handle_btn_press(usr_btn_pin: UserBtnPin, if cur_loop_rate == 100 { new_loop_rate = 10; - red_led.set_high(); + yellow_led.set_high(); } else if cur_loop_rate == 10 { new_loop_rate = 1; - yellow_led.set_high(); + red_led.set_high(); } else { green_led.set_high(); } @@ -154,8 +154,14 @@ async fn handle_btn_press(usr_btn_pin: UserBtnPin, } } +static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); static EXECUTOR_LOW: StaticCell = StaticCell::new(); +#[interrupt] +unsafe fn TIM2() { + EXECUTOR_HIGH.on_interrupt(); +} + bind_interrupts!(struct Irqs { USART2 => usart::InterruptHandler; }); @@ -166,6 +172,12 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let stm32_config: embassy_stm32::Config = Default::default(); let p = embassy_stm32::init(stm32_config); + // high priority executor handles kicking system + // High-priority executor: I2C1, priority level 6 + // TODO CHECK THIS IS THE HIGHEST PRIORITY + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::TIM2, embassy_stm32::interrupt::Priority::P6); + let high_pri_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // ////////////////////////////////// @@ -187,13 +199,17 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + unwrap!(high_pri_spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + unwrap!(high_pri_spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + + // MIGHT should put queues in mix prio, this could elicit the bug // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + // unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + // unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); unwrap!(spawner.spawn(rx_task(&COMS_QUEUE_RX))); unwrap!(spawner.spawn(tx_task(&COMS_QUEUE_TX))); }); diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index 3929e355..dad54dc7 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -51,13 +51,28 @@ impl<'buf, T: spi::Instance, const CS_POL_N: bool> AT25DF041B<'buf, T, CS_POL_N> self.select(); self.rx_buf[0] = 0x9F; + self.select(); let res = self.transfer(5).await; // send cmd byte, get 4 back + self.deselect(); if res.is_err() { return Err(()); } - + // let mfg_id = self.rx_buf[1]; + // let dev_id_upper = self.rx_buf[2]; + // let dev_id_lower = self.rx_buf[3]; + // let dev_edis_len = self.rx_buf[4]; + + let expected_mfg_info: [u8; 4] = [0x1F, 0x44, 0x02, 0x00]; + let mut received_mfg_info: [u8; 4] = [0; 4]; + received_mfg_info.copy_from_slice(&self.rx_buf[1..5]); + + if received_mfg_info != expected_mfg_info { + defmt::error!("AT25DF041B manufacturer information did not match datasheet."); + defmt::debug!("expected {}, got {}", expected_mfg_info, received_mfg_info); + return Err(()) + } - return Err(()) + return Ok(()) } } \ No newline at end of file diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 78ebd182..2466505d 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -4,6 +4,8 @@ #![feature(type_alias_impl_trait)] #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] +#![feature(unsized_locals)] + pub mod drivers; pub mod uart; From fc669bdd79c5956b2c784c94b7be55b129031291 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 4 May 2024 21:26:33 -0400 Subject: [PATCH 016/157] complete kicker pinout --- kicker-board/src/bin/kicker/main.rs | 10 +-- kicker-board/src/pins.rs | 114 +++++++++++++++++++++++++++- 2 files changed, 116 insertions(+), 8 deletions(-) diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 1f79715c..17032f37 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -5,7 +5,7 @@ #![feature(sync_unsafe_cell)] use core::cell::SyncUnsafeCell; -use ateam_kicker_board::{drivers::breakbeam::Breakbeam, tasks::get_system_config}; +use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::get_system_config}; use static_cell::StaticCell; use defmt::*; @@ -33,7 +33,7 @@ use ateam_kicker_board::{ kick_manager::{KickManager, KickType}, pins::{ BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, ComsUartModule, ComsUartRxDma, - ComsUartTxDma, PowerRail200vReadPin, KickPin, RedStatusLedPin, BreakbeamTxPin, BreakbeamRxPin, + ComsUartTxDma, PowerRail200vReadPin, KickPin, RedStatusLedPin, }, }; @@ -110,8 +110,8 @@ async fn high_pri_kick_task( charge_pin: ChargePin, kick_pin: KickPin, chip_pin: ChipPin, - breakbeam_tx: BreakbeamTxPin, - breakbeam_rx: BreakbeamRxPin, + breakbeam_tx: BreakbeamLeftAgpioPin, + breakbeam_rx: BreakbeamRightAgpioPin, mut rail_pin: PowerRail200vReadPin, err_led_pin: RedStatusLedPin, ball_detected_led1_pin: BlueStatusLed1Pin, @@ -416,7 +416,7 @@ fn main() -> ! { // TODO CHECK THIS IS THE HIGHEST PRIORITY embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA3, p.PA2, p.PC0, p.PE1, p.PE2, p.PE3))); + unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// diff --git a/kicker-board/src/pins.rs b/kicker-board/src/pins.rs index e4c79be6..60450331 100644 --- a/kicker-board/src/pins.rs +++ b/kicker-board/src/pins.rs @@ -1,17 +1,64 @@ use embassy_stm32::peripherals::*; +//////////////////// +// HV Functions // +//////////////////// + pub type ChargePin = PE4; pub type KickPin = PE5; pub type ChipPin = PE6; + +//////////////////////////// +// Voltage Measurements // +//////////////////////////// + pub type PowerRailAdc = ADC1; pub type PowerRail200vReadPin = PC0; +pub type PowerRailVBattReadPin = PA6; pub type PowerRail12vReadPin = PC1; pub type PowerRail6v2ReadPin = PC3; pub type PowerRail5v0ReadPin = PC2; +pub type PowerRail3v3ReadPin = PA4; +pub type PowerRailAdcDma = DMA2_CH4; + + +////////////////////// +// Breakbeam Pins // +////////////////////// + +pub type BreakbeamLeftI2c = I2C3; +pub type BreakbeamLeftAgpioPin = PA1; +pub type BreakbeamLeftSdaPin = PC9; +pub type BreakbeamLeftSclPin = PA8; +pub type BreakbeamLeftNdetPin = PC6; +pub type BreakbeamLeftRstPin = PC7; +pub type BreakbeamLeftIntPin = PC8; +pub type BreakbeamLeftI2cRxDma = DMA1_CH2; +pub type BreakbeamLeftI2cTxDma = DMA1_CH4; -pub type BreakbeamTxPin = PA3; -pub type BreakbeamRxPin = PA2; +pub type BreakbeamCenterI2c = I2C2; +pub type BreakbeamCenterAgpioPin = PC5; +pub type BreakbeamCenterSdaPin = PB11; +pub type BreakbeamCenterSclPin = PB10; +pub type BreakbeamCenterNdetPin = PE13; +pub type BreakbeamCenterRstPin = PE14; +pub type BreakbeamCenterIntPin = PE15; + +pub type BreakbeamRightI2c = I2C1; +pub type BreakbeamRightAgpioPin = PA0; +pub type BreakbeamRightSdaPin = PB7; +pub type BreakbeamRightSclPin = PB6; +pub type BreakbeamRightNdetPin = PB3; +pub type BreakbeamRightRstPin = PB4; +pub type BreakbeamRightIntPin = PB5; +pub type BreakbeamRightI2cRxDma = DMA1_CH0; +pub type BreakbeamRightI2cTxDma = DMA1_CH7; + + +/////////////// +// User IO // +/////////////// pub type GreenStatusLedPin = PE0; pub type RedStatusLedPin = PE1; @@ -21,6 +68,7 @@ pub type BlueStatusLed2Pin = PE3; pub type DotstarSpi = SPI1; pub type DotstarSpiSckPin = PA5; pub type DotstarSpiMosiPin = PA7; +pub type DotstarSpiRxDma = DMA2_CH0; pub type DotstarSpiTxDma = DMA2_CH3; pub type UserBtnPin = PD4; @@ -28,8 +76,68 @@ pub type PowerBtnIntPin = PD5; pub type PowerBtnIntExti = EXTI5; pub type PowerKillPin = PD6; +pub type Dip0Pin = PE9; +pub type Dip1Pin = PE10; +pub type Dip2Pin = PE11; +pub type Dip3Pin = PE12; + +pub type TrimPot0Pin = PB0; +pub type TrimPot1Pin = PB1; + +pub type DbgHdr1Pin = PB8; +pub type DbgHdr2Pin = PB9; + +pub type SwdSwclkPin = PA14; +pub type SwdSwdioPin = PA13; + + +////////////////////////////// +// Control Communications // +////////////////////////////// + pub type ComsUartModule = USART1; pub type ComsUartTxPin = PA9; pub type ComsUartRxPin = PA10; pub type ComsUartTxDma = DMA2_CH7; -pub type ComsUartRxDma = DMA2_CH2; \ No newline at end of file +pub type ComsUartRxDma = DMA2_CH2; + + +/////////////////// +// Peripherals // +/////////////////// + +pub type UsbOtgFsDNPin = PA11; +pub type UsbOtgFsDPPin = PA12; + +pub type FlashSpi = SPI2; +pub type FlashSpiSckPin = PB13; +pub type FlashSpiMosiPin = PB15; +pub type FlashSpiMisoPin = PB14; +pub type FlashSpiNssPin = PB12; + +pub type AuxSpi = SPI3; +pub type AuxSpiSckPin = PC10; +pub type AuxSpiMosiPin = PC12; +pub type AuxSpiMisoPin = PC11; +pub type AuxSpiNssPin = PA15; + +pub type BallSenseUart = USART2; +pub type BallSenseUartRxPin = PA3; +pub type BallSenseUartTxPin = PA2; +pub type BallSenseBoot0Pin = PE7; +pub type BallSenseRstPin = PE8; +pub type BallSenseNdetPin = PB2; +pub type BallSenseUartRxDma = DMA1_CH5; +pub type BallSenseUartTxDma = DMA1_CH6; + +pub type DribblerUart = USART3; +pub type DribblerUartRxPin = PD9; +pub type DribblerUartTxPin = PD8; +pub type DribblerBoot0Pin = PD10; +pub type DribblerRstPin = PD11; +pub type DribblerNdetPin = PD12; +pub type DribblerUartRxDma = DMA1_CH1; +pub type DribblerUartTxDma = DMA1_CH3; + +pub type TempProbeReadPin = PC4; + From 0ea7e243d6ca4293028ee37a7968210a9eaf2aaf Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 5 May 2024 16:10:15 -0400 Subject: [PATCH 017/157] add some error checking to RA shim to warn in invalid environments --- util/rust-analyzer-shim.bash | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/util/rust-analyzer-shim.bash b/util/rust-analyzer-shim.bash index 0f481229..5364f238 100755 --- a/util/rust-analyzer-shim.bash +++ b/util/rust-analyzer-shim.bash @@ -6,4 +6,17 @@ # and the hashed path changes, or a user wants to use a specific version on # the path. This shell script shims the analyzer using the which command. +if ! command -v rust-analyzer; then + echo "Rust Analyzer Shim Script Error. There is no rust-analyzer on the path." + echo "Did you start VS code from the Nix environemnt? OR Is the rust cross compiler installed?" + exit 1 +fi + +if ! which rust-analyzer | grep -q "nix"; then + echo "Rust Analyzer Shim Script Error. rust-analyzer is on path but is not in the Nix store." + echo "This script is intended to support the Nix environment on Linux, Mac, and WSL2" + echo "If you are not using the Nix env, please modify the project .vscode/settings.json to set your own rust language server" + exit 1 +fi + exec $(which rust-analyzer) "$@" From 32e72aa0f01b78a60075fbd122c3b2df79edeef1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 5 May 2024 16:24:34 -0400 Subject: [PATCH 018/157] fix bug in RA shim --- util/rust-analyzer-shim.bash | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/util/rust-analyzer-shim.bash b/util/rust-analyzer-shim.bash index 5364f238..a439a582 100755 --- a/util/rust-analyzer-shim.bash +++ b/util/rust-analyzer-shim.bash @@ -6,13 +6,13 @@ # and the hashed path changes, or a user wants to use a specific version on # the path. This shell script shims the analyzer using the which command. -if ! command -v rust-analyzer; then +if ! command -v rust-analyzer 2>&1 > /dev/null; then echo "Rust Analyzer Shim Script Error. There is no rust-analyzer on the path." echo "Did you start VS code from the Nix environemnt? OR Is the rust cross compiler installed?" exit 1 fi -if ! which rust-analyzer | grep -q "nix"; then +if ! which rust-analyzer 2>&1 | grep -q "nix"; then echo "Rust Analyzer Shim Script Error. rust-analyzer is on path but is not in the Nix store." echo "This script is intended to support the Nix environment on Linux, Mac, and WSL2" echo "If you are not using the Nix env, please modify the project .vscode/settings.json to set your own rust language server" @@ -20,3 +20,4 @@ if ! which rust-analyzer | grep -q "nix"; then fi exec $(which rust-analyzer) "$@" + From 43b0e60eb48cab421e535702f6e5fb430dc73d99 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 5 May 2024 16:41:26 -0400 Subject: [PATCH 019/157] desugar async traits to resolve warnings --- lib-stm32-test/Cargo.toml | 1 - lib-stm32-test/src/bin/hwtest-uart-queue/main.rs | 1 + lib-stm32/src/lib.rs | 2 +- lib-stm32/src/uart/queue.rs | 4 ++-- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index 42842024..f82fd743 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -40,7 +40,6 @@ critical-section = "1.1.1" const_format = "0.2.30" heapless = "0.7.16" libm = "0.2.6" -# ateam-lib-stm32 = { path = "../lib-stm32", default-featues = false, feature = "stm32h723zg" } ateam-lib-stm32 = { path = "../lib-stm32" } diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs index 93a98d4d..90df2a9b 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs @@ -56,6 +56,7 @@ static COMS_QUEUE_RX: UartReadQueue RET>(&self, fn_read: FN) -> Result; + fn read RET>(&self, fn_read: FN) -> impl core::future::Future>; } pub trait Writer { - async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; + fn write usize>(&self, fn_write: FN) -> impl core::future::Future>; } impl< From a75e5e4daca35128f39ac564e8b657df9fd25d4a Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 5 May 2024 18:31:22 -0400 Subject: [PATCH 020/157] move all deps forward, resolve RA VS Code integration problems after updates --- common/Cargo.lock | 291 +++++++--- common/Cargo.toml | 3 +- control-board/Cargo.lock | 1027 +++++++++++++++------------------- control-board/Cargo.toml | 48 +- kicker-board/Cargo.lock | 146 ++--- kicker-board/Cargo.toml | 2 +- lib-stm32-test/Cargo.lock | 36 +- lib-stm32-test/Cargo.toml | 2 +- lib-stm32/.cargo/config.toml | 7 - lib-stm32/Cargo.lock | 36 +- lib-stm32/Cargo.toml | 4 + lib-stm32/src/lib.rs | 1 - 12 files changed, 795 insertions(+), 808 deletions(-) diff --git a/common/Cargo.lock b/common/Cargo.lock index cbcb7499..d511c7b3 100644 --- a/common/Cargo.lock +++ b/common/Cargo.lock @@ -4,9 +4,9 @@ version = 3 [[package]] name = "aho-corasick" -version = "0.7.20" +version = "1.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc936419f96fa211c1b9166887b38e5e40b19958e5b895be7c1f93adec7071ac" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" dependencies = [ "memchr", ] @@ -40,9 +40,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bindgen" @@ -50,7 +50,7 @@ version = "0.60.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" dependencies = [ - "bitflags", + "bitflags 1.3.2", "cexpr", "clang-sys", "clap", @@ -73,11 +73,17 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" +[[package]] +name = "bitflags" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" + [[package]] name = "byteorder" -version = "1.4.3" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cexpr" @@ -96,9 +102,9 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "clang-sys" -version = "1.6.0" +version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "77ed9a53e5d4d9c573ae844bfac6872b159cb1d1585a83b29e7a64b7eef7332a" +checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" dependencies = [ "glob", "libc", @@ -107,12 +113,12 @@ dependencies = [ [[package]] name = "clap" -version = "3.2.23" +version = "3.2.25" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "71655c45cb9845d3270c9d6df84ebe72b4dad3c2ba3f7023ad47c144e4e473a5" +checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" dependencies = [ "atty", - "bitflags", + "bitflags 1.3.2", "clap_lex", "indexmap", "strsim", @@ -131,9 +137,9 @@ dependencies = [ [[package]] name = "either" -version = "1.8.1" +version = "1.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" +checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" [[package]] name = "env_logger" @@ -148,6 +154,16 @@ dependencies = [ "termcolor", ] +[[package]] +name = "errno" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + [[package]] name = "glob" version = "0.3.1" @@ -169,6 +185,15 @@ dependencies = [ "libc", ] +[[package]] +name = "home" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +dependencies = [ + "windows-sys 0.52.0", +] + [[package]] name = "humantime" version = "2.1.0" @@ -177,9 +202,9 @@ checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" [[package]] name = "indexmap" -version = "1.9.2" +version = "1.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1885e79c1fc4b10f0e172c475f458b7f7b93061064d98c3293e98c5ba0c8b399" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", "hashbrown", @@ -199,20 +224,26 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.140" +version = "0.2.154" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "99227334921fae1a979cf0bfdfcc6b3e5ce376ef57e16fb6fb3ea2ed6095f80c" +checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" [[package]] name = "libloading" -version = "0.7.4" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" dependencies = [ "cfg-if", - "winapi", + "windows-targets", ] +[[package]] +name = "linux-raw-sys" +version = "0.4.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" + [[package]] name = "local-ip-address" version = "0.4.9" @@ -222,23 +253,20 @@ dependencies = [ "libc", "neli", "thiserror", - "windows-sys", + "windows-sys 0.42.0", ] [[package]] name = "log" -version = "0.4.17" +version = "0.4.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" -dependencies = [ - "cfg-if", -] +checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" [[package]] name = "memchr" -version = "2.5.0" +version = "2.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" [[package]] name = "minimal-lexical" @@ -268,15 +296,15 @@ dependencies = [ [[package]] name = "once_cell" -version = "1.17.1" +version = "1.19.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b7e5500299e16ebb147ae15a00a942af264cf3688f47923b8fc2cd5858f23ad3" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "os_str_bytes" -version = "6.4.1" +version = "6.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b7820b9daea5457c9f21c69448905d723fbd21136ccf521748f23fd49e723ee" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "peeking_take_while" @@ -286,27 +314,39 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" [[package]] name = "proc-macro2" -version = "1.0.52" +version = "1.0.81" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d0e1ae9e836cc3beddd63db0df682593d7e2d3d891ae8c9083d2113e1744224" +checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.24" +version = "1.0.36" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "50686e0021c4136d1d453b2dfe059902278681512a34d4248435dc34b6b5c8ec" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" dependencies = [ "proc-macro2", ] [[package]] name = "regex" -version = "1.7.1" +version = "1.10.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48aaa5748ba571fb95cd2c85c09f629215d3a6ece942baa100950af03a34f733" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" dependencies = [ "aho-corasick", "memchr", @@ -315,9 +355,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.6.28" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "456c603be3e8d448b072f410900c09faf164fbce2d480456f50eea6e25f9c848" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rustc-hash" @@ -325,11 +365,24 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" +[[package]] +name = "rustix" +version = "0.38.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" +dependencies = [ + "bitflags 2.5.0", + "errno", + "libc", + "linux-raw-sys", + "windows-sys 0.52.0", +] + [[package]] name = "shlex" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" [[package]] name = "strsim" @@ -339,9 +392,9 @@ checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" [[package]] name = "syn" -version = "1.0.109" +version = "2.0.60" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" dependencies = [ "proc-macro2", "quote", @@ -350,33 +403,33 @@ dependencies = [ [[package]] name = "termcolor" -version = "1.2.0" +version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "be55cf8942feac5c765c2c993422806843c9a9a45d4d5c407ad6dd2ea95eb9b6" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" dependencies = [ "winapi-util", ] [[package]] name = "textwrap" -version = "0.16.0" +version = "0.16.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "222a222a5bfe1bba4a77b45ec488a741b3cb8872e5e499451fd7d0129c9c7c3d" +checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.39" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a5ab016db510546d856297882807df8da66a16fb8c4101cb8b30054b0d5b2d9c" +checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.39" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5420d42e90af0c38c3290abcca25b9b3bdf379fc9f55c528f53a269d9c9a267e" +checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" dependencies = [ "proc-macro2", "quote", @@ -385,19 +438,20 @@ dependencies = [ [[package]] name = "unicode-ident" -version = "1.0.8" +version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e5464a87b239f13a63a501f2701565754bae92d243d4bb7eb12f6d57d2269bf4" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" [[package]] name = "which" -version = "4.4.0" +version = "4.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2441c784c52b289a054b7201fc93253e288f094e2f4be9058343127c4226a269" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" dependencies = [ "either", - "libc", + "home", "once_cell", + "rustix", ] [[package]] @@ -418,11 +472,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.5" +version = "0.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" dependencies = [ - "winapi", + "windows-sys 0.52.0", ] [[package]] @@ -437,53 +491,126 @@ version = "0.42.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5a3e1820f08b8513f676f7ab6c1f99ff312fb97b553d30ff4dd86f9f15728aa7" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "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", +] + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +dependencies = [ + "windows_aarch64_gnullvm 0.52.5", + "windows_aarch64_msvc 0.52.5", + "windows_i686_gnu 0.52.5", + "windows_i686_gnullvm", + "windows_i686_msvc 0.52.5", + "windows_x86_64_gnu 0.52.5", + "windows_x86_64_gnullvm 0.52.5", + "windows_x86_64_msvc 0.52.5", ] [[package]] name = "windows_aarch64_gnullvm" -version = "0.42.1" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "597a5118570b68bc08d8d59125332c54f1ba9d9adeedeef5b99b02ba2b0698f8" + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8c9864e83243fdec7fc9c5444389dcbbfd258f745e7853198f365e3c4968a608" +checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" [[package]] name = "windows_aarch64_msvc" -version = "0.42.1" +version = "0.42.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c8b1b673ffc16c47a9ff48570a9d85e25d265735c503681332589af6253c6c7" +checksum = "e08e8864a60f06ef0d0ff4ba04124db8b0fb3be5776a5cd47641e942e58c4d43" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" [[package]] name = "windows_i686_gnu" -version = "0.42.1" +version = "0.42.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3887528ad530ba7bdbb1faa8275ec7a1155a45ffa57c37993960277145d640" +checksum = "c61d927d8da41da96a81f029489353e68739737d3beca43145c8afec9a31a84f" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" [[package]] name = "windows_i686_msvc" -version = "0.42.1" +version = "0.42.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf4d1122317eddd6ff351aa852118a2418ad4214e6613a50e0191f7004372605" +checksum = "44d840b6ec649f480a41c8d80f9c65108b92d89345dd94027bfe06ac444d1060" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" [[package]] name = "windows_x86_64_gnu" -version = "0.42.1" +version = "0.42.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c1040f221285e17ebccbc2591ffdc2d44ee1f9186324dd3e84e99ac68d699c45" +checksum = "8de912b8b8feb55c064867cf047dda097f92d51efad5b491dfb98f6bbb70cb36" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" + +[[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.42.1" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.42.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "628bfdf232daa22b0d64fdb62b09fcc36bb01f05a3939e20ab73aaf9470d0463" +checksum = "9aec5da331524158c6d1a4ac0ab1541149c0b9505fde06423b02f5ef0106b9f0" [[package]] name = "windows_x86_64_msvc" -version = "0.42.1" +version = "0.52.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "447660ad36a13288b1db4d4248e857b510e8c3a225c822ba4fb748c0aafecffd" +checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" diff --git a/common/Cargo.toml b/common/Cargo.toml index de90d09c..bf824fcd 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -7,4 +7,5 @@ edition = "2021" [dependencies] ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } -local-ip-address = "0.4.9" \ No newline at end of file +local-ip-address = "0.4.9" + diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 9b1aa1c8..d9a07f05 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -4,19 +4,13 @@ version = 3 [[package]] name = "aho-corasick" -version = "0.7.19" +version = "1.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b4f55bd91a0978cbfd91c457a164bab8b4001c833b7f323132c0a4e1922dd44e" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" dependencies = [ "memchr", ] -[[package]] -name = "anyhow" -version = "1.0.64" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b9a8f622bcf6ff3df478e9deba3e03e4e04b300f8e6a139e192c05fa3490afc7" - [[package]] name = "apa102-spi" version = "0.3.2" @@ -50,11 +44,12 @@ version = "0.1.0" dependencies = [ "apa102-spi", "ateam-common-packets", + "ateam-lib-stm32", "const_format", "cortex-m", "cortex-m-rt", "credentials", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", "defmt-rtt", "defmt-test", @@ -64,7 +59,7 @@ dependencies = [ "embassy-sync", "embassy-time", "futures-util", - "heapless", + "heapless 0.7.17", "libm", "nalgebra", "panic-probe", @@ -73,21 +68,23 @@ dependencies = [ ] [[package]] -name = "atomic-polyfill" -version = "0.1.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c041a8d9751a520ee19656232a18971f18946a7900f1520ee4400002244dd89" +name = "ateam-lib-stm32" +version = "0.1.0" dependencies = [ - "critical-section 0.2.7", + "critical-section 1.1.2", + "defmt", + "defmt-rtt", + "embassy-executor", + "embassy-stm32", ] [[package]] name = "atomic-polyfill" -version = "1.0.1" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d299f547288d6db8d5c3a2916f7b2f66134b15b8c1ac1c4357dd3b8752af7bb2" +checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" dependencies = [ - "critical-section 1.1.1", + "critical-section 1.1.2", ] [[package]] @@ -103,9 +100,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -128,7 +125,7 @@ version = "0.60.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" dependencies = [ - "bitflags", + "bitflags 1.3.2", "cexpr", "clang-sys", "clap", @@ -147,9 +144,9 @@ dependencies = [ [[package]] name = "bit_field" -version = "0.10.1" +version = "0.10.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dcb6dd1c2376d2e096796e234a70e17e94cc2d5d54ff8ce42b28cef1d0d359a4" +checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61" [[package]] name = "bitfield" @@ -164,29 +161,22 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] -name = "bxcan" -version = "0.7.0" +name = "bitflags" +version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "40ac3d0c0a542d0ab5521211f873f62706a7136df415676f676d347e5a41dd80" -dependencies = [ - "bitflags", - "defmt", - "embedded-hal 0.2.7", - "nb 1.0.0", - "vcell", -] +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" [[package]] name = "bytemuck" -version = "1.13.1" +version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "17febce684fd15d89027105661fec94afb475cb995fbc59d2865198446ba2eea" +checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" [[package]] name = "byteorder" -version = "1.4.3" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "cexpr" @@ -204,28 +194,19 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] -name = "chiptool" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/chiptool?rev=1d9e0a39a6acc291e50cabc4ed617a87f06d5e89#1d9e0a39a6acc291e50cabc4ed617a87f06d5e89" +name = "chrono" +version = "0.4.38" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a21f936df1771bf62b77f047b726c4625ff2e8aa607c01ec06e5a05bd8463401" dependencies = [ - "anyhow", - "clap", - "env_logger", - "inflections", - "log", - "proc-macro2", - "quote", - "regex", - "serde", - "serde_yaml", - "svd-parser", + "num-traits", ] [[package]] name = "clang-sys" -version = "1.4.0" +version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa2e27ae6ab525c3d369ded447057bca5438d86dc3a68f6faafb8269ba82ebf3" +checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" dependencies = [ "glob", "libc", @@ -234,34 +215,19 @@ dependencies = [ [[package]] name = "clap" -version = "3.2.20" +version = "3.2.25" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23b71c3ce99b7611011217b366d923f1d0a7e07a92bb2dbf1e84508c673ca3bd" +checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" dependencies = [ "atty", - "bitflags", - "clap_derive", + "bitflags 1.3.2", "clap_lex", "indexmap", - "once_cell", "strsim", "termcolor", "textwrap", ] -[[package]] -name = "clap_derive" -version = "3.2.18" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ea0c8bce528c4be4da13ea6fead8965e95b6073585a2f05204bd8f4119f82a65" -dependencies = [ - "heck", - "proc-macro-error", - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "clap_lex" version = "0.2.4" @@ -273,18 +239,18 @@ dependencies = [ [[package]] name = "const_format" -version = "0.2.30" +version = "0.2.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7309d9b4d3d2c0641e018d449232f2e28f1b22933c137f157d3dbc14228b8c0e" +checksum = "e3a214c7af3d04997541b18d432afaff4c455e79e2029079647e72fc2bd27673" dependencies = [ "const_format_proc_macros", ] [[package]] name = "const_format_proc_macros" -version = "0.2.29" +version = "0.2.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d897f47bf7270cf70d370f8f98c1abb6d2d4cf60a6845d30e05bfb90c6568650" +checksum = "c7f6ff08fd20f4f299298a28e2dfa8a8ba1036e6cd2460ac1de7b425d76f2500" dependencies = [ "proc-macro2", "quote", @@ -293,22 +259,22 @@ dependencies = [ [[package]] name = "cortex-m" -version = "0.7.6" +version = "0.7.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70858629a458fdfd39f9675c4dc309411f2a3f83bede76988d81bf1a0ecee9e0" +checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" dependencies = [ "bare-metal 0.2.5", "bitfield", - "critical-section 1.1.1", + "critical-section 1.1.2", "embedded-hal 0.2.7", "volatile-register", ] [[package]] name = "cortex-m-rt" -version = "0.7.1" +version = "0.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c433da385b720d5bb9f52362fa2782420798e68d40d67bfe4b0d992aba5dfe7" +checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" dependencies = [ "cortex-m-rt-macros", ] @@ -321,81 +287,43 @@ checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 1.0.109", ] [[package]] -name = "credentials" -version = "1.0.0" - -[[package]] -name = "critical-section" -version = "0.2.7" +name = "cortex-m-semihosting" +version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95da181745b56d4bd339530ec393508910c909c784e8962d15d722bacf0bcbcd" +checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" dependencies = [ - "bare-metal 1.0.0", - "cfg-if", "cortex-m", - "riscv", ] [[package]] -name = "critical-section" -version = "1.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6548a0ad5d2549e111e1f6a11a6c2e2d00ce6a3dafe22948d67c2b443f775e52" - -[[package]] -name = "crossbeam-channel" -version = "0.5.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c2dd04ddaf88237dc3b8d8f9a3c1004b506b54b3313403944054d23c0870c521" -dependencies = [ - "cfg-if", - "crossbeam-utils", -] - -[[package]] -name = "crossbeam-deque" -version = "0.8.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "715e8152b692bba2d374b53d4875445368fdf21a94751410af607a5ac677d1fc" -dependencies = [ - "cfg-if", - "crossbeam-epoch", - "crossbeam-utils", -] +name = "credentials" +version = "1.0.0" [[package]] -name = "crossbeam-epoch" -version = "0.9.10" +name = "critical-section" +version = "0.2.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "045ebe27666471bb549370b4b0b3e51b07f56325befa4284db65fc89c02511b1" +checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6" dependencies = [ - "autocfg", - "cfg-if", - "crossbeam-utils", - "memoffset", - "once_cell", - "scopeguard", + "bare-metal 1.0.0", + "critical-section 1.1.2", ] [[package]] -name = "crossbeam-utils" -version = "0.8.11" +name = "critical-section" +version = "1.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "51887d4adc7b564537b15adcfb307936f8075dfcd5f00dde9a9f1d29383682bc" -dependencies = [ - "cfg-if", - "once_cell", -] +checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.13.4" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a01d95850c592940db9b8194bc39f4bc0e89dee5c4265e4b1807c34a9aba453c" +checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" dependencies = [ "darling_core", "darling_macro", @@ -403,57 +331,60 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.13.4" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "859d65a907b6852c9361e3185c862aae7fafd2887876799fa55f5f99dc40d610" +checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn", + "syn 2.0.60", ] [[package]] name = "darling_macro" -version = "0.13.4" +version = "0.20.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c972679f83bdf9c42bd905396b6c3588a843a17f0f16dfcfa3e2c5d57441835" +checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" dependencies = [ "darling_core", "quote", - "syn", + "syn 2.0.60", ] [[package]] name = "defmt" -version = "0.3.2" +version = "0.3.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d3a0ae7494d9bff013d7b89471f4c424356a71e9752e0c78abe7e6c608a16bb3" +checksum = "3939552907426de152b3c2c6f51ed53f98f448babd26f28694c95f5906194595" dependencies = [ - "bitflags", + "bitflags 1.3.2", "defmt-macros", ] [[package]] name = "defmt-macros" -version = "0.3.2" +version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6d944432e281084511691b36e5e9c794c19c33675822c9019e3b64f5b89e10da" +checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn", + "syn 2.0.60", ] [[package]] name = "defmt-parser" -version = "0.3.1" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0db23d29972d99baa3de2ee2ae3f104c10564a6d05a346eb3f4c4f2c0525a06e" +checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" +dependencies = [ + "thiserror", +] [[package]] name = "defmt-rtt" @@ -461,113 +392,110 @@ version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1" dependencies = [ - "critical-section 0.2.7", + "critical-section 0.2.8", "defmt", ] [[package]] name = "defmt-test" -version = "0.3.0" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4df24f1ca104a0c1bce2047d8a21aa9fa29695e5d662118eb48daedb97edca88" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" dependencies = [ - "cortex-m", "cortex-m-rt", + "cortex-m-semihosting", "defmt", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94a0dfea4063d72e1ba20494dfbc4667f67420869328cf3670b5824a38a22dc1" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.60", ] [[package]] -name = "either" -version = "1.8.0" +name = "document-features" +version = "0.2.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90e5c1c8368803113bf0c9584fc495a58b86dc8a29edbf8fe877d21d9507e797" - -[[package]] -name = "embassy-cortex-m" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +checksum = "ef5282ad69563b5fc40319526ba27e0e7363d552a896f0297d54f767717f9b95" dependencies = [ - "atomic-polyfill 1.0.1", - "cfg-if", - "cortex-m", - "critical-section 1.1.1", - "embassy-executor", - "embassy-hal-common", - "embassy-macros", - "embassy-sync", + "litrs", ] +[[package]] +name = "either" +version = "1.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" + [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", + "embassy-futures", "embassy-sync", + "embassy-time", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.9", + "embedded-hal 1.0.0", "embedded-hal-async", "embedded-storage", "embedded-storage-async", - "nb 1.0.0", + "nb 1.1.0", ] [[package]] name = "embassy-executor" -version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ - "atomic-polyfill 1.0.1", - "cfg-if", - "critical-section 1.1.1", + "cortex-m", + "critical-section 1.1.2", "defmt", - "embassy-macros", - "embassy-time", - "futures-util", - "static_cell", + "document-features", + "embassy-executor-macros", + "embassy-time-driver", + "embassy-time-queue-driver", +] + +[[package]] +name = "embassy-executor-macros" +version = "0.4.1" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.60", ] [[package]] name = "embassy-futures" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "0.1.1" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] -name = "embassy-hal-common" +name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ + "cortex-m", + "critical-section 1.1.2", "defmt", "num-traits", ] -[[package]] -name = "embassy-macros" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" -dependencies = [ - "darling", - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "embassy-net-driver" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "0.2.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] @@ -575,80 +503,119 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ - "atomic-polyfill 1.0.1", - "bxcan", + "bit_field", + "bitflags 2.5.0", "cfg-if", + "chrono", "cortex-m", "cortex-m-rt", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", - "embassy-cortex-m", + "document-features", "embassy-embedded-hal", - "embassy-executor", "embassy-futures", - "embassy-hal-common", + "embassy-hal-internal", "embassy-net-driver", "embassy-sync", "embassy-time", + "embassy-time-driver", "embassy-usb-driver", + "embassy-usb-synopsys-otg", + "embedded-can", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.9", + "embedded-hal 1.0.0", "embedded-hal-async", "embedded-hal-nb", "embedded-io", + "embedded-io-async", "embedded-storage", "embedded-storage-async", - "futures", - "nb 1.0.0", + "futures-util", + "nb 1.1.0", "proc-macro2", "quote", "rand_core", "sdio-host", - "seq-macro", + "static_assertions", "stm32-fmc", "stm32-metapac", "vcell", + "volatile-register", ] [[package]] name = "embassy-sync" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "0.5.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", - "embedded-io", + "embedded-io-async", "futures-util", - "heapless", + "heapless 0.8.0", ] [[package]] name = "embassy-time" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "0.3.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ - "atomic-polyfill 1.0.1", "cfg-if", - "critical-section 1.1.1", + "critical-section 1.1.2", "defmt", - "embassy-sync", + "document-features", + "embassy-time-driver", + "embassy-time-queue-driver", "embedded-hal 0.2.7", - "embedded-hal 1.0.0-alpha.9", + "embedded-hal 1.0.0", + "embedded-hal-async", "futures-util", - "heapless", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-time-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +dependencies = [ + "document-features", ] +[[package]] +name = "embassy-time-queue-driver" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" + [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] +[[package]] +name = "embassy-usb-synopsys-otg" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +dependencies = [ + "critical-section 1.1.2", + "embassy-sync", + "embassy-usb-driver", +] + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb 1.1.0", +] + [[package]] name = "embedded-hal" version = "0.2.7" @@ -661,58 +628,68 @@ dependencies = [ [[package]] name = "embedded-hal" -version = "1.0.0-alpha.9" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "129b101ddfee640565f7c07b301a31d95aa21e5acef21a491c307139f5fa4c91" +checksum = "361a90feb7004eca4019fb28352a9465666b24f840f5c3cddf0ff13920590b89" [[package]] name = "embedded-hal-async" -version = "0.2.0-alpha.0" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "608a322808d65da06715e03109c0cb69f79a5459af756fba393ab83e875d4969" +checksum = "0c4c685bbef7fe13c3c6dd4da26841ed3980ef33e841cddfa15ce8a8fb3f1884" dependencies = [ - "embedded-hal 1.0.0-alpha.9", + "embedded-hal 1.0.0", ] [[package]] name = "embedded-hal-nb" -version = "1.0.0-alpha.1" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e0760ec0a3bf76859d5e33f39542af103f157d5b2ecfb00ace56dd461472e3a" +checksum = "fba4268c14288c828995299e59b12babdbe170f6c6d73731af1b4648142e8605" dependencies = [ - "embedded-hal 1.0.0-alpha.9", - "nb 1.0.0", + "embedded-hal 1.0.0", + "nb 1.1.0", ] [[package]] name = "embedded-io" -version = "0.4.0" +version = "0.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" dependencies = [ "defmt", ] +[[package]] +name = "embedded-io-async" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ff09972d4073aa8c299395be75161d582e7629cd663171d62af73c8d50dba3f" +dependencies = [ + "defmt", + "embedded-io", +] + [[package]] name = "embedded-storage" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "156d7a2fdd98ebbf9ae579cbceca3058cff946e13f8e17b90e3511db0508c723" +checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" [[package]] name = "embedded-storage-async" -version = "0.3.0" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ff04af74e47e9bb4315bd7aa2b01f3d1b05f33c03a6c4e9c3b20e9ce9cd8d79" +checksum = "1763775e2323b7d5f0aa6090657f5e21cfa02ede71f5dc40eead06d64dcd15cc" dependencies = [ "embedded-storage", ] [[package]] name = "env_logger" -version = "0.9.0" +version = "0.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b2cf0344971ee6c64c31be0d530793fba457d322dfec2810c453d0ef228f9c3" +checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" dependencies = [ "atty", "humantime", @@ -722,79 +699,40 @@ dependencies = [ ] [[package]] -name = "fnv" -version = "1.0.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" - -[[package]] -name = "futures" -version = "0.3.24" +name = "errno" +version = "0.3.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f21eda599937fba36daeb58a22e8f5cee2d14c4a17b5b7739c7c8e5e3b8230c" +checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" dependencies = [ - "futures-channel", - "futures-core", - "futures-io", - "futures-sink", - "futures-task", - "futures-util", + "libc", + "windows-sys", ] [[package]] -name = "futures-channel" -version = "0.3.24" +name = "fnv" +version = "1.0.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "30bdd20c28fadd505d0fd6712cdfcb0d4b5648baf45faef7f852afb2399bb050" -dependencies = [ - "futures-core", - "futures-sink", -] +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" [[package]] name = "futures-core" -version = "0.3.24" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4e5aa3de05362c3fb88de6531e6296e85cde7739cccad4b9dfeeb7f6ebce56bf" - -[[package]] -name = "futures-io" -version = "0.3.24" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bbf4d2a7a308fd4578637c0b17c7e1c7ba127b8f6ba00b29f717e9655d85eb68" - -[[package]] -name = "futures-macro" -version = "0.3.24" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "42cd15d1c7456c04dbdf7e88bcd69760d74f3a798d6444e16974b505b0e62f17" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "futures-sink" -version = "0.3.24" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "21b20ba5a92e727ba30e72834706623d94ac93a725410b6a6b6fbc1b07f7ba56" +checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" [[package]] name = "futures-task" -version = "0.3.24" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a6508c467c73851293f390476d4491cf4d227dbabcd4170f3bb6044959b294f1" +checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" [[package]] name = "futures-util" -version = "0.3.24" +version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "44fb6cb1be61cc1d2e43b262516aafcf63b241cffdb1d3fa115f91d9c7b09c90" +checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" dependencies = [ "futures-core", - "futures-macro", - "futures-sink", "futures-task", "pin-project-lite", "pin-utils", @@ -802,9 +740,9 @@ dependencies = [ [[package]] name = "glob" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b919933a397b79c37e33b77bb2aa3dc8eb6e165ad809e58ff75bc7db2e34574" +checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" [[package]] name = "hash32" @@ -815,6 +753,15 @@ dependencies = [ "byteorder", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -823,22 +770,26 @@ checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" [[package]] name = "heapless" -version = "0.7.16" +version = "0.7.17" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743" +checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f" dependencies = [ - "atomic-polyfill 0.1.10", - "hash32", + "atomic-polyfill", + "hash32 0.2.1", "rustc_version 0.4.0", "spin", "stable_deref_trait", ] [[package]] -name = "heck" -version = "0.4.1" +name = "heapless" +version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32 0.3.1", + "stable_deref_trait", +] [[package]] name = "hermit-abi" @@ -849,6 +800,15 @@ dependencies = [ "libc", ] +[[package]] +name = "home" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5" +dependencies = [ + "windows-sys", +] + [[package]] name = "humantime" version = "2.1.0" @@ -863,26 +823,14 @@ checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" [[package]] name = "indexmap" -version = "1.9.1" +version = "1.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "10a35a97730320ffe8e2d410b5d3b69279b98d2c14bdb8b70ea89ecf7888d41e" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", "hashbrown", ] -[[package]] -name = "inflections" -version = "1.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a257582fdcde896fd96463bf2d40eefea0580021c0712a0e2b028b60b47a837a" - -[[package]] -name = "itoa" -version = "1.0.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fad582f4b9e86b6caa621cabeb0963332d92eea04729ab12892c2533951e6440" - [[package]] name = "lazy_static" version = "1.4.0" @@ -897,37 +845,43 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.132" +version = "0.2.154" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8371e4e5341c3a96db127eb2465ac681ced4c433e01dd0e938adbef26ba93ba5" +checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" [[package]] name = "libloading" -version = "0.7.4" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +checksum = "0c2a198fb6b0eada2a8df47933734e6d35d350665a33a3593d7164fa52c75c19" dependencies = [ "cfg-if", - "winapi", + "windows-targets", ] [[package]] name = "libm" -version = "0.2.6" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + +[[package]] +name = "linux-raw-sys" +version = "0.4.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "348108ab3fba42ec82ff6e9564fc4ca0247bdccdc68dd8af9764bbc79c3c8ffb" +checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" [[package]] -name = "linked-hash-map" -version = "0.5.6" +name = "litrs" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" [[package]] name = "lock_api" -version = "0.4.8" +version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f80bf5aacaf25cbfc8210d1cfb718f2bf3b11c4c54e5afe36c236853a8ec390" +checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" dependencies = [ "autocfg", "scopeguard", @@ -935,27 +889,15 @@ dependencies = [ [[package]] name = "log" -version = "0.4.17" +version = "0.4.21" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" -dependencies = [ - "cfg-if", -] +checksum = "90ed8c1e510134f979dbc4f070f87d4313098b704861a105fe34231c70a3901c" [[package]] name = "memchr" -version = "2.5.0" +version = "2.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" - -[[package]] -name = "memoffset" -version = "0.6.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5aa361d4faea93603064a027415f07bd8e1d5c88c9fbf68bf56a285428fd79ce" -dependencies = [ - "autocfg", -] +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" [[package]] name = "minimal-lexical" @@ -965,9 +907,9 @@ checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" [[package]] name = "nalgebra" -version = "0.32.2" +version = "0.32.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d68d47bba83f9e2006d117a9a33af1524e655516b8919caac694427a6fb1e511" +checksum = "3ea4908d4f23254adda3daa60ffef0f1ac7b8c3e9a864cf3cc154b251908a2ef" dependencies = [ "approx", "nalgebra-macros", @@ -980,13 +922,13 @@ dependencies = [ [[package]] name = "nalgebra-macros" -version = "0.2.0" +version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d232c68884c0c99810a5a4d333ef7e47689cfd0edc85efc9e54e1e6bf5212766" +checksum = "91761aed67d03ad966ef783ae962ef9bbaca728d2dd7ceb7939ec110fffad998" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 1.0.109", ] [[package]] @@ -995,20 +937,20 @@ version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" dependencies = [ - "nb 1.0.0", + "nb 1.1.0", ] [[package]] name = "nb" -version = "1.0.0" +version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" [[package]] name = "nom" -version = "7.1.1" +version = "7.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8903e5a29a317527874d0402f867152a3d21c908bb0b933e416c65e301d4c36" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" dependencies = [ "memchr", "minimal-lexical", @@ -1016,20 +958,19 @@ dependencies = [ [[package]] name = "num-complex" -version = "0.4.2" +version = "0.4.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ae39348c8bc5fbd7f40c727a9925f03517afd2ab27d46702108b6a7e5414c19" +checksum = "23c6602fda94a57c990fe0df199a035d83576b496aa29f4e634a8ac6004e68a6" dependencies = [ "num-traits", ] [[package]] name = "num-integer" -version = "0.1.45" +version = "0.1.46" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "225d3389fb3509a24c93f5c29eb6bde2586b98d9f016636dff58d7c6f7569cd9" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" dependencies = [ - "autocfg", "num-traits", ] @@ -1046,41 +987,31 @@ dependencies = [ [[package]] name = "num-traits" -version = "0.2.15" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", "libm", ] -[[package]] -name = "num_cpus" -version = "1.13.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "19e64526ebdee182341572e50e9ad03965aa510cd94427a4549448f285e957a1" -dependencies = [ - "hermit-abi", - "libc", -] - [[package]] name = "once_cell" -version = "1.14.0" +version = "1.19.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2f7254b99e31cad77da24b08ebf628882739a608578bb1bcdfc1f9c21260d7c0" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" [[package]] name = "os_str_bytes" -version = "6.3.0" +version = "6.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ff7415e9ae3fff1225851df9e0d9e4e5479f947619774677a63572e55e80eff" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ab1f00eac22bd18f8e5cae9555f2820b3a0c166b5b556ee3e203746ea6dcf3a" +checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" dependencies = [ "cortex-m", "defmt", @@ -1088,9 +1019,9 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.9" +version = "1.0.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1de2e551fb905ac83f73f7aedf2f0cb4a0da7e35efa24a202a936269f1f18e1" +checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" [[package]] name = "peeking_take_while" @@ -1100,9 +1031,9 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" [[package]] name = "pin-project-lite" -version = "0.2.9" +version = "0.2.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e0a7ae3ac2f1173085d398531c705756c94a4c56843785df85a60c1a0afac116" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" [[package]] name = "pin-utils" @@ -1110,6 +1041,12 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" +[[package]] +name = "portable-atomic" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7170ef9988bc169ba16dd36a7fa041e5c4cbeb6a35b76d4c03daded371eae7c0" + [[package]] name = "proc-macro-error" version = "1.0.4" @@ -1119,7 +1056,7 @@ dependencies = [ "proc-macro-error-attr", "proc-macro2", "quote", - "syn", + "syn 1.0.109", "version_check", ] @@ -1136,57 +1073,45 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.66" +version = "1.0.81" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18fb31db3f9bddb2ea821cde30a9f70117e3f119938b5ee630b7403aa6e2ead9" +checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.31" +version = "1.0.36" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5fe8a65d69dd0808184ebb5f836ab526bb259db23c657efa38711b1072ee47f0" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" dependencies = [ "proc-macro2", ] [[package]] name = "rand_core" -version = "0.6.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d34f1408f55294453790c48b2f1ebbb1c5b4b7563eb1f418bcfcfdbb06ebb4e7" - -[[package]] -name = "rayon" -version = "1.5.3" +version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bd99e5772ead8baa5215278c9b15bf92087709e9c1b2d1f97cdb5a183c933a7d" -dependencies = [ - "autocfg", - "crossbeam-deque", - "either", - "rayon-core", -] +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" [[package]] -name = "rayon-core" -version = "1.9.3" +name = "regex" +version = "1.10.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "258bcdb5ac6dad48491bb2992db6b7cf74878b0384908af124823d118c99683f" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" dependencies = [ - "crossbeam-channel", - "crossbeam-deque", - "crossbeam-utils", - "num_cpus", + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", ] [[package]] -name = "regex" -version = "1.6.0" +name = "regex-automata" +version = "0.4.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c4eb3267174b8c6c2f654116623910a0fef09c4753f8dd83db29c48a0df988b" +checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" dependencies = [ "aho-corasick", "memchr", @@ -1195,40 +1120,19 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.6.27" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a3f87b73ce11b1619a3c6332f45341e0047173771e8b8b73f87bfeefb7b56244" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rgb" -version = "0.8.36" +version = "0.8.37" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "20ec2d3e3fc7a92ced357df9cebd5a10b6fb2aa1ee797bf7e9ce2f17dffc8f59" +checksum = "05aaa8004b64fd573fc9d002f4e632d51ad4f026c2b5ba95fcb6c2f32c2c47d8" dependencies = [ "bytemuck", ] -[[package]] -name = "riscv" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6907ccdd7a31012b70faf2af85cd9e5ba97657cc3987c4f13f8e4d2c2a088aba" -dependencies = [ - "bare-metal 1.0.0", - "bit_field", - "riscv-target", -] - -[[package]] -name = "riscv-target" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "88aa938cda42a0cf62a20cfe8d139ff1af20c2e681212b5b34adb5a58333f222" -dependencies = [ - "lazy_static", - "regex", -] - [[package]] name = "rustc-hash" version = "1.1.0" @@ -1250,20 +1154,27 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.13", + "semver 1.0.22", ] [[package]] -name = "ryu" -version = "1.0.11" +name = "rustix" +version = "0.38.34" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4501abdff3ae82a1c1b477a17252eb69cee9e66eb915c1abaa4f44d873df9f09" +checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" +dependencies = [ + "bitflags 2.5.0", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", +] [[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 = "sdio-host" @@ -1282,9 +1193,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.13" +version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93f6841e709003d68bb2deee8c343572bf446003ec20a583e76f7b15cebf3711" +checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" [[package]] name = "semver-parser" @@ -1292,60 +1203,11 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" -[[package]] -name = "seq-macro" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0772c5c30e1a0d91f6834f8e545c69281c099dfa9a3ac58d96a9fd629c8d4898" - -[[package]] -name = "serde" -version = "1.0.144" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0f747710de3dcd43b88c9168773254e809d8ddbdf9653b84e2554ab219f17860" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.144" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94ed3a816fb1d101812f83e789f888322c34e291f894f19590dc310963e87a00" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "serde_json" -version = "1.0.91" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "877c235533714907a8c2464236f5c4b2a17262ef1bd71f38f35ea592c8da6883" -dependencies = [ - "itoa", - "ryu", - "serde", -] - -[[package]] -name = "serde_yaml" -version = "0.8.26" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "578a7433b776b56a35785ed5ce9a7e777ac0598aac5a6dd1b4b18a307c7fc71b" -dependencies = [ - "indexmap", - "ryu", - "serde", - "yaml-rust", -] - [[package]] name = "shlex" -version = "1.1.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" [[package]] name = "simba" @@ -1379,9 +1241,9 @@ dependencies = [ [[package]] name = "spin" -version = "0.9.4" +version = "0.9.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f6002a767bff9e83f8eeecf883ecb8011875a21ae8da43bffb817a57e78cc09" +checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" dependencies = [ "lock_api", ] @@ -1392,46 +1254,37 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + [[package]] name = "static_cell" -version = "1.0.0" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e4c37c250d21f53fa7165e76e5401d7e6539c211a8d2cf449e3962956a5cc2ce" +checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" dependencies = [ - "atomic-polyfill 1.0.1", + "portable-atomic", ] [[package]] name = "stm32-fmc" -version = "0.2.4" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf16ee9bd5de754482883cf3eac9a49eb862baf1420f55ce408e001705e9ae74" +checksum = "830ed60f33e6194ecb377f5d6ab765dc0e37e7b65e765f1fa87df13336658d63" dependencies = [ "embedded-hal 0.2.7", ] [[package]] name = "stm32-metapac" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" +version = "15.0.0" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" dependencies = [ "cortex-m", "cortex-m-rt", - "regex", - "stm32-metapac-gen", -] - -[[package]] -name = "stm32-metapac-gen" -version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy?rev=0412d1922c6cca1200da0af54db3a5dbf60c035c#0412d1922c6cca1200da0af54db3a5dbf60c035c" -dependencies = [ - "chiptool", - "proc-macro2", - "regex", - "serde", - "serde_json", - "serde_yaml", ] [[package]] @@ -1441,24 +1294,21 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" [[package]] -name = "svd-parser" -version = "0.10.2" +name = "syn" +version = "1.0.109" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "697e7645ad9f5311fe3d872d094b135627b1616aea9e1573dddd28ca522579b9" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" dependencies = [ - "anyhow", - "once_cell", - "rayon", - "regex", - "thiserror", - "xmltree", + "proc-macro2", + "quote", + "unicode-ident", ] [[package]] name = "syn" -version = "1.0.99" +version = "2.0.60" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "58dbef6ec655055e20b86b15a8cc6d439cca19b667537ac6a1369572d151ab13" +checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" dependencies = [ "proc-macro2", "quote", @@ -1467,50 +1317,50 @@ dependencies = [ [[package]] name = "termcolor" -version = "1.1.3" +version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab24d30b911b2376f3a13cc2cd443142f0c81dda04c118693e35b3835757755" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" dependencies = [ "winapi-util", ] [[package]] name = "textwrap" -version = "0.15.0" +version = "0.16.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1141d4d61095b28419e22cb0bbf02755f5e54e0526f97f1e3d1d160e60885fb" +checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.34" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8c1b05ca9d106ba7d2e31a9dab4a64e7be2cce415321966ea3132c49a656e252" +checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.34" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e8f2591983642de85c921015f3f070c665a197ed69e417af436115e3a1407487" +checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" dependencies = [ "proc-macro2", "quote", - "syn", + "syn 2.0.60", ] [[package]] name = "typenum" -version = "1.15.0" +version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dcf81ac59edc17cc8697ff311e8f5ef2d99fcbd9817b34cec66f90b6c3dfd987" +checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" [[package]] name = "unicode-ident" -version = "1.0.3" +version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c4f5b37a154999a8f3f98cc23a628d850e154479cd94decf3414696e12e31aaf" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" [[package]] name = "unicode-xid" @@ -1538,22 +1388,23 @@ checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" [[package]] name = "volatile-register" -version = "0.2.1" +version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6" +checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" dependencies = [ "vcell", ] [[package]] name = "which" -version = "4.4.0" +version = "4.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2441c784c52b289a054b7201fc93253e288f094e2f4be9058343127c4226a269" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" dependencies = [ "either", - "libc", + "home", "once_cell", + "rustix", ] [[package]] @@ -1574,11 +1425,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.5" +version = "0.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" dependencies = [ - "winapi", + "windows-sys", ] [[package]] @@ -1588,28 +1439,74 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" [[package]] -name = "xml-rs" -version = "0.7.0" +name = "windows-sys" +version = "0.52.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c1cb601d29fe2c2ac60a2b2e5e293994d87a1f6fa9687a31a15270f909be9c2" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" dependencies = [ - "bitflags", + "windows-targets", ] [[package]] -name = "xmltree" -version = "0.8.0" +name = "windows-targets" +version = "0.52.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff8eaee9d17062850f1e6163b509947969242990ee59a35801af437abe041e70" +checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" dependencies = [ - "xml-rs", + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", ] [[package]] -name = "yaml-rust" -version = "0.4.5" +name = "windows_aarch64_gnullvm" +version = "0.52.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "56c1936c4cc7a1c9ab21a1ebb602eb942ba868cbd44a99cb7cdc5892335e1c85" -dependencies = [ - "linked-hash-map", -] +checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index f867747f..14607938 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -8,42 +8,50 @@ edition = "2021" [dependencies] cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.1" + defmt = "0.3" defmt-rtt = "0.3" -embassy-executor = { version = "0.1.0", features = [ + +embassy-executor = { version = "0.5.0", features = [ + "arch-cortex-m", + "executor-thread", + "executor-interrupt", "defmt", "integrated-timers", + "task-arena-size-32768", ] } -embassy-time = { version = "0.1.0", features = [ +embassy-time = { version = "0.3.0", features = [ "defmt", "defmt-timestamp-uptime", - "unstable-traits", "tick-hz-32_768", ] } embassy-stm32 = { version = "0.1.0", features = [ - "nightly", "defmt", "stm32h723zg", - "time-driver-any", + "unstable-pac", + "time-driver-tim1", "exti", - "unstable-pac", - "unstable-traits", + "chrono" ] } -smart-leds = "0.3.0" -apa102-spi = "0.3.2" embassy-futures = { version = "0.1.0" } +embassy-sync = { version = "0.5.0" } + +static_cell = "2.1.0" futures-util = { version = "0.3.17", default-features = false } -embassy-sync = { version = "0.1.0" } panic-probe = { version = "0.3", features = ["print-defmt"] } -static_cell = "1.0" critical-section = "1.1.1" -const_format = "0.2.30" heapless = "0.7.16" +const_format = "0.2.30" libm = "0.2.6" nalgebra = { version = "0.32.2", default-features = false, features = [ "libm", "macros", ] } + +smart-leds = "0.3.0" +apa102-spi = "0.3.2" + +ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } credentials = { path = "../credentials" } @@ -107,11 +115,6 @@ name = "kicker" test = false harness = false -[[bin]] -name = "proto-drivetrain" -test = false -harness = false - [[test]] name = "basic-test" harness = false @@ -123,8 +126,9 @@ name = "drivetrain-test" harness = false [patch.crates-io] -embassy-executor = { git = "https://github.com/embassy-rs/embassy", rev = "0412d1922c6cca1200da0af54db3a5dbf60c035c" } -embassy-sync = { git = "https://github.com/embassy-rs/embassy", rev = "0412d1922c6cca1200da0af54db3a5dbf60c035c" } -embassy-time = { git = "https://github.com/embassy-rs/embassy", rev = "0412d1922c6cca1200da0af54db3a5dbf60c035c" } -embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", rev = "0412d1922c6cca1200da0af54db3a5dbf60c035c" } -embassy-futures = { git = "https://github.com/embassy-rs/embassy", rev = "0412d1922c6cca1200da0af54db3a5dbf60c035c" } +embassy-executor = { git = "https://github.com/embassy-rs/embassy"} +embassy-sync = { git = "https://github.com/embassy-rs/embassy"} +embassy-time = { git = "https://github.com/embassy-rs/embassy"} +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} +embassy-futures = { git = "https://github.com/embassy-rs/embassy"} + diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 39a19b0c..3502663e 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -89,9 +89,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.2.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -325,7 +325,7 @@ dependencies = [ "proc-macro2", "quote", "strsim", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] @@ -336,7 +336,7 @@ checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" dependencies = [ "darling_core", "quote", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] @@ -359,7 +359,7 @@ dependencies = [ "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] @@ -401,7 +401,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] @@ -422,7 +422,7 @@ checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", "embassy-futures", @@ -439,7 +439,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -453,23 +453,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -480,7 +480,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] @@ -488,7 +488,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -507,6 +507,7 @@ dependencies = [ "embassy-time", "embassy-time-driver", "embassy-usb-driver", + "embassy-usb-synopsys-otg", "embedded-can", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -516,7 +517,7 @@ dependencies = [ "embedded-io-async", "embedded-storage", "embedded-storage-async", - "futures", + "futures-util", "nb 1.1.0", "proc-macro2", "quote", @@ -532,7 +533,7 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -545,7 +546,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -563,7 +564,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "document-features", ] @@ -571,16 +572,26 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#bab4affe7cd116f44eb378c8f32e58e6993adbf5" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] +[[package]] +name = "embassy-usb-synopsys-otg" +version = "0.1.0" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +dependencies = [ + "critical-section 1.1.2", + "embassy-sync", + "embassy-usb-driver", +] + [[package]] name = "embedded-can" version = "0.4.1" @@ -688,59 +699,12 @@ version = "1.0.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" -[[package]] -name = "futures" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0" -dependencies = [ - "futures-channel", - "futures-core", - "futures-io", - "futures-sink", - "futures-task", - "futures-util", -] - -[[package]] -name = "futures-channel" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" -dependencies = [ - "futures-core", - "futures-sink", -] - [[package]] name = "futures-core" version = "0.3.30" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" -[[package]] -name = "futures-io" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1" - -[[package]] -name = "futures-macro" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.59", -] - -[[package]] -name = "futures-sink" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" - [[package]] name = "futures-task" version = "0.3.30" @@ -754,8 +718,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" dependencies = [ "futures-core", - "futures-macro", - "futures-sink", "futures-task", "pin-project-lite", "pin-utils", @@ -868,9 +830,9 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.153" +version = "0.2.154" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" +checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" [[package]] name = "libloading" @@ -902,9 +864,9 @@ checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" [[package]] name = "lock_api" -version = "0.4.11" +version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45" +checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17" dependencies = [ "autocfg", "scopeguard", @@ -955,9 +917,9 @@ dependencies = [ [[package]] name = "num-traits" -version = "0.2.18" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", ] @@ -1034,9 +996,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.80" +version = "1.0.81" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a56dea16b0a29e94408b9aa5e2940a4eedbd128a1ba20e8f7ae60fd3d465af0e" +checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" dependencies = [ "unicode-ident", ] @@ -1120,9 +1082,9 @@ dependencies = [ [[package]] name = "rustix" -version = "0.38.32" +version = "0.38.34" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "65e04861e65f21776e67888bfbea442b3642beaa0138fdb1dd7a84a52dffdb89" +checksum = "70dc5ec042f7a43c4a73241207cecc9873a06d45debb38b329f8541d85c2730f" dependencies = [ "bitflags 2.5.0", "errno", @@ -1211,9 +1173,9 @@ checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" [[package]] name = "static_cell" -version = "2.0.0" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa6ba4cf83bf80d3eb25f098ea5e790a0a1fcb5e357442259b231e412c2d3ca0" +checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e" dependencies = [ "portable-atomic", ] @@ -1230,7 +1192,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-01ac9bfd035961dc75f32dcd6080501538246d5c#a5e8ddc393950d780eb95fba15833d8a68b4e216" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1255,9 +1217,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.59" +version = "2.0.60" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4a6531ffc7b071655e4ce2e04bd464c4830bb585a61cabb96cf808f05172615a" +checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" dependencies = [ "proc-macro2", "quote", @@ -1281,22 +1243,22 @@ checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.58" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" +checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.58" +version = "1.0.59" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" +checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" dependencies = [ "proc-macro2", "quote", - "syn 2.0.59", + "syn 2.0.60", ] [[package]] @@ -1368,11 +1330,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.6" +version = "0.1.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" +checksum = "4d4cc384e1e73b93bafa6fb4f1df8c41695c8a91cf9c4c64358067d15a7b6c6b" dependencies = [ - "winapi", + "windows-sys", ] [[package]] diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index e35216dd..50d21260 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -46,7 +46,7 @@ libm = "0.2.6" smart-leds = "0.4.0" apa102-spi = "0.4.0" -ateam-lib-stm32 = { path = "../lib-stm32" } +ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } ateam-common-packets = { path = "../software-communication/ateam-common-packets/rust-lib" } [dev-dependencies] diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index b11011fc..fa7afa0c 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -48,9 +48,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.2.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -302,7 +302,7 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", "embassy-futures", @@ -319,7 +319,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -333,7 +333,7 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "darling", "proc-macro2", @@ -344,12 +344,12 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -360,7 +360,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] @@ -368,7 +368,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -413,7 +413,7 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -426,7 +426,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -444,7 +444,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "document-features", ] @@ -452,12 +452,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "defmt", ] @@ -465,7 +465,7 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "critical-section 1.1.2", "embassy-sync", @@ -666,9 +666,9 @@ checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" [[package]] name = "num-traits" -version = "0.2.18" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", ] @@ -842,7 +842,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-823168933f3860770111f7bde2a82b912eac58c0#617678145a87435d1b19628b0cc03c1f89cf7bfe" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" dependencies = [ "cortex-m", "cortex-m-rt", diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index f82fd743..bb81db1a 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -40,7 +40,7 @@ critical-section = "1.1.1" const_format = "0.2.30" heapless = "0.7.16" libm = "0.2.6" -ateam-lib-stm32 = { path = "../lib-stm32" } +ateam-lib-stm32 = { path = "../lib-stm32", default-features = false } [dev-dependencies] diff --git a/lib-stm32/.cargo/config.toml b/lib-stm32/.cargo/config.toml index 2c1b4189..5d10902a 100644 --- a/lib-stm32/.cargo/config.toml +++ b/lib-stm32/.cargo/config.toml @@ -1,9 +1,2 @@ [build] target = "thumbv7em-none-eabihf" - -[target.thumbv7em-none-eabihf] -runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' -# runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' - -[env] -DEFMT_LOG = "trace" diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index abd75488..5aa29e17 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -15,9 +15,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.2.0" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" +checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0" [[package]] name = "bare-metal" @@ -207,7 +207,7 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "embassy-futures", "embassy-sync", @@ -223,7 +223,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "critical-section 1.1.2", "document-features", @@ -233,7 +233,7 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "darling", "proc-macro2", @@ -244,12 +244,12 @@ dependencies = [ [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -259,12 +259,12 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -305,7 +305,7 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -317,7 +317,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -334,7 +334,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "document-features", ] @@ -342,17 +342,17 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#51a4a73323880613988c9ac97544b505d738c859" +source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" dependencies = [ "critical-section 1.1.2", "embassy-sync", @@ -511,9 +511,9 @@ checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" [[package]] name = "num-traits" -version = "0.2.18" +version = "0.2.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da0df0e5185db44f69b44f26786fe401b6c293d1907744beaa7fa62b2e5a517a" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", ] @@ -632,7 +632,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-823168933f3860770111f7bde2a82b912eac58c0#617678145a87435d1b19628b0cc03c1f89cf7bfe" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" dependencies = [ "cortex-m", ] diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 30d4b099..f0785cc0 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -13,6 +13,10 @@ defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, w defmt-rtt = "0.3" critical-section = "1.1.1" +[features] +default = ["embassy-stm32/stm32h723zg"] +# stm32h723zg = [] + [patch.crates-io] embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index b33f8d57..f1965fbc 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -4,7 +4,6 @@ #![feature(type_alias_impl_trait)] #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] -// #![feature(unsized_locals)] pub mod drivers; From 8c699e26477848cbea00434207a381d97da82676 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 6 May 2024 00:21:09 -0400 Subject: [PATCH 021/157] update uart queues to support changes in config/baudrate using the new api --- kicker-board/Cargo.lock | 3 + kicker-board/src/bin/hwtest-coms/main.rs | 6 +- kicker-board/src/bin/kicker/main.rs | 6 +- lib-stm32-test/Cargo.lock | 3 + lib-stm32-test/Cargo.toml | 2 +- .../bin/hwtest-uart-queue-mixedbaud/main.rs | 229 ++++++++++++++++++ .../main.rs | 8 +- lib-stm32/Cargo.lock | 3 + lib-stm32/Cargo.toml | 4 + lib-stm32/src/queue.rs | 1 + lib-stm32/src/uart/queue.rs | 97 +++++--- 11 files changed, 326 insertions(+), 36 deletions(-) create mode 100644 lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs rename lib-stm32-test/src/bin/{hwtest-uart-queue => hwtest-uart-queue-mixedrate}/main.rs (95%) diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 3502663e..79260ecc 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -64,7 +64,10 @@ dependencies = [ "defmt", "defmt-rtt", "embassy-executor", + "embassy-futures", "embassy-stm32", + "embassy-sync", + "embassy-time", ] [[package]] diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 5e810073..b50ba221 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -5,6 +5,7 @@ #![feature(sync_unsafe_cell)] use core::cell::SyncUnsafeCell; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use static_cell::StaticCell; use defmt::*; @@ -45,13 +46,14 @@ const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; // control communications tx buffer +const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".bss"] static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX); + UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); // control communications rx buffer const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = @@ -60,7 +62,7 @@ const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX); + UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); fn get_empty_control_packet() -> KickerControl { KickerControl { diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 17032f37..d6ba9388 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -6,6 +6,7 @@ use core::cell::SyncUnsafeCell; use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::get_system_config}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use static_cell::StaticCell; use defmt::*; @@ -59,12 +60,13 @@ pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; // control communications tx buffer +const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = SyncUnsafeCell::new(Buffer::EMPTY); static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX); + UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); // control communications rx buffer const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = @@ -72,7 +74,7 @@ const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX); + UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); fn get_empty_control_packet() -> KickerControl { KickerControl { diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index fa7afa0c..ec540ea4 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -10,7 +10,10 @@ dependencies = [ "defmt", "defmt-rtt", "embassy-executor", + "embassy-futures", "embassy-stm32", + "embassy-sync", + "embassy-time", ] [[package]] diff --git a/lib-stm32-test/Cargo.toml b/lib-stm32-test/Cargo.toml index bb81db1a..42b866cf 100644 --- a/lib-stm32-test/Cargo.toml +++ b/lib-stm32-test/Cargo.toml @@ -60,7 +60,7 @@ test = false harness = false [[bin]] -name = "hwtest-uart-queue" +name = "hwtest-uart-queue-mixedrate" test = false harness = false diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs new file mode 100644 index 00000000..0e9f87f8 --- /dev/null +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -0,0 +1,229 @@ +#![no_std] +#![no_main] +#![feature(sync_unsafe_cell)] + +use core::{ + cell::SyncUnsafeCell, + sync::atomic::AtomicU32 +}; + +use embassy_stm32::{ + bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, *}, usart::{self, *} +}; +use embassy_executor::{Executor, InterruptExecutor}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; +use embassy_time::Timer; + +use defmt::*; +use defmt_rtt as _; +use panic_probe as _; + +use static_cell::StaticCell; + +use ateam_lib_stm32::{queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; + +type ComsUartModule = USART2; +type ComsUartTxDma = DMA1_CH0; +type ComsUartRxDma = DMA1_CH1; + +type LedGreenPin = PB0; +type LedYellowPin = PE1; +type LedRedPin = PB14; +type UserBtnPin = PC13; +type UserBtnExti = EXTI13; + +const MAX_TX_PACKET_SIZE: usize = 64; +const TX_BUF_DEPTH: usize = 5; +const MAX_RX_PACKET_SIZE: usize = 64; +const RX_BUF_DEPTH: usize = 5; + +// control communications tx buffer +const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); +const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +#[link_section = ".axisram.buffers"] +static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; +static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); + +// control communications rx buffer +const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); +#[link_section = ".axisram.buffers"] +static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; +static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); + +static BAUD_RATE: AtomicU32 = AtomicU32::new(115200); + +#[allow(dead_code)] +struct StupidPacket { + fields_of_minimal_intelligence: [usize; 16], +} + +#[embassy_executor::task] +async fn rx_task(coms_reader: &'static UartReadQueue) { + let mut rx_packet: StupidPacket = StupidPacket { + fields_of_minimal_intelligence: [0x55AA55AA; 16] + }; + + loop { + while let Ok(res) = coms_reader.try_dequeue() { + let buf = res.data(); + + if buf.len() != core::mem::size_of::() { + defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + continue; + } + + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe + unsafe { + // copy receieved uart bytes into packet + let state = &mut rx_packet as *mut _ as *mut u8; + for i in 0..core::mem::size_of::() { + *state.offset(i as isize) = buf[i]; + } + } + + defmt::info!("got a packet"); + } + + Timer::after_millis(10).await; + } +} + +#[embassy_executor::task] +async fn tx_task(coms_writer: &'static UartWriteQueue) { + let tx_packet: StupidPacket = StupidPacket { + fields_of_minimal_intelligence: [0x55AA55AA; 16] + }; + + loop { + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&tx_packet as *const StupidPacket) as *const u8, + core::mem::size_of::(), + ); + + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } + + Timer::after_millis(1000).await; + } +} + +#[embassy_executor::task] +async fn handle_btn_press(usr_btn_pin: UserBtnPin, + usr_btn_exti: UserBtnExti, + led_green_pin: LedGreenPin, + led_yellow_pin: LedYellowPin, + led_red_pin: LedRedPin, + coms_writer: &'static UartWriteQueue) { + + let mut usr_btn = ExtiInput::new(usr_btn_pin, usr_btn_exti, Pull::Down); + + let mut green_led = Output::new(led_green_pin, Level::High, Speed::Medium); + let mut yellow_led = Output::new(led_yellow_pin, Level::Low, Speed::Medium); + let mut red_led = Output::new(led_red_pin, Level::Low, Speed::Medium); + + loop { + usr_btn.wait_for_rising_edge().await; + + green_led.set_low(); + yellow_led.set_low(); + red_led.set_low(); + + defmt::info!("updating baudrate"); + + let mut coms_uart_config = Config::default(); + + let cur_baud_rate = BAUD_RATE.load(core::sync::atomic::Ordering::SeqCst); + if cur_baud_rate == 115_200 { + red_led.set_high(); + + defmt::info!("set 2M,PE,S1"); + coms_uart_config.baudrate = 2_000_000; // 2 Mbaud + coms_uart_config.parity = Parity::ParityEven; + coms_uart_config.stop_bits = StopBits::STOP1; + + BAUD_RATE.store(2_000_000, core::sync::atomic::Ordering::SeqCst); + } else { + green_led.set_high(); + + defmt::info!("set 115200,PN,S1"); + coms_uart_config.baudrate = 115_200; // 2 Mbaud + coms_uart_config.parity = Parity::ParityNone; + coms_uart_config.stop_bits = StopBits::STOP1; + + BAUD_RATE.store(115_200, core::sync::atomic::Ordering::SeqCst); + }; + + coms_writer.update_uart_config(coms_uart_config).await; + } +} + +static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[interrupt] +unsafe fn TIM2() { + EXECUTOR_HIGH.on_interrupt(); +} + +bind_interrupts!(struct Irqs { + USART2 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: embassy_executor::Spawner) -> !{ + // this actually gets us 64MHz peripheral bus clock + let stm32_config: embassy_stm32::Config = Default::default(); + let p = embassy_stm32::init(stm32_config); + + // high priority executor handles kicking system + // High-priority executor: I2C1, priority level 6 + // TODO CHECK THIS IS THE HIGHEST PRIORITY + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::TIM2, embassy_stm32::interrupt::Priority::P6); + let high_pri_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + + ////////////////////////////////// + // COMMUNICATIONS TASKS SETUP // + ////////////////////////////////// + + let mut coms_uart_config = Config::default(); + coms_uart_config.baudrate = 115_200; // 2 Mbaud + coms_uart_config.parity = Parity::ParityNone; + coms_uart_config.stop_bits = StopBits::STOP1; + + let coms_usart = Uart::new( + p.USART2, + p.PD6, // rx + p.PD5, // tx + Irqs, + p.DMA1_CH1, + p.DMA1_CH2, + coms_uart_config, + ).unwrap(); + + let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + + unwrap!(high_pri_spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + unwrap!(high_pri_spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + + + // MIGHT should put queues in mix prio, this could elicit the bug + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14, &COMS_QUEUE_TX))); + // unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); + // unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(spawner.spawn(rx_task(&COMS_QUEUE_RX))); + unwrap!(spawner.spawn(tx_task(&COMS_QUEUE_TX))); + }); +} \ No newline at end of file diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs similarity index 95% rename from lib-stm32-test/src/bin/hwtest-uart-queue/main.rs rename to lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index 90df2a9b..d00683c9 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -11,6 +11,7 @@ use embassy_stm32::{ bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, *}, usart::{self, *} }; use embassy_executor::{Executor, InterruptExecutor}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; use defmt::*; @@ -34,16 +35,17 @@ type UserBtnExti = EXTI13; const MAX_TX_PACKET_SIZE: usize = 64; const TX_BUF_DEPTH: usize = 5; const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 5; +const RX_BUF_DEPTH: usize = 200; // control communications tx buffer +const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = SyncUnsafeCell::new(Buffer::EMPTY); #[link_section = ".axisram.buffers"] static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX); + UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); // control communications rx buffer const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = @@ -52,7 +54,7 @@ const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX); + UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); static LOOP_RATE_MS: AtomicU16 = AtomicU16::new(100); diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 5aa29e17..8c1ad683 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -10,7 +10,10 @@ dependencies = [ "defmt", "defmt-rtt", "embassy-executor", + "embassy-futures", "embassy-stm32", + "embassy-sync", + "embassy-time", ] [[package]] diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index f0785cc0..6c95e793 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -9,6 +9,10 @@ repository = "https://github.com/SSL-A-Team/firmware" [dependencies] embassy-stm32 = { version = "0.1.0", default-features = false } embassy-executor = { version = "0.5.0", default-features = false } +embassy-sync = { version = "0.5.0" } +embassy-futures = { version = "0.1.0" } +embassy-time = { version = "0.3.0" } + defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) defmt-rtt = "0.3" critical-section = "1.1.1" diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index 787c4755..04d44bc5 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -62,6 +62,7 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> pub fn len(&mut self) -> &mut usize { self.len } + pub fn cancel(self) { self.queue.cancel_enqueue(); } diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 7f0fbe22..726d526f 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -2,7 +2,7 @@ use core::{ cell::SyncUnsafeCell, - future::Future}; + future::Future, sync::atomic::{AtomicBool, Ordering}}; use embassy_executor::{ raw::TaskStorage, @@ -11,8 +11,10 @@ use embassy_stm32::{ mode::Async, usart::{self, UartRx, UartTx} }; +use embassy_futures::select::{select, Either}; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; -use defmt::info; +use embassy_time::Timer; use crate::queue::{ self, @@ -27,6 +29,7 @@ pub struct UartReadQueue< const LENGTH: usize, const DEPTH: usize, > { + uart_mutex: Mutex, queue_rx: Queue, task: TaskStorage>, } @@ -57,36 +60,46 @@ impl< const DEPTH: usize, > UartReadQueue { - pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH]) -> Self { + pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH], uart_mutex: Mutex) -> Self { Self { + uart_mutex: uart_mutex, queue_rx: Queue::new(buffers), task: TaskStorage::new(), } } fn read_task( + &'static self, queue_rx: &'static Queue, mut rx: UartRx<'static, UART, Async>, ) -> ReadTaskFuture { async move { loop { let mut buf = queue_rx.enqueue().await.unwrap(); - let len = rx - .read_until_idle(buf.data()) - .await; - // .unwrap(); - // TODO: this - if let Ok(len) = len { - if len == 0 { - info!("uart zero"); - buf.cancel(); - } else { - *buf.len() = len; + + { + let _rw_tasks_config_lock = self.uart_mutex.lock().await; + + match select(rx.read_until_idle(buf.data()), Timer::after_millis(500)).await { + Either::First(len) => { + if let Ok(len) = len { + if len == 0 { + defmt::debug!("uart zero"); + buf.cancel(); + } else { + *buf.len() = len; + } + } else { + // Framing and Parity Error occur here + defmt::warn!("{}", len); + buf.cancel(); + } + }, + Either::Second(_) => { + defmt::trace!("UartReadQueue - Read to idle timed out") + } } - } else { - info!("{}", len); - buf.cancel(); - } + } // frees the inter-task uart config lock } } } @@ -95,7 +108,7 @@ impl< &'static self, rx: UartRx<'static, UART, Async>, ) -> SpawnToken { - self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) + self.task.spawn(|| self.read_task(&self.queue_rx, rx)) } pub fn try_dequeue(&self) -> Result, Error> { @@ -114,7 +127,10 @@ pub struct UartWriteQueue< const LENGTH: usize, const DEPTH: usize, > { + uart_mutex: Mutex, queue_tx: Queue, + has_new_uart_config: AtomicBool, + new_uart_config: Mutex>, task: TaskStorage>, } @@ -133,38 +149,55 @@ impl< const DEPTH: usize, > UartWriteQueue { - pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH]) -> Self { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH], uart_mutex: Mutex) -> Self { Self { + uart_mutex: uart_mutex, queue_tx: Queue::new(buffers), + has_new_uart_config: AtomicBool::new(false), + new_uart_config: Mutex::new(None), task: TaskStorage::new(), } } fn write_task( + &'static self, queue_tx: &'static Queue, mut tx: UartTx<'static, UART, Async>, ) -> WriteTaskFuture { async move { loop { let buf = queue_tx.dequeue().await.unwrap(); + + if self.has_new_uart_config.load(Ordering::Relaxed) { + // acquire the lock on the config + let new_config = self.new_uart_config.lock().await; + // acquire the lock on the shared UART config + let _rw_tasks_config_lock = self.uart_mutex.lock().await; + + let config_res = tx.set_config(&new_config.unwrap()); + if config_res.is_err() { + defmt::warn!("failed to apply uart config in uart write queue"); + } else { + defmt::debug!("updated config in uart write queue"); + } + + self.has_new_uart_config.store(false, Ordering::Relaxed) + } // frees the inter-task uart config lock + // defmt::info!("invoking API write"); tx.write(buf.data()).await.unwrap(); // we are blocked here! // defmt::info!("passed API write"); drop(buf); - // unsafe { - // // TODO: what does this do again? - // while !UART::regs().isr().read().tc() {} - // UART::regs().cr1().modify(|w| w.set_te(false)); - // while UART::regs().isr().read().teack() {} - // UART::regs().cr1().modify(|w| w.set_te(true)); - // } + + // NOTE: we used to check for DMA transaction complete here, but embassy added + // it some time ago. Doing it twice causes lockup. } } } pub fn spawn_task(&'static self, tx: UartTx<'static, UART, Async>) -> SpawnToken { - self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) + self.task.spawn(|| self.write_task(&self.queue_tx, tx)) } pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { @@ -180,6 +213,14 @@ impl< source.len() }) } + + pub async fn update_uart_config(&self, config: usart::Config) { + { + let mut new_config = self.new_uart_config.lock().await; + let _ = new_config.insert(config); + } + self.has_new_uart_config.store(true, Ordering::Relaxed); + } } pub trait Reader { From 5c38ebf75838b1f186ecb43b11281b3de345caab Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 6 May 2024 23:29:47 -0400 Subject: [PATCH 022/157] lots of fixes for control firmware on new deps --- Makefile | 4 +- control-board/Cargo.lock | 3 + control-board/Cargo.toml | 5 - control-board/src/bin/control/control.rs | 8 +- control-board/src/bin/control/main.rs | 35 +- control-board/src/bin/hwtest-enc/pins.rs | 42 -- control-board/src/bin/hwtest-imu/pins.rs | 43 -- .../src/bin/hwtest-kicker-coms/main.rs | 9 +- .../src/bin/hwtest-kicker-coms/pins.rs | 47 -- control-board/src/bin/hwtest-kicker/main.rs | 10 +- control-board/src/bin/hwtest-kicker/pins.rs | 47 -- control-board/src/bin/hwtest-motor/pins.rs | 42 -- .../src/bin/proto-drivetrain/main.rs | 336 ------------ control-board/src/bin/robot/control.rs | 508 ------------------ control-board/src/bin/robot/main.rs | 386 ------------- control-board/src/bin/robot/pins.rs | 42 -- control-board/src/bin/robot/radio.rs | 421 --------------- control-board/src/drivers/kicker.rs | 43 +- .../src/drivers/radio/at_protocol.rs | 40 +- control-board/src/drivers/radio/radio.rs | 10 +- .../src/drivers/radio/radio_robot.rs | 47 +- control-board/src/drivers/rotary.rs | 35 +- control-board/src/drivers/shell_indicator.rs | 14 +- control-board/src/lib.rs | 9 +- control-board/src/main.rs | 77 --- control-board/src/{bin/control => }/pins.rs | 0 control-board/src/queue.rs | 220 -------- control-board/src/stm32_interface.rs | 125 ++--- control-board/src/stspin_motor.rs | 24 +- control-board/src/uart_queue.rs | 204 ------- lib-stm32/src/uart/queue.rs | 41 +- 31 files changed, 208 insertions(+), 2669 deletions(-) delete mode 100644 control-board/src/bin/hwtest-enc/pins.rs delete mode 100644 control-board/src/bin/hwtest-imu/pins.rs delete mode 100644 control-board/src/bin/hwtest-kicker-coms/pins.rs delete mode 100644 control-board/src/bin/hwtest-kicker/pins.rs delete mode 100644 control-board/src/bin/hwtest-motor/pins.rs delete mode 100644 control-board/src/bin/proto-drivetrain/main.rs delete mode 100644 control-board/src/bin/robot/control.rs delete mode 100644 control-board/src/bin/robot/main.rs delete mode 100644 control-board/src/bin/robot/pins.rs delete mode 100644 control-board/src/bin/robot/radio.rs delete mode 100644 control-board/src/main.rs rename control-board/src/{bin/control => }/pins.rs (100%) delete mode 100644 control-board/src/queue.rs delete mode 100644 control-board/src/uart_queue.rs diff --git a/Makefile b/Makefile index ac5f58c7..ccba45bc 100644 --- a/Makefile +++ b/Makefile @@ -111,7 +111,7 @@ define create-kicker-board-rust-targets $1--$2: cd $1 && \ cargo build --release --bin $2 && \ - arm-none-eabi-objcopy -Obinary target/thumbv6m-none-eabi/release/$2 target/thumbv6m-none-eabi/release/$2.bin + arm-none-eabi-objcopy -Obinary target/thumbv7em-none-eabihf/release/$2 target/thumbv7em-none-eabihf/release/$2.bin kicker-board--all:: $1--$2 $1--$2--run: $1--$2 @@ -121,7 +121,7 @@ $1--$2--run: $1--$2 $1--$2--debug: $1--$2 cd $1 && \ cargo build --release --bin $2 && \ - ../util/program.sh $3 $1/target/thumbv6m-none-eabi/release/$2 + ../util/program.sh $3 $1/target/thumbv7em-none-eabihf/release/$2 endef $(foreach element,$(kicker_binaries),$(eval $(call create-kicker-board-rust-targets,kicker-board,$(element),$(kicker_openocd_cfg_file)))) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index d9a07f05..cf34f1cf 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -75,7 +75,10 @@ dependencies = [ "defmt", "defmt-rtt", "embassy-executor", + "embassy-futures", "embassy-stm32", + "embassy-sync", + "embassy-time", ] [[package]] diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index 14607938..020e1220 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -75,11 +75,6 @@ lto = 'fat' test = false harness = false -[[bin]] -name = "ateam-control-board" -test = false -harness = false - [[bin]] name = "control" test = false diff --git a/control-board/src/bin/control/control.rs b/control-board/src/bin/control/control.rs index 031e8d44..52e111ed 100644 --- a/control-board/src/bin/control/control.rs +++ b/control-board/src/bin/control/control.rs @@ -1,4 +1,4 @@ -use ateam_common_packets::{bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, ParameterName}}; +use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, ParameterName}; use embassy_executor::SendSpawner; use embassy_stm32::{ gpio::{Level, Output, Speed}, @@ -7,21 +7,21 @@ use embassy_stm32::{ use embassy_time::{Duration, Timer}; use ateam_control_board::{ include_external_cpp_bin, - queue::Buffer, stm32_interface::Stm32Interface, stspin_motor::{WheelMotor, DribblerMotor}, - uart_queue::{UartReadQueue, UartWriteQueue}, motion::{ robot_model::{RobotConstants, RobotModel}, robot_controller::BodyVelocityController }, BATTERY_MIN_VOLTAGE, parameter_interface::ParameterInterface }; +use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use nalgebra::{Vector3, Vector4}; use embassy_sync::pubsub::Subscriber; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use crate::pins::*; +use ateam_control_board::pins::*; include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 75245cc6..3edb25c4 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -8,11 +8,13 @@ use apa102_spi::Apa102; use ateam_control_board::{ adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ - radio::TeamColor, radio::WifiNetwork, rotary::Rotary, shell_indicator::ShellIndicator, kicker::Kicker, + radio::TeamColor, + radio::WifiNetwork, + rotary::Rotary, + shell_indicator::ShellIndicator, + kicker::Kicker, }, - queue::Buffer, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, - uart_queue::{UartReadQueue, UartWriteQueue}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE, include_kicker_bin, parameter_interface::ParameterInterface, }; use control::Control; @@ -20,19 +22,18 @@ use defmt::info; use embassy_stm32::{ adc::{Adc, SampleTime}, dma::NoDma, - executor::InterruptExecutor, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, + interrupt, peripherals::{DMA2_CH4, DMA2_CH5, USART6}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog, }; -use embassy_time::{Delay, Duration, Ticker, Timer}; -use futures_util::StreamExt; -use pins::{ +use embassy_executor::InterruptExecutor; +use embassy_time::{Duration, Ticker, Timer}; +use ateam_control_board::pins::{ PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, ShutdownCompletePin, }; @@ -45,10 +46,12 @@ use static_cell::StaticCell; use embassy_stm32::rcc::AdcClockSource; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::{PubSubChannel}; +use embassy_sync::pubsub::PubSubChannel; + +use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; mod control; -mod pins; mod radio; #[cfg(not(feature = "no-private-credentials"))] @@ -130,8 +133,6 @@ async fn main(_spawner: embassy_executor::Spawner) { p.SPI3, p.PB3, p.PB5, - NoDma, - NoDma, hz(1_000_000), spi::Config::default(), ); @@ -195,13 +196,15 @@ async fn main(_spawner: embassy_executor::Spawner) { // Battery Voltage // ////////////////////// - let mut adc3 = Adc::new(p.ADC3, &mut Delay); + let mut adc3 = Adc::new(p.ADC3); adc3.set_sample_time(SampleTime::Cycles1_5); let mut battery_pin = p.PF5; let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); + let imu_spi_config = spi::Config::default(); + imu_spi_config.frequency = mhz(1); let mut imu_spi = spi::Spi::new( p.SPI6, p.PA5, @@ -209,8 +212,7 @@ async fn main(_spawner: embassy_executor::Spawner) { p.PA6, p.BDMA_CH0, p.BDMA_CH1, - hz(1_000_000), - spi::Config::default(), + imu_spi_config, ); // acceleromter @@ -507,9 +509,8 @@ async fn power_off_task( exti: PowerStateExti, shutdown_pin: ShutdownCompletePin, ) { - let power_state = Input::new(power_state_pin, Pull::None); let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state, exti); + let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); power_state.wait_for_falling_edge().await; shutdown.set_high(); loop {} diff --git a/control-board/src/bin/hwtest-enc/pins.rs b/control-board/src/bin/hwtest-enc/pins.rs deleted file mode 100644 index fa1db175..00000000 --- a/control-board/src/bin/hwtest-enc/pins.rs +++ /dev/null @@ -1,42 +0,0 @@ -#![allow(dead_code)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/hwtest-imu/pins.rs b/control-board/src/bin/hwtest-imu/pins.rs deleted file mode 100644 index 4451ccd7..00000000 --- a/control-board/src/bin/hwtest-imu/pins.rs +++ /dev/null @@ -1,43 +0,0 @@ -#![allow(dead_code)] -#![allow(unused_variables)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index bc008ccf..aa674cf0 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -28,8 +28,6 @@ use panic_probe as _; use smart_leds::{SmartLedsWrite, RGB8}; use static_cell::StaticCell; -mod pins; - include_kicker_bin! {KICKER_FW_IMG, "hwtest-coms.bin"} const MAX_TX_PACKET_SIZE: usize = 16; @@ -69,14 +67,15 @@ async fn main(_spawner: embassy_executor::Spawner) { let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); let spawner = executor.start(); + let dotstar_spi_config = spi::Config::default(); + dotstar_spi_config.frequency = mhz(1); + let dotstar_spi = spi::Spi::new_txonly( p.SPI3, p.PB3, p.PB5, NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), + dotstar_spi_config, ); let mut dotstar = Apa102::new(dotstar_spi); let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); diff --git a/control-board/src/bin/hwtest-kicker-coms/pins.rs b/control-board/src/bin/hwtest-kicker-coms/pins.rs deleted file mode 100644 index 72e8998a..00000000 --- a/control-board/src/bin/hwtest-kicker-coms/pins.rs +++ /dev/null @@ -1,47 +0,0 @@ -#![allow(dead_code)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; -pub type KickerUart = USART6; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; -pub type KickerDmaTx = DMA2_CH4; -pub type KickerDmaRx = DMA2_CH5; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; -pub type KickerBootPin = PA8; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; -pub type KickerResetPin = PA9; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index 02b0a07a..719faef9 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -30,8 +30,6 @@ use static_cell::StaticCell; use ateam_common_packets::bindings_kicker::KickRequest; -mod pins; - include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} const MAX_TX_PACKET_SIZE: usize = 16; @@ -71,15 +69,17 @@ async fn main(_spawner: embassy_executor::Spawner) { let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); let spawner = executor.start(); + let dotstar_spi_config = spi::Config::default(); + dotstar_spi_config.frequency = mhz(1); + let dotstar_spi = spi::Spi::new_txonly( p.SPI3, p.PB3, p.PB5, NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), + dotstar_spi_config, ); + let mut dotstar = Apa102::new(dotstar_spi); let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); diff --git a/control-board/src/bin/hwtest-kicker/pins.rs b/control-board/src/bin/hwtest-kicker/pins.rs deleted file mode 100644 index 72e8998a..00000000 --- a/control-board/src/bin/hwtest-kicker/pins.rs +++ /dev/null @@ -1,47 +0,0 @@ -#![allow(dead_code)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; -pub type KickerUart = USART6; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; -pub type KickerDmaTx = DMA2_CH4; -pub type KickerDmaRx = DMA2_CH5; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; -pub type KickerBootPin = PA8; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; -pub type KickerResetPin = PA9; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/hwtest-motor/pins.rs b/control-board/src/bin/hwtest-motor/pins.rs deleted file mode 100644 index fa1db175..00000000 --- a/control-board/src/bin/hwtest-motor/pins.rs +++ /dev/null @@ -1,42 +0,0 @@ -#![allow(dead_code)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/proto-drivetrain/main.rs b/control-board/src/bin/proto-drivetrain/main.rs deleted file mode 100644 index 919fc5f1..00000000 --- a/control-board/src/bin/proto-drivetrain/main.rs +++ /dev/null @@ -1,336 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(async_closure)] - -use defmt_rtt as _; -use defmt::*; -use embassy_stm32::{ - self, - executor::InterruptExecutor, - interrupt::{self, InterruptExt}, - gpio::{Level, Speed, Output}, - peripherals::{DMA1_CH0, DMA1_CH1, DMA1_CH2, DMA1_CH3, DMA1_CH4, DMA1_CH5, DMA1_CH6, DMA1_CH7}, - peripherals::{USART3, UART4, UART5, UART7}, - usart::Uart, - time::mhz, -}; -use embassy_time::{Duration, Ticker, Timer}; -use futures_util::StreamExt; -use nalgebra::{Vector4, Vector3}; -use panic_probe as _; -use static_cell::StaticCell; - -use ateam_control_board::{ - stm32_interface::{Stm32Interface, self}, - queue::Buffer, - uart_queue::{UartReadQueue, UartWriteQueue}, - include_external_cpp_bin, stspin_motor::WheelMotor, motion::robot_model::{RobotConstants, RobotModel}, -}; - -// include_external_cpp_bin!{STEVAL3204_DRIB_POTCTRL_FW_IMG, "dev3204-drib-potctrl.bin"} -// include_external_cpp_bin!{STEVAL3204_DRIB_FW_IMG, "dev3204-drib.bin"} -include_external_cpp_bin!{STEVAL3204_WHEEL_FW_IMG, "wheel.bin"} - - -// motor pinout -// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA0/1 -// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA2/3 -// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA4/5 -// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA6/7 - -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 20; - -// buffers for front right -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_RX }); - -// buffers for front left -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_RX }); - -// buffers for back left -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_RX }); - -// buffers for back right -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_RX }); - -// executor queue -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); - -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - info!("Startup"); - - // setup system clocks - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - // place the stspins in reset as early as possible just in case we have lingering state - // that wants to spin motors - let front_right_reset_pin = - Output::new(p.PB2, Level::Low, Speed::Medium); // reset active -let front_left_reset_pin = - Output::new(p.PG3, Level::Low, Speed::Medium); // reset active -let back_left_reset_pin = - Output::new(p.PG1, Level::Low, Speed::Medium); // reset active -let back_right_reset_pin = - Output::new(p.PA3, Level::Low, Speed::Medium); // reset active - - // setup the async executor - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - - // setup leds - let mut _grn_led = Output::new(p.PB0, Level::High, Speed::Medium); - let mut ylw_led = Output::new(p.PE1, Level::High, Speed::Medium); - let mut red_led = Output::new(p.PB14, Level::High, Speed::Medium); - ylw_led.set_low(); - red_led.set_low(); - - - ///////////////////////////////STEVAL - /////////////////////////////// - - let wheel_firmware_image = STEVAL3204_WHEEL_FW_IMG; - - /* FRONT RIGHT WHEEL - UART5 */ - // FrontRight Wheel - UART5 - tx pb6, rx pb12, boot pb1, rst pb2 - - // front right IO - let front_right_uart_config = stm32_interface::get_bootloader_uart_config(); - let front_right_int = interrupt::take!(UART5); - let front_right_usart = Uart::new(p.UART5, p.PB12, p.PB6, front_right_int, p.DMA1_CH0, p.DMA1_CH1, front_right_uart_config); - let (front_right_tx, front_right_rx) = front_right_usart.split(); - let front_right_boot0_pin = Output::new(p.PB1, Level::Low, Speed::Medium); // boot0 not active - - // register front right uart primitives with executor - spawner.spawn(FRONT_RIGHT_QUEUE_RX.spawn_task(front_right_rx)).unwrap(); - spawner.spawn(FRONT_RIGHT_QUEUE_TX.spawn_task(front_right_tx)).unwrap(); - - // initialize the wheel - let front_right_stm32_interface = Stm32Interface::new(&FRONT_RIGHT_QUEUE_RX, &FRONT_RIGHT_QUEUE_TX, Some(front_right_boot0_pin), Some(front_right_reset_pin)); - let mut front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); - - - /* FRONT LEFT WHEEL - UART7 */ - // FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3 - - // front left IO - let front_left_uart_config = stm32_interface::get_bootloader_uart_config(); - let front_left_int = interrupt::take!(UART7); - let front_left_usart = Uart::new(p.UART7, p.PF6, p.PF7, front_left_int, p.DMA1_CH2, p.DMA1_CH3, front_left_uart_config); - let (front_left_tx, front_left_rx) = front_left_usart.split(); - let front_left_boot0_pin = Output::new(p.PG2, Level::Low, Speed::Medium); // boot0 not active - - // register front left uart primitives with executor - spawner.spawn(FRONT_LEFT_QUEUE_RX.spawn_task(front_left_rx)).unwrap(); - spawner.spawn(FRONT_LEFT_QUEUE_TX.spawn_task(front_left_tx)).unwrap(); - - // initialize the wheel - let front_left_stm32_interface = Stm32Interface::new(&FRONT_LEFT_QUEUE_RX, &FRONT_LEFT_QUEUE_TX, Some(front_left_boot0_pin), Some(front_left_reset_pin)); - let mut front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); - - - /* BACK LEFT WHEEL */ - // BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1 - - // back left IO - let back_left_uart_config = stm32_interface::get_bootloader_uart_config(); - let back_left_int = interrupt::take!(UART4); - let back_left_usart = Uart::new(p.UART4, p.PD0, p.PD1, back_left_int, p.DMA1_CH4, p.DMA1_CH5, back_left_uart_config); - let (back_left_tx, back_left_rx) = back_left_usart.split(); - let back_left_boot0_pin = Output::new(p.PG0, Level::Low, Speed::Medium); // boot0 not active - - // register front left uart primitives with executor - spawner.spawn(BACK_LEFT_QUEUE_RX.spawn_task(back_left_rx)).unwrap(); - spawner.spawn(BACK_LEFT_QUEUE_TX.spawn_task(back_left_tx)).unwrap(); - - // initialize the wheel - let back_left_stm32_interface = Stm32Interface::new(&BACK_LEFT_QUEUE_RX, &BACK_LEFT_QUEUE_TX, Some(back_left_boot0_pin), Some(back_left_reset_pin)); - let mut back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); - - - /* BACK RIGHT WHEEL */ - // BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3 - - // back right IO - let back_right_uart_config = stm32_interface::get_bootloader_uart_config(); - let back_right_int = interrupt::take!(USART3); - let back_right_usart = Uart::new(p.USART3, p.PB11, p.PB10, back_right_int, p.DMA1_CH6, p.DMA1_CH7, back_right_uart_config); - let (back_right_tx, back_right_rx) = back_right_usart.split(); - let back_right_boot0_pin = Output::new(p.PF4, Level::Low, Speed::Medium); // boot0 not active - - // register back right uart primitives with executor - spawner.spawn(BACK_RIGHT_QUEUE_RX.spawn_task(back_right_rx)).unwrap(); - spawner.spawn(BACK_RIGHT_QUEUE_TX.spawn_task(back_right_tx)).unwrap(); - - // initialize the wheel - let back_right_stm32_interface = Stm32Interface::new(&BACK_RIGHT_QUEUE_RX, &BACK_RIGHT_QUEUE_TX, Some(back_right_boot0_pin), Some(back_right_reset_pin)); - let mut back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); - - //////////////////////////// - // load firmware images // - //////////////////////////// - - // load firmware (use join?) - // // defmt::info!("flashing front right"); - // front_right_motor.load_default_firmware_image().await; - // // defmt::info!("flashing front left"); - // front_left_motor.load_default_firmware_image().await; - // // defmt::info!("flashing back left"); - // back_left_motor.load_default_firmware_image().await; - // // defmt::info!("flashing back right"); - // back_right_motor.load_default_firmware_image().await; - let _res = embassy_futures::join::join4(front_right_motor.load_default_firmware_image(), - front_left_motor.load_default_firmware_image(), - back_left_motor.load_default_firmware_image(), - back_right_motor.load_default_firmware_image() - ).await; - - info!("after load firmware"); - - // leave reset - // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue - embassy_futures::join::join4(front_right_motor.leave_reset(), - front_left_motor.leave_reset(), - back_left_motor.leave_reset(), - back_right_motor.leave_reset()).await; - - info!("after leave reset"); - - front_right_motor.set_telemetry_enabled(true); - front_left_motor.set_telemetry_enabled(true); - back_left_motor.set_telemetry_enabled(true); - back_right_motor.set_telemetry_enabled(true); - - info!("after telemetry enable"); - - // need to have telem off by default and enabled later - // theres a race condition to begin processing packets from the first part out - // of reset and waiting for the last part to boot up - Timer::after(Duration::from_millis(5)).await; - - // front_right_motor.leave_reset().await; - // front_left_motor.leave_reset().await; - // back_left_motor.leave_reset().await; - // back_right_motor.leave_reset().await; - - // robot params - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians()), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M), - - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - info!("before"); - Timer::after(Duration::from_millis(1000)).await; - - ///////////////// - // main loop // - ///////////////// - - // 100 Hz loop rate - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - // let mut angle: f32 = 0.0; - let mut angle: f32 = core::f32::consts::PI / 2.0; - loop { - // info!("tick"); - front_right_motor.process_packets(); - front_left_motor.process_packets(); - back_left_motor.process_packets(); - back_right_motor.process_packets(); - - let vel = 0.1; // DC - let cmd_vel: Vector3 = Vector3::new(libm::cosf(angle) * vel, libm::sinf(angle) * vel, 0.0); - defmt::info!("cmd vel [{:?},{:?},{:?}]", cmd_vel[0], cmd_vel[1], cmd_vel[2]); - let wheel_vels = robot_model.robot_vel_to_wheel_vel(cmd_vel); - defmt::info!("set duty cycles [{:?},{:?},{:?},{:?}]", wheel_vels[0], wheel_vels[1], wheel_vels[2], wheel_vels[3]); - let rec_body_vel = robot_model.wheel_vel_to_robot_vel(wheel_vels); - defmt::info!("rec_body_vel [{:?},{:?},{:?}]", rec_body_vel[0], rec_body_vel[1], rec_body_vel[2]); - - front_right_motor.set_setpoint(wheel_vels[0]); - front_left_motor.set_setpoint(wheel_vels[1]); - back_left_motor.set_setpoint(wheel_vels[2]); - back_right_motor.set_setpoint(wheel_vels[3]); - angle += core::f32::consts::FRAC_2_PI / 200.0; - - front_right_motor.send_motion_command(); - front_left_motor.send_motion_command(); - back_left_motor.send_motion_command(); - back_right_motor.send_motion_command(); - - defmt::info!("FR = torque: {:?}, RPM {:?}", front_right_motor.read_current(), front_right_motor.read_rpm()); - // defmt::info!("encoder delta: {:?}", front_right_motor.read_encoder_delta()); - // defmt::info!("vel rad/s: {:?}", front_right_motor.read_rads() * 60.0 / (2.0 * PI)); - // defmt::info!("vel rad/s: {:?}", front_right_motor.read_rads()); - // defmt::info!("vel RPM: {:?}", front_right_motor.read_rads() * 60.0 / (2.0 * PI)); - - - - main_loop_rate_ticker.next().await; - } -} \ No newline at end of file diff --git a/control-board/src/bin/robot/control.rs b/control-board/src/bin/robot/control.rs deleted file mode 100644 index f77e4375..00000000 --- a/control-board/src/bin/robot/control.rs +++ /dev/null @@ -1,508 +0,0 @@ -use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry}; -use embassy_executor::SendSpawner; -use embassy_stm32::{ - gpio::{Level, Output, Speed}, - usart::Uart, -}; -use embassy_time::{Duration, Timer}; -use ateam_control_board::{ - include_external_cpp_bin, - queue::Buffer, - motion::robot_model::{RobotConstants, RobotModel}, - stm32_interface::Stm32Interface, - stspin_motor::{WheelMotor, DribblerMotor}, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; -use nalgebra::{Vector3, Vector4}; - -use crate::pins::*; - -include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} -// include_external_cpp_bin! {DRIB_FW_IMG, "wheel.bin"} -include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} - -// motor pinout -// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 -// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 -// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 -// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 -// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 - -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 20; - -const TICKS_WITHOUT_PACKET_STOP: u16 = 25; - -// buffers for front right -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_RX }); - -// buffers for front left -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_RX }); - -// buffers for back left -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_RX }); - -// buffers for back right -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_RX }); - -// buffers for dribbler -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static DRIB_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut DRIB_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static DRIB_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut DRIB_BUFFERS_RX }); - -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -pub struct Control { - robot_model: RobotModel, - cmd_vel: Vector3, - drib_vel: f32, - front_right_motor: WheelMotor< - 'static, - MotorFRUart, - MotorFRDmaRx, - MotorFRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFRBootPin, - MotorFRResetPin, - >, - front_left_motor: WheelMotor< - 'static, - MotorFLUart, - MotorFLDmaRx, - MotorFLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFLBootPin, - MotorFLResetPin, - >, - back_left_motor: WheelMotor< - 'static, - MotorBLUart, - MotorBLDmaRx, - MotorBLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBLBootPin, - MotorBLResetPin, - >, - back_right_motor: WheelMotor< - 'static, - MotorBRUart, - MotorBRDmaRx, - MotorBRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBRBootPin, - MotorBRResetPin, - >, - drib_motor: DribblerMotor< - 'static, - MotorDUart, - MotorDDmaRx, - MotorDDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorDBootPin, - MotorDResetPin, - >, - ticks_since_packet: u16, -} - -// Uart - -impl Control { - pub fn new( - spawner: &SendSpawner, - front_right_usart: Uart<'static, MotorFRUart, MotorFRDmaTx, MotorFRDmaRx>, - front_left_usart: Uart<'static, MotorFLUart, MotorFLDmaTx, MotorFLDmaRx>, - back_left_usart: Uart<'static, MotorBLUart, MotorBLDmaTx, MotorBLDmaRx>, - back_right_usart: Uart<'static, MotorBRUart, MotorBRDmaTx, MotorBRDmaRx>, - drib_usart: Uart<'static, MotorDUart, MotorDDmaTx, MotorDDmaRx>, - front_right_boot0_pin: MotorFRBootPin, - front_left_boot0_pin: MotorFLBootPin, - back_left_boot0_pin: MotorBLBootPin, - back_right_boot0_pin: MotorBRBootPin, - drib_boot0_pin: MotorDBootPin, - front_right_reset_pin: MotorFRResetPin, - front_left_reset_pin: MotorFLResetPin, - back_left_reset_pin: MotorBLResetPin, - back_right_reset_pin: MotorBRResetPin, - drib_reset_pin: MotorDResetPin, - ball_detected_thresh: f32, - ) -> Control { - let wheel_firmware_image = WHEEL_FW_IMG; - let drib_firmware_image = DRIB_FW_IMG; - - let (front_right_tx, front_right_rx) = front_right_usart.split(); - let (front_left_tx, front_left_rx) = front_left_usart.split(); - let (back_left_tx, back_left_rx) = back_left_usart.split(); - let (back_right_tx, back_right_rx) = back_right_usart.split(); - let (drib_tx, drib_rx) = drib_usart.split(); - - let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - - let front_right_reset_pin = - Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active - let front_left_reset_pin = - Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_left_reset_pin = - Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_right_reset_pin = - Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active - let drib_reset_pin = - Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active - - spawner - .spawn(FRONT_RIGHT_QUEUE_RX.spawn_task(front_right_rx)) - .unwrap(); - spawner - .spawn(FRONT_RIGHT_QUEUE_TX.spawn_task(front_right_tx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_RX.spawn_task(front_left_rx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_TX.spawn_task(front_left_tx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_QUEUE_RX.spawn_task(back_left_rx)) - .unwrap(); - spawner // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); - // // info!("\n"); - // // // info!("vel set {:?}", mrp.data.motion.vel_setpoint + 0.); - // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); - // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); - // // info!("\n"); - // // // info!("vel set {:?}", mrp.data.motion.vel_setpoint + 0.); - // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); - // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); - // info!("master_error {:?}", mrp.data.motion.master_error()); - // info!("{:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); - // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); - // info!("bldc_transition_error {:?}", mrp.data.motion.bldc_transition_error()); - // info!("bldc_commutation_watchdog_error {:?}", mrp.data.motion.bldc_commutation_watchdog_error()); - // info!("enc_disconnected_error {:?}", mrp.data.motion.enc_disconnected_error()); - // info!("enc_decoding_error {:?}", mrp.data.motion.enc_decoding_error()); - // info!("hall_enc_vel_disagreement_error {:?}", mrp.data.motion.hall_enc_vel_disagreement_error()); - // info!("overcurrent_error {:?}", mrp.data.motion.overcurrent_error()); - // info!("undervoltage_error {:?}", mrp.data.motion.undervoltage_error()); - // info!("overvoltage_error {:?}", mrp.data.motion.overvoltage_error()); - // info!("torque_limited {:?}", mrp.data.motion.torque_limited()); - // info!("control_loop_time_error {:?}", mrp.data.motion.control_loop_time_error()); - // info!("reset_watchdog_independent {:?}", mrp.data.motion.reset_watchdog_independent()); - // info!("reset_watchdog_window {:?}", mrp.data.motion.reset_watchdog_window()); - // info!("reset_low_power {:?}", mrp.data.motion.reset_low_power()); - // info!("reset_software {:?}", mrp.data.motion.reset_software()); 0.); - // info!("master_error {:?}", mrp.data.motion.master_error()); - // info!("{:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); - // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); - // info!("bldc_transition_error {:?}", mrp.data.motion.bldc_transition_error()); - // info!("bldc_commutation_watchdog_error {:?}", mrp.data.motion.bldc_commutation_watchdog_error()); - // info!("enc_disconnected_error {:?}", mrp.data.motion.enc_disconnected_error()); - // info!("enc_decoding_error {:?}", mrp.data.motion.enc_decoding_error()); - // info!("hall_enc_vel_disagreement_error {:?}", mrp.data.motion.hall_enc_vel_disagreement_error()); - // info!("overcurrent_error {:?}", mrp.data.motion.overcurrent_error()); - // info!("undervoltage_error {:?}", mrp.data.motion.undervoltage_error()); - // info!("overvoltage_error {:?}", mrp.data.motion.overvoltage_error()); - // info!("torque_limited {:?}", mrp.data.motion.torque_limited()); - // info!("control_loop_time_error {:?}", mrp.data.motion.control_loop_time_error()); - // info!("reset_watchdog_independent {:?}", mrp.data.motion.reset_watchdog_independent()); - // info!("reset_watchdog_window {:?}", mrp.data.motion.reset_watchdog_window()); - // info!("reset_low_power {:?}", mrp.data.motion.reset_low_power()); - // info!("reset_software {:?}", mrp.data.motion.reset_software()); - .spawn(BACK_LEFT_QUEUE_TX.spawn_task(back_left_tx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_RX.spawn_task(back_right_rx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_TX.spawn_task(back_right_tx)) - .unwrap(); - spawner - .spawn(DRIB_QUEUE_RX.spawn_task(drib_rx)) - .unwrap(); - spawner - .spawn(DRIB_QUEUE_TX.spawn_task(drib_tx)) - .unwrap(); - - let front_right_stm32_interface = Stm32Interface::new( - &FRONT_RIGHT_QUEUE_RX, - &FRONT_RIGHT_QUEUE_TX, - Some(front_right_boot0_pin), - Some(front_right_reset_pin), - ); - let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); - - let front_left_stm32_interface = Stm32Interface::new( - &FRONT_LEFT_QUEUE_RX, - &FRONT_LEFT_QUEUE_TX, - Some(front_left_boot0_pin), - Some(front_left_reset_pin), - ); - let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); - - let back_left_stm32_interface = Stm32Interface::new( - &BACK_LEFT_QUEUE_RX, - &BACK_LEFT_QUEUE_TX, - Some(back_left_boot0_pin), - Some(back_left_reset_pin), - ); - let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); - - let back_right_stm32_interface = Stm32Interface::new( - &BACK_RIGHT_QUEUE_RX, - &BACK_RIGHT_QUEUE_TX, - Some(back_right_boot0_pin), - Some(back_right_reset_pin), - ); - let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); - - let drib_stm32_interface = Stm32Interface::new( - &DRIB_QUEUE_RX, - &DRIB_QUEUE_TX, - Some(drib_boot0_pin), - Some(drib_reset_pin), - ); - let drib_motor = DribblerMotor::new(drib_stm32_interface, drib_firmware_image, ball_detected_thresh); - - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - Control { - robot_model, - cmd_vel: Vector3::new(0., 0., 0.), - drib_vel: 0.0, - front_right_motor, - front_left_motor, - back_left_motor, - back_right_motor, - drib_motor, - ticks_since_packet: 0, - } - } - - pub async fn load_firmware(&mut self) { - let _res = embassy_futures::join::join5( - self.front_right_motor.load_default_firmware_image(), - self.front_left_motor.load_default_firmware_image(), - self.back_left_motor.load_default_firmware_image(), - self.back_right_motor.load_default_firmware_image(), - self.drib_motor.load_default_firmware_image(), - ) - .await; - - // defmt::info!("flashing firmware"); - - // self.front_right_motor.load_default_firmware_image().await; - // defmt::info!("FR flashed"); - - // self.front_left_motor.load_default_firmware_image().await; - // defmt::info!("FL flashed"); - - // self.back_left_motor.load_default_firmware_image().await; - // defmt::info!("BL flashed"); - - // self.back_right_motor.load_default_firmware_image().await; - // defmt::info!("BR flashed"); - - // self.drib_motor.load_default_firmware_image().await; - // defmt::info!("DRIB flashed"); - - - - defmt::info!("flashed"); - - // leave reset - // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue - embassy_futures::join::join5( - self.front_right_motor.leave_reset(), - self.front_left_motor.leave_reset(), - self.back_left_motor.leave_reset(), - self.back_right_motor.leave_reset(), - self.drib_motor.leave_reset(), - ) - .await; - - self.front_right_motor.set_telemetry_enabled(true); - self.front_left_motor.set_telemetry_enabled(true); - self.back_left_motor.set_telemetry_enabled(true); - self.back_right_motor.set_telemetry_enabled(true); - self.drib_motor.set_telemetry_enabled(true); - - // need to have telem off by default and enabled later - // theres a race condition to begin processing packets from the first part out - // of reset and waiting for the last part to boot up - Timer::after(Duration::from_millis(10)).await; - } - - pub fn tick(&mut self, latest_control: Option) -> Option { - self.front_right_motor.process_packets(); - self.front_left_motor.process_packets(); - self.back_left_motor.process_packets(); - self.back_right_motor.process_packets(); - self.drib_motor.process_packets(); - - self.front_right_motor.log_reset("FR"); - self.front_left_motor.log_reset("RL"); - self.back_left_motor.log_reset("BL"); - self.back_right_motor.log_reset("BR"); - self.drib_motor.log_reset("DRIB"); - - if self.drib_motor.ball_detected() { - defmt::info!("ball detected"); - } - - // let vel = 0.0005; // DC - // let angle: f32 = core::f32::consts::PI / 4.0; - // let cmd_vel: Vector3 = - // Vector3::new(libm::sinf(angle) * vel, libm::cosf(angle) * vel, 0.0); - if let Some(latest_control) = &latest_control { - let cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); - self.cmd_vel = cmd_vel; - self.drib_vel = latest_control.dribbler_speed; - self.ticks_since_packet = 0; - } else { - self.ticks_since_packet += 1; - if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - self.cmd_vel = Vector3::new(0., 0., 0.); - self.ticks_since_packet = 0; - } - } - let wheel_vels = self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel); - - // let c_vel = libm::sinf(angle) / 2.0; - // let c_vel = 0.2; - self.front_right_motor.set_setpoint(wheel_vels[0]); - self.front_left_motor.set_setpoint(wheel_vels[1]); - self.back_left_motor.set_setpoint(wheel_vels[2]); - self.back_right_motor.set_setpoint(wheel_vels[3]); - // angle += core::f32::consts::FRAC_2_PI / 200.0; - let drib_dc = -1.0 * self.drib_vel / 1000.0; - // defmt::info!("Drib Duty Cycle: {}", drib_dc); - // let drib_dc = -0.1; - self.drib_motor.set_setpoint(drib_dc); - - // let c_vel = 0.2; - // front_right_motor.set_setpoint(c_vel); - // front_left_motor.set_setpoint(c_vel); - // back_left_motor.set_setpoint(c_vel); - // back_right_motor.set_setpoint(c_vel); - - self.front_right_motor.send_motion_command(); - self.front_left_motor.send_motion_command(); - self.back_left_motor.send_motion_command(); - self.back_right_motor.send_motion_command(); - self.drib_motor.send_motion_command(); - - Some(BasicTelemetry { - sequence_number: 0, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: 0., - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }) - } -} diff --git a/control-board/src/bin/robot/main.rs b/control-board/src/bin/robot/main.rs deleted file mode 100644 index b8f4f08e..00000000 --- a/control-board/src/bin/robot/main.rs +++ /dev/null @@ -1,386 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] - -use apa102_spi::Apa102; -use ateam_common_packets::bindings_radio::KickRequest; -use ateam_control_board::{ - drivers::{radio::TeamColor, radio::WifiNetwork, rotary::Rotary, shell_indicator::ShellIndicator}, - stm32_interface::get_bootloader_uart_config, -}; -use control::Control; -use defmt::info; -use embassy_stm32::{ - dma::NoDma, - executor::InterruptExecutor, - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, - spi, - time::{hz, mhz}, - usart::{self, Uart}, -}; -use embassy_time::{Duration, Ticker, Timer}; -use futures_util::StreamExt; -use panic_probe as _; -use pins::{ - PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, - ShutdownCompletePin, -}; -use radio::{ - RadioTest, BUFFERS_RX, BUFFERS_TX, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, -}; -use smart_leds::{SmartLedsWrite, RGB8}; -use static_cell::StaticCell; - -mod control; -mod pins; -mod radio; - -static RADIO_TEST: RadioTest< - MAX_TX_PACKET_SIZE, - MAX_RX_PACKET_SIZE, - TX_BUF_DEPTH, - RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, -> = RadioTest::new(unsafe { &mut BUFFERS_TX }, unsafe { &mut BUFFERS_RX }); - -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - let config = usart::Config::default(); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - - let imu_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), - ); - - let mut dotstar = Apa102::new(imu_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - info!("booted"); - - let radio_int = interrupt::take!(USART10); - let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, radio_int, p.DMA2_CH0, p.DMA2_CH1, config, - ); - - let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); - let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); - let _kicker_det = Input::new(p.PG8, Pull::Down); - let dip1 = Input::new(p.PG7, Pull::Down); - let dip2 = Input::new(p.PG6, Pull::Down); - let dip3 = Input::new(p.PG5, Pull::Down); - let dip4 = Input::new(p.PG4, Pull::Down); - let dip5 = Input::new(p.PG3, Pull::Down); - let dip6 = Input::new(p.PG2, Pull::Down); - let dip7 = Input::new(p.PD15, Pull::Down); - - - // let mut kicker = Output::new(p.PF9, Level::Low, Speed::High); - - // let robot_id = rotary.read(); - let robot_id = (dip1.is_high() as u8) << 3 - | (dip2.is_high() as u8) << 2 - | (dip3.is_high() as u8) << 1 - | (dip4.is_high() as u8); - info!("id: {}", robot_id); - shell_indicator.set(robot_id); - - let wifi_network = if dip5.is_high() & dip6.is_high() { - WifiNetwork::Team - } else if dip5.is_low() & dip6.is_high() { - WifiNetwork::CompMain - } else if dip5.is_high() & dip6.is_low() { - WifiNetwork::CompPractice - } else { - WifiNetwork::Team - }; - - let team = if dip7.is_high() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - - // let mut test = Output::new(p.PA8, Level::High, Speed::Medium); - // let mut test2 = Output::new(p.PA9, Level::High, Speed::Medium); - // // let mut kicker_boot0_pin = Output::new(p.PA8, Level::High, Speed::Medium); - // // let mut kicker_reset_pin = Output::new(p.PA9, Level::High, Speed::Medium); - - // loop {} - - // let kicker_int = interrupt::take!(USART6); - // let kicker_usart = Uart::new( - // p.USART6, - // p.PC7, - // p.PC6, - // kicker_int, - // p.DMA2_CH4, - // p.DMA2_CH5, - // get_bootloader_uart_config(), - // ); - // let (kicker_tx, kicker_rx) = kicker_usart.split(); - // let mut kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - // let mut kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - // let kicker_reset_pin_bad = OutputOpenDrain::new(p.PA2, Level::Low, Speed::Medium, Pull::None); - - // spawner - // .spawn(KICKER_QUEUE_RX.spawn_task(kicker_rx)) - // .unwrap(); - // spawner - // .spawn(KICKER_QUEUE_TX.spawn_task(kicker_tx)) - // .unwrap(); - - // kicker_boot0_pin.set_high(); - // let mut kicker_stm32_interface = Stm32Interface::new( - // &KICKER_QUEUE_RX, - // &KICKER_QUEUE_TX, - // Some(kicker_boot0_pin), - // Some(kicker_reset_pin_bad), - // ); - // info!("start program"); - - // kicker_reset_pin.set_high(); - // Timer::after(Duration::from_micros(100)).await; - // kicker_reset_pin.set_low(); - - // kicker_stm32_interface.load_firmware_image(KICKER_FW_IMG).await; - // kicker_reset_pin.set_high(); - // Timer::after(Duration::from_micros(100)).await; - // kicker_reset_pin.set_low(); - - // info!("programmed"); - - // loop {} - - // let mut imu_spi = spi::Spi::new( - // p.SPI6, - // p.PA5, - // p.PA7, - // p.PA6, - // p.BDMA_CH0, - // p.BDMA_CH1, - // hz(1_000_000), - // spi::Config::default(), - // ); - - // // acceleromter - // let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // // gyro - // let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - // Timer::after(Duration::from_millis(1)).await; - - // // let mut buf = [0u8, 2]; - - // unsafe { - // SPI6_BUF[0] = 0x80; - // // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - // imu_cs1.set_low(); - // imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - // imu_cs1.set_high(); - // let accel_id = SPI6_BUF[1]; - // info!("accelerometer id: 0x{:x}", accel_id); - - // SPI6_BUF[0] = 0x80; - // imu_cs2.set_low(); - // imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - // imu_cs2.set_high(); - // let gyro_id = SPI6_BUF[1]; - // info!("gyro id: 0x{:x}", gyro_id); - - // loop { - // SPI6_BUF[0] = 0x86; - // // SPI6_BUF[0] = 0x86; - // imu_cs2.set_low(); - // imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - // imu_cs2.set_high(); - // let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - // info!("z rate: {}", rate_z); - // } - // } - - // loop {} - - // // loop { - // let mut buf = [0x00u8; 4]; - // buf[0] = 0x86; - // buf[2] = 0x87; - // imu_cs2.set_low(); - // imu_spi.blocking_transfer_in_place(&mut buf); - // imu_cs2.set_high(); - // // info!("xfer {=[u8]:x}", buf); - // info!("xfer {}", (buf[3] << 8 + buf[1]) as i16); - // // } - - // loop {} - - let front_right_int = interrupt::take!(USART1); - let front_left_int = interrupt::take!(UART4); - let back_left_int = interrupt::take!(UART7); - let back_right_int = interrupt::take!(UART8); - let drib_int = interrupt::take!(UART5); - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - front_right_int, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - front_left_int, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - back_left_int, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - back_right_int, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - drib_int, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - ); - - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - control.load_firmware().await; - - let _ = dotstar.write([RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - let token = unsafe { - (&mut *(&RADIO_TEST as *const _ - as *mut RadioTest< - MAX_TX_PACKET_SIZE, - MAX_RX_PACKET_SIZE, - TX_BUF_DEPTH, - RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, - >)) - .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - .await - }; - spawner.spawn(token).unwrap(); - - let _ = dotstar.write([RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }].iter().cloned()); - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - let mut last_kicked = 1000; - loop { - let latest = RADIO_TEST.get_latest_control(); - // let latest = Some(BasicControl{ - // vel_x_linear: 0., - // vel_y_linear: 0., - // vel_z_angular: 0.05, - // kick_vel: 0., - // dribbler_speed: 0.2, - // kick_request: 0, - // }); - let telemetry = control.tick(latest); - if let Some(telemetry) = telemetry { - // info!("{:?}", defmt::Debug2Format(&telemetry)); - RADIO_TEST.send_telemetry(telemetry).await; - } - if let Some(latest) = &latest { - if last_kicked > 100 && latest.kick_request == KickRequest::KR_KICK_NOW { - // kicker.set_high(); - // Timer::after(Duration::from_micros(3500)).await; - // kicker.set_low(); - last_kicked = 0; - } - } - last_kicked = core::cmp::min(last_kicked + 1, 1000); - - main_loop_rate_ticker.next().await; - } -} - -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let power_state = Input::new(power_state_pin, Pull::None); - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state, exti); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); - loop {} -} diff --git a/control-board/src/bin/robot/pins.rs b/control-board/src/bin/robot/pins.rs deleted file mode 100644 index fa1db175..00000000 --- a/control-board/src/bin/robot/pins.rs +++ /dev/null @@ -1,42 +0,0 @@ -#![allow(dead_code)] - -use embassy_stm32::peripherals::*; - -pub type RadioUART = USART10; -pub type RadioTxDMA = DMA2_CH0; -pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; - -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; - -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFLDmaTx = DMA1_CH2; -pub type MotorFLDmaRx = DMA1_CH3; -pub type MotorBLDmaTx = DMA1_CH4; -pub type MotorBLDmaRx = DMA1_CH5; -pub type MotorBRDmaTx = DMA1_CH6; -pub type MotorBRDmaRx = DMA1_CH7; -pub type MotorDDmaTx = DMA2_CH2; -pub type MotorDDmaRx = DMA2_CH3; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; -pub type MotorDBootPin = PD13; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; -pub type MotorDResetPin = PD12; - - -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file diff --git a/control-board/src/bin/robot/radio.rs b/control-board/src/bin/robot/radio.rs deleted file mode 100644 index 94d0d778..00000000 --- a/control-board/src/bin/robot/radio.rs +++ /dev/null @@ -1,421 +0,0 @@ -use ateam_common_packets::{bindings_radio::{BasicControl, BasicTelemetry}, radio::DataPacket}; -use core::future::Future; -use defmt::*; -use embassy_executor::{raw::TaskStorage, SendSpawner, SpawnToken}; -use embassy_stm32::{gpio::Pin, usart, Peripheral}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; -use embassy_time::Duration; -use ateam_control_board::{ - drivers::radio::{RobotRadio, TeamColor, WifiNetwork}, - queue, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; - -pub const MAX_TX_PACKET_SIZE: usize = 256; -pub const TX_BUF_DEPTH: usize = 4; -pub const MAX_RX_PACKET_SIZE: usize = 256; -pub const RX_BUF_DEPTH: usize = 4; - -#[link_section = ".axisram.buffers"] -pub static mut BUFFERS_TX: [queue::Buffer; TX_BUF_DEPTH] = - [queue::Buffer::EMPTY; TX_BUF_DEPTH]; -#[link_section = ".axisram.buffers"] -pub static mut BUFFERS_RX: [queue::Buffer; RX_BUF_DEPTH] = - [queue::Buffer::EMPTY; RX_BUF_DEPTH]; - -type ControlTaskFuture< - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - ResetPin: Pin + Send + Sync, -> where - UART::Interrupt: Send, -= impl Future + Sync; - -// pub struct Radio< -// UART: usart::BasicInstance + Send, -// RxDma: usart::RxDma + Send, -// TxDma: usart::TxDma + Send, -// ResetPin: Pin + Send, -// > where -// UART::Interrupt: Send, -// { -// // queue_rx: UartReadQueue<'static, UART, RxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH>, -// // queue_tx: UartWriteQueue<'static, UART, TxDma, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH>, -// task: TaskStorage>, - -// radio: RefCell< -// Option< -// RobotRadio< -// 'static, -// UART, -// RxDma, -// TxDma, -// MAX_RX_PACKET_SIZE, -// MAX_TX_PACKET_SIZE, -// RX_BUF_DEPTH, -// TX_BUF_DEPTH, -// ResetPin, -// >, -// >, -// >, -// } - -// TODO probably -// unsafe impl< -// UART: usart::BasicInstance + Send, -// RxDma: usart::RxDma + Send, -// TxDma: usart::TxDma + Send, -// ResetPin: Pin + Send, -// > Send for Radio -// where -// UART::Interrupt: Send, -// { -// } -// unsafe impl< -// UART: usart::BasicInstance + Send, -// RxDma: usart::RxDma + Send, -// TxDma: usart::TxDma + Send, -// ResetPin: Pin + Send, -// > Sync for Radio -// where -// UART::Interrupt: Send, -// { -// } - -/* - * Uart tx (Radio) - * mut - write task - * Uart rx (Radio) - * mut - read task - * Write Queue - * [mut] - write task - * [mut] - control task (through radio) - * Read Queue - * [mut] - read task - * [mut] - radio read task - * Radio State - * mut - radio read task - * Latest Control - * mut - radio read task - * mut - control task - */ - -pub struct RadioTest< - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - ResetPin: Pin + Send + Sync, -> where - UART::Interrupt: Send, -{ - queue_tx: UartWriteQueue<'static, UART, TxDma, LEN_TX, DEPTH_TX>, - queue_rx: UartReadQueue<'static, UART, RxDma, LEN_RX, DEPTH_RX>, - task: TaskStorage< - ControlTaskFuture, - >, - latest_control: Mutex>, - radio: Option< - RobotRadio<'static, UART, RxDma, TxDma, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX, ResetPin>, - >, -} - -// impl< -// const LEN_TX: usize, -// const LEN_RX: usize, -// const DEPTH_TX: usize, -// const DEPTH_RX: usize, -// UART: usart::BasicInstance + Send, -// RxDma: usart::RxDma + Send, -// TxDma: usart::TxDma + Send, -// ResetPin: Pin + Send + Sync, -// > RadioTest -// where -// UART::Interrupt: Send, -// { -// } - -impl< - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - ResetPin: Pin + Send + Sync, - > RadioTest -where - UART::Interrupt: Send, -{ - pub const fn new( - tx_buffer: &'static mut [queue::Buffer; DEPTH_TX], - rx_buffer: &'static mut [queue::Buffer; DEPTH_RX], - ) -> Self { - RadioTest { - queue_rx: UartReadQueue::new(rx_buffer), - queue_tx: UartWriteQueue::new(tx_buffer), - task: TaskStorage::new(), - latest_control: Mutex::new(None), - radio: None, - } - } - - // setup uart - // setup tasks - pub async fn setup( - &'static mut self, - spawner: &SendSpawner, - uart: usart::Uart<'static, UART, TxDma, RxDma>, - reset_pin: impl Peripheral

+ 'static, - id: u8, - team: TeamColor, - wifi_network: WifiNetwork - ) -> SpawnToken { - let (tx, rx) = uart.split(); - - spawner.spawn(self.queue_rx.spawn_task(rx)).unwrap(); - spawner.spawn(self.queue_tx.spawn_task(tx)).unwrap(); - - let mut radio = RobotRadio::new(&self.queue_rx, &self.queue_tx, reset_pin) - .await - .unwrap(); - - info!("radio created"); - radio.connect_to_network(wifi_network).await.unwrap(); - info!("radio connected"); - - radio.open_multicast().await.unwrap(); - info!("multicast open"); - - loop { - info!("sending hello"); - radio.send_hello(id, team).await.unwrap(); - let hello = radio.wait_hello(Duration::from_millis(1000)).await; - - match hello { - Ok(hello) => { - info!( - "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port - ); - radio.close_peer().await.unwrap(); - info!("multicast peer closed"); - radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); - info!("unicast open"); - break; - } - Err(_) => {} - } - } - - { - self.radio = Some(radio); - // let mut radio_self = self.radio.lock().await; - // *radio_self = Some(radio); - } - - - // self.task.spawn(|| self.control_task()) - self.task.spawn(|| Self::control_task(self.radio.as_ref().unwrap(), &self.latest_control)) - // spawner.spawn(token).unwrap(); - - } - - // fn spawn_task(&'static self, ) -> SpawnToken { - - // } - - fn control_task( - // &'static self - radio: &'static RobotRadio< - 'static, - UART, - RxDma, - TxDma, - LEN_TX, - LEN_RX, - DEPTH_TX, - DEPTH_RX, - ResetPin, - >, - latest_control: &'static Mutex>, - ) -> ControlTaskFuture { - async move { - loop { - let data_packet = radio.read_packet().await; - if let Ok(data_packet) = data_packet { - if let DataPacket::BasicControl(control) = data_packet { - let mut latest_control = latest_control.lock().await; - *latest_control = Some(control); - } - } - } - } - } - - // connect to network - // open multicast - // send hello request - // wait hello response - // close multicast - // open unicast - - // pub async fn connect(&mut self) { - // if let Some(radio) = &mut self.radio { - // radio.connect_to_network().await.unwrap(); - // } - // } - - // write telemetry to queue - pub async fn send_telemetry(&self, telemetry: BasicTelemetry) { - self.radio.as_ref().unwrap().send_telemetry(telemetry).await.unwrap(); - } - - // fetch latest stored control value - pub fn get_latest_control(&self) -> Option { - let mut latest_control = self.latest_control.try_lock(); - if let Ok(latest_control) = &mut latest_control { - latest_control.take() - } else { - None - } - } -} - -// impl< -// UART: usart::BasicInstance + Send, -// RxDma: usart::RxDma + Send, -// TxDma: usart::TxDma + Send, -// ResetPin: Pin + Send, -// > Radio -// where -// UART::Interrupt: Send, -// { -// pub const fn new() -> Self { -// Radio { -// // queue_rx: UartReadQueue::new(unsafe { &mut BUFFERS_RX }), -// // queue_tx: UartWriteQueue::new(unsafe { &mut BUFFERS_TX }), -// radio: RefCell::new(None), -// task: TaskStorage::new(), -// } -// } - -// fn control_task(radio: &'static Self) -> ControlTaskFuture { -// async move { loop {} } -// } - -// // fn spawn_task( -// // &'static self, -// // // rx: UartRx<'a, UART, DMA>, -// // // int: UART::Interrupt, -// // ) -> SpawnToken { -// // self.task.spawn(|| Self::control_task(self)) -// // } - -// // pub fn spawn_task( -// // &'static self, -// // rx: UartRx<'a, UART, DMA>, -// // int: UART::Interrupt, -// // ) -> SpawnToken { -// // self.task.spawn(|| Self::read_task(&self.queue_rx, rx, int)) -// // } - -// pub async fn start( -// &'static self, -// spawner: &SendSpawner, -// uart: usart::Uart<'static, UART, TxDma, RxDma>, -// reset_pin: impl Peripheral

+ 'static, -// ) { -// let (tx, rx) = uart.split(); -// let int = unsafe { ::steal() }; -// // spawner.spawn(self.queue_rx.spawn_task(rx, int)).unwrap(); -// // spawner.spawn(self.queue_tx.spawn_task(tx)).unwrap(); - -// // let mut radio = RobotRadio::new(&self.queue_rx, &self.queue_tx, reset_pin) -// // .await -// // .unwrap(); - -// // info!("radio created"); -// // radio.connect_to_network().await.unwrap(); -// // info!("radio connected"); - -// // loop { -// // info!("sending hello"); -// // radio.send_hello(0, TeamColor::Blue).await.unwrap(); -// // let hello = radio.wait_hello(Duration::from_millis(1000)).await; - -// // match hello { -// // Ok(hello) => { -// // info!( -// // "recieved hello resp to: {}.{}.{}.{}:{}", -// // hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port -// // ); -// // radio.close_peer().await.unwrap(); -// // info!("multicast peer closed"); -// // radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); -// // info!("unicast open"); -// // break; -// // } -// // Err(_) => {} -// // } -// // } - -// // self.radio.replace(Some(radio)); - -// // self.task.spawn(|| Self::control_task(&self)); - -// // spawner.spawn(self.spawn_task()).unwrap() - -// // loop { -// // let control = radio.read_control().await; -// // if let Ok(control) = control { -// // info!("{:?}", defmt::Debug2Format(&control)); -// // } -// // } -// } -// } - -// pub async fn setup_radio< -// 'a, -// UART: usart::BasicInstance + core::marker::Send, -// ResetPin: Pin, -// const LENGTH_RX: usize, -// const DEPTH_RX: usize, -// DMA_RX: usart::RxDma + core::marker::Send, -// const LENGTH_TX: usize, -// const DEPTH_TX: usize, -// DMA_TX: usart::TxDma + core::marker::Send, -// >( -// spawner: &SendSpawner, -// uart: usart::Uart<'a, UART, DMA_TX, DMA_RX>, -// // queue_rx: &'static UartReadQueue<'a, UART, DMA_RX, LENGTH_RX, DEPTH_RX>, -// queue_tx: &'static UartWriteQueue<'a, UART, DMA_TX, LENGTH_TX, DEPTH_TX>, -// uart_int: RadioUART::Interrupt, -// reset_pin: impl Peripheral

, -// ) where UART::Interrupt: Send { -// let (tx, rx) = uart.split(); - -// // TODO: hardcoded USART2 -// // let int = interrupt::take!(USART2); -// spawner.spawn(QUEUE_RX.spawn_task(rx, uart_int)).unwrap(); -// spawner.spawn(queue_tx.spawn_task(tx)).unwrap(); - -// let mut radio = RobotRadio::new(&QUEUE_RX, &queue_tx, reset_pin) -// .await -// .unwrap(); - -// info!("radio created"); -// radio.connect_to_network().await.unwrap(); -// info!("radio connected"); - -// loop {} -// } diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 92f6d95f..33d0d20f 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -1,10 +1,4 @@ - -use core::mem::MaybeUninit; - -use embassy_stm32::{ - gpio::Pin, - usart::{self, Parity}, -}; +use embassy_stm32::usart::{self, Parity}; use embassy_time::{Duration, Timer}; use crate::stm32_interface::Stm32Interface; @@ -20,8 +14,6 @@ pub struct Kicker< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, > { stm32_uart_interface: Stm32Interface< 'a, @@ -32,8 +24,6 @@ pub struct Kicker< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], @@ -52,9 +42,7 @@ impl< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, - > Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + > Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { pub fn new( stm32_interface: Stm32Interface< @@ -66,11 +54,9 @@ impl< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], - ) -> Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> { + ) -> Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { Kicker { stm32_uart_interface: stm32_interface, firmware_image, @@ -167,30 +153,23 @@ impl< ////////////////////////////////// pub async fn reset(&mut self) { - self.stm32_uart_interface.hard_reset().await; + self.stm32_uart_interface.hard_reset().await.unwrap(); } pub async fn enter_reset(&mut self) { - self.stm32_uart_interface.enter_reset().await; + self.stm32_uart_interface.enter_reset().await.unwrap(); } pub async fn leave_reset(&mut self) { - self.stm32_uart_interface.leave_reset().await; + self.stm32_uart_interface.leave_reset().await.unwrap(); } - pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> { - // self - // .stm32_uart_interface - // .load_firmware_image(fw_image_bytes) - // .await?; + pub async fn load_firmware_image(&mut self, _fw_image_bytes: &[u8]) -> Result<(), ()> { + defmt::warn!("currently skipping kicker firmware flash"); + // self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await?; + + self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; - // this is safe because load firmware image call will reset the target device - // it will begin issueing telemetry updates - // these are the only packets it sends so any blocked process should get the data it now needs - unsafe { - self.stm32_uart_interface - .update_uart_config(2_000_000, Parity::ParityEven) - }; Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset diff --git a/control-board/src/drivers/radio/at_protocol.rs b/control-board/src/drivers/radio/at_protocol.rs index b8837b97..be7a21a6 100644 --- a/control-board/src/drivers/radio/at_protocol.rs +++ b/control-board/src/drivers/radio/at_protocol.rs @@ -175,25 +175,25 @@ pub enum ATEvent<'a> { } impl<'b> ATEvent<'b> { - const CR_LF: &str = "\r\n"; + const CR_LF: &'static str = "\r\n"; - const PEER_CONNECTED: &str = "+UUDPC"; - const PEER_DISCONNECTED: &str = "+UUDPD"; - const WIFI_LINK_CONNECTED: &str = "+UUWLE"; - const WIFI_LINK_DISCONNECTED: &str = "+UUWLD"; - const WIFI_ACCESS_POINT_UP: &str = "+UUWAPU"; - const WIFI_ACCESS_POINT_DOWN: &str = "+UUWAPD"; - const WIFI_AP_STATION_CONNECTED: &str = "+UUWAPSTAC"; - const WIFI_AP_STATION_DISCONNECTED: &str = "+UUWAPSTAD"; - const ETHERNET_LINK_UP: &str = "+UUETHLU"; - const ETHERNET_LINK_DOWN: &str = "+UUETHLD"; - const NETWORK_UP: &str = "+UUNU"; - const NETWORK_DOWN: &str = "+UUND"; - const NETWORK_ERROR: &str = "UUNERR"; + const PEER_CONNECTED: &'static str = "+UUDPC"; + const PEER_DISCONNECTED: &'static str = "+UUDPD"; + const WIFI_LINK_CONNECTED: &'static str = "+UUWLE"; + const WIFI_LINK_DISCONNECTED: &'static str = "+UUWLD"; + const WIFI_ACCESS_POINT_UP: &'static str = "+UUWAPU"; + const WIFI_ACCESS_POINT_DOWN: &'static str = "+UUWAPD"; + const WIFI_AP_STATION_CONNECTED: &'static str = "+UUWAPSTAC"; + const WIFI_AP_STATION_DISCONNECTED: &'static str = "+UUWAPSTAD"; + const ETHERNET_LINK_UP: &'static str = "+UUETHLU"; + const ETHERNET_LINK_DOWN: &'static str = "+UUETHLD"; + const NETWORK_UP: &'static str = "+UUNU"; + const NETWORK_DOWN: &'static str = "+UUND"; + const NETWORK_ERROR: &'static str = "UUNERR"; - const PEER_CONNECTED_BLUETOOTH: &str = "1"; - const PEER_CONNECTED_IPV4: &str = "2"; - const PEER_CONNECTED_IPV6: &str = "3"; + const PEER_CONNECTED_BLUETOOTH: &'static str = "1"; + const PEER_CONNECTED_IPV4: &'static str = "2"; + const PEER_CONNECTED_IPV6: &'static str = "3"; pub fn new<'a>(buf: &'a [u8]) -> Result, ()> { let s = core::str::from_utf8(buf).or(Err(()))?; @@ -314,10 +314,10 @@ pub enum ATResponse<'a> { } impl<'b> ATResponse<'b> { - const CR_LF: &str = "\r\n"; + const CR_LF: &'static str = "\r\n"; - const RESP_OK: &str = "OK"; - const RESP_ERROR: &str = "ERROR"; + const RESP_OK: &'static str = "OK"; + const RESP_ERROR: &'static str = "ERROR"; pub fn new<'a>(buf: &'a [u8]) -> Result, ()> { let s = core::str::from_utf8(buf).or(Err(()))?; diff --git a/control-board/src/drivers/radio/radio.rs b/control-board/src/drivers/radio/radio.rs index da534742..9588c8fa 100644 --- a/control-board/src/drivers/radio/radio.rs +++ b/control-board/src/drivers/radio/radio.rs @@ -2,7 +2,7 @@ use core::fmt::Write; use embassy_stm32::usart; use heapless::String; -use crate::uart_queue::{UartReadQueue, UartWriteQueue}; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use super::at_protocol::{ATEvent, ATResponse}; use super::edm_protocol::EdmPacket; @@ -51,8 +51,8 @@ pub struct Radio< const DEPTH_TX: usize, const DEPTH_RX: usize, > { - reader: &'a UartReadQueue<'a, UART, RxDma, LEN_RX, DEPTH_RX>, - writer: &'a UartWriteQueue<'a, UART, TxDma, LEN_TX, DEPTH_TX>, + reader: &'a UartReadQueue, + writer: &'a UartWriteQueue, mode: RadioMode, wifi_connected: bool, } @@ -69,8 +69,8 @@ impl< > Radio<'a, UART, TxDma, RxDma, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { pub fn new( - reader: &'a UartReadQueue<'a, UART, RxDma, LEN_RX, DEPTH_RX>, - writer: &'a UartWriteQueue<'a, UART, TxDma, LEN_TX, DEPTH_TX>, + reader: &'a UartReadQueue, + writer: &'a UartWriteQueue, ) -> Self { Self { reader, diff --git a/control-board/src/drivers/radio/radio_robot.rs b/control-board/src/drivers/radio/radio_robot.rs index 5d37130b..fa017263 100644 --- a/control-board/src/drivers/radio/radio_robot.rs +++ b/control-board/src/drivers/radio/radio_robot.rs @@ -1,5 +1,5 @@ use super::radio::{PeerConnection, Radio, WifiAuth}; -use crate::uart_queue::{UartReadQueue, UartWriteQueue}; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings_radio::{ self, BasicControl, CommandCode, HelloRequest, HelloResponse, RadioPacket, RadioPacket_Data, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, }; @@ -9,9 +9,7 @@ use core::fmt::Write; use core::mem::size_of; use embassy_futures::select::{select, Either}; use embassy_stm32::gpio::{Level, Pin, Speed, Output}; -use embassy_stm32::pac; use embassy_stm32::usart; -use embassy_stm32::Peripheral; use embassy_time::{Duration, Timer}; use heapless::String; @@ -54,8 +52,7 @@ unsafe impl< const LEN_TX: usize, const DEPTH_TX: usize, const DEPTH_RX: usize, - PIN: Pin, - > Send for RobotRadio<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX, PIN> + > Send for RobotRadio<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX> { } @@ -70,7 +67,6 @@ pub struct RobotRadio< const LEN_RX: usize, const DEPTH_TX: usize, const DEPTH_RX: usize, - PIN: Pin, > { radio: Radio< 'a, @@ -82,7 +78,7 @@ pub struct RobotRadio< DEPTH_TX, DEPTH_RX, >, - reset_pin: Output<'a, PIN>, + reset_pin: Output<'a>, peer: Option, } @@ -95,14 +91,13 @@ impl< const LEN_RX: usize, const DEPTH_TX: usize, const DEPTH_RX: usize, - PIN: Pin, - > RobotRadio<'a, UART, DmaRx, DmaTx, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX, PIN> + > RobotRadio<'a, UART, DmaRx, DmaTx, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { pub async fn new( - read_queue: &'a UartReadQueue<'a, UART, DmaRx, LEN_RX, DEPTH_RX>, - write_queue: &'a UartWriteQueue<'a, UART, DmaTx, LEN_TX, DEPTH_TX>, - reset_pin: impl Peripheral

+ 'a, - ) -> Result, ()> + read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + reset_pin: impl Pin, + ) -> Result, ()> { let mut reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); Timer::after(Duration::from_micros(100)).await; @@ -115,22 +110,10 @@ impl< radio.set_echo(false).await?; radio.config_uart(baudrate, false, 8, true).await?; - let div = (UART::frequency().0 + (baudrate / 2)) / baudrate * UART::MULTIPLIER; - unsafe { - let r = UART::regs(); - r.cr1().modify(|w| { - w.set_ue(false); - }); - r.brr().modify(|w| { - w.set_brr(div); - }); - r.cr1().modify(|w| { - w.set_ue(true); - w.set_m0(pac::lpuart::vals::M0::BIT9); - w.set_pce(true); - w.set_ps(pac::lpuart::vals::Ps::EVEN); - }); - }; + let mut radio_uart_config = usart::Config::default(); + radio_uart_config.baudrate = 2_000_000; + radio_uart_config.parity = usart::Parity::ParityEven; + write_queue.update_uart_config(radio_uart_config).await; // Datasheet says wait at least 40ms after UART config change Timer::after(Duration::from_millis(50)).await; @@ -382,7 +365,7 @@ impl< return Err(()); } - let mut data_copy = [0u8; PACKET_SIZE]; + let mut data_copy = [0u8; size_of::()]; data_copy.clone_from_slice(&data[0..PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; @@ -409,7 +392,7 @@ impl< + size_of::(); if data.len() == CONTROL_PACKET_SIZE { - let mut data_copy = [0u8; CONTROL_PACKET_SIZE]; + let mut data_copy = [0u8; size_of::()]; data_copy.clone_from_slice(&data[0..CONTROL_PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; @@ -420,7 +403,7 @@ impl< Ok(unsafe { DataPacket::BasicControl(packet.data.control) }) } else if data.len() == PARAMERTER_PACKET_SIZE { - let mut data_copy = [0u8; PARAMERTER_PACKET_SIZE]; + let mut data_copy = [0u8; size_of::()]; data_copy.clone_from_slice(&data[0..PARAMERTER_PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; diff --git a/control-board/src/drivers/rotary.rs b/control-board/src/drivers/rotary.rs index ffebc194..437af400 100644 --- a/control-board/src/drivers/rotary.rs +++ b/control-board/src/drivers/rotary.rs @@ -1,38 +1,19 @@ -use embassy_stm32::gpio::{Input, Speed, Pin, Pull}; +use embassy_stm32::gpio::{Input, Pin, Pull}; -pub struct Rotary<'a, Pin0: Pin, Pin1: Pin, Pin2: Pin, Pin3: Pin> { - pin0: Input<'a, Pin0>, - pin1: Input<'a, Pin1>, - pin2: Input<'a, Pin2>, - pin3: Input<'a, Pin3>, - // pin0: Flex<'a, Pin0>, - // pin1: Flex<'a, Pin1>, - // pin2: Flex<'a, Pin2>, - // pin3: Flex<'a, Pin3>, +pub struct Rotary<'a> { + pin0: Input<'a>, + pin1: Input<'a>, + pin2: Input<'a>, + pin3: Input<'a>, } -impl<'a, Pin0: Pin, Pin1: Pin, Pin2: Pin, Pin3: Pin> Rotary<'a, Pin0, Pin1, Pin2, Pin3> { - pub fn new(pin0: Pin0, pin1: Pin1, pin2: Pin2, pin3: Pin3) -> Self { +impl<'a> Rotary<'a> { + pub fn new(pin0: impl Pin, pin1: impl Pin, pin2: impl Pin, pin3: impl Pin) -> Self { let pin0 = Input::new(pin0, Pull::Down); let pin1 = Input::new(pin1, Pull::Down); let pin2 = Input::new(pin2, Pull::Down); let pin3 = Input::new(pin3, Pull::Down); - // let mut pin0 = Flex::new(pin0); - // let mut pin1 = Flex::new(pin1); - // let mut pin2 = Flex::new(pin2); - // let mut pin3 = Flex::new(pin3); - // pin0.set_high(); - // pin1.set_high(); - // pin2.set_high(); - // pin3.set_high(); - - // pin0.set_as_input_output(Speed::VeryHigh, Pull::Down); - // pin1.set_as_input_output(Speed::VeryHigh, Pull::Down); - // pin2.set_as_input_output(Speed::VeryHigh, Pull::Down); - // pin3.set_as_input_output(Speed::VeryHigh, Pull::Down); - - Self { pin0, pin1, diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index e92a1d68..7aeffd81 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -1,13 +1,13 @@ use embassy_stm32::gpio::{Level, Output, Pin, Speed}; -pub struct ShellIndicator<'a, Pin0: Pin, Pin1: Pin, Pin2: Pin, Pin3: Pin> { - pin0: Output<'a, Pin0>, - pin1: Output<'a, Pin1>, - pin2: Output<'a, Pin2>, - pin3: Output<'a, Pin3>, +pub struct ShellIndicator<'a> { + pin0: Output<'a>, + pin1: Output<'a>, + pin2: Output<'a>, + pin3: Output<'a>, } -impl<'a, Pin0: Pin, Pin1: Pin, Pin2: Pin, Pin3: Pin> ShellIndicator<'a, Pin0, Pin1, Pin2, Pin3> { +impl<'a> ShellIndicator<'a> { // MSB to LSB, LSB "0" start the quadrant C (upper right to lower right 0-3), 0 = pink, 1 = green #[rustfmt::skip] const ID_TO_PATTERN: [u8; 16] = [ @@ -17,7 +17,7 @@ impl<'a, Pin0: Pin, Pin1: Pin, Pin2: Pin, Pin3: Pin> ShellIndicator<'a, Pin0, Pi 0x0E, 0x02, 0x0D, 0x01, ]; - pub fn new(pin0: Pin0, pin1: Pin1, pin2: Pin2, pin3: Pin3) -> Self { + pub fn new(pin0: impl Pin, pin1: impl Pin, pin2: impl Pin, pin3: impl Pin) -> Self { Self { pin0: Output::new(pin0, Level::Low, Speed::Low), pin1: Output::new(pin1, Level::Low, Speed::Low), diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 491c6a9e..a1cc93fc 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -9,17 +9,14 @@ maybe_uninit_write_slice )] #![feature(const_mut_refs)] -#![feature(adt_const_params)] #![feature(ptr_metadata)] -#![feature(async_fn_in_trait)] #![feature(const_fn_floating_point_arithmetic)] // pub mod fw_images; -pub mod queue; pub mod motion; +pub mod pins; pub mod stm32_interface; pub mod stspin_motor; -pub mod uart_queue; pub mod parameter_interface; pub mod drivers; @@ -45,8 +42,8 @@ macro_rules! include_external_cpp_bin { #[macro_export] macro_rules! include_kicker_bin { ($var_name:ident, $bin_file:literal) => { - pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv6m-none-eabi/release/", $bin_file)).len()] - = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv6m-none-eabi/release/", $bin_file)); + pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabi/release/", $bin_file)).len()] + = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabi/release/", $bin_file)); } } pub const BATTERY_MIN_VOLTAGE: f32 = 19.0; diff --git a/control-board/src/main.rs b/control-board/src/main.rs deleted file mode 100644 index ebacbfb5..00000000 --- a/control-board/src/main.rs +++ /dev/null @@ -1,77 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(async_closure)] -#![feature(const_mut_refs)] - -use defmt::*; -use defmt_rtt as _; -use embassy_stm32::usart::{self, Uart}; -use embassy_stm32::{ - self as _, - executor::InterruptExecutor, - interrupt::{self, InterruptExt}, - peripherals::{DMA1_CH0, DMA1_CH1, UART7}, -}; -use ateam_control_board::queue; -use ateam_control_board::uart_queue::{UartReadQueue, UartWriteQueue}; -use panic_probe as _; -use static_cell::StaticCell; - -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); - -#[link_section = ".axisram.buffers"] -static mut BUFFERS_TX: [queue::Buffer<8>; 4] = [queue::Buffer::EMPTY; 4]; -static QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BUFFERS_RX: [queue::Buffer<8>; 4] = [queue::Buffer::EMPTY; 4]; -static QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BUFFERS_RX }); - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - info!("Startup"); - - let p = embassy_stm32::init(Default::default()); - let config = usart::Config::default(); - let int = interrupt::take!(UART7); - let usart = Uart::new(p.UART7, p.PF6, p.PF7, int, p.DMA1_CH0, p.DMA1_CH1, config); - - let (tx, rx) = usart.split(); - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - info!("start1"); - - spawner.spawn(QUEUE_RX.spawn_task(rx)).unwrap(); - spawner.spawn(QUEUE_TX.spawn_task(tx)).unwrap(); - - QUEUE_TX.enqueue_copy(&[0, 1, 2, 3, 4, 5, 6, 7]).unwrap(); - QUEUE_TX.enqueue_copy(&[8, 9, 10, 11, 12, 13, 14]).unwrap(); - QUEUE_TX.enqueue_copy(&[15, 16, 17]).unwrap(); - info!("tx queued"); - - QUEUE_RX - .dequeue(|buf2| { - info!("{:?}", buf2); - }) - .await; - QUEUE_RX - .dequeue(|buf2| { - info!("{:?}", buf2); - }) - .await; - QUEUE_RX - .dequeue(|buf2| { - info!("{:?}", buf2); - }) - .await; - - info!("rx dequeued"); - - loop {} -} diff --git a/control-board/src/bin/control/pins.rs b/control-board/src/pins.rs similarity index 100% rename from control-board/src/bin/control/pins.rs rename to control-board/src/pins.rs diff --git a/control-board/src/queue.rs b/control-board/src/queue.rs deleted file mode 100644 index 8328e5ac..00000000 --- a/control-board/src/queue.rs +++ /dev/null @@ -1,220 +0,0 @@ -use core::{ - cell::UnsafeCell, - future::poll_fn, - mem::MaybeUninit, - task::{Poll, Waker}, -}; -use critical_section; - -pub struct Buffer { - pub data: [MaybeUninit; LENGTH], - pub len: MaybeUninit, -} - -impl Buffer { - pub const EMPTY: Buffer = Buffer { - data: MaybeUninit::uninit_array(), - len: MaybeUninit::uninit(), - }; -} - -pub struct DequeueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, - data: &'a [u8], -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> DequeueRef<'a, LENGTH, DEPTH> { - pub fn data(&self) -> &[u8] { - self.data - } - pub fn cancel(self) { - self.queue.cancel_dequeue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for DequeueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_dequeue(); - } -} - -pub struct EnqueueRef<'a, const LENGTH: usize, const DEPTH: usize> { - queue: &'a Queue<'a, LENGTH, DEPTH>, - data: &'a mut [u8], - len: &'a mut usize, -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> EnqueueRef<'a, LENGTH, DEPTH> { - pub fn data(&mut self) -> &mut [u8] { - self.data - } - - pub fn len(&mut self) -> &mut usize { - self.len - } - pub fn cancel(self) { - self.queue.cancel_enqueue(); - } -} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Drop for EnqueueRef<'a, LENGTH, DEPTH> { - fn drop(&mut self) { - self.queue.finish_enqueue(); - } -} - -#[derive(PartialEq, Eq, Clone, Copy, Debug, defmt::Format)] -pub enum Error { - QueueFullEmpty, - InProgress, -} - -pub struct Queue<'a, const LENGTH: usize, const DEPTH: usize> { - buffers: &'a [Buffer; DEPTH], - read_index: UnsafeCell, - read_in_progress: UnsafeCell, - write_index: UnsafeCell, - write_in_progress: UnsafeCell, - size: UnsafeCell, - enqueue_waker: UnsafeCell>, - dequeue_waker: UnsafeCell>, -} - -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Send for Queue<'a, LENGTH, DEPTH> {} -unsafe impl<'a, const LENGTH: usize, const DEPTH: usize> Sync for Queue<'a, LENGTH, DEPTH> {} - -impl<'a, const LENGTH: usize, const DEPTH: usize> Queue<'a, LENGTH, DEPTH> { - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - buffers, - read_index: UnsafeCell::new(0), - read_in_progress: UnsafeCell::new(false), - write_index: UnsafeCell::new(0), - write_in_progress: UnsafeCell::new(false), - size: UnsafeCell::new(0), - enqueue_waker: UnsafeCell::new(None), - dequeue_waker: UnsafeCell::new(None), - } - } - - pub fn try_dequeue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { - if *self.read_in_progress.get() { - return Err(Error::InProgress); - } - - if *self.size.get() > 0 { - *self.read_in_progress.get() = true; - let buf = &self.buffers[*self.read_index.get()]; - let len = buf.len.assume_init(); - let data = &MaybeUninit::slice_assume_init_ref(&buf.data)[..len]; - Ok(DequeueRef { queue: self, data }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn dequeue(&self) -> Result, Error> { - // TODO: look at this (when uncommented causes issue cancelling dequeue) - // if critical_section::with(|_| unsafe { (*self.dequeue_waker.get()).is_some() }) { - // return Err(Error::InProgress); - // } - - poll_fn(|cx| { - critical_section::with(|_| unsafe { - match self.try_dequeue() { - Err(Error::QueueFullEmpty) => { - *self.dequeue_waker.get() = Some(cx.waker().clone()); - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_dequeue(&self) { - critical_section::with(|_| unsafe { - *self.read_in_progress.get() = false; - }); - } - - fn finish_dequeue(&self) { - critical_section::with(|_| unsafe { - let read_in_progress = self.read_in_progress.get(); - if *read_in_progress { - *read_in_progress = false; - *self.read_index.get() = (*self.read_index.get() + 1) % DEPTH; - *self.size.get() -= 1; - } - if let Some(w) = (*self.enqueue_waker.get()).take() { - w.wake(); - } - }); - } - - pub fn try_enqueue(&self) -> Result, Error> { - critical_section::with(|_| unsafe { - if *self.write_in_progress.get() { - return Err(Error::InProgress); - } - - if *self.size.get() < DEPTH { - *self.write_in_progress.get() = true; - let buf = &self.buffers[*self.write_index.get()]; - let buf = &mut *(buf as *const _ as *mut Buffer); - let data = MaybeUninit::slice_assume_init_mut(&mut buf.data); - let len = buf.len.write(0); - - Ok(EnqueueRef { - queue: self, - data, - len: len, - }) - } else { - Err(Error::QueueFullEmpty) - } - }) - } - - pub async fn enqueue(&self) -> Result, Error> { - if critical_section::with(|_| unsafe { (*self.enqueue_waker.get()).is_some() }) { - return Err(Error::InProgress); - } - - poll_fn(|cx| { - critical_section::with(|_| unsafe { - match self.try_enqueue() { - Err(Error::QueueFullEmpty) => { - *self.enqueue_waker.get() = Some(cx.waker().clone()); - Poll::Pending - } - r => Poll::Ready(r), - } - }) - }) - .await - } - - fn cancel_enqueue(&self) { - critical_section::with(|_| unsafe { - *self.write_in_progress.get() = false; - }); - } - - fn finish_enqueue(&self) { - critical_section::with(|_| unsafe { - let write_in_progress = self.write_in_progress.get(); - if *write_in_progress { - *write_in_progress = false; - *self.write_index.get() = (*self.write_index.get() + 1) % DEPTH; - *self.size.get() += 1; - } - if let Some(w) = (*self.dequeue_waker.get()).take() { - w.wake(); - } - }); - } -} diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index ed386c97..170a55f9 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -9,8 +9,8 @@ use embassy_stm32::usart::{self, Parity, Config}; use embassy_time::{Duration, Timer}; use embassy_time::with_timeout; -use crate::queue::{DequeueRef, Error}; -use crate::uart_queue::{Reader, Writer, UartReadQueue, UartWriteQueue}; +use ateam_lib_stm32::queue::{DequeueRef, Error}; +use ateam_lib_stm32::uart::queue::{Reader, Writer, UartReadQueue, UartWriteQueue}; pub const STM32_BOOTLOADER_MAX_BAUD_RATE: u32 = 115_200; pub const STM32_BOOTLOADER_ACK: u8 = 0x79; @@ -47,13 +47,11 @@ pub struct Stm32Interface< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin > { - reader: &'a UartReadQueue<'a, UART, DmaRx, LEN_RX, DEPTH_RX>, - writer: &'a UartWriteQueue<'a, UART, DmaTx, LEN_TX, DEPTH_TX>, - boot0_pin: Option>, - reset_pin: Option>, + reader: &'a UartReadQueue, + writer: &'a UartWriteQueue, + boot0_pin: Option>, + reset_pin: Option>, reset_pin_noninverted: bool, @@ -69,16 +67,14 @@ impl< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin - > Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + > Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { pub fn new( - read_queue: &'a UartReadQueue<'a, UART, DmaRx, LEN_RX, DEPTH_RX>, - write_queue: &'a UartWriteQueue<'a, UART, DmaTx, LEN_TX, DEPTH_TX>, - boot0_pin: Option>, - reset_pin: Option> - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> { + read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: Option>, + reset_pin: Option> + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // let the user set the initial state // if boot0_pin.is_some() { // boot0_pin.as_mut().unwrap().set_low(); @@ -99,11 +95,11 @@ impl< } pub fn new_noninverted_reset( - read_queue: &'a UartReadQueue<'a, UART, DmaRx, LEN_RX, DEPTH_RX>, - write_queue: &'a UartWriteQueue<'a, UART, DmaTx, LEN_TX, DEPTH_TX>, - boot0_pin: Option>, - reset_pin: Option> - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> { + read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: Option>, + reset_pin: Option> + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { // let the user set the initial state // if boot0_pin.is_some() { // boot0_pin.as_mut().unwrap().set_low(); @@ -237,47 +233,54 @@ impl< Ok(()) } - pub unsafe fn update_uart_config(&self, baudrate: u32, parity: Parity) { - let div = (UART::frequency().0 + (baudrate / 2)) / baudrate * UART::MULTIPLIER; - - // let irq = UART::Interrupt::steal(); - // irq.disable(); - - let r = UART::regs(); - // disable the uart. Can't modify parity and baudrate while module is enabled - r.cr1().modify(|w| { - w.set_ue(false); - }); - - // set the baudrate - r.brr().modify(|w| { - w.set_brr(div); - }); - - // set parity - r.cr1().modify(|w| { - if parity != Parity::ParityNone { - // configure 9 bit transmission and enable parity control - w.set_m0(pac::lpuart::vals::M0::BIT9); - w.set_pce(true); - - // set polarity type - if parity == Parity::ParityEven { - w.set_ps(pac::lpuart::vals::Ps::EVEN); - } else { - w.set_ps(pac::lpuart::vals::Ps::ODD); - } - } else { - // disable parity (1 byte transmissions) - w.set_m0(pac::lpuart::vals::M0::BIT8); - w.set_pce(false); - } - }); + pub async fn update_uart_config(&self, baudrate: u32, parity: Parity) { + let mut config = usart::Config::default(); + config.baudrate = baudrate; + config.parity = parity; + + self.writer.update_uart_config(config).await; - // reenable UART - r.cr1().modify(|w| { - w.set_ue(true); - }); + + // let div = (UART::frequency().0 + (baudrate / 2)) / baudrate * UART::MULTIPLIER; + + // // let irq = UART::Interrupt::steal(); + // // irq.disable(); + + // let r = UART::regs(); + // // disable the uart. Can't modify parity and baudrate while module is enabled + // r.cr1().modify(|w| { + // w.set_ue(false); + // }); + + // // set the baudrate + // r.brr().modify(|w| { + // w.set_brr(div); + // }); + + // // set parity + // r.cr1().modify(|w| { + // if parity != Parity::ParityNone { + // // configure 9 bit transmission and enable parity control + // w.set_m0(pac::usart::vals::M0::BIT9); + // w.set_pce(true); + + // // set polarity type + // if parity == Parity::ParityEven { + // w.set_ps(pac::usart::vals::Ps::EVEN); + // } else { + // w.set_ps(pac::usart::vals::Ps::ODD); + // } + // } else { + // // disable parity (1 byte transmissions) + // w.set_m0(pac::usart::vals::M0::BIT8); + // w.set_pce(false); + // } + // }); + + // // reenable UART + // r.cr1().modify(|w| { + // w.set_ue(true); + // }); // let div = (pclk_freq.0 + (config.baudrate / 2)) / config.baudrate * multiplier; diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index a52bb618..c0cfb842 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -29,8 +29,6 @@ pub struct WheelMotor< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, > { stm32_uart_interface: Stm32Interface< 'a, @@ -41,8 +39,6 @@ pub struct WheelMotor< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], @@ -73,9 +69,7 @@ impl< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, - > WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + > WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { pub fn new( stm32_interface: Stm32Interface< @@ -87,11 +81,9 @@ impl< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], - ) -> WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + ) -> WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { let start_state: MotorResponse_Motion_Packet = { unsafe { MaybeUninit::zeroed().assume_init() } }; @@ -305,8 +297,6 @@ pub struct DribblerMotor< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, > { stm32_uart_interface: Stm32Interface< 'a, @@ -317,8 +307,6 @@ pub struct DribblerMotor< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], @@ -351,9 +339,7 @@ impl< const LEN_TX: usize, const DEPTH_RX: usize, const DEPTH_TX: usize, - Boot0Pin: Pin, - ResetPin: Pin, - > DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + > DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { pub fn new( stm32_interface: Stm32Interface< @@ -365,12 +351,10 @@ impl< LEN_TX, DEPTH_RX, DEPTH_TX, - Boot0Pin, - ResetPin, >, firmware_image: &'a [u8], ball_detected_thresh: f32, - ) -> DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, Boot0Pin, ResetPin> + ) -> DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { let start_state: MotorResponse_Motion_Packet = { unsafe { MaybeUninit::zeroed().assume_init() } }; diff --git a/control-board/src/uart_queue.rs b/control-board/src/uart_queue.rs deleted file mode 100644 index 8126fa59..00000000 --- a/control-board/src/uart_queue.rs +++ /dev/null @@ -1,204 +0,0 @@ -use crate::queue::{self, Buffer, DequeueRef, Error, Queue}; - -use core::future::Future; -use defmt::info; -use embassy_executor::{raw::TaskStorage, SpawnToken}; -use embassy_stm32::{ - usart::{self, UartRx, UartTx}, - Peripheral, -}; - -pub struct UartReadQueue< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_rx: Queue<'a, LENGTH, DEPTH>, - task: TaskStorage>, -} - -// TODO: pretty sure shouldn't do this -unsafe impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > Send for UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> -{ -} - -pub type ReadTaskFuture< - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::RxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartReadQueue<'a, UART, DMA, LENGTH, DEPTH> -{ - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - queue_rx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn read_task( - queue_rx: &'static Queue<'a, LENGTH, DEPTH>, - mut rx: UartRx<'a, UART, DMA>, - // mut int: UART::Interrupt, - ) -> ReadTaskFuture { - async move { - loop { - let mut buf = queue_rx.enqueue().await.unwrap(); - let len = rx - .read_until_idle(buf.data()) - .await; - // .unwrap(); - // TODO: this - if let Ok(len) = len { - if len == 0 { - // info!("uart zero"); - buf.cancel(); - } else { - *buf.len() = len; - } - } else { - // info!("{}", len); - buf.cancel(); - } - } - } - } - - pub fn spawn_task( - &'static self, - rx: UartRx<'a, UART, DMA>, - ) -> SpawnToken { - self.task.spawn(|| Self::read_task(&self.queue_rx, rx)) - } - - pub fn try_dequeue(&self) -> Result, Error> { - return self.queue_rx.try_dequeue(); - } - - pub async fn dequeue(&self, fn_write: impl FnOnce(&[u8]) -> RET) -> RET { - let buf = self.queue_rx.dequeue().await.unwrap(); - fn_write(buf.data()) - } -} - -pub struct UartWriteQueue< - 'a, - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> { - queue_tx: Queue<'a, LENGTH, DEPTH>, - task: TaskStorage>, -} - -type WriteTaskFuture< - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, -> = impl Future; - -impl< - 'a, - UART: usart::BasicInstance, - DMA: usart::TxDma, - const LENGTH: usize, - const DEPTH: usize, - > UartWriteQueue<'a, UART, DMA, LENGTH, DEPTH> -{ - pub const fn new(buffers: &'a mut [Buffer; DEPTH]) -> Self { - Self { - queue_tx: Queue::new(buffers), - task: TaskStorage::new(), - } - } - - fn write_task( - queue_tx: &'static Queue<'a, LENGTH, DEPTH>, - mut tx: UartTx<'a, UART, DMA>, - ) -> WriteTaskFuture { - async move { - loop { - let buf = queue_tx.dequeue().await.unwrap(); - tx.write(buf.data()).await.unwrap(); - drop(buf); - // unsafe { - // // TODO: what does this do again? - // while !UART::regs().isr().read().tc() {} - // UART::regs().cr1().modify(|w| w.set_te(false)); - // while UART::regs().isr().read().teack() {} - // UART::regs().cr1().modify(|w| w.set_te(true)); - // } - } - } - } - - pub fn spawn_task(&'static self, tx: UartTx<'a, UART, DMA>) -> SpawnToken { - self.task.spawn(|| Self::write_task(&self.queue_tx, tx)) - } - - pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { - let mut buf = self.queue_tx.try_enqueue()?; - let len = fn_write(buf.data()); - *buf.len() = len; - Ok(()) - } - - pub fn enqueue_copy(&self, source: &[u8]) -> Result<(), queue::Error> { - self.enqueue(|dest| { - dest[..source.len()].copy_from_slice(source); - source.len() - }) - } -} - -pub trait Reader { - async fn read RET>(&self, fn_read: FN) -> Result; -} - -pub trait Writer { - async fn write usize>(&self, fn_write: FN) -> Result<(), ()>; -} - -impl< - 'a, - UART: usart::BasicInstance, - Dma: usart::RxDma, - const LEN: usize, - const DEPTH: usize, - > Reader for crate::uart_queue::UartReadQueue<'a, UART, Dma, LEN, DEPTH> -{ - async fn read RET>(&self, fn_read: FN) -> Result { - Ok(self.dequeue(|buf| fn_read(buf)).await) - } -} - -impl< - 'a, - UART: usart::BasicInstance, - Dma: usart::TxDma, - const LEN: usize, - const DEPTH: usize, - > Writer for crate::uart_queue::UartWriteQueue<'a, UART, Dma, LEN, DEPTH> -{ - async fn write usize>(&self, fn_write: FN) -> Result<(), ()> { - self.enqueue(|buf| fn_write(buf)).or(Err(())) - } -} diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 726d526f..e9fdeb11 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -4,16 +4,19 @@ use core::{ cell::SyncUnsafeCell, future::Future, sync::atomic::{AtomicBool, Ordering}}; -use embassy_executor::{ - raw::TaskStorage, - SpawnToken}; use embassy_stm32::{ mode::Async, usart::{self, UartRx, UartTx} }; -use embassy_futures::select::{select, Either}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; +use embassy_executor::{ + raw::TaskStorage, + SpawnToken}; +use embassy_futures::select::{select, Either}; +use embassy_sync::{ + blocking_mutex::raw::CriticalSectionRawMutex, + mutex::Mutex +}; use embassy_time::Timer; use crate::queue::{ @@ -21,7 +24,33 @@ use crate::queue::{ Buffer, DequeueRef, Error, - Queue}; + Queue +}; + +macro_rules! make_uart_queues { + ($val:expr, $(#[$m:meta])*) => { + // shared mutex allowing safe runtime update to UART config + const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); + + // tx buffer + const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); + #[link_section = ".axisram.buffers"] + static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = + [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; + static COMS_QUEUE_TX: UartWriteQueue = + UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); + + // rx buffer + const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = + SyncUnsafeCell::new(Buffer::EMPTY); + #[link_section = ".axisram.buffers"] + static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = + [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; + static COMS_QUEUE_RX: UartReadQueue = + UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); + }; +} pub struct UartReadQueue< UART: usart::BasicInstance, From 215c070debd0763af709f2562883835ed2f2adbb Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 7 May 2024 00:48:42 -0400 Subject: [PATCH 023/157] father forgive me for I have committed macro sins --- control-board/Cargo.lock | 5 ++- kicker-board/Cargo.lock | 7 +++ lib-stm32-test/Cargo.lock | 7 +++ .../bin/hwtest-uart-queue-mixedbaud/main.rs | 45 ++++++------------- lib-stm32/Cargo.lock | 7 +++ lib-stm32/Cargo.toml | 1 + lib-stm32/src/lib.rs | 4 +- lib-stm32/src/uart/queue.rs | 44 ++++++++++-------- 8 files changed, 68 insertions(+), 52 deletions(-) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index cf34f1cf..62e10f1f 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -79,6 +79,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "paste", ] [[package]] @@ -1022,9 +1023,9 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.14" +version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" [[package]] name = "peeking_take_while" diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 79260ecc..176b3c2f 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -68,6 +68,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "paste", ] [[package]] @@ -949,6 +950,12 @@ dependencies = [ "defmt", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "peeking_take_while" version = "0.1.2" diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index ec540ea4..422a826b 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -14,6 +14,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "paste", ] [[package]] @@ -686,6 +687,12 @@ dependencies = [ "defmt", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pin-project-lite" version = "0.2.14" diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index 0e9f87f8..eb06c204 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -2,16 +2,12 @@ #![no_main] #![feature(sync_unsafe_cell)] -use core::{ - cell::SyncUnsafeCell, - sync::atomic::AtomicU32 -}; +use core::sync::atomic::AtomicU32; use embassy_stm32::{ bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, *}, usart::{self, *} }; use embassy_executor::{Executor, InterruptExecutor}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; use defmt::*; @@ -20,7 +16,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queues, queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; @@ -32,29 +28,16 @@ type LedRedPin = PB14; type UserBtnPin = PC13; type UserBtnExti = EXTI13; -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 5; const MAX_RX_PACKET_SIZE: usize = 64; const RX_BUF_DEPTH: usize = 5; +const MAX_TX_PACKET_SIZE: usize = 64; +const TX_BUF_DEPTH: usize = 5; -// control communications tx buffer -const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".axisram.buffers"] -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".axisram.buffers"] -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); +make_uart_queues!(COMS, + ComsUartModule, ComsUartRxDma, ComsUartTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); static BAUD_RATE: AtomicU32 = AtomicU32::new(115200); @@ -212,18 +195,18 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - unwrap!(high_pri_spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - unwrap!(high_pri_spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + unwrap!(high_pri_spawner.spawn(COMS_RX_UART_QUEUE.spawn_task(coms_uart_rx))); + unwrap!(high_pri_spawner.spawn(COMS_TX_UART_QUEUE.spawn_task(coms_uart_tx))); // MIGHT should put queues in mix prio, this could elicit the bug // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14, &COMS_QUEUE_TX))); + unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14, &COMS_TX_UART_QUEUE))); // unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); // unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(rx_task(&COMS_QUEUE_RX))); - unwrap!(spawner.spawn(tx_task(&COMS_QUEUE_TX))); + unwrap!(spawner.spawn(rx_task(&COMS_RX_UART_QUEUE))); + unwrap!(spawner.spawn(tx_task(&COMS_TX_UART_QUEUE))); }); } \ No newline at end of file diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 8c1ad683..efda7676 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -14,6 +14,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "paste", ] [[package]] @@ -521,6 +522,12 @@ dependencies = [ "autocfg", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "pin-project-lite" version = "0.2.14" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 6c95e793..aa6b0c92 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -12,6 +12,7 @@ embassy-executor = { version = "0.5.0", default-features = false } embassy-sync = { version = "0.5.0" } embassy-futures = { version = "0.1.0" } embassy-time = { version = "0.3.0" } +paste = { version = "1.0.15" } defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, which dropped in 3.4 (3.3 recalled) defmt-rtt = "0.3" diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index f1965fbc..1d0bac28 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -5,9 +5,11 @@ #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] - pub mod drivers; pub mod uart; pub mod queue; +// required for exported queue macros +pub extern crate paste; + diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index e9fdeb11..9d2eed3b 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -27,28 +27,33 @@ use crate::queue::{ Queue }; +#[macro_export] macro_rules! make_uart_queues { - ($val:expr, $(#[$m:meta])*) => { + ($name:ident, $uart:ty, $uart_rx_dma:ty, $uart_tx_dma:ty, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $(#[$m:meta])*) => { + use $crate::paste; + paste::paste! { // shared mutex allowing safe runtime update to UART config - const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); + const [<$name _UART_PERI_MUTEX>]: embassy_sync::mutex::Mutex = + embassy_sync::mutex::Mutex::new(false); // tx buffer - const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); - #[link_section = ".axisram.buffers"] - static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; - static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); + const [<$name _CONST_TX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>> = + core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + $(#[$m])* + static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = + [[<$name _CONST_TX_BUF_VAL>]; $tx_buffer_depth]; + static [<$name _TX_UART_QUEUE>]: $crate::uart::queue::UartWriteQueue<$uart, $uart_tx_dma, $tx_buffer_size, $tx_buffer_depth> = + $crate::uart::queue::UartWriteQueue::new(&[<$name _TX_BUFFER>], [<$name _UART_PERI_MUTEX>]); // rx buffer - const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); - #[link_section = ".axisram.buffers"] - static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; - static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); + const [<$name _CONST_RX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>> = + core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + $(#[$m])* + static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = + [[<$name _CONST_RX_BUF_VAL>]; $rx_buffer_depth]; + static [<$name _RX_UART_QUEUE>]: $crate::uart::queue::UartReadQueue<$uart, $uart_rx_dma, $rx_buffer_size, $rx_buffer_depth> = + $crate::uart::queue::UartReadQueue::new(&[<$name _RX_BUFFER>], [<$name _UART_PERI_MUTEX>]); + } }; } @@ -109,7 +114,9 @@ impl< { let _rw_tasks_config_lock = self.uart_mutex.lock().await; - match select(rx.read_until_idle(buf.data()), Timer::after_millis(500)).await { + // NOTE: this really shouldn't be a timeout, it should be a signal from tx side that a new config + // is desired. This works for now but the timeout is hacky. + match select(rx.read_until_idle(buf.data()), Timer::after_millis(2000)).await { Either::First(len) => { if let Ok(len) = len { if len == 0 { @@ -125,7 +132,8 @@ impl< } }, Either::Second(_) => { - defmt::trace!("UartReadQueue - Read to idle timed out") + defmt::trace!("UartReadQueue - Read to idle timed out"); + buf.cancel(); } } } // frees the inter-task uart config lock From e61e9ec467f65fb65abf827e6c1bec6d0fdda800 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 7 May 2024 23:17:44 -0400 Subject: [PATCH 024/157] fix many errors up to radio abstraction --- control-board/src/bin/control/main.rs | 230 +++++++++-------- control-board/src/bin/hwtest-enc/control.rs | 8 +- control-board/src/bin/hwtest-enc/main.rs | 10 +- control-board/src/lib.rs | 89 ++++++- control-board/src/motion/mod.rs | 1 + .../{bin/control => motion/tasks}/control.rs | 240 ++++++------------ control-board/src/motion/tasks/mod.rs | 1 + control-board/src/pins.rs | 63 +++-- control-board/src/{bin/control => }/radio.rs | 60 ++--- control-board/src/stm32_interface.rs | 83 +----- 10 files changed, 374 insertions(+), 411 deletions(-) rename control-board/src/{bin/control => motion/tasks}/control.rs (61%) create mode 100644 control-board/src/motion/tasks/mod.rs rename control-board/src/{bin/control => }/radio.rs (84%) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 3edb25c4..3d5c3b6b 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -3,33 +3,18 @@ #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] #![feature(async_closure)] +#![feature(sync_unsafe_cell)] use apa102_spi::Apa102; use ateam_control_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, - drivers::{ - radio::TeamColor, - radio::WifiNetwork, - rotary::Rotary, - shell_indicator::ShellIndicator, - kicker::Kicker, - }, - stm32_interface::{get_bootloader_uart_config, Stm32Interface}, - BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE, include_kicker_bin, parameter_interface::ParameterInterface, + adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ + kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator + }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE }; use control::Control; use defmt::info; use embassy_stm32::{ - adc::{Adc, SampleTime}, - dma::NoDma, - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt, - peripherals::{DMA2_CH4, DMA2_CH5, USART6}, - spi, - time::{hz, mhz}, - usart::{self, Uart}, - wdg::IndependentWatchdog, + adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog }; use embassy_executor::InterruptExecutor; use embassy_time::{Duration, Ticker, Timer}; @@ -37,27 +22,23 @@ use ateam_control_board::pins::{ PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, ShutdownCompletePin, }; -use radio::{ - RadioTest, BUFFERS_RX, BUFFERS_TX, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, -}; + use smart_leds::{SmartLedsWrite, RGB8}; -use static_cell::StaticCell; -use embassy_stm32::rcc::AdcClockSource; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_sync::pubsub::PubSubChannel; -use ateam_lib_stm32::queue::Buffer; -use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; +use ateam_lib_stm32::make_uart_queues; -mod control; -mod radio; +use ateam_control_board::motion::tasks::control; +use ateam_control_board::pins::*; +use ateam_control_board::radio::RadioTest; #[cfg(not(feature = "no-private-credentials"))] use credentials::private_credentials::wifi::wifi_credentials; #[cfg(feature = "no-private-credentials")] use credentials::public_credentials::wifi::wifi_credentials; +use static_cell::ConstStaticCell; // Uncomment for testing: // use panic_probe as _; @@ -72,16 +53,45 @@ fn panic(info: &core::panic::PanicInfo) -> ! { include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} -static RADIO_TEST: RadioTest< - MAX_TX_PACKET_SIZE, - MAX_RX_PACKET_SIZE, - TX_BUF_DEPTH, - RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, -> = RadioTest::new(unsafe { &mut BUFFERS_TX }, unsafe { &mut BUFFERS_RX }); +pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; +pub const RADIO_TX_BUF_DEPTH: usize = 4; +pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; +pub const RADIO_RX_BUF_DEPTH: usize = 4; + +pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; +pub const KICKER_TX_BUF_DEPTH: usize = 4; +pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; +pub const KICKER_RX_BUF_DEPTH: usize = 4; + +make_uart_queues!(RADIO, + RadioUART, RadioRxDMA, RadioTxDMA, + RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, + RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(KICKER, + KickerUart, KickerRxDma, KickerTxDma, + KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, + KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +#[link_section = ".sram4"] +static mut SPI6_BUF: [u8; 4] = [0x0; 4]; + +static RADIO_TEST: ConstStaticCell> = + ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); +// static RADIO_TEST: RadioTest< +// RADIO_MAX_TX_PACKET_SIZE, +// RADIO_MAX_RX_PACKET_SIZE, +// RADIO_TX_BUF_DEPTH, +// RADIO_RX_BUF_DEPTH, +// RadioUART, +// RadioRxDMA, +// RadioTxDMA, +// RadioReset, +// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); // pub sub channel for the gyro vals // CAP queue size, n_subs, n_pubs @@ -91,50 +101,45 @@ static GYRO_CHANNEL: PubSubChannel = PubSubCha // CAP queue size, n_subs, n_pubs static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; - -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static KICKER_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut KICKER_BUFFERS_TX }); +// static RADIO: Radio = Radio::new(); +static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static KICKER_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut KICKER_BUFFERS_RX }); +#[interrupt] +unsafe fn CEC() { + EXECUTOR_UART_QUEUE.on_interrupt(); +} -// static RADIO: Radio = Radio::new(); -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); +bind_interrupts!(struct Irqs { + USART10 => usart::InterruptHandler; + USART6 => usart::InterruptHandler; + USART1 => usart::InterruptHandler; + UART4 => usart::InterruptHandler; + UART7 => usart::InterruptHandler; + UART8 => usart::InterruptHandler; + UART5 => usart::InterruptHandler; +}); #[embassy_executor::main] async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - stm32_config.rcc.per_ck = Some(mhz(64)); - stm32_config.rcc.adc_clock_source = AdcClockSource::PerCk; - let p = embassy_stm32::init(stm32_config); + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + let config = usart::Config::default(); // Delay so dotstar and STSPIN can turn on Timer::after(Duration::from_millis(50)).await; - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); + interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); + let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); + let mut dotstar_spi_config = spi::Config::default(); + dotstar_spi_config.frequency = hz(1_000_000); let dotstar_spi = spi::Spi::new_txonly( p.SPI3, p.PB3, p.PB5, - hz(1_000_000), - spi::Config::default(), + p.DMA2_CH6, + dotstar_spi_config, ); let mut dotstar = Apa102::new(dotstar_spi); @@ -142,10 +147,9 @@ async fn main(_spawner: embassy_executor::Spawner) { info!("booted"); - let radio_int = interrupt::take!(USART10); let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, radio_int, p.DMA2_CH0, p.DMA2_CH1, config, - ); + p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, + ).unwrap(); let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); @@ -197,13 +201,13 @@ async fn main(_spawner: embassy_executor::Spawner) { ////////////////////// let mut adc3 = Adc::new(p.ADC3); - adc3.set_sample_time(SampleTime::Cycles1_5); + adc3.set_sample_time(SampleTime::CYCLES1_5); let mut battery_pin = p.PF5; let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); - let imu_spi_config = spi::Config::default(); + let mut imu_spi_config = spi::Config::default(); imu_spi_config.frequency = mhz(1); let mut imu_spi = spi::Spi::new( p.SPI6, @@ -240,57 +244,51 @@ async fn main(_spawner: embassy_executor::Spawner) { info!("gyro id: 0x{:x}", gyro_id); } - let front_right_int = interrupt::take!(USART1); - let front_left_int = interrupt::take!(UART4); - let back_left_int = interrupt::take!(UART7); - let back_right_int = interrupt::take!(UART8); - let drib_int = interrupt::take!(UART5); - let front_right_usart = Uart::new( p.USART1, p.PB15, p.PB14, - front_right_int, + Irqs, p.DMA1_CH0, p.DMA1_CH1, get_bootloader_uart_config(), - ); + ).unwrap(); let front_left_usart = Uart::new( p.UART4, p.PA1, p.PA0, - front_left_int, + Irqs, p.DMA1_CH2, p.DMA1_CH3, get_bootloader_uart_config(), - ); + ).unwrap(); let back_left_usart = Uart::new( p.UART7, p.PF6, p.PF7, - back_left_int, + Irqs, p.DMA1_CH4, p.DMA1_CH5, get_bootloader_uart_config(), - ); + ).unwrap(); let back_right_usart = Uart::new( p.UART8, p.PE0, p.PE1, - back_right_int, + Irqs, p.DMA1_CH6, p.DMA1_CH7, get_bootloader_uart_config(), - ); + ).unwrap(); let drib_usart = Uart::new( p.UART5, p.PB12, p.PB13, - drib_int, + Irqs, p.DMA2_CH2, p.DMA2_CH3, get_bootloader_uart_config(), - ); + ).unwrap(); let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); @@ -299,23 +297,22 @@ async fn main(_spawner: embassy_executor::Spawner) { defmt::warn!("kicker appears unplugged!"); } - let kicker_int = interrupt::take!(USART6); let kicker_usart = Uart::new( p.USART6, p.PC7, p.PC6, - kicker_int, + Irqs, p.DMA2_CH4, p.DMA2_CH5, get_bootloader_uart_config(), - ); + ).unwrap(); - let (kicker_tx, kicker_rx) = kicker_usart.split(); + let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); spawner - .spawn(KICKER_QUEUE_RX.spawn_task(kicker_rx)) + .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) .unwrap(); spawner - .spawn(KICKER_QUEUE_TX.spawn_task(kicker_tx)) + .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) .unwrap(); let ball_detected_thresh = 1.0; @@ -350,8 +347,8 @@ async fn main(_spawner: embassy_executor::Spawner) { let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_QUEUE_RX, - &KICKER_QUEUE_TX, + &KICKER_RX_UART_QUEUE, + &KICKER_TX_UART_QUEUE, Some(kicker_boot0_pin), Some(kicker_reset_pin), ); @@ -372,13 +369,29 @@ async fn main(_spawner: embassy_executor::Spawner) { .cloned(), ); + // let token = unsafe { + // (&mut *(&RADIO_TEST as *const _ + // as *mut RadioTest< + // RADIO_MAX_TX_PACKET_SIZE, + // RADIO_MAX_RX_PACKET_SIZE, + // RADIO_TX_BUF_DEPTH, + // RADIO_RX_BUF_DEPTH, + // RadioUART, + // RadioRxDMA, + // RadioTxDMA, + // RadioReset, + // >)) + // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) + // .await + // }; + let radio_test = RADIO_TEST.take(); let token = unsafe { (&mut *(&RADIO_TEST as *const _ as *mut RadioTest< - MAX_TX_PACKET_SIZE, - MAX_RX_PACKET_SIZE, - TX_BUF_DEPTH, - RX_BUF_DEPTH, + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, RadioUART, RadioRxDMA, RadioTxDMA, @@ -387,6 +400,7 @@ async fn main(_spawner: embassy_executor::Spawner) { .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) .await }; + // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; spawner.spawn(token).unwrap(); let _ = dotstar.write( @@ -454,7 +468,7 @@ async fn main(_spawner: embassy_executor::Spawner) { // Parameter Updates // ///////////////////////// - let latest_param_cmd = RADIO_TEST.get_latest_params_cmd(); + let latest_param_cmd = radio_test.get_latest_params_cmd(); if let Some(latest_param_cmd) = latest_param_cmd { let param_cmd_resp = control.apply_command(&latest_param_cmd); @@ -464,10 +478,10 @@ async fn main(_spawner: embassy_executor::Spawner) { // if param_cmd_resp is Ok, then the read/write was successful if let Ok(resp) = param_cmd_resp { defmt::info!("sending successful parameter update command response"); - RADIO_TEST.send_parameter_response(resp).await; + radio_test.send_parameter_response(resp).await; } else if let Err(resp) = param_cmd_resp { defmt::warn!("sending failed parameter updated command response"); - RADIO_TEST.send_parameter_response(resp).await; + radio_test.send_parameter_response(resp).await; } } @@ -475,7 +489,7 @@ async fn main(_spawner: embassy_executor::Spawner) { // Telemtry // //////////////// - let latest_control_cmd = RADIO_TEST.get_latest_control(); + let latest_control_cmd = radio_test.get_latest_control(); let telemetry = control.tick(latest_control_cmd).await; if let (Some(mut telemetry), control_debug_telem) = telemetry { @@ -484,10 +498,10 @@ async fn main(_spawner: embassy_executor::Spawner) { telemetry.kicker_charge_level = kicker.hv_rail_voltage(); telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); - RADIO_TEST.send_telemetry(telemetry).await; + radio_test.send_telemetry(telemetry).await; if control_debug_telemetry_enabled { - RADIO_TEST.send_control_debug_telemetry(control_debug_telem).await; + radio_test.send_control_debug_telemetry(control_debug_telem).await; } } diff --git a/control-board/src/bin/hwtest-enc/control.rs b/control-board/src/bin/hwtest-enc/control.rs index 025e65aa..1a7e5134 100644 --- a/control-board/src/bin/hwtest-enc/control.rs +++ b/control-board/src/bin/hwtest-enc/control.rs @@ -1,10 +1,12 @@ use ateam_control_board::{ include_external_cpp_bin, - queue::Buffer, motion::robot_model::{RobotConstants, RobotModel}, stm32_interface::Stm32Interface, stspin_motor::{DribblerMotor, WheelMotor}, - uart_queue::{UartReadQueue, UartWriteQueue}, +}; +use ateam_lib_stm32::{ + queue::Buffer, + uart::queue::{UartReadQueue, UartWriteQueue}, }; use embassy_executor::SendSpawner; use embassy_stm32::{ @@ -14,7 +16,7 @@ use embassy_stm32::{ use embassy_time::{Duration, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::pins::*; +use ateam_control_board::pins::*; include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} diff --git a/control-board/src/bin/hwtest-enc/main.rs b/control-board/src/bin/hwtest-enc/main.rs index 32dc4fa5..2b3a6a06 100644 --- a/control-board/src/bin/hwtest-enc/main.rs +++ b/control-board/src/bin/hwtest-enc/main.rs @@ -9,22 +9,22 @@ use embassy_time::{Duration, Ticker, Timer}; use panic_probe as _; use apa102_spi::Apa102; -use ateam_control_board::{colors::*, stm32_interface::get_bootloader_uart_config}; +use ateam_control_board::{ + colors::*, + stm32_interface::get_bootloader_uart_config}; use embassy_stm32::{ dma::NoDma, - executor::InterruptExecutor, gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, + interrupt, spi, time::{hz, mhz}, usart::Uart, }; -use futures_util::StreamExt; +use embassy_executor::InterruptExecutor; use smart_leds::SmartLedsWrite; use static_cell::StaticCell; mod control; -mod pins; static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index a1cc93fc..0d4958d9 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -11,10 +11,28 @@ #![feature(const_mut_refs)] #![feature(ptr_metadata)] #![feature(const_fn_floating_point_arithmetic)] +#![feature(sync_unsafe_cell)] + +use embassy_stm32::{ + rcc::{ + mux::{ + Adcsel, Saisel, Sdmmcsel, Spi6sel, Usart16910sel, Usart234578sel, Usbsel + }, + AHBPrescaler, + APBPrescaler, + Hse, HseMode, + Pll, PllDiv, PllMul, PllPreDiv, PllSource, + Sysclk, + VoltageScale + }, + time::Hertz, + Config +}; // pub mod fw_images; pub mod motion; pub mod pins; +pub mod radio; pub mod stm32_interface; pub mod stspin_motor; pub mod parameter_interface; @@ -42,10 +60,11 @@ macro_rules! include_external_cpp_bin { #[macro_export] macro_rules! include_kicker_bin { ($var_name:ident, $bin_file:literal) => { - pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabi/release/", $bin_file)).len()] - = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabi/release/", $bin_file)); + pub static $var_name: &[u8; include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabihf/release/", $bin_file)).len()] + = include_bytes!(concat!(env!("CARGO_MANIFEST_DIR"), "/../kicker-board/target/thumbv7em-none-eabihf/release/", $bin_file)); } } + pub const BATTERY_MIN_VOLTAGE: f32 = 19.0; pub const BATTERY_MAX_VOLTAGE: f32 = 25.2; pub const BATTERY_BUFFER_SIZE: usize = 10; @@ -59,4 +78,70 @@ pub const fn adc_v_to_battery_voltage(adc_mv: f32) -> f32 { (adc_mv / 2.762) * BATTERY_MAX_VOLTAGE } +pub fn get_system_config() -> Config { + let mut config = Config::default(); + + // we have an 8Mhz external crystal + config.rcc.hse = Some(Hse { + freq: Hertz(8_000_000), + mode: HseMode::Oscillator, + }); + + // I'm not actually sure what this does. We dont select it but other + // examples also don't use it in a mux selection, but they still turn + // it on. + config.rcc.csi = true; + + // turn on the hsi48 as a primordial ADC source + config.rcc.hsi48 = Some(Default::default()); + + // configure the PLLs + // validated in ST Cube MX + config.rcc.pll1 = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV1, + mul: PllMul::MUL68, + divp: Some(PllDiv::DIV1), // 544 MHz + divq: Some(PllDiv::DIV4), // 136 MHz + divr: Some(PllDiv::DIV2) // 272 MHz + }); + config.rcc.pll2 = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV1, + mul: PllMul::MUL31, + divp: Some(PllDiv::DIV5), // 49.6 MHz + divq: Some(PllDiv::DIV2), // 124 MHz + divr: Some(PllDiv::DIV1) // 248 MHz + }); + config.rcc.pll3 = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV2, + mul: PllMul::MUL93, + divp: Some(PllDiv::DIV2), // 186 Mhz + divq: Some(PllDiv::DIV3), // 124 MHz + divr: Some(PllDiv::DIV3) // 124 MHz + }); + + // configure + config.rcc.sys = Sysclk::PLL1_P; // 544 MHz + config.rcc.d1c_pre = AHBPrescaler::DIV1; // 544 MHz + config.rcc.ahb_pre = AHBPrescaler::DIV2; // 272 MHz + + config.rcc.apb1_pre = APBPrescaler::DIV2; // 136 MHz + config.rcc.apb2_pre = APBPrescaler::DIV2; // 136 MHz + config.rcc.apb3_pre = APBPrescaler::DIV2; // 136 MHz + config.rcc.apb4_pre = APBPrescaler::DIV2; // 136 MHz + + config.rcc.mux.spi123sel = Saisel::PLL1_Q; // 136 MHz + config.rcc.mux.usart234578sel = Usart234578sel::PCLK1; // 136 MHz + config.rcc.mux.usart16910sel = Usart16910sel::PCLK2; // 136 MHz + config.rcc.mux.spi6sel = Spi6sel::PCLK4; // 136 MHz + config.rcc.mux.sdmmcsel = Sdmmcsel::PLL2_R; // 248 MHz + config.rcc.mux.adcsel = Adcsel::PLL3_R; // 124 MHz + config.rcc.mux.usbsel = Usbsel::PLL3_Q; // 124 MHz + + config.rcc.voltage_scale = VoltageScale::Scale0; + + config +} diff --git a/control-board/src/motion/mod.rs b/control-board/src/motion/mod.rs index 8a59a057..5c00adf5 100644 --- a/control-board/src/motion/mod.rs +++ b/control-board/src/motion/mod.rs @@ -2,5 +2,6 @@ pub mod constant_gain_kalman_filter; pub mod pid; pub mod robot_controller; pub mod robot_model; +pub mod tasks; pub mod params; \ No newline at end of file diff --git a/control-board/src/bin/control/control.rs b/control-board/src/motion/tasks/control.rs similarity index 61% rename from control-board/src/bin/control/control.rs rename to control-board/src/motion/tasks/control.rs index 52e111ed..c808f51d 100644 --- a/control-board/src/bin/control/control.rs +++ b/control-board/src/motion/tasks/control.rs @@ -1,11 +1,11 @@ -use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, ParameterName}; use embassy_executor::SendSpawner; use embassy_stm32::{ - gpio::{Level, Output, Speed}, - usart::Uart, + gpio::{Level, Output, Speed}, mode::Async, usart::Uart }; +use embassy_sync::pubsub::Subscriber; +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_time::{Duration, Timer}; -use ateam_control_board::{ +use crate::{ include_external_cpp_bin, stm32_interface::Stm32Interface, stspin_motor::{WheelMotor, DribblerMotor}, @@ -13,15 +13,26 @@ use ateam_control_board::{ robot_model::{RobotConstants, RobotModel}, robot_controller::BodyVelocityController }, - BATTERY_MIN_VOLTAGE, parameter_interface::ParameterInterface + BATTERY_MIN_VOLTAGE, + parameter_interface::ParameterInterface }; -use ateam_lib_stm32::queue::Buffer; -use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; + use nalgebra::{Vector3, Vector4}; -use embassy_sync::pubsub::Subscriber; -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use ateam_control_board::pins::*; +use ateam_lib_stm32::{ + make_uart_queues, + queue::Buffer +}; + +use ateam_common_packets::bindings_radio::{ + BasicControl, + BasicTelemetry, + ControlDebugTelemetry, + ParameterCommand, + ParameterName +}; + +use crate::pins::*; include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -41,70 +52,35 @@ const RX_BUF_DEPTH: usize = 20; const TICKS_WITHOUT_PACKET_STOP: u16 = 25; -// buffers for front right -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_RX }); - -// buffers for front left -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_RX }); - -// buffers for back left -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_RX }); - -// buffers for back right -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_RX }); - -// buffers for dribbler -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static DRIB_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut DRIB_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static DRIB_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut DRIB_BUFFERS_RX }); +make_uart_queues!(FRONT_LEFT, + MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(BACK_LEFT, + MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(BACK_RIGHT, + MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(FRONT_RIGHT, + MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(DRIB, + MotorDUart, MotorDDmaRx, MotorDDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm @@ -123,9 +99,7 @@ pub struct Control<'a> { MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFRBootPin, - MotorFRResetPin, + TX_BUF_DEPTH >, front_left_motor: WheelMotor< 'static, @@ -135,9 +109,7 @@ pub struct Control<'a> { MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFLBootPin, - MotorFLResetPin, + TX_BUF_DEPTH >, back_left_motor: WheelMotor< 'static, @@ -147,9 +119,7 @@ pub struct Control<'a> { MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBLBootPin, - MotorBLResetPin, + TX_BUF_DEPTH >, back_right_motor: WheelMotor< 'static, @@ -159,9 +129,7 @@ pub struct Control<'a> { MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBRBootPin, - MotorBRResetPin, + TX_BUF_DEPTH >, drib_motor: DribblerMotor< 'static, @@ -171,9 +139,7 @@ pub struct Control<'a> { MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorDBootPin, - MotorDResetPin, + TX_BUF_DEPTH >, ticks_since_packet: u16, gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, @@ -185,11 +151,11 @@ pub struct Control<'a> { impl<'a> Control<'a> { pub fn new( spawner: &SendSpawner, - front_right_usart: Uart<'static, MotorFRUart, MotorFRDmaTx, MotorFRDmaRx>, - front_left_usart: Uart<'static, MotorFLUart, MotorFLDmaTx, MotorFLDmaRx>, - back_left_usart: Uart<'static, MotorBLUart, MotorBLDmaTx, MotorBLDmaRx>, - back_right_usart: Uart<'static, MotorBRUart, MotorBRDmaTx, MotorBRDmaRx>, - drib_usart: Uart<'static, MotorDUart, MotorDDmaTx, MotorDDmaRx>, + front_right_usart: Uart<'static, MotorFRUart, Async>, + front_left_usart: Uart<'static, MotorFLUart, Async>, + back_left_usart: Uart<'static, MotorBLUart, Async>, + back_right_usart: Uart<'static, MotorBRUart, Async>, + drib_usart: Uart<'static, MotorDUart, Async>, front_right_boot0_pin: MotorFRBootPin, front_left_boot0_pin: MotorFLBootPin, back_left_boot0_pin: MotorBLBootPin, @@ -231,115 +197,71 @@ impl<'a> Control<'a> { Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active spawner - .spawn(FRONT_RIGHT_QUEUE_RX.spawn_task(front_right_rx)) + .spawn(FRONT_RIGHT_RX_UART_QUEUE.spawn_task(front_right_rx)) .unwrap(); spawner - .spawn(FRONT_RIGHT_QUEUE_TX.spawn_task(front_right_tx)) + .spawn(FRONT_RIGHT_TX_UART_QUEUE.spawn_task(front_right_tx)) .unwrap(); spawner - .spawn(FRONT_LEFT_QUEUE_RX.spawn_task(front_left_rx)) + .spawn(FRONT_LEFT_RX_UART_QUEUE.spawn_task(front_left_rx)) .unwrap(); spawner - .spawn(FRONT_LEFT_QUEUE_TX.spawn_task(front_left_tx)) + .spawn(FRONT_LEFT_TX_UART_QUEUE.spawn_task(front_left_tx)) .unwrap(); spawner - .spawn(BACK_LEFT_QUEUE_RX.spawn_task(back_left_rx)) + .spawn(BACK_LEFT_RX_UART_QUEUE.spawn_task(back_left_rx)) .unwrap(); - spawner // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); - // // info!("\n"); - // // // info!("vel set {:?}", mrp.data.motion.vel_setpoint + 0.); - // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); - // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); - // // info!("\n"); - // // // info!("vel set {:?}", mrp.data.motion.vel_setpoint + 0.); - // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); - // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); - // info!("master_error {:?}", mrp.data.motion.master_error()); - // info!("{:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); - // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); - // info!("bldc_transition_error {:?}", mrp.data.motion.bldc_transition_error()); - // info!("bldc_commutation_watchdog_error {:?}", mrp.data.motion.bldc_commutation_watchdog_error()); - // info!("enc_disconnected_error {:?}", mrp.data.motion.enc_disconnected_error()); - // info!("enc_decoding_error {:?}", mrp.data.motion.enc_decoding_error()); - // info!("hall_enc_vel_disagreement_error {:?}", mrp.data.motion.hall_enc_vel_disagreement_error()); - // info!("overcurrent_error {:?}", mrp.data.motion.overcurrent_error()); - // info!("undervoltage_error {:?}", mrp.data.motion.undervoltage_error()); - // info!("overvoltage_error {:?}", mrp.data.motion.overvoltage_error()); - // info!("torque_limited {:?}", mrp.data.motion.torque_limited()); - // info!("control_loop_time_error {:?}", mrp.data.motion.control_loop_time_error()); - // info!("reset_watchdog_independent {:?}", mrp.data.motion.reset_watchdog_independent()); - // info!("reset_watchdog_window {:?}", mrp.data.motion.reset_watchdog_window()); - // info!("reset_low_power {:?}", mrp.data.motion.reset_low_power()); - // info!("reset_software {:?}", mrp.data.motion.reset_software()); 0.); - // info!("master_error {:?}", mrp.data.motion.master_error()); - // info!("{:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); - // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); - // info!("bldc_transition_error {:?}", mrp.data.motion.bldc_transition_error()); - // info!("bldc_commutation_watchdog_error {:?}", mrp.data.motion.bldc_commutation_watchdog_error()); - // info!("enc_disconnected_error {:?}", mrp.data.motion.enc_disconnected_error()); - // info!("enc_decoding_error {:?}", mrp.data.motion.enc_decoding_error()); - // info!("hall_enc_vel_disagreement_error {:?}", mrp.data.motion.hall_enc_vel_disagreement_error()); - // info!("overcurrent_error {:?}", mrp.data.motion.overcurrent_error()); - // info!("undervoltage_error {:?}", mrp.data.motion.undervoltage_error()); - // info!("overvoltage_error {:?}", mrp.data.motion.overvoltage_error()); - // info!("torque_limited {:?}", mrp.data.motion.torque_limited()); - // info!("control_loop_time_error {:?}", mrp.data.motion.control_loop_time_error()); - // info!("reset_watchdog_independent {:?}", mrp.data.motion.reset_watchdog_independent()); - // info!("reset_watchdog_window {:?}", mrp.data.motion.reset_watchdog_window()); - // info!("reset_low_power {:?}", mrp.data.motion.reset_low_power()); - // info!("reset_software {:?}", mrp.data.motion.reset_software()); - .spawn(BACK_LEFT_QUEUE_TX.spawn_task(back_left_tx)) + spawner + .spawn(BACK_LEFT_TX_UART_QUEUE.spawn_task(back_left_tx)) .unwrap(); spawner - .spawn(BACK_RIGHT_QUEUE_RX.spawn_task(back_right_rx)) + .spawn(BACK_RIGHT_RX_UART_QUEUE.spawn_task(back_right_rx)) .unwrap(); spawner - .spawn(BACK_RIGHT_QUEUE_TX.spawn_task(back_right_tx)) + .spawn(BACK_RIGHT_TX_UART_QUEUE.spawn_task(back_right_tx)) .unwrap(); spawner - .spawn(DRIB_QUEUE_RX.spawn_task(drib_rx)) + .spawn(DRIB_RX_UART_QUEUE.spawn_task(drib_rx)) .unwrap(); spawner - .spawn(DRIB_QUEUE_TX.spawn_task(drib_tx)) + .spawn(DRIB_TX_UART_QUEUE.spawn_task(drib_tx)) .unwrap(); let front_right_stm32_interface = Stm32Interface::new( - &FRONT_RIGHT_QUEUE_RX, - &FRONT_RIGHT_QUEUE_TX, + &FRONT_RIGHT_RX_UART_QUEUE, + &FRONT_RIGHT_TX_UART_QUEUE, Some(front_right_boot0_pin), Some(front_right_reset_pin), ); let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); let front_left_stm32_interface = Stm32Interface::new( - &FRONT_LEFT_QUEUE_RX, - &FRONT_LEFT_QUEUE_TX, + &FRONT_LEFT_RX_UART_QUEUE, + &FRONT_LEFT_TX_UART_QUEUE, Some(front_left_boot0_pin), Some(front_left_reset_pin), ); let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); let back_left_stm32_interface = Stm32Interface::new( - &BACK_LEFT_QUEUE_RX, - &BACK_LEFT_QUEUE_TX, + &BACK_LEFT_RX_UART_QUEUE, + &BACK_LEFT_TX_UART_QUEUE, Some(back_left_boot0_pin), Some(back_left_reset_pin), ); let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); let back_right_stm32_interface = Stm32Interface::new( - &BACK_RIGHT_QUEUE_RX, - &BACK_RIGHT_QUEUE_TX, + &BACK_RIGHT_RX_UART_QUEUE, + &BACK_RIGHT_TX_UART_QUEUE, Some(back_right_boot0_pin), Some(back_right_reset_pin), ); let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); let drib_stm32_interface = Stm32Interface::new( - &DRIB_QUEUE_RX, - &DRIB_QUEUE_TX, + &DRIB_RX_UART_QUEUE, + &DRIB_TX_UART_QUEUE, Some(drib_boot0_pin), Some(drib_reset_pin), ); diff --git a/control-board/src/motion/tasks/mod.rs b/control-board/src/motion/tasks/mod.rs new file mode 100644 index 00000000..9cc39d68 --- /dev/null +++ b/control-board/src/motion/tasks/mod.rs @@ -0,0 +1 @@ +pub mod control; \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index fa1db175..7ef71f9d 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -2,41 +2,72 @@ use embassy_stm32::peripherals::*; +///////////// +// Radio // +///////////// + pub type RadioUART = USART10; pub type RadioTxDMA = DMA2_CH0; pub type RadioRxDMA = DMA2_CH1; pub type RadioReset = PC13; -pub type MotorFRUart = USART1; -pub type MotorFLUart = UART4; -pub type MotorBLUart = UART7; -pub type MotorBRUart = UART8; -pub type MotorDUart = UART5; +////////////// +// Kicker // +////////////// + +pub type KickerUart = USART6; +pub type KickerRxDma = DMA2_CH5; +pub type KickerTxDma = DMA2_CH4; + + +////////////// +// Motors // +////////////// + +pub type MotorFRUart = USART1; pub type MotorFRDmaTx = DMA1_CH0; pub type MotorFRDmaRx = DMA1_CH1; +pub type MotorFRBootPin = PD8; +pub type MotorFRResetPin = PD9; + +pub type MotorFLUart = UART4; pub type MotorFLDmaTx = DMA1_CH2; pub type MotorFLDmaRx = DMA1_CH3; +pub type MotorFLBootPin = PC1; +pub type MotorFLResetPin = PC0; + +pub type MotorBLUart = UART7; pub type MotorBLDmaTx = DMA1_CH4; pub type MotorBLDmaRx = DMA1_CH5; +pub type MotorBLBootPin = PF8; +pub type MotorBLResetPin = PF9; + +pub type MotorBRUart = UART8; pub type MotorBRDmaTx = DMA1_CH6; pub type MotorBRDmaRx = DMA1_CH7; +pub type MotorBRBootPin = PB9; +pub type MotorBRResetPin = PB8; + +pub type MotorDUart = UART5; pub type MotorDDmaTx = DMA2_CH2; pub type MotorDDmaRx = DMA2_CH3; - -pub type MotorFRBootPin = PD8; -pub type MotorFLBootPin = PC1; -pub type MotorBLBootPin = PF8; -pub type MotorBRBootPin = PB9; pub type MotorDBootPin = PD13; - -pub type MotorFRResetPin = PD9; -pub type MotorFLResetPin = PC0; -pub type MotorBLResetPin = PF9; -pub type MotorBRResetPin = PB8; pub type MotorDResetPin = PD12; +////////////////////////// +// Power Control Pins // +////////////////////////// + pub type PowerStatePin = PF5; pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; \ No newline at end of file +pub type ShutdownCompletePin = PF4; + + +/////////////// +// User IO // +/////////////// + +pub type DotstarSpi = SPI3; +pub type DotstarTxDma = DMA2_CH6; \ No newline at end of file diff --git a/control-board/src/bin/control/radio.rs b/control-board/src/radio.rs similarity index 84% rename from control-board/src/bin/control/radio.rs rename to control-board/src/radio.rs index 905a367a..b997e348 100644 --- a/control-board/src/bin/control/radio.rs +++ b/control-board/src/radio.rs @@ -1,32 +1,23 @@ -// use core::cell::RefCell; - -use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand}; -use ateam_common_packets::radio::DataPacket; -use ateam_control_board::{ - drivers::radio::{RobotRadio, TeamColor, WifiNetwork}, - queue, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; use core::future::Future; -use defmt::*; + use embassy_executor::{raw::TaskStorage, SendSpawner, SpawnToken}; use embassy_futures::join::join; -use embassy_stm32::{gpio::Pin, usart, Peripheral}; +use embassy_stm32::{gpio::Pin, usart}; +use embassy_stm32::mode::Async; +use embassy_stm32::usart::Uart; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::{Duration, Ticker}; -use futures_util::StreamExt; -pub const MAX_TX_PACKET_SIZE: usize = 256; -pub const TX_BUF_DEPTH: usize = 4; -pub const MAX_RX_PACKET_SIZE: usize = 256; -pub const RX_BUF_DEPTH: usize = 4; +use defmt::*; -#[link_section = ".axisram.buffers"] -pub static mut BUFFERS_TX: [queue::Buffer; TX_BUF_DEPTH] = - [queue::Buffer::EMPTY; TX_BUF_DEPTH]; -#[link_section = ".axisram.buffers"] -pub static mut BUFFERS_RX: [queue::Buffer; RX_BUF_DEPTH] = - [queue::Buffer::EMPTY; RX_BUF_DEPTH]; +use ateam_lib_stm32::make_uart_queues; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; + +use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand}; +use ateam_common_packets::radio::DataPacket; + +use crate::drivers::radio::{RobotRadio, TeamColor, WifiNetwork}; +use crate::pins::*; type ControlTaskFuture< UART: usart::BasicInstance + Send, @@ -71,16 +62,14 @@ pub struct RadioTest< > where UART::Interrupt: Send, { - queue_tx: UartWriteQueue<'static, UART, TxDma, LEN_TX, DEPTH_TX>, - queue_rx: UartReadQueue<'static, UART, RxDma, LEN_RX, DEPTH_RX>, + queue_tx: &'static UartWriteQueue, + queue_rx: &'static UartReadQueue, task: TaskStorage< ControlTaskFuture, >, latest_control: Mutex>, latest_params: Mutex>, - radio: Option< - RobotRadio<'static, UART, RxDma, TxDma, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX, ResetPin>, - >, + radio: Option>, } impl< @@ -97,12 +86,12 @@ where UART::Interrupt: Send, { pub const fn new( - tx_buffer: &'static mut [queue::Buffer; DEPTH_TX], - rx_buffer: &'static mut [queue::Buffer; DEPTH_RX], + tx_queue: &'static UartWriteQueue, + rx_queue: &'static UartReadQueue, ) -> Self { RadioTest { - queue_rx: UartReadQueue::new(rx_buffer), - queue_tx: UartWriteQueue::new(tx_buffer), + queue_rx: rx_queue, + queue_tx: tx_queue, task: TaskStorage::new(), latest_control: Mutex::new(None), latest_params: Mutex::new(None), @@ -115,13 +104,13 @@ where pub async fn setup( &'static mut self, spawner: &SendSpawner, - uart: usart::Uart<'static, UART, TxDma, RxDma>, - reset_pin: impl Peripheral

+ 'static, + uart: usart::Uart<'static, UART, Async>, + reset_pin: impl Pin, id: u8, team: TeamColor, wifi_network: WifiNetwork, ) -> SpawnToken { - let (tx, rx) = uart.split(); + let (tx, rx) = Uart::split(uart); spawner.spawn(self.queue_rx.spawn_task(rx)).unwrap(); spawner.spawn(self.queue_tx.spawn_task(tx)).unwrap(); @@ -175,8 +164,7 @@ where LEN_TX, LEN_RX, DEPTH_TX, - DEPTH_RX, - ResetPin, + DEPTH_RX >, latest_control: &'static Mutex>, latest_param_cmd: &'static Mutex>, diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 170a55f9..4f10bc64 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -182,7 +182,7 @@ impl< // ensure UART is in the expected config for the bootloader // this operation is unsafe because it takes the uart module offline // when the executor may be relying on rx interrupts to unblock a thread - unsafe { self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven); } + self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven).await; defmt::debug!("sending the bootloader baud calibration command..."); self.writer @@ -239,87 +239,6 @@ impl< config.parity = parity; self.writer.update_uart_config(config).await; - - - // let div = (UART::frequency().0 + (baudrate / 2)) / baudrate * UART::MULTIPLIER; - - // // let irq = UART::Interrupt::steal(); - // // irq.disable(); - - // let r = UART::regs(); - // // disable the uart. Can't modify parity and baudrate while module is enabled - // r.cr1().modify(|w| { - // w.set_ue(false); - // }); - - // // set the baudrate - // r.brr().modify(|w| { - // w.set_brr(div); - // }); - - // // set parity - // r.cr1().modify(|w| { - // if parity != Parity::ParityNone { - // // configure 9 bit transmission and enable parity control - // w.set_m0(pac::usart::vals::M0::BIT9); - // w.set_pce(true); - - // // set polarity type - // if parity == Parity::ParityEven { - // w.set_ps(pac::usart::vals::Ps::EVEN); - // } else { - // w.set_ps(pac::usart::vals::Ps::ODD); - // } - // } else { - // // disable parity (1 byte transmissions) - // w.set_m0(pac::usart::vals::M0::BIT8); - // w.set_pce(false); - // } - // }); - - // // reenable UART - // r.cr1().modify(|w| { - // w.set_ue(true); - // }); - - // let div = (pclk_freq.0 + (config.baudrate / 2)) / config.baudrate * multiplier; - - // unsafe { - // r.brr().write_value(regs::Brr(div)); - // r.cr2().write(|_w| {}); - // r.cr1().write(|w| { - // // enable uart - // w.set_ue(true); - // // enable transceiver - // w.set_te(true); - // // enable receiver - // w.set_re(true); - // // configure word size - // w.set_m0(if parity != Parity::ParityNone { - // pac::lpuart::vals::M0::BIT9 - // } else { - // pac::lpuart::vals::M0::BIT8 - // }); - // // configure parity - // w.set_pce(parity != Parity::ParityNone); - // w.set_ps(match parity { - // Parity::ParityOdd => pac::lpuart::vals::Ps::ODD, - // Parity::ParityEven => pac::lpuart::vals::Ps::EVEN, - // _ => pac::lpuart::vals::Ps::EVEN, - // }); - // }); - // } - - // r.icr().write(|w| { - // w.set_fecf(true); - // }); - - // irq.unpend(); - // irq.enable(); - - // r.cr1().modify(|w| { - // w.set_idleie(true); - // }); } pub async fn read_latest_packet(&self) { From 968c7683c5aceb674cb967b67520ba529e0d712a Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 10 May 2024 18:09:37 -0400 Subject: [PATCH 025/157] initial radio task up after refactor --- control-board/.cargo/config.toml | 4 +- control-board/src/bin/control-old/main.rs | 531 +++++++++++++++++ control-board/src/bin/control/main.rs | 538 ++---------------- control-board/src/drivers/radio/radio.rs | 4 + .../src/drivers/radio/radio_robot.rs | 66 ++- control-board/src/lib.rs | 22 +- control-board/src/motion/tasks/control.rs | 5 +- control-board/src/motion/tasks/mod.rs | 3 +- control-board/src/motion/tasks/radio_task.rs | 137 +++++ control-board/src/pins.rs | 20 +- control-board/src/radio.rs | 3 +- .../bin/hwtest-uart-queue-mixedbaud/main.rs | 2 +- lib-stm32/src/uart/queue.rs | 7 +- 13 files changed, 794 insertions(+), 548 deletions(-) create mode 100644 control-board/src/bin/control-old/main.rs create mode 100644 control-board/src/motion/tasks/radio_task.rs diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index 796af318..ddc8783f 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -2,9 +2,7 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] -# runner = 'probe-run --chip STM32H743ZITx --connect-under-reset --probe 0017000F4D46500F20383832' -runner = 'probe-run --chip STM32H723ZGTx --connect-under-reset' -# runner = 'probe-run --chip STM32H7A3ZITx --connect-under-reset' +runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' [env] DEFMT_LOG = "trace" diff --git a/control-board/src/bin/control-old/main.rs b/control-board/src/bin/control-old/main.rs new file mode 100644 index 00000000..3d5c3b6b --- /dev/null +++ b/control-board/src/bin/control-old/main.rs @@ -0,0 +1,531 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(const_mut_refs)] +#![feature(async_closure)] +#![feature(sync_unsafe_cell)] + +use apa102_spi::Apa102; +use ateam_control_board::{ + adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ + kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator + }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE +}; +use control::Control; +use defmt::info; +use embassy_stm32::{ + adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog +}; +use embassy_executor::InterruptExecutor; +use embassy_time::{Duration, Ticker, Timer}; +use ateam_control_board::pins::{ + PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, + ShutdownCompletePin, +}; + +use smart_leds::{SmartLedsWrite, RGB8}; + +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +use embassy_sync::pubsub::PubSubChannel; + +use ateam_lib_stm32::make_uart_queues; + +use ateam_control_board::motion::tasks::control; +use ateam_control_board::pins::*; +use ateam_control_board::radio::RadioTest; + +#[cfg(not(feature = "no-private-credentials"))] +use credentials::private_credentials::wifi::wifi_credentials; +#[cfg(feature = "no-private-credentials")] +use credentials::public_credentials::wifi::wifi_credentials; +use static_cell::ConstStaticCell; + +// Uncomment for testing: +// use panic_probe as _; + +#[panic_handler] +fn panic(info: &core::panic::PanicInfo) -> ! { + defmt::error!("{}", defmt::Display2Format(info)); + // Delay to give it a change to print + cortex_m::asm::delay(10_000_000); + cortex_m::peripheral::SCB::sys_reset(); +} + +include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} + +pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; +pub const RADIO_TX_BUF_DEPTH: usize = 4; +pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; +pub const RADIO_RX_BUF_DEPTH: usize = 4; + +pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; +pub const KICKER_TX_BUF_DEPTH: usize = 4; +pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; +pub const KICKER_RX_BUF_DEPTH: usize = 4; + +make_uart_queues!(RADIO, + RadioUART, RadioRxDMA, RadioTxDMA, + RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, + RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queues!(KICKER, + KickerUart, KickerRxDma, KickerTxDma, + KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, + KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +#[link_section = ".sram4"] +static mut SPI6_BUF: [u8; 4] = [0x0; 4]; + +static RADIO_TEST: ConstStaticCell> = + ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); +// static RADIO_TEST: RadioTest< +// RADIO_MAX_TX_PACKET_SIZE, +// RADIO_MAX_RX_PACKET_SIZE, +// RADIO_TX_BUF_DEPTH, +// RADIO_RX_BUF_DEPTH, +// RadioUART, +// RadioRxDMA, +// RadioTxDMA, +// RadioReset, +// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); + +// pub sub channel for the gyro vals +// CAP queue size, n_subs, n_pubs +static GYRO_CHANNEL: PubSubChannel = PubSubChannel::new(); + +// pub sub channel for the battery raw adc vals +// CAP queue size, n_subs, n_pubs +static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); + +// static RADIO: Radio = Radio::new(); +static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + EXECUTOR_UART_QUEUE.on_interrupt(); +} + +bind_interrupts!(struct Irqs { + USART10 => usart::InterruptHandler; + USART6 => usart::InterruptHandler; + USART1 => usart::InterruptHandler; + UART4 => usart::InterruptHandler; + UART7 => usart::InterruptHandler; + UART8 => usart::InterruptHandler; + UART5 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: embassy_executor::Spawner) { + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + let config = usart::Config::default(); + + // Delay so dotstar and STSPIN can turn on + Timer::after(Duration::from_millis(50)).await; + + interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); + let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); + + let mut dotstar_spi_config = spi::Config::default(); + dotstar_spi_config.frequency = hz(1_000_000); + let dotstar_spi = spi::Spi::new_txonly( + p.SPI3, + p.PB3, + p.PB5, + p.DMA2_CH6, + dotstar_spi_config, + ); + + let mut dotstar = Apa102::new(dotstar_spi); + let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); + + info!("booted"); + + let radio_usart = Uart::new( + p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, + ).unwrap(); + + let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); + let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); + let kicker_det = Input::new(p.PG8, Pull::Down); + let dip1 = Input::new(p.PG7, Pull::Down); + let dip2 = Input::new(p.PG6, Pull::Down); + let dip3 = Input::new(p.PG5, Pull::Down); + let dip4 = Input::new(p.PG4, Pull::Down); + let dip5 = Input::new(p.PG3, Pull::Down); + let _dip6 = Input::new(p.PG2, Pull::Down); + let dip7 = Input::new(p.PD15, Pull::Down); + + // let robot_id = rotary.read(); + + //////////////////////// + // Dip Switch Inputs // + //////////////////////// + let robot_id = (dip1.is_high() as u8) << 3 + | (dip2.is_high() as u8) << 2 + | (dip3.is_high() as u8) << 1 + | (dip4.is_high() as u8); + info!("id: {}", robot_id); + shell_indicator.set(robot_id); + + let wifi_network = WifiNetwork::Team; + // let wifi_network = if dip5.is_high() & dip6.is_high() { + // WifiNetwork::Team + // } else if dip5.is_low() & dip6.is_high() { + // WifiNetwork::CompMain + // } else if dip5.is_high() & dip6.is_low() { + // WifiNetwork::CompPractice + // } else { + // WifiNetwork::Team + // }; + + let control_debug_telemetry_enabled = dip5.is_high(); + if control_debug_telemetry_enabled { + info!("Debug control telemetry transmission enabled"); + } + + let team = if dip7.is_high() { + TeamColor::Blue + } else { + TeamColor::Yellow + }; + + ////////////////////// + // Battery Voltage // + ////////////////////// + + let mut adc3 = Adc::new(p.ADC3); + adc3.set_sample_time(SampleTime::CYCLES1_5); + let mut battery_pin = p.PF5; + let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = + [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; + let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); + + let mut imu_spi_config = spi::Config::default(); + imu_spi_config.frequency = mhz(1); + let mut imu_spi = spi::Spi::new( + p.SPI6, + p.PA5, + p.PA7, + p.PA6, + p.BDMA_CH0, + p.BDMA_CH1, + imu_spi_config, + ); + + // acceleromter + let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); + // gyro + let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); + + Timer::after(Duration::from_millis(1)).await; + + let gyro_pub = GYRO_CHANNEL.publisher().unwrap(); + unsafe { + SPI6_BUF[0] = 0x80; + // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); + imu_cs1.set_low(); + let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; + imu_cs1.set_high(); + let accel_id = SPI6_BUF[1]; + info!("accelerometer id: 0x{:x}", accel_id); + + SPI6_BUF[0] = 0x80; + imu_cs2.set_low(); + let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; + imu_cs2.set_high(); + let gyro_id = SPI6_BUF[1]; + info!("gyro id: 0x{:x}", gyro_id); + } + + let front_right_usart = Uart::new( + p.USART1, + p.PB15, + p.PB14, + Irqs, + p.DMA1_CH0, + p.DMA1_CH1, + get_bootloader_uart_config(), + ).unwrap(); + let front_left_usart = Uart::new( + p.UART4, + p.PA1, + p.PA0, + Irqs, + p.DMA1_CH2, + p.DMA1_CH3, + get_bootloader_uart_config(), + ).unwrap(); + let back_left_usart = Uart::new( + p.UART7, + p.PF6, + p.PF7, + Irqs, + p.DMA1_CH4, + p.DMA1_CH5, + get_bootloader_uart_config(), + ).unwrap(); + let back_right_usart = Uart::new( + p.UART8, + p.PE0, + p.PE1, + Irqs, + p.DMA1_CH6, + p.DMA1_CH7, + get_bootloader_uart_config(), + ).unwrap(); + let drib_usart = Uart::new( + p.UART5, + p.PB12, + p.PB13, + Irqs, + p.DMA2_CH2, + p.DMA2_CH3, + get_bootloader_uart_config(), + ).unwrap(); + + let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); + let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); + + if kicker_det.is_low() { + defmt::warn!("kicker appears unplugged!"); + } + + let kicker_usart = Uart::new( + p.USART6, + p.PC7, + p.PC6, + Irqs, + p.DMA2_CH4, + p.DMA2_CH5, + get_bootloader_uart_config(), + ).unwrap(); + + let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); + spawner + .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) + .unwrap(); + spawner + .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) + .unwrap(); + + let ball_detected_thresh = 1.0; + let mut control = Control::new( + &spawner, + front_right_usart, + front_left_usart, + back_left_usart, + back_right_usart, + drib_usart, + p.PD8, + p.PC1, + p.PF8, + p.PB9, + p.PD13, + p.PD9, + p.PC0, + p.PF9, + p.PB8, + p.PD12, + ball_detected_thresh, + gyro_sub, + battery_sub, + ); + + let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); + + control.load_firmware().await; + + info!("flashing kicker..."); + + let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); + let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); + let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( + &KICKER_RX_UART_QUEUE, + &KICKER_TX_UART_QUEUE, + Some(kicker_boot0_pin), + Some(kicker_reset_pin), + ); + let kicker_firmware_image = KICKER_FW_IMG; + let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); + let res = kicker.load_default_firmware_image().await; + + if res.is_err() { + defmt::warn!("kicker flashing failed."); + loop {} + } else { + info!("kicker flash complete"); + } + + let _ = dotstar.write( + [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }] + .iter() + .cloned(), + ); + + // let token = unsafe { + // (&mut *(&RADIO_TEST as *const _ + // as *mut RadioTest< + // RADIO_MAX_TX_PACKET_SIZE, + // RADIO_MAX_RX_PACKET_SIZE, + // RADIO_TX_BUF_DEPTH, + // RADIO_RX_BUF_DEPTH, + // RadioUART, + // RadioRxDMA, + // RadioTxDMA, + // RadioReset, + // >)) + // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) + // .await + // }; + let radio_test = RADIO_TEST.take(); + let token = unsafe { + (&mut *(&RADIO_TEST as *const _ + as *mut RadioTest< + RADIO_MAX_TX_PACKET_SIZE, + RADIO_MAX_RX_PACKET_SIZE, + RADIO_TX_BUF_DEPTH, + RADIO_RX_BUF_DEPTH, + RadioUART, + RadioRxDMA, + RadioTxDMA, + RadioReset, + >)) + .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) + .await + }; + // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; + spawner.spawn(token).unwrap(); + + let _ = dotstar.write( + [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] + .iter() + .cloned(), + ); + + let mut wdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); + unsafe { wdg.unleash() } + + let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); + + kicker.set_telemetry_enabled(true); + kicker.send_command(); + + defmt::info!("using SSID: {}", wifi_credentials[0].get_ssid()); + + loop { + unsafe { wdg.pet() }; + + unsafe { + SPI6_BUF[0] = 0x86; + imu_cs2.set_low(); + let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; + imu_cs2.set_high(); + let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; + // info!("z rate: {}", rate_z); + let gyro_conversion = 2000.0 / 32767.0; + gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion); + } + + // could just feed gyro in here but the comment in control said to use a channel + + /////////////////////// + // Battery reading // + /////////////////////// + + let current_battery_v = + adc_v_to_battery_voltage(adc_raw_to_v(adc3.read(&mut battery_pin) as f32)); + // Shift buffer through + for i in (0..(BATTERY_BUFFER_SIZE - 1)).rev() { + battery_voltage_buffer[i + 1] = battery_voltage_buffer[i]; + } + // Add new battery read + battery_voltage_buffer[0] = current_battery_v; + let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); + let filter_battery_v = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); + battery_pub.publish_immediate(filter_battery_v); + + if filter_battery_v < BATTERY_MIN_VOLTAGE { + let _ = dotstar.write( + [RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 10, g: 0, b: 0 }] + .iter() + .cloned(), + ); + defmt::error!("Error filtered battery voltage too low"); + critical_section::with(|_| loop { + cortex_m::asm::delay(1_000_000); + unsafe { wdg.pet() }; + }); + } + + ///////////////////////// + // Parameter Updates // + ///////////////////////// + + let latest_param_cmd = radio_test.get_latest_params_cmd(); + + if let Some(latest_param_cmd) = latest_param_cmd { + let param_cmd_resp = control.apply_command(&latest_param_cmd); + + // if param_cmd_resp is Err, then the requested parameter update had no submodule acceping the + // field, or the type was invalid, or the update code is unimplemented + // if param_cmd_resp is Ok, then the read/write was successful + if let Ok(resp) = param_cmd_resp { + defmt::info!("sending successful parameter update command response"); + radio_test.send_parameter_response(resp).await; + } else if let Err(resp) = param_cmd_resp { + defmt::warn!("sending failed parameter updated command response"); + radio_test.send_parameter_response(resp).await; + } + } + + //////////////// + // Telemtry // + //////////////// + + let latest_control_cmd = radio_test.get_latest_control(); + + let telemetry = control.tick(latest_control_cmd).await; + if let (Some(mut telemetry), control_debug_telem) = telemetry { + // info!("{:?}", defmt::Debug2Format(&telemetry)); + + telemetry.kicker_charge_level = kicker.hv_rail_voltage(); + telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); + + radio_test.send_telemetry(telemetry).await; + + if control_debug_telemetry_enabled { + radio_test.send_control_debug_telemetry(control_debug_telem).await; + } + } + + kicker.process_telemetry(); + + if let Some(control) = latest_control_cmd { + kicker.set_kick_strength(control.kick_vel); + kicker.request_kick(control.kick_request); + kicker.send_command(); + } + + main_loop_rate_ticker.next().await; + } +} + +#[embassy_executor::task] +async fn power_off_task( + power_state_pin: PowerStatePin, + exti: PowerStateExti, + shutdown_pin: ShutdownCompletePin, +) { + let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); + let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); + power_state.wait_for_falling_edge().await; + shutdown.set_high(); + loop {} +} diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 3d5c3b6b..67aed410 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -1,531 +1,85 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(async_closure)] -#![feature(sync_unsafe_cell)] -use apa102_spi::Apa102; -use ateam_control_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ - kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator - }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE -}; -use control::Control; -use defmt::info; +use embassy_executor::InterruptExecutor; use embassy_stm32::{ - adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog + interrupt, + pac::Interrupt }; -use embassy_executor::InterruptExecutor; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_control_board::pins::{ - PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, - ShutdownCompletePin, +use embassy_sync::{ + blocking_mutex::raw::ThreadModeRawMutex, + pubsub::PubSubChannel }; -use smart_leds::{SmartLedsWrite, RGB8}; +use defmt_rtt as _; -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::PubSubChannel; +use ateam_common_packets::radio::{DataPacket, TelemetryPacket}; -use ateam_lib_stm32::make_uart_queues; +use ateam_control_board::{get_system_config, motion::tasks::radio_task::start_radio_task}; -use ateam_control_board::motion::tasks::control; -use ateam_control_board::pins::*; -use ateam_control_board::radio::RadioTest; +// load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] use credentials::private_credentials::wifi::wifi_credentials; #[cfg(feature = "no-private-credentials")] use credentials::public_credentials::wifi::wifi_credentials; -use static_cell::ConstStaticCell; - -// Uncomment for testing: -// use panic_probe as _; - -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - defmt::error!("{}", defmt::Display2Format(info)); - // Delay to give it a change to print - cortex_m::asm::delay(10_000_000); - cortex_m::peripheral::SCB::sys_reset(); -} - -include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} - -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; -pub const RADIO_TX_BUF_DEPTH: usize = 4; -pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; -pub const RADIO_RX_BUF_DEPTH: usize = 4; -pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; -pub const KICKER_TX_BUF_DEPTH: usize = 4; -pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; -pub const KICKER_RX_BUF_DEPTH: usize = 4; +// provide embedded panic probe +use panic_probe as _; -make_uart_queues!(RADIO, - RadioUART, RadioRxDMA, RadioTxDMA, - RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, - RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); +static RADIO_C2_CHANNEL: PubSubChannel = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: PubSubChannel = PubSubChannel::new(); -make_uart_queues!(KICKER, - KickerUart, KickerRxDma, KickerTxDma, - KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, - KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; - -static RADIO_TEST: ConstStaticCell> = - ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); -// static RADIO_TEST: RadioTest< -// RADIO_MAX_TX_PACKET_SIZE, -// RADIO_MAX_RX_PACKET_SIZE, -// RADIO_TX_BUF_DEPTH, -// RADIO_RX_BUF_DEPTH, -// RadioUART, -// RadioRxDMA, -// RadioTxDMA, -// RadioReset, -// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); - -// pub sub channel for the gyro vals -// CAP queue size, n_subs, n_pubs -static GYRO_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// pub sub channel for the battery raw adc vals -// CAP queue size, n_subs, n_pubs -static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// static RADIO: Radio = Radio::new(); -static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[interrupt] unsafe fn CEC() { - EXECUTOR_UART_QUEUE.on_interrupt(); + UART_QUEUE_EXECUTOR.on_interrupt(); } -bind_interrupts!(struct Irqs { - USART10 => usart::InterruptHandler; - USART6 => usart::InterruptHandler; - USART1 => usart::InterruptHandler; - UART4 => usart::InterruptHandler; - UART7 => usart::InterruptHandler; - UART8 => usart::InterruptHandler; - UART5 => usart::InterruptHandler; -}); - #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { +async fn main(main_spawner: embassy_executor::Spawner) { + // init system let sys_config = get_system_config(); let p = embassy_stm32::init(sys_config); - let config = usart::Config::default(); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); - let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); - - let mut dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = hz(1_000_000); - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - p.DMA2_CH6, - dotstar_spi_config, - ); - - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - info!("booted"); - - let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, - ).unwrap(); - - let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); - let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); - let kicker_det = Input::new(p.PG8, Pull::Down); - let dip1 = Input::new(p.PG7, Pull::Down); - let dip2 = Input::new(p.PG6, Pull::Down); - let dip3 = Input::new(p.PG5, Pull::Down); - let dip4 = Input::new(p.PG4, Pull::Down); - let dip5 = Input::new(p.PG3, Pull::Down); - let _dip6 = Input::new(p.PG2, Pull::Down); - let dip7 = Input::new(p.PD15, Pull::Down); - - // let robot_id = rotary.read(); + defmt::info!("embassy HAL configured."); //////////////////////// - // Dip Switch Inputs // + // setup task pools // //////////////////////// - let robot_id = (dip1.is_high() as u8) << 3 - | (dip2.is_high() as u8) << 2 - | (dip3.is_high() as u8) << 1 - | (dip4.is_high() as u8); - info!("id: {}", robot_id); - shell_indicator.set(robot_id); - - let wifi_network = WifiNetwork::Team; - // let wifi_network = if dip5.is_high() & dip6.is_high() { - // WifiNetwork::Team - // } else if dip5.is_low() & dip6.is_high() { - // WifiNetwork::CompMain - // } else if dip5.is_high() & dip6.is_low() { - // WifiNetwork::CompPractice - // } else { - // WifiNetwork::Team - // }; - - let control_debug_telemetry_enabled = dip5.is_high(); - if control_debug_telemetry_enabled { - info!("Debug control telemetry transmission enabled"); - } - - let team = if dip7.is_high() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - - ////////////////////// - // Battery Voltage // - ////////////////////// - - let mut adc3 = Adc::new(p.ADC3); - adc3.set_sample_time(SampleTime::CYCLES1_5); - let mut battery_pin = p.PF5; - let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = - [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; - let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); - - let mut imu_spi_config = spi::Config::default(); - imu_spi_config.frequency = mhz(1); - let mut imu_spi = spi::Spi::new( - p.SPI6, - p.PA5, - p.PA7, - p.PA6, - p.BDMA_CH0, - p.BDMA_CH1, - imu_spi_config, - ); - - // acceleromter - let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // gyro - let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - Timer::after(Duration::from_millis(1)).await; - - let gyro_pub = GYRO_CHANNEL.publisher().unwrap(); - unsafe { - SPI6_BUF[0] = 0x80; - // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - imu_cs1.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs1.set_high(); - let accel_id = SPI6_BUF[1]; - info!("accelerometer id: 0x{:x}", accel_id); - SPI6_BUF[0] = 0x80; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs2.set_high(); - let gyro_id = SPI6_BUF[1]; - info!("gyro id: 0x{:x}", gyro_id); - } + // uart queue executor should be highest priority + // NOTE: maybe this should be all DMA tasks? No computation tasks here + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P6); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - Irqs, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ).unwrap(); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - Irqs, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ).unwrap(); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - Irqs, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ).unwrap(); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - Irqs, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ).unwrap(); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - Irqs, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ).unwrap(); + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// - let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); - let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); + // commands channel + let radio_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); + let _control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); - if kicker_det.is_low() { - defmt::warn!("kicker appears unplugged!"); - } + // telemetry channel + let _control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); - let kicker_usart = Uart::new( - p.USART6, - p.PC7, - p.PC6, - Irqs, - p.DMA2_CH4, - p.DMA2_CH5, - get_bootloader_uart_config(), - ).unwrap(); + // TODO imu channel - let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); - spawner - .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) - .unwrap(); + /////////////////// + // start tasks // + /////////////////// - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - gyro_sub, - battery_sub, - ); + let wifi_network = wifi_credentials[0]; + start_radio_task( + uart_queue_spawner, main_spawner, + radio_command_publisher, radio_telemetry_subscriber, + wifi_network, + p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, + p.DMA2_CH1, p.DMA2_CH0, + p.PC13, p.PE4); - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - control.load_firmware().await; - - info!("flashing kicker..."); - - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_RX_UART_QUEUE, - &KICKER_TX_UART_QUEUE, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), - ); - let kicker_firmware_image = KICKER_FW_IMG; - let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; - - if res.is_err() { - defmt::warn!("kicker flashing failed."); - loop {} - } else { - info!("kicker flash complete"); - } - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }] - .iter() - .cloned(), - ); - - // let token = unsafe { - // (&mut *(&RADIO_TEST as *const _ - // as *mut RadioTest< - // RADIO_MAX_TX_PACKET_SIZE, - // RADIO_MAX_RX_PACKET_SIZE, - // RADIO_TX_BUF_DEPTH, - // RADIO_RX_BUF_DEPTH, - // RadioUART, - // RadioRxDMA, - // RadioTxDMA, - // RadioReset, - // >)) - // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - // .await - // }; - let radio_test = RADIO_TEST.take(); - let token = unsafe { - (&mut *(&RADIO_TEST as *const _ - as *mut RadioTest< - RADIO_MAX_TX_PACKET_SIZE, - RADIO_MAX_RX_PACKET_SIZE, - RADIO_TX_BUF_DEPTH, - RADIO_RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, - >)) - .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - .await - }; - // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; - spawner.spawn(token).unwrap(); - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] - .iter() - .cloned(), - ); - - let mut wdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); - unsafe { wdg.unleash() } - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - kicker.set_telemetry_enabled(true); - kicker.send_command(); - - defmt::info!("using SSID: {}", wifi_credentials[0].get_ssid()); - - loop { - unsafe { wdg.pet() }; - - unsafe { - SPI6_BUF[0] = 0x86; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - imu_cs2.set_high(); - let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - // info!("z rate: {}", rate_z); - let gyro_conversion = 2000.0 / 32767.0; - gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion); - } - - // could just feed gyro in here but the comment in control said to use a channel - - /////////////////////// - // Battery reading // - /////////////////////// - - let current_battery_v = - adc_v_to_battery_voltage(adc_raw_to_v(adc3.read(&mut battery_pin) as f32)); - // Shift buffer through - for i in (0..(BATTERY_BUFFER_SIZE - 1)).rev() { - battery_voltage_buffer[i + 1] = battery_voltage_buffer[i]; - } - // Add new battery read - battery_voltage_buffer[0] = current_battery_v; - let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); - let filter_battery_v = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); - battery_pub.publish_immediate(filter_battery_v); - - if filter_battery_v < BATTERY_MIN_VOLTAGE { - let _ = dotstar.write( - [RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 10, g: 0, b: 0 }] - .iter() - .cloned(), - ); - defmt::error!("Error filtered battery voltage too low"); - critical_section::with(|_| loop { - cortex_m::asm::delay(1_000_000); - unsafe { wdg.pet() }; - }); - } - - ///////////////////////// - // Parameter Updates // - ///////////////////////// - - let latest_param_cmd = radio_test.get_latest_params_cmd(); - - if let Some(latest_param_cmd) = latest_param_cmd { - let param_cmd_resp = control.apply_command(&latest_param_cmd); - - // if param_cmd_resp is Err, then the requested parameter update had no submodule acceping the - // field, or the type was invalid, or the update code is unimplemented - // if param_cmd_resp is Ok, then the read/write was successful - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - radio_test.send_parameter_response(resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - radio_test.send_parameter_response(resp).await; - } - } - - //////////////// - // Telemtry // - //////////////// - - let latest_control_cmd = radio_test.get_latest_control(); - - let telemetry = control.tick(latest_control_cmd).await; - if let (Some(mut telemetry), control_debug_telem) = telemetry { - // info!("{:?}", defmt::Debug2Format(&telemetry)); - - telemetry.kicker_charge_level = kicker.hv_rail_voltage(); - telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); - - radio_test.send_telemetry(telemetry).await; - - if control_debug_telemetry_enabled { - radio_test.send_control_debug_telemetry(control_debug_telem).await; - } - } - - kicker.process_telemetry(); - - if let Some(control) = latest_control_cmd { - kicker.set_kick_strength(control.kick_vel); - kicker.request_kick(control.kick_request); - kicker.send_command(); - } - - main_loop_rate_ticker.next().await; - } -} - -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); loop {} -} +} \ No newline at end of file diff --git a/control-board/src/drivers/radio/radio.rs b/control-board/src/drivers/radio/radio.rs index 9588c8fa..45c7a1a0 100644 --- a/control-board/src/drivers/radio/radio.rs +++ b/control-board/src/drivers/radio/radio.rs @@ -80,6 +80,10 @@ impl< } } + pub async fn update_uart_config(&self, config: usart::Config) { + self.writer.update_uart_config(config).await; + } + pub async fn wait_startup(&self) -> Result<(), ()> { self.reader .dequeue(|buf| { diff --git a/control-board/src/drivers/radio/radio_robot.rs b/control-board/src/drivers/radio/radio_robot.rs index fa017263..30953535 100644 --- a/control-board/src/drivers/radio/radio_robot.rs +++ b/control-board/src/drivers/radio/radio_robot.rs @@ -5,6 +5,7 @@ use ateam_common_packets::bindings_radio::{ }; use ateam_common_packets::radio::DataPacket; use const_format::formatcp; +use credentials::WifiCredential; use core::fmt::Write; use core::mem::size_of; use embassy_futures::select::{select, Either}; @@ -99,63 +100,60 @@ impl< reset_pin: impl Pin, ) -> Result, ()> { - let mut reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); + let reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); + let radio = Radio::new(read_queue, write_queue); + + Ok(Self { + radio, + reset_pin, + peer: None, + }) + } + + pub async fn connect_uart(&mut self) -> Result<(), ()> { + self.reset_pin.set_high(); Timer::after(Duration::from_micros(100)).await; - reset_pin.set_low(); + self.reset_pin.set_low(); - let mut radio = Radio::new(read_queue, write_queue); - radio.wait_startup().await?; + self.radio.wait_startup().await?; let baudrate = 5_250_000; - radio.set_echo(false).await?; - radio.config_uart(baudrate, false, 8, true).await?; + self.radio.set_echo(false).await?; + self.radio.config_uart(baudrate, false, 8, true).await?; let mut radio_uart_config = usart::Config::default(); - radio_uart_config.baudrate = 2_000_000; + radio_uart_config.baudrate = 5_250_000; radio_uart_config.parity = usart::Parity::ParityEven; - write_queue.update_uart_config(radio_uart_config).await; + self.radio.update_uart_config(radio_uart_config).await; // Datasheet says wait at least 40ms after UART config change Timer::after(Duration::from_millis(50)).await; // Datasheet says wait at least 50ms after entering data mode - radio.enter_edm().await?; - radio.wait_edm_startup().await?; + self.radio.enter_edm().await?; + self.radio.wait_edm_startup().await?; Timer::after(Duration::from_millis(50)).await; - Ok(Self { - radio, - reset_pin, - peer: None, - }) + Ok(()) } - pub async fn connect_to_network(&mut self, wifi_network: WifiNetwork) -> Result<(), ()> { + pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential) -> Result<(), ()> { + // set radio hardware name enumeration let mut s = String::<17>::new(); core::write!(&mut s, "A-Team Robot {:04X}", get_uuid()).unwrap(); self.radio.set_host_name(s.as_str()).await?; - let wifi_ssid = match wifi_network { - WifiNetwork::Team => TEAM_WIFI_SSID, - WifiNetwork::CompMain => COMP_MAIN_WIFI_SSID, - WifiNetwork::CompPractice => COMP_PRACTICE_WIFI_SSID - }; - let wifi_pass = match wifi_network { - WifiNetwork::Team => TEAM_WIFI_PASS, - WifiNetwork::CompMain => COMP_MAIN_WIFI_PASS, - WifiNetwork::CompPractice => COMP_PRACTICE_WIFI_PASS + // load the wifi network configuration into config slot 1 + let wifi_ssid = wifi_credential.get_ssid(); + let wifi_pass = WifiAuth::WPA { + passphrase: wifi_credential.get_password(), }; + self.radio.config_wifi(1, wifi_ssid,wifi_pass).await?; - self.radio - .config_wifi( - 1, - wifi_ssid, - WifiAuth::WPA { - passphrase: wifi_pass, - }, - ) - .await?; + // connect to config slot 1 self.radio.connect_wifi(1).await?; + + // if we made it this far, we're connected Ok(()) } diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 0d4958d9..7f46530f 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -14,7 +14,7 @@ #![feature(sync_unsafe_cell)] use embassy_stm32::{ - rcc::{ + bind_interrupts, peripherals, rcc::{ mux::{ Adcsel, Saisel, Sdmmcsel, Spi6sel, Usart16910sel, Usart234578sel, Usbsel }, @@ -24,9 +24,7 @@ use embassy_stm32::{ Pll, PllDiv, PllMul, PllPreDiv, PllSource, Sysclk, VoltageScale - }, - time::Hertz, - Config + }, time::Hertz, usart, Config }; // pub mod fw_images; @@ -39,6 +37,16 @@ pub mod parameter_interface; pub mod drivers; +bind_interrupts!(struct SystemIrqs { + USART10 => usart::InterruptHandler; + USART6 => usart::InterruptHandler; + USART1 => usart::InterruptHandler; + UART4 => usart::InterruptHandler; + UART7 => usart::InterruptHandler; + UART8 => usart::InterruptHandler; + UART5 => usart::InterruptHandler; +}); + pub mod colors { use smart_leds::RGB8; @@ -98,7 +106,7 @@ pub fn get_system_config() -> Config { // configure the PLLs // validated in ST Cube MX config.rcc.pll1 = Some(Pll { - source: PllSource::HSI, + source: PllSource::HSE, prediv: PllPreDiv::DIV1, mul: PllMul::MUL68, divp: Some(PllDiv::DIV1), // 544 MHz @@ -106,7 +114,7 @@ pub fn get_system_config() -> Config { divr: Some(PllDiv::DIV2) // 272 MHz }); config.rcc.pll2 = Some(Pll { - source: PllSource::HSI, + source: PllSource::HSE, prediv: PllPreDiv::DIV1, mul: PllMul::MUL31, divp: Some(PllDiv::DIV5), // 49.6 MHz @@ -114,7 +122,7 @@ pub fn get_system_config() -> Config { divr: Some(PllDiv::DIV1) // 248 MHz }); config.rcc.pll3 = Some(Pll { - source: PllSource::HSI, + source: PllSource::HSE, prediv: PllPreDiv::DIV2, mul: PllMul::MUL93, divp: Some(PllDiv::DIV2), // 186 Mhz diff --git a/control-board/src/motion/tasks/control.rs b/control-board/src/motion/tasks/control.rs index c808f51d..e4232218 100644 --- a/control-board/src/motion/tasks/control.rs +++ b/control-board/src/motion/tasks/control.rs @@ -19,10 +19,7 @@ use crate::{ use nalgebra::{Vector3, Vector4}; -use ateam_lib_stm32::{ - make_uart_queues, - queue::Buffer -}; +use ateam_lib_stm32::make_uart_queues; use ateam_common_packets::bindings_radio::{ BasicControl, diff --git a/control-board/src/motion/tasks/mod.rs b/control-board/src/motion/tasks/mod.rs index 9cc39d68..a537ba19 100644 --- a/control-board/src/motion/tasks/mod.rs +++ b/control-board/src/motion/tasks/mod.rs @@ -1 +1,2 @@ -pub mod control; \ No newline at end of file +pub mod control; +pub mod radio_task; \ No newline at end of file diff --git a/control-board/src/motion/tasks/radio_task.rs b/control-board/src/motion/tasks/radio_task.rs new file mode 100644 index 00000000..d482fb44 --- /dev/null +++ b/control-board/src/motion/tasks/radio_task.rs @@ -0,0 +1,137 @@ + +use ateam_common_packets::radio::TelemetryPacket; +use ateam_lib_stm32::make_uart_queues; +use credentials::WifiCredential; +use embassy_executor::{SendSpawner, Spawner}; +use embassy_futures::select::{select, Either}; +use embassy_stm32::{ + gpio::{Input, Pull}, + usart::{self, Uart} +}; +use embassy_sync::pubsub::WaitResult; +use embassy_time::Timer; + +use crate::{drivers::radio::{RobotRadio, WifiNetwork}, pins::*, SystemIrqs}; + +pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; +pub const RADIO_TX_BUF_DEPTH: usize = 4; +pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; +pub const RADIO_RX_BUF_DEPTH: usize = 4; + +make_uart_queues!(RADIO, + RadioUART, RadioRxDMA, RadioTxDMA, + RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, + RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + + +#[embassy_executor::task] +pub async fn radio_task_entry( + command_publisher: CommandsPublisher, + mut telemetry_subscriber: TelemetrySubcriber, + wifi_network: WifiCredential, + radio_reset_pin: RadioResetPin, + radio_ndet_pin: RadioNDetectPin) { + + defmt::info!("radio task startup"); + + let radio_ndet = Input::new(radio_ndet_pin, Pull::None); + + let mut radio: RobotRadio<'static, RadioUART, RadioRxDMA, RadioTxDMA, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH> = + RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin).await.unwrap(); + + #[allow(unused_labels)] + 'connect_loop: loop { + if radio_ndet.is_high() { + defmt::error!("radio appears unplugged."); + Timer::after_millis(1000).await; + continue 'connect_loop; + } + + let res = radio.connect_uart().await; + if res.is_err() { + defmt::error!("failed to establish radio UART connection."); + Timer::after_millis(1000).await; + continue 'connect_loop; + } + defmt::info!("established radio uart coms"); + + while radio.connect_to_network(wifi_network).await.is_err() { + defmt::error!("failed to connect to wifi network."); + Timer::after_millis(1000).await; + } + defmt::info!("radio connected"); + + let res = radio.open_multicast().await; + if res.is_err() { + defmt::error!("failed to establish multicast socket to network."); + continue 'connect_loop; + } + defmt::info!("multicast open"); + + // TODO add inbound timeout somewhere, maybe not here. + 'process_packets: loop { + match select(radio.read_packet(), telemetry_subscriber.next_message()).await { + Either::First(res) => { + if let Ok(c2_pkt) = res { + command_publisher.publish_immediate(c2_pkt); + } else { + defmt::warn!("radio read packet returned an error"); + } + } + Either::Second(telem_msg) => { + match telem_msg { + WaitResult::Lagged(num_missed_pkts) => { + defmt::warn!("radio task missed sending {} outbound packets. Should channel have higher capacity?", num_missed_pkts); + }, + WaitResult::Message(msg) => { + match msg { + TelemetryPacket::Basic(basic_telem_pkt) => { + let res = radio.send_telemetry(basic_telem_pkt).await; + if res.is_err() { + defmt::warn!("radio task failed to send basic telemetry packet. Is the backing queue too small?"); + } + } + TelemetryPacket::Control(control_telem_pkt) => { + let res = radio.send_control_debug_telemetry(control_telem_pkt).await; + if res.is_err() { + defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); + } + } + } + } + } + } + } + + if radio_ndet.is_high() { + defmt::error!("radio was unplugged."); + break 'process_packets; + } + } + } +} + +pub fn start_radio_task(radio_task_spawner: SendSpawner, + queue_spawner: Spawner, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + wifi_network: WifiCredential, + radio_uart: RadioUART, + radio_uart_rx_pin: RadioUartRxPin, + radio_uart_tx_pin: RadioUartTxPin, + _radio_uart_cts_pin: RadioUartCtsPin, + _radio_uart_rts_pin: RadioUartRtsPin, + radio_uart_rx_dma: RadioRxDMA, + radio_uart_tx_dma: RadioTxDMA, + radio_reset_pin: RadioResetPin, + radio_ndet_pin: RadioNDetectPin) { + let radio_uart_config = usart::Config::default(); + let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); + let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); + + queue_spawner.spawn(RADIO_RX_UART_QUEUE.spawn_task(radio_uart_rx)).unwrap(); + queue_spawner.spawn(RADIO_TX_UART_QUEUE.spawn_task(radio_uart_tx)).unwrap(); + + radio_task_spawner.spawn(radio_task_entry(command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); +} \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 7ef71f9d..09dc892c 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -1,15 +1,33 @@ #![allow(dead_code)] +use ateam_common_packets::radio::{DataPacket, TelemetryPacket}; use embassy_stm32::peripherals::*; +use embassy_sync::{ + blocking_mutex::raw::ThreadModeRawMutex, + pubsub::{Publisher, Subscriber}}; + +///////////////////// +// Pub Sub Types // +///////////////////// + +const COMMANDS_PUBSUB_DEPTH: usize = 1; +const TELEMETRY_PUBSUB_DEPTH: usize = 3; +pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type TelemetrySubcriber = Subscriber<'static, ThreadModeRawMutex, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; ///////////// // Radio // ///////////// pub type RadioUART = USART10; +pub type RadioUartRxPin = PE2; +pub type RadioUartTxPin = PE3; +pub type RadioUartCtsPin = PG13; +pub type RadioUartRtsPin = PG14; pub type RadioTxDMA = DMA2_CH0; pub type RadioRxDMA = DMA2_CH1; -pub type RadioReset = PC13; +pub type RadioResetPin = PC13; +pub type RadioNDetectPin = PE4; ////////////// diff --git a/control-board/src/radio.rs b/control-board/src/radio.rs index b997e348..f8201ba9 100644 --- a/control-board/src/radio.rs +++ b/control-board/src/radio.rs @@ -120,7 +120,8 @@ where .unwrap(); info!("radio created"); - radio.connect_to_network(wifi_network).await.unwrap(); + defmt::panic!("radio temporarily left unconnected"); + // radio.connect_to_network(wifi_network).await.unwrap(); info!("radio connected"); radio.open_multicast().await.unwrap(); diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index eb06c204..096841f7 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -16,7 +16,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{make_uart_queues, queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queues, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 9d2eed3b..67260730 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -30,15 +30,14 @@ use crate::queue::{ #[macro_export] macro_rules! make_uart_queues { ($name:ident, $uart:ty, $uart_rx_dma:ty, $uart_tx_dma:ty, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $(#[$m:meta])*) => { - use $crate::paste; - paste::paste! { + $crate::paste::paste! { // shared mutex allowing safe runtime update to UART config const [<$name _UART_PERI_MUTEX>]: embassy_sync::mutex::Mutex = embassy_sync::mutex::Mutex::new(false); // tx buffer const [<$name _CONST_TX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>> = - core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + core::cell::SyncUnsafeCell::new($crate::queue::Buffer::EMPTY); $(#[$m])* static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = [[<$name _CONST_TX_BUF_VAL>]; $tx_buffer_depth]; @@ -47,7 +46,7 @@ macro_rules! make_uart_queues { // rx buffer const [<$name _CONST_RX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>> = - core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + core::cell::SyncUnsafeCell::new($crate::queue::Buffer::EMPTY); $(#[$m])* static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = [[<$name _CONST_RX_BUF_VAL>]; $rx_buffer_depth]; From ad03c5be137224bb964300d66c5a17d8dcbc729b Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 11 May 2024 23:36:58 -0400 Subject: [PATCH 026/157] control firmware work --- control-board/src/lib.rs | 19 +- control-board/src/motion/mod.rs | 1 - control-board/src/pins.rs | 76 ++- .../src/{motion => }/tasks/control.rs | 0 control-board/src/tasks/control_task.rs | 0 control-board/src/tasks/imu_task.rs | 121 ++++ control-board/src/tasks/kicker_task.rs | 0 control-board/src/{motion => }/tasks/mod.rs | 1 + .../src/{motion => }/tasks/radio_task.rs | 4 +- lib-stm32/src/drivers/imu/bmi085.rs | 588 ++++++++++++++++++ lib-stm32/src/drivers/imu/mod.rs | 2 + lib-stm32/src/drivers/mod.rs | 3 +- 12 files changed, 794 insertions(+), 21 deletions(-) rename control-board/src/{motion => }/tasks/control.rs (100%) create mode 100644 control-board/src/tasks/control_task.rs create mode 100644 control-board/src/tasks/imu_task.rs create mode 100644 control-board/src/tasks/kicker_task.rs rename control-board/src/{motion => }/tasks/mod.rs (66%) rename control-board/src/{motion => }/tasks/radio_task.rs (98%) create mode 100644 lib-stm32/src/drivers/imu/bmi085.rs create mode 100644 lib-stm32/src/drivers/imu/mod.rs diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 7f46530f..e7defe63 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -15,11 +15,8 @@ use embassy_stm32::{ bind_interrupts, peripherals, rcc::{ - mux::{ - Adcsel, Saisel, Sdmmcsel, Spi6sel, Usart16910sel, Usart234578sel, Usbsel - }, - AHBPrescaler, - APBPrescaler, + mux::{Adcsel, Saisel, Sdmmcsel, Spi6sel, Usart16910sel, Usart234578sel, Usbsel}, + AHBPrescaler, APBPrescaler, Hse, HseMode, Pll, PllDiv, PllMul, PllPreDiv, PllSource, Sysclk, @@ -27,15 +24,15 @@ use embassy_stm32::{ }, time::Hertz, usart, Config }; -// pub mod fw_images; -pub mod motion; +pub mod parameter_interface; pub mod pins; pub mod radio; pub mod stm32_interface; pub mod stspin_motor; -pub mod parameter_interface; pub mod drivers; +pub mod motion; +pub mod tasks; bind_interrupts!(struct SystemIrqs { USART10 => usart::InterruptHandler; @@ -130,16 +127,20 @@ pub fn get_system_config() -> Config { divr: Some(PllDiv::DIV3) // 124 MHz }); - // configure + // configure core busses config.rcc.sys = Sysclk::PLL1_P; // 544 MHz config.rcc.d1c_pre = AHBPrescaler::DIV1; // 544 MHz config.rcc.ahb_pre = AHBPrescaler::DIV2; // 272 MHz + // configure peripheral busses config.rcc.apb1_pre = APBPrescaler::DIV2; // 136 MHz config.rcc.apb2_pre = APBPrescaler::DIV2; // 136 MHz config.rcc.apb3_pre = APBPrescaler::DIV2; // 136 MHz config.rcc.apb4_pre = APBPrescaler::DIV2; // 136 MHz + // configure peripheral subgroup clock selection muxes + // this is non exhaustive, if other things are turned on + // add an entry config.rcc.mux.spi123sel = Saisel::PLL1_Q; // 136 MHz config.rcc.mux.usart234578sel = Usart234578sel::PCLK1; // 136 MHz config.rcc.mux.usart16910sel = Usart16910sel::PCLK2; // 136 MHz diff --git a/control-board/src/motion/mod.rs b/control-board/src/motion/mod.rs index 5c00adf5..8a59a057 100644 --- a/control-board/src/motion/mod.rs +++ b/control-board/src/motion/mod.rs @@ -2,6 +2,5 @@ pub mod constant_gain_kalman_filter; pub mod pid; pub mod robot_controller; pub mod robot_model; -pub mod tasks; pub mod params; \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 09dc892c..60c820de 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -15,6 +15,7 @@ const TELEMETRY_PUBSUB_DEPTH: usize = 3; pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; pub type TelemetrySubcriber = Subscriber<'static, ThreadModeRawMutex, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; + ///////////// // Radio // ///////////// @@ -30,6 +31,27 @@ pub type RadioResetPin = PC13; pub type RadioNDetectPin = PE4; +/////////////// +// Sensors // +/////////////// + +pub type ImuSpi = SPI1; +pub type ImuSpiSckPin = PA5; +pub type ImuSpiMosiPin = PA7; +pub type ImuSpiMisoPin = PA6; +pub type ImuSpiNss0Pin = PA4; +pub type ExtImuSpiNss1Pin = PC4; +pub type ExtImuSpiNss2Pin = PC5; +pub type ExtImuSpiNss3Pin = PB0; +pub type ImuSpiRxDma = DMA2_CH6; +pub type ImuSpiTxDma = DMA2_CH7; +pub type ImuSpiInt1Pin = PB1; +pub type ImuSpiInt2Pin = PB2; +pub type ImuSpiInt1Exti = EXTI1; +pub type ImuSpiInt2Exti = EXTI2; +pub type ExtImuNDetPin = PF11; + + ////////////// // Kicker // ////////////// @@ -37,6 +59,7 @@ pub type RadioNDetectPin = PE4; pub type KickerUart = USART6; pub type KickerRxDma = DMA2_CH5; pub type KickerTxDma = DMA2_CH4; +pub type KickerPowerOnPin = PG8; ////////////// @@ -74,18 +97,55 @@ pub type MotorDBootPin = PD13; pub type MotorDResetPin = PD12; -////////////////////////// -// Power Control Pins // -////////////////////////// +///////////////////// +// Power Control // +///////////////////// -pub type PowerStatePin = PF5; -pub type PowerStateExti = EXTI5; -pub type ShutdownCompletePin = PF4; +pub type PowerBtnPressedIntPin = PE10; +pub type PowerBtnPressedIntExti = EXTI11; +pub type PowerKillPin = PE11; +pub type ShutdownInitiatedLedPin = PF4; /////////////// // User IO // /////////////// -pub type DotstarSpi = SPI3; -pub type DotstarTxDma = DMA2_CH6; \ No newline at end of file +pub type UsrBtn0Pin = PD6; +pub type UsrBtn1Pin = PD5; +pub type UsrBtn0Exti = EXTI6; +pub type UsrBtn1Exti = EXTI5; + +pub type UsrDip1Pin = PD15; +pub type UsrDip2Pin = PG2; +pub type UsrDip3Pin = PG3; +pub type UsrDip4Pin = PG4; +pub type UsrDip5Pin = PG5; +pub type UsrDip6Pin = PG6; +pub type UsrDip7IsBluePin = PG7; + +pub type RobotIdSelector0Pin = PG9; +pub type RobotIdSelector1Pin = PG10; +pub type RobotIdSelector2Pin = PG11; +pub type RobotIdSelector3Pin = PG12; + + +pub type UsrLed0Pin = PF3; +pub type UsrLed1Pin = PF2; +pub type UsrLed2Pin = PF1; +pub type UsrLed3Pin = PF0; + +pub type RobotIdIndicator0FrPin = PD0; +pub type RobotIdIndicator1FfPin = PD1; +pub type RobotIdIndicator2BlPin = PD3; +pub type RobotIdIndicator3BrPin = PD4; +pub type RobotIdIndicator4TeamIsBluePin = PD14; + +pub type DotstarSpi = SPI6; +pub type DotstarSpiSck = PB3; +pub type DotstarSpiMosi = PB5; +pub type DotstarTxDma = BDMA_CH0; + +pub type BuzzerPin = PE6; +pub type BuzzerTimer = TIM15; // ch2 + diff --git a/control-board/src/motion/tasks/control.rs b/control-board/src/tasks/control.rs similarity index 100% rename from control-board/src/motion/tasks/control.rs rename to control-board/src/tasks/control.rs diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs new file mode 100644 index 00000000..e69de29b diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs new file mode 100644 index 00000000..25ed4845 --- /dev/null +++ b/control-board/src/tasks/imu_task.rs @@ -0,0 +1,121 @@ +use embassy_executor::{SpawnError, Spawner}; +use embassy_stm32::Peripheral; +use embassy_stm32::exti::ExtiInput; +use embassy_stm32::gpio::{Input, Pull}; +use embassy_stm32::spi::{SckPin, MisoPin, MosiPin}; +use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +use embassy_sync::pubsub::{PubSubChannel, Subscriber, Error}; + +use nalgebra::Vector3; + +use static_cell::ConstStaticCell; + +use ateam_lib_stm32::drivers::imu::bmi085::{Bmi085, GyroRange, GyroBandwidth, GyroIntMap, GyroIntPinActiveState, GyroIntPinMode, AccelRange, AccelConfOdr, AccelConfBwp}; +use crate::drivers::imu::{GyroFrame, AccelFrame}; + +use crate::pins::{ImuSpi, ImuTxDma, ImuRxDma, ImuAccelCsPin, ImuGyroCsPin, ImuAccelIntPin, ImuGyroIntPin}; + +type AccelDataChannel = PubSubChannel; +type GyroDataChannel = PubSubChannel; + +pub type AccelSub<'a> = Subscriber<'a, ThreadModeRawMutex, AccelFrame, 1, 1, 1>; +pub type GyroSub<'a> = Subscriber<'a, ThreadModeRawMutex, GyroFrame, 1, 1, 1>; + +#[link_section = ".axisram.buffers"] +static IMU_BUFFER_CELL: ConstStaticCell<[u8; 8]> = ConstStaticCell::new([0; 8]); + +static ACCEL_DATA_CHANNEL: AccelDataChannel = PubSubChannel::new(); +static GYRO_DATA_CHANNEL: GyroDataChannel = PubSubChannel::new(); + +#[embassy_executor::task] +async fn imu_task_entry(mut imu: Bmi085<'static, 'static, ImuSpi>, mut _accel_int: ExtiInput<'static, ImuAccelIntPin>, mut gyro_int: ExtiInput<'static, ImuGyroIntPin>) { + let accel_pub = ACCEL_DATA_CHANNEL.publisher().expect("accel data channel had no publisher left"); + let gyro_pub = GYRO_DATA_CHANNEL.publisher().expect("gyro data channel had no publisher left"); + + imu.init().await; + let self_test_res = imu.self_test().await; + if self_test_res.is_err() { + defmt::error!("IMU self test failed"); + } + + // set measurement ranges + imu.accel_set_range(AccelRange::Range2g).await; // range +- 19.6 m/s^2 + imu.accel_set_bandwidth(AccelConfBwp::OverSampling2Fold, AccelConfOdr::OutputDataRate400p0).await; // bandwidth 80Hz + + // set interrupt config + imu.gyro_set_int_config(GyroIntPinActiveState::ActiveLow, + GyroIntPinMode::OpenDrain, + GyroIntPinActiveState::ActiveLow, + GyroIntPinMode::OpenDrain).await; + // enable BMI085 Gyro INT3 which is electrically wired to IMU breakout INT2 named gyro_int_pin + imu.gyro_set_int_map(GyroIntMap::Int3).await; + + imu.gyro_set_range(GyroRange::PlusMinus2000DegPerSec).await; + imu.gyro_set_bandwidth(GyroBandwidth::FilterBw64Hz).await; + + // enable interrupts + imu.gyro_enable_interrupts().await; + + // BMI085 internally shares a SPI bus so we can't actually asynchronously read both the IMU and Accel + // unless we used a mutex but that's still a lot of overhead. We'll just sync on the Gyro INT since + // that gyro data is far more critical for control. When the gyro has an update, we'll also grab the + // accel update/ + + loop { + // block on gyro interrupt, active low + gyro_int.wait_for_falling_edge().await; + + // read gyro data + let imu_data = imu.gyro_get_data_rads().await; + gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2])); + + // read accel data + // TODO: don't use raw data, impl conversion + let accel_data = imu.accel_get_raw_data().await; + accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); + } +} + +pub fn start_imu_task(spawner: &Spawner, + peri: impl Peripheral

+ 'static, + sck: impl Peripheral

> + 'static, + mosi: impl Peripheral

> + 'static, + miso: impl Peripheral

> + 'static, + txdma: impl Peripheral

+ 'static, + rxdma: impl Peripheral

+ 'static, + accel_cs: impl Peripheral

+ 'static, + gyro_cs: impl Peripheral

+ 'static, + accel_int_pin: impl Peripheral

+ 'static, + gyro_int_pin: impl Peripheral

+ 'static, + accel_int: impl Peripheral

::ExtiChannel> + 'static, + gyro_int: impl Peripheral

::ExtiChannel> + 'static, + ) -> Result<(), SpawnError> { + + defmt::warn!("here3!"); + + let imu_buf = IMU_BUFFER_CELL.get(); + + defmt::warn!("here4!"); + + let imu = Bmi085::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, accel_cs, gyro_cs, imu_buf); + + let accel_int_input = Input::new(accel_int_pin, Pull::Down); + let accel_int = ExtiInput::new(accel_int_input, accel_int); + + // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and + // imu open drain + let gyro_int_input = Input::new(gyro_int_pin, Pull::Up); + let gyro_int = ExtiInput::new(gyro_int_input, gyro_int); + + defmt::warn!("here!"); + + spawner.spawn(imu_task_entry(imu, accel_int, gyro_int)) +} + +pub fn get_accel_sub() -> Result, Error> { + return ACCEL_DATA_CHANNEL.subscriber(); +} + +pub fn get_gyro_sub() -> Result, Error> { + return GYRO_DATA_CHANNEL.subscriber(); +} diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs new file mode 100644 index 00000000..e69de29b diff --git a/control-board/src/motion/tasks/mod.rs b/control-board/src/tasks/mod.rs similarity index 66% rename from control-board/src/motion/tasks/mod.rs rename to control-board/src/tasks/mod.rs index a537ba19..9d069a8a 100644 --- a/control-board/src/motion/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,2 +1,3 @@ pub mod control; +pub mod imu_task; pub mod radio_task; \ No newline at end of file diff --git a/control-board/src/motion/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs similarity index 98% rename from control-board/src/motion/tasks/radio_task.rs rename to control-board/src/tasks/radio_task.rs index d482fb44..854363f1 100644 --- a/control-board/src/motion/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -11,7 +11,7 @@ use embassy_stm32::{ use embassy_sync::pubsub::WaitResult; use embassy_time::Timer; -use crate::{drivers::radio::{RobotRadio, WifiNetwork}, pins::*, SystemIrqs}; +use crate::{drivers::radio::RobotRadio, pins::*, SystemIrqs}; pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -26,7 +26,7 @@ make_uart_queues!(RADIO, #[embassy_executor::task] -pub async fn radio_task_entry( +async fn radio_task_entry( command_publisher: CommandsPublisher, mut telemetry_subscriber: TelemetrySubcriber, wifi_network: WifiCredential, diff --git a/lib-stm32/src/drivers/imu/bmi085.rs b/lib-stm32/src/drivers/imu/bmi085.rs new file mode 100644 index 00000000..ea6bbc12 --- /dev/null +++ b/lib-stm32/src/drivers/imu/bmi085.rs @@ -0,0 +1,588 @@ +/* + * Driver for the Bosch BMI085 IMU. + * + * https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi085-ds001.pdf + * + */ + +use core::{cmp::min, f32::consts::PI}; + +use embassy_stm32::{ + gpio::{Level, Output, Pin, Speed}, + mode::Async, + spi::{self, MisoPin, MosiPin, SckPin}, + time::hz, + Peripheral +}; +use embassy_time::{Timer, Duration}; + +use defmt::*; + + +pub const SPI_MIN_BUF_LEN: usize = 8; + +/// SPI driver for the Bosch BMI085 IMU: Accel + Gyro +pub struct Bmi085<'a, 'buf, SpiPeri: spi::Instance> { + spi: spi::Spi<'a, SpiPeri, Async>, + accel_cs: Output<'a>, + gyro_cs: Output<'a>, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + accel_range: AccelRange, + gyro_range: GyroRange, +} + +#[repr(u8)] +#[allow(non_camel_case_types, dead_code)] +#[derive(Clone, Copy, Debug)] +enum AccelRegisters { + ACC_CHIP_ID = 0x00, + ACC_ERR_REG = 0x02, + ACC_STATUS = 0x03, + ACC_X_LSB = 0x12, + ACC_X_MSB = 0x13, + ACC_Y_LSB = 0x14, + ACC_Y_MSB = 0x15, + ACC_Z_LSB = 0x16, + ACC_Z_MSB = 0x17, + SENSORTIME_0 = 0x18, + SENSORTIME_1 = 0x19, + SENSORTIME_2 = 0x1A, + ACC_INT_STAT_1 = 0x1D, + TEMP_MSB = 0x22, + TEMP_LSB = 0x23, + ACC_CONF = 0x40, + ACC_RANGE = 0x41, + INT1_IO_CTRL = 0x53, + INT2_IO_CTRL = 0x54, + INT_MAP_DATA = 0x58, + ACC_SELF_TEST = 0x6D, + ACC_PWR_CONF = 0x7C, + ACC_PWR_CTRL = 0x7D, + ACC_SOFTRESET = 0x7E, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum AccelRange { + Range2g = 0x00, + Range4g = 0x01, + Range8g = 0x02, + Range16g = 0x03, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum AccelConfBwp { + OverSampling4Fold = 0x08, + OverSampling2Fold = 0x09, + OverSamplingNormal = 0x0A, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum AccelConfOdr { + OutputDataRate12p5 = 0x05, + OutputDataRate25p0 = 0x06, + OutputDataRate50p0 = 0x07, + OutputDataRate100p0 = 0x08, + OutputDataRate200p0 = 0x09, + OutputDataRate400p0 = 0x0A, + OutputDataRate800p0 = 0x0B, + OutputDataRate1600p0 = 0x0C, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +enum AccelSelfTestReg { + Off = 0x00, + PositiveSelfTest = 0x0D, + NegativeSelfTest = 0x09, +} + +const ACCEL_CHIP_ID: u8 = 0x1F; + +#[repr(u8)] +#[allow(non_camel_case_types, dead_code)] +#[derive(Clone, Copy, Debug)] +enum GyroRegisters { + GYRO_CHIP_ID = 0x00, + RATE_X_LSB = 0x02, + RATE_X_MSB = 0x03, + RATE_Y_LSB = 0x04, + RATE_Y_MSB = 0x05, + RATE_Z_LSB = 0x06, + RATE_Z_MSB = 0x07, + GYRO_INT_STAT_1 = 0x0A, + GYRO_RANGE = 0x0F, + GYRO_BANDWIDTH = 0x10, + GYRO_LPM1 = 0x11, + GYRO_SOFTRESET = 0x14, + GYRO_INT_CONTROL = 0x15, + INT3_INT4_IO_CONF = 0x16, + INT3_INT4_IO_MAP = 0x18, + GYRO_SELF_TEST = 0x3C, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +enum GyroSelfTestReg { + RateOk = 0b10000, + BistFail = 0b00100, + BistRdy = 0b00010, + TrigBist = 0b00001, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroRange { + PlusMinus2000DegPerSec = 0x00, + PlusMinus1000DegPerSec = 0x01, + PlusMinus500DegPerSec = 0x02, + PlusMinus250DegPerSec = 0x03, + PlusMinus125DegPerSec = 0x04, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroBandwidth { + FilterBw532Hz = 0x00, + FilterBw230Hz = 0x01, + FilterBw116Hz = 0x02, + FilterBw47Hz = 0x03, + FilterBw23Hz = 0x04, + FilterBw12Hz = 0x05, + FilterBw64Hz = 0x06, + FilterBw32Hz = 0x07, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +enum GyroIntCtrl { + InterruptOff = 0x00, + InterruptOnNewData = 0x80, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroIntPinMode { + PushPull = 0b0, + OpenDrain = 0b1, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroIntPinActiveState { + ActiveLow = 0b0, + ActiveHigh = 0b1, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroIntMap { + NotMapped = 0x00, + Int3 = 0x01, + Int4 = 0x80, + Int3AndInt4 = 0x81, +} + +const GYRO_CHIP_ID: u8 = 0x0F; + +const READ_BIT: u8 = 0x80; + +impl<'a, 'buf, SpiPeri: spi::Instance> + Bmi085<'a, 'buf, SpiPeri> { + /// creates a new BMI085 instance from a pre-existing Spi peripheral + pub fn new_from_spi( + spi: spi::Spi<'a, SpiPeri, Async>, + accel_cs: Output<'a>, + gyro_cs: Output<'a>, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + ) -> Self { + Bmi085 { + spi: spi, + accel_cs: accel_cs, + gyro_cs: gyro_cs, + spi_buf: spi_buf, + accel_range: AccelRange::Range4g, + gyro_range: GyroRange::PlusMinus2000DegPerSec, + } + } + + ///t creates a new BMI085 instance from uninitialized pins + pub fn new_from_pins( + peri: impl Peripheral

+ 'a, + sck: impl Peripheral

> + 'a, + mosi: impl Peripheral

> + 'a, + miso: impl Peripheral

> + 'a, + txdma: impl Peripheral

> + 'a, + rxdma: impl Peripheral

> + 'a, + accel_cs: impl Pin, + gyro_cs: impl Pin, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + ) -> Self { + let mut spi_config = spi::Config::default(); + spi_config.frequency = hz(1_000_000); + + let imu_spi = spi::Spi::new( + peri, + sck, + mosi, + miso, + txdma, + rxdma, + spi_config + ); + + let accel_cs = Output::new(accel_cs, Level::High, Speed::VeryHigh); + let imu_cs = Output::new(gyro_cs, Level::High, Speed::VeryHigh); + + Bmi085 { + spi: imu_spi, + accel_cs: accel_cs, + gyro_cs: imu_cs, + spi_buf: spi_buf, + accel_range: AccelRange::Range4g, + gyro_range: GyroRange::PlusMinus2000DegPerSec, + } + } + + fn select_accel(&mut self) { + self.gyro_cs.set_high(); + self.accel_cs.set_low(); + } + + fn select_gyro(&mut self) { + self.accel_cs.set_high(); + self.gyro_cs.set_low(); + } + + fn deselect(&mut self) { + self.accel_cs.set_high(); + self.gyro_cs.set_high(); + } + + async fn read(&mut self, reg: u8) -> u8 { + self.spi_buf[0] = reg | READ_BIT; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..2]).await; + self.spi_buf[1] + } + + async fn accel_burst_read(&mut self, reg: AccelRegisters, dest: &mut [u8]) { + // the transaction length is either the dest buf size + 2 + // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) + // OR upper bounded by internal length of the buffer. + let trx_len = min(dest.len() + 2, self.spi_buf.len()); + + self.spi_buf[0] = reg as u8 | READ_BIT; + self.spi_buf[1] = 0x00; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + dest.copy_from_slice(&self.spi_buf[2..trx_len]); + } + + async fn gyro_burst_read(&mut self, reg: GyroRegisters, dest: &mut [u8]) { + // the transaction length is either the dest buf size + 1 + // (the start addr + N data bytes) + // OR upper bounded by internal length of the buffer. + let trx_len = min(dest.len() + 1, self.spi_buf.len()); + + self.spi_buf[0] = reg as u8 | READ_BIT; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + dest.copy_from_slice(&self.spi_buf[1..trx_len]); + } + + async fn accel_read(&mut self, reg: AccelRegisters) -> u8 { + self.select_accel(); + self.read(reg as u8).await + } + + async fn gyro_read(&mut self, reg: GyroRegisters) -> u8 { + self.select_gyro(); + self.read(reg as u8).await + } + + async fn write(&mut self, reg: u8, val: u8) { + self.spi_buf[0] = reg & !READ_BIT; + self.spi_buf[1] = val; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..2]).await; + } + + async fn accel_write(&mut self, reg: AccelRegisters, val: u8) { + self.select_accel(); + self.write(reg as u8, val).await; + } + + async fn gyro_write(&mut self, reg: GyroRegisters, val: u8) { + self.select_gyro(); + self.write(reg as u8, val).await; + } + + pub async fn init(&mut self) { + // imu accel needs at least one dummy read to set the data mode to SPI from POn I2C + // second read should be valid here for sanity purposes + + let _ = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; + let _ = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; + } + + async fn accel_self_test(&mut self) -> Result<(), ()> { + // test is non-trivial, e.g. not self contained + // procedure is described in section 4.6.1 of the datasheet + + self.accel_set_range(AccelRange::Range16g).await; + self.accel_set_bandwidth(AccelConfBwp::OverSamplingNormal, AccelConfOdr::OutputDataRate1600p0).await; + Timer::after(Duration::from_millis(2 + 1)).await; + + self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::PositiveSelfTest as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + let positive_test_data = self.accel_get_data_mg().await; + + self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::NegativeSelfTest as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + let negative_test_data = self.accel_get_data_mg().await; + + self.accel_write(AccelRegisters::ACC_SELF_TEST, AccelSelfTestReg::Off as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + + let x_diff = positive_test_data[0] - negative_test_data[0]; + let y_diff = positive_test_data[1] - negative_test_data[1]; + let z_diff = positive_test_data[2] - negative_test_data[2]; + + let mut result = Ok(()); + if x_diff < 1000.0 { + trace!("accel self test failed x-axis needed polarity difference >=1000 got {}", x_diff); + result = Err(()) + } + + if y_diff < 1000.0 { + trace!("accel self test failed y-axis needed polarity difference >=1000 got {}", y_diff); + result = Err(()) + } + + if z_diff < 500.0 { + trace!("accel self test failed z-axis needed polarity difference >=500 got {}", z_diff); + result = Err(()) + } + + if result.is_err() { + warn!("accel failed BIST. (one or more axis diverged)"); + } + + result + } + + async fn gyro_self_test(&mut self) -> Result<(), ()> { + self.gyro_write(GyroRegisters::GYRO_SELF_TEST, GyroSelfTestReg::TrigBist as u8).await; + let mut try_ct = 0; + loop { + let strreg_val = self.gyro_read(GyroRegisters::GYRO_SELF_TEST).await; + if (strreg_val & GyroSelfTestReg::BistRdy as u8) != 0 { + if (strreg_val & GyroSelfTestReg::BistFail as u8) != 0 { + warn!("gyro failed BIST. (the bist fail bit is high)"); + return Err(()); + } else { + debug!("gyro passed self test."); + return Ok(()); + } + } + + Timer::after(Duration::from_millis(10)).await; + try_ct += 1; + + if try_ct > 10 { + warn!("gyro BIST did not converge."); + return Err(()); + } + } + } + + pub async fn self_test(&mut self) -> Result<(), ()> { + let mut has_self_test_error = Ok(()); + + self.deselect(); + + let _ = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; + let acc_chip_id = self.accel_read(AccelRegisters::ACC_CHIP_ID).await; + if acc_chip_id != ACCEL_CHIP_ID { + warn!("read accel ID (0x{:x}) does not match expected BMI085 accel ID (0x{:x})", acc_chip_id, ACCEL_CHIP_ID); + has_self_test_error = Err(()); + } else { + debug!("accel id verified: 0x{:x}", acc_chip_id); + } + + let _ = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; + let gyro_chip_id = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; + if gyro_chip_id != GYRO_CHIP_ID { + warn!("read gyro ID (0x{:x}) does not match expected BMI085 gyro ID (0x{:x})", gyro_chip_id, GYRO_CHIP_ID); + has_self_test_error = Err(()); + } else { + debug!("gyro id verified: 0x{:x}", gyro_chip_id); + } + + if (self.accel_self_test().await).is_err() { + has_self_test_error = Err(()); + } + + if (self.gyro_self_test().await).is_err() { + has_self_test_error = Err(()); + } + + has_self_test_error + } + + const fn read_pair_to_i16(&self, lsb: u8, msb: u8) -> i16 { + (msb as u16 * 256 + lsb as u16) as i16 + } + + pub async fn accel_get_raw_data(&mut self) -> [i16; 3] { + let mut buf: [u8; 6] = [0; 6]; + self.accel_burst_read(AccelRegisters::ACC_X_LSB, &mut buf).await; + + [self.read_pair_to_i16(buf[0], buf[1]), + self.read_pair_to_i16(buf[2], buf[3]), + self.read_pair_to_i16(buf[4], buf[5])] + } + + pub fn accel_range_mg(&self) -> f32 { + match self.accel_range { + AccelRange::Range2g => 2000.0, + AccelRange::Range4g => 4000.0, + AccelRange::Range8g => 8000.0, + AccelRange::Range16g => 16000.0, + } + } + + pub fn convert_accel_raw_sample_mg(&self, raw_sample: i16) -> f32 { + let conversion_num = self.accel_range_mg(); + + raw_sample as f32 * (conversion_num / i16::MAX as f32) + } + + pub fn convert_accel_raw_sample_mps(&self, raw_sample: i16) -> f32 { + self.convert_accel_raw_sample_mg(raw_sample) / 1000.0 * 9.80665 + } + + pub async fn accel_get_data_mg(&mut self) -> [f32; 3] { + let raw_data = self.accel_get_raw_data().await; + + return [self.convert_accel_raw_sample_mg(raw_data[0]), + self.convert_accel_raw_sample_mg(raw_data[1]), + self.convert_accel_raw_sample_mg(raw_data[2])] + } + + pub async fn accel_get_data_mps(&mut self) -> [f32; 3] { + let raw_data = self.accel_get_raw_data().await; + + return [self.convert_accel_raw_sample_mps(raw_data[0]), + self.convert_accel_raw_sample_mps(raw_data[1]), + self.convert_accel_raw_sample_mps(raw_data[2])] + } + + pub async fn accel_set_bandwidth(&mut self, oversampling_mode: AccelConfBwp, output_data_rate: AccelConfOdr) { + self.accel_write(AccelRegisters::ACC_CONF, (oversampling_mode as u8) << 4 | output_data_rate as u8 ).await; + } + + pub async fn accel_set_range(&mut self, range: AccelRange) { + self.accel_write(AccelRegisters::ACC_RANGE, range as u8).await; + + self.accel_range = range; + } + + pub async fn gyro_get_raw_data(&mut self) -> [i16; 3] { + let mut buf: [u8; 6] = [0; 6]; + self.gyro_burst_read(GyroRegisters::RATE_X_LSB, &mut buf).await; + + [self.read_pair_to_i16(buf[0], buf[1]), + self.read_pair_to_i16(buf[2], buf[3]), + self.read_pair_to_i16(buf[4], buf[5])] + } + + pub fn dps_to_rads(dps: f32) -> f32 { + (dps / 360.0) * 2.0 * PI + } + + fn gyro_rate(&self) -> f32 { + match self.gyro_range { + GyroRange::PlusMinus2000DegPerSec => 2000.0, + GyroRange::PlusMinus1000DegPerSec => 1000.0, + GyroRange::PlusMinus500DegPerSec => 500.0, + GyroRange::PlusMinus250DegPerSec => 250.0, + GyroRange::PlusMinus125DegPerSec => 125.0, + } + } + + pub fn max_dps(&self) -> f32 { + self.gyro_rate() + } + + pub fn max_rads(&self) -> f32 { + Self::dps_to_rads(self.max_dps()) + } + + pub fn convert_raw_gyro_sample_dps(&self, raw_sample: i16) -> f32 { + let conversion_num = self.gyro_rate(); + + raw_sample as f32 * (conversion_num / i16::MAX as f32) + } + + pub fn convert_raw_gyro_sample_rads(&self, raw_sample: i16) -> f32 { + Self::dps_to_rads(self.convert_raw_gyro_sample_dps(raw_sample)) + } + + pub async fn gyro_get_data_dps(&mut self) -> [f32; 3] { + let raw_data = self.gyro_get_raw_data().await; + + return [self.convert_raw_gyro_sample_dps(raw_data[0]), + self.convert_raw_gyro_sample_dps(raw_data[1]), + self.convert_raw_gyro_sample_dps(raw_data[2])] + } + + pub async fn gyro_get_data_rads(&mut self) -> [f32; 3] { + let raw_data = self.gyro_get_raw_data().await; + + return [self.convert_raw_gyro_sample_rads(raw_data[0]), + self.convert_raw_gyro_sample_rads(raw_data[1]), + self.convert_raw_gyro_sample_rads(raw_data[2])] + } + + pub async fn gyro_set_range(&mut self, range: GyroRange) { + self.gyro_write(GyroRegisters::GYRO_RANGE, range as u8).await; + + self.gyro_range = range; + } + + pub async fn gyro_set_bandwidth(&mut self, bandwidth: GyroBandwidth) { + self.gyro_write(GyroRegisters::GYRO_BANDWIDTH, bandwidth as u8).await; + } + + pub async fn gyro_enable_interrupts(&mut self) { + self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOnNewData as u8).await; + } + + pub async fn gyro_disable_interrupts(&mut self) { + self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOff as u8).await; + } + + pub async fn gyro_set_int_config(&mut self, + int3_active_state: GyroIntPinActiveState, + int3_mode: GyroIntPinMode, + int4_active_state: GyroIntPinActiveState, + int4_mode: GyroIntPinMode) { + let reg_val = (int4_mode as u8) << 3 | (int4_active_state as u8) << 2 | (int3_mode as u8) << 1 | int3_active_state as u8; + self.gyro_write(GyroRegisters::INT3_INT4_IO_CONF, reg_val).await; + } + + pub async fn gyro_set_int_map(&mut self, map: GyroIntMap) { + self.gyro_write(GyroRegisters::INT3_INT4_IO_MAP, map as u8).await; + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/imu/mod.rs b/lib-stm32/src/drivers/imu/mod.rs new file mode 100644 index 00000000..5bfbfc78 --- /dev/null +++ b/lib-stm32/src/drivers/imu/mod.rs @@ -0,0 +1,2 @@ +pub mod bmi085; + diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index faa0d38e..5ef73ea4 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -1,2 +1,3 @@ -pub mod flash; \ No newline at end of file +pub mod flash; +pub mod imu; \ No newline at end of file From 9dbc29d5a999b408e6a7a10fffb7fef64e9b82ce Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 17:55:07 -0400 Subject: [PATCH 027/157] bmi323 WIP --- control-board/src/pins.rs | 16 +- control-board/src/tasks/imu_task.rs | 170 ++++---- lib-stm32/src/drivers/imu/bmi323.rs | 601 ++++++++++++++++++++++++++++ lib-stm32/src/drivers/imu/mod.rs | 1 + 4 files changed, 699 insertions(+), 89 deletions(-) create mode 100644 lib-stm32/src/drivers/imu/bmi323.rs diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 60c820de..739df962 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -4,17 +4,27 @@ use ateam_common_packets::radio::{DataPacket, TelemetryPacket}; use embassy_stm32::peripherals::*; use embassy_sync::{ blocking_mutex::raw::ThreadModeRawMutex, - pubsub::{Publisher, Subscriber}}; + pubsub::{PubSubChannel, Publisher, Subscriber}}; ///////////////////// // Pub Sub Types // ///////////////////// -const COMMANDS_PUBSUB_DEPTH: usize = 1; -const TELEMETRY_PUBSUB_DEPTH: usize = 3; +const COMMANDS_PUBSUB_DEPTH: usize = 4; +const TELEMETRY_PUBSUB_DEPTH: usize = 4; +const GYRO_DATA_PUBSUB_DEPTH: usize = 1; +const ACCEL_DATA_PUBSUB_DEPTH: usize = 1; + pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; + pub type TelemetrySubcriber = Subscriber<'static, ThreadModeRawMutex, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; +pub type GyroDataPubSub = PubSubChannel, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type GyroDataPublisher = Publisher<'static, ThreadModeRawMutex, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type GyroDataSubscriber = Subscriber<'static, ThreadModeRawMutex, nalgebra::Vector3, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataPubSub = PubSubChannel, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataPublisher = Publisher<'static, ThreadModeRawMutex, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type AccelDataSubscriber = Subscriber<'static, ThreadModeRawMutex, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; ///////////// // Radio // diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 25ed4845..36560491 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -1,121 +1,119 @@ use embassy_executor::{SpawnError, Spawner}; +use embassy_futures::select::{select, Either}; use embassy_stm32::Peripheral; use embassy_stm32::exti::ExtiInput; -use embassy_stm32::gpio::{Input, Pull}; +use embassy_stm32::gpio::Pull; use embassy_stm32::spi::{SckPin, MisoPin, MosiPin}; -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::{PubSubChannel, Subscriber, Error}; +use embassy_sync::pubsub::{PubSubChannel, Error}; +use embassy_time::Timer; use nalgebra::Vector3; use static_cell::ConstStaticCell; use ateam_lib_stm32::drivers::imu::bmi085::{Bmi085, GyroRange, GyroBandwidth, GyroIntMap, GyroIntPinActiveState, GyroIntPinMode, AccelRange, AccelConfOdr, AccelConfBwp}; -use crate::drivers::imu::{GyroFrame, AccelFrame}; -use crate::pins::{ImuSpi, ImuTxDma, ImuRxDma, ImuAccelCsPin, ImuGyroCsPin, ImuAccelIntPin, ImuGyroIntPin}; - -type AccelDataChannel = PubSubChannel; -type GyroDataChannel = PubSubChannel; - -pub type AccelSub<'a> = Subscriber<'a, ThreadModeRawMutex, AccelFrame, 1, 1, 1>; -pub type GyroSub<'a> = Subscriber<'a, ThreadModeRawMutex, GyroFrame, 1, 1, 1>; +use crate::pins::{ + AccelDataPublisher, ExtImuNDetPin, ExtImuSpiNss1Pin, ExtImuSpiNss2Pin, ExtImuSpiNss3Pin, GyroDataPublisher, ImuSpi, ImuSpiInt1Exti, ImuSpiInt1Pin, ImuSpiInt2Exti, ImuSpiInt2Pin, ImuSpiMisoPin, ImuSpiMosiPin, ImuSpiNss0Pin, ImuSpiRxDma, ImuSpiSckPin, ImuSpiTxDma}; #[link_section = ".axisram.buffers"] static IMU_BUFFER_CELL: ConstStaticCell<[u8; 8]> = ConstStaticCell::new([0; 8]); -static ACCEL_DATA_CHANNEL: AccelDataChannel = PubSubChannel::new(); -static GYRO_DATA_CHANNEL: GyroDataChannel = PubSubChannel::new(); - #[embassy_executor::task] -async fn imu_task_entry(mut imu: Bmi085<'static, 'static, ImuSpi>, mut _accel_int: ExtiInput<'static, ImuAccelIntPin>, mut gyro_int: ExtiInput<'static, ImuGyroIntPin>) { - let accel_pub = ACCEL_DATA_CHANNEL.publisher().expect("accel data channel had no publisher left"); - let gyro_pub = GYRO_DATA_CHANNEL.publisher().expect("gyro data channel had no publisher left"); - - imu.init().await; - let self_test_res = imu.self_test().await; - if self_test_res.is_err() { - defmt::error!("IMU self test failed"); - } +async fn imu_task_entry( + accel_pub: AccelDataPublisher, + gyro_pub: GyroDataPublisher, + mut imu: Bmi085<'static, 'static, ImuSpi>, + mut _accel_int: ExtiInput<'static>, + mut gyro_int: ExtiInput<'static>) { - // set measurement ranges - imu.accel_set_range(AccelRange::Range2g).await; // range +- 19.6 m/s^2 - imu.accel_set_bandwidth(AccelConfBwp::OverSampling2Fold, AccelConfOdr::OutputDataRate400p0).await; // bandwidth 80Hz - - // set interrupt config - imu.gyro_set_int_config(GyroIntPinActiveState::ActiveLow, - GyroIntPinMode::OpenDrain, - GyroIntPinActiveState::ActiveLow, - GyroIntPinMode::OpenDrain).await; - // enable BMI085 Gyro INT3 which is electrically wired to IMU breakout INT2 named gyro_int_pin - imu.gyro_set_int_map(GyroIntMap::Int3).await; - - imu.gyro_set_range(GyroRange::PlusMinus2000DegPerSec).await; - imu.gyro_set_bandwidth(GyroBandwidth::FilterBw64Hz).await; - - // enable interrupts - imu.gyro_enable_interrupts().await; - - // BMI085 internally shares a SPI bus so we can't actually asynchronously read both the IMU and Accel - // unless we used a mutex but that's still a lot of overhead. We'll just sync on the Gyro INT since - // that gyro data is far more critical for control. When the gyro has an update, we'll also grab the - // accel update/ + defmt::info!("imu start startup."); + 'imu_configuration_loop: loop { - // block on gyro interrupt, active low - gyro_int.wait_for_falling_edge().await; + imu.init().await; + let self_test_res = imu.self_test().await; + if self_test_res.is_err() { + defmt::error!("IMU self test failed"); + Timer::after_millis(1000).await; + continue 'imu_configuration_loop; + } + + // set measurement ranges + imu.accel_set_range(AccelRange::Range2g).await; // range +- 19.6 m/s^2 + imu.accel_set_bandwidth(AccelConfBwp::OverSampling2Fold, AccelConfOdr::OutputDataRate400p0).await; // bandwidth 80Hz + + // set interrupt config + imu.gyro_set_int_config(GyroIntPinActiveState::ActiveLow, + GyroIntPinMode::OpenDrain, + GyroIntPinActiveState::ActiveLow, + GyroIntPinMode::OpenDrain).await; + // enable BMI085 Gyro INT3 which is electrically wired to IMU breakout INT2 named gyro_int_pin + imu.gyro_set_int_map(GyroIntMap::Int3).await; + + imu.gyro_set_range(GyroRange::PlusMinus2000DegPerSec).await; + imu.gyro_set_bandwidth(GyroBandwidth::FilterBw64Hz).await; + + // enable interrupts + imu.gyro_enable_interrupts().await; + + // BMI085 internally shares a SPI bus so we can't actually asynchronously read both the IMU and Accel + // unless we used a mutex but that's still a lot of overhead. We'll just sync on the Gyro INT since + // that gyro data is far more critical for control. When the gyro has an update, we'll also grab the + // accel update/ + + 'imu_data_loop: + loop { + // block on gyro interrupt, active low + match select(gyro_int.wait_for_falling_edge(), Timer::after_millis(1000)).await { + Either::First(_) => { + // read gyro data + let imu_data = imu.gyro_get_data_rads().await; + gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2])); + + // read accel data + // TODO: don't use raw data, impl conversion + let accel_data = imu.accel_get_raw_data().await; + accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); + } + Either::Second(_) => { + defmt::warn!("imu interrupt based data acq timed out."); + // attempt connect validation and reconfig + break 'imu_data_loop + } + }; + } + } - // read gyro data - let imu_data = imu.gyro_get_data_rads().await; - gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2])); - // read accel data - // TODO: don't use raw data, impl conversion - let accel_data = imu.accel_get_raw_data().await; - accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); - } } -pub fn start_imu_task(spawner: &Spawner, +pub fn start_imu_task( + imu_task_spawner: &Spawner, + gyro_data_publisher: GyroDataPublisher, + accel_data_publisher: AccelDataPublisher, peri: impl Peripheral

+ 'static, sck: impl Peripheral

> + 'static, mosi: impl Peripheral

> + 'static, miso: impl Peripheral

> + 'static, - txdma: impl Peripheral

+ 'static, - rxdma: impl Peripheral

+ 'static, - accel_cs: impl Peripheral

+ 'static, - gyro_cs: impl Peripheral

+ 'static, - accel_int_pin: impl Peripheral

+ 'static, - gyro_int_pin: impl Peripheral

+ 'static, - accel_int: impl Peripheral

::ExtiChannel> + 'static, - gyro_int: impl Peripheral

::ExtiChannel> + 'static, - ) -> Result<(), SpawnError> { - - defmt::warn!("here3!"); - - let imu_buf = IMU_BUFFER_CELL.get(); - - defmt::warn!("here4!"); + txdma: impl Peripheral

+ 'static, + rxdma: impl Peripheral

+ 'static, + accel_cs: ExtImuSpiNss1Pin, + gyro_cs: ExtImuSpiNss2Pin, + accel_int_pin: impl Peripheral

+ 'static, + gyro_int_pin: impl Peripheral

+ 'static, + accel_int: impl Peripheral

::ExtiChannel> + 'static, + gyro_int: impl Peripheral

::ExtiChannel> + 'static, + ext_imu_det_pin: ExtImuNDetPin) { + let imu_buf = IMU_BUFFER_CELL.take(); let imu = Bmi085::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, accel_cs, gyro_cs, imu_buf); - let accel_int_input = Input::new(accel_int_pin, Pull::Down); - let accel_int = ExtiInput::new(accel_int_input, accel_int); - // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain - let gyro_int_input = Input::new(gyro_int_pin, Pull::Up); - let gyro_int = ExtiInput::new(gyro_int_input, gyro_int); + let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::Down); + let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::Up); - defmt::warn!("here!"); - - spawner.spawn(imu_task_entry(imu, accel_int, gyro_int)) -} - -pub fn get_accel_sub() -> Result, Error> { - return ACCEL_DATA_CHANNEL.subscriber(); + imu_task_spawner.spawn(imu_task_entry(accel_data_publisher, gyro_data_publisher, imu, accel_int, gyro_int)).unwrap(); } -pub fn get_gyro_sub() -> Result, Error> { - return GYRO_DATA_CHANNEL.subscriber(); -} diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs new file mode 100644 index 00000000..c949e54a --- /dev/null +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -0,0 +1,601 @@ +/* + * Driver for the Bosch BMI323 IMU. + * + * https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi323-ds000.pdf * + */ + +use core::{cmp::min, f32::consts::PI}; + +use embassy_stm32::{ + gpio::{Level, Output, Pin, Speed}, + mode::Async, + spi::{self, MisoPin, MosiPin, SckPin}, + time::hz, + Peripheral +}; +use embassy_time::{Timer, Duration}; + +use defmt::*; + + +pub const SPI_MIN_BUF_LEN: usize = 8; + +/// SPI driver for the Bosch BMI085 IMU: Accel + Gyro +pub struct Bmi323<'a, 'buf, SpiPeri: spi::Instance> { + spi: spi::Spi<'a, SpiPeri, Async>, + spi_cs: Output<'a>, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], +} + +#[repr(u8)] +#[allow(non_camel_case_types, dead_code)] +#[derive(Clone, Copy, Debug)] +enum ImuRegisters { + ACC_CHIP_ID = 0x00, + ACC_ERR_REG = 0x01, + ACC_STATUS = 0x02, + ACC_X_LSB = 0x03, + ACC_X_MSB = 0x04, + ACC_Y_LSB = 0x05, + ACC_Y_MSB = 0x06, + ACC_Z_LSB = 0x07, + ACC_Z_MSB = 0x08, + TEMP_DATA = 0x09, + SENSORTIME_0 = 0x0A, + SENSORTIME_1 = 0x0B, + SAT_FLAGS = 0x0C, + INT_STATUS_INT1 = 0x0D, + INT_STATUS_INT2 = 0x0E, + INT_STATUS_IBI = 0x0F, + FEATURE_IO0 = 0x10, + FEATURE_IO1 = 0x11, + FEATURE_IO2 = 0x12, + FEATURE_IO3 = 0x13, + FEATURE_IO_STATUS = 0x14, + FIFO_FILL_LEVEL = 0x15, + FIFO_DATA = 0x16, + // 0x17 - 0x19 reserved + ACC_CONF = 0x20, + GYR_CONF = 0x21, + // 0x22 - 0x27 reserved + ALT_ACC_CONF = 0x28, + ALT_GYR_CONF = 0x29, + ALT_CONF = 0x2A, + ALT_STATUS = 0x2B, + // 0x2B - 0x34 reserved + FIFO_WATERMARK = 0x35, + FIFO_CONF = 0x36, + FIFO_CTRL = 0x37, + IO_INT_CTRL = 0x38, + INT_CONF = 0x39, + INT_MAP1 = 0x3A, + INT_MAP2 = 0x3B, + // 0x3C - 0x3F reserved + FEATURE_CTRL = 0x40, + FEATURE_DATA_ADDR = 0x41, + FEATURE_DATA_TX = 0x42, + FEATURE_DATA_STATUS = 0x43, + // 0x44 reserved + FEATURE_ENGINE_STATUS = 0x45, + // 0x46 reserved + FEATURE_EVENT_EXT = 0x47, + // 0x48 - 0x4E reserved + IO_PDN_CTRL = 0x4F, + IO_SPI_IF = 0x50, + IO_PAD_STRENGTH = 0x51, + IO_I2C_IF = 0x52, + IO_ODR_DEVIATION = 0x53, + // 0x54 - 0x5F reserved + ACC_DP_OFF_X = 0x60, + ACC_DP_DGAIN_X = 0x61, + ACC_DP_OFF_Y = 0x62, + ACC_DP_DGAIN_Y = 0x63, + ACC_DP_OFF_Z = 0x64, + ACC_DP_DGAIN_Z = 0x65, + GYR_DP_OFF_X = 0x66, + GYR_DP_DGAIN_X = 0x67, + GYR_DP_OFF_Y = 0x68, + GYR_DP_DGAIN_Y = 0x69, + GYR_DP_OFF_Z = 0x6A, + GYR_DP_DGAIN_Z = 0x6B, + // 0x6C - 0x6F reserved + I3C_TC_SYNC_TPH = 0x70, + I3C_TC_SYNC_TU = 0x71, + I3C_TC_SYNC_ODR = 0x72, + // 0x73 - 0x7D reserved + CMD = 0x7E, + CFG_RES = 0x7F, +} + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum AccelRange { +// Range2g = 0x00, +// Range4g = 0x01, +// Range8g = 0x02, +// Range16g = 0x03, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum AccelConfBwp { +// OverSampling4Fold = 0x08, +// OverSampling2Fold = 0x09, +// OverSamplingNormal = 0x0A, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum AccelConfOdr { +// OutputDataRate12p5 = 0x05, +// OutputDataRate25p0 = 0x06, +// OutputDataRate50p0 = 0x07, +// OutputDataRate100p0 = 0x08, +// OutputDataRate200p0 = 0x09, +// OutputDataRate400p0 = 0x0A, +// OutputDataRate800p0 = 0x0B, +// OutputDataRate1600p0 = 0x0C, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// enum AccelSelfTestReg { +// Off = 0x00, +// PositiveSelfTest = 0x0D, +// NegativeSelfTest = 0x09, +// } + +// const ACCEL_CHIP_ID: u8 = 0x1F; + +// #[repr(u8)] +// #[allow(non_camel_case_types, dead_code)] +// #[derive(Clone, Copy, Debug)] +// enum GyroRegisters { +// GYRO_CHIP_ID = 0x00, +// RATE_X_LSB = 0x02, +// RATE_X_MSB = 0x03, +// RATE_Y_LSB = 0x04, +// RATE_Y_MSB = 0x05, +// RATE_Z_LSB = 0x06, +// RATE_Z_MSB = 0x07, +// GYRO_INT_STAT_1 = 0x0A, +// GYRO_RANGE = 0x0F, +// GYRO_BANDWIDTH = 0x10, +// GYRO_LPM1 = 0x11, +// GYRO_SOFTRESET = 0x14, +// GYRO_INT_CONTROL = 0x15, +// INT3_INT4_IO_CONF = 0x16, +// INT3_INT4_IO_MAP = 0x18, +// GYRO_SELF_TEST = 0x3C, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// enum GyroSelfTestReg { +// RateOk = 0b10000, +// BistFail = 0b00100, +// BistRdy = 0b00010, +// TrigBist = 0b00001, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum GyroRange { +// PlusMinus2000DegPerSec = 0x00, +// PlusMinus1000DegPerSec = 0x01, +// PlusMinus500DegPerSec = 0x02, +// PlusMinus250DegPerSec = 0x03, +// PlusMinus125DegPerSec = 0x04, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum GyroBandwidth { +// FilterBw532Hz = 0x00, +// FilterBw230Hz = 0x01, +// FilterBw116Hz = 0x02, +// FilterBw47Hz = 0x03, +// FilterBw23Hz = 0x04, +// FilterBw12Hz = 0x05, +// FilterBw64Hz = 0x06, +// FilterBw32Hz = 0x07, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// enum GyroIntCtrl { +// InterruptOff = 0x00, +// InterruptOnNewData = 0x80, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum GyroIntPinMode { +// PushPull = 0b0, +// OpenDrain = 0b1, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum GyroIntPinActiveState { +// ActiveLow = 0b0, +// ActiveHigh = 0b1, +// } + +// #[repr(u8)] +// #[allow(dead_code)] +// #[derive(Clone, Copy, Debug)] +// pub enum GyroIntMap { +// NotMapped = 0x00, +// Int3 = 0x01, +// Int4 = 0x80, +// Int3AndInt4 = 0x81, +// } + +// const GYRO_CHIP_ID: u8 = 0x0F; + +const READ_BIT: u8 = 0x80; + +impl<'a, 'buf, SpiPeri: spi::Instance> + Bmi323<'a, 'buf, SpiPeri> { + /// creates a new BMI085 instance from a pre-existing Spi peripheral + pub fn new_from_spi( + spi: spi::Spi<'a, SpiPeri, Async>, + spi_cs: Output<'a>, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + ) -> Self { + Bmi323 { + spi: spi, + spi_cs: accel_cs, + spi_buf: spi_buf, + } + } + + ///t creates a new BMI085 instance from uninitialized pins + pub fn new_from_pins( + peri: impl Peripheral

+ 'a, + sck_pin: impl Peripheral

> + 'a, + mosi_pin: impl Peripheral

> + 'a, + miso_pin: impl Peripheral

> + 'a, + tx_dma: impl Peripheral

> + 'a, + rx_dma: impl Peripheral

> + 'a, + spi_cs_pin: impl Pin, + spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + ) -> Self { + let mut spi_config = spi::Config::default(); + // Bmi323 max SPI frequency is 10MHz + spi_config.frequency = hz(10_000_000); + + let imu_spi = spi::Spi::new( + peri, + sck_pin, + mosi_pin, + miso_pin, + tx_dma, + rx_dma, + spi_config + ); + + let spi_cs = Output::new(spi_cs_pin, Level::High, Speed::VeryHigh); + + Bmi323 { + spi: imu_spi, + spi_cs: spi_cs, + spi_buf: spi_buf + } + } + + fn select(&mut self) { + self.spi_cs.set_low(); + } + + fn deselect(&mut self) { + self.spi_cs.set_high(); + } + + async fn read(&mut self, reg: u8) -> u16 { + // addresses are 7 bits with MSB flagging read/!write + // data is 16 bits + + self.spi_buf[0] = reg | READ_BIT; + self.spi_buf[1] = 0x00; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..4]).await; + + ((self.spi_buf[2] as u16) << 8) | self.spi_buf[3] as u16 + } + + async fn burst_read(&mut self, reg: ImuRegisters, dest: &mut [u16]) { + // the transaction length is either the dest buf size * 2 + 2 + // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) + // OR upper bounded by internal length of the buffer. + let trx_len = min((dest.len() * 2) + 2, self.spi_buf.len()); + + self.spi_buf[0] = reg as u8 | READ_BIT; + self.spi_buf[1] = 0x00; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + + for dword in dest.iter_mut() { + dword = + } + dest.copy_from_slice(&self.spi_buf[2..trx_len]); + } + + async fn write(&mut self, reg: u8, val: u8) { + self.spi_buf[0] = reg & !READ_BIT; + self.spi_buf[1] = val; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..2]).await; + } + + async fn burst_write(&mut self, reg: ImuRegisters, dest: &mut [u16]) { + + } + + pub async fn init(&mut self) { + // imu accel needs at least one dummy read to set the data mode to SPI from POn I2C + // second read should be valid here for sanity purposes + + let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; + let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; + } + + async fn accel_self_test(&mut self) -> Result<(), ()> { + // test is non-trivial, e.g. not self contained + // procedure is described in section 4.6.1 of the datasheet + + self.accel_set_range(AccelRange::Range16g).await; + self.accel_set_bandwidth(AccelConfBwp::OverSamplingNormal, AccelConfOdr::OutputDataRate1600p0).await; + Timer::after(Duration::from_millis(2 + 1)).await; + + self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::PositiveSelfTest as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + let positive_test_data = self.accel_get_data_mg().await; + + self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::NegativeSelfTest as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + let negative_test_data = self.accel_get_data_mg().await; + + self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::Off as u8).await; + Timer::after(Duration::from_millis(50 + 1)).await; + + let x_diff = positive_test_data[0] - negative_test_data[0]; + let y_diff = positive_test_data[1] - negative_test_data[1]; + let z_diff = positive_test_data[2] - negative_test_data[2]; + + let mut result = Ok(()); + if x_diff < 1000.0 { + trace!("accel self test failed x-axis needed polarity difference >=1000 got {}", x_diff); + result = Err(()) + } + + if y_diff < 1000.0 { + trace!("accel self test failed y-axis needed polarity difference >=1000 got {}", y_diff); + result = Err(()) + } + + if z_diff < 500.0 { + trace!("accel self test failed z-axis needed polarity difference >=500 got {}", z_diff); + result = Err(()) + } + + if result.is_err() { + warn!("accel failed BIST. (one or more axis diverged)"); + } + + result + } + + async fn gyro_self_test(&mut self) -> Result<(), ()> { + self.gyro_write(GyroRegisters::GYRO_SELF_TEST, GyroSelfTestReg::TrigBist as u8).await; + let mut try_ct = 0; + loop { + let strreg_val = self.gyro_read(GyroRegisters::GYRO_SELF_TEST).await; + if (strreg_val & GyroSelfTestReg::BistRdy as u8) != 0 { + if (strreg_val & GyroSelfTestReg::BistFail as u8) != 0 { + warn!("gyro failed BIST. (the bist fail bit is high)"); + return Err(()); + } else { + debug!("gyro passed self test."); + return Ok(()); + } + } + + Timer::after(Duration::from_millis(10)).await; + try_ct += 1; + + if try_ct > 10 { + warn!("gyro BIST did not converge."); + return Err(()); + } + } + } + + pub async fn self_test(&mut self) -> Result<(), ()> { + let mut has_self_test_error = Ok(()); + + self.deselect(); + + let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; + let acc_chip_id = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; + if acc_chip_id != ACCEL_CHIP_ID { + warn!("read accel ID (0x{:x}) does not match expected BMI085 accel ID (0x{:x})", acc_chip_id, ACCEL_CHIP_ID); + has_self_test_error = Err(()); + } else { + debug!("accel id verified: 0x{:x}", acc_chip_id); + } + + let _ = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; + let gyro_chip_id = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; + if gyro_chip_id != GYRO_CHIP_ID { + warn!("read gyro ID (0x{:x}) does not match expected BMI085 gyro ID (0x{:x})", gyro_chip_id, GYRO_CHIP_ID); + has_self_test_error = Err(()); + } else { + debug!("gyro id verified: 0x{:x}", gyro_chip_id); + } + + if (self.accel_self_test().await).is_err() { + has_self_test_error = Err(()); + } + + if (self.gyro_self_test().await).is_err() { + has_self_test_error = Err(()); + } + + has_self_test_error + } + + const fn read_pair_to_i16(&self, lsb: u8, msb: u8) -> i16 { + (msb as u16 * 256 + lsb as u16) as i16 + } + + pub async fn accel_get_raw_data(&mut self) -> [i16; 3] { + let mut buf: [u8; 6] = [0; 6]; + self.accel_burst_read(ImuRegisters::ACC_X_LSB, &mut buf).await; + + [self.read_pair_to_i16(buf[0], buf[1]), + self.read_pair_to_i16(buf[2], buf[3]), + self.read_pair_to_i16(buf[4], buf[5])] + } + + pub fn accel_range_mg(&self) -> f32 { + match self.accel_range { + AccelRange::Range2g => 2000.0, + AccelRange::Range4g => 4000.0, + AccelRange::Range8g => 8000.0, + AccelRange::Range16g => 16000.0, + } + } + + pub fn convert_accel_raw_sample_mg(&self, raw_sample: i16) -> f32 { + let conversion_num = self.accel_range_mg(); + + raw_sample as f32 * (conversion_num / i16::MAX as f32) + } + + pub fn convert_accel_raw_sample_mps(&self, raw_sample: i16) -> f32 { + self.convert_accel_raw_sample_mg(raw_sample) / 1000.0 * 9.80665 + } + + pub async fn accel_get_data_mg(&mut self) -> [f32; 3] { + let raw_data = self.accel_get_raw_data().await; + + return [self.convert_accel_raw_sample_mg(raw_data[0]), + self.convert_accel_raw_sample_mg(raw_data[1]), + self.convert_accel_raw_sample_mg(raw_data[2])] + } + + pub async fn accel_get_data_mps(&mut self) -> [f32; 3] { + let raw_data = self.accel_get_raw_data().await; + + return [self.convert_accel_raw_sample_mps(raw_data[0]), + self.convert_accel_raw_sample_mps(raw_data[1]), + self.convert_accel_raw_sample_mps(raw_data[2])] + } + + pub async fn accel_set_bandwidth(&mut self, oversampling_mode: AccelConfBwp, output_data_rate: AccelConfOdr) { + self.accel_write(ImuRegisters::ACC_CONF, (oversampling_mode as u8) << 4 | output_data_rate as u8 ).await; + } + + pub async fn accel_set_range(&mut self, range: AccelRange) { + self.accel_write(ImuRegisters::ACC_RANGE, range as u8).await; + + self.accel_range = range; + } + + pub async fn gyro_get_raw_data(&mut self) -> [i16; 3] { + let mut buf: [u8; 6] = [0; 6]; + self.gyro_burst_read(GyroRegisters::RATE_X_LSB, &mut buf).await; + + [self.read_pair_to_i16(buf[0], buf[1]), + self.read_pair_to_i16(buf[2], buf[3]), + self.read_pair_to_i16(buf[4], buf[5])] + } + + pub fn dps_to_rads(dps: f32) -> f32 { + (dps / 360.0) * 2.0 * PI + } + + fn gyro_rate(&self) -> f32 { + match self.gyro_range { + GyroRange::PlusMinus2000DegPerSec => 2000.0, + GyroRange::PlusMinus1000DegPerSec => 1000.0, + GyroRange::PlusMinus500DegPerSec => 500.0, + GyroRange::PlusMinus250DegPerSec => 250.0, + GyroRange::PlusMinus125DegPerSec => 125.0, + } + } + + pub fn max_dps(&self) -> f32 { + self.gyro_rate() + } + + pub fn max_rads(&self) -> f32 { + Self::dps_to_rads(self.max_dps()) + } + + pub fn convert_raw_gyro_sample_dps(&self, raw_sample: i16) -> f32 { + let conversion_num = self.gyro_rate(); + + raw_sample as f32 * (conversion_num / i16::MAX as f32) + } + + pub fn convert_raw_gyro_sample_rads(&self, raw_sample: i16) -> f32 { + Self::dps_to_rads(self.convert_raw_gyro_sample_dps(raw_sample)) + } + + pub async fn gyro_get_data_dps(&mut self) -> [f32; 3] { + let raw_data = self.gyro_get_raw_data().await; + + return [self.convert_raw_gyro_sample_dps(raw_data[0]), + self.convert_raw_gyro_sample_dps(raw_data[1]), + self.convert_raw_gyro_sample_dps(raw_data[2])] + } + + pub async fn gyro_get_data_rads(&mut self) -> [f32; 3] { + let raw_data = self.gyro_get_raw_data().await; + + return [self.convert_raw_gyro_sample_rads(raw_data[0]), + self.convert_raw_gyro_sample_rads(raw_data[1]), + self.convert_raw_gyro_sample_rads(raw_data[2])] + } + + pub async fn gyro_set_range(&mut self, range: GyroRange) { + self.gyro_write(GyroRegisters::GYRO_RANGE, range as u8).await; + + self.gyro_range = range; + } + + pub async fn gyro_set_bandwidth(&mut self, bandwidth: GyroBandwidth) { + self.gyro_write(GyroRegisters::GYRO_BANDWIDTH, bandwidth as u8).await; + } + + pub async fn gyro_enable_interrupts(&mut self) { + self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOnNewData as u8).await; + } + + pub async fn gyro_disable_interrupts(&mut self) { + self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOff as u8).await; + } + + pub async fn gyro_set_int_config(&mut self, + int3_active_state: GyroIntPinActiveState, + int3_mode: GyroIntPinMode, + int4_active_state: GyroIntPinActiveState, + int4_mode: GyroIntPinMode) { + let reg_val = (int4_mode as u8) << 3 | (int4_active_state as u8) << 2 | (int3_mode as u8) << 1 | int3_active_state as u8; + self.gyro_write(GyroRegisters::INT3_INT4_IO_CONF, reg_val).await; + } + + pub async fn gyro_set_int_map(&mut self, map: GyroIntMap) { + self.gyro_write(GyroRegisters::INT3_INT4_IO_MAP, map as u8).await; + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/imu/mod.rs b/lib-stm32/src/drivers/imu/mod.rs index 5bfbfc78..d1d87e2c 100644 --- a/lib-stm32/src/drivers/imu/mod.rs +++ b/lib-stm32/src/drivers/imu/mod.rs @@ -1,2 +1,3 @@ pub mod bmi085; +pub mod bmi323; From e3cdf06f94e8f7e77add4ed3922661d6c418abe2 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 18:02:11 -0400 Subject: [PATCH 028/157] import and macro cleanups --- control-board/src/bin/hwtest-enc/control.rs | 2 ++ .../src/bin/hwtest-uart-queue-mixedbaud/main.rs | 2 +- .../src/bin/hwtest-uart-queue-mixedrate/main.rs | 8 +++++++- lib-stm32/src/uart/queue.rs | 7 +++---- 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/control-board/src/bin/hwtest-enc/control.rs b/control-board/src/bin/hwtest-enc/control.rs index 1a7e5134..538679ec 100644 --- a/control-board/src/bin/hwtest-enc/control.rs +++ b/control-board/src/bin/hwtest-enc/control.rs @@ -33,6 +33,8 @@ const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 64; const RX_BUF_DEPTH: usize = 20; +create_uart_ + // buffers for front right #[link_section = ".axisram.buffers"] static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index eb06c204..096841f7 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -16,7 +16,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{make_uart_queues, queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queues, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index d00683c9..d61bdca4 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -8,7 +8,13 @@ use core::{ }; use embassy_stm32::{ - bind_interrupts, exti::ExtiInput, gpio::{Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, *}, usart::{self, *} + bind_interrupts, + exti::ExtiInput, + gpio::{Level, Output, Pull, Speed}, + interrupt, + pac::Interrupt, + peripherals::{self, *}, + usart::{self, *} }; use embassy_executor::{Executor, InterruptExecutor}; use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 9d2eed3b..67260730 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -30,15 +30,14 @@ use crate::queue::{ #[macro_export] macro_rules! make_uart_queues { ($name:ident, $uart:ty, $uart_rx_dma:ty, $uart_tx_dma:ty, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $(#[$m:meta])*) => { - use $crate::paste; - paste::paste! { + $crate::paste::paste! { // shared mutex allowing safe runtime update to UART config const [<$name _UART_PERI_MUTEX>]: embassy_sync::mutex::Mutex = embassy_sync::mutex::Mutex::new(false); // tx buffer const [<$name _CONST_TX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>> = - core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + core::cell::SyncUnsafeCell::new($crate::queue::Buffer::EMPTY); $(#[$m])* static [<$name _TX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>>; $tx_buffer_depth] = [[<$name _CONST_TX_BUF_VAL>]; $tx_buffer_depth]; @@ -47,7 +46,7 @@ macro_rules! make_uart_queues { // rx buffer const [<$name _CONST_RX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>> = - core::cell::SyncUnsafeCell::new(Buffer::EMPTY); + core::cell::SyncUnsafeCell::new($crate::queue::Buffer::EMPTY); $(#[$m])* static [<$name _RX_BUFFER>]: [core::cell::SyncUnsafeCell<$crate::queue::Buffer<$rx_buffer_size>>; $rx_buffer_depth] = [[<$name _CONST_RX_BUF_VAL>]; $rx_buffer_depth]; From b7f7d77a058944e4d22aea08928b58b24d7ae4d8 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 21:05:51 -0400 Subject: [PATCH 029/157] main and hwtest-imu firmware compiling and passing hwlimited tests --- control-board/src/bin/control/main.rs | 30 +- control-board/src/bin/hwtest-imu/main.rs | 164 +++---- control-board/src/pins.rs | 4 + control-board/src/tasks/imu_task.rs | 73 +-- control-board/src/tasks/radio_task.rs | 6 +- lib-stm32/src/drivers/imu/bmi323.rs | 601 ++++++++++++----------- software-communication | 2 +- 7 files changed, 468 insertions(+), 412 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 67aed410..4862dcc4 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -6,16 +6,11 @@ use embassy_stm32::{ interrupt, pac::Interrupt }; -use embassy_sync::{ - blocking_mutex::raw::ThreadModeRawMutex, - pubsub::PubSubChannel -}; +use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_common_packets::radio::{DataPacket, TelemetryPacket}; - -use ateam_control_board::{get_system_config, motion::tasks::radio_task::start_radio_task}; +use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, tasks::{imu_task::start_imu_task, radio_task::start_radio_task}}; // load credentials from correct crate @@ -24,11 +19,14 @@ use credentials::private_credentials::wifi::wifi_credentials; #[cfg(feature = "no-private-credentials")] use credentials::public_credentials::wifi::wifi_credentials; +use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; -static RADIO_C2_CHANNEL: PubSubChannel = PubSubChannel::new(); -static RADIO_TELEMETRY_CHANNEL: PubSubChannel = PubSubChannel::new(); +static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); +static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -67,6 +65,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); // TODO imu channel + let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); + let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); /////////////////// // start tasks // @@ -81,5 +81,15 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.DMA2_CH1, p.DMA2_CH0, p.PC13, p.PE4); - loop {} + start_imu_task(&main_spawner, + imu_gyro_data_publisher, imu_accel_data_publisher, + p.SPI1, p.PA5, p.PA7, p.PA6, + p.DMA2_CH7, p.DMA2_CH6, + p.PA4, p.PC4, p.PC5, + p.PB1, p.PB2, p.EXTI1, p.EXTI2, + p.PF11); + + loop { + Timer::after_millis(10).await; + } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index 0382f15e..db739372 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -1,98 +1,90 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -mod pins; +use embassy_executor::InterruptExecutor; +use embassy_futures::select::{self, Either3}; +use embassy_stm32::interrupt; +use embassy_sync::pubsub::{PubSubChannel, WaitResult}; -use defmt::*; -use defmt_rtt as _; +use defmt_rtt as _; + +use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}, tasks::imu_task::start_imu_task}; + +use embassy_time::Timer; +// provide embedded panic probe use panic_probe as _; -use embassy_stm32::{ - dma::NoDma, - gpio::{Level, Output, Speed}, - spi, - time::{hz, mhz}, -}; -use embassy_time::{Duration, Timer}; +static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); -use apa102_spi::Apa102; -use smart_leds::{SmartLedsWrite, RGB8}; +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - - info!("system core initialized"); - - let p = embassy_stm32::init(stm32_config); - - // Delay so dotstar can turn on - Timer::after(Duration::from_millis(50)).await; - - let dot_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), - ); - - let mut dotstar = Apa102::new(dot_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - let mut imu_spi = spi::Spi::new( - p.SPI6, - p.PA5, - p.PA7, - p.PA6, - p.BDMA_CH0, - p.BDMA_CH1, - hz(1_000_000), - spi::Config::default(), - ); - - // // acceleromter - let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // // gyro - let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - Timer::after(Duration::from_millis(1)).await; - - unsafe { - SPI6_BUF[0] = 0x80; - // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - imu_cs1.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs1.set_high(); - let accel_id = SPI6_BUF[1]; - info!("accelerometer id: 0x{:x}", accel_id); - - SPI6_BUF[0] = 0x80; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs2.set_high(); - let gyro_id = SPI6_BUF[1]; - info!("gyro id: 0x{:x}", gyro_id); - - loop { - SPI6_BUF[0] = 0x86; - // SPI6_BUF[0] = 0x86; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - imu_cs2.set_high(); - let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - info!("z rate: {}", rate_z); +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + //////////////////////// + // setup task pools // + //////////////////////// + + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); + let mut imu_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + let mut imu_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); + + /////////////////// + // start tasks // + /////////////////// + + start_imu_task(&main_spawner, + imu_gyro_data_publisher, imu_accel_data_publisher, + p.SPI1, p.PA5, p.PA7, p.PA6, + p.DMA2_CH7, p.DMA2_CH6, + p.PA4, p.PC4, p.PC5, + p.PB1, p.PB2, p.EXTI1, p.EXTI2, + p.PF11); + + loop { + match select::select3(imu_gyro_data_subscriber.next_message(), imu_accel_data_subscriber.next_message(), Timer::after_millis(1000)).await { + Either3::First(gyro_data) => { + match gyro_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("publishing gyro data lagged by {}", amnt); + } + WaitResult::Message(msg) => { + defmt::info!("got gyro data (x: {}, y: {}, z: {})", msg[1], msg[1], msg[2]); + + } + } + } + Either3::Second(accel_data) => { + match accel_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("publishing accel data lagged by {}", amnt); + } + WaitResult::Message(msg) => { + defmt::info!("got accel data (x: {}, y: {}, z: {})", msg[1], msg[1], msg[2]); + + } + } + } + Either3::Third(_) => { + defmt::warn!("receiving timed out."); + } } } -} +} \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 739df962..a2cabd8f 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -15,8 +15,12 @@ const TELEMETRY_PUBSUB_DEPTH: usize = 4; const GYRO_DATA_PUBSUB_DEPTH: usize = 1; const ACCEL_DATA_PUBSUB_DEPTH: usize = 1; +pub type CommandsPubSub = PubSubChannel; pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type CommandsSubscriber = Subscriber<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type TelemetryPubSub = PubSubChannel; +pub type TelemetryPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; pub type TelemetrySubcriber = Subscriber<'static, ThreadModeRawMutex, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; pub type GyroDataPubSub = PubSubChannel, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 36560491..c5bf7093 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -1,29 +1,27 @@ -use embassy_executor::{SpawnError, Spawner}; +use embassy_executor::Spawner; use embassy_futures::select::{select, Either}; use embassy_stm32::Peripheral; use embassy_stm32::exti::ExtiInput; use embassy_stm32::gpio::Pull; use embassy_stm32::spi::{SckPin, MisoPin, MosiPin}; -use embassy_sync::pubsub::{PubSubChannel, Error}; use embassy_time::Timer; use nalgebra::Vector3; use static_cell::ConstStaticCell; -use ateam_lib_stm32::drivers::imu::bmi085::{Bmi085, GyroRange, GyroBandwidth, GyroIntMap, GyroIntPinActiveState, GyroIntPinMode, AccelRange, AccelConfOdr, AccelConfBwp}; +use ateam_lib_stm32::drivers::imu::bmi323::{self, *}; -use crate::pins::{ - AccelDataPublisher, ExtImuNDetPin, ExtImuSpiNss1Pin, ExtImuSpiNss2Pin, ExtImuSpiNss3Pin, GyroDataPublisher, ImuSpi, ImuSpiInt1Exti, ImuSpiInt1Pin, ImuSpiInt2Exti, ImuSpiInt2Pin, ImuSpiMisoPin, ImuSpiMosiPin, ImuSpiNss0Pin, ImuSpiRxDma, ImuSpiSckPin, ImuSpiTxDma}; +use crate::pins::*; #[link_section = ".axisram.buffers"] -static IMU_BUFFER_CELL: ConstStaticCell<[u8; 8]> = ConstStaticCell::new([0; 8]); +static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstStaticCell::new([0; bmi323::SPI_MIN_BUF_LEN]); #[embassy_executor::task] async fn imu_task_entry( accel_pub: AccelDataPublisher, gyro_pub: GyroDataPublisher, - mut imu: Bmi085<'static, 'static, ImuSpi>, + mut imu: Bmi323<'static, 'static, ImuSpi>, mut _accel_int: ExtiInput<'static>, mut gyro_int: ExtiInput<'static>) { @@ -39,28 +37,36 @@ async fn imu_task_entry( continue 'imu_configuration_loop; } - // set measurement ranges - imu.accel_set_range(AccelRange::Range2g).await; // range +- 19.6 m/s^2 - imu.accel_set_bandwidth(AccelConfBwp::OverSampling2Fold, AccelConfOdr::OutputDataRate400p0).await; // bandwidth 80Hz - - // set interrupt config - imu.gyro_set_int_config(GyroIntPinActiveState::ActiveLow, - GyroIntPinMode::OpenDrain, - GyroIntPinActiveState::ActiveLow, - GyroIntPinMode::OpenDrain).await; - // enable BMI085 Gyro INT3 which is electrically wired to IMU breakout INT2 named gyro_int_pin - imu.gyro_set_int_map(GyroIntMap::Int3).await; - - imu.gyro_set_range(GyroRange::PlusMinus2000DegPerSec).await; - imu.gyro_set_bandwidth(GyroBandwidth::FilterBw64Hz).await; - - // enable interrupts - imu.gyro_enable_interrupts().await; - - // BMI085 internally shares a SPI bus so we can't actually asynchronously read both the IMU and Accel - // unless we used a mutex but that's still a lot of overhead. We'll just sync on the Gyro INT since - // that gyro data is far more critical for control. When the gyro has an update, we'll also grab the - // accel update/ + // configure the gyro, map int to int pin 2 + let gyro_config_res = imu.set_gyro_config(GyroMode::ContinuousHighPerformance, + GyroRange::PlusMinus2000DegPerSec, + Bandwidth3DbCutoffFreq::AccOdrOver2, + OutputDataRate::Odr6400p0, + DataAveragingWindow::Average64Samples).await; + imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2).await; + + if gyro_config_res.is_err() { + defmt::error!("gyro configration failed."); + } + + // configure the gyro, map int to int pin 1 + let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance, + AccelRange::Range2g, + Bandwidth3DbCutoffFreq::AccOdrOver2, + OutputDataRate::Odr6400p0, + DataAveragingWindow::Average64Samples).await; + imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1).await; + + if acc_config_res.is_err() { + defmt::error!("accel configration failed."); + } + + // configure the phys properties of the int pins + imu.set_int1_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::OpenDrain).await; + imu.set_int2_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::OpenDrain).await; + + // enable gyro int + imu.set_int2_enabled(true).await; 'imu_data_loop: loop { @@ -98,16 +104,17 @@ pub fn start_imu_task( miso: impl Peripheral

> + 'static, txdma: impl Peripheral

+ 'static, rxdma: impl Peripheral

+ 'static, - accel_cs: ExtImuSpiNss1Pin, - gyro_cs: ExtImuSpiNss2Pin, + bmi323_nss: ImuSpiNss0Pin, + _ext_nss1_pin: ExtImuSpiNss1Pin, + _ext_nss2_pin: ExtImuSpiNss2Pin, accel_int_pin: impl Peripheral

+ 'static, gyro_int_pin: impl Peripheral

+ 'static, accel_int: impl Peripheral

::ExtiChannel> + 'static, gyro_int: impl Peripheral

::ExtiChannel> + 'static, - ext_imu_det_pin: ExtImuNDetPin) { + _ext_imu_det_pin: ExtImuNDetPin) { let imu_buf = IMU_BUFFER_CELL.take(); - let imu = Bmi085::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, accel_cs, gyro_cs, imu_buf); + let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 854363f1..a2fb91fc 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -41,7 +41,8 @@ async fn radio_task_entry( RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin).await.unwrap(); #[allow(unused_labels)] - 'connect_loop: loop { + 'connect_loop: + loop { if radio_ndet.is_high() { defmt::error!("radio appears unplugged."); Timer::after_millis(1000).await; @@ -70,7 +71,8 @@ async fn radio_task_entry( defmt::info!("multicast open"); // TODO add inbound timeout somewhere, maybe not here. - 'process_packets: loop { + 'process_packets: + loop { match select(radio.read_packet(), telemetry_subscriber.next_message()).await { Either::First(res) => { if let Ok(c2_pkt) = res { diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index c949e54a..5eef8926 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -13,33 +13,40 @@ use embassy_stm32::{ time::hz, Peripheral }; -use embassy_time::{Timer, Duration}; +use embassy_time::Timer; -use defmt::*; - - -pub const SPI_MIN_BUF_LEN: usize = 8; +pub const SPI_MIN_BUF_LEN: usize = 14; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro pub struct Bmi323<'a, 'buf, SpiPeri: spi::Instance> { spi: spi::Spi<'a, SpiPeri, Async>, spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], + accel_mode: AccelMode, + accel_range: AccelRange, + accel_odr: OutputDataRate, + accel_bw_mode: Bandwidth3DbCutoffFreq, + accel_avg_window: DataAveragingWindow, + gyro_mode: GyroMode, + gyro_range: GyroRange, + gyro_odr: OutputDataRate, + gyro_bw_mode: Bandwidth3DbCutoffFreq, + gyro_avg_window: DataAveragingWindow, } #[repr(u8)] #[allow(non_camel_case_types, dead_code)] #[derive(Clone, Copy, Debug)] enum ImuRegisters { - ACC_CHIP_ID = 0x00, - ACC_ERR_REG = 0x01, - ACC_STATUS = 0x02, - ACC_X_LSB = 0x03, - ACC_X_MSB = 0x04, - ACC_Y_LSB = 0x05, - ACC_Y_MSB = 0x06, - ACC_Z_LSB = 0x07, - ACC_Z_MSB = 0x08, + CHIP_ID = 0x00, + ERR_REG = 0x01, + STATUS = 0x02, + ACC_DATA_X = 0x03, + ACC_DATA_Y = 0x04, + ACC_DATA_Z = 0x05, + GYR_DATA_X = 0x06, + GYR_DATA_Y = 0x07, + GYR_DATA_Z = 0x08, TEMP_DATA = 0x09, SENSORTIME_0 = 0x0A, SENSORTIME_1 = 0x0B, @@ -107,148 +114,124 @@ enum ImuRegisters { CFG_RES = 0x7F, } -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum AccelRange { -// Range2g = 0x00, -// Range4g = 0x01, -// Range8g = 0x02, -// Range16g = 0x03, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum AccelConfBwp { -// OverSampling4Fold = 0x08, -// OverSampling2Fold = 0x09, -// OverSamplingNormal = 0x0A, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum AccelConfOdr { -// OutputDataRate12p5 = 0x05, -// OutputDataRate25p0 = 0x06, -// OutputDataRate50p0 = 0x07, -// OutputDataRate100p0 = 0x08, -// OutputDataRate200p0 = 0x09, -// OutputDataRate400p0 = 0x0A, -// OutputDataRate800p0 = 0x0B, -// OutputDataRate1600p0 = 0x0C, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// enum AccelSelfTestReg { -// Off = 0x00, -// PositiveSelfTest = 0x0D, -// NegativeSelfTest = 0x09, -// } - -// const ACCEL_CHIP_ID: u8 = 0x1F; - -// #[repr(u8)] -// #[allow(non_camel_case_types, dead_code)] -// #[derive(Clone, Copy, Debug)] -// enum GyroRegisters { -// GYRO_CHIP_ID = 0x00, -// RATE_X_LSB = 0x02, -// RATE_X_MSB = 0x03, -// RATE_Y_LSB = 0x04, -// RATE_Y_MSB = 0x05, -// RATE_Z_LSB = 0x06, -// RATE_Z_MSB = 0x07, -// GYRO_INT_STAT_1 = 0x0A, -// GYRO_RANGE = 0x0F, -// GYRO_BANDWIDTH = 0x10, -// GYRO_LPM1 = 0x11, -// GYRO_SOFTRESET = 0x14, -// GYRO_INT_CONTROL = 0x15, -// INT3_INT4_IO_CONF = 0x16, -// INT3_INT4_IO_MAP = 0x18, -// GYRO_SELF_TEST = 0x3C, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// enum GyroSelfTestReg { -// RateOk = 0b10000, -// BistFail = 0b00100, -// BistRdy = 0b00010, -// TrigBist = 0b00001, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum GyroRange { -// PlusMinus2000DegPerSec = 0x00, -// PlusMinus1000DegPerSec = 0x01, -// PlusMinus500DegPerSec = 0x02, -// PlusMinus250DegPerSec = 0x03, -// PlusMinus125DegPerSec = 0x04, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum GyroBandwidth { -// FilterBw532Hz = 0x00, -// FilterBw230Hz = 0x01, -// FilterBw116Hz = 0x02, -// FilterBw47Hz = 0x03, -// FilterBw23Hz = 0x04, -// FilterBw12Hz = 0x05, -// FilterBw64Hz = 0x06, -// FilterBw32Hz = 0x07, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// enum GyroIntCtrl { -// InterruptOff = 0x00, -// InterruptOnNewData = 0x80, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum GyroIntPinMode { -// PushPull = 0b0, -// OpenDrain = 0b1, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum GyroIntPinActiveState { -// ActiveLow = 0b0, -// ActiveHigh = 0b1, -// } - -// #[repr(u8)] -// #[allow(dead_code)] -// #[derive(Clone, Copy, Debug)] -// pub enum GyroIntMap { -// NotMapped = 0x00, -// Int3 = 0x01, -// Int4 = 0x80, -// Int3AndInt4 = 0x81, -// } - -// const GYRO_CHIP_ID: u8 = 0x0F; +#[repr(u16)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum OutputDataRate { + Odr0p78125 = 0x01, + Odr1p5625 = 0x02, + Odr3p125 = 0x03, + Odr6p25 = 0x04, + Odr12p5 = 0x05, + Odr25p0 = 0x06, + Odr50p0 = 0x07, + Odr100p0 = 0x08, + Odr200p0 = 0x09, + Odr400p0 = 0x0A, + Odr800p0 = 0x0B, + Odr1600p0 = 0x0C, + Odr3200p0 = 0x0D, + Odr6400p0 = 0x0E, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum DataAveragingWindow { + NoFiltering = 0x0, + Average2Samples = 0x1, + Average4Samples = 0x2, + Average8Samples = 0x3, + Average16Samples = 0x4, + Average32Samples = 0x5, + Average64Samples = 0x6, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum Bandwidth3DbCutoffFreq { + AccOdrOver2 = 0x0, + AccOdrOver4 = 0x1, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum IntPinLevel { + ActiveLow = 0x0, + ActiveHigh = 0x1, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum IntPinDriveMode { + PushPull = 0x0, + OpenDrain = 0x1, +} + + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum AccelRange { + Range2g = 0x00, + Range4g = 0x01, + Range8g = 0x02, + Range16g = 0x03, +} +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum AccelMode { + Disabled = 0x0, + DutyCycling = 0x3, + ReducedCurrent = 0x4, + ContinuousHighPerformance = 0x7, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum InterruptMode { + Disabled = 0x0, + MappedToInt1 = 0x1, + MappedToInt2 = 0x2, + MappedToI3cIbi = 0x3, +} + + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroRange { + PlusMinus125DegPerSec = 0x0, + PlusMinus250DegPerSec = 0x1, + PlusMinus500DegPerSec = 0x2, + PlusMinus1000DegPerSec = 0x3, + PlusMinus2000DegPerSec = 0x4, +} + +#[repr(u8)] +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +pub enum GyroMode { + Disabled = 0x0, + DisabledGyroDriveEnabled = 0x1, + DutyCycling = 0x3, + ContinuousReducedCurrent = 0x4, + ContinuousHighPerformance = 0x7, +} + + +const BMI323_CHIP_ID: u16 = 0x0043; const READ_BIT: u8 = 0x80; impl<'a, 'buf, SpiPeri: spi::Instance> Bmi323<'a, 'buf, SpiPeri> { - /// creates a new BMI085 instance from a pre-existing Spi peripheral + /// creates a new BMI323 instance from a pre-existing Spi peripheral pub fn new_from_spi( spi: spi::Spi<'a, SpiPeri, Async>, spi_cs: Output<'a>, @@ -256,8 +239,18 @@ impl<'a, 'buf, SpiPeri: spi::Instance> ) -> Self { Bmi323 { spi: spi, - spi_cs: accel_cs, + spi_cs: spi_cs, spi_buf: spi_buf, + accel_mode: AccelMode::Disabled, + accel_range: AccelRange::Range8g, + accel_bw_mode: Bandwidth3DbCutoffFreq::AccOdrOver2, + accel_odr: OutputDataRate::Odr100p0, + accel_avg_window: DataAveragingWindow::NoFiltering, + gyro_mode: GyroMode::Disabled, + gyro_range: GyroRange::PlusMinus2000DegPerSec, + gyro_bw_mode: Bandwidth3DbCutoffFreq::AccOdrOver2, + gyro_odr: OutputDataRate::Odr100p0, + gyro_avg_window: DataAveragingWindow::NoFiltering, } } @@ -291,7 +284,17 @@ impl<'a, 'buf, SpiPeri: spi::Instance> Bmi323 { spi: imu_spi, spi_cs: spi_cs, - spi_buf: spi_buf + spi_buf: spi_buf, + accel_mode: AccelMode::Disabled, + accel_range: AccelRange::Range8g, + accel_bw_mode: Bandwidth3DbCutoffFreq::AccOdrOver2, + accel_odr: OutputDataRate::Odr100p0, + accel_avg_window: DataAveragingWindow::NoFiltering, + gyro_mode: GyroMode::Disabled, + gyro_range: GyroRange::PlusMinus2000DegPerSec, + gyro_bw_mode: Bandwidth3DbCutoffFreq::AccOdrOver2, + gyro_odr: OutputDataRate::Odr100p0, + gyro_avg_window: DataAveragingWindow::NoFiltering, } } @@ -303,11 +306,11 @@ impl<'a, 'buf, SpiPeri: spi::Instance> self.spi_cs.set_high(); } - async fn read(&mut self, reg: u8) -> u16 { + async fn read(&mut self, reg: ImuRegisters) -> u16 { // addresses are 7 bits with MSB flagging read/!write // data is 16 bits - self.spi_buf[0] = reg | READ_BIT; + self.spi_buf[0] = (reg as u8) | READ_BIT; self.spi_buf[1] = 0x00; let _ = self.spi.transfer_in_place(&mut self.spi_buf[..4]).await; @@ -324,122 +327,69 @@ impl<'a, 'buf, SpiPeri: spi::Instance> self.spi_buf[1] = 0x00; let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; - for dword in dest.iter_mut() { - dword = + for (i, dword) in dest.iter_mut().enumerate() { + *dword = (self.spi_buf[(i * 2) + 2 + 1] as u16) << 8 | self.spi_buf[(i * 2) + 2 + 0] as u16; } - dest.copy_from_slice(&self.spi_buf[2..trx_len]); } - async fn write(&mut self, reg: u8, val: u8) { - self.spi_buf[0] = reg & !READ_BIT; - self.spi_buf[1] = val; + async fn write(&mut self, reg: ImuRegisters, val: u16) { + self.spi_buf[0] = reg as u8 & !READ_BIT; + self.spi_buf[1] = (val & 0x00FF) as u8; + self.spi_buf[2] = ((val & 0xFF00) >> 8) as u8; let _ = self.spi.transfer_in_place(&mut self.spi_buf[..2]).await; } - async fn burst_write(&mut self, reg: ImuRegisters, dest: &mut [u16]) { + #[allow(dead_code)] + async fn burst_write(&mut self, reg: ImuRegisters, data: &[u16]) { + // the transaction length is either the dest buf size * 2 + 2 + // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) + // OR upper bounded by internal length of the buffer. + let trx_len = min((data.len() * 2) + 1, self.spi_buf.len()); + + self.spi_buf[0] = reg as u8 | READ_BIT; + + for (i, word) in data.iter().enumerate() { + self.spi_buf[(i * 2) + 1 + 0] = (*word & 0x00FF as u16) as u8; + self.spi_buf[(i * 2) + 1 + 1] = ((*word & 0xFF00 as u16) >> 8) as u8; + } + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; } pub async fn init(&mut self) { - // imu accel needs at least one dummy read to set the data mode to SPI from POn I2C + // device needs at least one dummy read to set the data mode to SPI from POn I2C // second read should be valid here for sanity purposes - let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; - let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; + let _ = self.read(ImuRegisters::CHIP_ID).await; + let _ = self.read(ImuRegisters::CHIP_ID).await; } async fn accel_self_test(&mut self) -> Result<(), ()> { - // test is non-trivial, e.g. not self contained - // procedure is described in section 4.6.1 of the datasheet - - self.accel_set_range(AccelRange::Range16g).await; - self.accel_set_bandwidth(AccelConfBwp::OverSamplingNormal, AccelConfOdr::OutputDataRate1600p0).await; - Timer::after(Duration::from_millis(2 + 1)).await; - - self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::PositiveSelfTest as u8).await; - Timer::after(Duration::from_millis(50 + 1)).await; - let positive_test_data = self.accel_get_data_mg().await; - - self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::NegativeSelfTest as u8).await; - Timer::after(Duration::from_millis(50 + 1)).await; - let negative_test_data = self.accel_get_data_mg().await; - - self.accel_write(ImuRegisters::ACC_SELF_TEST, AccelSelfTestReg::Off as u8).await; - Timer::after(Duration::from_millis(50 + 1)).await; - - let x_diff = positive_test_data[0] - negative_test_data[0]; - let y_diff = positive_test_data[1] - negative_test_data[1]; - let z_diff = positive_test_data[2] - negative_test_data[2]; - - let mut result = Ok(()); - if x_diff < 1000.0 { - trace!("accel self test failed x-axis needed polarity difference >=1000 got {}", x_diff); - result = Err(()) - } - - if y_diff < 1000.0 { - trace!("accel self test failed y-axis needed polarity difference >=1000 got {}", y_diff); - result = Err(()) - } + defmt::warn!("BMI323 accel self test unimplemented"); - if z_diff < 500.0 { - trace!("accel self test failed z-axis needed polarity difference >=500 got {}", z_diff); - result = Err(()) - } - - if result.is_err() { - warn!("accel failed BIST. (one or more axis diverged)"); - } - - result + Ok(()) } async fn gyro_self_test(&mut self) -> Result<(), ()> { - self.gyro_write(GyroRegisters::GYRO_SELF_TEST, GyroSelfTestReg::TrigBist as u8).await; - let mut try_ct = 0; - loop { - let strreg_val = self.gyro_read(GyroRegisters::GYRO_SELF_TEST).await; - if (strreg_val & GyroSelfTestReg::BistRdy as u8) != 0 { - if (strreg_val & GyroSelfTestReg::BistFail as u8) != 0 { - warn!("gyro failed BIST. (the bist fail bit is high)"); - return Err(()); - } else { - debug!("gyro passed self test."); - return Ok(()); - } - } - - Timer::after(Duration::from_millis(10)).await; - try_ct += 1; - - if try_ct > 10 { - warn!("gyro BIST did not converge."); - return Err(()); - } - } + defmt::warn!("BMI323 gyro self test unimplemented"); + + Ok(()) } pub async fn self_test(&mut self) -> Result<(), ()> { let mut has_self_test_error = Ok(()); self.deselect(); + Timer::after_millis(10).await; + self.select(); - let _ = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; - let acc_chip_id = self.accel_read(ImuRegisters::ACC_CHIP_ID).await; - if acc_chip_id != ACCEL_CHIP_ID { - warn!("read accel ID (0x{:x}) does not match expected BMI085 accel ID (0x{:x})", acc_chip_id, ACCEL_CHIP_ID); + let _ = self.read(ImuRegisters::CHIP_ID).await; + let chip_id = self.read(ImuRegisters::CHIP_ID).await; + if chip_id != BMI323_CHIP_ID { + defmt::warn!("read IMU ID (0x{:x}) does not match expected BMI323 ID (0x{:x})", chip_id, BMI323_CHIP_ID); has_self_test_error = Err(()); } else { - debug!("accel id verified: 0x{:x}", acc_chip_id); - } - - let _ = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; - let gyro_chip_id = self.gyro_read(GyroRegisters::GYRO_CHIP_ID).await; - if gyro_chip_id != GYRO_CHIP_ID { - warn!("read gyro ID (0x{:x}) does not match expected BMI085 gyro ID (0x{:x})", gyro_chip_id, GYRO_CHIP_ID); - has_self_test_error = Err(()); - } else { - debug!("gyro id verified: 0x{:x}", gyro_chip_id); + defmt::debug!("BMI323 id verified: 0x{:x}", chip_id); } if (self.accel_self_test().await).is_err() { @@ -452,18 +402,12 @@ impl<'a, 'buf, SpiPeri: spi::Instance> has_self_test_error } - - const fn read_pair_to_i16(&self, lsb: u8, msb: u8) -> i16 { - (msb as u16 * 256 + lsb as u16) as i16 - } pub async fn accel_get_raw_data(&mut self) -> [i16; 3] { - let mut buf: [u8; 6] = [0; 6]; - self.accel_burst_read(ImuRegisters::ACC_X_LSB, &mut buf).await; + let mut buf: [u16; 3] = [0; 3]; + self.burst_read(ImuRegisters::ACC_DATA_X, &mut buf).await; - [self.read_pair_to_i16(buf[0], buf[1]), - self.read_pair_to_i16(buf[2], buf[3]), - self.read_pair_to_i16(buf[4], buf[5])] + [buf[0] as i16, buf[1] as i16, buf[2] as i16] } pub fn accel_range_mg(&self) -> f32 { @@ -501,23 +445,45 @@ impl<'a, 'buf, SpiPeri: spi::Instance> self.convert_accel_raw_sample_mps(raw_data[2])] } - pub async fn accel_set_bandwidth(&mut self, oversampling_mode: AccelConfBwp, output_data_rate: AccelConfOdr) { - self.accel_write(ImuRegisters::ACC_CONF, (oversampling_mode as u8) << 4 | output_data_rate as u8 ).await; + pub async fn apply_accel_config(&mut self) -> Result<(), ()> { + let mut new_acc_conf_reg_val: u16 = 0x0000; + new_acc_conf_reg_val |= (self.accel_mode as u16 & 0x07) << 12; + new_acc_conf_reg_val |= (self.accel_avg_window as u16 & 0x07) << 8; + new_acc_conf_reg_val |= (self.accel_bw_mode as u16 & 0x01) << 7; + new_acc_conf_reg_val |= (self.accel_range as u16 & 0x07) << 4; + new_acc_conf_reg_val |= (self.accel_odr as u16 & 0x0F) << 0; + + self.write(ImuRegisters::ACC_CONF, new_acc_conf_reg_val).await; + + let err_reg_val = self.read(ImuRegisters::ERR_REG).await; + if err_reg_val & 0x0020 != 0 { + defmt::error!("BMI323 accel config is invalid. Accel may be inop."); + return Err(()) + } + + Ok(()) } - pub async fn accel_set_range(&mut self, range: AccelRange) { - self.accel_write(ImuRegisters::ACC_RANGE, range as u8).await; + pub async fn set_accel_config(&mut self, + accel_mode: AccelMode, + accel_range: AccelRange, + accel_bw_mode: Bandwidth3DbCutoffFreq, + accel_odr: OutputDataRate, + accel_avg_window: DataAveragingWindow) -> Result<(), ()> { + self.accel_mode = accel_mode; + self.accel_range = accel_range; + self.accel_bw_mode = accel_bw_mode; + self.accel_odr = accel_odr; + self.accel_avg_window = accel_avg_window; - self.accel_range = range; + self.apply_accel_config().await } pub async fn gyro_get_raw_data(&mut self) -> [i16; 3] { - let mut buf: [u8; 6] = [0; 6]; - self.gyro_burst_read(GyroRegisters::RATE_X_LSB, &mut buf).await; + let mut buf: [u16; 3] = [0; 3]; + self.burst_read(ImuRegisters::GYR_DATA_X, &mut buf).await; - [self.read_pair_to_i16(buf[0], buf[1]), - self.read_pair_to_i16(buf[2], buf[3]), - self.read_pair_to_i16(buf[4], buf[5])] + [buf[0] as i16, buf[1] as i16, buf[2] as i16] } pub fn dps_to_rads(dps: f32) -> f32 { @@ -568,34 +534,109 @@ impl<'a, 'buf, SpiPeri: spi::Instance> self.convert_raw_gyro_sample_rads(raw_data[2])] } - pub async fn gyro_set_range(&mut self, range: GyroRange) { - self.gyro_write(GyroRegisters::GYRO_RANGE, range as u8).await; + pub async fn apply_gyro_config(&mut self) -> Result<(), ()> { + let mut new_gyr_conf_reg_val: u16 = 0x0000; + new_gyr_conf_reg_val |= (self.gyro_mode as u16 & 0x07) << 12; + new_gyr_conf_reg_val |= (self.gyro_avg_window as u16 & 0x07) << 8; + new_gyr_conf_reg_val |= (self.gyro_bw_mode as u16 & 0x01) << 7; + new_gyr_conf_reg_val |= (self.gyro_range as u16 & 0x07) << 4; + new_gyr_conf_reg_val |= (self.gyro_odr as u16 & 0x0F) << 0; + + self.write(ImuRegisters::GYR_CONF, new_gyr_conf_reg_val).await; + + let err_reg_val = self.read(ImuRegisters::ERR_REG).await; + if err_reg_val & 0x0040 != 0 { + defmt::error!("BMI323 gyro config is invalid. Gyro may be inop."); + return Err(()) + } + + Ok(()) + } + + pub async fn set_gyro_config(&mut self, + gyro_mode: GyroMode, + gyro_range: GyroRange, + gyro_bw_mode: Bandwidth3DbCutoffFreq, + gyro_odr: OutputDataRate, + gyro_avg_window: DataAveragingWindow) -> Result<(), ()> { + self.gyro_mode = gyro_mode; + self.gyro_range = gyro_range; + self.gyro_bw_mode = gyro_bw_mode; + self.gyro_odr = gyro_odr; + self.gyro_avg_window = gyro_avg_window; + + self.apply_gyro_config().await + } + + pub async fn set_gyro_interrupt_mode(&mut self, int_mode: InterruptMode) { + // read the current int map 2 register + let mut int_map2_reg_val = self.read(ImuRegisters::INT_MAP2).await; + + // clear the mapping for gyro int mode + int_map2_reg_val &= !(0x3 << 8); + // set the new mapping for gyro + int_map2_reg_val |= (int_mode as u16 & 0x03) << 8; - self.gyro_range = range; + // write the modified register value + self.write(ImuRegisters::INT_MAP2, int_map2_reg_val).await; } - pub async fn gyro_set_bandwidth(&mut self, bandwidth: GyroBandwidth) { - self.gyro_write(GyroRegisters::GYRO_BANDWIDTH, bandwidth as u8).await; + pub async fn set_accel_interrupt_mode(&mut self, int_mode: InterruptMode) { + // read the current int map 2 register + let mut int_map2_reg_val = self.read(ImuRegisters::INT_MAP2).await; + + // clear the mapping for accel int mode + int_map2_reg_val &= !(0x3 << 10); + // set the new mapping for accel + int_map2_reg_val |= (int_mode as u16 & 0x03) << 10; + + // write the modified register value + self.write(ImuRegisters::INT_MAP2, int_map2_reg_val).await; } - pub async fn gyro_enable_interrupts(&mut self) { - self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOnNewData as u8).await; + pub async fn set_int1_pin_config(&mut self, pin_level: IntPinLevel, pin_drive_mode: IntPinDriveMode) { + let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; + + // clear the config for int1 pin + io_int_ctrl_reg_val &= !(0x3); + // set the new mapping for accel + io_int_ctrl_reg_val |= pin_level as u16 & 0x1; + io_int_ctrl_reg_val |= (pin_drive_mode as u16 & 0x1) << 1; + + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; } - pub async fn gyro_disable_interrupts(&mut self) { - self.gyro_write(GyroRegisters::GYRO_INT_CONTROL, GyroIntCtrl::InterruptOff as u8).await; + pub async fn set_int2_pin_config(&mut self, pin_level: IntPinLevel, pin_drive_mode: IntPinDriveMode) { + let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; + + // clear the config for int1 pin + io_int_ctrl_reg_val &= !(0x3 << 8); + // set the new mapping for accel + io_int_ctrl_reg_val |= (pin_level as u16 & 0x1) << 8; + io_int_ctrl_reg_val |= (pin_drive_mode as u16 & 0x1) << 9; + + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; } - pub async fn gyro_set_int_config(&mut self, - int3_active_state: GyroIntPinActiveState, - int3_mode: GyroIntPinMode, - int4_active_state: GyroIntPinActiveState, - int4_mode: GyroIntPinMode) { - let reg_val = (int4_mode as u8) << 3 | (int4_active_state as u8) << 2 | (int3_mode as u8) << 1 | int3_active_state as u8; - self.gyro_write(GyroRegisters::INT3_INT4_IO_CONF, reg_val).await; + pub async fn set_int1_enabled(&mut self, enabled: bool) { + let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; + + // clear the config for int1 pin + io_int_ctrl_reg_val &= !(0x1 << 2); + // set the new mapping for accel + io_int_ctrl_reg_val |= (enabled as u16 & 0x1) << 2; + + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; } - pub async fn gyro_set_int_map(&mut self, map: GyroIntMap) { - self.gyro_write(GyroRegisters::INT3_INT4_IO_MAP, map as u8).await; + pub async fn set_int2_enabled(&mut self, enabled: bool) { + let mut io_int_ctrl_reg_val: u16 = self.read(ImuRegisters::IO_INT_CTRL).await; + + // clear the config for int1 pin + io_int_ctrl_reg_val &= !(0x1 << 10); + // set the new mapping for accel + io_int_ctrl_reg_val |= (enabled as u16 & 0x1) << 10; + + self.write(ImuRegisters::IO_INT_CTRL, io_int_ctrl_reg_val).await; } } \ No newline at end of file diff --git a/software-communication b/software-communication index cd5205b0..12e47895 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit cd5205b04b4e781a408ba7009eb0e750482fa3f6 +Subproject commit 12e4789593a4087b3cc4c130ffbed79322e1f710 From 6df0874efd974533749bcf89201cd2be553f7f1b Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 21:34:49 -0400 Subject: [PATCH 030/157] add radio test fw image --- control-board/src/bin/hwtest-radio/main.rs | 252 ++++++++++----------- control-board/src/tasks/radio_task.rs | 6 + software-communication | 2 +- 3 files changed, 123 insertions(+), 137 deletions(-) diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index ad35252a..42d4857e 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -1,155 +1,135 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] -#![feature(async_closure)] -#![feature(const_mut_refs)] -#![feature(ptr_metadata)] - -use defmt::*; -use defmt_rtt as _; -use panic_probe as _; - -use static_cell::StaticCell; +use ateam_common_packets::{bindings_radio::{BasicTelemetry, ParameterCommandCode}, radio::{DataPacket, TelemetryPacket}}; +use embassy_executor::InterruptExecutor; +use embassy_futures::select::{self, Either}; use embassy_stm32::{ - self as _, - executor::InterruptExecutor, - interrupt::{self, InterruptExt}, - peripherals::{DMA1_CH0, DMA1_CH1, USART2}, - usart::{self, Uart}, - time::mhz, gpio::{Input, Pull} + interrupt, + pac::Interrupt }; -use embassy_time::{Duration, Timer}; +use embassy_sync::pubsub::{PubSubChannel, WaitResult}; -use ateam_control_board::drivers::radio::{RobotRadio, TeamColor, WifiNetwork}; -use ateam_control_board::queue; -use ateam_control_board::uart_queue::{UartReadQueue, UartWriteQueue}; +use defmt_rtt as _; -use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::DataPacket}; +use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, tasks::radio_task::start_radio_task}; -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); +// load credentials from correct crate +#[cfg(not(feature = "no-private-credentials"))] +use credentials::private_credentials::wifi::wifi_credentials; +#[cfg(feature = "no-private-credentials")] +use credentials::public_credentials::wifi::wifi_credentials; + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; + +static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); -#[link_section = ".axisram.buffers"] -static mut BUFFERS_TX: [queue::Buffer<256>; 10] = [queue::Buffer::EMPTY; 10]; -static QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BUFFERS_TX }); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); -#[link_section = ".axisram.buffers"] -static mut BUFFERS_RX: [queue::Buffer<256>; 20] = [queue::Buffer::EMPTY; 20]; -static QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BUFFERS_RX }); +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} #[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - info!("Startup"); - - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - - // Delay so dotstar can turn on - Timer::after(Duration::from_millis(50)).await; - - let config = usart::Config::default(); - let int = interrupt::take!(USART2); - let usart = Uart::new(p.USART2, p.PD6, p.PD5, int, p.DMA1_CH0, p.DMA1_CH1, config); - - let (tx, rx) = usart.split(); - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - info!("start"); - - spawner.spawn(QUEUE_RX.spawn_task(rx)).unwrap(); - spawner.spawn(QUEUE_TX.spawn_task(tx)).unwrap(); - - // let reset = p.PC0; - let reset = p.PA3; - let mut radio = RobotRadio::new(&QUEUE_RX, &QUEUE_TX, reset).await.unwrap(); - - - ///////////////////// - // Dip Switch Inputs - ///////////////////// - let dip5 = Input::new(p.PG3, Pull::Down); - let dip6 = Input::new(p.PG4, Pull::Down); - - let wifi_network = if dip5.is_high() & dip6.is_high() { - WifiNetwork::Team - } else if dip5.is_low() & dip6.is_high() { - WifiNetwork::CompMain - } else if dip5.is_high() & dip6.is_low() { - WifiNetwork::CompPractice - } else { - WifiNetwork::Team - }; - - info!("radio created"); - radio.connect_to_network(wifi_network).await.unwrap(); - info!("radio connected"); - - radio.open_multicast().await.unwrap(); - info!("multicast open"); +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + //////////////////////// + // setup task pools // + //////////////////////// + + // uart queue executor should be highest priority + // NOTE: maybe this should be all DMA tasks? No computation tasks here + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P6); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + // commands channel + let radio_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); + let mut control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + + // telemetry channel + let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); + + /////////////////// + // start tasks // + /////////////////// + + let wifi_network = wifi_credentials[0]; + start_radio_task( + uart_queue_spawner, main_spawner, + radio_command_publisher, radio_telemetry_subscriber, + wifi_network, + p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, + p.DMA2_CH1, p.DMA2_CH0, + p.PC13, p.PE4); loop { - info!("sending hello"); - radio.send_hello(0, TeamColor::Yellow).await.unwrap(); - let hello = radio.wait_hello(Duration::from_millis(1000)).await; - - match hello { - Ok(hello) => { - info!( - "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port - ); - radio.close_peer().await.unwrap(); - info!("multicast peer closed"); - radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); - info!("unicast open"); - break; + match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { + Either::First(gyro_data) => { + match gyro_data { + WaitResult::Lagged(amnt) => { + defmt::warn!("receiving control packets lagged by {}", amnt); + } + WaitResult::Message(msg) => { + match msg { + DataPacket::BasicControl(_bc) => { + defmt::info!("got command packet"); + + let basic_telem = BasicTelemetry { + sequence_number: 0, + robot_revision_major: 0, + robot_revision_minor: 0, + battery_level: 0.0, + battery_temperature: 0.0, + _bitfield_align_1: [0; 0], + _bitfield_1: BasicTelemetry::new_bitfield_1( + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ), + motor_0_temperature: 0.0, + motor_1_temperature: 0.0, + motor_2_temperature: 0.0, + motor_3_temperature: 0.0, + motor_4_temperature: 0.0, + kicker_charge_level: 0.0 + }; + + let pkt_wrapped = TelemetryPacket::Basic(basic_telem); + control_telemetry_publisher.publish(pkt_wrapped).await; + + defmt::info!("send basic telem resp"); + } + DataPacket::ParameterCommand(pc) => { + defmt::info!("got parameter packet"); + + let mut param_resp = pc; + param_resp.command_code = ParameterCommandCode::PCC_ACK; + + let wrapped_pkt = TelemetryPacket::ParameterCommandResponse(param_resp); + control_telemetry_publisher.publish(wrapped_pkt).await; + + defmt::info!("sent param resp packet"); + } + } + } + } } - Err(_) => {} - } - } - - let mut seq: u16 = 0; - loop { - let _ = radio - .send_telemetry(BasicTelemetry { - sequence_number: seq, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: 0., - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }) - .await; - info!("send"); - - let data_packet = radio.read_packet().await; - if let Ok(data_packet) = data_packet { - if let DataPacket::BasicControl(control) = data_packet { - info!("received control packet."); - info!("{:?}", defmt::Debug2Format(&control)); - - info!("{}", seq); - seq = (seq + 1) % 10000; + Either::Second(_) => { + defmt::warn!("packet processing timed out."); } } } -} +} \ No newline at end of file diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index a2fb91fc..2d252df4 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -100,6 +100,12 @@ async fn radio_task_entry( defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); } } + TelemetryPacket::ParameterCommandResponse(param_resp) => { + let res = radio.send_parameter_response(param_resp).await; + if res.is_err() { + defmt::warn!("radio task failed to send param resp packet. Is the backing queue too small?") + } + } } } } diff --git a/software-communication b/software-communication index 12e47895..f1517dde 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit 12e4789593a4087b3cc4c130ffbed79322e1f710 +Subproject commit f1517dde41ea5940894d484489b1888623f74f78 From 5a12bfa73829ff9d9ec2a1e0a3d8ee2e7666206c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 22:31:30 -0400 Subject: [PATCH 031/157] switches driver work --- control-board/src/tasks/mod.rs | 1 + lib-stm32/src/drivers/mod.rs | 3 +- lib-stm32/src/drivers/switches/dip.rs | 41 +++++++++++++++++++ lib-stm32/src/drivers/switches/mod.rs | 2 + .../src/drivers/switches/rotary_encoder.rs | 37 +++++++++++++++++ 5 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 lib-stm32/src/drivers/switches/dip.rs create mode 100644 lib-stm32/src/drivers/switches/mod.rs create mode 100644 lib-stm32/src/drivers/switches/rotary_encoder.rs diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index 9d069a8a..6d33279c 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,3 +1,4 @@ pub mod control; +pub mod control_task; pub mod imu_task; pub mod radio_task; \ No newline at end of file diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index 5ef73ea4..47eaa95f 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -1,3 +1,4 @@ pub mod flash; -pub mod imu; \ No newline at end of file +pub mod imu; +pub mod switches; \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs new file mode 100644 index 00000000..79054764 --- /dev/null +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -0,0 +1,41 @@ +use core::{cmp::min, ops::Range}; + +use embassy_stm32::gpio::{AnyPin, Input, Pull}; + + + +pub struct DipSwitch<'a, const PIN_CT: usize> { + inputs: [Input<'a>; PIN_CT], + inversion_map: [bool; PIN_CT] +} + +impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { + pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { + let inputs = pins.map(|pin| Input::new(pin, pull)); + + let inversion_map = if let Some(map) = inversion_map { + map + } else { + [false; PIN_CT] + }; + + DipSwitch { + inputs, + inversion_map + } + } + + pub fn read_pin(&self, ind: usize) -> bool { + let ind = min(ind, PIN_CT); + + if self.inversion_map[ind] { + self.inputs[ind].is_low() + } else { + self.inputs[ind].is_high() + } + } + + pub fn read_block(r: Range) { + + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/mod.rs b/lib-stm32/src/drivers/switches/mod.rs new file mode 100644 index 00000000..4ab7d81f --- /dev/null +++ b/lib-stm32/src/drivers/switches/mod.rs @@ -0,0 +1,2 @@ +pub mod dip; +pub mod rotary_encoder; \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs new file mode 100644 index 00000000..5569ff81 --- /dev/null +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -0,0 +1,37 @@ + +use embassy_stm32::gpio::Input; + +pub struct RotaryEncoder<'a, const PIN_CT: usize> { + pins: &'a [Input<'a>; PIN_CT], + inversion_map: [bool; PIN_CT], +} + +impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { + pub const fn new_from_inputs(inputs: &'a [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + let inversion_map = if let Some(map) = inversion_map { + map + } else { + [false; PIN_CT] + }; + + RotaryEncoder { + pins: inputs, + inversion_map, + } + } + + pub fn read_value(&self) -> u8 { + let mut val: u8 = 0; + for i in 0..self.pins.len() { + let pin_state = if self.inversion_map[i] { + self.pins[i].is_low() + } else { + self.pins[i].is_high() + }; + + val |= (pin_state as u8 & 0x1) << i + } + + return val; + } +} \ No newline at end of file From bc27b352ccd7588dc4fa0b0add5effb19d7f8713 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 May 2024 22:55:57 -0400 Subject: [PATCH 032/157] add switch drivers --- control-board/src/bin/hwtest-radio/main.rs | 3 +-- lib-stm32/src/drivers/switches/dip.rs | 20 +++++++++++++++---- .../src/drivers/switches/rotary_encoder.rs | 11 +++++++--- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 42d4857e..ea937502 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -5,8 +5,7 @@ use ateam_common_packets::{bindings_radio::{BasicTelemetry, ParameterCommandCode use embassy_executor::InterruptExecutor; use embassy_futures::select::{self, Either}; use embassy_stm32::{ - interrupt, - pac::Interrupt + interrupt, pac::Interrupt }; use embassy_sync::pubsub::{PubSubChannel, WaitResult}; diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs index 79054764..7d8e2dfc 100644 --- a/lib-stm32/src/drivers/switches/dip.rs +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -10,9 +10,7 @@ pub struct DipSwitch<'a, const PIN_CT: usize> { } impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { - pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { - let inputs = pins.map(|pin| Input::new(pin, pull)); - + pub const fn new_from_inputs(inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { let inversion_map = if let Some(map) = inversion_map { map } else { @@ -25,6 +23,11 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { } } + pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> DipSwitch<'a, PIN_CT> { + let inputs = pins.map(|pin| Input::new(pin, pull)); + DipSwitch::new_from_inputs(inputs, inversion_map) + } + pub fn read_pin(&self, ind: usize) -> bool { let ind = min(ind, PIN_CT); @@ -35,7 +38,16 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { } } - pub fn read_block(r: Range) { + pub fn read_block(&self, r: &mut Range) -> u8 { + if !r.all(|val| 0 <= val && val < PIN_CT as i32) { + return 0; + } + + let mut val: u8 = 0; + for i in r.enumerate() { + val |= (self.read_pin(i.1 as usize) as u8 & 0x1) << i.0; + } + val } } \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs index 5569ff81..26d512c4 100644 --- a/lib-stm32/src/drivers/switches/rotary_encoder.rs +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -1,13 +1,13 @@ -use embassy_stm32::gpio::Input; +use embassy_stm32::gpio::{AnyPin, Input, Pull}; pub struct RotaryEncoder<'a, const PIN_CT: usize> { - pins: &'a [Input<'a>; PIN_CT], + pins: [Input<'a>; PIN_CT], inversion_map: [bool; PIN_CT], } impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { - pub const fn new_from_inputs(inputs: &'a [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + pub const fn new_from_inputs(inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { let inversion_map = if let Some(map) = inversion_map { map } else { @@ -20,6 +20,11 @@ impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { } } + pub fn new_from_pins(pins: [AnyPin; PIN_CT], pull: Pull, inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + let inputs = pins.map(|pin| Input::new(pin, pull)); + RotaryEncoder::new_from_inputs(inputs, inversion_map) + } + pub fn read_value(&self) -> u8 { let mut val: u8 = 0; for i in 0..self.pins.len() { From d7be5f662bdf8894087b8c0a0434a109a617106a Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 May 2024 18:21:23 -0400 Subject: [PATCH 033/157] control IO firmware work --- control-board/src/drivers/mod.rs | 1 - control-board/src/drivers/rotary.rs | 31 --------- control-board/src/drivers/shell_indicator.rs | 46 +++++++++----- control-board/src/lib.rs | 1 + control-board/src/pins.rs | 8 +-- control-board/src/robot_state.rs | 66 ++++++++++++++++++++ control-board/src/tasks/mod.rs | 3 +- control-board/src/tasks/user_io_task.rs | 51 +++++++++++++++ 8 files changed, 156 insertions(+), 51 deletions(-) delete mode 100644 control-board/src/drivers/rotary.rs create mode 100644 control-board/src/robot_state.rs create mode 100644 control-board/src/tasks/user_io_task.rs diff --git a/control-board/src/drivers/mod.rs b/control-board/src/drivers/mod.rs index 5231e583..42dac1a5 100644 --- a/control-board/src/drivers/mod.rs +++ b/control-board/src/drivers/mod.rs @@ -1,4 +1,3 @@ pub mod kicker; pub mod radio; -pub mod rotary; pub mod shell_indicator; \ No newline at end of file diff --git a/control-board/src/drivers/rotary.rs b/control-board/src/drivers/rotary.rs deleted file mode 100644 index 437af400..00000000 --- a/control-board/src/drivers/rotary.rs +++ /dev/null @@ -1,31 +0,0 @@ -use embassy_stm32::gpio::{Input, Pin, Pull}; - -pub struct Rotary<'a> { - pin0: Input<'a>, - pin1: Input<'a>, - pin2: Input<'a>, - pin3: Input<'a>, -} - -impl<'a> Rotary<'a> { - pub fn new(pin0: impl Pin, pin1: impl Pin, pin2: impl Pin, pin3: impl Pin) -> Self { - let pin0 = Input::new(pin0, Pull::Down); - let pin1 = Input::new(pin1, Pull::Down); - let pin2 = Input::new(pin2, Pull::Down); - let pin3 = Input::new(pin3, Pull::Down); - - Self { - pin0, - pin1, - pin2, - pin3, - } - } - - pub fn read(&self) -> u8 { - (self.pin3.is_high() as u8) << 3 - | (self.pin2.is_high() as u8) << 2 - | (self.pin1.is_high() as u8) << 1 - | (self.pin0.is_high() as u8) - } -} diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index 7aeffd81..7678bdb6 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -1,10 +1,13 @@ +use core::borrow::BorrowMut; + use embassy_stm32::gpio::{Level, Output, Pin, Speed}; pub struct ShellIndicator<'a> { - pin0: Output<'a>, - pin1: Output<'a>, - pin2: Output<'a>, - pin3: Output<'a>, + fr_pin0: Output<'a>, + fl_pin1: Output<'a>, + bl_pin2: Output<'a>, + br_pin3: Output<'a>, + team_pin4: Option>, } impl<'a> ShellIndicator<'a> { @@ -17,20 +20,35 @@ impl<'a> ShellIndicator<'a> { 0x0E, 0x02, 0x0D, 0x01, ]; - pub fn new(pin0: impl Pin, pin1: impl Pin, pin2: impl Pin, pin3: impl Pin) -> Self { + // TODO: refactor pin ordering + pub fn new(fr_pin0: impl Pin, fl_pin1: impl Pin, br_pin2: impl Pin, bl_pin3: impl Pin, team_pin4: Option) -> Self { + let team_pin4: Option> = if let Some(pin4) = team_pin4 { + Some(Output::new(pin4, Level::Low, Speed::Low)) + } else { + None + }; + Self { - pin0: Output::new(pin0, Level::Low, Speed::Low), - pin1: Output::new(pin1, Level::Low, Speed::Low), - pin2: Output::new(pin2, Level::Low, Speed::Low), - pin3: Output::new(pin3, Level::Low, Speed::Low), + fr_pin0: Output::new(fr_pin0, Level::Low, Speed::Low), + fl_pin1: Output::new(fl_pin1, Level::Low, Speed::Low), + bl_pin2: Output::new(br_pin2, Level::Low, Speed::Low), + br_pin3: Output::new(bl_pin3, Level::Low, Speed::Low), + team_pin4: team_pin4, } } - pub fn set(&mut self, robot_id: u8) { + pub fn set(&mut self, robot_id: u8, team_is_blue: bool) { let shell_bits = Self::ID_TO_PATTERN[robot_id as usize]; - self.pin0.set_level((shell_bits & 0x01 == 0).into()); - self.pin1.set_level((shell_bits & 0x02 == 0).into()); - self.pin2.set_level((shell_bits & 0x04 == 0).into()); - self.pin3.set_level((shell_bits & 0x08 == 0).into()); + self.fr_pin0.set_level((shell_bits & 0x01 == 0).into()); + self.fl_pin1.set_level((shell_bits & 0x02 == 0).into()); + self.bl_pin2.set_level((shell_bits & 0x04 == 0).into()); + self.br_pin3.set_level((shell_bits & 0x08 == 0).into()); + if self.team_pin4.is_some() { + if team_is_blue { + self.team_pin4.as_mut().unwrap().set_high(); + } else { + self.team_pin4.as_mut().unwrap().set_low(); + } + } } } diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index e7defe63..371f0fca 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -27,6 +27,7 @@ use embassy_stm32::{ pub mod parameter_interface; pub mod pins; pub mod radio; +pub mod robot_state; pub mod stm32_interface; pub mod stspin_motor; diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index a2cabd8f..6924b568 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -149,10 +149,10 @@ pub type UsrLed1Pin = PF2; pub type UsrLed2Pin = PF1; pub type UsrLed3Pin = PF0; -pub type RobotIdIndicator0FrPin = PD0; -pub type RobotIdIndicator1FfPin = PD1; -pub type RobotIdIndicator2BlPin = PD3; -pub type RobotIdIndicator3BrPin = PD4; +pub type RobotIdIndicator0FlPin = PD0; +pub type RobotIdIndicator1BlPin = PD1; +pub type RobotIdIndicator2BrPin = PD3; +pub type RobotIdIndicator3FrPin = PD4; pub type RobotIdIndicator4TeamIsBluePin = PD14; pub type DotstarSpi = SPI6; diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs new file mode 100644 index 00000000..e1b89b2b --- /dev/null +++ b/control-board/src/robot_state.rs @@ -0,0 +1,66 @@ +use core::sync::atomic::{AtomicBool, AtomicU32, AtomicU8, Ordering}; + +pub struct RobotState { + hw_init_state_valid: AtomicBool, + + hw_robot_id: AtomicU8, + hw_robot_team_is_blue: AtomicBool, + hw_wifi_network_index: AtomicU8, + hw_debug_mode: AtomicBool, + + radio_inop: AtomicBool, + imu_inop: AtomicBool, + kicker_inop: AtomicBool, + wheels_inop: AtomicU8, + dribbler_inop: AtomicBool, + + // systick is 1us which in a u64 gives an uptime of ~500000 yrs, + // 32bit Cortex M has no AtomicU64, so we'll sync on milliseconds + // overflow occurs in ~49.7 days + last_packet_receive_time_ticks_ms: AtomicU32, + radio_connection_ok: AtomicBool, + + battery_pct: AtomicU8, + battery_ok: AtomicBool, +} + +impl RobotState { + pub fn new() -> RobotState { + RobotState { + hw_init_state_valid: AtomicBool::new(false), + hw_robot_id: AtomicU8::new(0), + hw_robot_team_is_blue: AtomicBool::new(false), + hw_wifi_network_index: AtomicU8::new(0), + hw_debug_mode: AtomicBool::new(true), + radio_inop: AtomicBool::new(true), + imu_inop: AtomicBool::new(true), + kicker_inop: AtomicBool::new(true), + wheels_inop: AtomicU8::new(0x0F), + dribbler_inop: AtomicBool::new(true), + last_packet_receive_time_ticks_ms: AtomicU32::new(0), + radio_connection_ok: AtomicBool::new(false), + battery_pct: AtomicU8::new(0), + battery_ok: AtomicBool::new(false), + } + } + + pub fn hw_init_state_valid(&self) -> bool { + self.hw_init_state_valid.load(Ordering::Relaxed) + } + + pub fn set_hw_init_state_valid(&self, hw_init_state_valid: bool) { + self.hw_init_state_valid.store(hw_init_state_valid, Ordering::Relaxed); + } + + pub fn get_hw_robot_id(&self) -> u8 { + self.hw_robot_id.load(Ordering::Relaxed) + } + + pub fn set_hw_robot_id(&self, new_hw_robot_id: u8) { + self.hw_robot_id.store(new_hw_robot_id, Ordering::Relaxed); + } + + pub fn hw_robot_team_is_blue(&self) -> bool { + self.hw_robot_team_is_blue.load(Ordering::Relaxed) + } +} \ No newline at end of file diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index 6d33279c..54194e09 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,4 +1,5 @@ pub mod control; pub mod control_task; pub mod imu_task; -pub mod radio_task; \ No newline at end of file +pub mod radio_task; +pub mod user_io_task; \ No newline at end of file diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs new file mode 100644 index 00000000..78bffde8 --- /dev/null +++ b/control-board/src/tasks/user_io_task.rs @@ -0,0 +1,51 @@ +use ateam_lib_stm32::drivers::switches::dip::DipSwitch; +use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; +use embassy_executor::Spawner; +use embassy_stm32::gpio::{AnyPin, Pull}; + +use crate::drivers::shell_indicator::ShellIndicator; +use crate::robot_state::RobotState; + +use crate::pins::*; + + +#[embassy_executor::task] +async fn user_io_task_entry(robot_state: &'static RobotState, + dip_switch: DipSwitch<'static, 7>, + robot_id_rotary: RotaryEncoder<'static, 4>, + robot_id_indicator: ShellIndicator<'static>, +) { + loop { + // TODO read switches + + // TODO publish updates to robot_state + + // TODO read messages + + // TODO update indicators + } +} + +pub fn start_io_task(spawner: Spawner, + robot_state: &'static RobotState, + usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, + usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, + usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, + robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, + robot_id_selector1_pin: RobotIdSelector1Pin, robot_id_selector0_pin: RobotIdSelector0Pin, + usr_led0_pin: UsrLed0Pin, usr_led1_pin: UsrLed1Pin, usr_led2_pin: UsrLed2Pin, usr_led3_pin: UsrLed3Pin, + robot_id_indicator_fl: RobotIdIndicator0FlPin, robot_id_indicator_bl: RobotIdIndicator1BlPin, + robot_id_indicator_br: RobotIdIndicator2BrPin, robot_id_indicator_fr: RobotIdIndicator3FrPin, + robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, + ) { + + let dip_sw_pins: [AnyPin; 7] = [usr_dip7_pin.into(), usr_dip6_pin.into(), usr_dip5_pin.into(), usr_dip4_pin.into(), usr_dip3_pin.into(), usr_dip2_pin.into(), usr_dip1_pin.into()]; + let dip_switch = DipSwitch::new_from_pins(dip_sw_pins, Pull::None, None); + + let robot_id_selector_pins: [AnyPin; 4] = [robot_id_selector3_pin.into(), robot_id_selector2_pin.into(), robot_id_selector1_pin.into(), robot_id_selector0_pin.into()]; + let robot_id_rotary = RotaryEncoder::new_from_pins(robot_id_selector_pins, Pull::None, None); + + let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); + + spawner.spawn(user_io_task_entry(robot_state, dip_switch, robot_id_rotary, robot_id_indicator)).unwrap(); +} \ No newline at end of file From d63b95548677abb9db0c25dc0d70575d08268de9 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 15 May 2024 00:16:45 -0400 Subject: [PATCH 034/157] firmware progress --- control-board/src/bin/hwtest-radio/main.rs | 20 ++++++- .../src/drivers/radio/at_protocol.rs | 9 +++- control-board/src/drivers/radio/radio.rs | 35 ++++++++++-- .../src/drivers/radio/radio_robot.rs | 53 +++++++++++++++---- control-board/src/lib.rs | 4 +- control-board/src/robot_state.rs | 6 ++- control-board/src/tasks/imu_task.rs | 10 ++-- control-board/src/tasks/radio_task.rs | 48 +++++++++++++---- control-board/src/tasks/user_io_task.rs | 32 +++++++++-- kicker-board/src/bin/hwtest-blinky/main.rs | 23 +++++--- lib-stm32/src/drivers/imu/bmi323.rs | 32 +++++++---- 11 files changed, 216 insertions(+), 56 deletions(-) diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index ea937502..42cd65f0 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -11,7 +11,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, tasks::radio_task::start_radio_task}; +use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::{self, RobotState}, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; // load credentials from correct crate @@ -23,6 +23,9 @@ use credentials::public_credentials::wifi::wifi_credentials; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; +use static_cell::{ConstStaticCell, StaticCell}; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(RobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -42,6 +45,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { defmt::info!("embassy HAL configured."); + let robot_state = ROBOT_STATE.take(); + //////////////////////// // setup task pools // //////////////////////// @@ -70,12 +75,25 @@ async fn main(main_spawner: embassy_executor::Spawner) { let wifi_network = wifi_credentials[0]; start_radio_task( uart_queue_spawner, main_spawner, + robot_state, radio_command_publisher, radio_telemetry_subscriber, wifi_network, p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, p.DMA2_CH1, p.DMA2_CH0, p.PC13, p.PE4); + start_io_task(main_spawner, + robot_state, + p.PD6, p.PD5, p.EXTI6, p.EXTI5, + p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, + p.PG12, p.PG11, p.PG10, p.PG9, + p.PF3, p.PF2, p.PF1, p.PF0, + p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); + + loop { + Timer::after_millis(1000).await; + } + loop { match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { Either::First(gyro_data) => { diff --git a/control-board/src/drivers/radio/at_protocol.rs b/control-board/src/drivers/radio/at_protocol.rs index be7a21a6..f7415424 100644 --- a/control-board/src/drivers/radio/at_protocol.rs +++ b/control-board/src/drivers/radio/at_protocol.rs @@ -320,10 +320,17 @@ impl<'b> ATResponse<'b> { const RESP_ERROR: &'static str = "ERROR"; pub fn new<'a>(buf: &'a [u8]) -> Result, ()> { + defmt::trace!("new AT Response: {}", buf); let s = core::str::from_utf8(buf).or(Err(()))?; + + defmt::trace!("searching for CR_LF: {}", s); let i_echo = s.find(Self::CR_LF).ok_or(())?; - let _echo = &s[..i_echo]; + defmt::trace!("i_echo: {}", i_echo); + //let _echo = &s[..i_echo]; + //defmt::trace!("_echo: {}", _echo) + let s = &s[i_echo + Self::CR_LF.len()..]; + defmt::trace!("s: {}", s); if !s.ends_with(Self::CR_LF) { return Err(()); } diff --git a/control-board/src/drivers/radio/radio.rs b/control-board/src/drivers/radio/radio.rs index 45c7a1a0..8c91f99f 100644 --- a/control-board/src/drivers/radio/radio.rs +++ b/control-board/src/drivers/radio/radio.rs @@ -87,6 +87,7 @@ impl< pub async fn wait_startup(&self) -> Result<(), ()> { self.reader .dequeue(|buf| { + defmt::info!("dequeueing"); if let EdmPacket::ATResponse(ATResponse::Other("+STARTUP")) = self.to_packet(buf)? { Ok(()) } else { @@ -145,11 +146,20 @@ impl< Ok(()) } - pub async fn enter_edm(&mut self) -> Result<(), ()> { - self.send_command("ATO2").await?; - self.read_ok().await?; + pub async fn enter_edm(&mut self) -> Result { + if self.send_command("ATO2").await.is_err() { + return Err(()); + } + + // TODO this is getting CR LF O K CR LF [EDM START, 0, 2, 0, 113, EDM END] + // 0, 2 decodes as payload length 2 + // 0 113 decodes as payload event EDM_START + // this appear valid, we just captured two events at once. + // write a new function to handle this + // self.read_ok().await?; + let res = self.read_ok_at_edm_transition().await; self.mode = RadioMode::ExtendedDataMode; - Ok(()) + return res; } pub async fn set_host_name(&self, host_name: &str) -> Result<(), ()> { @@ -416,6 +426,23 @@ impl< .await } + async fn read_ok_at_edm_transition(&self) -> Result { + let transition_buf: [u8; 12] = [13, 10, 79, 75, 13, 10, 170, 0, 2, 0, 113, 85]; + let res = self.reader.dequeue(|buf| { + if buf.iter().zip(transition_buf.iter()).all(|(a,b)| a == b) { + Ok(true) + } else { + if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { + Ok(false) + } else { + Err(()) + } + } + }).await; + + return res; + } + fn to_packet<'b>(&self, buf: &'b [u8]) -> Result, ()> { match self.mode { RadioMode::CommandMode => Ok(EdmPacket::ATResponse(ATResponse::new(buf)?)), diff --git a/control-board/src/drivers/radio/radio_robot.rs b/control-board/src/drivers/radio/radio_robot.rs index 30953535..985a2071 100644 --- a/control-board/src/drivers/radio/radio_robot.rs +++ b/control-board/src/drivers/radio/radio_robot.rs @@ -10,7 +10,7 @@ use core::fmt::Write; use core::mem::size_of; use embassy_futures::select::{select, Either}; use embassy_stm32::gpio::{Level, Pin, Speed, Output}; -use embassy_stm32::usart; +use embassy_stm32::usart::{self, DataBits, StopBits}; use embassy_time::{Duration, Timer}; use heapless::String; @@ -94,20 +94,19 @@ impl< const DEPTH_RX: usize, > RobotRadio<'a, UART, DmaRx, DmaTx, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { - pub async fn new( + pub fn new( read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, reset_pin: impl Pin, - ) -> Result, ()> - { + ) -> RobotRadio<'a, UART, DmaRx, DmaTx, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { let reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); let radio = Radio::new(read_queue, write_queue); - Ok(Self { + Self { radio, reset_pin, peer: None, - }) + } } pub async fn connect_uart(&mut self) -> Result<(), ()> { @@ -115,23 +114,55 @@ impl< Timer::after(Duration::from_micros(100)).await; self.reset_pin.set_low(); - self.radio.wait_startup().await?; + if self.radio.wait_startup().await.is_err() { + defmt::debug!("error processing radio wait startup command"); + return Err(()) + } + + defmt::info!("increasing link speed"); let baudrate = 5_250_000; - self.radio.set_echo(false).await?; - self.radio.config_uart(baudrate, false, 8, true).await?; + if self.radio.set_echo(false).await.is_err() { + defmt::debug!("error disabling echo on radio"); + return Err(()); + } + + if self.radio.config_uart(baudrate, false, 8, true).await.is_err() { + defmt::debug!("error increasing radio baud rate."); + return Err(()) + } + defmt::info!("configured radio link speed"); + let mut radio_uart_config = usart::Config::default(); radio_uart_config.baudrate = 5_250_000; + radio_uart_config.stop_bits = StopBits::STOP1; + radio_uart_config.data_bits = DataBits::DataBits9; radio_uart_config.parity = usart::Parity::ParityEven; self.radio.update_uart_config(radio_uart_config).await; + defmt::info!("configured host link speed"); + // Datasheet says wait at least 40ms after UART config change Timer::after(Duration::from_millis(50)).await; // Datasheet says wait at least 50ms after entering data mode - self.radio.enter_edm().await?; - self.radio.wait_edm_startup().await?; + if let Ok(got_edm_startup) = self.radio.enter_edm().await { + defmt::info!("entered edm at high link speed"); + + if ! got_edm_startup { + if self.radio.wait_edm_startup().await.is_err() { + defmt::debug!("error waiting for EDM startup after uart baudrate increase"); + return Err(()); + } else { + defmt::info!("got EDM startup command"); + } + } + } else { + defmt::debug!("error entering EDM mode after uart baudrate increase"); + return Err(()) + } + Timer::after(Duration::from_millis(50)).await; Ok(()) diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 371f0fca..a224229e 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -26,7 +26,7 @@ use embassy_stm32::{ pub mod parameter_interface; pub mod pins; -pub mod radio; +// pub mod radio; pub mod robot_state; pub mod stm32_interface; pub mod stspin_motor; @@ -106,7 +106,7 @@ pub fn get_system_config() -> Config { config.rcc.pll1 = Some(Pll { source: PllSource::HSE, prediv: PllPreDiv::DIV1, - mul: PllMul::MUL68, + mul: PllMul::MUL64, // PllMul::MUL68, divp: Some(PllDiv::DIV1), // 544 MHz divq: Some(PllDiv::DIV4), // 136 MHz divr: Some(PllDiv::DIV2) // 272 MHz diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index e1b89b2b..a8d2e4fb 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -25,7 +25,7 @@ pub struct RobotState { } impl RobotState { - pub fn new() -> RobotState { + pub const fn new() -> RobotState { RobotState { hw_init_state_valid: AtomicBool::new(false), hw_robot_id: AtomicU8::new(0), @@ -63,4 +63,8 @@ impl RobotState { pub fn hw_robot_team_is_blue(&self) -> bool { self.hw_robot_team_is_blue.load(Ordering::Relaxed) } + + pub fn set_hw_robot_team_is_blue(&self, is_blue: bool) { + self.hw_robot_team_is_blue.store(is_blue, Ordering::Relaxed); + } } \ No newline at end of file diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index c5bf7093..f56dfd06 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -62,12 +62,12 @@ async fn imu_task_entry( } // configure the phys properties of the int pins - imu.set_int1_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::OpenDrain).await; - imu.set_int2_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::OpenDrain).await; + imu.set_int1_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull).await; + imu.set_int2_pin_config(IntPinLevel::ActiveLow, IntPinDriveMode::PushPull).await; // enable gyro int imu.set_int2_enabled(true).await; - + 'imu_data_loop: loop { // block on gyro interrupt, active low @@ -118,8 +118,8 @@ pub fn start_imu_task( // IMU breakout INT2 is directly connected to the MCU with no hardware PU/PD. Select software Pull::Up and // imu open drain - let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::Down); - let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::Up); + let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::None); + let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::None); imu_task_spawner.spawn(imu_task_entry(accel_data_publisher, gyro_data_publisher, imu, accel_int, gyro_int)).unwrap(); } diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 2d252df4..fcefb4c1 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -6,12 +6,12 @@ use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ gpio::{Input, Pull}, - usart::{self, Uart} + usart::{self, DataBits, Parity, StopBits, Uart} }; use embassy_sync::pubsub::WaitResult; use embassy_time::Timer; -use crate::{drivers::radio::RobotRadio, pins::*, SystemIrqs}; +use crate::{drivers::radio::RobotRadio, pins::*, robot_state::RobotState, SystemIrqs}; pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -27,6 +27,7 @@ make_uart_queues!(RADIO, #[embassy_executor::task] async fn radio_task_entry( + robot_state: &'static RobotState, command_publisher: CommandsPublisher, mut telemetry_subscriber: TelemetrySubcriber, wifi_network: WifiCredential, @@ -38,24 +39,42 @@ async fn radio_task_entry( let radio_ndet = Input::new(radio_ndet_pin, Pull::None); let mut radio: RobotRadio<'static, RadioUART, RadioRxDMA, RadioTxDMA, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH> = - RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin).await.unwrap(); + RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin); + + let mut startup_radio_uart_config = usart::Config::default(); + startup_radio_uart_config.baudrate = 115_200; + startup_radio_uart_config.data_bits = DataBits::DataBits8; + startup_radio_uart_config.stop_bits = StopBits::STOP1; + startup_radio_uart_config.parity = Parity::ParityNone; #[allow(unused_labels)] 'connect_loop: loop { + RADIO_TX_UART_QUEUE.update_uart_config(startup_radio_uart_config).await; + // TODO fix me this doesn't block + if radio_ndet.is_high() { defmt::error!("radio appears unplugged."); Timer::after_millis(1000).await; continue 'connect_loop; } - let res = radio.connect_uart().await; - if res.is_err() { - defmt::error!("failed to establish radio UART connection."); - Timer::after_millis(1000).await; - continue 'connect_loop; + defmt::info!("connecting radio uart"); + match select(radio.connect_uart(), Timer::after_millis(10000)).await { + Either::First(res) => { + if res.is_err() { + defmt::error!("failed to establish radio UART connection."); + Timer::after_millis(1000).await; + continue 'connect_loop; + } else { + defmt::debug!("established radio uart coms"); + } + } + Either::Second(_) => { + defmt::error!("initial radio uart connection timed out"); + continue 'connect_loop; + } } - defmt::info!("established radio uart coms"); while radio.connect_to_network(wifi_network).await.is_err() { defmt::error!("failed to connect to wifi network."); @@ -122,6 +141,7 @@ async fn radio_task_entry( pub fn start_radio_task(radio_task_spawner: SendSpawner, queue_spawner: Spawner, + robot_state: &'static RobotState, command_publisher: CommandsPublisher, telemetry_subscriber: TelemetrySubcriber, wifi_network: WifiCredential, @@ -134,12 +154,18 @@ pub fn start_radio_task(radio_task_spawner: SendSpawner, radio_uart_tx_dma: RadioTxDMA, radio_reset_pin: RadioResetPin, radio_ndet_pin: RadioNDetectPin) { - let radio_uart_config = usart::Config::default(); + + let mut radio_uart_config = usart::Config::default(); + radio_uart_config.baudrate = 115_200; + radio_uart_config.data_bits = DataBits::DataBits8; + radio_uart_config.stop_bits = StopBits::STOP1; + radio_uart_config.parity = Parity::ParityNone; + let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); queue_spawner.spawn(RADIO_RX_UART_QUEUE.spawn_task(radio_uart_rx)).unwrap(); queue_spawner.spawn(RADIO_TX_UART_QUEUE.spawn_task(radio_uart_tx)).unwrap(); - radio_task_spawner.spawn(radio_task_entry(command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); + radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); } \ No newline at end of file diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 78bffde8..7ebdb2d9 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,7 +1,8 @@ use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; -use embassy_executor::Spawner; +use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::gpio::{AnyPin, Pull}; +use embassy_time::Timer; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::RobotState; @@ -13,16 +14,37 @@ use crate::pins::*; async fn user_io_task_entry(robot_state: &'static RobotState, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, - robot_id_indicator: ShellIndicator<'static>, + mut robot_id_indicator: ShellIndicator<'static>, ) { loop { - // TODO read switches + // read switches + let robot_id = robot_id_rotary.read_value(); + let robot_team_isblue = dip_switch.read_pin(6); - // TODO publish updates to robot_state + // publish updates to robot_state + let glob_robot_id = robot_state.get_hw_robot_id(); + let glob_robot_is_blue = robot_state.hw_robot_team_is_blue(); + if robot_id != glob_robot_id { + robot_state.set_hw_robot_id(robot_id); + defmt::debug!("updated robot id {} -> {}", glob_robot_id, robot_id); + } + + if robot_team_isblue != glob_robot_is_blue { + robot_state.set_hw_robot_team_is_blue(robot_team_isblue); + defmt::debug!("updated robot team is blue {} -> {}", glob_robot_is_blue, robot_team_isblue); + } // TODO read messages - // TODO update indicators + // update indicators + robot_id_indicator.set(robot_id, robot_team_isblue); + + if !robot_state.hw_init_state_valid() { + defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); + robot_state.set_hw_init_state_valid(true); + } + + Timer::after_millis(100).await; } } diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 9d8b1c67..461eeb64 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -105,25 +105,36 @@ async fn dotstar_lerp_task(dotstar_spi: DotstarSpi, let mut dotstar_spi_config = Config::default(); dotstar_spi_config.frequency = mhz(1); - let dotstar_spi = Spi::new_txonly( + let mut dotstar_spi = Spi::new_txonly( dotstar_spi, dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, dotstar_spi_config); - let mut dotstar = Apa102::new(dotstar_spi); + // let mut dotstar = Apa102::new(dotstar_spi); + // dotstar.write([RGB8 {r: 10, g: 10, b: 10}, RGB8 {r: 10, g: 10, b: 10}]); + + let mut counting_up: bool = true; let mut val = 0; loop { - let _ = dotstar.write([RGB8 { r: val, g: val, b: val }, RGB8 { r: val / 2, g: val / 2, b: val / 2 }].iter().cloned()); - val += 1; + // let _ = dotstar.write([RGB8 { r: val, g: val, b: val }, RGB8 { r: val / 2, g: val / 2, b: val / 2 }].iter().cloned()); + let _ = dotstar_spi.write(&[0x00 as u8, 0x00, 0x00, 0x00, 0xE7, val, 0x00, val, 0xE7, val, 0x00, val, 0xFF, 0xFF, 0xFF, 0xFF]).await; + + if counting_up { + val += 1; + } else { + val -= 1; + } if val == 255 { - val = 0; + counting_up = false; + } else if val == 0 { + counting_up = true; } - Timer::after_millis(10).await; + Timer::after_millis(5).await; } } diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 5eef8926..6267ef89 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -9,7 +9,7 @@ use core::{cmp::min, f32::consts::PI}; use embassy_stm32::{ gpio::{Level, Output, Pin, Speed}, mode::Async, - spi::{self, MisoPin, MosiPin, SckPin}, + spi::{self, MisoPin, Mode, MosiPin, SckPin}, time::hz, Peripheral }; @@ -267,7 +267,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> ) -> Self { let mut spi_config = spi::Config::default(); // Bmi323 max SPI frequency is 10MHz - spi_config.frequency = hz(10_000_000); + spi_config.frequency = hz(1_000_000); let imu_spi = spi::Spi::new( peri, @@ -307,6 +307,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } async fn read(&mut self, reg: ImuRegisters) -> u16 { + self.select(); // addresses are 7 bits with MSB flagging read/!write // data is 16 bits @@ -314,10 +315,13 @@ impl<'a, 'buf, SpiPeri: spi::Instance> self.spi_buf[1] = 0x00; let _ = self.spi.transfer_in_place(&mut self.spi_buf[..4]).await; - ((self.spi_buf[2] as u16) << 8) | self.spi_buf[3] as u16 + self.deselect(); + + ((self.spi_buf[3] as u16) << 8) | self.spi_buf[2] as u16 } async fn burst_read(&mut self, reg: ImuRegisters, dest: &mut [u16]) { + self.select(); // the transaction length is either the dest buf size * 2 + 2 // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) // OR upper bounded by internal length of the buffer. @@ -330,17 +334,25 @@ impl<'a, 'buf, SpiPeri: spi::Instance> for (i, dword) in dest.iter_mut().enumerate() { *dword = (self.spi_buf[(i * 2) + 2 + 1] as u16) << 8 | self.spi_buf[(i * 2) + 2 + 0] as u16; } + + self.deselect(); } async fn write(&mut self, reg: ImuRegisters, val: u16) { - self.spi_buf[0] = reg as u8 & !READ_BIT; + self.select(); + + self.spi_buf[0] = reg as u8; self.spi_buf[1] = (val & 0x00FF) as u8; self.spi_buf[2] = ((val & 0xFF00) >> 8) as u8; - let _ = self.spi.transfer_in_place(&mut self.spi_buf[..2]).await; + let _ = self.spi.transfer_in_place(&mut self.spi_buf[..3]).await; + + self.deselect(); } #[allow(dead_code)] async fn burst_write(&mut self, reg: ImuRegisters, data: &[u16]) { + self.select(); + // the transaction length is either the dest buf size * 2 + 2 // (the start addr + N data bytes) + 1 spurious byte for data to settle (or something) // OR upper bounded by internal length of the buffer. @@ -354,6 +366,8 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } let _ = self.spi.transfer_in_place(&mut self.spi_buf[..trx_len]).await; + + self.deselect(); } pub async fn init(&mut self) { @@ -379,12 +393,12 @@ impl<'a, 'buf, SpiPeri: spi::Instance> pub async fn self_test(&mut self) -> Result<(), ()> { let mut has_self_test_error = Ok(()); - self.deselect(); - Timer::after_millis(10).await; - self.select(); + // self.deselect(); + // Timer::after_millis(10).await; + // self.select(); let _ = self.read(ImuRegisters::CHIP_ID).await; - let chip_id = self.read(ImuRegisters::CHIP_ID).await; + let chip_id = self.read(ImuRegisters::CHIP_ID).await & 0x00FF; if chip_id != BMI323_CHIP_ID { defmt::warn!("read IMU ID (0x{:x}) does not match expected BMI323 ID (0x{:x})", chip_id, BMI323_CHIP_ID); has_self_test_error = Err(()); From f98611b0402431e866118d9e00e0a6469ca7a34e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 15 May 2024 22:36:03 -0400 Subject: [PATCH 035/157] queue WIP --- lib-stm32/src/uart/queue.rs | 69 +++++++++++++++++++++++++++++++------ 1 file changed, 58 insertions(+), 11 deletions(-) diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 67260730..1cce7aa1 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -14,8 +14,8 @@ use embassy_executor::{ use embassy_futures::select::{select, Either}; use embassy_sync::{ - blocking_mutex::raw::CriticalSectionRawMutex, - mutex::Mutex + blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, + mutex::Mutex, signal::Signal }; use embassy_time::Timer; @@ -63,6 +63,7 @@ pub struct UartReadQueue< const DEPTH: usize, > { uart_mutex: Mutex, + uart_config_update_signal: Signal, queue_rx: Queue, task: TaskStorage>, } @@ -93,9 +94,10 @@ impl< const DEPTH: usize, > UartReadQueue { - pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH], uart_mutex: Mutex) -> Self { + pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH], uart_mutex: Mutex, uart_config_update_signal: Signal) -> Self { Self { uart_mutex: uart_mutex, + uart_config_update_signal: uart_config_update_signal, queue_rx: Queue::new(buffers), task: TaskStorage::new(), } @@ -107,15 +109,31 @@ impl< mut rx: UartRx<'static, UART, Async>, ) -> ReadTaskFuture { async move { - loop { + let mut block_for_config_update = false; + + loop { + /* + * Multitasking Logic: + * + * + */ + + if block_for_config_update { + // block until we receive a signal telling to unpause the task because a config update is not active + while self.uart_config_update_signal.wait().await { } + defmt::trace!("UartReadQueue - standing down from rx thread pause for config update."); + + block_for_config_update = false; + } + let mut buf = queue_rx.enqueue().await.unwrap(); - + { let _rw_tasks_config_lock = self.uart_mutex.lock().await; // NOTE: this really shouldn't be a timeout, it should be a signal from tx side that a new config // is desired. This works for now but the timeout is hacky. - match select(rx.read_until_idle(buf.data()), Timer::after_millis(2000)).await { + match select(rx.read_until_idle(buf.data()), self.uart_config_update_signal.wait()).await { Either::First(len) => { if let Ok(len) = len { if len == 0 { @@ -130,9 +148,15 @@ impl< buf.cancel(); } }, - Either::Second(_) => { - defmt::trace!("UartReadQueue - Read to idle timed out"); + Either::Second(block_for_config) => { + defmt::trace!("UartReadQueue - read to idle cancelled to update config."); buf.cancel(); + + block_for_config_update = block_for_config; + + if !block_for_config { + defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); + } } } } // frees the inter-task uart config lock @@ -164,6 +188,8 @@ pub struct UartWriteQueue< const DEPTH: usize, > { uart_mutex: Mutex, + uart_config_update_signal: Signal, + uart_config_applied_update_signal: Signal, queue_tx: Queue, has_new_uart_config: AtomicBool, new_uart_config: Mutex>, @@ -185,9 +211,11 @@ impl< const DEPTH: usize, > UartWriteQueue { - pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH], uart_mutex: Mutex) -> Self { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH], uart_mutex: Mutex, uart_config_update_signal: Signal) -> Self { Self { uart_mutex: uart_mutex, + uart_config_update_signal: uart_config_update_signal, + uart_config_applied_update_signal: Signal::new(), queue_tx: Queue::new(buffers), has_new_uart_config: AtomicBool::new(false), new_uart_config: Mutex::new(None), @@ -207,9 +235,17 @@ impl< if self.has_new_uart_config.load(Ordering::Relaxed) { // acquire the lock on the config let new_config = self.new_uart_config.lock().await; + + // rx task probably has a uart read_to_idle pending, meaning it also holds the uart_mutex lock + // signal the rx task to break the read_to_idle and spin waiting for a resume signal + self.uart_config_update_signal.signal(true); + + // rx task is now idling an will shortly release the uart_mutex // acquire the lock on the shared UART config let _rw_tasks_config_lock = self.uart_mutex.lock().await; + // now that we have exclusive control over the shared hardware, update the config + let config_res = tx.set_config(&new_config.unwrap()); if config_res.is_err() { defmt::warn!("failed to apply uart config in uart write queue"); @@ -217,8 +253,17 @@ impl< defmt::debug!("updated config in uart write queue"); } - self.has_new_uart_config.store(false, Ordering::Relaxed) - } // frees the inter-task uart config lock + // now that the config has taken hold + // tell the rx task to resume + self.uart_config_update_signal.signal(false); + + // clear the update pending flag + self.has_new_uart_config.store(false, Ordering::Relaxed); + + // signal the async task that requested the config update it can resume + // knowing the config has been applied + self.uart_config_applied_update_signal.signal(true); + } // frees the config update lock and the inter-task uart config lock // defmt::info!("invoking API write"); tx.write(buf.data()).await.unwrap(); // we are blocked here! @@ -256,6 +301,8 @@ impl< let _ = new_config.insert(config); } self.has_new_uart_config.store(true, Ordering::Relaxed); + + self.uart_config_applied_update_signal.wait().await; } } From 97d4ee00518138c97bff74133c03ba5a0e371c81 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 17 May 2024 01:48:10 -0400 Subject: [PATCH 036/157] 3rd attempt at resolving uart queue race condition --- control-board/src/bin/control-old/main.rs | 6 +- control-board/src/tasks/control.rs | 12 +- control-board/src/tasks/radio_task.rs | 4 +- .../bin/hwtest-uart-queue-mixedbaud/main.rs | 17 +- .../bin/hwtest-uart-queue-mixedrate/main.rs | 40 +-- lib-stm32-test/src/lib.rs | 1 + lib-stm32/src/drivers/imu/bmi323.rs | 3 +- lib-stm32/src/uart/queue.rs | 323 +++++++++++++----- 8 files changed, 277 insertions(+), 129 deletions(-) diff --git a/control-board/src/bin/control-old/main.rs b/control-board/src/bin/control-old/main.rs index 3d5c3b6b..3a3fe7a7 100644 --- a/control-board/src/bin/control-old/main.rs +++ b/control-board/src/bin/control-old/main.rs @@ -28,7 +28,7 @@ use smart_leds::{SmartLedsWrite, RGB8}; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_sync::pubsub::PubSubChannel; -use ateam_lib_stm32::make_uart_queues; +use ateam_lib_stm32::make_uart_queue_pair; use ateam_control_board::motion::tasks::control; use ateam_control_board::pins::*; @@ -63,13 +63,13 @@ pub const KICKER_TX_BUF_DEPTH: usize = 4; pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; pub const KICKER_RX_BUF_DEPTH: usize = 4; -make_uart_queues!(RADIO, +make_uart_queue_pair!(RADIO, RadioUART, RadioRxDMA, RadioTxDMA, RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -make_uart_queues!(KICKER, +make_uart_queue_pair!(KICKER, KickerUart, KickerRxDma, KickerTxDma, KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, diff --git a/control-board/src/tasks/control.rs b/control-board/src/tasks/control.rs index e4232218..f8b96549 100644 --- a/control-board/src/tasks/control.rs +++ b/control-board/src/tasks/control.rs @@ -19,7 +19,7 @@ use crate::{ use nalgebra::{Vector3, Vector4}; -use ateam_lib_stm32::make_uart_queues; +use ateam_lib_stm32::make_uart_queue_pair; use ateam_common_packets::bindings_radio::{ BasicControl, @@ -49,31 +49,31 @@ const RX_BUF_DEPTH: usize = 20; const TICKS_WITHOUT_PACKET_STOP: u16 = 25; -make_uart_queues!(FRONT_LEFT, +make_uart_queue_pair!(FRONT_LEFT, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -make_uart_queues!(BACK_LEFT, +make_uart_queue_pair!(BACK_LEFT, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -make_uart_queues!(BACK_RIGHT, +make_uart_queue_pair!(BACK_RIGHT, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -make_uart_queues!(FRONT_RIGHT, +make_uart_queue_pair!(FRONT_RIGHT, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -make_uart_queues!(DRIB, +make_uart_queue_pair!(DRIB, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index fcefb4c1..91435989 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,6 +1,6 @@ use ateam_common_packets::radio::TelemetryPacket; -use ateam_lib_stm32::make_uart_queues; +use ateam_lib_stm32::make_uart_queue_pair; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; @@ -18,7 +18,7 @@ pub const RADIO_TX_BUF_DEPTH: usize = 4; pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; pub const RADIO_RX_BUF_DEPTH: usize = 4; -make_uart_queues!(RADIO, +make_uart_queue_pair!(RADIO, RadioUART, RadioRxDMA, RadioTxDMA, RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index 096841f7..d27260eb 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -16,7 +16,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{make_uart_queues, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_signals, queue_pair_rx_task, queue_pair_tx_task, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; @@ -33,7 +33,7 @@ const RX_BUF_DEPTH: usize = 5; const MAX_TX_PACKET_SIZE: usize = 64; const TX_BUF_DEPTH: usize = 5; -make_uart_queues!(COMS, +make_uart_queue_pair!(COMS, ComsUartModule, ComsUartRxDma, ComsUartTxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, @@ -146,7 +146,10 @@ async fn handle_btn_press(usr_btn_pin: UserBtnPin, BAUD_RATE.store(115_200, core::sync::atomic::Ordering::SeqCst); }; - coms_writer.update_uart_config(coms_uart_config).await; + let baud_update_res = coms_writer.update_uart_config(coms_uart_config).await; + if baud_update_res.is_err() { + defmt::panic!("failed to update baud rate"); + } } } @@ -195,8 +198,12 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - unwrap!(high_pri_spawner.spawn(COMS_RX_UART_QUEUE.spawn_task(coms_uart_rx))); - unwrap!(high_pri_spawner.spawn(COMS_TX_UART_QUEUE.spawn_task(coms_uart_tx))); + // COMS_TX_UART_QUEUE.attach_pubsub(COMS_UART_SYNC_PUBSUB.publisher().unwrap(), COMS_UART_SYNC_PUBSUB.subscriber().unwrap()).await; + // unwrap!(high_pri_spawner.spawn(COMS_RX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_rx, &COMS_UART_SYNC_PUBSUB))); + // unwrap!(high_pri_spawner.spawn(COMS_TX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_tx, &COMS_UART_SYNC_PUBSUB))); + queue_pair_register_signals!(COMS); + unwrap!(high_pri_spawner.spawn(queue_pair_rx_task!(COMS, coms_uart_rx))); + unwrap!(high_pri_spawner.spawn(queue_pair_tx_task!(COMS, coms_uart_tx))); // MIGHT should put queues in mix prio, this could elicit the bug diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs index d61bdca4..f481ea9c 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedrate/main.rs @@ -2,10 +2,7 @@ #![no_main] #![feature(sync_unsafe_cell)] -use core::{ - cell::SyncUnsafeCell, - sync::atomic::AtomicU16 -}; +use core::sync::atomic::AtomicU16; use embassy_stm32::{ bind_interrupts, @@ -17,7 +14,6 @@ use embassy_stm32::{ usart::{self, *} }; use embassy_executor::{Executor, InterruptExecutor}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use embassy_time::Timer; use defmt::*; @@ -26,7 +22,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{queue::Buffer, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queue_pair, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; @@ -43,24 +39,11 @@ const TX_BUF_DEPTH: usize = 5; const MAX_RX_PACKET_SIZE: usize = 64; const RX_BUF_DEPTH: usize = 200; -// control communications tx buffer -const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".axisram.buffers"] -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".axisram.buffers"] -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); +make_uart_queue_pair!(COMS, + ComsUartModule, ComsUartRxDma, ComsUartTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); static LOOP_RATE_MS: AtomicU16 = AtomicU16::new(100); @@ -208,8 +191,9 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - unwrap!(high_pri_spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - unwrap!(high_pri_spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); + COMS_TX_UART_QUEUE.attach_pubsub(COMS_UART_SYNC_PUBSUB.publisher().unwrap(), COMS_UART_SYNC_PUBSUB.subscriber().unwrap()).await; + unwrap!(high_pri_spawner.spawn(COMS_RX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_rx, &COMS_UART_SYNC_PUBSUB))); + unwrap!(high_pri_spawner.spawn(COMS_TX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_tx, &COMS_UART_SYNC_PUBSUB))); // MIGHT should put queues in mix prio, this could elicit the bug @@ -219,7 +203,7 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ unwrap!(spawner.spawn(handle_btn_press(p.PC13, p.EXTI13, p.PB0, p.PE1, p.PB14))); // unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); // unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(rx_task(&COMS_QUEUE_RX))); - unwrap!(spawner.spawn(tx_task(&COMS_QUEUE_TX))); + unwrap!(spawner.spawn(rx_task(&COMS_RX_UART_QUEUE))); + unwrap!(spawner.spawn(tx_task(&COMS_TX_UART_QUEUE))); }); } \ No newline at end of file diff --git a/lib-stm32-test/src/lib.rs b/lib-stm32-test/src/lib.rs index 5ec76710..ec78104d 100644 --- a/lib-stm32-test/src/lib.rs +++ b/lib-stm32-test/src/lib.rs @@ -1,4 +1,5 @@ #![no_std] +#![feature(const_refs_to_cell)] // #![feature(maybe_uninit_uninit_array)] // #![feature(sync_unsafe_cell)] // #![feature(type_alias_impl_trait)] diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 6267ef89..69973866 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -9,11 +9,10 @@ use core::{cmp::min, f32::consts::PI}; use embassy_stm32::{ gpio::{Level, Output, Pin, Speed}, mode::Async, - spi::{self, MisoPin, Mode, MosiPin, SckPin}, + spi::{self, MisoPin, MosiPin, SckPin}, time::hz, Peripheral }; -use embassy_time::Timer; pub const SPI_MIN_BUF_LEN: usize = 14; diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 1cce7aa1..09af671c 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -2,7 +2,8 @@ use core::{ cell::SyncUnsafeCell, - future::Future, sync::atomic::{AtomicBool, Ordering}}; + future::Future, +}; use embassy_stm32::{ mode::Async, @@ -14,10 +15,10 @@ use embassy_executor::{ use embassy_futures::select::{select, Either}; use embassy_sync::{ - blocking_mutex::raw::{CriticalSectionRawMutex, ThreadModeRawMutex}, - mutex::Mutex, signal::Signal + blocking_mutex::raw::CriticalSectionRawMutex, + mutex::Mutex, + pubsub::{PubSubChannel, Publisher, Subscriber, WaitResult}, }; -use embassy_time::Timer; use crate::queue::{ self, @@ -28,12 +29,13 @@ use crate::queue::{ }; #[macro_export] -macro_rules! make_uart_queues { +macro_rules! make_uart_queue_pair { ($name:ident, $uart:ty, $uart_rx_dma:ty, $uart_tx_dma:ty, $rx_buffer_size:expr, $rx_buffer_depth:expr, $tx_buffer_size:expr, $tx_buffer_depth:expr, $(#[$m:meta])*) => { $crate::paste::paste! { // shared mutex allowing safe runtime update to UART config const [<$name _UART_PERI_MUTEX>]: embassy_sync::mutex::Mutex = embassy_sync::mutex::Mutex::new(false); + static [<$name _UART_SYNC_PUBSUB>]: $crate::uart::queue::UartQueueSyncPubSub = embassy_sync::pubsub::PubSubChannel::new(); // tx buffer const [<$name _CONST_TX_BUF_VAL>]: core::cell::SyncUnsafeCell<$crate::queue::Buffer<$tx_buffer_size>> = @@ -56,6 +58,46 @@ macro_rules! make_uart_queues { }; } +#[macro_export] +macro_rules! queue_pair_register_signals { + ($name:ident) => { + $crate::paste::paste! { + [<$name _TX_UART_QUEUE>].attach_pubsub( + [<$name _UART_SYNC_PUBSUB>].publisher().unwrap(), + [<$name _UART_SYNC_PUBSUB>].subscriber().unwrap()).await; + } + } +} + +#[macro_export] +macro_rules! queue_pair_rx_task { + ($name:ident, $uart_rx:ident) => { + $crate::paste::paste! { + [<$name _RX_UART_QUEUE>].spawn_task_with_pubsub($uart_rx, &[<$name _UART_SYNC_PUBSUB>]) + } + } +} + +#[macro_export] +macro_rules! queue_pair_tx_task { + ($name:ident, $uart_tx:ident) => { + $crate::paste::paste! { + [<$name _TX_UART_QUEUE>].spawn_task_with_pubsub($uart_tx, &[<$name _UART_SYNC_PUBSUB>]) + } + } +} + +pub type UartQueueSyncPubSub = PubSubChannel; +type UartQueueConfigSyncPub = Publisher<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 3, 2>; +type UartQueueConfigSyncSub = Subscriber<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 3, 2>; + +#[derive(Clone, Copy, PartialEq, Debug)] +pub enum UartTaskCommand { + Pause, + UnpauseSuccess, + UnpauseFailure, +} + pub struct UartReadQueue< UART: usart::BasicInstance, DMA: usart::RxDma, @@ -63,7 +105,6 @@ pub struct UartReadQueue< const DEPTH: usize, > { uart_mutex: Mutex, - uart_config_update_signal: Signal, queue_rx: Queue, task: TaskStorage>, } @@ -94,10 +135,10 @@ impl< const DEPTH: usize, > UartReadQueue { - pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH], uart_mutex: Mutex, uart_config_update_signal: Signal) -> Self { + pub const fn new(buffers: &'static[SyncUnsafeCell>; DEPTH], + uart_mutex: Mutex ) -> Self { Self { uart_mutex: uart_mutex, - uart_config_update_signal: uart_config_update_signal, queue_rx: Queue::new(buffers), task: TaskStorage::new(), } @@ -107,25 +148,32 @@ impl< &'static self, queue_rx: &'static Queue, mut rx: UartRx<'static, UART, Async>, + mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> ReadTaskFuture { async move { - let mut block_for_config_update = false; + // if you panic here you spawned multiple ReadQueues from the same instance + // that isn't allowed + let mut pause_task_for_config_update = false; loop { - /* - * Multitasking Logic: - * - * - */ - - if block_for_config_update { - // block until we receive a signal telling to unpause the task because a config update is not active - while self.uart_config_update_signal.wait().await { } - defmt::trace!("UartReadQueue - standing down from rx thread pause for config update."); - - block_for_config_update = false; - } + // block if/until we receive a signal telling to unpause the task because a config update is not active + while pause_task_for_config_update { + defmt::trace!("UartReadQueue - pausing rx task for config update"); + + match uart_config_signal_subscriber.next_message().await { + WaitResult::Lagged(amnt) => { + defmt::debug!("UartReadQueue - lagged {} processing config signal", amnt) + } + WaitResult::Message(task_command) => { + if task_command == UartTaskCommand::UnpauseSuccess || task_command == UartTaskCommand::UnpauseFailure { + defmt::trace!("UartReadQueue - resuming rx thread paused for config update."); + pause_task_for_config_update = false; + } + } + } + } + // get enqueue ref to pass to the DMA layer let mut buf = queue_rx.enqueue().await.unwrap(); { @@ -133,7 +181,7 @@ impl< // NOTE: this really shouldn't be a timeout, it should be a signal from tx side that a new config // is desired. This works for now but the timeout is hacky. - match select(rx.read_until_idle(buf.data()), self.uart_config_update_signal.wait()).await { + match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { Either::First(len) => { if let Ok(len) = len { if len == 0 { @@ -148,15 +196,24 @@ impl< buf.cancel(); } }, - Either::Second(block_for_config) => { + Either::Second(config_signal_result) => { defmt::trace!("UartReadQueue - read to idle cancelled to update config."); + // clear the buffer record keeping, the transaction may have been interrupted buf.cancel(); - block_for_config_update = block_for_config; - - if !block_for_config { - defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); + match config_signal_result { + WaitResult::Lagged(amnt) => { + defmt::trace!("UartReadQueue - lagged {} processing config update signal while blocked on read_to_idle", amnt); + } + WaitResult::Message(task_command) => { + if task_command == UartTaskCommand::Pause { + pause_task_for_config_update = true; + } else { + defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); + } + } } + } } } // frees the inter-task uart config lock @@ -167,8 +224,17 @@ impl< pub fn spawn_task( &'static self, rx: UartRx<'static, UART, Async>, + uart_config_signal_subscriber: UartQueueConfigSyncSub ) -> SpawnToken { - self.task.spawn(|| self.read_task(&self.queue_rx, rx)) + self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_subscriber)) + } + + pub fn spawn_task_with_pubsub( + &'static self, + rx: UartRx<'static, UART, Async>, + uart_config_signal_pubsub: &'static UartQueueSyncPubSub + ) -> SpawnToken { + self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_pubsub.subscriber().unwrap())) } pub fn try_dequeue(&self) -> Result, Error> { @@ -188,10 +254,9 @@ pub struct UartWriteQueue< const DEPTH: usize, > { uart_mutex: Mutex, - uart_config_update_signal: Signal, - uart_config_applied_update_signal: Signal, + uart_config_signal_publisher: Mutex>, + uart_config_signal_subscriber: Mutex>, queue_tx: Queue, - has_new_uart_config: AtomicBool, new_uart_config: Mutex>, task: TaskStorage>, } @@ -211,74 +276,128 @@ impl< const DEPTH: usize, > UartWriteQueue { - pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH], uart_mutex: Mutex, uart_config_update_signal: Signal) -> Self { + pub const fn new(buffers: &'static [SyncUnsafeCell>; DEPTH], + uart_mutex: Mutex + ) -> Self { Self { uart_mutex: uart_mutex, - uart_config_update_signal: uart_config_update_signal, - uart_config_applied_update_signal: Signal::new(), + uart_config_signal_publisher: Mutex::new(None), + uart_config_signal_subscriber: Mutex::new(None), queue_tx: Queue::new(buffers), - has_new_uart_config: AtomicBool::new(false), new_uart_config: Mutex::new(None), task: TaskStorage::new(), } } + pub async fn attach_pubsub(&self, + uart_config_signal_publisher: UartQueueConfigSyncPub, + uart_config_signal_subscriber: UartQueueConfigSyncSub + ) { + let mut pb = self.uart_config_signal_publisher.lock().await; + *pb = Some(uart_config_signal_publisher); + + let mut sub = self.uart_config_signal_subscriber.lock().await; + *sub = Some(uart_config_signal_subscriber); + } + fn write_task( &'static self, queue_tx: &'static Queue, mut tx: UartTx<'static, UART, Async>, + uart_config_signal_publisher: UartQueueConfigSyncPub, + mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> WriteTaskFuture { async move { loop { - let buf = queue_tx.dequeue().await.unwrap(); - - if self.has_new_uart_config.load(Ordering::Relaxed) { - // acquire the lock on the config - let new_config = self.new_uart_config.lock().await; - - // rx task probably has a uart read_to_idle pending, meaning it also holds the uart_mutex lock - // signal the rx task to break the read_to_idle and spin waiting for a resume signal - self.uart_config_update_signal.signal(true); - - // rx task is now idling an will shortly release the uart_mutex - // acquire the lock on the shared UART config - let _rw_tasks_config_lock = self.uart_mutex.lock().await; - - // now that we have exclusive control over the shared hardware, update the config - - let config_res = tx.set_config(&new_config.unwrap()); - if config_res.is_err() { - defmt::warn!("failed to apply uart config in uart write queue"); - } else { - defmt::debug!("updated config in uart write queue"); + // the tx task primarily blocks on queue_tx.queue(), e.g. waiting for other async tasks + // to enqueue data. Use a select to break waiting if another task signals there's a UART + // config update. They probably want the update before the next data is enqueued + match select(queue_tx.dequeue(), uart_config_signal_subscriber.next_message()).await { + // we are dequeing data + Either::First(dq_res) => { + if let Ok(buf) = dq_res { + // write the message to the DMA + tx.write(buf.data()).await.unwrap(); // we are blocked here! + + // drop the buffer, cleans up queue + drop(buf); + + // NOTE: we used to check for DMA transaction complete here, but embassy added + // it some time ago. Doing it twice causes lockup. + } else { + defmt::warn!("UartWriteQueue - result of dequeue for DMA TX was error"); + } } - - // now that the config has taken hold - // tell the rx task to resume - self.uart_config_update_signal.signal(false); - - // clear the update pending flag - self.has_new_uart_config.store(false, Ordering::Relaxed); - - // signal the async task that requested the config update it can resume - // knowing the config has been applied - self.uart_config_applied_update_signal.signal(true); - } // frees the config update lock and the inter-task uart config lock - - // defmt::info!("invoking API write"); - tx.write(buf.data()).await.unwrap(); // we are blocked here! - // defmt::info!("passed API write"); - - drop(buf); - - // NOTE: we used to check for DMA transaction complete here, but embassy added - // it some time ago. Doing it twice causes lockup. + // we are processing a request to update the UART hardware config + Either::Second(config_signal_res) => { + match config_signal_res { + WaitResult::Lagged(amnt) => { + defmt::trace!("UartWriteQueue - lagged {} while processing signal to pause for config update", amnt); + } + WaitResult::Message(task_command) => { + // we received a task command, possible asking us to pasue for a new config + let mut success = false; + if task_command == UartTaskCommand::Pause { + { // open a scope so we can explicitly track the config mutex lifetime + // acquire the lock on the config + defmt::debug!("UartWriteQueue - waiting for config lock"); + let mut new_config = self.new_uart_config.lock().await; + + // rx task should have also received the pause request and is now (or will be shortly) + // idling and will release the uart config mutex which it nominally holds during read_to_idle + // we can now acquire the lock on the shared UART config + defmt::debug!("UartWriteQueue - waiting for hardware lock"); + let _rw_tasks_config_lock = self.uart_mutex.lock().await; + + // now that we have exclusive control over the shared hardware, update the config + defmt::debug!("UartWriteQueue - applying new hardware config"); + if new_config.is_some() { + let config_res = tx.set_config(&new_config.unwrap()); + if config_res.is_err() { + defmt::warn!("UartWriteQueue - failed to apply uart config in uart write queue"); + } else { + defmt::debug!("UartWriteQueue - updated config in uart write queue"); + success = true; + } + } else { + defmt::warn!("UartWriteQueue - task was signaled to update config but none was provided.") + } + + // clear config + *new_config = None; + } // free uart hardware config and new config mutexes + + defmt::debug!("UartWriteQueue - released locks"); + + // we've either succeeded or not succeeded + // regardless, clean up by signaling other tasks to unpause + if success { + uart_config_signal_publisher.publish(UartTaskCommand::UnpauseSuccess).await; + } else { + uart_config_signal_publisher.publish(UartTaskCommand::UnpauseFailure).await; + } + } + } + } + } + } } } } - pub fn spawn_task(&'static self, tx: UartTx<'static, UART, Async>) -> SpawnToken { - self.task.spawn(|| self.write_task(&self.queue_tx, tx)) + pub fn spawn_task(&'static self, + tx: UartTx<'static, UART, Async>, + uart_config_signal_publisher: UartQueueConfigSyncPub, + uart_config_signal_subscriber: UartQueueConfigSyncSub, + ) -> SpawnToken { + self.task.spawn(|| self.write_task(&self.queue_tx, tx, uart_config_signal_publisher, uart_config_signal_subscriber)) + } + + pub fn spawn_task_with_pubsub(&'static self, + tx: UartTx<'static, UART, Async>, + uart_config_signal_pubsub: &'static UartQueueSyncPubSub + ) -> SpawnToken { + self.task.spawn(|| self.write_task(&self.queue_tx, tx, uart_config_signal_pubsub.publisher().unwrap(), uart_config_signal_pubsub.subscriber().unwrap())) } pub fn enqueue(&self, fn_write: impl FnOnce(&mut [u8]) -> usize) -> Result<(), queue::Error> { @@ -295,14 +414,52 @@ impl< }) } - pub async fn update_uart_config(&self, config: usart::Config) { + pub async fn update_uart_config(&self, config: usart::Config) -> Result<(), ()> { + // acquire the config lock and insert the config { let mut new_config = self.new_uart_config.lock().await; let _ = new_config.insert(config); + } // drop config lock + + // signal the tasks to pause and apply the config before the next write + // tx task will acquire the config lock and do the application once the rx task + // releases the hardware uart lock predominantly held by read_to_idle. + { + let config_signal_pub = self.uart_config_signal_publisher.lock().await; + config_signal_pub.as_ref().unwrap().publish(UartTaskCommand::Pause).await; } - self.has_new_uart_config.store(true, Ordering::Relaxed); - self.uart_config_applied_update_signal.wait().await; + // multiple tasks (not the queues) could call this from a multi-prio context + // so we need to acquire the lock on the subscriber + #[allow(unused_assignments)] // value isn't read but is retunred + let mut ret_val: Result<(), ()> = Err(()); + { + let mut success_subscriber = self.uart_config_signal_subscriber.lock().await; + + // wait for tasks to indicate success + loop { + let success_result = success_subscriber.as_mut().unwrap().next_message().await; + match success_result { + WaitResult::Lagged(amnt) => { + defmt::debug!("UartQueue - lagged {} waiting for status response from config update", amnt); + } + WaitResult::Message(task_command_reply) => { + if task_command_reply == UartTaskCommand::Pause { + // we are probably back processing our own command to Pause + defmt::debug!("UartQueue - received spurious value waiting for response"); + } + + // tx thread will release locks and send the Unpause command indicating success + if task_command_reply == UartTaskCommand::UnpauseSuccess { + ret_val = Ok(()); + break; + } + } + } + } + } // subscriber lock freed here + + return ret_val; } } From a0026ba91bd02c48bfd5cef5871e054950575fc7 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 17 May 2024 17:20:10 -0400 Subject: [PATCH 037/157] refactor radio lib-stm32 --- control-board/Cargo.lock | 1 + control-board/drivers/odin_w26x.rs | 13 --- control-board/src/bin/hwtest-radio/main.rs | 6 +- control-board/src/drivers/mod.rs | 2 +- control-board/src/drivers/radio/mod.rs | 6 -- .../src/drivers/{radio => }/radio_robot.rs | 101 +++++++++++------- control-board/src/drivers/shell_indicator.rs | 2 - control-board/src/tasks/mod.rs | 2 +- control-board/src/tasks/radio_task.rs | 20 ++-- kicker-board/Cargo.lock | 1 + lib-stm32-test/Cargo.lock | 1 + .../bin/hwtest-uart-queue-mixedbaud/main.rs | 11 +- lib-stm32/Cargo.lock | 1 + lib-stm32/Cargo.toml | 2 + lib-stm32/src/drivers/mod.rs | 1 + .../src/drivers/radio/at_protocol.rs | 0 .../src/drivers/radio/edm_protocol.rs | 0 lib-stm32/src/drivers/radio/mod.rs | 3 + .../src/drivers/radio/odin_w26x.rs | 40 ++++--- lib-stm32/src/lib.rs | 1 + lib-stm32/src/uart/mod.rs | 3 +- lib-stm32/src/uart/queue.rs | 11 ++ 22 files changed, 129 insertions(+), 99 deletions(-) delete mode 100644 control-board/drivers/odin_w26x.rs delete mode 100644 control-board/src/drivers/radio/mod.rs rename control-board/src/drivers/{radio => }/radio_robot.rs (84%) rename {control-board => lib-stm32}/src/drivers/radio/at_protocol.rs (100%) rename {control-board => lib-stm32}/src/drivers/radio/edm_protocol.rs (100%) create mode 100644 lib-stm32/src/drivers/radio/mod.rs rename control-board/src/drivers/radio/radio.rs => lib-stm32/src/drivers/radio/odin_w26x.rs (94%) diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 62e10f1f..41cd4a44 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -79,6 +79,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "heapless 0.8.0", "paste", ] diff --git a/control-board/drivers/odin_w26x.rs b/control-board/drivers/odin_w26x.rs deleted file mode 100644 index 22a17885..00000000 --- a/control-board/drivers/odin_w26x.rs +++ /dev/null @@ -1,13 +0,0 @@ -pub trait WifiRadio { - fn network_connect(ssid: &str, password: &str); - fn network_disconnect(); - fn network_connected() -> Option<&str>; // return SSID - - fn socket_udp_open(port: u16) -> u8; - fn socket_udp_close(sock_num: u8); - - fn has_packet() -> bool; - fn get_packet() -> None; - fn can_send_packet() -> bool; - fn send_packet(pkt: None) -> None; // result -} \ No newline at end of file diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 42cd65f0..a55fbb85 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -11,7 +11,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::{self, RobotState}, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; +use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::RobotState, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; // load credentials from correct crate @@ -23,7 +23,7 @@ use credentials::public_credentials::wifi::wifi_credentials; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; -use static_cell::{ConstStaticCell, StaticCell}; +use static_cell::ConstStaticCell; static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(RobotState::new()); @@ -80,7 +80,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { wifi_network, p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, p.DMA2_CH1, p.DMA2_CH0, - p.PC13, p.PE4); + p.PC13, p.PE4).await; start_io_task(main_spawner, robot_state, diff --git a/control-board/src/drivers/mod.rs b/control-board/src/drivers/mod.rs index 42dac1a5..1cae09b3 100644 --- a/control-board/src/drivers/mod.rs +++ b/control-board/src/drivers/mod.rs @@ -1,3 +1,3 @@ pub mod kicker; -pub mod radio; +pub mod radio_robot; pub mod shell_indicator; \ No newline at end of file diff --git a/control-board/src/drivers/radio/mod.rs b/control-board/src/drivers/radio/mod.rs deleted file mode 100644 index 683b0c86..00000000 --- a/control-board/src/drivers/radio/mod.rs +++ /dev/null @@ -1,6 +0,0 @@ -mod at_protocol; -mod edm_protocol; -mod radio; - -mod radio_robot; -pub use radio_robot::*; \ No newline at end of file diff --git a/control-board/src/drivers/radio/radio_robot.rs b/control-board/src/drivers/radio_robot.rs similarity index 84% rename from control-board/src/drivers/radio/radio_robot.rs rename to control-board/src/drivers/radio_robot.rs index 985a2071..3d61a272 100644 --- a/control-board/src/drivers/radio/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -1,4 +1,4 @@ -use super::radio::{PeerConnection, Radio, WifiAuth}; +use ateam_lib_stm32::drivers::radio::odin_w26x::{PeerConnection, Radio, WifiAuth}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings_radio::{ self, BasicControl, CommandCode, HelloRequest, HelloResponse, RadioPacket, RadioPacket_Data, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, @@ -6,6 +6,7 @@ use ateam_common_packets::bindings_radio::{ use ateam_common_packets::radio::DataPacket; use const_format::formatcp; use credentials::WifiCredential; +use embassy_stm32::uid; use core::fmt::Write; use core::mem::size_of; use embassy_futures::select::{select, Either}; @@ -17,15 +18,6 @@ use heapless::String; const MULTICAST_IP: &str = "224.4.20.69"; const MULTICAST_PORT: u16 = 42069; const LOCAL_PORT: u16 = 42069; -const TEAM_WIFI_SSID: &str = "A-Team Field"; -const COMP_MAIN_WIFI_SSID: &str = "T3_SSL_RBC23"; -const COMP_PRACTICE_WIFI_SSID: &str = "T1_SSL_RBC23"; -// const WIFI_SSID: &str = "PROMISED_LAN_DC_DEVEL"; - -const TEAM_WIFI_PASS: &str = "plancomestogether"; -const COMP_MAIN_WIFI_PASS: &str = "1fNrzxtSHG5o9"; -const COMP_PRACTICE_WIFI_PASS: &str = "e568Cwg0PjwcI"; -// const WIFI_PASS: &str = "plddevel"; #[derive(Copy, Clone)] pub enum WifiNetwork { @@ -40,8 +32,22 @@ pub enum TeamColor { Blue, } -fn get_uuid() -> u16 { - unsafe { *(0x1FF1_E800 as *const u16) } +#[derive(Clone, Copy, PartialEq, Debug)] +pub enum RobotRadioError { + ConnectUartBadStartup, + ConnectUartBadEcho, + ConnectUartBadRadioConfigUpdate, + ConnectUartBadHostConfigUpdate, + ConnectUartCannotEnterEdm, + ConnectUartNoEdmStartup, + + ConnectWifiBadHostName, + ConnectWifiBadConfig, + ConnectWifiConnectionFailed, + + OpenMulticastError, + + } unsafe impl< @@ -53,12 +59,9 @@ unsafe impl< const LEN_TX: usize, const DEPTH_TX: usize, const DEPTH_RX: usize, - > Send for RobotRadio<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX> -{ -} + > Send for RobotRadio<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_TX, DEPTH_RX> {} + -// suppresses unused reset_pin warning. Even if unused, the radio should certainly own its own reset pin -#[allow(dead_code)] pub struct RobotRadio< 'a, UART: usart::BasicInstance, @@ -109,29 +112,30 @@ impl< } } - pub async fn connect_uart(&mut self) -> Result<(), ()> { + pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { + // reset the radio so we can listen for the startup event self.reset_pin.set_high(); Timer::after(Duration::from_micros(100)).await; self.reset_pin.set_low(); + // wait until startup event is received if self.radio.wait_startup().await.is_err() { defmt::debug!("error processing radio wait startup command"); - return Err(()) + return Err(RobotRadioError::ConnectUartBadStartup); } - - defmt::info!("increasing link speed"); + defmt::trace!("increasing link speed"); let baudrate = 5_250_000; if self.radio.set_echo(false).await.is_err() { defmt::debug!("error disabling echo on radio"); - return Err(()); + return Err(RobotRadioError::ConnectUartBadEcho); } if self.radio.config_uart(baudrate, false, 8, true).await.is_err() { defmt::debug!("error increasing radio baud rate."); - return Err(()) + return Err(RobotRadioError::ConnectUartBadRadioConfigUpdate); } - defmt::info!("configured radio link speed"); + defmt::trace!("configured radio link speed"); let mut radio_uart_config = usart::Config::default(); @@ -139,8 +143,11 @@ impl< radio_uart_config.stop_bits = StopBits::STOP1; radio_uart_config.data_bits = DataBits::DataBits9; radio_uart_config.parity = usart::Parity::ParityEven; - self.radio.update_uart_config(radio_uart_config).await; - defmt::info!("configured host link speed"); + if self.radio.update_host_uart_config(radio_uart_config).await.is_err() { + defmt::debug!("error increasing host baud rate."); + return Err(RobotRadioError::ConnectUartBadHostConfigUpdate); + } + defmt::trace!("configured host link speed"); // Datasheet says wait at least 40ms after UART config change @@ -148,19 +155,19 @@ impl< // Datasheet says wait at least 50ms after entering data mode if let Ok(got_edm_startup) = self.radio.enter_edm().await { - defmt::info!("entered edm at high link speed"); + defmt::trace!("entered edm at high link speed"); if ! got_edm_startup { if self.radio.wait_edm_startup().await.is_err() { defmt::debug!("error waiting for EDM startup after uart baudrate increase"); - return Err(()); + return Err(RobotRadioError::ConnectUartNoEdmStartup); } else { defmt::info!("got EDM startup command"); } } } else { defmt::debug!("error entering EDM mode after uart baudrate increase"); - return Err(()) + return Err(RobotRadioError::ConnectUartCannotEnterEdm) } Timer::after(Duration::from_millis(50)).await; @@ -168,34 +175,46 @@ impl< Ok(()) } - pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential) -> Result<(), ()> { + pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential, robot_number: u8) -> Result<(), RobotRadioError> { // set radio hardware name enumeration - let mut s = String::<17>::new(); - core::write!(&mut s, "A-Team Robot {:04X}", get_uuid()).unwrap(); - self.radio.set_host_name(s.as_str()).await?; + let uid = uid::uid(); + let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; + + let mut s = String::<23>::new(); + core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); + if self.radio.set_host_name(s.as_str()).await.is_err() { + return Err(RobotRadioError::ConnectWifiBadHostName); + } // load the wifi network configuration into config slot 1 let wifi_ssid = wifi_credential.get_ssid(); let wifi_pass = WifiAuth::WPA { passphrase: wifi_credential.get_password(), }; - self.radio.config_wifi(1, wifi_ssid,wifi_pass).await?; + if self.radio.config_wifi(1, wifi_ssid,wifi_pass).await.is_err() { + return Err(RobotRadioError::ConnectWifiBadConfig); + } // connect to config slot 1 - self.radio.connect_wifi(1).await?; + if self.radio.connect_wifi(1).await.is_err() { + return Err(RobotRadioError::ConnectWifiConnectionFailed); + } // if we made it this far, we're connected Ok(()) } - pub async fn open_multicast(&mut self) -> Result<(), ()> { - let peer = self - .radio - .connect_peer(formatcp!( + pub async fn open_multicast(&mut self) -> Result<(), RobotRadioError> { + let peer = self.radio.connect_peer(formatcp!( "udp://{MULTICAST_IP}:{MULTICAST_PORT}/?flags=1&local_port={LOCAL_PORT}" )) - .await?; - self.peer = Some(peer); + .await; + + if peer.is_err() { + return Err(RobotRadioError::OpenMulticastError); + } + + self.peer = Some(peer.unwrap()); Ok(()) } diff --git a/control-board/src/drivers/shell_indicator.rs b/control-board/src/drivers/shell_indicator.rs index 7678bdb6..dc24b40a 100644 --- a/control-board/src/drivers/shell_indicator.rs +++ b/control-board/src/drivers/shell_indicator.rs @@ -1,5 +1,3 @@ -use core::borrow::BorrowMut; - use embassy_stm32::gpio::{Level, Output, Pin, Speed}; pub struct ShellIndicator<'a> { diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index 54194e09..93eb1559 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,4 +1,4 @@ -pub mod control; +// pub mod control; pub mod control_task; pub mod imu_task; pub mod radio_task; diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 91435989..9c1380d2 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,6 +1,6 @@ use ateam_common_packets::radio::TelemetryPacket; -use ateam_lib_stm32::make_uart_queue_pair; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; @@ -11,7 +11,7 @@ use embassy_stm32::{ use embassy_sync::pubsub::WaitResult; use embassy_time::Timer; -use crate::{drivers::radio::RobotRadio, pins::*, robot_state::RobotState, SystemIrqs}; +use crate::{drivers::radio_robot::RobotRadio, pins::*, robot_state::RobotState, SystemIrqs}; pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -50,15 +50,18 @@ async fn radio_task_entry( #[allow(unused_labels)] 'connect_loop: loop { - RADIO_TX_UART_QUEUE.update_uart_config(startup_radio_uart_config).await; - // TODO fix me this doesn't block - if radio_ndet.is_high() { defmt::error!("radio appears unplugged."); Timer::after_millis(1000).await; continue 'connect_loop; } + if RADIO_TX_UART_QUEUE.update_uart_config(startup_radio_uart_config).await.is_err() { + defmt::error!("failed to reset radio config."); + Timer::after_millis(1000).await; + continue 'connect_loop; + } + defmt::info!("connecting radio uart"); match select(radio.connect_uart(), Timer::after_millis(10000)).await { Either::First(res) => { @@ -76,7 +79,7 @@ async fn radio_task_entry( } } - while radio.connect_to_network(wifi_network).await.is_err() { + while radio.connect_to_network(wifi_network, robot_state.get_hw_robot_id()).await.is_err() { defmt::error!("failed to connect to wifi network."); Timer::after_millis(1000).await; } @@ -139,7 +142,7 @@ async fn radio_task_entry( } } -pub fn start_radio_task(radio_task_spawner: SendSpawner, +pub async fn start_radio_task(radio_task_spawner: SendSpawner, queue_spawner: Spawner, robot_state: &'static RobotState, command_publisher: CommandsPublisher, @@ -164,8 +167,7 @@ pub fn start_radio_task(radio_task_spawner: SendSpawner, let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); - queue_spawner.spawn(RADIO_RX_UART_QUEUE.spawn_task(radio_uart_rx)).unwrap(); - queue_spawner.spawn(RADIO_TX_UART_QUEUE.spawn_task(radio_uart_tx)).unwrap(); + queue_pair_register_and_spawn!(queue_spawner, RADIO, radio_uart_rx, radio_uart_tx); radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); } \ No newline at end of file diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 176b3c2f..07275fc1 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -68,6 +68,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "heapless 0.8.0", "paste", ] diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index 422a826b..c7726f86 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -14,6 +14,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "heapless 0.8.0", "paste", ] diff --git a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs index d27260eb..3f064074 100644 --- a/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs +++ b/lib-stm32-test/src/bin/hwtest-uart-queue-mixedbaud/main.rs @@ -16,7 +16,7 @@ use panic_probe as _; use static_cell::StaticCell; -use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_signals, queue_pair_rx_task, queue_pair_tx_task, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; type ComsUartModule = USART2; type ComsUartTxDma = DMA1_CH0; @@ -197,13 +197,8 @@ async fn main(_spawner: embassy_executor::Spawner) -> !{ ).unwrap(); let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); - - // COMS_TX_UART_QUEUE.attach_pubsub(COMS_UART_SYNC_PUBSUB.publisher().unwrap(), COMS_UART_SYNC_PUBSUB.subscriber().unwrap()).await; - // unwrap!(high_pri_spawner.spawn(COMS_RX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_rx, &COMS_UART_SYNC_PUBSUB))); - // unwrap!(high_pri_spawner.spawn(COMS_TX_UART_QUEUE.spawn_task_with_pubsub(coms_uart_tx, &COMS_UART_SYNC_PUBSUB))); - queue_pair_register_signals!(COMS); - unwrap!(high_pri_spawner.spawn(queue_pair_rx_task!(COMS, coms_uart_rx))); - unwrap!(high_pri_spawner.spawn(queue_pair_tx_task!(COMS, coms_uart_tx))); + + queue_pair_register_and_spawn!(high_pri_spawner, COMS, coms_uart_rx, coms_uart_tx); // MIGHT should put queues in mix prio, this could elicit the bug diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index efda7676..28d67430 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -14,6 +14,7 @@ dependencies = [ "embassy-stm32", "embassy-sync", "embassy-time", + "heapless", "paste", ] diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index aa6b0c92..8ef2aa09 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -18,6 +18,8 @@ defmt = "=0.3.6" # pin this for now, probe run doesn't support wire version 4, w defmt-rtt = "0.3" critical-section = "1.1.1" +heapless = "0.8.0" + [features] default = ["embassy-stm32/stm32h723zg"] # stm32h723zg = [] diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index 47eaa95f..8a7859bb 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -1,4 +1,5 @@ pub mod flash; pub mod imu; +pub mod radio; pub mod switches; \ No newline at end of file diff --git a/control-board/src/drivers/radio/at_protocol.rs b/lib-stm32/src/drivers/radio/at_protocol.rs similarity index 100% rename from control-board/src/drivers/radio/at_protocol.rs rename to lib-stm32/src/drivers/radio/at_protocol.rs diff --git a/control-board/src/drivers/radio/edm_protocol.rs b/lib-stm32/src/drivers/radio/edm_protocol.rs similarity index 100% rename from control-board/src/drivers/radio/edm_protocol.rs rename to lib-stm32/src/drivers/radio/edm_protocol.rs diff --git a/lib-stm32/src/drivers/radio/mod.rs b/lib-stm32/src/drivers/radio/mod.rs new file mode 100644 index 00000000..257e966b --- /dev/null +++ b/lib-stm32/src/drivers/radio/mod.rs @@ -0,0 +1,3 @@ +pub mod at_protocol; +pub mod edm_protocol; +pub mod odin_w26x; \ No newline at end of file diff --git a/control-board/src/drivers/radio/radio.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs similarity index 94% rename from control-board/src/drivers/radio/radio.rs rename to lib-stm32/src/drivers/radio/odin_w26x.rs index 8c91f99f..8ca8e675 100644 --- a/control-board/src/drivers/radio/radio.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -2,7 +2,7 @@ use core::fmt::Write; use embassy_stm32::usart; use heapless::String; -use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; +use crate::uart::queue::{UartReadQueue, UartWriteQueue}; use super::at_protocol::{ATEvent, ATResponse}; use super::edm_protocol::EdmPacket; @@ -54,7 +54,6 @@ pub struct Radio< reader: &'a UartReadQueue, writer: &'a UartWriteQueue, mode: RadioMode, - wifi_connected: bool, } impl< @@ -76,12 +75,11 @@ impl< reader, writer, mode: RadioMode::CommandMode, - wifi_connected: false, } } - pub async fn update_uart_config(&self, config: usart::Config) { - self.writer.update_uart_config(config).await; + pub async fn update_host_uart_config(&self, config: usart::Config) -> Result<(), ()> { + self.writer.update_uart_config(config).await } pub async fn wait_startup(&self) -> Result<(), ()> { @@ -356,15 +354,19 @@ impl< } pub async fn send_data(&self, channel_id: u8, data: &[u8]) -> Result<(), ()> { - self.writer - .enqueue(|buf| { + let res = self.writer.enqueue(|buf| { EdmPacket::DataCommand { channel: channel_id, data: data, } - .write(buf) - .unwrap() - }); // TODO: unwrap + .write(buf).unwrap() + }); + + if res.is_err() { + // queue was full + return Err(()); + } + Ok(()) } @@ -385,17 +387,27 @@ impl< pub async fn send_command(&self, cmd: &str) -> Result<(), ()> { match self.mode { RadioMode::CommandMode => { - self.writer - .enqueue(|buf| { + let res = self.writer.enqueue(|buf| { buf[..cmd.len()].clone_from_slice(cmd.as_bytes()); buf[cmd.len()] = b'\r'; cmd.len() + 1 }); + + if res.is_err() { + // queue was full + return Err(()) + } + Ok(()) } RadioMode::ExtendedDataMode => { - self.writer - .enqueue(|buf| EdmPacket::ATRequest(cmd).write(buf).unwrap()); + let res = self.writer.enqueue(|buf| EdmPacket::ATRequest(cmd).write(buf).unwrap()); + + if res.is_err() { + // queue was full + return Err(()) + } + Ok(()) } _ => Err(()), diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 1d0bac28..1f469a3e 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -4,6 +4,7 @@ #![feature(type_alias_impl_trait)] #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] +#![feature(ptr_metadata)] pub mod drivers; pub mod uart; diff --git a/lib-stm32/src/uart/mod.rs b/lib-stm32/src/uart/mod.rs index 837fbf44..ac719a97 100644 --- a/lib-stm32/src/uart/mod.rs +++ b/lib-stm32/src/uart/mod.rs @@ -1 +1,2 @@ -pub mod queue; \ No newline at end of file +pub mod queue; + diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 09af671c..5843100e 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -87,6 +87,17 @@ macro_rules! queue_pair_tx_task { } } +#[macro_export] +macro_rules! queue_pair_register_and_spawn { + ($spawner:ident, $name:ident, $uart_rx:ident, $uart_tx:ident) => { + $crate::paste::paste! { + $crate::queue_pair_register_signals!($name); + $spawner.spawn($crate::queue_pair_rx_task!($name, $uart_rx)).unwrap(); + $spawner.spawn($crate::queue_pair_tx_task!($name, $uart_tx)).unwrap(); + } + }; +} + pub type UartQueueSyncPubSub = PubSubChannel; type UartQueueConfigSyncPub = Publisher<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 3, 2>; type UartQueueConfigSyncSub = Subscriber<'static, CriticalSectionRawMutex, UartTaskCommand, 1, 3, 2>; From b4b8431d97706a6a0aaab8bbcf31b0089b77163c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 19 May 2024 17:55:42 -0400 Subject: [PATCH 038/157] radio work --- control-board/src/drivers/radio_robot.rs | 9 +++- control-board/src/pins.rs | 22 ++++++--- control-board/src/tasks/control_task.rs | 60 ++++++++++++++++++++++++ lib-stm32/src/drivers/radio/odin_w26x.rs | 1 + 4 files changed, 84 insertions(+), 8 deletions(-) diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 3d61a272..cb315746 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -180,9 +180,12 @@ impl< let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; - let mut s = String::<23>::new(); - core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); + // let mut s = String::<23>::new(); + // core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); + let mut s = String::<15>::new(); + core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); if self.radio.set_host_name(s.as_str()).await.is_err() { + defmt::trace!("could not set radio host name"); return Err(RobotRadioError::ConnectWifiBadHostName); } @@ -192,11 +195,13 @@ impl< passphrase: wifi_credential.get_password(), }; if self.radio.config_wifi(1, wifi_ssid,wifi_pass).await.is_err() { + defmt::trace!("could not configure wifi profile"); return Err(RobotRadioError::ConnectWifiBadConfig); } // connect to config slot 1 if self.radio.connect_wifi(1).await.is_err() { + defmt::trace!("could not connect to wifi"); return Err(RobotRadioError::ConnectWifiConnectionFailed); } diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 6924b568..57f25f24 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -80,31 +80,41 @@ pub type KickerPowerOnPin = PG8; // Motors // ////////////// -pub type MotorFRUart = USART1; -pub type MotorFRDmaTx = DMA1_CH0; -pub type MotorFRDmaRx = DMA1_CH1; -pub type MotorFRBootPin = PD8; -pub type MotorFRResetPin = PD9; - pub type MotorFLUart = UART4; +pub type MotorFLUartRxPin = PA1; +pub type MotorFLUartTxPin = PA0; pub type MotorFLDmaTx = DMA1_CH2; pub type MotorFLDmaRx = DMA1_CH3; pub type MotorFLBootPin = PC1; pub type MotorFLResetPin = PC0; pub type MotorBLUart = UART7; +pub type MotorBLUartRxPin = PF6; +pub type MotorBLUartTxPin = PF7; pub type MotorBLDmaTx = DMA1_CH4; pub type MotorBLDmaRx = DMA1_CH5; pub type MotorBLBootPin = PF8; pub type MotorBLResetPin = PF9; pub type MotorBRUart = UART8; +pub type MotorBRUartRxPin = PE0; +pub type MotorBRUartTxPin = PE1; pub type MotorBRDmaTx = DMA1_CH6; pub type MotorBRDmaRx = DMA1_CH7; pub type MotorBRBootPin = PB9; pub type MotorBRResetPin = PB8; +pub type MotorFRUart = USART1; +pub type MotorFRUartRxPin = PB15; +pub type MotorFRUartTxPin = PB14; +pub type MotorFRDmaTx = DMA1_CH0; +pub type MotorFRDmaRx = DMA1_CH1; +pub type MotorFRBootPin = PD8; +pub type MotorFRResetPin = PD9; + pub type MotorDUart = UART5; +pub type MotorDUartRxPin = PB12; +pub type MotorDUartTxPin = PB13; pub type MotorDDmaTx = DMA2_CH2; pub type MotorDDmaRx = DMA2_CH3; pub type MotorDBootPin = PD13; diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index e69de29b..b73349f3 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -0,0 +1,60 @@ +use ateam_lib_stm32::make_uart_queue_pair; +use embassy_executor::{SendSpawner, Spawner}; + +use crate::pins::*; + +const MAX_TX_PACKET_SIZE: usize = 64; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 64; +const RX_BUF_DEPTH: usize = 20; + +make_uart_queue_pair!(FRONT_LEFT, + MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queue_pair!(BACK_LEFT, + MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queue_pair!(BACK_RIGHT, + MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queue_pair!(FRONT_RIGHT, + MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +make_uart_queue_pair!(DRIB, + MotorDUart, MotorDDmaRx, MotorDDmaTx, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + + +#[embassy_executor::task] +async fn control_task_entry() { + +} + +pub async fn start_control_task( + uart_queue_spawner: SendSpawner, + control_task_spawner: Spawner, + motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, + motor_bl_uart: MotorFLUart, motor_bl_rx_pin: MotorFLUartRxPin, motor_bl_tx_pin: MotorFLUartTxPin, motor_bl_rx_dma: MotorFLDmaRx, motor_bl_tx_dma: MotorFLDmaTx, motor_bl_boot0_pin: MotorFLBootPin, motor_bl_nrst_pin: MotorFLResetPin, + motor_br_uart: MotorFLUart, motor_br_rx_pin: MotorFLUartRxPin, motor_br_tx_pin: MotorFLUartTxPin, motor_br_rx_dma: MotorFLDmaRx, motor_br_tx_dma: MotorFLDmaTx, motor_br_boot0_pin: MotorFLBootPin, motor_br_nrst_pin: MotorFLResetPin, + motor_fr_uart: MotorFLUart, motor_fr_rx_pin: MotorFLUartRxPin, motor_fr_tx_pin: MotorFLUartTxPin, motor_fr_rx_dma: MotorFLDmaRx, motor_fr_tx_dma: MotorFLDmaTx, motor_fr_boot0_pin: MotorFLBootPin, motor_fr_nrst_pin: MotorFLResetPin, + motor_d_uart: MotorFLUart, motor_d_rx_pin: MotorFLUartRxPin, motor_d_tx_pin: MotorFLUartTxPin, motor_d_rx_dma: MotorFLDmaRx, motor_d_tx_dma: MotorFLDmaTx, motor_d_boot0_pin: MotorFLBootPin, motor_d_nrst_pin: MotorFLResetPin, + +) { + + + +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 8ca8e675..ad1f81a7 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -163,6 +163,7 @@ impl< pub async fn set_host_name(&self, host_name: &str) -> Result<(), ()> { let mut str: String<64> = String::new(); write!(str, "AT+UNHN=\"{host_name}\"").or(Err(()))?; + defmt::trace!("host configuration string: {}", str.as_str()); self.send_command(str.as_str()).await?; self.read_ok().await?; Ok(()) From 611b1b25a8b1c01026a7d97ab45fb24bdc24c044 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 19 May 2024 20:58:23 -0400 Subject: [PATCH 039/157] work on control task --- .vscode/settings.json | 4 + control-board/src/robot_state.rs | 8 ++ control-board/src/stm32_interface.rs | 125 ++++++++---------------- control-board/src/stspin_motor.rs | 91 +++++++++++++++-- control-board/src/tasks/control_task.rs | 118 ++++++++++++++++++++-- 5 files changed, 249 insertions(+), 97 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index d871f1f6..15f88614 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,5 +18,9 @@ // don't show notifications for unlinked Cargo.toml files. This is misleads folks into // including projects under incorrect ABIs. "rust-analyzer.showUnlinkedFileNotification": false, + // disable type hints by default + // this gnarly in too many files + // ctrl + alt to show + "editor.inlayHints.enabled": "offUnlessPressed", "ros.distro": "humble" } \ No newline at end of file diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index a8d2e4fb..f4f5434c 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -67,4 +67,12 @@ impl RobotState { pub fn set_hw_robot_team_is_blue(&self, is_blue: bool) { self.hw_robot_team_is_blue.store(is_blue, Ordering::Relaxed); } + + pub fn hw_in_debug_mode(&self) -> bool { + self.hw_debug_mode.load(Ordering::Relaxed) + } + + pub fn set_hw_in_debug_mode(&self, in_debug_mode: bool) { + self.hw_debug_mode.store(in_debug_mode, Ordering::Relaxed); + } } \ No newline at end of file diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 4f10bc64..7b28ab2d 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -3,7 +3,7 @@ use core::cmp::min; use defmt_rtt as _; use defmt::*; -use embassy_stm32::gpio::{Output, Pin}; +use embassy_stm32::gpio::{Level, Output, Pin, Speed}; use embassy_stm32::pac; use embassy_stm32::usart::{self, Parity, Config}; use embassy_time::{Duration, Timer}; @@ -50,8 +50,8 @@ pub struct Stm32Interface< > { reader: &'a UartReadQueue, writer: &'a UartWriteQueue, - boot0_pin: Option>, - reset_pin: Option>, + boot0_pin: Output<'a>, + reset_pin: Output<'a>, reset_pin_noninverted: bool, @@ -72,49 +72,42 @@ impl< pub fn new( read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: Option>, - reset_pin: Option> - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - // let the user set the initial state - // if boot0_pin.is_some() { - // boot0_pin.as_mut().unwrap().set_low(); - // } - - // if reset_pin.is_some() { - // reset_pin.as_mut().unwrap().set_high(); - // } - + boot0_pin: Output<'a>, + reset_pin: Output<'a>, + reset_polarity_high: bool, + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { Stm32Interface { reader: read_queue, writer: write_queue, boot0_pin, reset_pin, - reset_pin_noninverted: false, + reset_pin_noninverted: reset_polarity_high, in_bootloader: false, } } - pub fn new_noninverted_reset( + pub fn new_from_pins( read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, - boot0_pin: Option>, - reset_pin: Option> - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - // let the user set the initial state - // if boot0_pin.is_some() { - // boot0_pin.as_mut().unwrap().set_low(); - // } - - // if reset_pin.is_some() { - // reset_pin.as_mut().unwrap().set_high(); - // } + boot0_pin: impl Pin, + reset_pin: impl Pin, + reset_polarity_high: bool, + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { + let boot0_output = Output::new(boot0_pin, Level::Low, Speed::Medium); + let initial_reset_level = if reset_polarity_high { + Level::Low + } else { + Level::High + }; + let reset_output = Output::new(reset_pin, initial_reset_level, Speed::Medium); + Stm32Interface { reader: read_queue, writer: write_queue, - boot0_pin, - reset_pin, - reset_pin_noninverted: true, + boot0_pin: boot0_output, + reset_pin: reset_output, + reset_pin_noninverted: reset_polarity_high, in_bootloader: false, } } @@ -123,57 +116,35 @@ impl< defmt::panic!("implement soft reset if needed."); } - pub async fn enter_reset(&mut self) -> Result<(), ()> { - if self.reset_pin.is_none() { - return Err(()); - } - + pub async fn enter_reset(&mut self) { if self.reset_pin_noninverted { - self.reset_pin.as_mut().unwrap().set_high(); + self.reset_pin.set_high(); } else { - self.reset_pin.as_mut().unwrap().set_low(); + self.reset_pin.set_low(); } Timer::after(Duration::from_micros(100)).await; - - Ok(()) } - pub async fn leave_reset(&mut self) -> Result<(), ()> { - if self.reset_pin.is_none() { - return Err(()); - } - + pub async fn leave_reset(&mut self) { if self.reset_pin_noninverted { - self.reset_pin.as_mut().unwrap().set_low(); + self.reset_pin.set_low(); } else { - self.reset_pin.as_mut().unwrap().set_high(); + self.reset_pin.set_high(); } Timer::after(Duration::from_micros(100)).await; - - Ok(()) } - pub async fn hard_reset(&mut self) -> Result<(), ()> { - if self.reset_pin.is_none() { - return Err(()); - } - - self.enter_reset().await?; - self.leave_reset().await?; - - Ok(()) + pub async fn hard_reset(&mut self) { + self.enter_reset().await; + self.leave_reset().await; } pub async fn reset_into_bootloader(&mut self) -> Result<(), ()> { - if self.boot0_pin.is_none() || self.reset_pin.is_none() { - return Err(()); - } - // set the boot0 line high to enter the UART bootloader upon reset - self.boot0_pin.as_mut().unwrap().set_high(); + self.boot0_pin.set_high(); // reset the device - self.hard_reset().await?; + self.hard_reset().await; // ensure it has time to setup it's bootloader // this time isn't documented and can possibly be lowered. @@ -213,24 +184,18 @@ impl< res } - pub async fn reset_into_program(&mut self, stay_in_reset: bool) -> Result<(), ()> { - if self.boot0_pin.is_none() || self.reset_pin.is_none() { - return Err(()); - } - + pub async fn reset_into_program(&mut self, stay_in_reset: bool) { // set the boot0 line low to disable startup bootloader - self.boot0_pin.as_mut().unwrap().set_low(); + self.boot0_pin.set_low(); Timer::after(Duration::from_millis(5)).await; if stay_in_reset { // nrst, so reset and hold - self.enter_reset().await?; + self.enter_reset().await; } else { // reset the device - self.hard_reset().await?; + self.hard_reset().await; } - - Ok(()) } pub async fn update_uart_config(&self, baudrate: u32, parity: Parity) { @@ -558,20 +523,14 @@ impl< return Err(err); } - info!("before reset"); - - if let Err(err) = self.reset_into_program(false).await { - return Err(err); - } - - info!("after reset"); + self.reset_into_program(false).await; Ok(()) } pub async fn execute_code(&mut self, start_address: Option) -> Result<(), ()> { defmt::info!("firmware jump/go command implementation not working. will reset part."); - self.reset_into_program(false).await?; + self.reset_into_program(false).await; return Ok(()); if !self.in_bootloader { @@ -585,7 +544,7 @@ impl< // set the boot0 line low to disable startup bootloader // not needed for command, but makes sense on principle - self.boot0_pin.as_mut().unwrap().set_low(); + self.boot0_pin.set_low(); defmt::debug!("sending the go command..."); self.writer diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index c0cfb842..56977d63 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -1,5 +1,6 @@ use core::{mem::MaybeUninit, f32::consts::PI}; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use defmt::*; use embassy_stm32::{ gpio::Pin, @@ -18,7 +19,7 @@ use ateam_common_packets::bindings_stspin::{ MotorResponse_Motion_Packet, MotorResponse_Params_Packet, }; -use crate::stm32_interface::Stm32Interface; +use crate::stm32_interface::{self, Stm32Interface}; pub struct WheelMotor< 'a, @@ -113,6 +114,44 @@ impl< } } + pub fn new_from_pins( + read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: impl Pin, + reset_pin: impl Pin, + firmware_image: &'a [u8], + ) -> WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> + { + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, false); + + let start_state: MotorResponse_Motion_Packet = + { unsafe { MaybeUninit::zeroed().assume_init() } }; + + let start_params_state: MotorResponse_Params_Packet = + { unsafe { MaybeUninit::zeroed().assume_init() } }; + + WheelMotor { + stm32_uart_interface: stm32_interface, + firmware_image, + + version_major: 0, + version_minor: 0, + version_patch: 0, + current_state: start_state, + current_params_state: start_params_state, + vel_pid_constants: Vector3::new(0.0, 0.0, 0.0), + vel_pid_i_max: 0.0, + torque_pid_constants: Vector3::new(0.0, 0.0, 0.0), + torque_pid_i_max: 0.0, + torque_limit: 0.0, + + setpoint: 0.0, + motion_type: OPEN_LOOP, + reset_flagged: false, + telemetry_enabled: false, + } + } + pub async fn reset(&mut self) { self.stm32_uart_interface.hard_reset().await; } @@ -142,7 +181,7 @@ impl< Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset - self.stm32_uart_interface.leave_reset().await?; + self.stm32_uart_interface.leave_reset().await; return res; } @@ -386,6 +425,47 @@ impl< } } + pub fn new_from_pins( + read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: impl Pin, + reset_pin: impl Pin, + firmware_image: &'a [u8], + ball_detected_thresh: f32, + ) -> DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> + { + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, false); + + let start_state: MotorResponse_Motion_Packet = + { unsafe { MaybeUninit::zeroed().assume_init() } }; + + let start_params_state: MotorResponse_Params_Packet = + { unsafe { MaybeUninit::zeroed().assume_init() } }; + + DribblerMotor { + stm32_uart_interface: stm32_interface, + firmware_image, + + version_major: 0, + version_minor: 0, + version_patch: 0, + current_state: start_state, + current_params_state: start_params_state, + vel_pid_constants: Vector3::new(0.0, 0.0, 0.0), + vel_pid_i_max: 0.0, + torque_pid_constants: Vector3::new(0.0, 0.0, 0.0), + torque_pid_i_max: 0.0, + torque_limit: 0.0, + + setpoint: 0.0, + motion_type: OPEN_LOOP, + reset_flagged: false, + telemetry_enabled: false, + + ball_detected_thresh: ball_detected_thresh, + } + } + pub async fn reset(&mut self) { self.stm32_uart_interface.hard_reset().await; } @@ -408,14 +488,11 @@ impl< // it will begin issueing telemetry updates // these are the only packets it sends so any blocked process should get the data it now needs info!("update config"); - unsafe { - self.stm32_uart_interface - .update_uart_config(2_000_000, Parity::ParityEven) - }; + self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven); Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset - self.stm32_uart_interface.leave_reset().await?; + self.stm32_uart_interface.leave_reset().await; return res; } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index b73349f3..0f8d3e00 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,7 +1,12 @@ -use ateam_lib_stm32::make_uart_queue_pair; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; +use embassy_stm32::usart::Uart; +use embassy_time::Timer; -use crate::pins::*; +use crate::{include_external_cpp_bin, pins::*, robot_state::{self, RobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; + +include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} +include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} const MAX_TX_PACKET_SIZE: usize = 64; const TX_BUF_DEPTH: usize = 3; @@ -40,21 +45,120 @@ make_uart_queue_pair!(DRIB, #[embassy_executor::task] -async fn control_task_entry() { +async fn control_task_entry( + robot_state: RobotState, + command_subscriber: CommandsSubscriber, + telemetry_publisher: TelemetryPublisher, + mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + mut motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + mut motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> +) { + defmt::info!("control task init."); + + // wait for the switch state to be read + while !robot_state.hw_init_state_valid() { + Timer::after_millis(10).await; + } + + if robot_state.hw_in_debug_mode() { + defmt::info!("flashing firmware"); + + motor_fl.load_default_firmware_image().await; + defmt::info!("FL flashed"); + + motor_bl.load_default_firmware_image().await; + defmt::info!("BL flashed"); + + motor_br.load_default_firmware_image().await; + defmt::info!("BR flashed"); + + motor_fr.load_default_firmware_image().await; + defmt::info!("FR flashed"); + motor_drib.load_default_firmware_image().await; + defmt::info!("DRIB flashed"); + } else { + let _res = embassy_futures::join::join5( + motor_fl.load_default_firmware_image(), + motor_bl.load_default_firmware_image(), + motor_br.load_default_firmware_image(), + motor_fr.load_default_firmware_image(), + motor_drib.load_default_firmware_image(), + ) + .await; + + defmt::debug!("motor firmware flashed"); + } + + embassy_futures::join::join5( + motor_fl.leave_reset(), + motor_bl.leave_reset(), + motor_br.leave_reset(), + motor_fr.leave_reset(), + motor_drib.leave_reset(), + ) + .await; + + loop { + Timer::after_millis(1000); + defmt::info!("motor firmware flashed"); + } } pub async fn start_control_task( uart_queue_spawner: SendSpawner, control_task_spawner: Spawner, + robot_state: RobotState, + command_subscriber: CommandsSubscriber, + telemetry_publisher: TelemetryPublisher, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, - motor_bl_uart: MotorFLUart, motor_bl_rx_pin: MotorFLUartRxPin, motor_bl_tx_pin: MotorFLUartTxPin, motor_bl_rx_dma: MotorFLDmaRx, motor_bl_tx_dma: MotorFLDmaTx, motor_bl_boot0_pin: MotorFLBootPin, motor_bl_nrst_pin: MotorFLResetPin, - motor_br_uart: MotorFLUart, motor_br_rx_pin: MotorFLUartRxPin, motor_br_tx_pin: MotorFLUartTxPin, motor_br_rx_dma: MotorFLDmaRx, motor_br_tx_dma: MotorFLDmaTx, motor_br_boot0_pin: MotorFLBootPin, motor_br_nrst_pin: MotorFLResetPin, - motor_fr_uart: MotorFLUart, motor_fr_rx_pin: MotorFLUartRxPin, motor_fr_tx_pin: MotorFLUartTxPin, motor_fr_rx_dma: MotorFLDmaRx, motor_fr_tx_dma: MotorFLDmaTx, motor_fr_boot0_pin: MotorFLBootPin, motor_fr_nrst_pin: MotorFLResetPin, - motor_d_uart: MotorFLUart, motor_d_rx_pin: MotorFLUartRxPin, motor_d_tx_pin: MotorFLUartTxPin, motor_d_rx_dma: MotorFLDmaRx, motor_d_tx_dma: MotorFLDmaTx, motor_d_boot0_pin: MotorFLBootPin, motor_d_nrst_pin: MotorFLResetPin, + motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin, + motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin, + motor_fr_uart: MotorFRUart, motor_fr_rx_pin: MotorFRUartRxPin, motor_fr_tx_pin: MotorFRUartTxPin, motor_fr_rx_dma: MotorFRDmaRx, motor_fr_tx_dma: MotorFRDmaTx, motor_fr_boot0_pin: MotorFRBootPin, motor_fr_nrst_pin: MotorFRResetPin, + motor_d_uart: MotorDUart, motor_d_rx_pin: MotorDUartRxPin, motor_d_tx_pin: MotorDUartTxPin, motor_d_rx_dma: MotorDDmaRx, motor_d_tx_dma: MotorDDmaTx, motor_d_boot0_pin: MotorDBootPin, motor_d_nrst_pin: MotorDResetPin, ) { + let initial_motor_controller_uart_conifg = stm32_interface::get_bootloader_uart_config(); + + ////////////////////////// + // create motor uarts // + ////////////////////////// + + let fl_uart = Uart::new(motor_fl_uart, motor_fl_rx_pin, motor_fl_tx_pin, SystemIrqs, motor_fl_tx_dma, motor_fl_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + let bl_uart = Uart::new(motor_bl_uart, motor_bl_rx_pin, motor_bl_tx_pin, SystemIrqs, motor_bl_tx_dma, motor_bl_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + let br_uart = Uart::new(motor_br_uart, motor_br_rx_pin, motor_br_tx_pin, SystemIrqs, motor_br_tx_dma, motor_br_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + let fr_uart = Uart::new(motor_fr_uart, motor_fr_rx_pin, motor_fr_tx_pin, SystemIrqs, motor_fr_tx_dma, motor_fr_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + let drib_uart = Uart::new(motor_d_uart, motor_d_rx_pin, motor_d_tx_pin, SystemIrqs, motor_d_tx_dma, motor_d_rx_dma, initial_motor_controller_uart_conifg).unwrap(); + + ////////////////////////////////////////////// + // register motor queues and DMA hardware // + ////////////////////////////////////////////// + + let (fl_uart_tx, fl_uart_rx) = Uart::split(fl_uart); + queue_pair_register_and_spawn!(uart_queue_spawner, FRONT_LEFT, fl_uart_rx, fl_uart_tx); + let (bl_uart_tx, bl_uart_rx) = Uart::split(bl_uart); + queue_pair_register_and_spawn!(uart_queue_spawner, BACK_LEFT, bl_uart_rx, bl_uart_tx); + let (br_uart_tx, br_uart_rx) = Uart::split(br_uart); + queue_pair_register_and_spawn!(uart_queue_spawner, BACK_RIGHT, br_uart_rx, br_uart_tx); + let (fr_uart_tx, fr_uart_rx) = Uart::split(fr_uart); + queue_pair_register_and_spawn!(uart_queue_spawner, FRONT_RIGHT, fr_uart_rx, fr_uart_tx); + let (drib_uart_tx, drib_uart_rx) = Uart::split(drib_uart); + queue_pair_register_and_spawn!(uart_queue_spawner, DRIB, drib_uart_rx, drib_uart_tx); + //////////////////////////////// + // create motor controllers // + //////////////////////////////// + + let motor_fl = WheelMotor::new_from_pins(&FRONT_LEFT_RX_UART_QUEUE, &FRONT_LEFT_TX_UART_QUEUE, motor_fl_boot0_pin, motor_fl_nrst_pin, WHEEL_FW_IMG); + let motor_bl = WheelMotor::new_from_pins(&BACK_LEFT_RX_UART_QUEUE, &BACK_LEFT_TX_UART_QUEUE, motor_bl_boot0_pin, motor_bl_nrst_pin, WHEEL_FW_IMG); + let motor_br = WheelMotor::new_from_pins(&BACK_RIGHT_RX_UART_QUEUE, &BACK_RIGHT_TX_UART_QUEUE, motor_br_boot0_pin, motor_br_nrst_pin, WHEEL_FW_IMG); + let motor_fr = WheelMotor::new_from_pins(&FRONT_RIGHT_RX_UART_QUEUE, &FRONT_RIGHT_TX_UART_QUEUE, motor_fr_boot0_pin, motor_fr_nrst_pin, WHEEL_FW_IMG); + let motor_drib = DribblerMotor::new_from_pins(&DRIB_RX_UART_QUEUE, &DRIB_TX_UART_QUEUE, motor_d_boot0_pin, motor_d_nrst_pin, DRIB_FW_IMG, 1.0); + control_task_spawner.spawn(control_task_entry(robot_state, + command_subscriber, telemetry_publisher, + motor_fl, motor_bl, motor_br, motor_fr, motor_drib)).unwrap(); } \ No newline at end of file From 9995d37435cb710ea875fde9df0a5fb232e7546e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 20 May 2024 00:24:10 -0400 Subject: [PATCH 040/157] motors spinning --- control-board/.cargo/config.toml | 3 +- control-board/src/bin/control/main.rs | 78 ++++++++++++++----- control-board/src/drivers/kicker.rs | 8 +- .../src/{tasks => motion}/control.rs | 0 control-board/src/pins.rs | 2 +- control-board/src/stm32_interface.rs | 22 +++--- control-board/src/stspin_motor.rs | 13 ++-- control-board/src/tasks/control_task.rs | 67 +++++++++++----- control-board/src/tasks/user_io_task.rs | 28 +++++-- 9 files changed, 154 insertions(+), 67 deletions(-) rename control-board/src/{tasks => motion}/control.rs (100%) diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index ddc8783f..6e4a803b 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -5,4 +5,5 @@ target = "thumbv7em-none-eabihf" runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' [env] -DEFMT_LOG = "trace" +# DEFMT_LOG = "error,ateam-control-board::tasks::control_task=trace" +DEFMT_LOG="trace" diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 4862dcc4..01fd1eb9 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -3,14 +3,13 @@ use embassy_executor::InterruptExecutor; use embassy_stm32::{ - interrupt, - pac::Interrupt + gpio::OutputType, interrupt, pac::Interrupt, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} }; use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, tasks::{imu_task::start_imu_task, radio_task::start_radio_task}}; +use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::RobotState, tasks::{control_task::start_control_task, imu_task::start_imu_task, radio_task::start_radio_task, user_io_task::start_io_task}}; // load credentials from correct crate @@ -22,6 +21,9 @@ use credentials::public_credentials::wifi::wifi_credentials; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(RobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -43,6 +45,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { defmt::info!("embassy HAL configured."); + let robot_state = ROBOT_STATE.take(); + //////////////////////// // setup task pools // //////////////////////// @@ -58,10 +62,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { // commands channel let radio_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); - let _control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); // telemetry channel - let _control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); // TODO imu channel @@ -72,22 +76,56 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// + // let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); + // let mut pwm = SimplePwm::new(p.TIM15, None, Some(ch2), None, None, khz(2), Default::default()); + // let max = pwm.get_max_duty(); + // pwm.enable(Channel::Ch2); + + + // pwm.set_duty(Channel::Ch2, max / 2); + + // loop { + // pwm.set_frequency(hz(500)); + // Timer::after_millis(1000).await; + // pwm.set_frequency(hz(2000)); + // Timer::after_millis(1000).await; + // } + + start_io_task(main_spawner, + robot_state, + p.PD6, p.PD5, p.EXTI6, p.EXTI5, + p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, + p.PG12, p.PG11, p.PG10, p.PG9, + p.PF3, p.PF2, p.PF1, p.PF0, + p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); + let wifi_network = wifi_credentials[0]; - start_radio_task( - uart_queue_spawner, main_spawner, - radio_command_publisher, radio_telemetry_subscriber, - wifi_network, - p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, - p.DMA2_CH1, p.DMA2_CH0, - p.PC13, p.PE4); - - start_imu_task(&main_spawner, - imu_gyro_data_publisher, imu_accel_data_publisher, - p.SPI1, p.PA5, p.PA7, p.PA6, - p.DMA2_CH7, p.DMA2_CH6, - p.PA4, p.PC4, p.PC5, - p.PB1, p.PB2, p.EXTI1, p.EXTI2, - p.PF11); + // start_radio_task( + // uart_queue_spawner, main_spawner, + // robot_state, + // radio_command_publisher, radio_telemetry_subscriber, + // wifi_network, + // p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, + // p.DMA2_CH1, p.DMA2_CH0, + // p.PC13, p.PE4); + + // start_imu_task(&main_spawner, + // imu_gyro_data_publisher, imu_accel_data_publisher, + // p.SPI1, p.PA5, p.PA7, p.PA6, + // p.DMA2_CH7, p.DMA2_CH6, + // p.PA4, p.PC4, p.PC5, + // p.PB1, p.PB2, p.EXTI1, p.EXTI2, + // p.PF11); + + start_control_task( + uart_queue_spawner, main_spawner, + robot_state, + control_command_subscriber, control_telemetry_publisher, + p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, + p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, + p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, + p.USART1, p.PB15, p.PB14, p.DMA1_CH1, p.DMA1_CH0, p.PD8, p.PD9, + p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; loop { Timer::after_millis(10).await; diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 33d0d20f..d6b4ab0b 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -153,15 +153,15 @@ impl< ////////////////////////////////// pub async fn reset(&mut self) { - self.stm32_uart_interface.hard_reset().await.unwrap(); + self.stm32_uart_interface.hard_reset().await; } pub async fn enter_reset(&mut self) { - self.stm32_uart_interface.enter_reset().await.unwrap(); + self.stm32_uart_interface.enter_reset().await; } pub async fn leave_reset(&mut self) { - self.stm32_uart_interface.leave_reset().await.unwrap(); + self.stm32_uart_interface.leave_reset().await; } pub async fn load_firmware_image(&mut self, _fw_image_bytes: &[u8]) -> Result<(), ()> { @@ -173,7 +173,7 @@ impl< Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset - self.stm32_uart_interface.leave_reset().await?; + self.stm32_uart_interface.leave_reset().await; return Ok(()); } diff --git a/control-board/src/tasks/control.rs b/control-board/src/motion/control.rs similarity index 100% rename from control-board/src/tasks/control.rs rename to control-board/src/motion/control.rs diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 57f25f24..e16eb7b8 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -20,7 +20,7 @@ pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, pub type CommandsSubscriber = Subscriber<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; pub type TelemetryPubSub = PubSubChannel; -pub type TelemetryPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type TelemetryPublisher = Publisher<'static, ThreadModeRawMutex, TelemetryPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; pub type TelemetrySubcriber = Subscriber<'static, ThreadModeRawMutex, TelemetryPacket, TELEMETRY_PUBSUB_DEPTH, 1, 1>; pub type GyroDataPubSub = PubSubChannel, GYRO_DATA_PUBSUB_DEPTH, 1, 1>; diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 7b28ab2d..89619c1f 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -167,11 +167,11 @@ impl< let sync_res = with_timeout(Duration::from_millis(100), self.reader.read(|buf| { if buf.len() >= 1 { if buf[0] == STM32_BOOTLOADER_ACK { - defmt::info!("bootloader replied with ACK after calibration."); + defmt::debug!("bootloader replied with ACK after calibration."); self.in_bootloader = true; res = Ok(()); } else { - defmt::error!("bootloader replied with NACK after calibration."); + defmt::debug!("bootloader replied with NACK after calibration."); } } })).await; @@ -203,7 +203,9 @@ impl< config.baudrate = baudrate; config.parity = parity; - self.writer.update_uart_config(config).await; + if self.writer.update_uart_config(config).await.is_err() { + defmt::panic!("failed to update uart config"); + } } pub async fn read_latest_packet(&self) { @@ -304,11 +306,11 @@ impl< res = Err(()); } else if buf.len() == 5 && buf[1] == 1 && buf[4] == STM32_BOOTLOADER_ACK { let pid: u16 = buf[3] as u16; - defmt::info!("found 1 byte pid {:?}", pid); + defmt::trace!("found 1 byte pid {:?}", pid); res = Ok(pid); } else if buf.len() == 5 && buf[1] == 2 && buf[4] == STM32_BOOTLOADER_ACK { let pid: u16 = ((buf[2] as u16) << 8) | (buf[3] as u16); - defmt::info!("found 2 byte pid {:?}", pid); + defmt::trace!("found 2 byte pid {:?}", pid); res = Ok(pid); } else { defmt::error!("malformed response in device ID read."); @@ -438,7 +440,7 @@ impl< addr += step_size as u32; } - defmt::info!("wrote device memory"); + defmt::debug!("wrote device memory"); Ok(()) } @@ -460,7 +462,7 @@ impl< let mut res = Err(()); self.reader.read(|buf| { - defmt::info!("extended erase reply {:?}", buf); + defmt::trace!("extended erase reply {:?}", buf); if buf.len() >= 1 { if buf[0] == STM32_BOOTLOADER_ACK { res = Ok(()); @@ -482,10 +484,10 @@ impl< let mut res = Err(()); self.reader.read(|buf| { - defmt::info!("erase reply {:?}", buf); + defmt::debug!("erase reply {:?}", buf); if buf.len() >= 1 { if buf[0] == STM32_BOOTLOADER_ACK { - defmt::info!("flash erased"); + defmt::debug!("flash erased"); res = Ok(()); } else { defmt::error!("bootloader replied with NACK"); @@ -529,7 +531,7 @@ impl< } pub async fn execute_code(&mut self, start_address: Option) -> Result<(), ()> { - defmt::info!("firmware jump/go command implementation not working. will reset part."); + defmt::debug!("firmware jump/go command implementation not working. will reset part."); self.reset_into_program(false).await; return Ok(()); diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 56977d63..5df713e6 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -19,7 +19,7 @@ use ateam_common_packets::bindings_stspin::{ MotorResponse_Motion_Packet, MotorResponse_Params_Packet, }; -use crate::stm32_interface::{self, Stm32Interface}; +use crate::stm32_interface::Stm32Interface; pub struct WheelMotor< 'a, @@ -173,11 +173,8 @@ impl< // this is safe because load firmware image call will reset the target device // it will begin issueing telemetry updates // these are the only packets it sends so any blocked process should get the data it now needs - info!("update config"); - unsafe { - self.stm32_uart_interface - .update_uart_config(2_000_000, Parity::ParityEven) - }; + defmt::debug!("update config"); + self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset @@ -487,8 +484,8 @@ impl< // this is safe because load firmware image call will reset the target device // it will begin issueing telemetry updates // these are the only packets it sends so any blocked process should get the data it now needs - info!("update config"); - self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven); + defmt::debug!("update config"); + self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; Timer::after(Duration::from_millis(1)).await; // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 0f8d3e00..9ca839ad 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -46,7 +46,7 @@ make_uart_queue_pair!(DRIB, #[embassy_executor::task] async fn control_task_entry( - robot_state: RobotState, + robot_state: &'static RobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -65,20 +65,35 @@ async fn control_task_entry( if robot_state.hw_in_debug_mode() { defmt::info!("flashing firmware"); - motor_fl.load_default_firmware_image().await; - defmt::info!("FL flashed"); - - motor_bl.load_default_firmware_image().await; - defmt::info!("BL flashed"); - - motor_br.load_default_firmware_image().await; - defmt::info!("BR flashed"); - - motor_fr.load_default_firmware_image().await; - defmt::info!("FR flashed"); - - motor_drib.load_default_firmware_image().await; - defmt::info!("DRIB flashed"); + if motor_fl.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash FL"); + } else { + defmt::info!("FL flashed"); + } + + if motor_bl.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash BL"); + } else { + defmt::info!("BL flashed"); + } + + if motor_br.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash BR"); + } else { + defmt::info!("BR flashed"); + } + + if motor_fr.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash FR"); + } else { + defmt::info!("FR flashed"); + } + + if motor_drib.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash DRIB"); + } else { + defmt::info!("DRIB flashed"); + } } else { let _res = embassy_futures::join::join5( motor_fl.load_default_firmware_image(), @@ -92,6 +107,7 @@ async fn control_task_entry( defmt::debug!("motor firmware flashed"); } + embassy_futures::join::join5( motor_fl.leave_reset(), motor_bl.leave_reset(), @@ -101,16 +117,31 @@ async fn control_task_entry( ) .await; + motor_fl.set_telemetry_enabled(true); + motor_bl.set_telemetry_enabled(true); + motor_br.set_telemetry_enabled(true); + motor_fr.set_telemetry_enabled(true); + motor_drib.set_telemetry_enabled(true); + + Timer::after_millis(10).await; + loop { - Timer::after_millis(1000); - defmt::info!("motor firmware flashed"); + motor_fl.process_packets(); + + let rads = motor_fl.read_rads(); + defmt::info!("read motor rads {}", rads); + motor_fl.set_setpoint(3.1415 * 10.0); + + motor_fl.send_motion_command(); + + Timer::after_millis(10).await; } } pub async fn start_control_task( uart_queue_spawner: SendSpawner, control_task_spawner: Spawner, - robot_state: RobotState, + robot_state: &'static RobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 7ebdb2d9..a74832d5 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,7 +1,7 @@ use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; use embassy_executor::{SendSpawner, Spawner}; -use embassy_stm32::gpio::{AnyPin, Pull}; +use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_time::Timer; use crate::drivers::shell_indicator::ShellIndicator; @@ -14,26 +14,42 @@ use crate::pins::*; async fn user_io_task_entry(robot_state: &'static RobotState, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, + mut debug_led0: Output<'static>, mut robot_id_indicator: ShellIndicator<'static>, ) { loop { // read switches let robot_id = robot_id_rotary.read_value(); let robot_team_isblue = dip_switch.read_pin(6); + let robot_debug_mode = dip_switch.read_pin(5); + + let glob_robot_debug = robot_state.hw_in_debug_mode(); + if robot_debug_mode != glob_robot_debug { + robot_state.set_hw_in_debug_mode(robot_debug_mode); + if robot_debug_mode { + defmt::info!("robot entered debug mode"); + } + } + + if robot_debug_mode { + debug_led0.set_high(); + } else { + debug_led0.set_low(); + } // publish updates to robot_state let glob_robot_id = robot_state.get_hw_robot_id(); let glob_robot_is_blue = robot_state.hw_robot_team_is_blue(); if robot_id != glob_robot_id { robot_state.set_hw_robot_id(robot_id); - defmt::debug!("updated robot id {} -> {}", glob_robot_id, robot_id); + defmt::info!("updated robot id {} -> {}", glob_robot_id, robot_id); } if robot_team_isblue != glob_robot_is_blue { robot_state.set_hw_robot_team_is_blue(robot_team_isblue); - defmt::debug!("updated robot team is blue {} -> {}", glob_robot_is_blue, robot_team_isblue); + defmt::info!("updated robot team is blue {} -> {}", glob_robot_is_blue, robot_team_isblue); } - + // TODO read messages // update indicators @@ -67,7 +83,9 @@ pub fn start_io_task(spawner: Spawner, let robot_id_selector_pins: [AnyPin; 4] = [robot_id_selector3_pin.into(), robot_id_selector2_pin.into(), robot_id_selector1_pin.into(), robot_id_selector0_pin.into()]; let robot_id_rotary = RotaryEncoder::new_from_pins(robot_id_selector_pins, Pull::None, None); + let debug_led0 = Output::new(usr_led0_pin, Level::Low, Speed::Low); + let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); - spawner.spawn(user_io_task_entry(robot_state, dip_switch, robot_id_rotary, robot_id_indicator)).unwrap(); + spawner.spawn(user_io_task_entry(robot_state, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); } \ No newline at end of file From dbddf7de9175939bfcbe22e52930ea0114945980 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 May 2024 18:12:51 -0400 Subject: [PATCH 041/157] radio work --- control-board/src/bin/hwtest-radio/main.rs | 5 +- control-board/src/drivers/radio_robot.rs | 16 +++-- lib-stm32/src/drivers/radio/odin_w26x.rs | 34 ++++++++-- lib-stm32/src/uart/queue.rs | 72 +++++++++++----------- 4 files changed, 79 insertions(+), 48 deletions(-) diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index a55fbb85..955567df 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -53,7 +53,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -72,7 +72,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - let wifi_network = wifi_credentials[0]; + let wifi_network = wifi_credentials[1]; + defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); start_radio_task( uart_queue_spawner, main_spawner, robot_state, diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index cb315746..8231b8f9 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -115,7 +115,7 @@ impl< pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { // reset the radio so we can listen for the startup event self.reset_pin.set_high(); - Timer::after(Duration::from_micros(100)).await; + Timer::after(Duration::from_millis(1)).await; self.reset_pin.set_low(); // wait until startup event is received @@ -161,9 +161,9 @@ impl< if self.radio.wait_edm_startup().await.is_err() { defmt::debug!("error waiting for EDM startup after uart baudrate increase"); return Err(RobotRadioError::ConnectUartNoEdmStartup); - } else { - defmt::info!("got EDM startup command"); - } + } + } else { + defmt::trace!("got EDM startup command"); } } else { defmt::debug!("error entering EDM mode after uart baudrate increase"); @@ -180,9 +180,9 @@ impl< let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; - // let mut s = String::<23>::new(); + // let mut s = String::<24>::new(); // core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); - let mut s = String::<15>::new(); + let mut s = String::<16>::new(); core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); if self.radio.set_host_name(s.as_str()).await.is_err() { defmt::trace!("could not set radio host name"); @@ -210,12 +210,16 @@ impl< } pub async fn open_multicast(&mut self) -> Result<(), RobotRadioError> { + defmt::warn!("trigger scope"); + Timer::after_millis(5000).await; + let peer = self.radio.connect_peer(formatcp!( "udp://{MULTICAST_IP}:{MULTICAST_PORT}/?flags=1&local_port={LOCAL_PORT}" )) .await; if peer.is_err() { + defmt::debug!("failed to connect peer"); return Err(RobotRadioError::OpenMulticastError); } diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index ad1f81a7..8cad3e93 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -82,10 +82,13 @@ impl< self.writer.update_uart_config(config).await } - pub async fn wait_startup(&self) -> Result<(), ()> { + pub async fn wait_startup(&mut self) -> Result<(), ()> { + // if were waiting for startup, we fellback to command mode + self.mode = RadioMode::CommandMode; + self.reader .dequeue(|buf| { - defmt::info!("dequeueing"); + defmt::info!("dequeueing {}", buf); if let EdmPacket::ATResponse(ATResponse::Other("+STARTUP")) = self.to_packet(buf)? { Ok(()) } else { @@ -156,7 +159,12 @@ impl< // write a new function to handle this // self.read_ok().await?; let res = self.read_ok_at_edm_transition().await; - self.mode = RadioMode::ExtendedDataMode; + if res.is_err() { + defmt::trace!("failed to enter EDM mode!"); + } else { + self.mode = RadioMode::ExtendedDataMode; + } + return res; } @@ -165,7 +173,10 @@ impl< write!(str, "AT+UNHN=\"{host_name}\"").or(Err(()))?; defmt::trace!("host configuration string: {}", str.as_str()); self.send_command(str.as_str()).await?; + defmt::trace!("sent configuration command"); self.read_ok().await?; + defmt::trace!("read OK"); + Ok(()) } @@ -275,7 +286,13 @@ impl< while peer_id.is_none() || !peer_connected_ip || channel_ret.is_none() { self.reader .dequeue(|buf| { - match self.to_packet(buf)? { + defmt::info!("buf contents: {} ", {buf}); + let pkt = self.to_packet(buf); + if pkt.is_err() { + defmt::error!("got undecodable pkt {}", buf); + } + + match pkt? { EdmPacket::ATEvent(ATEvent::PeerConnectedIP { peer_handle: _, is_ipv6: _, @@ -285,15 +302,21 @@ impl< remote_address: _, remote_port: _, }) => { + defmt::info!("AT event"); + peer_connected_ip = true; } EdmPacket::ConnectEvent { channel, event_type: _, } => { + defmt::info!("connect event"); + channel_ret = Some(channel); } EdmPacket::ATResponse(ATResponse::Ok(resp)) => { + defmt::info!("AT resp connect event"); + if let Some(i) = resp.find("+UDCP:") { peer_id = Some(resp[i + 6..].parse::().or(Err(()))?); } else { @@ -430,6 +453,7 @@ impl< async fn read_ok(&self) -> Result<(), ()> { self.reader .dequeue(|buf| { + defmt::trace!("read ok got {}", buf); if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { Ok(()) } else { @@ -442,7 +466,7 @@ impl< async fn read_ok_at_edm_transition(&self) -> Result { let transition_buf: [u8; 12] = [13, 10, 79, 75, 13, 10, 170, 0, 2, 0, 113, 85]; let res = self.reader.dequeue(|buf| { - if buf.iter().zip(transition_buf.iter()).all(|(a,b)| a == b) { + if buf.len() == transition_buf.len() && buf.iter().zip(transition_buf.iter()).all(|(a,b)| a == b) { Ok(true) } else { if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 5843100e..4dc099e5 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -16,7 +16,7 @@ use embassy_executor::{ use embassy_futures::select::{select, Either}; use embassy_sync::{ blocking_mutex::raw::CriticalSectionRawMutex, - mutex::Mutex, + mutex::{Mutex, MutexGuard}, pubsub::{PubSubChannel, Publisher, Subscriber, WaitResult}, }; @@ -166,11 +166,18 @@ impl< // that isn't allowed let mut pause_task_for_config_update = false; + // look like this might be taking too long to acquire between read_to_idle calls + // need to acquire this upfront and be smarter about releasing and reacq it + let mut rw_tasks_config_lock: Option> = Some(self.uart_mutex.lock().await); + loop { // block if/until we receive a signal telling to unpause the task because a config update is not active while pause_task_for_config_update { defmt::trace!("UartReadQueue - pausing rx task for config update"); + // release the uart hw mutex lock by dropping the MutexGuard + drop(rw_tasks_config_lock.take().unwrap()); + match uart_config_signal_subscriber.next_message().await { WaitResult::Lagged(amnt) => { defmt::debug!("UartReadQueue - lagged {} processing config signal", amnt) @@ -179,6 +186,9 @@ impl< if task_command == UartTaskCommand::UnpauseSuccess || task_command == UartTaskCommand::UnpauseFailure { defmt::trace!("UartReadQueue - resuming rx thread paused for config update."); pause_task_for_config_update = false; + + // we are told to resume, reacquire the lock + rw_tasks_config_lock = Some(self.uart_mutex.lock().await); } } } @@ -186,48 +196,40 @@ impl< // get enqueue ref to pass to the DMA layer let mut buf = queue_rx.enqueue().await.unwrap(); - - { - let _rw_tasks_config_lock = self.uart_mutex.lock().await; - - // NOTE: this really shouldn't be a timeout, it should be a signal from tx side that a new config - // is desired. This works for now but the timeout is hacky. - match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { - Either::First(len) => { - if let Ok(len) = len { - if len == 0 { - defmt::debug!("uart zero"); - buf.cancel(); - } else { - *buf.len() = len; - } - } else { - // Framing and Parity Error occur here - defmt::warn!("{}", len); + match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { + Either::First(len) => { + if let Ok(len) = len { + if len == 0 { + defmt::debug!("uart zero"); buf.cancel(); + } else { + *buf.len() = len; } - }, - Either::Second(config_signal_result) => { - defmt::trace!("UartReadQueue - read to idle cancelled to update config."); - // clear the buffer record keeping, the transaction may have been interrupted + } else { + // Framing and Parity Error occur here + defmt::warn!("{}", len); buf.cancel(); + } + }, + Either::Second(config_signal_result) => { + defmt::trace!("UartReadQueue - read to idle cancelled to update config."); + // clear the buffer record keeping, the transaction may have been interrupted + buf.cancel(); - match config_signal_result { - WaitResult::Lagged(amnt) => { - defmt::trace!("UartReadQueue - lagged {} processing config update signal while blocked on read_to_idle", amnt); - } - WaitResult::Message(task_command) => { - if task_command == UartTaskCommand::Pause { - pause_task_for_config_update = true; - } else { - defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); - } + match config_signal_result { + WaitResult::Lagged(amnt) => { + defmt::trace!("UartReadQueue - lagged {} processing config update signal while blocked on read_to_idle", amnt); + } + WaitResult::Message(task_command) => { + if task_command == UartTaskCommand::Pause { + pause_task_for_config_update = true; + } else { + defmt::warn!("UartReadQueue - config update standdown cancelled read to idle. Should this event sequence occur?"); } } - } } - } // frees the inter-task uart config lock + } } } } From afc527f95a392b57c59be36c980f320edc4625d3 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 May 2024 18:41:00 -0400 Subject: [PATCH 042/157] more radio work --- control-board/src/bin/hwtest-radio/main.rs | 2 +- control-board/src/tasks/radio_task.rs | 5 +++-- lib-stm32/src/drivers/radio/odin_w26x.rs | 10 +++++----- lib-stm32/src/uart/queue.rs | 8 ++++++++ 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 955567df..d708eadf 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -75,7 +75,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { let wifi_network = wifi_credentials[1]; defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); start_radio_task( - uart_queue_spawner, main_spawner, + main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, wifi_network, diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 9c1380d2..71a06988 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -142,8 +142,8 @@ async fn radio_task_entry( } } -pub async fn start_radio_task(radio_task_spawner: SendSpawner, - queue_spawner: Spawner, +pub async fn start_radio_task(radio_task_spawner: Spawner, + queue_spawner: SendSpawner, robot_state: &'static RobotState, command_publisher: CommandsPublisher, telemetry_subscriber: TelemetrySubcriber, @@ -165,6 +165,7 @@ pub async fn start_radio_task(radio_task_spawner: SendSpawner, radio_uart_config.parity = Parity::ParityNone; let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); + // let radio_uart = Uart::new_with_rtscts(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, _radio_uart_rts_pin, _radio_uart_cts_pin, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); queue_pair_register_and_spawn!(queue_spawner, RADIO, radio_uart_rx, radio_uart_tx); diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 8cad3e93..50510bba 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -286,10 +286,10 @@ impl< while peer_id.is_none() || !peer_connected_ip || channel_ret.is_none() { self.reader .dequeue(|buf| { - defmt::info!("buf contents: {} ", {buf}); + // defmt::info!("buf contents: {} ", {buf}); let pkt = self.to_packet(buf); if pkt.is_err() { - defmt::error!("got undecodable pkt {}", buf); + // defmt::error!("got undecodable pkt {}", buf); } match pkt? { @@ -302,7 +302,7 @@ impl< remote_address: _, remote_port: _, }) => { - defmt::info!("AT event"); + // defmt::info!("AT event"); peer_connected_ip = true; } @@ -310,12 +310,12 @@ impl< channel, event_type: _, } => { - defmt::info!("connect event"); + // defmt::info!("connect event"); channel_ret = Some(channel); } EdmPacket::ATResponse(ATResponse::Ok(resp)) => { - defmt::info!("AT resp connect event"); + // defmt::info!("AT resp connect event"); if let Some(i) = resp.find("+UDCP:") { peer_id = Some(resp[i + 6..].parse::().or(Err(()))?); diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 4dc099e5..aa8f68d0 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -19,6 +19,7 @@ use embassy_sync::{ mutex::{Mutex, MutexGuard}, pubsub::{PubSubChannel, Publisher, Subscriber, WaitResult}, }; +use embassy_time::{Duration, Instant, Timer}; use crate::queue::{ self, @@ -169,6 +170,7 @@ impl< // look like this might be taking too long to acquire between read_to_idle calls // need to acquire this upfront and be smarter about releasing and reacq it let mut rw_tasks_config_lock: Option> = Some(self.uart_mutex.lock().await); + let mut time_ended_trx = Instant::now(); loop { // block if/until we receive a signal telling to unpause the task because a config update is not active @@ -194,10 +196,16 @@ impl< } } + // get enqueue ref to pass to the DMA layer let mut buf = queue_rx.enqueue().await.unwrap(); + let read_to_idle_start_time = Instant::now(); match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { Either::First(len) => { + defmt::info!("elapsed time to process and restart read_to_idle was: {}", read_to_idle_start_time - time_ended_trx); + + time_ended_trx = Instant::now(); + if let Ok(len) = len { if len == 0 { defmt::debug!("uart zero"); From 87d8046c1ea0d2e5c6da2967feaa5eb6ec3300d6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 22 May 2024 16:49:37 -0400 Subject: [PATCH 043/157] radio and kicker work --- control-board/Cargo.toml | 1 + .../src/bin/hwtest-kicker-coms/main.rs | 145 ++++++++---------- control-board/src/bin/hwtest-radio/main.rs | 6 +- control-board/src/drivers/kicker.rs | 6 +- control-board/src/drivers/radio_robot.rs | 17 +- control-board/src/lib.rs | 2 +- control-board/src/pins.rs | 2 + control-board/src/stm32_interface.rs | 1 + control-board/src/tasks/radio_task.rs | 36 ++++- kicker-board/src/bin/hwtest-blinky/main.rs | 2 +- kicker-board/src/bin/hwtest-coms/main.rs | 54 +++---- lib-stm32/src/drivers/radio/at_protocol.rs | 7 +- lib-stm32/src/drivers/radio/odin_w26x.rs | 2 - lib-stm32/src/uart/queue.rs | 2 +- 14 files changed, 137 insertions(+), 146 deletions(-) diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index 020e1220..8dcf2197 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -69,6 +69,7 @@ debug-assertions = false [profile.release] debug = true +opt-level = 3 lto = 'fat' [lib] diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index aa674cf0..ae669a09 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -2,117 +2,93 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] + -use apa102_spi::Apa102; use ateam_control_board::{ - drivers::kicker::Kicker, - include_kicker_bin, - queue::Buffer, - stm32_interface::{get_bootloader_uart_config, Stm32Interface}, - uart_queue::{UartReadQueue, UartWriteQueue}, + drivers::kicker::Kicker, get_system_config, include_kicker_bin, pins::{KickerRxDma, KickerTxDma, KickerUart}, stm32_interface::{self, get_bootloader_uart_config, Stm32Interface}, }; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use defmt::info; +use embassy_executor::InterruptExecutor; use embassy_stm32::{ - dma::NoDma, - executor::InterruptExecutor, - gpio::{Input, Level, Output, Speed, Pull}, - interrupt::{self, InterruptExt}, - peripherals::{DMA2_CH4, DMA2_CH5, USART6}, - spi, - time::{hz, mhz}, - usart::Uart, + gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, usart::Uart }; use embassy_time::{Duration, Ticker, Timer}; -use futures_util::StreamExt; use panic_probe as _; -use smart_leds::{SmartLedsWrite, RGB8}; -use static_cell::StaticCell; -include_kicker_bin! {KICKER_FW_IMG, "hwtest-coms.bin"} +include_kicker_bin! {KICKER_FW_IMG, "hwtest-blinky.bin"} const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 20; -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static KICKER_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut KICKER_BUFFERS_TX }); +make_uart_queue_pair!(KICKER, + KickerUart, KickerRxDma, KickerTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static KICKER_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut KICKER_BUFFERS_RX }); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} #[embassy_executor::main] async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - + let stm32_config = get_system_config(); let p = embassy_stm32::init(stm32_config); - // Delay so dotstar can turn on - Timer::after(Duration::from_millis(50)).await; + defmt::info!("Kicker system init"); - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); - let dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = mhz(1); + // let kicker_det = Input::new(p.PG8, Pull::Up); + // if kicker_det.is_high() { + // defmt::warn!("kicker appears unplugged!"); + // } - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - dotstar_spi_config, - ); - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); + let mut kicker_pwr_pin = Output::new(p.PG8, Level::Low, Speed::Medium); - info!("booted"); + // defmt::info!("attempting to power on kicker."); + // Timer::after_millis(1000).await; + // kicker_pwr_pin.set_high(); + // Timer::after_millis(200).await; + // kicker_pwr_pin.set_low(); + // defmt::info!("power on attempt done"); - let kicker_det = Input::new(p.PG8, Pull::Up); - if kicker_det.is_high() { - defmt::warn!("kicker appears unplugged!"); - } + // loop { + // Timer::after_millis(1000).await; + // } - let kicker_int = interrupt::take!(USART6); let kicker_usart = Uart::new( p.USART6, p.PC7, p.PC6, - kicker_int, + ateam_control_board::SystemIrqs, p.DMA2_CH4, p.DMA2_CH5, - get_bootloader_uart_config(), - ); - let (kicker_tx, kicker_rx) = kicker_usart.split(); - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - - spawner - .spawn(KICKER_QUEUE_RX.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_QUEUE_TX.spawn_task(kicker_tx)) - .unwrap(); - - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_QUEUE_RX, - &KICKER_QUEUE_TX, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), + stm32_interface::get_bootloader_uart_config(), + ).unwrap(); + + defmt::info!("init uart"); + + let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); + queue_pair_register_and_spawn!(uart_queue_spawner, KICKER, kicker_rx, kicker_tx); + + defmt::info!("start qs"); + + let kicker_stm32_interface = Stm32Interface::new_from_pins( + &KICKER_RX_UART_QUEUE, + &KICKER_TX_UART_QUEUE, + p.PA8, + p.PA9, + true ); info!("flashing kicker..."); @@ -128,9 +104,20 @@ async fn main(_spawner: embassy_executor::Spawner) { info!("kicker flash complete"); } - kicker.set_telemetry_enabled(true); + Timer::after_millis(3000).await; + + // defmt::info!("attempting to power off kicker."); + // Timer::after_millis(1000).await; + // kicker_pwr_pin.set_high(); + // Timer::after_millis(2000).await; + // kicker_pwr_pin.set_low(); + // defmt::info!("power off attempt done"); - let _ = dotstar.write([RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }].iter().cloned()); + loop { + Timer::after_millis(1000).await; + } + + kicker.set_telemetry_enabled(true); let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); loop { diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index d708eadf..32a74cd7 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -72,7 +72,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - let wifi_network = wifi_credentials[1]; + let wifi_network = wifi_credentials[0]; defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); start_radio_task( main_spawner, uart_queue_spawner, @@ -91,10 +91,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.PF3, p.PF2, p.PF1, p.PF0, p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); - loop { - Timer::after_millis(1000).await; - } - loop { match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { Either::First(gyro_data) => { diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index d6b4ab0b..43e5acad 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -164,9 +164,9 @@ impl< self.stm32_uart_interface.leave_reset().await; } - pub async fn load_firmware_image(&mut self, _fw_image_bytes: &[u8]) -> Result<(), ()> { - defmt::warn!("currently skipping kicker firmware flash"); - // self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await?; + pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> { + // defmt::warn!("currently skipping kicker firmware flash"); + self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await?; self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 8231b8f9..2a4cdea5 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -180,10 +180,10 @@ impl< let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; - // let mut s = String::<24>::new(); - // core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); - let mut s = String::<16>::new(); - core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); + let mut s = String::<25>::new(); + core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); + // let mut s = String::<16>::new(); + // core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); if self.radio.set_host_name(s.as_str()).await.is_err() { defmt::trace!("could not set radio host name"); return Err(RobotRadioError::ConnectWifiBadHostName); @@ -210,9 +210,6 @@ impl< } pub async fn open_multicast(&mut self) -> Result<(), RobotRadioError> { - defmt::warn!("trigger scope"); - Timer::after_millis(5000).await; - let peer = self.radio.connect_peer(formatcp!( "udp://{MULTICAST_IP}:{MULTICAST_PORT}/?flags=1&local_port={LOCAL_PORT}" )) @@ -423,7 +420,7 @@ impl< } let mut data_copy = [0u8; size_of::()]; - data_copy.clone_from_slice(&data[0..PACKET_SIZE]); + data_copy[0..PACKET_SIZE].clone_from_slice(&data[0..PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; @@ -450,7 +447,7 @@ impl< if data.len() == CONTROL_PACKET_SIZE { let mut data_copy = [0u8; size_of::()]; - data_copy.clone_from_slice(&data[0..CONTROL_PACKET_SIZE]); + data_copy[0..CONTROL_PACKET_SIZE].clone_from_slice(&data[0..CONTROL_PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; @@ -461,7 +458,7 @@ impl< Ok(unsafe { DataPacket::BasicControl(packet.data.control) }) } else if data.len() == PARAMERTER_PACKET_SIZE { let mut data_copy = [0u8; size_of::()]; - data_copy.clone_from_slice(&data[0..PARAMERTER_PACKET_SIZE]); + data_copy[0..PARAMERTER_PACKET_SIZE].clone_from_slice(&data[0..PARAMERTER_PACKET_SIZE]); let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index a224229e..6412394c 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -35,7 +35,7 @@ pub mod drivers; pub mod motion; pub mod tasks; -bind_interrupts!(struct SystemIrqs { +bind_interrupts!(pub struct SystemIrqs { USART10 => usart::InterruptHandler; USART6 => usart::InterruptHandler; USART1 => usart::InterruptHandler; diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index e16eb7b8..b8141c9c 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -74,6 +74,8 @@ pub type KickerUart = USART6; pub type KickerRxDma = DMA2_CH5; pub type KickerTxDma = DMA2_CH4; pub type KickerPowerOnPin = PG8; +pub type KickerBootPin = PA8; +pub type KickerResetPIn = PA9; ////////////// diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 89619c1f..0f90bc8f 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -142,6 +142,7 @@ impl< pub async fn reset_into_bootloader(&mut self) -> Result<(), ()> { // set the boot0 line high to enter the UART bootloader upon reset self.boot0_pin.set_high(); + Timer::after_millis(1).await; // reset the device self.hard_reset().await; diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 71a06988..444df018 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -9,9 +9,9 @@ use embassy_stm32::{ usart::{self, DataBits, Parity, StopBits, Uart} }; use embassy_sync::pubsub::WaitResult; -use embassy_time::Timer; +use embassy_time::{Duration, Timer}; -use crate::{drivers::radio_robot::RobotRadio, pins::*, robot_state::RobotState, SystemIrqs}; +use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::RobotState, SystemIrqs}; pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -24,7 +24,6 @@ make_uart_queue_pair!(RADIO, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); - #[embassy_executor::task] async fn radio_task_entry( robot_state: &'static RobotState, @@ -92,6 +91,37 @@ async fn radio_task_entry( } defmt::info!("multicast open"); + 'connect_hello: + loop { + defmt::info!("sending hello"); + + let robot_id = robot_state.get_hw_robot_id(); + let team_color = if robot_state.hw_robot_team_is_blue() { + TeamColor::Blue + } else { + TeamColor::Yellow + }; + radio.send_hello(robot_id, team_color).await.unwrap(); + let hello = radio.wait_hello(Duration::from_millis(1000)).await; + + match hello { + Ok(hello) => { + defmt::info!( + "recieved hello resp to: {}.{}.{}.{}:{}", + hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port + ); + radio.close_peer().await.unwrap(); + defmt::info!("multicast peer closed"); + + radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); + defmt::info!("unicast open"); + + break 'connect_hello; + } + Err(_) => {} + } + } + // TODO add inbound timeout somewhere, maybe not here. 'process_packets: loop { diff --git a/kicker-board/src/bin/hwtest-blinky/main.rs b/kicker-board/src/bin/hwtest-blinky/main.rs index 461eeb64..43f3d3b7 100644 --- a/kicker-board/src/bin/hwtest-blinky/main.rs +++ b/kicker-board/src/bin/hwtest-blinky/main.rs @@ -178,7 +178,7 @@ async fn main(_spawner: Spawner) -> ! { // Low priority executor: runs in thread mode, using WFE/SEV let executor = EXECUTOR_LOW.init(Executor::new()); executor.run(|spawner| { - unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); + // unwrap!(spawner.spawn(shutdown_int(p.PD5, p.EXTI5, p.PD6))); unwrap!(spawner.spawn(blink(p.PE4, p.PE1, p.PE0, p.PE2, p.PE3, p.PD4, adc, p.PC0, p.PC1, p.PC3, p.PC2))); unwrap!(spawner.spawn(dotstar_lerp_task(p.SPI1, p.PA5, p.PA7, p.DMA2_CH3))); }); diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index b50ba221..75dc93f0 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -4,8 +4,6 @@ #![feature(const_mut_refs)] #![feature(sync_unsafe_cell)] -use core::cell::SyncUnsafeCell; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; use static_cell::StaticCell; use defmt::*; @@ -13,7 +11,7 @@ use {defmt_rtt as _, panic_probe as _}; use cortex_m_rt::entry; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::{Executor, InterruptExecutor, Spawner}; use embassy_stm32::{ adc::{Adc, Resolution, SampleTime}, gpio::{Level, Output, Speed}, @@ -25,7 +23,7 @@ use embassy_stm32::{ }; use embassy_stm32::{bind_interrupts, peripherals, usart}; -use embassy_time::{Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker, Timer}; use ateam_kicker_board::{ adc_200v_to_rail_voltage, adc_raw_to_v, @@ -35,7 +33,7 @@ use ateam_kicker_board::{ pins::* }; -use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; @@ -45,24 +43,12 @@ const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; -// control communications tx buffer -const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".bss"] -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -#[link_section = ".bss"] -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); +make_uart_queue_pair!(COMS, + ComsUartModule, ComsUartRxDma, ComsUartTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -230,8 +216,8 @@ bind_interrupts!(struct Irqs { USART1 => usart::InterruptHandler; }); -#[entry] -fn main() -> ! { +#[embassy_executor::main] +async fn main(spawner: Spawner) -> ! { let mut stm32_config: embassy_stm32::Config = Default::default(); stm32_config.rcc.hsi = true; stm32_config.rcc.pll_src = PllSource::HSI; // internal 16Mhz source @@ -261,9 +247,7 @@ fn main() -> ! { // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); - let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3))); + let hp_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); ////////////////////////////////// // COMMUNICATIONS TASKS SETUP // @@ -285,12 +269,12 @@ fn main() -> ! { ).unwrap(); let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + queue_pair_register_and_spawn!(spawner, COMS, coms_uart_rx, coms_uart_tx); - // low priority executor handles coms and user IO - // Low priority executor: runs in thread mode, using WFE/SEV - let lp_executor = EXECUTOR_LOW.init(Executor::new()); - lp_executor.run(|spawner| { - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - }); + hp_spawner.spawn(high_pri_kick_task(&COMS_RX_UART_QUEUE, &COMS_TX_UART_QUEUE, adc, p.PE4, p.PE5, p.PE6, p.PC0, p.PE1, p.PE2, p.PE3)).unwrap(); + + + loop { + Timer::after_millis(1000).await; + } } \ No newline at end of file diff --git a/lib-stm32/src/drivers/radio/at_protocol.rs b/lib-stm32/src/drivers/radio/at_protocol.rs index f7415424..dd6e4aff 100644 --- a/lib-stm32/src/drivers/radio/at_protocol.rs +++ b/lib-stm32/src/drivers/radio/at_protocol.rs @@ -320,17 +320,12 @@ impl<'b> ATResponse<'b> { const RESP_ERROR: &'static str = "ERROR"; pub fn new<'a>(buf: &'a [u8]) -> Result, ()> { - defmt::trace!("new AT Response: {}", buf); + // TODO: error handling in this function is bad let s = core::str::from_utf8(buf).or(Err(()))?; - defmt::trace!("searching for CR_LF: {}", s); let i_echo = s.find(Self::CR_LF).ok_or(())?; - defmt::trace!("i_echo: {}", i_echo); - //let _echo = &s[..i_echo]; - //defmt::trace!("_echo: {}", _echo) let s = &s[i_echo + Self::CR_LF.len()..]; - defmt::trace!("s: {}", s); if !s.ends_with(Self::CR_LF) { return Err(()); } diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 50510bba..8f6e3755 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -286,7 +286,6 @@ impl< while peer_id.is_none() || !peer_connected_ip || channel_ret.is_none() { self.reader .dequeue(|buf| { - // defmt::info!("buf contents: {} ", {buf}); let pkt = self.to_packet(buf); if pkt.is_err() { // defmt::error!("got undecodable pkt {}", buf); @@ -453,7 +452,6 @@ impl< async fn read_ok(&self) -> Result<(), ()> { self.reader .dequeue(|buf| { - defmt::trace!("read ok got {}", buf); if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { Ok(()) } else { diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index aa8f68d0..d62b4ee4 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -202,7 +202,7 @@ impl< let read_to_idle_start_time = Instant::now(); match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { Either::First(len) => { - defmt::info!("elapsed time to process and restart read_to_idle was: {}", read_to_idle_start_time - time_ended_trx); + // defmt::info!("elapsed time to process and restart read_to_idle was: {}", read_to_idle_start_time - time_ended_trx); time_ended_trx = Instant::now(); From 69fa690a69fe5e42e665e8092745f036be71f01e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 22 May 2024 20:34:54 -0400 Subject: [PATCH 044/157] radio refactor work --- control-board/src/bin/control/main.rs | 4 +- control-board/src/bin/hwtest-radio/main.rs | 10 +- control-board/src/drivers/radio_robot.rs | 25 +- control-board/src/lib.rs | 1 + control-board/src/robot_state.rs | 52 ++- control-board/src/tasks/control_task.rs | 6 +- control-board/src/tasks/radio_task.rs | 365 ++++++++++++++++++++- control-board/src/tasks/user_io_task.rs | 10 +- 8 files changed, 438 insertions(+), 35 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 01fd1eb9..f9083cd0 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -9,7 +9,7 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::RobotState, tasks::{control_task::start_control_task, imu_task::start_imu_task, radio_task::start_radio_task, user_io_task::start_io_task}}; +use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{control_task::start_control_task, imu_task::start_imu_task, radio_task::start_radio_task, user_io_task::start_io_task}}; // load credentials from correct crate @@ -23,7 +23,7 @@ use embassy_time::Timer; use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(RobotState::new()); +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 32a74cd7..c92ef88e 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -11,7 +11,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::RobotState, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; +use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; // load credentials from correct crate @@ -25,7 +25,7 @@ use embassy_time::Timer; use panic_probe as _; use static_cell::ConstStaticCell; -static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(RobotState::new()); +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); @@ -72,13 +72,13 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - let wifi_network = wifi_credentials[0]; - defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); + // let wifi_network = wifi_credentials[0]; + // defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); start_radio_task( main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, - wifi_network, + &wifi_credentials, p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, p.DMA2_CH1, p.DMA2_CH0, p.PC13, p.PE4).await; diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 2a4cdea5..58cf5d6c 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -11,7 +11,7 @@ use core::fmt::Write; use core::mem::size_of; use embassy_futures::select::{select, Either}; use embassy_stm32::gpio::{Level, Pin, Speed, Output}; -use embassy_stm32::usart::{self, DataBits, StopBits}; +use embassy_stm32::usart::{self, DataBits, Parity, StopBits}; use embassy_time::{Duration, Timer}; use heapless::String; @@ -112,7 +112,30 @@ impl< } } + pub fn get_startup_uart_config(&self) -> usart::Config { + let mut startup_radio_uart_config = usart::Config::default(); + startup_radio_uart_config.baudrate = 115_200; + startup_radio_uart_config.data_bits = DataBits::DataBits8; + startup_radio_uart_config.stop_bits = StopBits::STOP1; + startup_radio_uart_config.parity = Parity::ParityNone; + + startup_radio_uart_config + } + + pub fn get_highspeed_uart_config(&self) -> usart::Config { + let mut highspeed_radio_uart_config = usart::Config::default(); + highspeed_radio_uart_config.baudrate = 5_250_000; + highspeed_radio_uart_config.stop_bits = StopBits::STOP1; + highspeed_radio_uart_config.data_bits = DataBits::DataBits9; + highspeed_radio_uart_config.parity = usart::Parity::ParityEven; + + highspeed_radio_uart_config + } + pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { + // were about to reset the radio, so we also need to reset the uart queue config to match the startup config + self.radio.update_host_uart_config(self.get_startup_uart_config()); + // reset the radio so we can listen for the startup event self.reset_pin.set_high(); Timer::after(Duration::from_millis(1)).await; diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 6412394c..5eac9d51 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -12,6 +12,7 @@ #![feature(ptr_metadata)] #![feature(const_fn_floating_point_arithmetic)] #![feature(sync_unsafe_cell)] +#![feature(inherent_associated_types)] use embassy_stm32::{ bind_interrupts, peripherals, rcc::{ diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index f4f5434c..748f284d 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -1,6 +1,6 @@ use core::sync::atomic::{AtomicBool, AtomicU32, AtomicU8, Ordering}; -pub struct RobotState { +pub struct SharedRobotState { hw_init_state_valid: AtomicBool, hw_robot_id: AtomicU8, @@ -24,9 +24,9 @@ pub struct RobotState { battery_ok: AtomicBool, } -impl RobotState { - pub const fn new() -> RobotState { - RobotState { +impl SharedRobotState { + pub const fn new() -> SharedRobotState { + SharedRobotState { hw_init_state_valid: AtomicBool::new(false), hw_robot_id: AtomicU8::new(0), hw_robot_team_is_blue: AtomicBool::new(false), @@ -44,6 +44,28 @@ impl RobotState { } } + pub fn get_state(&self) -> RobotState { + RobotState { + hw_init_state_valid: self.hw_init_state_valid(), + hw_robot_id: self.get_hw_robot_id(), + hw_robot_team_is_blue: self.hw_robot_team_is_blue(), + hw_wifi_network_index: 0, + hw_debug_mode: self.hw_in_debug_mode(), + + radio_inop: true, + imu_inop: true, + kicker_inop: true, + wheels_inop: 0xFF, + dribbler_inop: true, + + last_packet_receive_time_ticks_ms: 0, + radio_connection_ok: false, + + battery_pct: 0, + battery_ok: false, + } + } + pub fn hw_init_state_valid(&self) -> bool { self.hw_init_state_valid.load(Ordering::Relaxed) } @@ -75,4 +97,26 @@ impl RobotState { pub fn set_hw_in_debug_mode(&self, in_debug_mode: bool) { self.hw_debug_mode.store(in_debug_mode, Ordering::Relaxed); } +} + +#[derive(Clone, Copy, PartialEq, Debug)] +pub struct RobotState { + pub hw_init_state_valid: bool, + + pub hw_robot_id: u8, + pub hw_robot_team_is_blue: bool, + pub hw_wifi_network_index: u8, + pub hw_debug_mode: bool, + + pub radio_inop: bool, + pub imu_inop: bool, + pub kicker_inop: bool, + pub wheels_inop: u8, + pub dribbler_inop: bool, + + pub last_packet_receive_time_ticks_ms: u32, + pub radio_connection_ok: bool, + + pub battery_pct: u8, + pub battery_ok: bool, } \ No newline at end of file diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 9ca839ad..9e1cb166 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -3,7 +3,7 @@ use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; use embassy_time::Timer; -use crate::{include_external_cpp_bin, pins::*, robot_state::{self, RobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -46,7 +46,7 @@ make_uart_queue_pair!(DRIB, #[embassy_executor::task] async fn control_task_entry( - robot_state: &'static RobotState, + robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -141,7 +141,7 @@ async fn control_task_entry( pub async fn start_control_task( uart_queue_spawner: SendSpawner, control_task_spawner: Spawner, - robot_state: &'static RobotState, + robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 444df018..ca2080a1 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,17 +1,18 @@ use ateam_common_packets::radio::TelemetryPacket; -use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; +use ateam_lib_stm32::{drivers::radio::odin_w26x::Radio, make_uart_queue_pair, queue_pair_register_and_spawn, uart::{self, queue::{UartReadQueue, UartWriteQueue}}}; use credentials::WifiCredential; -use embassy_executor::{SendSpawner, Spawner}; +use embassy_executor::{raw::TaskStorage, SendSpawner, SpawnToken, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ - gpio::{Input, Pull}, + gpio::{Input, Pin, Pull}, usart::{self, DataBits, Parity, StopBits, Uart} }; use embassy_sync::pubsub::WaitResult; use embassy_time::{Duration, Timer}; +use futures_util::Future; -use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::RobotState, SystemIrqs}; +use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::{RobotState, SharedRobotState}, SystemIrqs}; pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -24,9 +25,343 @@ make_uart_queue_pair!(RADIO, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); +// pub type RadioTaskFuture, +// UartTxDma: usart::TxDma, +// const RADIO_MAX_TX_PACKET_SIZE: usize, +// const RADIO_MAX_RX_PACKET_SIZE: usize, +// const RADIO_TX_BUF_DEPTH: usize, +// const RADIO_RX_BUF_DEPTH: usize> +// = impl Future; + +#[derive(Clone, Copy, PartialEq, PartialOrd, Debug)] +enum RadioConnectionState { + Unconnected, + ConnectPhys, + ConnectUart, + ConnectNetwork, + ConnectSoftware, + Connected, +} + +pub struct RadioTask< + UartPeripherial: usart::BasicInstance, + UartRxDma: usart::RxDma, + UartTxDma: usart::TxDma, + const RADIO_MAX_TX_PACKET_SIZE: usize, + const RADIO_MAX_RX_PACKET_SIZE: usize, + const RADIO_TX_BUF_DEPTH: usize, + const RADIO_RX_BUF_DEPTH: usize> { + shared_robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + radio: RobotRadio<'static, UartPeripherial, UartRxDma, UartTxDma, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH>, + radio_ndet_input: Input<'static>, + + connection_state: RadioConnectionState, + wifi_credentials: &'static [WifiCredential], + + // task: TaskStorage>, +} + +impl< + UartPeripherial: usart::BasicInstance, + UartRxDma: usart::RxDma, + UartTxDma: usart::TxDma, + const RADIO_MAX_TX_PACKET_SIZE: usize, + const RADIO_MAX_RX_PACKET_SIZE: usize, + const RADIO_TX_BUF_DEPTH: usize, + const RADIO_RX_BUF_DEPTH: usize> + RadioTask { + pub type TaskRobotRadio = RobotRadio<'static, UartPeripherial, UartRxDma, UartTxDma, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH>; + + const RETRY_DELAY_MS: u64 = 1000; + const UART_CONNECT_TIMEOUT_MS: u64 = 5000; + + pub fn new(robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + radio: Self::TaskRobotRadio, + radio_ndet_input: Input<'static>, + wifi_credentials: &'static [WifiCredential]) -> Self { + RadioTask { + shared_robot_state: robot_state, + command_publisher: command_publisher, + telemetry_subscriber: telemetry_subscriber, + radio: radio, + radio_ndet_input: radio_ndet_input, + connection_state: RadioConnectionState::Unconnected, + wifi_credentials: wifi_credentials, + // task: TaskStorage::new(), + } + } + + pub fn new_from_pins(robot_state: &'static SharedRobotState, + command_publisher: CommandsPublisher, + telemetry_subscriber: TelemetrySubcriber, + radio_rx_uart_queue: &'static UartReadQueue, + radio_tx_uart_queue: &'static UartWriteQueue, + radio_reset_pin: impl Pin, + radio_ndet_pin: impl Pin, + wifi_credentials: &'static [WifiCredential]) -> Self { + + let radio: Self::TaskRobotRadio = RobotRadio::new(radio_rx_uart_queue, radio_tx_uart_queue, radio_reset_pin); + + let radio_ndet = Input::new(radio_ndet_pin, Pull::None); + + Self::new(robot_state, command_publisher, telemetry_subscriber, radio, radio_ndet, wifi_credentials) + } + + async fn radio_task_entry(&mut self) { + defmt::info!("radio task startup"); + + // initialize a copy of the robot state so we can track updates + let mut last_robot_state = self.shared_robot_state.get_state(); + + // allow default fallback state transition of none + #[allow(unused_assignments)] + let mut next_connection_state = self.connection_state; + loop { + + // check for hardware config changes that affect radio connection + + let cur_robot_state = self.shared_robot_state.get_state(); + if cur_robot_state != last_robot_state { + // we are connected to software and robot id or team was changed on hardware + if self.connection_state == RadioConnectionState::Connected && + (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id + || cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) { + // enter software connect state (disconnecting) + self.connection_state = RadioConnectionState::ConnectSoftware; + } + + // we ar at least connected to Wifi and the wifi network id changed + if self.connection_state > RadioConnectionState::ConnectNetwork + && cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { + self.connection_state = RadioConnectionState::ConnectNetwork; + } + } + last_robot_state = cur_robot_state; + + // check for missing radio + + if self.radio_ndet_input.is_high() { + defmt::error!("radio appears unplugged."); + next_connection_state = RadioConnectionState::ConnectPhys; + } + + // execute on the connection state + + match self.connection_state { + RadioConnectionState::Unconnected => { + next_connection_state = RadioConnectionState::ConnectPhys; + }, + RadioConnectionState::ConnectPhys => { + if self.radio_ndet_input.is_high() { + Timer::after_millis(Self::RETRY_DELAY_MS).await; + next_connection_state = RadioConnectionState::ConnectPhys; + } else { + next_connection_state = RadioConnectionState::ConnectUart; + } + }, + RadioConnectionState::ConnectUart => { + if self.connect_uart().await.is_err() { + Timer::after_millis(Self::RETRY_DELAY_MS).await; + // failed to connect, go back to physical connection check + next_connection_state = RadioConnectionState::ConnectPhys; + } else { + next_connection_state = RadioConnectionState::ConnectNetwork; + } + }, + RadioConnectionState::ConnectNetwork => { + let wifi_network_ind = cur_robot_state.hw_wifi_network_index as usize; + let wifi_credential = if wifi_network_ind >= self.wifi_credentials.len() { + self.wifi_credentials[self.wifi_credentials.len() - 1] + } else { + self.wifi_credentials[wifi_network_ind] + }; + + defmt::debug!("connecting to network ({}): ssid: {}, password: {}", wifi_network_ind, wifi_credential.get_ssid(), wifi_credential.get_password()); + if self.connect_network(wifi_credential, cur_robot_state.hw_robot_id).await.is_err() { + Timer::after_millis(Self::RETRY_DELAY_MS).await; + // TODO make error handling smarter, e.g. if we get a timeout or low level errors + // we should fall back to ConnectPhys or ConnectUart, not keep retrying forever + } else { + next_connection_state = RadioConnectionState::ConnectSoftware + } + }, + RadioConnectionState::ConnectSoftware => { + if let Ok(connected) = self.connect_software(cur_robot_state.hw_robot_id, cur_robot_state.hw_robot_team_is_blue).await { + if connected { + next_connection_state = RadioConnectionState::Connected; + } else { + // software didn't respond to our hello, it may not be started yet + Timer::after_millis(1000).await; + } + } else { + // a hard error occurred + Timer::after_millis(Self::RETRY_DELAY_MS).await; + + // TODO where should we retry? + } + }, + RadioConnectionState::Connected => { + let _ = self.process_packet().await; + // no delay to imediately process the next one + }, + } + + // update state + self.connection_state = next_connection_state; + } + + } + + async fn connect_uart(&mut self) -> Result<(), ()> { + defmt::info!("connecting radio uart"); + match select(self.radio.connect_uart(), Timer::after_millis(Self::UART_CONNECT_TIMEOUT_MS)).await { + Either::First(res) => { + if res.is_err() { + defmt::error!("failed to establish radio UART connection."); + return Err(()) + } else { + defmt::debug!("established radio uart coms"); + return Ok(()) + } + } + Either::Second(_) => { + defmt::error!("initial radio uart connection timed out"); + return Err(()) + } + } + } + + async fn connect_network(&mut self, wifi_network: WifiCredential, robot_id: u8) -> Result<(), ()> { + if self.radio.connect_to_network(wifi_network, robot_id).await.is_err() { + defmt::error!("failed to connect to wifi network."); + return Err(()); + } + defmt::info!("radio connected"); + + let res = self.radio.open_multicast().await; + if res.is_err() { + defmt::error!("failed to establish multicast socket to network."); + return Err(()) + } + defmt::info!("multicast open"); + + return Ok(()) + } + + async fn connect_software(&mut self, robot_id: u8, is_blue: bool) -> Result { + defmt::info!("sending hello"); + + let team_color = if is_blue { + TeamColor::Blue + } else { + TeamColor::Yellow + }; + + if self.radio.send_hello(robot_id, team_color).await.is_err() { + return Err(()) + } + + let hello = self.radio.wait_hello(Duration::from_millis(1000)).await; + match hello { + Ok(hello) => { + defmt::info!( + "recieved hello resp to: {}.{}.{}.{}:{}", + hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port + ); + self.radio.close_peer().await.unwrap(); + defmt::info!("multicast peer closed"); + + self.radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); + defmt::info!("unicast open"); + + Ok(true) + } + Err(_) => { + Ok(false) + } + } + } + + async fn process_packet(&mut self) -> Result<(), ()> { + match select(self.radio.read_packet(), self.telemetry_subscriber.next_message()).await { + Either::First(res) => { + if let Ok(c2_pkt) = res { + self.command_publisher.publish_immediate(c2_pkt); + Ok(()) + } else { + defmt::warn!("radio read packet returned an error"); + Err(()) + } + } + Either::Second(telem_msg) => { + match telem_msg { + WaitResult::Lagged(num_missed_pkts) => { + defmt::warn!("radio task missed sending {} outbound packets. Should channel have higher capacity?", num_missed_pkts); + Ok(()) + }, + WaitResult::Message(msg) => { + match msg { + TelemetryPacket::Basic(basic_telem_pkt) => { + let res = self.radio.send_telemetry(basic_telem_pkt).await; + if res.is_err() { + defmt::warn!("radio task failed to send basic telemetry packet. Is the backing queue too small?"); + Err(()) + } else { + Ok(()) + } + } + TelemetryPacket::Control(control_telem_pkt) => { + let res = self.radio.send_control_debug_telemetry(control_telem_pkt).await; + if res.is_err() { + defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); + Err(()) + } else { + Ok(()) + } + } + TelemetryPacket::ParameterCommandResponse(param_resp) => { + let res = self.radio.send_parameter_response(param_resp).await; + if res.is_err() { + defmt::warn!("radio task failed to send param resp packet. Is the backing queue too small?"); + Err(()) + } else { + Ok(()) + } + } + } + } + } + } + } + } +} + +pub fn startup_uart_config() -> usart::Config { + let mut radio_uart_config = usart::Config::default(); + radio_uart_config.baudrate = 115_200; + radio_uart_config.data_bits = DataBits::DataBits8; + radio_uart_config.stop_bits = StopBits::STOP1; + radio_uart_config.parity = Parity::ParityNone; + + radio_uart_config +} + +#[embassy_executor::task] +async fn radio_task_entry(mut radio_task: RadioTask) { + loop { + radio_task.radio_task_entry().await; + defmt::error!("radio task returned"); + } +} + #[embassy_executor::task] -async fn radio_task_entry( - robot_state: &'static RobotState, +async fn radio_task_entry2( + robot_state: &'static SharedRobotState, command_publisher: CommandsPublisher, mut telemetry_subscriber: TelemetrySubcriber, wifi_network: WifiCredential, @@ -174,10 +509,10 @@ async fn radio_task_entry( pub async fn start_radio_task(radio_task_spawner: Spawner, queue_spawner: SendSpawner, - robot_state: &'static RobotState, + robot_state: &'static SharedRobotState, command_publisher: CommandsPublisher, telemetry_subscriber: TelemetrySubcriber, - wifi_network: WifiCredential, + wifi_credentials: &'static [WifiCredential], radio_uart: RadioUART, radio_uart_rx_pin: RadioUartRxPin, radio_uart_tx_pin: RadioUartTxPin, @@ -188,17 +523,17 @@ pub async fn start_radio_task(radio_task_spawner: Spawner, radio_reset_pin: RadioResetPin, radio_ndet_pin: RadioNDetectPin) { - let mut radio_uart_config = usart::Config::default(); - radio_uart_config.baudrate = 115_200; - radio_uart_config.data_bits = DataBits::DataBits8; - radio_uart_config.stop_bits = StopBits::STOP1; - radio_uart_config.parity = Parity::ParityNone; - let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); + let uart_conifg = startup_uart_config(); + let radio_uart = Uart::new(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, radio_uart_tx_dma, radio_uart_rx_dma, uart_conifg).unwrap(); // let radio_uart = Uart::new_with_rtscts(radio_uart, radio_uart_rx_pin, radio_uart_tx_pin, SystemIrqs, _radio_uart_rts_pin, _radio_uart_cts_pin, radio_uart_tx_dma, radio_uart_rx_dma, radio_uart_config).unwrap(); let (radio_uart_tx, radio_uart_rx) = Uart::split(radio_uart); queue_pair_register_and_spawn!(queue_spawner, RADIO, radio_uart_rx, radio_uart_tx); - radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); + let radio_task = RadioTask::new_from_pins(robot_state, command_publisher, telemetry_subscriber, &RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin, radio_ndet_pin, wifi_credentials); + + radio_task_spawner.spawn(radio_task_entry(radio_task)).unwrap(); + + // radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); } \ No newline at end of file diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index a74832d5..94773ca5 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -5,13 +5,13 @@ use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_time::Timer; use crate::drivers::shell_indicator::ShellIndicator; -use crate::robot_state::RobotState; +use crate::robot_state::SharedRobotState; use crate::pins::*; #[embassy_executor::task] -async fn user_io_task_entry(robot_state: &'static RobotState, +async fn user_io_task_entry(robot_state: &'static SharedRobotState, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, @@ -20,8 +20,8 @@ async fn user_io_task_entry(robot_state: &'static RobotState, loop { // read switches let robot_id = robot_id_rotary.read_value(); - let robot_team_isblue = dip_switch.read_pin(6); - let robot_debug_mode = dip_switch.read_pin(5); + let robot_team_isblue = dip_switch.read_pin(0); + let robot_debug_mode = dip_switch.read_pin(1); let glob_robot_debug = robot_state.hw_in_debug_mode(); if robot_debug_mode != glob_robot_debug { @@ -65,7 +65,7 @@ async fn user_io_task_entry(robot_state: &'static RobotState, } pub fn start_io_task(spawner: Spawner, - robot_state: &'static RobotState, + robot_state: &'static SharedRobotState, usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, From b13cf32812d96c3f3daa8c75555c5e16d458fd39 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 27 May 2024 22:58:00 -0400 Subject: [PATCH 045/157] dip driver working, some progress on live radio reconfig --- control-board/src/bin/hwtest-io/main.rs | 59 ++++++++++++++++++++++ control-board/src/bin/hwtest-radio/main.rs | 2 +- control-board/src/drivers/radio_robot.rs | 13 ++++- control-board/src/robot_state.rs | 12 ++++- control-board/src/tasks/radio_task.rs | 23 ++++----- control-board/src/tasks/user_io_task.rs | 42 ++++++++------- lib-stm32/src/drivers/radio/odin_w26x.rs | 26 ++++++++++ lib-stm32/src/drivers/switches/dip.rs | 13 +++-- 8 files changed, 152 insertions(+), 38 deletions(-) create mode 100644 control-board/src/bin/hwtest-io/main.rs diff --git a/control-board/src/bin/hwtest-io/main.rs b/control-board/src/bin/hwtest-io/main.rs new file mode 100644 index 00000000..c64b7357 --- /dev/null +++ b/control-board/src/bin/hwtest-io/main.rs @@ -0,0 +1,59 @@ +#![no_std] +#![no_main] + +use embassy_executor::InterruptExecutor; +use embassy_stm32::interrupt; + +use defmt_rtt as _; + +use ateam_control_board::{get_system_config, robot_state::SharedRobotState, tasks::user_io_task::start_io_task}; + + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); + +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + /////////////////// + // start tasks // + /////////////////// + + start_io_task(main_spawner, + robot_state, + p.PD6, p.PD5, p.EXTI6, p.EXTI5, + p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, + p.PG12, p.PG11, p.PG10, p.PG9, + p.PF3, p.PF2, p.PF1, p.PF0, + p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); + + loop { + Timer::after_millis(1000).await; + } +} \ No newline at end of file diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index c92ef88e..731e3d7e 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -142,7 +142,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { } } Either::Second(_) => { - defmt::warn!("packet processing timed out."); + // defmt::warn!("packet processing timed out."); } } } diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 58cf5d6c..bed494dd 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -134,7 +134,9 @@ impl< pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { // were about to reset the radio, so we also need to reset the uart queue config to match the startup config - self.radio.update_host_uart_config(self.get_startup_uart_config()); + if self.radio.update_host_uart_config(self.get_startup_uart_config()).await.is_err() { + defmt::debug!("failed to reset host uart to startup config."); + } // reset the radio so we can listen for the startup event self.reset_pin.set_high(); @@ -198,7 +200,16 @@ impl< Ok(()) } + pub async fn disconnect_network(&mut self) -> Result<(), RobotRadioError> { + let _ = self.radio.disconnect_wifi(1).await; + + Ok(()) + } + pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential, robot_number: u8) -> Result<(), RobotRadioError> { + // TODO better error handling here + let _ = self.disconnect_network().await; + // set radio hardware name enumeration let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 748f284d..848cf625 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -49,7 +49,7 @@ impl SharedRobotState { hw_init_state_valid: self.hw_init_state_valid(), hw_robot_id: self.get_hw_robot_id(), hw_robot_team_is_blue: self.hw_robot_team_is_blue(), - hw_wifi_network_index: 0, + hw_wifi_network_index: self.hw_wifi_network_index(), hw_debug_mode: self.hw_in_debug_mode(), radio_inop: true, @@ -90,6 +90,14 @@ impl SharedRobotState { self.hw_robot_team_is_blue.store(is_blue, Ordering::Relaxed); } + pub fn hw_wifi_network_index(&self) -> usize { + self.hw_wifi_network_index.load(Ordering::Relaxed) as usize + } + + pub fn set_hw_network_index(&self, ind: u8) { + self.hw_wifi_network_index.store(ind, Ordering::Relaxed); + } + pub fn hw_in_debug_mode(&self) -> bool { self.hw_debug_mode.load(Ordering::Relaxed) } @@ -105,7 +113,7 @@ pub struct RobotState { pub hw_robot_id: u8, pub hw_robot_team_is_blue: bool, - pub hw_wifi_network_index: u8, + pub hw_wifi_network_index: usize, pub hw_debug_mode: bool, pub radio_inop: bool, diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index ca2080a1..cc36c8f2 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -120,7 +120,7 @@ impl< // allow default fallback state transition of none #[allow(unused_assignments)] - let mut next_connection_state = self.connection_state; + // let mut next_connection_state = self.connection_state; loop { // check for hardware config changes that affect radio connection @@ -133,12 +133,14 @@ impl< || cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) { // enter software connect state (disconnecting) self.connection_state = RadioConnectionState::ConnectSoftware; + defmt::info!("shell state change triggering software reconnect"); } // we ar at least connected to Wifi and the wifi network id changed if self.connection_state > RadioConnectionState::ConnectNetwork && cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { self.connection_state = RadioConnectionState::ConnectNetwork; + defmt::info!("dip state change triggering wifi network change"); } } last_robot_state = cur_robot_state; @@ -147,30 +149,30 @@ impl< if self.radio_ndet_input.is_high() { defmt::error!("radio appears unplugged."); - next_connection_state = RadioConnectionState::ConnectPhys; + self.connection_state = RadioConnectionState::ConnectPhys; } // execute on the connection state match self.connection_state { RadioConnectionState::Unconnected => { - next_connection_state = RadioConnectionState::ConnectPhys; + self.connection_state = RadioConnectionState::ConnectPhys; }, RadioConnectionState::ConnectPhys => { if self.radio_ndet_input.is_high() { Timer::after_millis(Self::RETRY_DELAY_MS).await; - next_connection_state = RadioConnectionState::ConnectPhys; + self.connection_state = RadioConnectionState::ConnectPhys; } else { - next_connection_state = RadioConnectionState::ConnectUart; + self.connection_state = RadioConnectionState::ConnectUart; } }, RadioConnectionState::ConnectUart => { if self.connect_uart().await.is_err() { Timer::after_millis(Self::RETRY_DELAY_MS).await; // failed to connect, go back to physical connection check - next_connection_state = RadioConnectionState::ConnectPhys; + self.connection_state = RadioConnectionState::ConnectPhys; } else { - next_connection_state = RadioConnectionState::ConnectNetwork; + self.connection_state = RadioConnectionState::ConnectNetwork; } }, RadioConnectionState::ConnectNetwork => { @@ -187,13 +189,13 @@ impl< // TODO make error handling smarter, e.g. if we get a timeout or low level errors // we should fall back to ConnectPhys or ConnectUart, not keep retrying forever } else { - next_connection_state = RadioConnectionState::ConnectSoftware + self.connection_state = RadioConnectionState::ConnectSoftware; } }, RadioConnectionState::ConnectSoftware => { if let Ok(connected) = self.connect_software(cur_robot_state.hw_robot_id, cur_robot_state.hw_robot_team_is_blue).await { if connected { - next_connection_state = RadioConnectionState::Connected; + self.connection_state = RadioConnectionState::Connected; } else { // software didn't respond to our hello, it may not be started yet Timer::after_millis(1000).await; @@ -210,9 +212,6 @@ impl< // no delay to imediately process the next one }, } - - // update state - self.connection_state = next_connection_state; } } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 94773ca5..149685c6 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -18,43 +18,49 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, mut robot_id_indicator: ShellIndicator<'static>, ) { loop { + let cur_robot_state = robot_state.get_state(); + // read switches let robot_id = robot_id_rotary.read_value(); + let robot_team_isblue = dip_switch.read_pin(0); - let robot_debug_mode = dip_switch.read_pin(1); + let hw_debug_mode = dip_switch.read_pin(1); + let robot_network_index = dip_switch.read_block(6..4); - let glob_robot_debug = robot_state.hw_in_debug_mode(); - if robot_debug_mode != glob_robot_debug { - robot_state.set_hw_in_debug_mode(robot_debug_mode); - if robot_debug_mode { + if hw_debug_mode != cur_robot_state.hw_debug_mode { + robot_state.set_hw_in_debug_mode(hw_debug_mode); + if hw_debug_mode { defmt::info!("robot entered debug mode"); } } - if robot_debug_mode { - debug_led0.set_high(); - } else { - debug_led0.set_low(); - } - // publish updates to robot_state - let glob_robot_id = robot_state.get_hw_robot_id(); - let glob_robot_is_blue = robot_state.hw_robot_team_is_blue(); - if robot_id != glob_robot_id { + if robot_id != cur_robot_state.hw_robot_id { robot_state.set_hw_robot_id(robot_id); - defmt::info!("updated robot id {} -> {}", glob_robot_id, robot_id); + defmt::info!("updated robot id {} -> {}", cur_robot_state.hw_robot_id, robot_id); } - if robot_team_isblue != glob_robot_is_blue { + if robot_team_isblue != cur_robot_state.hw_robot_team_is_blue { robot_state.set_hw_robot_team_is_blue(robot_team_isblue); - defmt::info!("updated robot team is blue {} -> {}", glob_robot_is_blue, robot_team_isblue); + defmt::info!("updated robot team is blue {} -> {}", cur_robot_state.hw_robot_team_is_blue, robot_team_isblue); } - + + if robot_network_index != cur_robot_state.hw_wifi_network_index as u8 { + robot_state.set_hw_network_index(robot_network_index); + defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); + } + // TODO read messages // update indicators robot_id_indicator.set(robot_id, robot_team_isblue); + if hw_debug_mode { + debug_led0.set_high(); + } else { + debug_led0.set_low(); + } + if !robot_state.hw_init_state_valid() { defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); robot_state.set_hw_init_state_valid(true); diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 8f6e3755..be9eae3f 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -1,5 +1,7 @@ use core::fmt::Write; +use embassy_futures::select::select; use embassy_stm32::usart; +use embassy_time::Timer; use heapless::String; use crate::uart::queue::{UartReadQueue, UartWriteQueue}; @@ -225,6 +227,30 @@ impl< todo!("implement if needed"); } + pub async fn disconnect_wifi(&self, config_id: u8) -> Result<(), ()> { + let mut str: String<64> = String::new(); + write!(str, "AT+UWSCA={config_id},4").or(Err(()))?; + self.send_command(str.as_str()).await?; + self.read_ok().await?; + + match select(self.reader.dequeue(|buf| { + let packet = self.to_packet(buf)?; + if let EdmPacket::ATEvent(ATEvent::NetworkDown { interface_id: 0 }) = packet { + return Ok(()); + } else { + return Err(()); + } + }), + Timer::after_millis(250)).await { + embassy_futures::select::Either::First(res) => { + return res; + }, + embassy_futures::select::Either::Second(_) => { + return Err(()); + }, + } + } + pub async fn connect_wifi(&self, config_id: u8) -> Result<(), ()> { let mut str: String<64> = String::new(); write!(str, "AT+UWSCA={config_id},3").or(Err(()))?; diff --git a/lib-stm32/src/drivers/switches/dip.rs b/lib-stm32/src/drivers/switches/dip.rs index 7d8e2dfc..6eae51fa 100644 --- a/lib-stm32/src/drivers/switches/dip.rs +++ b/lib-stm32/src/drivers/switches/dip.rs @@ -1,4 +1,4 @@ -use core::{cmp::min, ops::Range}; +use core::{cmp::{max, min}, ops::Range}; use embassy_stm32::gpio::{AnyPin, Input, Pull}; @@ -38,13 +38,18 @@ impl<'a, const PIN_CT: usize> DipSwitch<'a, PIN_CT> { } } - pub fn read_block(&self, r: &mut Range) -> u8 { - if !r.all(|val| 0 <= val && val < PIN_CT as i32) { + pub fn read_block(&self, mut pin_range: Range) -> u8 { + if !pin_range.all(|val| 0 <= val && val < PIN_CT as i32) { + defmt::warn!("dip switch block read outside valid range"); return 0; } + // since pins are physical, we'll use an inclusive range + let start_pin = min(pin_range.start, pin_range.end); + let end_pin = max(pin_range.start, pin_range.end) + 1; + let mut val: u8 = 0; - for i in r.enumerate() { + for i in (start_pin..end_pin).enumerate() { val |= (self.read_pin(i.1 as usize) as u8 & 0x1) << i.0; } From a152276eff91b9dc0a017275e1a91bb66cd19534 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 27 May 2024 23:57:52 -0400 Subject: [PATCH 046/157] some radio progress --- control-board/src/drivers/radio_robot.rs | 26 +++++++++++++++++------- control-board/src/tasks/radio_task.rs | 6 ++++-- lib-stm32/src/drivers/radio/odin_w26x.rs | 1 + 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index bed494dd..294b3c29 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -47,7 +47,7 @@ pub enum RobotRadioError { OpenMulticastError, - + DisconnectFailed, } unsafe impl< @@ -201,15 +201,27 @@ impl< } pub async fn disconnect_network(&mut self) -> Result<(), RobotRadioError> { - let _ = self.radio.disconnect_wifi(1).await; + let mut had_error = false; + if let Some(peer) = self.peer.take() { + if self.radio.close_peer(peer.peer_id).await.is_err() { + defmt::warn!("failed to close peer on network dc"); + had_error = true; + } + } - Ok(()) + if self.radio.disconnect_wifi(1).await.is_err() { + defmt::warn!("failed to disconnect network."); + had_error = true; + } + + if had_error { + Err(RobotRadioError::DisconnectFailed) + } else { + Ok(()) + } } pub async fn connect_to_network(&mut self, wifi_credential: WifiCredential, robot_number: u8) -> Result<(), RobotRadioError> { - // TODO better error handling here - let _ = self.disconnect_network().await; - // set radio hardware name enumeration let uid = uid::uid(); let uid_u16 = (uid[1] as u16) << 8 | uid[0] as u16; @@ -228,7 +240,7 @@ impl< let wifi_pass = WifiAuth::WPA { passphrase: wifi_credential.get_password(), }; - if self.radio.config_wifi(1, wifi_ssid,wifi_pass).await.is_err() { + if self.radio.config_wifi(1, wifi_ssid, wifi_pass).await.is_err() { defmt::trace!("could not configure wifi profile"); return Err(RobotRadioError::ConnectWifiBadConfig); } diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index cc36c8f2..c8d8df7a 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -60,8 +60,6 @@ pub struct RadioTask< connection_state: RadioConnectionState, wifi_credentials: &'static [WifiCredential], - - // task: TaskStorage>, } impl< @@ -139,6 +137,10 @@ impl< // we ar at least connected to Wifi and the wifi network id changed if self.connection_state > RadioConnectionState::ConnectNetwork && cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { + if self.radio.disconnect_network().await.is_err() { + defmt::error!("failed to cleanly disconnect - consider radio reboot"); + } + self.connection_state = RadioConnectionState::ConnectNetwork; defmt::info!("dip state change triggering wifi network change"); } diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index be9eae3f..171623c1 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -246,6 +246,7 @@ impl< return res; }, embassy_futures::select::Either::Second(_) => { + defmt::warn!("disconnect timed out"); return Err(()); }, } From 4cebafabe9a32d5727c8448a049d1725cfeef0c0 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 May 2024 18:14:17 -0400 Subject: [PATCH 047/157] radio work from last night --- control-board/src/bin/control/main.rs | 35 ++---- control-board/src/bin/hwtest-imu/main.rs | 10 +- control-board/src/bin/hwtest-io/main.rs | 10 +- control-board/src/bin/hwtest-radio/main.rs | 26 ++-- control-board/src/drivers/radio_robot.rs | 12 ++ control-board/src/tasks/imu_task.rs | 13 ++ control-board/src/tasks/radio_task.rs | 31 ++++- control-board/src/tasks/user_io_task.rs | 18 ++- lib-stm32/src/drivers/radio/edm_protocol.rs | 1 + lib-stm32/src/drivers/radio/odin_w26x.rs | 119 +++++++++++++++--- .../src/drivers/switches/rotary_encoder.rs | 7 +- lib-stm32/src/uart/queue.rs | 7 -- 12 files changed, 191 insertions(+), 98 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index f9083cd0..7acfdece 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -3,13 +3,13 @@ use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::OutputType, interrupt, pac::Interrupt, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} + interrupt, pac::Interrupt, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} }; use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{control_task::start_control_task, imu_task::start_imu_task, radio_task::start_radio_task, user_io_task::start_io_task}}; +use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; // load credentials from correct crate @@ -91,31 +91,14 @@ async fn main(main_spawner: embassy_executor::Spawner) { // Timer::after_millis(1000).await; // } - start_io_task(main_spawner, + create_radio_task!(main_spawner, uart_queue_spawner, robot_state, - p.PD6, p.PD5, p.EXTI6, p.EXTI5, - p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, - p.PG12, p.PG11, p.PG10, p.PG9, - p.PF3, p.PF2, p.PF1, p.PF0, - p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); - - let wifi_network = wifi_credentials[0]; - // start_radio_task( - // uart_queue_spawner, main_spawner, - // robot_state, - // radio_command_publisher, radio_telemetry_subscriber, - // wifi_network, - // p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, - // p.DMA2_CH1, p.DMA2_CH0, - // p.PC13, p.PE4); - - // start_imu_task(&main_spawner, - // imu_gyro_data_publisher, imu_accel_data_publisher, - // p.SPI1, p.PA5, p.PA7, p.PA6, - // p.DMA2_CH7, p.DMA2_CH6, - // p.PA4, p.PC4, p.PC5, - // p.PB1, p.PB2, p.EXTI1, p.EXTI2, - // p.PF11); + radio_command_publisher, radio_telemetry_subscriber, + wifi_credentials, p); + + create_io_task!(main_spawner, robot_state, p); + + create_imu_task!(main_spawner, imu_gyro_data_publisher, imu_accel_data_publisher, p); start_control_task( uart_queue_spawner, main_spawner, diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index db739372..b7a8daf0 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -8,7 +8,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}, tasks::imu_task::start_imu_task}; +use ateam_control_board::{create_imu_task, get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}}; use embassy_time::Timer; // provide embedded panic probe @@ -50,13 +50,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - start_imu_task(&main_spawner, - imu_gyro_data_publisher, imu_accel_data_publisher, - p.SPI1, p.PA5, p.PA7, p.PA6, - p.DMA2_CH7, p.DMA2_CH6, - p.PA4, p.PC4, p.PC5, - p.PB1, p.PB2, p.EXTI1, p.EXTI2, - p.PF11); + create_imu_task!(main_spawner, imu_gyro_data_publisher, imu_accel_data_publisher, p); loop { match select::select3(imu_gyro_data_subscriber.next_message(), imu_accel_data_subscriber.next_message(), Timer::after_millis(1000)).await { diff --git a/control-board/src/bin/hwtest-io/main.rs b/control-board/src/bin/hwtest-io/main.rs index c64b7357..826dfcd7 100644 --- a/control-board/src/bin/hwtest-io/main.rs +++ b/control-board/src/bin/hwtest-io/main.rs @@ -6,7 +6,7 @@ use embassy_stm32::interrupt; use defmt_rtt as _; -use ateam_control_board::{get_system_config, robot_state::SharedRobotState, tasks::user_io_task::start_io_task}; +use ateam_control_board::{create_io_task, get_system_config, robot_state::SharedRobotState}; use embassy_time::Timer; @@ -45,13 +45,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - start_io_task(main_spawner, - robot_state, - p.PD6, p.PD5, p.EXTI6, p.EXTI5, - p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, - p.PG12, p.PG11, p.PG10, p.PG9, - p.PF3, p.PF2, p.PF1, p.PF0, - p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); + create_io_task!(main_spawner, robot_state, p); loop { Timer::after_millis(1000).await; diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index 731e3d7e..a191e196 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -11,7 +11,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{radio_task::start_radio_task, user_io_task::start_io_task}}; +use ateam_control_board::{create_io_task, create_radio_task, get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; // load credentials from correct crate @@ -72,24 +72,12 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - // let wifi_network = wifi_credentials[0]; - // defmt::info!("connecting with {}, {}", wifi_network.get_ssid(), wifi_network.get_password()); - start_radio_task( - main_spawner, uart_queue_spawner, - robot_state, - radio_command_publisher, radio_telemetry_subscriber, - &wifi_credentials, - p.USART10, p.PE2, p.PE3, p.PG13, p.PG14, - p.DMA2_CH1, p.DMA2_CH0, - p.PC13, p.PE4).await; - - start_io_task(main_spawner, - robot_state, - p.PD6, p.PD5, p.EXTI6, p.EXTI5, - p.PG7, p.PG6, p.PG5, p.PG4, p.PG3, p.PG2, p.PD15, - p.PG12, p.PG11, p.PG10, p.PG9, - p.PF3, p.PF2, p.PF1, p.PF0, - p.PD0, p.PD1, p.PD3, p.PD4, p.PD14); + create_radio_task!(main_spawner, uart_queue_spawner, + robot_state, + radio_command_publisher, radio_telemetry_subscriber, + wifi_credentials, p); + + create_io_task!(main_spawner, robot_state, p); loop { match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index 294b3c29..ed7ac3ce 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -203,15 +203,21 @@ impl< pub async fn disconnect_network(&mut self) -> Result<(), RobotRadioError> { let mut had_error = false; if let Some(peer) = self.peer.take() { + defmt::debug!("closing peer.."); if self.radio.close_peer(peer.peer_id).await.is_err() { defmt::warn!("failed to close peer on network dc"); had_error = true; + } else { + defmt::debug!("closed peer.") } } + defmt::debug!("closing wifi."); if self.radio.disconnect_wifi(1).await.is_err() { defmt::warn!("failed to disconnect network."); had_error = true; + } else { + defmt::debug!("disconnected wifi.") } if had_error { @@ -248,6 +254,12 @@ impl< // connect to config slot 1 if self.radio.connect_wifi(1).await.is_err() { defmt::trace!("could not connect to wifi"); + + // can never configure a profile that "active" even when unconnected + // we're not really in a known state with out a lot more effort + // so ignore the result + let _ = self.disconnect_network().await; + return Err(RobotRadioError::ConnectWifiConnectionFailed); } diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index f56dfd06..12dba3ff 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -14,6 +14,19 @@ use ateam_lib_stm32::drivers::imu::bmi323::{self, *}; use crate::pins::*; +#[macro_export] +macro_rules! create_imu_task { + ($main_spawner:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $p:ident) => { + ateam_control_board::tasks::imu_task::start_imu_task(&$main_spawner, + $imu_gyro_data_publisher, $imu_accel_data_publisher, + $p.SPI1, $p.PA5, $p.PA7, $p.PA6, + $p.DMA2_CH7, $p.DMA2_CH6, + $p.PA4, $p.PC4, $p.PC5, + $p.PB1, $p.PB2, $p.EXTI1, $p.EXTI2, + $p.PF11); + }; +} + #[link_section = ".axisram.buffers"] static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstStaticCell::new([0; bmi323::SPI_MIN_BUF_LEN]); diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index c8d8df7a..1e862f2e 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,8 +1,8 @@ use ateam_common_packets::radio::TelemetryPacket; -use ateam_lib_stm32::{drivers::radio::odin_w26x::Radio, make_uart_queue_pair, queue_pair_register_and_spawn, uart::{self, queue::{UartReadQueue, UartWriteQueue}}}; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; -use embassy_executor::{raw::TaskStorage, SendSpawner, SpawnToken, Spawner}; +use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; use embassy_stm32::{ gpio::{Input, Pin, Pull}, @@ -10,9 +10,24 @@ use embassy_stm32::{ }; use embassy_sync::pubsub::WaitResult; use embassy_time::{Duration, Timer}; -use futures_util::Future; -use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::{RobotState, SharedRobotState}, SystemIrqs}; +use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::SharedRobotState, SystemIrqs}; + +#[macro_export] +macro_rules! create_radio_task { + ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, + $radio_command_publisher:ident, $radio_telemetry_subscriber:ident, + $wifi_credentials:ident, $p:ident) => { + ateam_control_board::tasks::radio_task::start_radio_task( + $main_spawner, $uart_queue_spawner, + $robot_state, + $radio_command_publisher, $radio_telemetry_subscriber, + &$wifi_credentials, + $p.USART10, $p.PE2, $p.PE3, $p.PG13, $p.PG14, + $p.DMA2_CH1, $p.DMA2_CH0, + $p.PC13, $p.PE4).await; + }; +} pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; @@ -135,14 +150,18 @@ impl< } // we ar at least connected to Wifi and the wifi network id changed - if self.connection_state > RadioConnectionState::ConnectNetwork + if self.connection_state >= RadioConnectionState::ConnectNetwork && cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { + defmt::info!("dip state change triggering wifi network change"); + if self.radio.disconnect_network().await.is_err() { + // this is really only an error if we think we're connected + // this separation is really poorly handled right now + // TODO move all statefulness down into driver defmt::error!("failed to cleanly disconnect - consider radio reboot"); } self.connection_state = RadioConnectionState::ConnectNetwork; - defmt::info!("dip state change triggering wifi network change"); } } last_robot_state = cur_robot_state; diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 149685c6..8b8db7af 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,6 +1,6 @@ use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; -use embassy_executor::{SendSpawner, Spawner}; +use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_time::Timer; @@ -9,6 +9,18 @@ use crate::robot_state::SharedRobotState; use crate::pins::*; +#[macro_export] +macro_rules! create_io_task { + ($spawner:ident, $robot_state:ident, $p:ident) => { + ateam_control_board::tasks::user_io_task::start_io_task($spawner, + $robot_state, + $p.PD6, $p.PD5, $p.EXTI6, $p.EXTI5, + $p.PG7, $p.PG6, $p.PG5, $p.PG4, $p.PG3, $p.PG2, $p.PD15, + $p.PG12, $p.PG11, $p.PG10, $p.PG9, + $p.PF3, $p.PF2, $p.PF1, $p.PF0, + $p.PD0, $p.PD1, $p.PD3, $p.PD4, $p.PD14); + }; +} #[embassy_executor::task] async fn user_io_task_entry(robot_state: &'static SharedRobotState, @@ -72,12 +84,12 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, pub fn start_io_task(spawner: Spawner, robot_state: &'static SharedRobotState, - usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, + _usr_btn0_pin: UsrBtn0Pin, _usr_btn1_pin: UsrBtn1Pin, _usr_btn0_exti: UsrBtn0Exti, _usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, robot_id_selector1_pin: RobotIdSelector1Pin, robot_id_selector0_pin: RobotIdSelector0Pin, - usr_led0_pin: UsrLed0Pin, usr_led1_pin: UsrLed1Pin, usr_led2_pin: UsrLed2Pin, usr_led3_pin: UsrLed3Pin, + usr_led0_pin: UsrLed0Pin, _usr_led1_pin: UsrLed1Pin, _usr_led2_pin: UsrLed2Pin, _usr_led3_pin: UsrLed3Pin, robot_id_indicator_fl: RobotIdIndicator0FlPin, robot_id_indicator_bl: RobotIdIndicator1BlPin, robot_id_indicator_br: RobotIdIndicator2BrPin, robot_id_indicator_fr: RobotIdIndicator3FrPin, robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, diff --git a/lib-stm32/src/drivers/radio/edm_protocol.rs b/lib-stm32/src/drivers/radio/edm_protocol.rs index 08c8e3c8..6bea9c8e 100644 --- a/lib-stm32/src/drivers/radio/edm_protocol.rs +++ b/lib-stm32/src/drivers/radio/edm_protocol.rs @@ -5,6 +5,7 @@ use super::at_protocol::*; // Data to/from radio is in big endian mode, ensure proper conversions are done #[repr(C)] +#[derive(Debug)] struct EdmPacketRaw { payload_length: u16, // actually only 12bits, 4 bits reserved identifier: u16, diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 171623c1..ba193e5b 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -6,7 +6,7 @@ use heapless::String; use crate::uart::queue::{UartReadQueue, UartWriteQueue}; -use super::at_protocol::{ATEvent, ATResponse}; +use super::at_protocol::{ATEvent, ATResponse, WifiLinkDisconnectedReason}; use super::edm_protocol::EdmPacket; #[allow(dead_code)] @@ -233,17 +233,34 @@ impl< self.send_command(str.as_str()).await?; self.read_ok().await?; - match select(self.reader.dequeue(|buf| { - let packet = self.to_packet(buf)?; - if let EdmPacket::ATEvent(ATEvent::NetworkDown { interface_id: 0 }) = packet { - return Ok(()); - } else { - return Err(()); - } - }), - Timer::after_millis(250)).await { - embassy_futures::select::Either::First(res) => { - return res; + match select( + async move { + let mut run = true; + while run { + let _ = self.reader.dequeue(|buf| { + // defmt::warn!("buf {}", buf); + let packet = self.to_packet(buf); + if packet.is_err() { + defmt::warn!("parsed invalid packet"); + } else { + let packet = packet.unwrap(); + if let EdmPacket::ATEvent(ATEvent::NetworkDown { interface_id: 0 }) = packet { + defmt::debug!("got network down event."); + run = false; + } else if let EdmPacket::ATEvent(ATEvent::WifiLinkDisconnected { conn_id:_, reason: WifiLinkDisconnectedReason::NetworkDisabled }) = packet { + run = false; + } else if let EdmPacket::ATResponse(ATResponse::Ok(_)) = packet { + run = false; + } else { + defmt::warn!("got edm packet: {}", packet); + } + } + }).await; + } + }, + Timer::after_millis(2500)).await { + embassy_futures::select::Either::First(_) => { + return Ok(()); }, embassy_futures::select::Either::Second(_) => { defmt::warn!("disconnect timed out"); @@ -477,17 +494,81 @@ impl< } async fn read_ok(&self) -> Result<(), ()> { - self.reader - .dequeue(|buf| { - if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { - Ok(()) + let mut res = Err(()); + loop { + let brk = self.reader.dequeue(|buf| { + // defmt::warn!("buf: {}", buf); + let brk = if let Ok(packet) = self.to_packet(buf) { + match packet { + EdmPacket::ConnectEvent { channel, event_type } => false, + EdmPacket::DisconnectEvent { channel } => false, + EdmPacket::ATResponse(at_resp) => { + match at_resp { + ATResponse::Ok(_) => { + res = Ok(()); + true + }, + ATResponse::Error => return true, + ATResponse::Other(_) => { + defmt::debug!("unhandled ATResponse in read_ok()"); + false + }, + } + }, + EdmPacket::ATEvent(at_event) => { + match at_event { + ATEvent::PeerConnectedIP { peer_handle, is_ipv6, protocol, local_address, local_port, remote_address, remote_port } => false, + ATEvent::PeerDisconnected { peer_handle } => false, + ATEvent::WifiLinkConnected { conn_id, bssid, channel } => false, + ATEvent::WifiLinkDisconnected { conn_id, reason } => false, + ATEvent::WifiAccessPointUp { id } => false, + ATEvent::WifiAccessPointDown { id } => false, + ATEvent::WifiAccessPointStationConnected { id, mac_addr } => false, + ATEvent::WifiAccessPointStationDisconnected { id } => false, + ATEvent::NetworkUp { interface_id } => false, + ATEvent::NetworkDown { interface_id } => false, + ATEvent::NetworkError { interface_id, code } => false, + _ => { + defmt::debug!("ignoring ATEvent in read_ok(). event: {}", at_event); + false + } + } + }, + _ => { + defmt::debug!("ignoring unhandled EdmPacket variant in read_ok(), variant: {}", packet); + false + } + } } else { - Err(()) - } + defmt::debug!("read ok could not parse packet"); + false + }; + + return brk; }) - .await + .await; + + if brk { + break; + } + + } + + return res; } + // async fn read_ok(&self) -> Result<(), ()> { + // self.reader + // .dequeue(|buf| { + // if let EdmPacket::ATResponse(ATResponse::Ok("")) = self.to_packet(buf)? { + // Ok(()) + // } else { + // Err(()) + // } + // }) + // .await + // } + async fn read_ok_at_edm_transition(&self) -> Result { let transition_buf: [u8; 12] = [13, 10, 79, 75, 13, 10, 170, 0, 2, 0, 113, 85]; let res = self.reader.dequeue(|buf| { diff --git a/lib-stm32/src/drivers/switches/rotary_encoder.rs b/lib-stm32/src/drivers/switches/rotary_encoder.rs index 26d512c4..7b8d4751 100644 --- a/lib-stm32/src/drivers/switches/rotary_encoder.rs +++ b/lib-stm32/src/drivers/switches/rotary_encoder.rs @@ -7,13 +7,16 @@ pub struct RotaryEncoder<'a, const PIN_CT: usize> { } impl<'a, const PIN_CT: usize> RotaryEncoder<'a, PIN_CT> { - pub const fn new_from_inputs(inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { - let inversion_map = if let Some(map) = inversion_map { + pub fn new_from_inputs(mut inputs: [Input<'a>; PIN_CT], inversion_map: Option<[bool; PIN_CT]>) -> RotaryEncoder<'a, PIN_CT> { + let mut inversion_map = if let Some(map) = inversion_map { map } else { [false; PIN_CT] }; + inputs.reverse(); + inversion_map.reverse(); + RotaryEncoder { pins: inputs, inversion_map, diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index d62b4ee4..6f5b965b 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -19,7 +19,6 @@ use embassy_sync::{ mutex::{Mutex, MutexGuard}, pubsub::{PubSubChannel, Publisher, Subscriber, WaitResult}, }; -use embassy_time::{Duration, Instant, Timer}; use crate::queue::{ self, @@ -170,7 +169,6 @@ impl< // look like this might be taking too long to acquire between read_to_idle calls // need to acquire this upfront and be smarter about releasing and reacq it let mut rw_tasks_config_lock: Option> = Some(self.uart_mutex.lock().await); - let mut time_ended_trx = Instant::now(); loop { // block if/until we receive a signal telling to unpause the task because a config update is not active @@ -199,13 +197,8 @@ impl< // get enqueue ref to pass to the DMA layer let mut buf = queue_rx.enqueue().await.unwrap(); - let read_to_idle_start_time = Instant::now(); match select(rx.read_until_idle(buf.data()), uart_config_signal_subscriber.next_message()).await { Either::First(len) => { - // defmt::info!("elapsed time to process and restart read_to_idle was: {}", read_to_idle_start_time - time_ended_trx); - - time_ended_trx = Instant::now(); - if let Ok(len) = len { if len == 0 { defmt::debug!("uart zero"); From 304de621bc2d1791ca60f69d73755eb2fdd59f53 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 May 2024 18:40:06 -0400 Subject: [PATCH 048/157] formatting --- control-board/src/bin/control/main.rs | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 7acfdece..f14beef9 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -94,11 +94,16 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_radio_task!(main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, - wifi_credentials, p); + wifi_credentials, + p); - create_io_task!(main_spawner, robot_state, p); + create_io_task!(main_spawner, + robot_state, + p); - create_imu_task!(main_spawner, imu_gyro_data_publisher, imu_accel_data_publisher, p); + create_imu_task!(main_spawner, + imu_gyro_data_publisher, imu_accel_data_publisher, + p); start_control_task( uart_queue_spawner, main_spawner, From ea92480736322788c1ec34eeb5c4bcf21da16626 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 May 2024 21:18:11 -0400 Subject: [PATCH 049/157] control task tweaks --- control-board/src/tasks/control_task.rs | 95 +++++++++++++++++++++++++ 1 file changed, 95 insertions(+) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 9e1cb166..b3bbae38 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -2,6 +2,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; use embassy_time::Timer; +use nalgebra::{Vector3, Vector4}; use crate::{include_external_cpp_bin, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; @@ -125,6 +126,100 @@ async fn control_task_entry( Timer::after_millis(10).await; + // loop { + // motor_fl.process_packets(); + // motor_bl.process_packets(); + // motor_br.process_packets(); + // motor_fr.process_packets(); + // motor_drib.process_packets(); + + // motor_fl.log_reset("FL"); + // motor_bl.log_reset("BL"); + // motor_br.log_reset("BR"); + // motor_fr.log_reset("FR"); + // motor_drib.log_reset("DRIB"); + + // if motor_drib.ball_detected() { + // defmt::info!("ball detected"); + // } + + // if let Some(latest_control) = &latest_control { + // let cmd_vel = Vector3::new( + // latest_control.vel_x_linear, + // latest_control.vel_y_linear, + // latest_control.vel_z_angular, + // ); + // self.cmd_vel = cmd_vel; + // self.drib_vel = latest_control.dribbler_speed; + // self.ticks_since_packet = 0; + // } else { + // self.ticks_since_packet += 1; + // if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { + // self.cmd_vel = Vector3::new(0., 0., 0.); + // self.ticks_since_packet = 0; + // } + // } + + // // now we have setpoint r(t) in self.cmd_vel + // let battery_v = self.battery_sub.next_message_pure().await as f32; + // let controls_enabled = true; + // let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0; + // let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + // if controls_enabled + // { + // // TODO check order + // let wheel_vels = Vector4::new( + // motor_fr.read_rads(), + // motor_fl.read_rads(), + // motor_bl.read_rads(), + // motor_br.read_rads() + // ); + + // // torque values are computed on the spin but put in the current variable + // // TODO update this when packet/var names are updated to match software + // let wheel_torques = Vector4::new( + // self.front_right_motor.read_current(), + // self.front_left_motor.read_current(), + // self.back_left_motor.read_current(), + // self.back_right_motor.read_current() + // ); + + // // TODO read from channel or something + + // self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + + // self.robot_controller.get_wheel_velocities() + // } + // else + // { + // self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel) + // } + // } + // else + // { + // // Battery is too low, set velocity to zero + // Vector4::new( + // 0.0, + // 0.0, + // 0.0, + // 0.0) + // }; + + // self.front_right_motor.set_setpoint(wheel_vels[0]); + // self.front_left_motor.set_setpoint(wheel_vels[1]); + // self.back_left_motor.set_setpoint(wheel_vels[2]); + // self.back_right_motor.set_setpoint(wheel_vels[3]); + + // let drib_dc = -1.0 * self.drib_vel / 1000.0; + // self.drib_motor.set_setpoint(drib_dc); + + // self.front_right_motor.send_motion_command(); + // self.front_left_motor.send_motion_command(); + // self.back_left_motor.send_motion_command(); + // self.back_right_motor.send_motion_command(); + // self.drib_motor.send_motion_command(); + // } + loop { motor_fl.process_packets(); From 774691fbbb4b3bfa3f3cde0d84bee6903b1764b5 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 May 2024 23:09:19 -0400 Subject: [PATCH 050/157] kicker task work --- control-board/src/drivers/kicker.rs | 14 +- control-board/src/pins.rs | 6 +- control-board/src/robot_state.rs | 15 ++ control-board/src/tasks/kicker_task.rs | 206 +++++++++++++++++++++++++ control-board/src/tasks/mod.rs | 1 + 5 files changed, 240 insertions(+), 2 deletions(-) diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 43e5acad..7b1ce8b9 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -1,4 +1,5 @@ -use embassy_stm32::usart::{self, Parity}; +use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; +use embassy_stm32::{gpio::Pin, usart::{self, Parity}}; use embassy_time::{Duration, Timer}; use crate::stm32_interface::Stm32Interface; @@ -68,6 +69,17 @@ impl< } } + pub fn new_from_pins(read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: impl Pin, + reset_pin: impl Pin, + firmware_image: &'a [u8]) -> Self { + + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, true); + + Self::new(stm32_interface, firmware_image) + } + pub fn process_telemetry(&mut self) { while let Ok(res) = self.stm32_uart_interface.try_read_data() { let buf = res.data(); diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index b8141c9c..c0303061 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -71,11 +71,15 @@ pub type ExtImuNDetPin = PF11; ////////////// pub type KickerUart = USART6; +pub type KickerUartRxPin = PC7; +pub type KickerUartTxPin = PC6; pub type KickerRxDma = DMA2_CH5; pub type KickerTxDma = DMA2_CH4; pub type KickerPowerOnPin = PG8; pub type KickerBootPin = PA8; -pub type KickerResetPIn = PA9; +pub type KickerResetPin = PA9; +// pub type KickerNDetPin = ; + ////////////// diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 848cf625..66a11f2c 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -22,6 +22,8 @@ pub struct SharedRobotState { battery_pct: AtomicU8, battery_ok: AtomicBool, + + shutdown_requested: AtomicBool, } impl SharedRobotState { @@ -41,6 +43,7 @@ impl SharedRobotState { radio_connection_ok: AtomicBool::new(false), battery_pct: AtomicU8::new(0), battery_ok: AtomicBool::new(false), + shutdown_requested: AtomicBool::new(false), } } @@ -63,6 +66,8 @@ impl SharedRobotState { battery_pct: 0, battery_ok: false, + + shutdown_requested: self.shutdown_requested(), } } @@ -105,6 +110,14 @@ impl SharedRobotState { pub fn set_hw_in_debug_mode(&self, in_debug_mode: bool) { self.hw_debug_mode.store(in_debug_mode, Ordering::Relaxed); } + + pub fn shutdown_requested(&self) -> bool { + self.shutdown_requested.load(Ordering::Relaxed) + } + + pub fn flag_shutdown_requested(&self) { + self.shutdown_requested.store(true, Ordering::Relaxed); + } } #[derive(Clone, Copy, PartialEq, Debug)] @@ -127,4 +140,6 @@ pub struct RobotState { pub battery_pct: u8, pub battery_ok: bool, + + pub shutdown_requested: bool, } \ No newline at end of file diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index e69de29b..ddeb80a9 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -0,0 +1,206 @@ +use ateam_common_packets::radio::DataPacket; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; +use embassy_executor::{SendSpawner, Spawner}; +use embassy_stm32::{gpio::{Level, Output, Pin, Speed}, usart::{self, Uart}}; +use embassy_sync::pubsub::WaitResult; +use embassy_time::Timer; + +use crate::{drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, stm32_interface, SystemIrqs}; + +include_kicker_bin! {KICKER_FW_IMG, "hwtest-blinky.bin"} + +const MAX_TX_PACKET_SIZE: usize = 16; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 16; +const RX_BUF_DEPTH: usize = 20; + +make_uart_queue_pair!(KICKER, + KickerUart, KickerRxDma, KickerTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); + +#[derive(PartialEq, PartialOrd, Debug)] +enum KickerTaskState { + PoweredOff, + PowerOn, + ConnectUart, + Connected, + InitiateShutdown, + WaitShutdown, +} + +pub struct KickerTask<'a, + UART: usart::BasicInstance, + DmaRx: usart::RxDma, + DmaTx: usart::TxDma, + const LEN_RX: usize, + const LEN_TX: usize, + const DEPTH_RX: usize, + const DEPTH_TX: usize> { + kicker_driver: Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX>, + remote_power_btn: Output<'a>, + kicker_task_state: KickerTaskState, + robot_state: &'static SharedRobotState, + commands_subscriber: CommandsSubscriber, +} + +impl<'a, +UART: usart::BasicInstance, +DmaRx: usart::RxDma, +DmaTx: usart::TxDma, +const LEN_RX: usize, +const LEN_TX: usize, +const DEPTH_RX: usize, +const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { + pub fn new(kicker_driver: Kicker<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX>, + power_output: Output<'a>, + robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber) -> Self { + KickerTask { + kicker_driver: kicker_driver, + remote_power_btn: power_output, + kicker_task_state: KickerTaskState::PoweredOff, + robot_state: robot_state, + commands_subscriber: command_subscriber, + } + } + + pub fn new_from_pins(read_queue: &'a UartReadQueue, + write_queue: &'a UartWriteQueue, + boot0_pin: impl Pin, + reset_pin: impl Pin, + power_pin: impl Pin, + firmware_image: &'a [u8], + robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber) -> Self { + + let kicker_driver = Kicker::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, firmware_image); + let power_output = Output::new(power_pin, Level::Low, Speed::Medium); + + Self::new(kicker_driver, power_output, robot_state, command_subscriber) + } + + pub async fn kicker_task_entry(&mut self) { + loop { + let cur_robot_state = self.robot_state.get_state(); + + self.kicker_driver.process_telemetry(); + + // TODO global state overrides of kicker state + // e.g. external shutdown requsts, battery votlage, etc + if self.kicker_task_state == KickerTaskState::Connected && cur_robot_state.shutdown_requested { + self.kicker_task_state = KickerTaskState::InitiateShutdown; + } + + match self.kicker_task_state { + KickerTaskState::PoweredOff => { + if cur_robot_state.hw_init_state_valid && cur_robot_state.shutdown_requested { + self.kicker_task_state = KickerTaskState::PowerOn; + } + }, + KickerTaskState::PowerOn => { + self.remote_power_btn_press().await; + // power should be coming on, attempt connection + self.kicker_task_state = KickerTaskState::ConnectUart; + }, + KickerTaskState::ConnectUart => { + if self.kicker_driver.load_default_firmware_image().await.is_err() { + // attempt to power on the board again + // if the kicker is missing or bugged we'll stay in a power on -> attempt + // uart loop forever + self.kicker_task_state = KickerTaskState::PowerOn; + } else { + self.kicker_task_state = KickerTaskState::Connected; + } + }, + KickerTaskState::Connected => { + self.connected_poll_loop().await; + // external events will move us out of this state + }, + KickerTaskState::InitiateShutdown => { + self.kicker_driver.request_shutdown(); + self.kicker_task_state = KickerTaskState::WaitShutdown; + }, + KickerTaskState::WaitShutdown => { + if self.kicker_driver.shutdown_completed() { + self.kicker_task_state = KickerTaskState::PoweredOff; + } + }, + } + + // if we are in any substate of connected, then send + // commands to the kicker + if self.kicker_task_state >= KickerTaskState::Connected { + self.kicker_driver.send_command(); + } + } + } + + pub async fn remote_power_btn_press(&mut self) { + self.remote_power_btn.set_high(); + Timer::after_millis(200).await; + self.remote_power_btn.set_low(); + Timer::after_millis(10).await; + } + + pub async fn remote_power_btn_hold(&mut self) { + self.remote_power_btn.set_high(); + Timer::after_millis(1500).await; + self.remote_power_btn.set_low(); + Timer::after_millis(10).await; + } + + pub async fn connected_poll_loop(&mut self) { + if let Some(pkt) = self.commands_subscriber.try_next_message() { + match pkt { + WaitResult::Lagged(amnt) => { + defmt::warn!("kicker task lagged processing commands by {} msgs", amnt); + }, + WaitResult::Message(cmd) => { + match cmd { + DataPacket::BasicControl(bc_pkt) => { + self.kicker_driver.set_kick_strength(bc_pkt.kick_vel); + self.kicker_driver.request_kick(bc_pkt.kick_request); + }, + DataPacket::ParameterCommand(_) => { + // we currently don't have any kicker parameters + }, + } + }, + } + } + } +} + +#[embassy_executor::task] +async fn kicker_task_entry(mut kicker_task: KickerTask<'static, KickerUart, KickerRxDma, KickerTxDma, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>) { + loop { + kicker_task.kicker_task_entry().await; + defmt::error!("radio task returned"); + } +} + +pub async fn start_kicker_task(kicker_task_spawner: Spawner, + queue_spawner: SendSpawner, + robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + kicker_uart: KickerUart, + kicker_uart_rx_pin: KickerUartRxPin, + kicker_uart_tx_pin: KickerUartTxPin, + kicker_uart_rx_dma: KickerRxDma, + kicker_uart_tx_dma: KickerTxDma, + kicker_boot0_pin: KickerBootPin, + kicker_reset_pin: KickerResetPin, + kicker_power_pin: KickerPowerOnPin) { + + let initial_kicker_uart_conifg = stm32_interface::get_bootloader_uart_config(); + + let kicker_uart = Uart::new(kicker_uart, kicker_uart_rx_pin, kicker_uart_tx_pin, SystemIrqs, kicker_uart_tx_dma, kicker_uart_rx_dma, initial_kicker_uart_conifg).unwrap(); + + let (kicker_tx, kicker_rx) = Uart::split(kicker_uart); + queue_pair_register_and_spawn!(queue_spawner, KICKER, kicker_rx, kicker_tx); + + let kicker_task = KickerTask::new_from_pins(&KICKER_RX_UART_QUEUE, &KICKER_TX_UART_QUEUE, kicker_boot0_pin, kicker_reset_pin, kicker_power_pin, KICKER_FW_IMG, robot_state, command_subscriber); + kicker_task_spawner.spawn(kicker_task_entry(kicker_task)).unwrap(); +} \ No newline at end of file diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index 93eb1559..f5927642 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,5 +1,6 @@ // pub mod control; pub mod control_task; pub mod imu_task; +pub mod kicker_task; pub mod radio_task; pub mod user_io_task; \ No newline at end of file From afc4f10fc8e0c71cd4e8e0cf88aa9c2c869e7ab1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 May 2024 23:11:45 -0400 Subject: [PATCH 051/157] kicker task WIP --- control-board/src/tasks/kicker_task.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index ddeb80a9..2e664332 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -137,21 +137,21 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } } - pub async fn remote_power_btn_press(&mut self) { + async fn remote_power_btn_press(&mut self) { self.remote_power_btn.set_high(); Timer::after_millis(200).await; self.remote_power_btn.set_low(); Timer::after_millis(10).await; } - pub async fn remote_power_btn_hold(&mut self) { + async fn remote_power_btn_hold(&mut self) { self.remote_power_btn.set_high(); Timer::after_millis(1500).await; self.remote_power_btn.set_low(); Timer::after_millis(10).await; } - pub async fn connected_poll_loop(&mut self) { + async fn connected_poll_loop(&mut self) { if let Some(pkt) = self.commands_subscriber.try_next_message() { match pkt { WaitResult::Lagged(amnt) => { From 2bb97e19493f97d097bb43425f262db2edf64ee5 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 31 May 2024 22:41:00 -0400 Subject: [PATCH 052/157] peripheral work --- control-board/Cargo.lock | 35 +- control-board/src/bin/hwtest-shutdown/main.rs | 55 +++ control-board/src/pins.rs | 4 +- control-board/src/tasks/imu_task.rs | 2 +- control-board/src/tasks/mod.rs | 1 + control-board/src/tasks/shutdown_task.rs | 63 ++++ control-board/src/tasks/user_io_task.rs | 68 +++- kicker-board/Cargo.lock | 2 + kicker-board/Cargo.toml | 4 +- lib-stm32-test/Cargo.lock | 35 ++ .../src/bin/hwtest-adv-btn-async/main.rs | 26 ++ .../src/bin/hwtest-adv-btn-poll/main.rs | 27 ++ lib-stm32/Cargo.lock | 35 ++ lib-stm32/Cargo.toml | 5 +- lib-stm32/src/drivers/led/apa102.rs | 314 ++++++++++++++++++ lib-stm32/src/drivers/led/mod.rs | 1 + lib-stm32/src/drivers/mod.rs | 1 + lib-stm32/src/drivers/switches/button.rs | 197 +++++++++++ lib-stm32/src/drivers/switches/mod.rs | 1 + lib-stm32/src/lib.rs | 3 + lib-stm32/src/math/lerp.rs | 76 +++++ lib-stm32/src/math/mod.rs | 1 + 22 files changed, 934 insertions(+), 22 deletions(-) create mode 100644 control-board/src/bin/hwtest-shutdown/main.rs create mode 100644 control-board/src/tasks/shutdown_task.rs create mode 100644 lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs create mode 100644 lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs create mode 100644 lib-stm32/src/drivers/led/apa102.rs create mode 100644 lib-stm32/src/drivers/led/mod.rs create mode 100644 lib-stm32/src/drivers/switches/button.rs create mode 100644 lib-stm32/src/math/lerp.rs create mode 100644 lib-stm32/src/math/mod.rs diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 41cd4a44..889c00f7 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -18,7 +18,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d92cd690fd2aae5bc549e2dc392d9082aab3ccf851aa39b0ab84b2934f4c57a3" dependencies = [ "embedded-hal 0.2.7", - "smart-leds-trait", + "smart-leds-trait 0.2.1", ] [[package]] @@ -63,7 +63,7 @@ dependencies = [ "libm", "nalgebra", "panic-probe", - "smart-leds", + "smart-leds 0.3.0", "static_cell", ] @@ -80,7 +80,9 @@ dependencies = [ "embassy-sync", "embassy-time", "heapless 0.8.0", + "num-traits", "paste", + "smart-leds 0.4.0", ] [[package]] @@ -963,9 +965,9 @@ dependencies = [ [[package]] name = "num-complex" -version = "0.4.5" +version = "0.4.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23c6602fda94a57c990fe0df199a035d83576b496aa29f4e634a8ac6004e68a6" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" dependencies = [ "num-traits", ] @@ -981,11 +983,10 @@ dependencies = [ [[package]] name = "num-rational" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" dependencies = [ - "autocfg", "num-integer", "num-traits", ] @@ -1232,7 +1233,16 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "38dd45fa275f70b4110eac5f5182611ad384f88bb22b68b9a9c3cafd7015290b" dependencies = [ - "smart-leds-trait", + "smart-leds-trait 0.2.1", +] + +[[package]] +name = "smart-leds" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "66df34e571fa9993fa6f99131a374d58ca3d694b75f9baac93458fe0d6057bf0" +dependencies = [ + "smart-leds-trait 0.3.0", ] [[package]] @@ -1244,6 +1254,15 @@ dependencies = [ "rgb", ] +[[package]] +name = "smart-leds-trait" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +dependencies = [ + "rgb", +] + [[package]] name = "spin" version = "0.9.8" diff --git a/control-board/src/bin/hwtest-shutdown/main.rs b/control-board/src/bin/hwtest-shutdown/main.rs new file mode 100644 index 00000000..f03079f0 --- /dev/null +++ b/control-board/src/bin/hwtest-shutdown/main.rs @@ -0,0 +1,55 @@ +#![no_std] +#![no_main] + +use embassy_executor::InterruptExecutor; +use embassy_stm32::interrupt; + +use defmt_rtt as _; + +use ateam_control_board::{create_io_task, create_shutdown_task, get_system_config, robot_state::SharedRobotState}; + + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); + +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + /////////////////// + // start tasks // + /////////////////// + + create_io_task!(main_spawner, robot_state, p); + + create_shutdown_task!(main_spawner, robot_state, p); + + loop { + Timer::after_millis(1000).await; + } +} \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index c0303061..943ab19e 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -131,9 +131,9 @@ pub type MotorDResetPin = PD12; // Power Control // ///////////////////// -pub type PowerBtnPressedIntPin = PE10; +pub type PowerBtnPressedIntPin = PE11; pub type PowerBtnPressedIntExti = EXTI11; -pub type PowerKillPin = PE11; +pub type PowerKillPin = PE10; pub type ShutdownInitiatedLedPin = PF4; diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 12dba3ff..9837f4ab 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -125,7 +125,7 @@ pub fn start_imu_task( accel_int: impl Peripheral

::ExtiChannel> + 'static, gyro_int: impl Peripheral

::ExtiChannel> + 'static, _ext_imu_det_pin: ExtImuNDetPin) { - let imu_buf = IMU_BUFFER_CELL.take(); + let imu_buf = IMU_BUFFER_CELL.take(); let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index f5927642..c5fe1757 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -3,4 +3,5 @@ pub mod control_task; pub mod imu_task; pub mod kicker_task; pub mod radio_task; +pub mod shutdown_task; pub mod user_io_task; \ No newline at end of file diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs new file mode 100644 index 00000000..23e1536e --- /dev/null +++ b/control-board/src/tasks/shutdown_task.rs @@ -0,0 +1,63 @@ +use ateam_lib_stm32::drivers::switches::button::{AdvExtiButton, ADV_BTN_EVENT_DOUBLE_TAP}; +use embassy_executor::Spawner; +use embassy_futures::select; +use embassy_stm32::gpio::{Level, Output, OutputOpenDrain, Pull, Speed}; +use embassy_time::Timer; + +use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::SharedRobotState}; + +const HARD_SHUTDOWN_TIME_MS: u64 = 10000; + +#[macro_export] +macro_rules! create_shutdown_task { + ($spawner:ident, $robot_state:ident, $p:ident) => { + ateam_control_board::tasks::shutdown_task::start_shutdown_task($spawner, + $robot_state, + $p.PE11, $p.EXTI11, $p.PE10, $p.PF4); + }; +} + +#[embassy_executor::task] +async fn shutdown_task_entry(robot_state: &'static SharedRobotState, + mut power_btn: AdvExtiButton, + mut system_kill_pin: OutputOpenDrain<'static>, + mut shutdown_initiated_led: Output<'static>) { + + defmt::info!("shutdown task initialized"); + + power_btn.wait_for_btn_event(ADV_BTN_EVENT_DOUBLE_TAP).await; + + robot_state.flag_shutdown_requested(); + shutdown_initiated_led.set_high(); + + defmt::warn!("shutdown initiated via user btn! syncing..."); + + // wait for tasks to flag shutdown complete, or hard temrinate after 10s + select::select(async move { + loop { + // TODO wait for other tasks + Timer::after_millis(1000).await; + } + }, Timer::after_millis(HARD_SHUTDOWN_TIME_MS)).await; + + defmt::info!("initiating power off"); + Timer::after_millis(100).await; + + loop { + system_kill_pin.set_low(); + } +} + +pub fn start_shutdown_task(spawner: Spawner, + robot_state: &'static SharedRobotState, + power_btn_pin: PowerBtnPressedIntPin, + power_btn_pin_exti: PowerBtnPressedIntExti, + system_kill_pin: PowerKillPin, + shutdown_initiated_led_pin: ShutdownInitiatedLedPin) { + + let power_btn: AdvExtiButton = AdvExtiButton::new_from_pins(power_btn_pin, power_btn_pin_exti, true); + let system_kill_output = OutputOpenDrain::new(system_kill_pin, Level::High, Speed::Medium, Pull::None); + let shutdown_initiated_led = Output::new(shutdown_initiated_led_pin, Level::Low, Speed::Medium); + + spawner.spawn(shutdown_task_entry(robot_state, power_btn, system_kill_output, shutdown_initiated_led)).unwrap(); +} \ No newline at end of file diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 8b8db7af..3f9fb3d9 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,34 +1,65 @@ -use ateam_lib_stm32::drivers::switches::dip::DipSwitch; -use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; + use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; -use embassy_time::Timer; +use embassy_stm32::spi::{Config, Spi}; +use embassy_stm32::time::mhz; +use embassy_time::{Duration, Timer}; + +use smart_leds::RGB8; +use static_cell::ConstStaticCell; + +use ateam_lib_stm32::drivers::led::apa102::{Apa102, Apa102AnimationRepeat, Apa102AnimationTrait, Apa102Blink}; +use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; +use ateam_lib_stm32::drivers::switches::dip::DipSwitch; +use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; +use ateam_lib_stm32::math::lerp::{Lerp, TimeLerp}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; use crate::pins::*; +// #[link_section = ".sram4"] +// static DOTSTAR_SPI_BUFFER_CELL: ConstStaticCell<[u8; 16]> = ConstStaticCell::new([0; 16]); + +#[link_section = ".sram4"] +static mut DOTSTAR_SPI_BUFFER_CELL: [u8; 16] = [0; 16]; + #[macro_export] macro_rules! create_io_task { ($spawner:ident, $robot_state:ident, $p:ident) => { - ateam_control_board::tasks::user_io_task::start_io_task($spawner, + ateam_control_board::tasks::user_io_task::start_io_task(&$spawner, $robot_state, $p.PD6, $p.PD5, $p.EXTI6, $p.EXTI5, $p.PG7, $p.PG6, $p.PG5, $p.PG4, $p.PG3, $p.PG2, $p.PD15, $p.PG12, $p.PG11, $p.PG10, $p.PG9, $p.PF3, $p.PF2, $p.PF1, $p.PF0, - $p.PD0, $p.PD1, $p.PD3, $p.PD4, $p.PD14); + $p.PD0, $p.PD1, $p.PD3, $p.PD4, $p.PD14, + $p.SPI6, $p.PB3, $p.PB5, $p.BDMA_CH0).await; }; } #[embassy_executor::task] async fn user_io_task_entry(robot_state: &'static SharedRobotState, + mut usr_btn0: AdvExtiButton, + mut usr_btn1: AdvExtiButton, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, mut robot_id_indicator: ShellIndicator<'static>, + mut dotstars: Apa102<'static, 'static, DotstarSpi, 2>, ) { + defmt::info!("user io task initialized"); + + dotstars.set_drv_str_all(128); + + let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); + color_lerp.start(); + + let mut animation = Apa102Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), Apa102AnimationRepeat::RepeatForever); + animation.start_animation(); + dotstars.set_animation(animation, 1); + loop { let cur_robot_state = robot_state.get_state(); @@ -73,6 +104,12 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, debug_led0.set_low(); } + let color = color_lerp.update(); + dotstars.set_color(color, 0); + // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); + dotstars.update().await; + + if !robot_state.hw_init_state_valid() { defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); robot_state.set_hw_init_state_valid(true); @@ -82,9 +119,9 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, } } -pub fn start_io_task(spawner: Spawner, +pub async fn start_io_task(spawner: &Spawner, robot_state: &'static SharedRobotState, - _usr_btn0_pin: UsrBtn0Pin, _usr_btn1_pin: UsrBtn1Pin, _usr_btn0_exti: UsrBtn0Exti, _usr_btn1_exti: UsrBtn1Exti, + usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, @@ -93,8 +130,22 @@ pub fn start_io_task(spawner: Spawner, robot_id_indicator_fl: RobotIdIndicator0FlPin, robot_id_indicator_bl: RobotIdIndicator1BlPin, robot_id_indicator_br: RobotIdIndicator2BrPin, robot_id_indicator_fr: RobotIdIndicator3FrPin, robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, + dotstar_peri: DotstarSpi, + dotstar_sck_pin: DotstarSpiSck, + dotstar_mosi_pin: DotstarSpiMosi, + dotstar_tx_dma: DotstarTxDma, ) { + // defmt::info!("taking buf"); + // let dotstar_spi_buf: &'static mut [u8; 16] = DOTSTAR_SPI_BUFFER_CELL.take(); + // defmt::info!("took buf"); + + let dotstar_spi_buf: &'static mut [u8; 16] = unsafe { &mut DOTSTAR_SPI_BUFFER_CELL }; + let dotstars = Apa102::<_, 2>::new_from_pins(dotstar_peri, dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, dotstar_spi_buf.into()); + + let adv_usr_btn0: AdvExtiButton = AdvExtiButton::new_from_pins(usr_btn0_pin, usr_btn0_exti, false); + let adv_usr_btn1: AdvExtiButton = AdvExtiButton::new_from_pins(usr_btn1_pin, usr_btn1_exti, false); + let dip_sw_pins: [AnyPin; 7] = [usr_dip7_pin.into(), usr_dip6_pin.into(), usr_dip5_pin.into(), usr_dip4_pin.into(), usr_dip3_pin.into(), usr_dip2_pin.into(), usr_dip1_pin.into()]; let dip_switch = DipSwitch::new_from_pins(dip_sw_pins, Pull::None, None); @@ -105,5 +156,6 @@ pub fn start_io_task(spawner: Spawner, let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); - spawner.spawn(user_io_task_entry(robot_state, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); + + spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); } \ No newline at end of file diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index 07275fc1..cf4bdad3 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -69,7 +69,9 @@ dependencies = [ "embassy-sync", "embassy-time", "heapless 0.8.0", + "num-traits", "paste", + "smart-leds", ] [[package]] diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 50d21260..03125c00 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -53,8 +53,8 @@ ateam-common-packets = { path = "../software-communication/ateam-common-packets/ defmt-test = "0.3.0" [profile.dev] -opt-level = 3 -lto = 'fat' +# opt-level = 3 +# lto = 'fat' debug-assertions = false [profile.release] diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index c7726f86..cfc13b71 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -15,7 +15,9 @@ dependencies = [ "embassy-sync", "embassy-time", "heapless 0.8.0", + "num-traits", "paste", + "smart-leds", ] [[package]] @@ -96,6 +98,12 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" +[[package]] +name = "bytemuck" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" + [[package]] name = "byteorder" version = "1.5.0" @@ -760,6 +768,15 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rgb" +version = "0.8.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05aaa8004b64fd573fc9d002f4e632d51ad4f026c2b5ba95fcb6c2f32c2c47d8" +dependencies = [ + "bytemuck", +] + [[package]] name = "rustc_version" version = "0.2.3" @@ -811,6 +828,24 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" +[[package]] +name = "smart-leds" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "66df34e571fa9993fa6f99131a374d58ca3d694b75f9baac93458fe0d6057bf0" +dependencies = [ + "smart-leds-trait", +] + +[[package]] +name = "smart-leds-trait" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +dependencies = [ + "rgb", +] + [[package]] name = "spin" version = "0.9.8" diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs new file mode 100644 index 00000000..da4473c6 --- /dev/null +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-async/main.rs @@ -0,0 +1,26 @@ +#![no_std] +#![no_main] +#![feature(generic_arg_infer)] + +use ateam_lib_stm32::drivers::switches::button::{AdvExtiButton, ADV_BTN_EVENT_DOUBLE_TAP}; + +use defmt_rtt as _; +use embassy_time::Timer; +use panic_probe as _; + + +#[embassy_executor::main] +async fn main(_spawner: embassy_executor::Spawner) -> !{ + // this actually gets us 64MHz peripheral bus clock + let stm32_config: embassy_stm32::Config = Default::default(); + let p = embassy_stm32::init(stm32_config); + + let mut adv_btn: AdvExtiButton = AdvExtiButton::new_from_pins(p.PC13, p.EXTI13, false); + + loop { + adv_btn.wait_for_btn_event(ADV_BTN_EVENT_DOUBLE_TAP).await; + defmt::info!("event!"); + + Timer::after_millis(10).await; + } +} \ No newline at end of file diff --git a/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs new file mode 100644 index 00000000..9fb00795 --- /dev/null +++ b/lib-stm32-test/src/bin/hwtest-adv-btn-poll/main.rs @@ -0,0 +1,27 @@ +#![no_std] +#![no_main] +#![feature(generic_arg_infer)] + +use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; + +use defmt_rtt as _; +use embassy_time::Timer; +use panic_probe as _; + + +#[embassy_executor::main] +async fn main(_spawner: embassy_executor::Spawner) -> !{ + // this actually gets us 64MHz peripheral bus clock + let stm32_config: embassy_stm32::Config = Default::default(); + let p = embassy_stm32::init(stm32_config); + + let mut adv_btn: AdvExtiButton = AdvExtiButton::new_from_pins(p.PC13, p.EXTI13, false); + + loop { + if let Some(ev) = adv_btn.poll_btn_event() { + defmt::info!("btn event: {}", ev); + } + + Timer::after_millis(10).await; + } +} \ No newline at end of file diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 28d67430..a26debe8 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -15,7 +15,9 @@ dependencies = [ "embassy-sync", "embassy-time", "heapless", + "num-traits", "paste", + "smart-leds", ] [[package]] @@ -63,6 +65,12 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" +[[package]] +name = "bytemuck" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" + [[package]] name = "byteorder" version = "1.5.0" @@ -589,6 +597,15 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rgb" +version = "0.8.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05aaa8004b64fd573fc9d002f4e632d51ad4f026c2b5ba95fcb6c2f32c2c47d8" +dependencies = [ + "bytemuck", +] + [[package]] name = "rustc_version" version = "0.2.3" @@ -619,6 +636,24 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" +[[package]] +name = "smart-leds" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "66df34e571fa9993fa6f99131a374d58ca3d694b75f9baac93458fe0d6057bf0" +dependencies = [ + "smart-leds-trait", +] + +[[package]] +name = "smart-leds-trait" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bc64ee02bbbf469603016df746c0ed224f263280b6ebb49b7ebadbff375c572" +dependencies = [ + "rgb", +] + [[package]] name = "stable_deref_trait" version = "1.2.0" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 8ef2aa09..9926ee85 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -7,7 +7,7 @@ description = "A common library for ateam stm32 targets" repository = "https://github.com/SSL-A-Team/firmware" [dependencies] -embassy-stm32 = { version = "0.1.0", default-features = false } +embassy-stm32 = { version = "0.1.0", default-features = false, features = ["exti"]} embassy-executor = { version = "0.5.0", default-features = false } embassy-sync = { version = "0.5.0" } embassy-futures = { version = "0.1.0" } @@ -20,6 +20,9 @@ critical-section = "1.1.1" heapless = "0.8.0" +num-traits = { verison = "0.2.19", default-features = false } +smart-leds = "0.4.0" + [features] default = ["embassy-stm32/stm32h723zg"] # stm32h723zg = [] diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs new file mode 100644 index 00000000..48fbf9da --- /dev/null +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -0,0 +1,314 @@ + +use core::ops::Range; + +use embassy_stm32::{mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, time::mhz, Peripheral}; +use embassy_time::{Duration, Instant}; +use smart_leds::RGB8; + +pub struct Apa102<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> +where [(); (NUM_LEDS * 4) + 8]: { + spi: spi::Spi<'a, SpiPeri, Async>, + spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + animation_buf: [Option; NUM_LEDS], +} + +impl<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102<'a, 'buf, SpiPeri, NUM_LEDS> +where [(); (NUM_LEDS * 4) + 8]: { + pub fn new( + spi: spi::Spi<'a, SpiPeri, Async>, + spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + ) -> Self { + // set start frame + spi_buf[0] = 0x00; + spi_buf[1] = 0x00; + spi_buf[2] = 0x00; + spi_buf[3] = 0x00; + + // set end frame + spi_buf[8 + (NUM_LEDS * 4) - 4] = 0xFF; + spi_buf[8 + (NUM_LEDS * 4) - 3] = 0xFF; + spi_buf[8 + (NUM_LEDS * 4) - 2] = 0xFF; + spi_buf[8 + (NUM_LEDS * 4) - 1] = 0xFF; + + Apa102 { + spi: spi, + spi_buf: spi_buf, + animation_buf: [None; NUM_LEDS], + } + } + + pub fn new_from_pins( + peri: impl Peripheral

+ 'a, + sck_pin: impl Peripheral

> + 'a, + mosi_pin: impl Peripheral

> + 'a, + tx_dma: impl Peripheral

> + 'a, + spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + ) -> Self { + let mut dotstar_spi_config = Config::default(); + dotstar_spi_config.frequency = mhz(1); + + let spi = Spi::new_txonly( + peri, + sck_pin, + mosi_pin, + tx_dma, + dotstar_spi_config, + ); + + Self::new(spi, spi_buf) + } + + const fn l2d(led_index: usize) -> usize { + 4 + (led_index * 4) + 0 + } + + const fn l2r(led_index: usize) -> usize { + 4 + (led_index * 4) + 3 + } + + const fn l2g(led_index: usize) -> usize { + 4 + (led_index * 4) + 2 + } + + const fn l2b(led_index: usize) -> usize { + 4 + (led_index * 4) + 1 + } + + pub fn set_drv_str(&mut self, str: u8, led_index: usize) { + assert!(led_index < NUM_LEDS); + + let str = ((str >> 3) & 0x1F) | 0xE0; + self.spi_buf[Self::l2d(led_index)] = str; + } + + pub fn set_drv_str_range(&mut self, str: u8, led_index_range: Range) { + for i in led_index_range { + self.set_drv_str(str, i) + } + } + + pub fn set_drv_str_all(&mut self, str: u8) { + self.set_drv_str_range(str, 0..NUM_LEDS) + } + + pub fn set_color(&mut self, color: RGB8, led_index: usize) { + assert!(led_index < NUM_LEDS); + + self.spi_buf[Self::l2r(led_index)] = color.r; + self.spi_buf[Self::l2g(led_index)] = color.g; + self.spi_buf[Self::l2b(led_index)] = color.b; + } + + pub fn set_color_range(&mut self, color: RGB8, led_index_range: Range) { + for i in led_index_range { + self.set_color(color, i) + } + } + + pub fn set_color_all(&mut self, color: RGB8) { + self.set_color_range(color, 0..NUM_LEDS) + } + + pub fn set_animation(&mut self, anim: Apa102Blink, led_index: usize) { + assert!(led_index < NUM_LEDS); + self.animation_buf[led_index] = Some(anim); + } + + pub fn clear_animation(&mut self, led_index: usize) { + assert!(led_index < NUM_LEDS); + self.animation_buf[led_index] = None; + } + + pub async fn update(&mut self) { + // update animations + for anim in self.animation_buf.iter_mut() { + if let Some(anim) = anim { + anim.update(); + } + } + + // set colors from animations + for (i, anim) in self.animation_buf.into_iter().enumerate() { + if let Some(anim) = anim { + self.set_color(anim.get_color(), i); + } + } + + let _ = self.spi.write(self.spi_buf).await; + } +} + +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub enum Apa102AnimationRepeat { + NoRepeat, + RepeatForever, + RepeatFixed(usize), +} + +pub trait Apa102AnimationTrait: Sized { + fn start_animation(&mut self); + fn stop_animation(&mut self); + fn animation_running(&self) -> bool; + fn update(&mut self); + fn get_color(&self) -> RGB8; +} + +// #[derive(Clone, Copy, Debug)] +// pub enum Apa102Animation { +// Blink(Apa102Blink), +// } + +// impl Apa102AnimationTrait for Apa102Animation { +// fn start_animation(&mut self) { +// match self { +// Apa102Animation::Blink(anim), Apa102Animation::Blink2(anim) => { + +// }, +// } +// } + +// fn stop_animation(&mut self) { +// todo!() +// } + +// fn animation_running(&self) -> bool { +// todo!() +// } + +// fn update(&mut self) -> RGB8 { +// todo!() +// } +// } + +// pub struct Apa102AnimationSequence { +// animations: [Apa102Animation; NUM_ANIM], +// repeat_style: Apa102AnimationRepeat, + +// animation_running: bool, +// repeat_counter: usize, +// animation_index: usize, +// } + +// impl Apa102AnimationSequence { +// pub fn new(anim_sequence: [Apa102Animation; NUM_ANIM], repeat_style: Apa102AnimationRepeat) -> Self { +// Apa102AnimationSequence { +// animations: anim_sequence, +// repeat_style: repeat_style, + +// animation_running: false, +// repeat_counter: 0, +// animation_index: 0, +// } +// } +// } + +// impl Apa102AnimationTrait for Apa102AnimationSequence { +// fn start_animation(&mut self) { +// self.animation_index = 0; +// self.animation_running = true; + +// if let Apa102AnimationRepeat::RepeatFixed(num) = self.repeat_style { +// self.repeat_counter = num; +// } +// } + +// fn stop_animation(&mut self) { +// self.animation_running = false; +// } + +// fn animation_running(&self) -> bool { +// return self.animation_running; +// } + +// fn update(&mut self) -> RGB8 { + +// } +// } + +#[derive(Clone, Copy, Debug)] +pub struct Apa102Blink { + color_one: RGB8, + color_two: RGB8, + c1_time: u64, + c2_time: u64, + repeat_style: Apa102AnimationRepeat, + + animation_running: bool, + repeat_counter: usize, + start_time: Instant, + last_color: RGB8, +} + +impl Apa102Blink { + pub fn new(color_one: RGB8, color_two: RGB8, c1_time: Duration, c2_time: Duration, repeat_style: Apa102AnimationRepeat) -> Self { + Apa102Blink { + color_one: color_one, + color_two: color_two, + c1_time: c1_time.as_millis(), + c2_time: c2_time.as_millis(), + repeat_style: repeat_style, + + animation_running: false, + repeat_counter: 0, + start_time: Instant::now(), + last_color: color_one, + } + } +} + +impl Apa102AnimationTrait for Apa102Blink { + fn start_animation(&mut self) { + self.start_time = Instant::now(); + self.animation_running = true; + + if let Apa102AnimationRepeat::RepeatFixed(num) = self.repeat_style { + self.repeat_counter = num; + } + } + + fn stop_animation(&mut self) { + self.animation_running = false; + } + + fn animation_running(&self) -> bool { + self.animation_running + } + + fn update(&mut self) { + if !self.animation_running { + return; + } + + let now = Instant::now(); + let elapsed_time = (now - self.start_time).as_millis(); + + if elapsed_time <= self.c1_time { + self.last_color = self.color_one; + } else if self.c1_time < elapsed_time && elapsed_time <= self.c1_time + self.c2_time { + self.last_color = self.color_two; + } else if elapsed_time > self.c1_time + self.c2_time { + match self.repeat_style { + Apa102AnimationRepeat::NoRepeat => { + self.animation_running = false; + }, + Apa102AnimationRepeat::RepeatForever => { + self.start_time = now; + self.last_color = self.color_one; + }, + Apa102AnimationRepeat::RepeatFixed(_) => { + if self.repeat_counter == 0 { + self.animation_running = false; + } else { + self.repeat_counter = self.repeat_counter - 1; + self.start_time = now; + self.last_color = self.color_one; + } + }, + } + } + } + + fn get_color(&self) -> RGB8 { + self.last_color + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/led/mod.rs b/lib-stm32/src/drivers/led/mod.rs new file mode 100644 index 00000000..c8c96b61 --- /dev/null +++ b/lib-stm32/src/drivers/led/mod.rs @@ -0,0 +1 @@ +pub mod apa102; \ No newline at end of file diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index 8a7859bb..2c1e7d4a 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -1,5 +1,6 @@ pub mod flash; pub mod imu; +pub mod led; pub mod radio; pub mod switches; \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/button.rs b/lib-stm32/src/drivers/switches/button.rs new file mode 100644 index 00000000..2851350e --- /dev/null +++ b/lib-stm32/src/drivers/switches/button.rs @@ -0,0 +1,197 @@ +use defmt::Format; +use embassy_futures::select; +use embassy_stm32::{exti::ExtiInput, gpio::{Pin, Pull}, Peripheral}; +use embassy_time::{Instant, Timer}; + +#[derive(Clone, Copy, Eq, PartialEq, Debug, Format)] +pub enum AdvButtonEvent { + None, + ShortPress, + LongPress, + Held, +} + +pub const ADV_BTN_EVENT_DOUBLE_TAP: [AdvButtonEvent; 3] = [AdvButtonEvent::ShortPress, AdvButtonEvent::ShortPress, AdvButtonEvent::None]; + +#[derive(Clone, Copy, Debug)] +enum BtnState { + Pressed(Instant), + Released(Instant), +} + +pub struct AdvExtiButton { + input: ExtiInput<'static>, + input_inverted: bool, + + prev_btn_state: BtnState, + btn_event_ind: usize, + btn_event_buf: [AdvButtonEvent; 3], +} + +impl AdvExtiButton { + pub fn new(input: ExtiInput<'static>, input_inverted: bool) -> Self { + Self { + input: input, + input_inverted: input_inverted, + prev_btn_state: BtnState::Released(Instant::now()), + btn_event_ind: 0, + btn_event_buf: [AdvButtonEvent::None; 3], + } + } + + pub fn new_from_pins(input_pin: PIN, input_pin_exti: impl Peripheral

::ExtiChannel> + 'static, input_inverted: bool) -> Self { + let input = ExtiInput::new(input_pin, input_pin_exti, Pull::None); + Self::new(input, input_inverted) + } + + fn btn_pressed(&self) -> bool { + if self.input_inverted { + self.input.is_low() + } else { + self.input.is_high() + } + } + + pub async fn wait_for_press(&mut self) { + if self.input_inverted { + self.input.wait_for_falling_edge().await; + } else { + self.input.wait_for_rising_edge().await; + } + } + + pub async fn wait_for_release(&mut self) { + if self.input_inverted { + self.input.wait_for_rising_edge().await; + } else { + self.input.wait_for_falling_edge().await; + } + } + + pub fn poll_btn_event(&mut self) -> Option<[AdvButtonEvent; 3]> { + let now = Instant::now(); + + let is_btn_pressed = self.btn_pressed(); + match (self.prev_btn_state, is_btn_pressed) { + (BtnState::Pressed(_), true) => { + // btn state didn't change + }, + (BtnState::Pressed(pressed_time), false) => { + let btn_held_time = (now - pressed_time).as_millis(); + // 50 ms debounce + if 50 <= btn_held_time { + if btn_held_time <= SHORT_PRESS_TIME_MS { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::ShortPress; + self.btn_event_ind += 1; + } else if SHORT_PRESS_TIME_MS <= btn_held_time && btn_held_time <= LONG_PRESS_TIME_MS { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::LongPress; + self.btn_event_ind += 1; + } else if HOLD_PRESS_TIME_MS <= btn_held_time { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::Held; + self.btn_event_ind += 1; + } + + self.prev_btn_state = BtnState::Released(now); + + // we've filled the max number of events, return the chain + if self.btn_event_ind >= self.btn_event_buf.len() { + let ret = Some(self.btn_event_buf); + + // reset the buffer + self.btn_event_ind = 0; + self.btn_event_buf = [AdvButtonEvent::None; 3]; + return ret; + } + } + }, + (BtnState::Released(released_time), true) => { + let btn_rel_time = (now - released_time).as_millis(); + + // 50 ms debounce + if 50 <= btn_rel_time { + self.prev_btn_state = BtnState::Pressed(now); + } + }, + (BtnState::Released(released_time), false) => { + // btn state didn't change + let btn_rel_time = (now - released_time).as_millis(); + + // button has been released for a while, return the events so far + if PRESS_TO_MS <= btn_rel_time && self.btn_event_ind > 0 { + let ret = Some(self.btn_event_buf); + + self.btn_event_ind = 0; + self.btn_event_buf = [AdvButtonEvent::None; 3]; + + return ret; + } + }, + } + + return None; + } + + pub async fn wait_for_btn_event(&mut self, btn_event: [AdvButtonEvent; 3]) { + 'event_loop: + loop { + match self.prev_btn_state { + BtnState::Pressed(prev_press_time) => { + self.wait_for_release().await; + + let now = Instant::now(); + let btn_held_time = (now - prev_press_time).as_millis(); + // 50 ms debounce + if 50 <= btn_held_time { + if btn_held_time <= SHORT_PRESS_TIME_MS { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::ShortPress; + self.btn_event_ind += 1; + } else if SHORT_PRESS_TIME_MS <= btn_held_time && btn_held_time <= LONG_PRESS_TIME_MS { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::LongPress; + self.btn_event_ind += 1; + } else if HOLD_PRESS_TIME_MS <= btn_held_time { + self.btn_event_buf[self.btn_event_ind] = AdvButtonEvent::Held; + self.btn_event_ind += 1; + } + + self.prev_btn_state = BtnState::Released(now); + + // check if our events match the requested event + if self.btn_event_buf == btn_event { + // reset the buffer + self.btn_event_ind = 0; + self.btn_event_buf = [AdvButtonEvent::None; 3]; + + break 'event_loop; + } + + // we've filled the max number of events, but didn't match in the + // previous block, clear the buffer + if self.btn_event_ind >= self.btn_event_buf.len() { + // reset the buffer + self.btn_event_ind = 0; + self.btn_event_buf = [AdvButtonEvent::None; 3]; + } + } + }, + BtnState::Released(prev_rel_time) => { + match select::select(self.wait_for_press(), Timer::after_millis(PRESS_TO_MS)).await { + select::Either::First(_) => { + let now = Instant::now(); + let btn_rel_time = (now - prev_rel_time).as_millis(); + + // 50 ms debounce + if 50 <= btn_rel_time { + self.prev_btn_state = BtnState::Pressed(now); + } + }, + select::Either::Second(_) => { + // timed out waiting for a press, reset the buffer + self.btn_event_buf = [AdvButtonEvent::None; 3]; + self.btn_event_ind = 0; + }, + } + }, + } + } + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/switches/mod.rs b/lib-stm32/src/drivers/switches/mod.rs index 4ab7d81f..817d5fe2 100644 --- a/lib-stm32/src/drivers/switches/mod.rs +++ b/lib-stm32/src/drivers/switches/mod.rs @@ -1,2 +1,3 @@ +pub mod button; pub mod dip; pub mod rotary_encoder; \ No newline at end of file diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 1f469a3e..94782252 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -1,12 +1,15 @@ #![no_std] +#![feature(generic_const_exprs)] #![feature(maybe_uninit_uninit_array)] #![feature(sync_unsafe_cell)] #![feature(type_alias_impl_trait)] #![feature(const_maybe_uninit_uninit_array)] #![feature(maybe_uninit_slice)] #![feature(ptr_metadata)] +#![feature(trait_alias)] pub mod drivers; +pub mod math; pub mod uart; pub mod queue; diff --git a/lib-stm32/src/math/lerp.rs b/lib-stm32/src/math/lerp.rs new file mode 100644 index 00000000..2be65349 --- /dev/null +++ b/lib-stm32/src/math/lerp.rs @@ -0,0 +1,76 @@ +use core::marker::PhantomData; + +use embassy_time::{Duration, Instant}; +use num_traits::{clamp, Bounded, CheckedAdd, CheckedDiv, CheckedMul, CheckedSub, FromPrimitive, ToPrimitive}; +use smart_leds::RGB8; + +pub fn lerp + CheckedSub + CheckedMul + CheckedDiv>(a: N, b: N, t: N) -> N { + let t_pct = t.to_f32().unwrap() / N::max_value().to_f32().unwrap(); + return N::from_f32(lerp_f(a.to_f32().unwrap(), b.to_f32().unwrap(), t_pct)).unwrap(); +} + +pub fn lerp_f(a: f32, b: f32, t: f32) -> f32 { + let t = clamp(t, 0.0, 1.0); + return a + (b - a) * t; +} + +pub trait Lerp + CheckedSub + CheckedMul + CheckedDiv> { + fn lerp(a: Self, b: Self, t: N) -> Self; + fn lerp_f(a: Self, b: Self, t: f32) -> Self; +} + +impl Lerp for RGB8 { + fn lerp(a: RGB8, b: RGB8, t: u8) -> RGB8 { + RGB8 { + r: lerp(a.r, b.r, t), + g: lerp(a.g, b.g, t), + b: lerp(a.b, b.b, t), + } + } + + fn lerp_f(a: RGB8, b: RGB8, t: f32) -> RGB8 { + RGB8 { + r: lerp_f(a.r as f32, b.r as f32, t) as u8, + g: lerp_f(a.g as f32, b.g as f32, t) as u8, + b: lerp_f(a.b as f32, b.b as f32, t) as u8, + } + } +} + +pub struct TimeLerp<'a, N: Copy + Bounded + CheckedAdd + CheckedSub + CheckedMul + CheckedDiv, L: Copy + Sized + Lerp> { + a: L, + b: L, + + duration: Duration, + start_time: Instant, + + pd2: PhantomData<&'a N>, +} + +impl<'a, N: Copy + Bounded + CheckedAdd + CheckedSub + CheckedMul + CheckedDiv, L: Copy + Sized + Lerp> TimeLerp<'a, N, L> { + pub fn new(a: L, b: L, duration: Duration) -> TimeLerp<'a, N, L> { + TimeLerp { + a: a, + b: b, + duration: duration, + start_time: Instant::now(), + pd2: PhantomData, + } + } + + pub fn start(&mut self) { + self.start_time = Instant::now(); + } + + pub fn update(&self) -> L { + let now = Instant::now(); + let elapsed_time = (now - self.start_time).as_millis(); + let pct_complete = elapsed_time as f32 / self.duration.as_millis() as f32; + L::lerp_f(self.a, self.b, pct_complete) + } +} + +#[test] +fn test_lerp() { + print("hi") +} \ No newline at end of file diff --git a/lib-stm32/src/math/mod.rs b/lib-stm32/src/math/mod.rs new file mode 100644 index 00000000..8eb51f5c --- /dev/null +++ b/lib-stm32/src/math/mod.rs @@ -0,0 +1 @@ +pub mod lerp; \ No newline at end of file From 3638d685833a19160b8e667a332a0aab09e886d6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 00:08:38 -0400 Subject: [PATCH 053/157] animation work --- control-board/src/tasks/user_io_task.rs | 31 +- lib-stm32/src/anim/mod.rs | 409 ++++++++++++++++++++++++ lib-stm32/src/drivers/led/apa102.rs | 184 +---------- lib-stm32/src/lib.rs | 1 + lib-stm32/src/math/lerp.rs | 53 ++- 5 files changed, 478 insertions(+), 200 deletions(-) create mode 100644 lib-stm32/src/anim/mod.rs diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 3f9fb3d9..2be038a2 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,4 +1,6 @@ +use ateam_lib_stm32::anim::{AnimInterface, AnimRepeatMode, Blink, Lerp}; +use ateam_lib_stm32::drivers::led::apa102::Apa102; use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_stm32::spi::{Config, Spi}; @@ -8,11 +10,10 @@ use embassy_time::{Duration, Timer}; use smart_leds::RGB8; use static_cell::ConstStaticCell; -use ateam_lib_stm32::drivers::led::apa102::{Apa102, Apa102AnimationRepeat, Apa102AnimationTrait, Apa102Blink}; +// use ateam_lib_stm32::drivers::led::apa102::{Apa102, Apa102AnimationRepeat, Apa102AnimationTrait, Apa102Blink}; use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; -use ateam_lib_stm32::math::lerp::{Lerp, TimeLerp}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; @@ -51,14 +52,22 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, ) { defmt::info!("user io task initialized"); - dotstars.set_drv_str_all(128); + dotstars.set_drv_str_all(8); - let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); - color_lerp.start(); + // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); + // color_lerp.start(); - let mut animation = Apa102Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), Apa102AnimationRepeat::RepeatForever); - animation.start_animation(); - dotstars.set_animation(animation, 1); + let mut anim0 = Lerp::new(RGB8 { r: 255, g: 255, b: 255 }, RGB8 { r: 0, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::Forever); + anim0.start_animation(); + dotstars.set_animation(anim0, 0); + + let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); + anim1.start_animation(); + dotstars.set_animation(anim1, 1); + + // let mut blink_anim = Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), AnimRepeatMode::Forever); + // blink_anim.start_animation(); + // dotstars.set_animation(blink_anim, 1); loop { let cur_robot_state = robot_state.get_state(); @@ -104,8 +113,8 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, debug_led0.set_low(); } - let color = color_lerp.update(); - dotstars.set_color(color, 0); + // let color = color_lerp.update(); + // dotstars.set_color(color, 0); // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); dotstars.update().await; @@ -115,7 +124,7 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, robot_state.set_hw_init_state_valid(true); } - Timer::after_millis(100).await; + Timer::after_millis(50).await; } } diff --git a/lib-stm32/src/anim/mod.rs b/lib-stm32/src/anim/mod.rs new file mode 100644 index 00000000..4d184d8f --- /dev/null +++ b/lib-stm32/src/anim/mod.rs @@ -0,0 +1,409 @@ +use core::marker::PhantomData; + +use embassy_time::{Duration, Instant}; + +use crate::math::lerp::LerpNumeric; + +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +enum AnimState { + Waiting, + Running, + Completed, +} + +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub enum AnimRepeatMode { + None, + Fixed(usize), + Forever, +} + +pub trait AnimInterface: Sized { + fn start_animation(&mut self); + fn reset_animation(&mut self); + fn animation_running(&self) -> bool; + fn animation_completed(&self) -> bool; + fn update(&mut self); + fn get_value(&self) -> T; +} + +///////////////////////////////// +// composite animation types // +///////////////////////////////// + +pub enum Animation +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + Blink(Blink), + Lerp(Lerp), +} + +impl AnimInterface for Animation +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + fn start_animation(&mut self) { + match self { + Animation::Blink(a) => { + a.start_animation(); + }, + Animation::Lerp(a) => { + a.start_animation(); + }, + } + } + + fn reset_animation(&mut self) { + match self { + Animation::Blink(a) => { + a.reset_animation(); + }, + Animation::Lerp(a) => { + a.reset_animation(); + }, + } + } + + fn animation_running(&self) -> bool { + match self { + Animation::Blink(a) => { + a.animation_running() + }, + Animation::Lerp(a) => { + a.animation_running() + }, + } + } + + fn animation_completed(&self) -> bool { + match self { + Animation::Blink(a) => { + a.animation_completed() + }, + Animation::Lerp(a) => { + a.animation_completed() + }, + } + } + + fn update(&mut self) { + match self { + Animation::Blink(a) => { + a.update(); + }, + Animation::Lerp(a) => { + a.update(); + }, + } + } + + fn get_value(&self) -> L { + match self { + Animation::Blink(a) => { + a.get_value() + }, + Animation::Lerp(a) => { + a.get_value() + }, + } + } +} + +pub struct CompositeAnimation +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp { + animations: [Animation; LEN], + active_animation: usize, + + repeat_mode: AnimRepeatMode, + anim_state: AnimState, + repeat_counter: usize, + + last_value: L, +} + +impl CompositeAnimation +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + pub fn new(animations: [Animation; LEN], repeat_mode: AnimRepeatMode) -> Self { + let first_val = animations[0].get_value(); + CompositeAnimation { + animations: animations, + active_animation: 0, + repeat_mode: repeat_mode, + anim_state: AnimState::Waiting, + repeat_counter: 0, + last_value: first_val, + } + } +} + +impl AnimInterface for CompositeAnimation +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + fn start_animation(&mut self) { + self.active_animation = 0; + self.anim_state = AnimState::Running; + + if let AnimRepeatMode::Fixed(num) = self.repeat_mode { + self.repeat_counter = num; + } + } + + fn reset_animation(&mut self) { + self.anim_state = AnimState::Waiting; + } + + fn animation_running(&self) -> bool { + self.anim_state == AnimState::Running + } + + fn animation_completed(&self) -> bool { + self.anim_state == AnimState::Completed + } + + fn update(&mut self) { + // update the current animation + self.animations[self.active_animation].update(); + self.last_value = self.animations[self.active_animation].get_value(); + + // check if the current animation is done + if self.animations[self.active_animation].animation_completed() { + // advance to the next animation + self.active_animation = self.active_animation + 1; + + if self.active_animation >= LEN { + self.active_animation = 0; + + // we've completed the last animation in the buffer + match self.repeat_mode { + AnimRepeatMode::None => { + self.anim_state = AnimState::Completed; + }, + AnimRepeatMode::Fixed(_) => { + if self.repeat_counter == 0 { + self.anim_state = AnimState::Completed; + } else { + self.repeat_counter = self.repeat_counter - 1; + } + }, + AnimRepeatMode::Forever => { + // we've already reset the animation index so it'll loop around + }, + } + } + } + } + + fn get_value(&self) -> L { + self.last_value + } +} + +////////////////// +// animations // +////////////////// + +#[derive(Clone, Copy, Debug)] +pub struct Blink { + val_one: T, + val_two: T, + v1_time: u64, + v2_time: u64, + repeat_style: AnimRepeatMode, + + anim_state: AnimState, + repeat_counter: usize, + start_time: Instant, + last_value: T, +} + +impl Blink { + pub fn new(val_one: T, val_two: T, v1_time: Duration, v2_time: Duration, repeat_style: AnimRepeatMode) -> Self { + Blink { + val_one, + val_two, + v1_time: v1_time.as_millis(), + v2_time: v2_time.as_millis(), + repeat_style: repeat_style, + + anim_state: AnimState::Waiting, + repeat_counter: 0, + start_time: Instant::now(), + last_value: val_one, + } + } +} + +impl AnimInterface for Blink { + fn start_animation(&mut self) { + self.start_time = Instant::now(); + self.anim_state = AnimState::Running; + + if let AnimRepeatMode::Fixed(num) = self.repeat_style { + self.repeat_counter = num; + } + } + + fn reset_animation(&mut self) { + self.anim_state = AnimState::Waiting; + } + + fn animation_running(&self) -> bool { + self.anim_state == AnimState::Running + } + + fn animation_completed(&self) -> bool { + self.anim_state == AnimState::Completed + } + + fn update(&mut self) { + if self.anim_state != AnimState::Running { + return; + } + + let now = Instant::now(); + let elapsed_time = (now - self.start_time).as_millis(); + + if elapsed_time <= self.v1_time { + self.last_value = self.val_one; + } else if self.v1_time < elapsed_time && elapsed_time <= self.v1_time + self.v2_time { + self.last_value = self.val_two; + } else if elapsed_time > self.v1_time + self.v2_time { + match self.repeat_style { + AnimRepeatMode::None => { + self.anim_state = AnimState::Completed; + }, + AnimRepeatMode::Forever => { + self.start_time = now; + self.last_value = self.val_one; + }, + AnimRepeatMode::Fixed(_) => { + if self.repeat_counter == 0 { + self.anim_state = AnimState::Completed; + } else { + self.repeat_counter = self.repeat_counter - 1; + self.start_time = now; + self.last_value = self.val_one; + } + }, + } + } + } + + fn get_value(&self) -> T { + self.last_value + } +} + +#[derive(Clone, Copy, Debug)] +pub struct Lerp +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp { + val_one: L, + val_two: L, + lerp_duration_ms: u64, + repeat_style: AnimRepeatMode, + + anim_state: AnimState, + repeat_counter: usize, + start_time: Instant, + last_value: L, + + pd: PhantomData, +} + +impl Lerp +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + pub fn new(val_one: L, val_two: L, lerp_duration_ms: Duration, repeat_style: AnimRepeatMode) -> Self { + Lerp { + val_one, + val_two, + lerp_duration_ms: lerp_duration_ms.as_millis(), + repeat_style: repeat_style, + + anim_state: AnimState::Waiting, + repeat_counter: 0, + start_time: Instant::now(), + last_value: val_one, + + pd: PhantomData, + } + } +} + +impl AnimInterface for Lerp +where N: LerpNumeric, +f32: core::convert::From, +L: crate::math::lerp::Lerp +{ + fn start_animation(&mut self) { + self.start_time = Instant::now(); + self.anim_state = AnimState::Running; + + if let AnimRepeatMode::Fixed(num) = self.repeat_style { + self.repeat_counter = num; + } + } + + fn reset_animation(&mut self) { + self.anim_state = AnimState::Waiting; + } + + fn animation_running(&self) -> bool { + self.anim_state == AnimState::Running + } + + fn animation_completed(&self) -> bool { + self.anim_state == AnimState::Completed + } + + fn update(&mut self) { + if self.anim_state != AnimState::Running { + return; + } + + let now = Instant::now(); + let elapsed_time = (now - self.start_time).as_millis(); + let elapsed_time_frac: f32 = elapsed_time as f32 / self.lerp_duration_ms as f32; + + self.last_value = L::lerp_f(self.val_one, self.val_two, elapsed_time_frac); + + if elapsed_time_frac > 1.0 { + match self.repeat_style { + AnimRepeatMode::None => { + self.anim_state = AnimState::Completed; + }, + AnimRepeatMode::Forever => { + self.start_time = now; + self.last_value = self.val_one; + }, + AnimRepeatMode::Fixed(_) => { + if self.repeat_counter == 0 { + self.anim_state = AnimState::Completed; + } else { + self.repeat_counter = self.repeat_counter - 1; + self.start_time = now; + self.last_value = self.val_one; + } + }, + } + } + } + + fn get_value(&self) -> L { + self.last_value + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index 48fbf9da..a31bc7f5 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -5,11 +5,13 @@ use embassy_stm32::{mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, time use embassy_time::{Duration, Instant}; use smart_leds::RGB8; +use crate::anim::{AnimInterface, Blink, CompositeAnimation, Lerp}; + pub struct Apa102<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { spi: spi::Spi<'a, SpiPeri, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], - animation_buf: [Option; NUM_LEDS], + animation_buf: [Option>; NUM_LEDS], } impl<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102<'a, 'buf, SpiPeri, NUM_LEDS> @@ -109,7 +111,7 @@ where [(); (NUM_LEDS * 4) + 8]: { self.set_color_range(color, 0..NUM_LEDS) } - pub fn set_animation(&mut self, anim: Apa102Blink, led_index: usize) { + pub fn set_animation(&mut self, anim: Lerp, led_index: usize) { assert!(led_index < NUM_LEDS); self.animation_buf[led_index] = Some(anim); } @@ -128,9 +130,9 @@ where [(); (NUM_LEDS * 4) + 8]: { } // set colors from animations - for (i, anim) in self.animation_buf.into_iter().enumerate() { + for (i, anim) in self.animation_buf.iter().enumerate() { if let Some(anim) = anim { - self.set_color(anim.get_color(), i); + self.set_color(anim.get_value(), i); } } @@ -138,177 +140,3 @@ where [(); (NUM_LEDS * 4) + 8]: { } } -#[derive(Clone, Copy, PartialEq, Eq, Debug)] -pub enum Apa102AnimationRepeat { - NoRepeat, - RepeatForever, - RepeatFixed(usize), -} - -pub trait Apa102AnimationTrait: Sized { - fn start_animation(&mut self); - fn stop_animation(&mut self); - fn animation_running(&self) -> bool; - fn update(&mut self); - fn get_color(&self) -> RGB8; -} - -// #[derive(Clone, Copy, Debug)] -// pub enum Apa102Animation { -// Blink(Apa102Blink), -// } - -// impl Apa102AnimationTrait for Apa102Animation { -// fn start_animation(&mut self) { -// match self { -// Apa102Animation::Blink(anim), Apa102Animation::Blink2(anim) => { - -// }, -// } -// } - -// fn stop_animation(&mut self) { -// todo!() -// } - -// fn animation_running(&self) -> bool { -// todo!() -// } - -// fn update(&mut self) -> RGB8 { -// todo!() -// } -// } - -// pub struct Apa102AnimationSequence { -// animations: [Apa102Animation; NUM_ANIM], -// repeat_style: Apa102AnimationRepeat, - -// animation_running: bool, -// repeat_counter: usize, -// animation_index: usize, -// } - -// impl Apa102AnimationSequence { -// pub fn new(anim_sequence: [Apa102Animation; NUM_ANIM], repeat_style: Apa102AnimationRepeat) -> Self { -// Apa102AnimationSequence { -// animations: anim_sequence, -// repeat_style: repeat_style, - -// animation_running: false, -// repeat_counter: 0, -// animation_index: 0, -// } -// } -// } - -// impl Apa102AnimationTrait for Apa102AnimationSequence { -// fn start_animation(&mut self) { -// self.animation_index = 0; -// self.animation_running = true; - -// if let Apa102AnimationRepeat::RepeatFixed(num) = self.repeat_style { -// self.repeat_counter = num; -// } -// } - -// fn stop_animation(&mut self) { -// self.animation_running = false; -// } - -// fn animation_running(&self) -> bool { -// return self.animation_running; -// } - -// fn update(&mut self) -> RGB8 { - -// } -// } - -#[derive(Clone, Copy, Debug)] -pub struct Apa102Blink { - color_one: RGB8, - color_two: RGB8, - c1_time: u64, - c2_time: u64, - repeat_style: Apa102AnimationRepeat, - - animation_running: bool, - repeat_counter: usize, - start_time: Instant, - last_color: RGB8, -} - -impl Apa102Blink { - pub fn new(color_one: RGB8, color_two: RGB8, c1_time: Duration, c2_time: Duration, repeat_style: Apa102AnimationRepeat) -> Self { - Apa102Blink { - color_one: color_one, - color_two: color_two, - c1_time: c1_time.as_millis(), - c2_time: c2_time.as_millis(), - repeat_style: repeat_style, - - animation_running: false, - repeat_counter: 0, - start_time: Instant::now(), - last_color: color_one, - } - } -} - -impl Apa102AnimationTrait for Apa102Blink { - fn start_animation(&mut self) { - self.start_time = Instant::now(); - self.animation_running = true; - - if let Apa102AnimationRepeat::RepeatFixed(num) = self.repeat_style { - self.repeat_counter = num; - } - } - - fn stop_animation(&mut self) { - self.animation_running = false; - } - - fn animation_running(&self) -> bool { - self.animation_running - } - - fn update(&mut self) { - if !self.animation_running { - return; - } - - let now = Instant::now(); - let elapsed_time = (now - self.start_time).as_millis(); - - if elapsed_time <= self.c1_time { - self.last_color = self.color_one; - } else if self.c1_time < elapsed_time && elapsed_time <= self.c1_time + self.c2_time { - self.last_color = self.color_two; - } else if elapsed_time > self.c1_time + self.c2_time { - match self.repeat_style { - Apa102AnimationRepeat::NoRepeat => { - self.animation_running = false; - }, - Apa102AnimationRepeat::RepeatForever => { - self.start_time = now; - self.last_color = self.color_one; - }, - Apa102AnimationRepeat::RepeatFixed(_) => { - if self.repeat_counter == 0 { - self.animation_running = false; - } else { - self.repeat_counter = self.repeat_counter - 1; - self.start_time = now; - self.last_color = self.color_one; - } - }, - } - } - } - - fn get_color(&self) -> RGB8 { - self.last_color - } -} \ No newline at end of file diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index 94782252..b35b18c6 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -8,6 +8,7 @@ #![feature(ptr_metadata)] #![feature(trait_alias)] +pub mod anim; pub mod drivers; pub mod math; pub mod uart; diff --git a/lib-stm32/src/math/lerp.rs b/lib-stm32/src/math/lerp.rs index 2be65349..62390f7a 100644 --- a/lib-stm32/src/math/lerp.rs +++ b/lib-stm32/src/math/lerp.rs @@ -1,20 +1,44 @@ use core::marker::PhantomData; use embassy_time::{Duration, Instant}; -use num_traits::{clamp, Bounded, CheckedAdd, CheckedDiv, CheckedMul, CheckedSub, FromPrimitive, ToPrimitive}; +use num_traits::{clamp, Bounded, FromPrimitive}; use smart_leds::RGB8; -pub fn lerp + CheckedSub + CheckedMul + CheckedDiv>(a: N, b: N, t: N) -> N { - let t_pct = t.to_f32().unwrap() / N::max_value().to_f32().unwrap(); - return N::from_f32(lerp_f(a.to_f32().unwrap(), b.to_f32().unwrap(), t_pct)).unwrap(); +pub trait LerpNumeric = Copy + Bounded + FromPrimitive; + +// pub trait LerpNumeric = Copy + Bounded + FromPrimitive + ToPrimitive; +// pub trait LerpTrait = Copy + Sized + Lerp; + +// pub fn lerp + CheckedSub + CheckedMul + CheckedDiv>(a: N, b: N, t: N) -> N { +// { +// let t_pct = t.to_f32().unwrap() / N::max_value().to_f32().unwrap(); +// return N::from_f32(lerp_f(a.to_f32().unwrap(), b.to_f32().unwrap(), t_pct)).unwrap(); +// } + +pub fn lerp(a: N, b: N, t: N) -> N +where N: FromPrimitive + Copy + Bounded, + f32: core::convert::From +{ + let t_pct = Into::::into(t) / Into::::into(N::max_value()); + return N::from_f32(f_lerp_f(Into::::into(a), Into::::into(b), t_pct)).unwrap(); +} + +pub fn lerp_f(a: N, b: N, t: f32) -> N +where N: FromPrimitive + Copy + Bounded, + f32: core::convert::From +{ + return N::from_f32(f_lerp_f(Into::::into(a), Into::::into(b), t)).unwrap(); } -pub fn lerp_f(a: f32, b: f32, t: f32) -> f32 { +pub fn f_lerp_f(a: f32, b: f32, t: f32) -> f32 { let t = clamp(t, 0.0, 1.0); return a + (b - a) * t; } -pub trait Lerp + CheckedSub + CheckedMul + CheckedDiv> { +pub trait Lerp: Clone + Copy +where N: FromPrimitive + Copy + Bounded, + f32: core::convert::From +{ fn lerp(a: Self, b: Self, t: N) -> Self; fn lerp_f(a: Self, b: Self, t: f32) -> Self; } @@ -30,14 +54,17 @@ impl Lerp for RGB8 { fn lerp_f(a: RGB8, b: RGB8, t: f32) -> RGB8 { RGB8 { - r: lerp_f(a.r as f32, b.r as f32, t) as u8, - g: lerp_f(a.g as f32, b.g as f32, t) as u8, - b: lerp_f(a.b as f32, b.b as f32, t) as u8, + r: f_lerp_f(a.r as f32, b.r as f32, t) as u8, + g: f_lerp_f(a.g as f32, b.g as f32, t) as u8, + b: f_lerp_f(a.b as f32, b.b as f32, t) as u8, } } } -pub struct TimeLerp<'a, N: Copy + Bounded + CheckedAdd + CheckedSub + CheckedMul + CheckedDiv, L: Copy + Sized + Lerp> { +pub struct TimeLerp<'a, N, L: Lerp> +where N: FromPrimitive + Copy + Bounded, + f32: core::convert::From +{ a: L, b: L, @@ -47,7 +74,11 @@ pub struct TimeLerp<'a, N: Copy + Bounded + CheckedAdd + CheckedSub, } -impl<'a, N: Copy + Bounded + CheckedAdd + CheckedSub + CheckedMul + CheckedDiv, L: Copy + Sized + Lerp> TimeLerp<'a, N, L> { +impl<'a, N, L> TimeLerp<'a, N, L> +where N: FromPrimitive + Copy + Bounded, +f32: core::convert::From, +L: Lerp +{ pub fn new(a: L, b: L, duration: Duration) -> TimeLerp<'a, N, L> { TimeLerp { a: a, From ec21b17166242c75230f8df6d0e52690ee8e5aa6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 17:10:23 -0400 Subject: [PATCH 054/157] led animation framework done --- control-board/src/tasks/user_io_task.rs | 36 ++++++++++++----- lib-stm32/src/anim/mod.rs | 17 +++++--- lib-stm32/src/drivers/led/apa102.rs | 52 ++++++++++++++++++++----- lib-stm32/src/lib.rs | 1 + 4 files changed, 80 insertions(+), 26 deletions(-) diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 2be038a2..9456cbf7 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,6 +1,6 @@ -use ateam_lib_stm32::anim::{AnimInterface, AnimRepeatMode, Blink, Lerp}; -use ateam_lib_stm32::drivers::led::apa102::Apa102; +use ateam_lib_stm32::anim::{self, AnimInterface, AnimRepeatMode, Blink, CompositeAnimation, Lerp}; +use ateam_lib_stm32::drivers::led::apa102::{Apa102, Apa102Anim}; use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; use embassy_stm32::spi::{Config, Spi}; @@ -52,18 +52,34 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, ) { defmt::info!("user io task initialized"); - dotstars.set_drv_str_all(8); + let anim0 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim2 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let mut anim_seq = [anim0, anim1, anim2]; + let mut composite_anim0 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); + composite_anim0.start_animation(); + + let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim1_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 255 }, RGB8 { r: 255, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim2_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 255 }, RGB8 { r: 255, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let mut anim_seq = [anim0_1, anim1_1, anim2_1]; + let mut composite_anim1 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); + composite_anim1.start_animation(); + + dotstars.set_drv_str_all(32); + let mut dotstars_anim = Apa102Anim::new(dotstars); // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); // color_lerp.start(); - let mut anim0 = Lerp::new(RGB8 { r: 255, g: 255, b: 255 }, RGB8 { r: 0, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::Forever); - anim0.start_animation(); - dotstars.set_animation(anim0, 0); - let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); - anim1.start_animation(); - dotstars.set_animation(anim1, 1); + + dotstars_anim.set_animation(&mut composite_anim0, 0); + dotstars_anim.set_animation(&mut composite_anim1, 1); + + // let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); + // anim1.start_animation(); + // dotstars_anim.set_animation(anim1, 1); // let mut blink_anim = Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), AnimRepeatMode::Forever); // blink_anim.start_animation(); @@ -116,7 +132,7 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, // let color = color_lerp.update(); // dotstars.set_color(color, 0); // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); - dotstars.update().await; + dotstars_anim.update().await; if !robot_state.hw_init_state_valid() { diff --git a/lib-stm32/src/anim/mod.rs b/lib-stm32/src/anim/mod.rs index 4d184d8f..e251f93b 100644 --- a/lib-stm32/src/anim/mod.rs +++ b/lib-stm32/src/anim/mod.rs @@ -112,11 +112,11 @@ L: crate::math::lerp::Lerp } } -pub struct CompositeAnimation +pub struct CompositeAnimation<'a, N, L> where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { - animations: [Animation; LEN], + animations: &'a mut [Animation], active_animation: usize, repeat_mode: AnimRepeatMode, @@ -126,12 +126,12 @@ L: crate::math::lerp::Lerp { last_value: L, } -impl CompositeAnimation +impl<'a, N, L> CompositeAnimation<'a, N, L> where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { - pub fn new(animations: [Animation; LEN], repeat_mode: AnimRepeatMode) -> Self { + pub fn new(animations: &'a mut [Animation], repeat_mode: AnimRepeatMode) -> Self { let first_val = animations[0].get_value(); CompositeAnimation { animations: animations, @@ -144,7 +144,7 @@ L: crate::math::lerp::Lerp } } -impl AnimInterface for CompositeAnimation +impl<'a, N, L> AnimInterface for CompositeAnimation<'a, N, L> where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp @@ -152,6 +152,7 @@ L: crate::math::lerp::Lerp fn start_animation(&mut self) { self.active_animation = 0; self.anim_state = AnimState::Running; + self.animations[self.active_animation].start_animation(); if let AnimRepeatMode::Fixed(num) = self.repeat_mode { self.repeat_counter = num; @@ -180,7 +181,7 @@ L: crate::math::lerp::Lerp // advance to the next animation self.active_animation = self.active_animation + 1; - if self.active_animation >= LEN { + if self.active_animation >= self.animations.len() { self.active_animation = 0; // we've completed the last animation in the buffer @@ -200,6 +201,10 @@ L: crate::math::lerp::Lerp }, } } + + if self.anim_state != AnimState::Completed { + self.animations[self.active_animation].start_animation(); + } } } diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index a31bc7f5..dc86c1eb 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -11,7 +11,6 @@ pub struct Apa102<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { spi: spi::Spi<'a, SpiPeri, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], - animation_buf: [Option>; NUM_LEDS], } impl<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102<'a, 'buf, SpiPeri, NUM_LEDS> @@ -35,7 +34,6 @@ where [(); (NUM_LEDS * 4) + 8]: { Apa102 { spi: spi, spi_buf: spi_buf, - animation_buf: [None; NUM_LEDS], } } @@ -111,14 +109,24 @@ where [(); (NUM_LEDS * 4) + 8]: { self.set_color_range(color, 0..NUM_LEDS) } - pub fn set_animation(&mut self, anim: Lerp, led_index: usize) { - assert!(led_index < NUM_LEDS); - self.animation_buf[led_index] = Some(anim); + pub async fn update(&mut self) { + let _ = self.spi.write(self.spi_buf).await; } +} - pub fn clear_animation(&mut self, led_index: usize) { - assert!(led_index < NUM_LEDS); - self.animation_buf[led_index] = None; +pub struct Apa102Anim<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> +where [(); (NUM_LEDS * 4) + 8]: { + apa102_driver: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>, + animation_buf: [Option<&'ca mut CompositeAnimation<'ca, u8, RGB8>>; NUM_LEDS], +} + +impl<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, SpiPeri, NUM_LEDS> +where [(); (NUM_LEDS * 4) + 8]: { + pub fn new(apa102: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>) -> Self { + Apa102Anim { + apa102_driver: apa102, + animation_buf: [const { None }; NUM_LEDS], + } } pub async fn update(&mut self) { @@ -132,11 +140,35 @@ where [(); (NUM_LEDS * 4) + 8]: { // set colors from animations for (i, anim) in self.animation_buf.iter().enumerate() { if let Some(anim) = anim { - self.set_color(anim.get_value(), i); + self.apa102_driver.set_color(anim.get_value(), i); } } - let _ = self.spi.write(self.spi_buf).await; + self.apa102_driver.update().await; + } + + pub fn set_animation(&mut self, anim: &'ca mut CompositeAnimation<'ca, u8, smart_leds::RGB>, led_index: usize) { + assert!(led_index < NUM_LEDS); + self.animation_buf[led_index] = Some(anim); } + + pub fn clear_animation(&mut self, led_index: usize) { + assert!(led_index < NUM_LEDS); + self.animation_buf[led_index] = None; + } + + // pub fn start_animation(&mut self, led_index: usize) { // pub fn start_animation(&mut self, led_index: usize) { + // assert!(led_index < NUM_LEDS); + + // if let Some(anim) = self.animation_buf[led_index].as_mut() { + // anim.start_animation(); + // } + // } + // assert!(led_index < NUM_LEDS); + + // if let Some(anim) = self.animation_buf[led_index].as_mut() { + // anim.start_animation(); + // } + // } } diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index b35b18c6..4bbc72f3 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -1,5 +1,6 @@ #![no_std] #![feature(generic_const_exprs)] +#![feature(inline_const)] #![feature(maybe_uninit_uninit_array)] #![feature(sync_unsafe_cell)] #![feature(type_alias_impl_trait)] From c3301aeefe56363a5927444bbd175f96f81fa341 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 1 Jun 2024 17:34:40 -0400 Subject: [PATCH 055/157] Sharing with Will <3 --- lib-stm32/src/audio/mod.rs | 1 + lib-stm32/src/audio/pitch.rs | 0 lib-stm32/src/lib.rs | 1 + 3 files changed, 2 insertions(+) create mode 100644 lib-stm32/src/audio/mod.rs create mode 100644 lib-stm32/src/audio/pitch.rs diff --git a/lib-stm32/src/audio/mod.rs b/lib-stm32/src/audio/mod.rs new file mode 100644 index 00000000..90ab0eb2 --- /dev/null +++ b/lib-stm32/src/audio/mod.rs @@ -0,0 +1 @@ +pub mod pitch; \ No newline at end of file diff --git a/lib-stm32/src/audio/pitch.rs b/lib-stm32/src/audio/pitch.rs new file mode 100644 index 00000000..e69de29b diff --git a/lib-stm32/src/lib.rs b/lib-stm32/src/lib.rs index b35b18c6..fd56feeb 100644 --- a/lib-stm32/src/lib.rs +++ b/lib-stm32/src/lib.rs @@ -9,6 +9,7 @@ #![feature(trait_alias)] pub mod anim; +pub mod audio; pub mod drivers; pub mod math; pub mod uart; From f2aa6476275e136d93c93c2e7cfa7105ca436a52 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 1 Jun 2024 18:21:26 -0400 Subject: [PATCH 056/157] Change pitch to note --- lib-stm32/build.rs | 20 ++++++++++++++++++++ lib-stm32/src/audio/mod.rs | 4 +++- lib-stm32/src/audio/note.rs | 4 ++++ lib-stm32/src/audio/pitch.rs | 0 4 files changed, 27 insertions(+), 1 deletion(-) create mode 100644 lib-stm32/build.rs create mode 100644 lib-stm32/src/audio/note.rs delete mode 100644 lib-stm32/src/audio/pitch.rs diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs new file mode 100644 index 00000000..1b3f8aa9 --- /dev/null +++ b/lib-stm32/build.rs @@ -0,0 +1,20 @@ +fn generate_pitch_set(ref_pitch: u16) { + // (Ref pitch is assumed to be A4) + // Calculate all (reasonable) pitches + // Open the file (audio/pitches.rs) + // Write to it + // Close the file +} + +fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ + if semitones < 0 { + let diff = (semitones * -1) as u16; + return ref_pitch / 2.pow(diff/12); + } else { + return 2.pow(semitones/12) * ref_pitch; + } +} + +fn main() { + +} \ No newline at end of file diff --git a/lib-stm32/src/audio/mod.rs b/lib-stm32/src/audio/mod.rs index 90ab0eb2..834ad035 100644 --- a/lib-stm32/src/audio/mod.rs +++ b/lib-stm32/src/audio/mod.rs @@ -1 +1,3 @@ -pub mod pitch; \ No newline at end of file +pub mod note; + +pub mod pitches; \ No newline at end of file diff --git a/lib-stm32/src/audio/note.rs b/lib-stm32/src/audio/note.rs new file mode 100644 index 00000000..174c16aa --- /dev/null +++ b/lib-stm32/src/audio/note.rs @@ -0,0 +1,4 @@ +enum Note { + Pitch {pitch: u16, duration: u32}, + Rest(u32), +} \ No newline at end of file diff --git a/lib-stm32/src/audio/pitch.rs b/lib-stm32/src/audio/pitch.rs deleted file mode 100644 index e69de29b..00000000 From 8048d0204e94d356e3d1ca2dcee96cb683296650 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 1 Jun 2024 21:14:06 -0400 Subject: [PATCH 057/157] Build.rs file to write initial pitch set --- lib-stm32/build.rs | 50 +++++++++++++++++++++++++++++-------- lib-stm32/src/audio/note.rs | 4 +-- 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index 1b3f8aa9..b7e1a89c 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -1,20 +1,50 @@ -fn generate_pitch_set(ref_pitch: u16) { +use std::fs; +use std::io::{BufWriter, Write}; + +fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) { // (Ref pitch is assumed to be A4) - // Calculate all (reasonable) pitches + let mut pitches = Vec::<(&str, u16)>::new(); + let scale : [&str; 12] = ["a", "as", "b", "c", "cs", "d", "ds", "e", "f", "fs", "g", "gs"]; + // Calculate ALL the pitches! + for o in 1..octaves_below { + for semitone in 0..11 { + // vector of (pitch name, freq in hertz) + // -12 semitones for each octave + number of semitones above A in that octave + let freq = generate_pitch_from_reference(ref_freq, (-12 * o) + semitone); + let pitch = (scale[semitone as usize], freq); + pitches.push(pitch); + } + } + // Start at 0 since we also have to include a4-a5 + // (starting with the reference pitch) + for o in 0..octaves_above - 1 { + for semitone in 0..11 { + // vector of (pitch name, freq in hertz) + // +12 semitones for each octave + number of semitones above A in that octave + let freq = generate_pitch_from_reference(ref_freq, (12 * o) + semitone); + let pitch = (scale[semitone as usize] + String::from(4 + o), freq); + pitches.push(pitch); + } +} + +fn write_pitches_to_file(mut pitch_set: Vec<(&str, u16)>){ // Open the file (audio/pitches.rs) + let f = fs::File::create("./audio/pitches.rs").expect("Unable to create pitches.rs"); + let mut f = BufWriter::new(f); // Write to it - // Close the file + for pitch in pitch_set { + writeln!(f, "let {}: u16 = {}", pitch[0], pitch[1]); + } + f.write_all(data.as_bytes()).expect("Unable to write pitch data"); } fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ - if semitones < 0 { - let diff = (semitones * -1) as u16; - return ref_pitch / 2.pow(diff/12); - } else { - return 2.pow(semitones/12) * ref_pitch; - } + let diff = semitones as f32 / 12_f32; + return (ref_pitch as f32/ 2_f32.powf(diff)) as u16; } fn main() { - + let a4 = 440; + let mut pitch_set = generate_pitch_set(a4, 2, 2); + write_pitches_to_file(pitch_set); } \ No newline at end of file diff --git a/lib-stm32/src/audio/note.rs b/lib-stm32/src/audio/note.rs index 174c16aa..7200a304 100644 --- a/lib-stm32/src/audio/note.rs +++ b/lib-stm32/src/audio/note.rs @@ -1,4 +1,4 @@ -enum Note { - Pitch {pitch: u16, duration: u32}, +enum Beat { + Note {tone: u16, duration: u32}, Rest(u32), } \ No newline at end of file From ecedb6422a127ff4e7b3c0e33dad79b2e55bc508 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 21:28:10 -0400 Subject: [PATCH 058/157] buzzer driver and tone player --- control-board/Cargo.toml | 5 ++ control-board/src/bin/hwtest-piezo/main.rs | 90 ++++++++++++++++++++++ control-board/src/lib.rs | 1 + control-board/src/songs.rs | 24 ++++++ lib-stm32/build.rs | 30 ++++---- lib-stm32/src/audio/mod.rs | 3 +- lib-stm32/src/audio/note.rs | 8 +- lib-stm32/src/audio/tone_player.rs | 64 +++++++++++++++ lib-stm32/src/drivers/audio/buzzer.rs | 59 ++++++++++++++ lib-stm32/src/drivers/audio/mod.rs | 6 ++ lib-stm32/src/drivers/mod.rs | 2 +- 11 files changed, 272 insertions(+), 20 deletions(-) create mode 100644 control-board/src/bin/hwtest-piezo/main.rs create mode 100644 control-board/src/songs.rs create mode 100644 lib-stm32/src/audio/tone_player.rs create mode 100644 lib-stm32/src/drivers/audio/buzzer.rs create mode 100644 lib-stm32/src/drivers/audio/mod.rs diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index 8dcf2197..eba17d69 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -101,6 +101,11 @@ name = "hwtest-motor" test = false harness = false +[[bin]] +name = "hwtest-piezo" +test = false +harness = false + [[bin]] name = "hwtest-radio" test = false diff --git a/control-board/src/bin/hwtest-piezo/main.rs b/control-board/src/bin/hwtest-piezo/main.rs new file mode 100644 index 00000000..70d90bfc --- /dev/null +++ b/control-board/src/bin/hwtest-piezo/main.rs @@ -0,0 +1,90 @@ +#![no_std] +#![no_main] + +use ateam_lib_stm32::{audio::tone_player::{self, TonePlayer}, drivers::audio::buzzer::Buzzer}; +use embassy_executor::InterruptExecutor; +use embassy_stm32::{ + gpio::OutputType, interrupt, peripherals::TIM15, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} +}; + +use defmt_rtt as _; + +use ateam_control_board::{create_io_task, get_system_config, robot_state::SharedRobotState, songs::TEST_SONG}; + + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); + +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + /////////////////// + // start tasks // + /////////////////// + + + + create_io_task!(main_spawner, + robot_state, + p); + + let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); + let pwm = SimplePwm::new(p.TIM15, None, Some(ch2), None, None, khz(2), Default::default()); + + let audio_driver = Buzzer::new(pwm, Channel::Ch2); + let mut tone_player = TonePlayer::new(audio_driver); + + if tone_player.load_song(&TEST_SONG).is_err() { + defmt::warn!("song uses pitch or duration outside of operating range"); + } + + tone_player.play_song().await; + + + + // let max = pwm.get_max_duty(); + // pwm.enable(Channel::Ch2); + + + // pwm.set_duty(Channel::Ch2, max / 2); + + // loop { + // pwm.set_frequency(hz(35)); + // Timer::after_millis(1000).await; + // pwm.set_frequency(hz(7000)); + // Timer::after_millis(1000).await; + // pwm.disable(Channel::Ch2); + + // break; + // } + + loop { + Timer::after_millis(10).await; + } +} \ No newline at end of file diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 5eac9d51..884fe776 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -29,6 +29,7 @@ pub mod parameter_interface; pub mod pins; // pub mod radio; pub mod robot_state; +pub mod songs; pub mod stm32_interface; pub mod stspin_motor; diff --git a/control-board/src/songs.rs b/control-board/src/songs.rs new file mode 100644 index 00000000..03fa4f70 --- /dev/null +++ b/control-board/src/songs.rs @@ -0,0 +1,24 @@ +use ateam_lib_stm32::audio::note::Beat; + + +pub const TEST_SONG: [Beat; 17] = [ + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { tone: 440, duration: 250_000 }, + Beat::Note { tone: 523, duration: 250_000 }, + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Note { tone: 440, duration: 250_000 }, + Beat::Note { tone: 523, duration: 250_000 }, + + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { tone: 329, duration: 125_000 }, + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { tone: 329, duration: 125_000 }, + Beat::Note { tone: 440, duration: 125_000 }, + Beat::Note { tone: 329, duration: 125_000 }, + Beat::Note { tone: 523, duration: 125_000 }, + Beat::Note { tone: 329, duration: 125_000 }, + + Beat::Note { tone: 392, duration: 1_000_000 }, + ]; \ No newline at end of file diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index 1b3f8aa9..15a8fde5 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -1,19 +1,19 @@ -fn generate_pitch_set(ref_pitch: u16) { - // (Ref pitch is assumed to be A4) - // Calculate all (reasonable) pitches - // Open the file (audio/pitches.rs) - // Write to it - // Close the file -} +// fn generate_pitch_set(ref_pitch: u16) { +// // (Ref pitch is assumed to be A4) +// // Calculate all (reasonable) pitches +// // Open the file (audio/pitches.rs) +// // Write to it +// // Close the file +// } -fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ - if semitones < 0 { - let diff = (semitones * -1) as u16; - return ref_pitch / 2.pow(diff/12); - } else { - return 2.pow(semitones/12) * ref_pitch; - } -} +// fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ +// if semitones < 0 { +// let diff = (semitones * -1) as u16; +// return ref_pitch / 2.pow(diff/12); +// } else { +// return 2.pow(semitones/12) * ref_pitch; +// } +// } fn main() { diff --git a/lib-stm32/src/audio/mod.rs b/lib-stm32/src/audio/mod.rs index 834ad035..3cd10bef 100644 --- a/lib-stm32/src/audio/mod.rs +++ b/lib-stm32/src/audio/mod.rs @@ -1,3 +1,4 @@ pub mod note; +pub mod tone_player; -pub mod pitches; \ No newline at end of file +// pub mod pitches; \ No newline at end of file diff --git a/lib-stm32/src/audio/note.rs b/lib-stm32/src/audio/note.rs index 174c16aa..c5d5bdb5 100644 --- a/lib-stm32/src/audio/note.rs +++ b/lib-stm32/src/audio/note.rs @@ -1,4 +1,6 @@ -enum Note { - Pitch {pitch: u16, duration: u32}, +pub enum Beat { + Note {tone: u16, duration: u32}, Rest(u32), -} \ No newline at end of file +} + +pub type Song = [Beat]; \ No newline at end of file diff --git a/lib-stm32/src/audio/tone_player.rs b/lib-stm32/src/audio/tone_player.rs new file mode 100644 index 00000000..f6c1d529 --- /dev/null +++ b/lib-stm32/src/audio/tone_player.rs @@ -0,0 +1,64 @@ + +use embassy_time::Timer; + +use crate::drivers::audio::PlayTone; +use super::note::{Beat, Song}; + + +pub struct TonePlayer<'a, D: PlayTone> { + audio_driver: D, + song: Option<&'a Song>, +} + +impl<'a, D: PlayTone> TonePlayer<'a, D> { + pub fn new(audio_driver: D) -> Self { + TonePlayer { + audio_driver: audio_driver, + song: None, + } + } + + pub fn load_song(&mut self, song: &'a Song) -> Result<(), ()> { + let mut can_play = true; + + 'note_loop: + for beat in song.iter() { + match beat { + Beat::Note { tone, duration: _ } => { + if !self.audio_driver.can_play_tone(*tone) { + can_play = false; + break 'note_loop; + } + }, + Beat::Rest(_) => { }, + } + } + + if !can_play { + return Err(()); + } + + self.song = Some(song); + + Ok(()) + } + + pub async fn play_song(&mut self) { + if let Some(song) = self.song { + for beat in song.iter() { + match beat { + Beat::Note { tone, duration } => { + self.audio_driver.play_tone(*tone); + Timer::after_micros(*duration as u64).await; + }, + Beat::Rest(duration) => { + self.audio_driver.play_tone(0); + Timer::after_micros(*duration as u64).await; + }, + } + } + + self.audio_driver.play_tone(0); + } + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/audio/buzzer.rs b/lib-stm32/src/drivers/audio/buzzer.rs new file mode 100644 index 00000000..3b2406d8 --- /dev/null +++ b/lib-stm32/src/drivers/audio/buzzer.rs @@ -0,0 +1,59 @@ +use embassy_stm32::{time::hz, timer::{simple_pwm::SimplePwm, Channel, GeneralInstance4Channel}}; +use num_traits::clamp; + +use super::PlayTone; + +pub const BUZZER_MIN_FREQ: u16 = 35; +pub const BUZZER_MAX_FREQ: u16 = 7000; + +pub struct Buzzer<'d, T: GeneralInstance4Channel> { + pwm: SimplePwm<'d, T>, + channel: Channel, + + min_freq: u16, + max_freq: u16, +} + +impl<'d, T: GeneralInstance4Channel> Buzzer<'d, T> { + pub fn new(pwm: SimplePwm<'d, T>, channel: Channel) -> Self { + Buzzer { + pwm: pwm, + channel: channel, + min_freq: BUZZER_MIN_FREQ, + max_freq: BUZZER_MAX_FREQ, + } + } + + pub fn new_with_fcontrs(pwm: SimplePwm<'d, T>, channel: Channel, min_freq: u16, max_freq: u16) -> Self { + Buzzer { + pwm: pwm, + channel: channel, + min_freq: min_freq, + max_freq: max_freq, + } + } + + pub fn init(&mut self) { + let max_duty = self.pwm.get_max_duty(); + self.pwm.set_duty(self.channel, max_duty / 2); + self.pwm.enable(self.channel); + } +} + +impl<'d, T: GeneralInstance4Channel> PlayTone for Buzzer<'d, T> { + fn play_tone(&mut self, freq: u16) { + if freq == 0 { + self.pwm.disable(self.channel); + } else { + let freq = clamp(freq, self.min_freq, self.max_freq); + self.pwm.set_frequency(hz(freq.into())); + let max_duty = self.pwm.get_max_duty(); + self.pwm.set_duty(self.channel, max_duty / 2); + self.pwm.enable(self.channel); + } + } + + fn can_play_tone(&self, freq: u16) -> bool { + self.min_freq < freq && freq < self.max_freq + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/audio/mod.rs b/lib-stm32/src/drivers/audio/mod.rs new file mode 100644 index 00000000..9b58d074 --- /dev/null +++ b/lib-stm32/src/drivers/audio/mod.rs @@ -0,0 +1,6 @@ +pub mod buzzer; + +pub trait PlayTone { + fn play_tone(&mut self, tone: u16); + fn can_play_tone(&self, tone: u16) -> bool; +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index 2c1e7d4a..a6106cbf 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -1,4 +1,4 @@ - +pub mod audio; pub mod flash; pub mod imu; pub mod led; From 1847d197a50d7a9e6d0ad15866879aa4f9099a11 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 1 Jun 2024 21:43:25 -0400 Subject: [PATCH 059/157] It's integration time baby --- lib-stm32/build.rs | 25 ++++++++++++++----------- lib-stm32/src/audio/pitches.rs | 22 ++++++++++++++++++++++ 2 files changed, 36 insertions(+), 11 deletions(-) create mode 100644 lib-stm32/src/audio/pitches.rs diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index b7e1a89c..2c5edafd 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -1,9 +1,9 @@ use std::fs; use std::io::{BufWriter, Write}; -fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) { +fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) -> Vec<(String, u16)>{ // (Ref pitch is assumed to be A4) - let mut pitches = Vec::<(&str, u16)>::new(); + let mut pitches = Vec::<(String, u16)>::new(); let scale : [&str; 12] = ["a", "as", "b", "c", "cs", "d", "ds", "e", "f", "fs", "g", "gs"]; // Calculate ALL the pitches! for o in 1..octaves_below { @@ -11,7 +11,7 @@ fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) { // vector of (pitch name, freq in hertz) // -12 semitones for each octave + number of semitones above A in that octave let freq = generate_pitch_from_reference(ref_freq, (-12 * o) + semitone); - let pitch = (scale[semitone as usize], freq); + let pitch = (format!("{}{}", scale[semitone as usize], 4 - o), freq); pitches.push(pitch); } } @@ -22,20 +22,23 @@ fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) { // vector of (pitch name, freq in hertz) // +12 semitones for each octave + number of semitones above A in that octave let freq = generate_pitch_from_reference(ref_freq, (12 * o) + semitone); - let pitch = (scale[semitone as usize] + String::from(4 + o), freq); + let pitch = (format!("{}{}", scale[semitone as usize], 4 + o), freq); pitches.push(pitch); } + } + return pitches; } -fn write_pitches_to_file(mut pitch_set: Vec<(&str, u16)>){ - // Open the file (audio/pitches.rs) - let f = fs::File::create("./audio/pitches.rs").expect("Unable to create pitches.rs"); +fn write_pitches_to_file(pitch_set: Vec<(String, u16)>){ + /// Write each pitch name as a constant + /// in the format PITCH_NAME_OCTAVE, ex. + /// pub const A4: u16 = 440 + let f = fs::File::create("./src/audio/pitches.rs").expect("Unable to create pitches.rs"); let mut f = BufWriter::new(f); - // Write to it for pitch in pitch_set { - writeln!(f, "let {}: u16 = {}", pitch[0], pitch[1]); + let _ = writeln!(f, "pub const {}: u16 = {};", pitch.0.to_uppercase(), pitch.1); } - f.write_all(data.as_bytes()).expect("Unable to write pitch data"); + let _ = f.flush(); } fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ @@ -45,6 +48,6 @@ fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ fn main() { let a4 = 440; - let mut pitch_set = generate_pitch_set(a4, 2, 2); + let pitch_set = generate_pitch_set(a4, 2, 2); write_pitches_to_file(pitch_set); } \ No newline at end of file diff --git a/lib-stm32/src/audio/pitches.rs b/lib-stm32/src/audio/pitches.rs new file mode 100644 index 00000000..80fdfe4b --- /dev/null +++ b/lib-stm32/src/audio/pitches.rs @@ -0,0 +1,22 @@ +pub const A3: u16 = 880; +pub const AS3: u16 = 830; +pub const B3: u16 = 783; +pub const C3: u16 = 739; +pub const CS3: u16 = 698; +pub const D3: u16 = 659; +pub const DS3: u16 = 622; +pub const E3: u16 = 587; +pub const F3: u16 = 554; +pub const FS3: u16 = 523; +pub const G3: u16 = 493; +pub const A4: u16 = 440; +pub const AS4: u16 = 415; +pub const B4: u16 = 391; +pub const C4: u16 = 369; +pub const CS4: u16 = 349; +pub const D4: u16 = 329; +pub const DS4: u16 = 311; +pub const E4: u16 = 293; +pub const F4: u16 = 277; +pub const FS4: u16 = 261; +pub const G4: u16 = 246; From 6cb1a6a4e66dee38d904bb151f21403a9c84ea5b Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 21:43:36 -0400 Subject: [PATCH 060/157] audio task wip, prep merge --- control-board/src/robot_state.rs | 7 +++++++ control-board/src/tasks/audio_task.rs | 0 2 files changed, 7 insertions(+) create mode 100644 control-board/src/tasks/audio_task.rs diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 66a11f2c..631072b3 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -23,6 +23,8 @@ pub struct SharedRobotState { battery_pct: AtomicU8, battery_ok: AtomicBool, + robot_tipped: AtomicBool, + shutdown_requested: AtomicBool, } @@ -43,6 +45,7 @@ impl SharedRobotState { radio_connection_ok: AtomicBool::new(false), battery_pct: AtomicU8::new(0), battery_ok: AtomicBool::new(false), + robot_tipped: AtomicBool::new(false), shutdown_requested: AtomicBool::new(false), } } @@ -67,6 +70,8 @@ impl SharedRobotState { battery_pct: 0, battery_ok: false, + robot_tipped: false, + shutdown_requested: self.shutdown_requested(), } } @@ -141,5 +146,7 @@ pub struct RobotState { pub battery_pct: u8, pub battery_ok: bool, + pub robot_tipped: bool, + pub shutdown_requested: bool, } \ No newline at end of file diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs new file mode 100644 index 00000000..e69de29b From 0e6535d72932c50b8dc6c0f10422bec1b70367c9 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 22:25:44 -0400 Subject: [PATCH 061/157] robot tip detection --- control-board/src/bin/control/main.rs | 1 + control-board/src/bin/hwtest-imu/main.rs | 18 ++++++++- control-board/src/robot_state.rs | 11 +++++- control-board/src/songs.rs | 16 ++++++++ control-board/src/tasks/audio_task.rs | 48 ++++++++++++++++++++++++ control-board/src/tasks/imu_task.rs | 15 +++++++- control-board/src/tasks/mod.rs | 2 +- 7 files changed, 105 insertions(+), 6 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index f14beef9..505e2d23 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -102,6 +102,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { p); create_imu_task!(main_spawner, + robot_state, imu_gyro_data_publisher, imu_accel_data_publisher, p); diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index b7a8daf0..01797460 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -8,11 +8,14 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{create_imu_task, get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}}; +use ateam_control_board::{create_audio_task, create_imu_task, create_io_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}, robot_state::SharedRobotState}; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); @@ -32,6 +35,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { defmt::info!("embassy HAL configured."); + let robot_state = ROBOT_STATE.take(); + //////////////////////// // setup task pools // //////////////////////// @@ -50,7 +55,16 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_imu_task!(main_spawner, imu_gyro_data_publisher, imu_accel_data_publisher, p); + create_io_task!(main_spawner, robot_state, p); + + create_shutdown_task!(main_spawner, robot_state, p); + + create_audio_task!(main_spawner, robot_state, p); + + create_imu_task!(main_spawner, + robot_state, + imu_gyro_data_publisher, imu_accel_data_publisher, + p); loop { match select::select3(imu_gyro_data_subscriber.next_message(), imu_accel_data_subscriber.next_message(), Timer::after_millis(1000)).await { diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 631072b3..e194b055 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -70,7 +70,7 @@ impl SharedRobotState { battery_pct: 0, battery_ok: false, - robot_tipped: false, + robot_tipped: self.robot_tipped(), shutdown_requested: self.shutdown_requested(), } @@ -116,6 +116,14 @@ impl SharedRobotState { self.hw_debug_mode.store(in_debug_mode, Ordering::Relaxed); } + pub fn robot_tipped(&self) -> bool { + self.robot_tipped.load(Ordering::Relaxed) + } + + pub fn set_robot_tipped(&self, tipped: bool) { + self.robot_tipped.store(tipped, Ordering::Relaxed); + } + pub fn shutdown_requested(&self) -> bool { self.shutdown_requested.load(Ordering::Relaxed) } @@ -123,6 +131,7 @@ impl SharedRobotState { pub fn flag_shutdown_requested(&self) { self.shutdown_requested.store(true, Ordering::Relaxed); } + } #[derive(Clone, Copy, PartialEq, Debug)] diff --git a/control-board/src/songs.rs b/control-board/src/songs.rs index 03fa4f70..9c25ab16 100644 --- a/control-board/src/songs.rs +++ b/control-board/src/songs.rs @@ -1,5 +1,21 @@ use ateam_lib_stm32::audio::note::Beat; +pub const TIPPED_WARNING_SONG: [Beat; 14] = [ + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(125_000), +]; pub const TEST_SONG: [Beat; 17] = [ Beat::Note { tone: 392, duration: 250_000 }, diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index e69de29b..a0809f64 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -0,0 +1,48 @@ +use ateam_lib_stm32::{audio::tone_player::TonePlayer, drivers::audio::buzzer::Buzzer}; +use embassy_executor::Spawner; +use embassy_stm32::{gpio::OutputType, time::hz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}}; +use embassy_time::Timer; + +use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::TIPPED_WARNING_SONG}; + +#[macro_export] +macro_rules! create_audio_task { + ($main_spawner:ident, $robot_state:ident, $p:ident) => { + ateam_control_board::tasks::audio_task::start_audio_task( + &$main_spawner, $robot_state, $p.TIM15, $p.PE6 + ); + }; +} + +#[embassy_executor::task] +async fn audio_task_entry( + robot_state: &'static SharedRobotState, + mut tone_player: TonePlayer<'static, Buzzer<'static, BuzzerTimer>>, +) { + loop { + let cur_robot_state = robot_state.get_state(); + + if cur_robot_state.robot_tipped { + defmt::warn!("robot tipped audio"); + let _ = tone_player.load_song(&TIPPED_WARNING_SONG); + tone_player.play_song().await; + } + + Timer::after_millis(100).await; + } +} + +pub fn start_audio_task( + task_spawner: &Spawner, + robot_state: &'static SharedRobotState, + buzzer_timer: BuzzerTimer, + buzzer_pin: BuzzerPin, +) { + let ch2 = PwmPin::new_ch2(buzzer_pin, OutputType::PushPull); + let pwm = SimplePwm::new(buzzer_timer, None, Some(ch2), None, None, hz(1), Default::default()); + + let audio_driver = Buzzer::new(pwm, Channel::Ch2); + let tone_player = TonePlayer::new(audio_driver); + + task_spawner.spawn(audio_task_entry(robot_state, tone_player)).unwrap(); +} \ No newline at end of file diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 9837f4ab..a706c547 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -13,11 +13,13 @@ use static_cell::ConstStaticCell; use ateam_lib_stm32::drivers::imu::bmi323::{self, *}; use crate::pins::*; +use crate::robot_state::SharedRobotState; #[macro_export] macro_rules! create_imu_task { - ($main_spawner:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $p:ident) => { + ($main_spawner:ident, $robot_state:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $p:ident) => { ateam_control_board::tasks::imu_task::start_imu_task(&$main_spawner, + $robot_state, $imu_gyro_data_publisher, $imu_accel_data_publisher, $p.SPI1, $p.PA5, $p.PA7, $p.PA6, $p.DMA2_CH7, $p.DMA2_CH6, @@ -32,6 +34,7 @@ static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstSt #[embassy_executor::task] async fn imu_task_entry( + robot_state: &'static SharedRobotState, accel_pub: AccelDataPublisher, gyro_pub: GyroDataPublisher, mut imu: Bmi323<'static, 'static, ImuSpi>, @@ -94,6 +97,13 @@ async fn imu_task_entry( // TODO: don't use raw data, impl conversion let accel_data = imu.accel_get_raw_data().await; accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); + + // TODO: magic number, fix after raw data conversion + if accel_data[2] < 8000 { + robot_state.set_robot_tipped(true); + } else { + robot_state.set_robot_tipped(false); + } } Either::Second(_) => { defmt::warn!("imu interrupt based data acq timed out."); @@ -109,6 +119,7 @@ async fn imu_task_entry( pub fn start_imu_task( imu_task_spawner: &Spawner, + robot_state: &'static SharedRobotState, gyro_data_publisher: GyroDataPublisher, accel_data_publisher: AccelDataPublisher, peri: impl Peripheral

+ 'static, @@ -134,6 +145,6 @@ pub fn start_imu_task( let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::None); let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::None); - imu_task_spawner.spawn(imu_task_entry(accel_data_publisher, gyro_data_publisher, imu, accel_int, gyro_int)).unwrap(); + imu_task_spawner.spawn(imu_task_entry(robot_state, accel_data_publisher, gyro_data_publisher, imu, accel_int, gyro_int)).unwrap(); } diff --git a/control-board/src/tasks/mod.rs b/control-board/src/tasks/mod.rs index c5fe1757..867ecf4f 100644 --- a/control-board/src/tasks/mod.rs +++ b/control-board/src/tasks/mod.rs @@ -1,4 +1,4 @@ -// pub mod control; +pub mod audio_task; pub mod control_task; pub mod imu_task; pub mod kicker_task; From 8eadb87cdce240a8b5a522c3068416e7daaa4ddf Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 1 Jun 2024 22:39:49 -0400 Subject: [PATCH 062/157] shutdown task work --- control-board/src/tasks/shutdown_task.rs | 4 +++- control-board/src/tasks/user_io_task.rs | 12 ++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs index 23e1536e..faa30978 100644 --- a/control-board/src/tasks/shutdown_task.rs +++ b/control-board/src/tasks/shutdown_task.rs @@ -6,7 +6,7 @@ use embassy_time::Timer; use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::SharedRobotState}; -const HARD_SHUTDOWN_TIME_MS: u64 = 10000; +pub const HARD_SHUTDOWN_TIME_MS: u64 = 10000; #[macro_export] macro_rules! create_shutdown_task { @@ -35,6 +35,8 @@ async fn shutdown_task_entry(robot_state: &'static SharedRobotState, // wait for tasks to flag shutdown complete, or hard temrinate after 10s select::select(async move { loop { + + // TODO wait for other tasks Timer::after_millis(1000).await; } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 9456cbf7..c2667589 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -7,6 +7,7 @@ use embassy_stm32::spi::{Config, Spi}; use embassy_stm32::time::mhz; use embassy_time::{Duration, Timer}; +use smart_leds::colors::{BLACK, WHITE}; use smart_leds::RGB8; use static_cell::ConstStaticCell; @@ -19,6 +20,7 @@ use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; use crate::pins::*; +use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; // #[link_section = ".sram4"] // static DOTSTAR_SPI_BUFFER_CELL: ConstStaticCell<[u8; 16]> = ConstStaticCell::new([0; 16]); @@ -52,6 +54,10 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, ) { defmt::info!("user io task initialized"); + let shutdown_anim = anim::Animation::Lerp(Lerp::new(WHITE, BLACK, Duration::from_millis(HARD_SHUTDOWN_TIME_MS), AnimRepeatMode::None)); + let mut sd_anim_seq = [shutdown_anim]; + let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); + let anim0 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); let anim1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); let anim2 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); @@ -85,6 +91,7 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, // blink_anim.start_animation(); // dotstars.set_animation(blink_anim, 1); + let mut prev_robot_state = robot_state.get_state(); loop { let cur_robot_state = robot_state.get_state(); @@ -129,6 +136,11 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, debug_led0.set_low(); } + // if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { + // shutdown_anim.start_animation(); + // // dotstars_anim.set_animation(&shutdown_anim, 0); + // } + // let color = color_lerp.update(); // dotstars.set_color(color, 0); // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); From 348a4606627c954d68c441ccc50a6d3ab47907e1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 2 Jun 2024 18:03:38 -0400 Subject: [PATCH 063/157] control task almost ready --- control-board/src/tasks/control_task.rs | 245 +++++++++++++++--------- 1 file changed, 153 insertions(+), 92 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index b3bbae38..32cd9db1 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,10 +1,11 @@ +use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; use embassy_time::Timer; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -44,12 +45,20 @@ make_uart_queue_pair!(DRIB, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); +const TICKS_WITHOUT_PACKET_STOP: usize = 10; +const BATTERY_MIN_VOLTAGE: f32 = 18.0; + +const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); +const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm +const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot + #[embassy_executor::task] async fn control_task_entry( robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, + gyro_subscriber: GyroDataSubscriber, mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -126,99 +135,151 @@ async fn control_task_entry( Timer::after_millis(10).await; - // loop { - // motor_fl.process_packets(); - // motor_bl.process_packets(); - // motor_br.process_packets(); - // motor_fr.process_packets(); - // motor_drib.process_packets(); - - // motor_fl.log_reset("FL"); - // motor_bl.log_reset("BL"); - // motor_br.log_reset("BR"); - // motor_fr.log_reset("FR"); - // motor_drib.log_reset("DRIB"); - - // if motor_drib.ball_detected() { - // defmt::info!("ball detected"); - // } - - // if let Some(latest_control) = &latest_control { - // let cmd_vel = Vector3::new( - // latest_control.vel_x_linear, - // latest_control.vel_y_linear, - // latest_control.vel_z_angular, - // ); - // self.cmd_vel = cmd_vel; - // self.drib_vel = latest_control.dribbler_speed; - // self.ticks_since_packet = 0; - // } else { - // self.ticks_since_packet += 1; - // if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - // self.cmd_vel = Vector3::new(0., 0., 0.); - // self.ticks_since_packet = 0; - // } - // } - - // // now we have setpoint r(t) in self.cmd_vel - // let battery_v = self.battery_sub.next_message_pure().await as f32; - // let controls_enabled = true; - // let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0; - // let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - // if controls_enabled - // { - // // TODO check order - // let wheel_vels = Vector4::new( - // motor_fr.read_rads(), - // motor_fl.read_rads(), - // motor_bl.read_rads(), - // motor_br.read_rads() - // ); - - // // torque values are computed on the spin but put in the current variable - // // TODO update this when packet/var names are updated to match software - // let wheel_torques = Vector4::new( - // self.front_right_motor.read_current(), - // self.front_left_motor.read_current(), - // self.back_left_motor.read_current(), - // self.back_right_motor.read_current() - // ); - - // // TODO read from channel or something + let robot_model_constants: RobotConstants = RobotConstants { + wheel_angles_rad: Vector4::new( + WHEEL_ANGLES_DEG[0].to_radians(), + WHEEL_ANGLES_DEG[1].to_radians(), + WHEEL_ANGLES_DEG[2].to_radians(), + WHEEL_ANGLES_DEG[3].to_radians(), + ), + wheel_radius_m: Vector4::new( + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + ), + wheel_dist_to_cent_m: Vector4::new( + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + ), + }; + + let robot_model: RobotModel = RobotModel::new(robot_model_constants); + let robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + + let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); + let mut drib_vel = 0.0; + let mut ticks_since_packet = 0; + loop { + motor_fl.process_packets(); + motor_bl.process_packets(); + motor_br.process_packets(); + motor_fr.process_packets(); + motor_drib.process_packets(); + + motor_fl.log_reset("FL"); + motor_bl.log_reset("BL"); + motor_br.log_reset("BR"); + motor_fr.log_reset("FR"); + motor_drib.log_reset("DRIB"); + + if motor_drib.ball_detected() { + defmt::info!("ball detected"); + } - // self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + if let Some(latest_packet) = command_subscriber.try_next_message_pure() { + match latest_packet { + ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + let new_cmd_vel = Vector3::new( + latest_control.vel_x_linear, + latest_control.vel_y_linear, + latest_control.vel_z_angular, + ); + cmd_vel = new_cmd_vel; + drib_vel = latest_control.dribbler_speed; + ticks_since_packet = 0; + }, + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { + defmt::warn!("param updates aren't supported yet"); + }, + } + } else { + ticks_since_packet += 1; + if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + // ticks_since_packet = 0; + } + } + + // now we have setpoint r(t) in self.cmd_vel + let battery_v = battery_sub.next_message_pure().await as f32; + let controls_enabled = true; + let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; + let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + if controls_enabled + { + // TODO check order + let wheel_vels = Vector4::new( + motor_fr.read_rads(), + motor_fl.read_rads(), + motor_bl.read_rads(), + motor_br.read_rads() + ); + + // torque values are computed on the spin but put in the current variable + // TODO update this when packet/var names are updated to match software + let wheel_torques = Vector4::new( + motor_fr.read_current(), + motor_fl.read_current(), + motor_bl.read_current(), + motor_br.read_current() + ); - // self.robot_controller.get_wheel_velocities() - // } - // else - // { - // self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel) - // } - // } - // else - // { - // // Battery is too low, set velocity to zero - // Vector4::new( - // 0.0, - // 0.0, - // 0.0, - // 0.0) - // }; - - // self.front_right_motor.set_setpoint(wheel_vels[0]); - // self.front_left_motor.set_setpoint(wheel_vels[1]); - // self.back_left_motor.set_setpoint(wheel_vels[2]); - // self.back_right_motor.set_setpoint(wheel_vels[3]); - - // let drib_dc = -1.0 * self.drib_vel / 1000.0; - // self.drib_motor.set_setpoint(drib_dc); - - // self.front_right_motor.send_motion_command(); - // self.front_left_motor.send_motion_command(); - // self.back_left_motor.send_motion_command(); - // self.back_right_motor.send_motion_command(); - // self.drib_motor.send_motion_command(); - // } + // TODO read from channel or something + + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + robot_controller.get_wheel_velocities() + } else { + robot_model.robot_vel_to_wheel_vel(cmd_vel) + } + } else { + // Battery is too low, set velocity to zero + Vector4::new( + 0.0, + 0.0, + 0.0, + 0.0) + }; + + motor_fr.set_setpoint(wheel_vels[0]); + motor_fl.set_setpoint(wheel_vels[1]); + motor_bl.set_setpoint(wheel_vels[2]); + motor_br.set_setpoint(wheel_vels[3]); + + let drib_dc = -1.0 * drib_vel / 1000.0; + motor_drib.set_setpoint(drib_dc); + + motor_fr.send_motion_command(); + motor_fl.send_motion_command(); + motor_bl.send_motion_command(); + motor_br.send_motion_command(); + motor_drib.send_motion_command(); + + + let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + sequence_number: 0, + robot_revision_major: 0, + robot_revision_minor: 0, + battery_level: battery_v, + battery_temperature: 0., + _bitfield_align_1: [], + _bitfield_1: BasicTelemetry::new_bitfield_1( + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ), + motor_0_temperature: 0., + motor_1_temperature: 0., + motor_2_temperature: 0., + motor_3_temperature: 0., + motor_4_temperature: 0., + kicker_charge_level: 0., + }); + telemetry_publisher.publish_immediate(basic_telem); + + let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); + telemetry_publisher.publish_immediate(control_debug_telem); + } loop { motor_fl.process_packets(); From dc07ca30ce823bf1361bdf62da642ea73a8ffbaf Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 2 Jun 2024 18:14:00 -0400 Subject: [PATCH 064/157] compile fixes --- control-board/src/bin/control/main.rs | 4 +++- control-board/src/tasks/control_task.rs | 12 +++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 505e2d23..2a212c5b 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -72,6 +72,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + /////////////////// // start tasks // /////////////////// @@ -109,7 +111,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { start_control_task( uart_queue_spawner, main_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, + control_command_subscriber, control_telemetry_publisher, control_gyro_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 32cd9db1..d40b39a9 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -56,9 +56,9 @@ const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of whee #[embassy_executor::task] async fn control_task_entry( robot_state: &'static SharedRobotState, - command_subscriber: CommandsSubscriber, + mut command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, - gyro_subscriber: GyroDataSubscriber, + mut gyro_subscriber: GyroDataSubscriber, mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -157,7 +157,7 @@ async fn control_task_entry( }; let robot_model: RobotModel = RobotModel::new(robot_model_constants); - let robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); let mut drib_vel = 0.0; @@ -204,7 +204,8 @@ async fn control_task_entry( } // now we have setpoint r(t) in self.cmd_vel - let battery_v = battery_sub.next_message_pure().await as f32; + // let battery_v = battery_sub.next_message_pure().await as f32; + let battery_v = 0.0; let controls_enabled = true; let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { @@ -300,6 +301,7 @@ pub async fn start_control_task( robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, + gyro_subscriber: GyroDataSubscriber, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin, motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin, @@ -346,6 +348,6 @@ pub async fn start_control_task( let motor_drib = DribblerMotor::new_from_pins(&DRIB_RX_UART_QUEUE, &DRIB_TX_UART_QUEUE, motor_d_boot0_pin, motor_d_nrst_pin, DRIB_FW_IMG, 1.0); control_task_spawner.spawn(control_task_entry(robot_state, - command_subscriber, telemetry_publisher, + command_subscriber, telemetry_publisher, gyro_subscriber, motor_fl, motor_bl, motor_br, motor_fr, motor_drib)).unwrap(); } \ No newline at end of file From 3b5533fcc0e7d03ffd3792fef2a2d0e814789b13 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 2 Jun 2024 23:43:44 -0400 Subject: [PATCH 065/157] working drivetrain image --- control-board/src/bin/control/main.rs | 27 +++- control-board/src/drivers/radio_robot.rs | 161 ++++++++++++++++------- control-board/src/stspin_motor.rs | 6 +- control-board/src/tasks/control_task.rs | 17 ++- control-board/src/tasks/imu_task.rs | 12 +- control-board/src/tasks/radio_task.rs | 72 +++++++++- lib-stm32/src/drivers/radio/odin_w26x.rs | 43 +++++- lib-stm32/src/queue.rs | 4 + lib-stm32/src/uart/queue.rs | 8 +- 9 files changed, 273 insertions(+), 77 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 2a212c5b..eeeb48e1 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -9,7 +9,7 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; +use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; // load credentials from correct crate @@ -30,6 +30,7 @@ static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); +static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[interrupt] @@ -37,6 +38,11 @@ unsafe fn CEC() { UART_QUEUE_EXECUTOR.on_interrupt(); } +#[interrupt] +unsafe fn CORDIC() { + RADIO_UART_QUEUE_EXECUTOR.on_interrupt(); +} + #[embassy_executor::main] async fn main(main_spawner: embassy_executor::Spawner) { // init system @@ -51,9 +57,12 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup task pools // //////////////////////// + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CORDIC, embassy_stm32::interrupt::Priority::P6); + let radio_uart_queue_spawner = RADIO_UART_QUEUE_EXECUTOR.start(Interrupt::CORDIC); + // uart queue executor should be highest priority // NOTE: maybe this should be all DMA tasks? No computation tasks here - interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P6); + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); ////////////////////////////////////// @@ -93,16 +102,20 @@ async fn main(main_spawner: embassy_executor::Spawner) { // Timer::after_millis(1000).await; // } - create_radio_task!(main_spawner, uart_queue_spawner, + create_io_task!(main_spawner, robot_state, - radio_command_publisher, radio_telemetry_subscriber, - wifi_credentials, p); - - create_io_task!(main_spawner, + + create_shutdown_task!(main_spawner, robot_state, p); + create_radio_task!(main_spawner, radio_uart_queue_spawner, + robot_state, + radio_command_publisher, radio_telemetry_subscriber, + wifi_credentials, + p); + create_imu_task!(main_spawner, robot_state, imu_gyro_data_publisher, imu_accel_data_publisher, diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index ed7ac3ce..f4b78086 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -1,4 +1,4 @@ -use ateam_lib_stm32::drivers::radio::odin_w26x::{PeerConnection, Radio, WifiAuth}; +use ateam_lib_stm32::drivers::radio::odin_w26x::{PeerConnection, OdinW262, WifiAuth}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings_radio::{ self, BasicControl, CommandCode, HelloRequest, HelloResponse, RadioPacket, RadioPacket_Data, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, @@ -72,7 +72,7 @@ pub struct RobotRadio< const DEPTH_TX: usize, const DEPTH_RX: usize, > { - radio: Radio< + odin_driver: OdinW262< 'a, UART, DmaTx, @@ -103,10 +103,10 @@ impl< reset_pin: impl Pin, ) -> RobotRadio<'a, UART, DmaRx, DmaTx, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { let reset_pin = Output::new(reset_pin, Level::High, Speed::Medium); - let radio = Radio::new(read_queue, write_queue); + let radio = OdinW262::new(read_queue, write_queue); Self { - radio, + odin_driver: radio, reset_pin, peer: None, } @@ -134,7 +134,7 @@ impl< pub async fn connect_uart(&mut self) -> Result<(), RobotRadioError> { // were about to reset the radio, so we also need to reset the uart queue config to match the startup config - if self.radio.update_host_uart_config(self.get_startup_uart_config()).await.is_err() { + if self.odin_driver.update_host_uart_config(self.get_startup_uart_config()).await.is_err() { defmt::debug!("failed to reset host uart to startup config."); } @@ -144,19 +144,19 @@ impl< self.reset_pin.set_low(); // wait until startup event is received - if self.radio.wait_startup().await.is_err() { + if self.odin_driver.wait_startup().await.is_err() { defmt::debug!("error processing radio wait startup command"); return Err(RobotRadioError::ConnectUartBadStartup); } defmt::trace!("increasing link speed"); let baudrate = 5_250_000; - if self.radio.set_echo(false).await.is_err() { + if self.odin_driver.set_echo(false).await.is_err() { defmt::debug!("error disabling echo on radio"); return Err(RobotRadioError::ConnectUartBadEcho); } - if self.radio.config_uart(baudrate, false, 8, true).await.is_err() { + if self.odin_driver.config_uart(baudrate, false, 8, true).await.is_err() { defmt::debug!("error increasing radio baud rate."); return Err(RobotRadioError::ConnectUartBadRadioConfigUpdate); } @@ -168,7 +168,7 @@ impl< radio_uart_config.stop_bits = StopBits::STOP1; radio_uart_config.data_bits = DataBits::DataBits9; radio_uart_config.parity = usart::Parity::ParityEven; - if self.radio.update_host_uart_config(radio_uart_config).await.is_err() { + if self.odin_driver.update_host_uart_config(radio_uart_config).await.is_err() { defmt::debug!("error increasing host baud rate."); return Err(RobotRadioError::ConnectUartBadHostConfigUpdate); } @@ -179,11 +179,11 @@ impl< Timer::after(Duration::from_millis(50)).await; // Datasheet says wait at least 50ms after entering data mode - if let Ok(got_edm_startup) = self.radio.enter_edm().await { + if let Ok(got_edm_startup) = self.odin_driver.enter_edm().await { defmt::trace!("entered edm at high link speed"); if ! got_edm_startup { - if self.radio.wait_edm_startup().await.is_err() { + if self.odin_driver.wait_edm_startup().await.is_err() { defmt::debug!("error waiting for EDM startup after uart baudrate increase"); return Err(RobotRadioError::ConnectUartNoEdmStartup); } @@ -204,7 +204,7 @@ impl< let mut had_error = false; if let Some(peer) = self.peer.take() { defmt::debug!("closing peer.."); - if self.radio.close_peer(peer.peer_id).await.is_err() { + if self.odin_driver.close_peer(peer.peer_id).await.is_err() { defmt::warn!("failed to close peer on network dc"); had_error = true; } else { @@ -213,7 +213,7 @@ impl< } defmt::debug!("closing wifi."); - if self.radio.disconnect_wifi(1).await.is_err() { + if self.odin_driver.disconnect_wifi(1).await.is_err() { defmt::warn!("failed to disconnect network."); had_error = true; } else { @@ -236,7 +236,7 @@ impl< core::write!(&mut s, "A-Team Robot #{:02X} ({:04X})", robot_number, uid_u16).unwrap(); // let mut s = String::<16>::new(); // core::write!(&mut s, "A-Team Robot {:02X}", robot_number).unwrap(); - if self.radio.set_host_name(s.as_str()).await.is_err() { + if self.odin_driver.set_host_name(s.as_str()).await.is_err() { defmt::trace!("could not set radio host name"); return Err(RobotRadioError::ConnectWifiBadHostName); } @@ -246,13 +246,13 @@ impl< let wifi_pass = WifiAuth::WPA { passphrase: wifi_credential.get_password(), }; - if self.radio.config_wifi(1, wifi_ssid, wifi_pass).await.is_err() { + if self.odin_driver.config_wifi(1, wifi_ssid, wifi_pass).await.is_err() { defmt::trace!("could not configure wifi profile"); return Err(RobotRadioError::ConnectWifiBadConfig); } // connect to config slot 1 - if self.radio.connect_wifi(1).await.is_err() { + if self.odin_driver.connect_wifi(1).await.is_err() { defmt::trace!("could not connect to wifi"); // can never configure a profile that "active" even when unconnected @@ -268,7 +268,7 @@ impl< } pub async fn open_multicast(&mut self) -> Result<(), RobotRadioError> { - let peer = self.radio.connect_peer(formatcp!( + let peer = self.odin_driver.connect_peer(formatcp!( "udp://{MULTICAST_IP}:{MULTICAST_PORT}/?flags=1&local_port={LOCAL_PORT}" )) .await; @@ -294,14 +294,15 @@ impl< port ) .unwrap(); - let peer = self.radio.connect_peer(s.as_str()).await?; + + let peer = self.odin_driver.connect_peer(s.as_str()).await?; self.peer = Some(peer); Ok(()) } pub async fn close_peer(&mut self) -> Result<(), ()> { if let Some(peer) = &self.peer { - self.radio.close_peer(peer.peer_id).await?; + self.odin_driver.close_peer(peer.peer_id).await?; self.peer = None; Ok(()) } else { @@ -311,7 +312,7 @@ impl< pub async fn send_data(&self, data: &[u8]) -> Result<(), ()> { if let Some(peer) = &self.peer { - self.radio.send_data(peer.channel_id, data).await + self.odin_driver.send_data(peer.channel_id, data).await } else { Err(()) } @@ -322,7 +323,29 @@ impl< FN: FnOnce(&[u8]) -> RET, { if self.peer.is_some() { - self.radio.read_data(fn_read).await + self.odin_driver.read_data(fn_read).await + } else { + Err(()) + } + } + + fn read_data_nonblocking(&'a self, fn_read: FN) -> Result, ()> + where + FN: FnOnce(&[u8]) -> RET, + { + if self.peer.is_some() { + if self.odin_driver.can_read_data() { + match self.odin_driver.try_read_data(fn_read) { + Ok(ret) => { + Ok(Some(ret)) + }, + Err(_) => { + Err(()) + }, + } + } else { + Ok(None) + } } else { Err(()) } @@ -496,39 +519,75 @@ impl< } } - pub async fn read_packet(&self) -> Result { - self.read_data(|data| { - const CONTROL_PACKET_SIZE: usize = size_of::() - size_of::() - + size_of::(); - const PARAMERTER_PACKET_SIZE: usize = size_of::() - size_of::() - + size_of::(); - - if data.len() == CONTROL_PACKET_SIZE { - let mut data_copy = [0u8; size_of::()]; - data_copy[0..CONTROL_PACKET_SIZE].clone_from_slice(&data[0..CONTROL_PACKET_SIZE]); - - let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; - - if packet.command_code != CommandCode::CC_CONTROL { - return Err(()); - } - - Ok(unsafe { DataPacket::BasicControl(packet.data.control) }) - } else if data.len() == PARAMERTER_PACKET_SIZE { - let mut data_copy = [0u8; size_of::()]; - data_copy[0..PARAMERTER_PACKET_SIZE].clone_from_slice(&data[0..PARAMERTER_PACKET_SIZE]); - - let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; - - if packet.command_code != CommandCode::CC_ROBOT_PARAMETER_COMMAND { - return Err(()); - } - - Ok(unsafe { DataPacket::ParameterCommand(packet.data.robot_parameter_command) }) - } else { + pub fn parse_data_packet(&self, data: &[u8]) -> Result { + const CONTROL_PACKET_SIZE: usize = size_of::() - size_of::() + + size_of::(); + const PARAMERTER_PACKET_SIZE: usize = size_of::() - size_of::() + + size_of::(); + + if data.len() == CONTROL_PACKET_SIZE { + let mut data_copy = [0u8; size_of::()]; + data_copy[0..CONTROL_PACKET_SIZE].clone_from_slice(&data[0..CONTROL_PACKET_SIZE]); + + let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; + + if packet.command_code != CommandCode::CC_CONTROL { return Err(()); } + + Ok(unsafe { DataPacket::BasicControl(packet.data.control) }) + } else if data.len() == PARAMERTER_PACKET_SIZE { + let mut data_copy = [0u8; size_of::()]; + data_copy[0..PARAMERTER_PACKET_SIZE].clone_from_slice(&data[0..PARAMERTER_PACKET_SIZE]); + + let packet = unsafe { &*(&data_copy as *const _ as *const RadioPacket) }; + + if packet.command_code != CommandCode::CC_ROBOT_PARAMETER_COMMAND { + return Err(()); + } + + Ok(unsafe { DataPacket::ParameterCommand(packet.data.robot_parameter_command) }) + } else { + return Err(()); + } + } + + pub async fn read_packet(&self) -> Result { + self.read_data(|data| { + self.parse_data_packet(data) }) .await? } + + pub fn read_packet_nonblocking(&self) -> Result, ()> { + let res = self.read_data_nonblocking(|data| { + self.parse_data_packet(data) + }); + + match res { + Ok(res) => { + match res { + Some(pkt) => { + match pkt { + Ok(pkt) => { + return Ok(Some(pkt)); + }, + Err(_) => { + // we got data that was a valid EDM DataPacket, but couldn't parse it + // into any known A-Team packet format + return Err(()); + }, + } + }, + None => { + return Ok(None) + }, + } + }, + Err(_) => { + // read_data_nonblocking failed because the radio was in an invalid state + return Err(()) + }, + } + } } diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 5df713e6..c3a04eeb 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -524,9 +524,9 @@ impl< if mrp.type_ == MRP_MOTION { self.current_state = mrp.data.motion; - if mrp.data.motion.master_error() != 0 { - error!("drib error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - } + // if mrp.data.motion.master_error() != 0 { + // error!("drib error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + // } // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); // // info!("\n"); diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index d40b39a9..8221dde8 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -2,7 +2,7 @@ use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacke use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; -use embassy_time::Timer; +use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; use crate::{include_external_cpp_bin, motion::{robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; @@ -159,6 +159,8 @@ async fn control_task_entry( let robot_model: RobotModel = RobotModel::new(robot_model_constants); let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); + let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); let mut drib_vel = 0.0; let mut ticks_since_packet = 0; @@ -182,11 +184,15 @@ async fn control_task_entry( if let Some(latest_packet) = command_subscriber.try_next_message_pure() { match latest_packet { ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + let new_cmd_vel = Vector3::new( latest_control.vel_x_linear, latest_control.vel_y_linear, latest_control.vel_z_angular, ); + + defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); + cmd_vel = new_cmd_vel; drib_vel = latest_control.dribbler_speed; ticks_since_packet = 0; @@ -203,10 +209,13 @@ async fn control_task_entry( } } + defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); + + // now we have setpoint r(t) in self.cmd_vel // let battery_v = battery_sub.next_message_pure().await as f32; - let battery_v = 0.0; - let controls_enabled = true; + let battery_v = 25.0; + let controls_enabled = false; let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { if controls_enabled @@ -280,6 +289,8 @@ async fn control_task_entry( let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); telemetry_publisher.publish_immediate(control_debug_telem); + + loop_rate_ticker.next().await; } loop { diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index a706c547..f3962a7d 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -29,8 +29,12 @@ macro_rules! create_imu_task { }; } +// #[link_section = ".axisram.buffers"] +// static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstStaticCell::new([0; bmi323::SPI_MIN_BUF_LEN]); + #[link_section = ".axisram.buffers"] -static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstStaticCell::new([0; bmi323::SPI_MIN_BUF_LEN]); +static mut IMU_BUFFER_CELL: [u8; bmi323::SPI_MIN_BUF_LEN] = [0; bmi323::SPI_MIN_BUF_LEN]; + #[embassy_executor::task] async fn imu_task_entry( @@ -136,7 +140,11 @@ pub fn start_imu_task( accel_int: impl Peripheral

::ExtiChannel> + 'static, gyro_int: impl Peripheral

::ExtiChannel> + 'static, _ext_imu_det_pin: ExtImuNDetPin) { - let imu_buf = IMU_BUFFER_CELL.take(); + defmt::debug!("starting imu task..."); + + // let imu_buf = IMU_BUFFER_CELL.take(); + + let imu_buf: &'static mut [u8; 14] = unsafe { &mut IMU_BUFFER_CELL }; let imu = Bmi323::new_from_pins(peri, sck, mosi, miso, txdma, rxdma, bmi323_nss, imu_buf); diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 1e862f2e..63d4250a 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,5 +1,5 @@ -use ateam_common_packets::radio::TelemetryPacket; +use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; @@ -9,7 +9,7 @@ use embassy_stm32::{ usart::{self, DataBits, Parity, StopBits, Uart} }; use embassy_sync::pubsub::WaitResult; -use embassy_time::{Duration, Timer}; +use embassy_time::{Duration, Ticker, Timer}; use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::SharedRobotState, SystemIrqs}; @@ -29,6 +29,8 @@ macro_rules! create_radio_task { }; } +pub const RADIO_LOOP_RATE_MS: u64 = 10; + pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; pub const RADIO_TX_BUF_DEPTH: usize = 4; pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; @@ -75,6 +77,8 @@ pub struct RadioTask< connection_state: RadioConnectionState, wifi_credentials: &'static [WifiCredential], + + last_basic_telemetry: BasicTelemetry, } impl< @@ -105,7 +109,23 @@ impl< radio_ndet_input: radio_ndet_input, connection_state: RadioConnectionState::Unconnected, wifi_credentials: wifi_credentials, - // task: TaskStorage::new(), + last_basic_telemetry: BasicTelemetry { + sequence_number: 0, + robot_revision_major: 0, + robot_revision_minor: 0, + battery_level: 0., + battery_temperature: 0., + _bitfield_align_1: [], + _bitfield_1: BasicTelemetry::new_bitfield_1( + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ), + motor_0_temperature: 0., + motor_1_temperature: 0., + motor_2_temperature: 0., + motor_3_temperature: 0., + motor_4_temperature: 0., + kicker_charge_level: 0., + }, } } @@ -128,6 +148,8 @@ impl< async fn radio_task_entry(&mut self) { defmt::info!("radio task startup"); + let mut radio_loop_rate_ticker = Ticker::every(Duration::from_millis(RADIO_LOOP_RATE_MS)); + // initialize a copy of the robot state so we can track updates let mut last_robot_state = self.shared_robot_state.get_state(); @@ -229,10 +251,12 @@ impl< } }, RadioConnectionState::Connected => { - let _ = self.process_packet().await; - // no delay to imediately process the next one + let _ = self.process_packets2().await; + // if we're stably connected, process packets at 100Hz }, } + + radio_loop_rate_ticker.next().await; } } @@ -307,7 +331,43 @@ impl< } } - async fn process_packet(&mut self) -> Result<(), ()> { + async fn process_packets2(&mut self) -> Result<(), ()> { + // read any packets + if let Ok(pkt) = self.radio.read_packet_nonblocking() { + if let Some(c2_pkt) = pkt { + self.command_publisher.publish_immediate(c2_pkt); + } + } else { + defmt::warn!("RadioTask - error reading data packet"); + } + + if let Some(telemetry) = self.telemetry_subscriber.try_next_message_pure() { + match telemetry { + TelemetryPacket::Basic(basic) => { + self.last_basic_telemetry = basic; + }, + TelemetryPacket::Control(control) => { + if self.radio.send_control_debug_telemetry(control).await.is_err() { + defmt::warn!("RadioTask - failed to send control debug telemetry packet"); + } + }, + TelemetryPacket::ParameterCommandResponse(pc_resp) => { + if self.radio.send_parameter_response(pc_resp).await.is_err() { + defmt::warn!("RadioTask - failed to send control parameter response packet"); + } + }, + } + } + + // always send the latest telemetry + if self.radio.send_telemetry(self.last_basic_telemetry).await.is_err() { + defmt::warn!("RadioTask - failed to send basic telem packet"); + } + + return Ok(()) + } + + async fn process_packets(&mut self) -> Result<(), ()> { match select(self.radio.read_packet(), self.telemetry_subscriber.next_message()).await { Either::First(res) => { if let Ok(c2_pkt) = res { diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index ba193e5b..51d252bd 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -4,6 +4,7 @@ use embassy_stm32::usart; use embassy_time::Timer; use heapless::String; +use crate::queue; use crate::uart::queue::{UartReadQueue, UartWriteQueue}; use super::at_protocol::{ATEvent, ATResponse, WifiLinkDisconnectedReason}; @@ -43,7 +44,7 @@ pub struct PeerConnection { pub channel_id: u8, } -pub struct Radio< +pub struct OdinW262< 'a, UART: usart::BasicInstance, TxDma: usart::TxDma, @@ -67,7 +68,7 @@ impl< const LEN_RX: usize, const DEPTH_TX: usize, const DEPTH_RX: usize, - > Radio<'a, UART, TxDma, RxDma, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> + > OdinW262<'a, UART, TxDma, RxDma, LEN_TX, LEN_RX, DEPTH_TX, DEPTH_RX> { pub fn new( reader: &'a UartReadQueue, @@ -330,9 +331,10 @@ impl< while peer_id.is_none() || !peer_connected_ip || channel_ret.is_none() { self.reader .dequeue(|buf| { + // defmt::trace!("connect peer buf {}", buf); let pkt = self.to_packet(buf); if pkt.is_err() { - // defmt::error!("got undecodable pkt {}", buf); + defmt::error!("got undecodable pkt {}", buf); } match pkt? { @@ -451,6 +453,41 @@ impl< }).await } + pub fn can_read_data(&'a self) -> bool { + self.reader.can_dequque() + } + + pub fn try_read_data(&'a self, fn_read: FN) -> Result + where FN: FnOnce(&[u8]) -> RET, + { + match self.reader.try_dequeue() { + Ok(buf) => { + match self.to_packet(buf.data()) { + Ok(pkt) => { + if let EdmPacket::DataEvent { channel: _ , data } = pkt { + return Ok(fn_read(data)) + } else { + return Err(()); + } + }, + Err(_) => { + return Err(()); + }, + } + // we read something + + }, + Err(queue::Error::QueueFullEmpty) => { + // nothing to read + return Err(()); + } + Err(queue::Error::InProgress) => { + // you did something illegal + return Err(()); + }, + } + } + pub async fn send_command(&self, cmd: &str) -> Result<(), ()> { match self.mode { RadioMode::CommandMode => { diff --git a/lib-stm32/src/queue.rs b/lib-stm32/src/queue.rs index 04d44bc5..e1f151bf 100644 --- a/lib-stm32/src/queue.rs +++ b/lib-stm32/src/queue.rs @@ -108,6 +108,10 @@ impl<'a, const LENGTH: usize, const DEPTH: usize> Queue { } } + pub fn can_dequeue(&self) -> bool { + !self.read_in_progress.load(Ordering::Relaxed) && self.size.load(Ordering::Relaxed) > 0 + } + pub fn try_dequeue(&self) -> Result, Error> { critical_section::with(|_| { if self.read_in_progress.load(Ordering::SeqCst) { diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 6f5b965b..b4eda235 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -201,14 +201,14 @@ impl< Either::First(len) => { if let Ok(len) = len { if len == 0 { - defmt::debug!("uart zero"); + // defmt::debug!("uart zero"); buf.cancel(); } else { *buf.len() = len; } } else { // Framing and Parity Error occur here - defmt::warn!("{}", len); + // defmt::warn!("{}", len); buf.cancel(); } }, @@ -251,6 +251,10 @@ impl< self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_pubsub.subscriber().unwrap())) } + pub fn can_dequque(&self) -> bool { + self.queue_rx.can_dequeue() + } + pub fn try_dequeue(&self) -> Result, Error> { return self.queue_rx.try_dequeue(); } From be415bf345cdbbd97cb9da3c3a86c4a64d66a9b1 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 9 Jun 2024 21:30:27 -0400 Subject: [PATCH 066/157] WIP refactoring of control task --- control-board/src/tasks/control_task.rs | 759 ++++++++++++++++-------- 1 file changed, 525 insertions(+), 234 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 8221dde8..67239809 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -5,7 +5,7 @@ use embassy_stm32::usart::Uart; use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{self, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -52,259 +52,550 @@ const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -#[embassy_executor::task] -async fn control_task_entry( - robot_state: &'static SharedRobotState, - mut command_subscriber: CommandsSubscriber, +pub struct ControlTask< + const MAX_RX_PACKET_SIZE: usize, + const MAX_TX_PACKET_SIZE: usize, + const RX_BUF_DEPTH: usize, + const TX_BUF_DEPTH: usize> { + shared_robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + gyro_subscriber: GyroDataSubscriber, telemetry_publisher: TelemetryPublisher, - mut gyro_subscriber: GyroDataSubscriber, - mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, - mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, - mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, - mut motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, - mut motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> -) { - defmt::info!("control task init."); - - // wait for the switch state to be read - while !robot_state.hw_init_state_valid() { - Timer::after_millis(10).await; - } - - if robot_state.hw_in_debug_mode() { - defmt::info!("flashing firmware"); + + motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> +} - if motor_fl.load_default_firmware_image().await.is_err() { - defmt::error!("failed to flash FL"); - } else { - defmt::info!("FL flashed"); +impl < + const MAX_RX_PACKET_SIZE: usize, + const MAX_TX_PACKET_SIZE: usize, + const RX_BUF_DEPTH: usize, + const TX_BUF_DEPTH: usize> + ControlTask + { + + pub fn new(robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + telemetry_publisher: TelemetryPublisher, + gyro_subscriber: GyroDataSubscriber, + motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, + motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> + ) -> Self { + ControlTask { + shared_robot_state: robot_state, + command_subscriber: command_subscriber, + telemetry_publisher: telemetry_publisher, + gyro_subscriber: gyro_subscriber, + motor_fl: motor_fl, + motor_bl: motor_bl, + motor_br: motor_br, + motor_fr: motor_fr, + motor_drib: motor_drib + } } + + async fn control_task_entry(&mut self) { + defmt::info!("control task init."); - if motor_bl.load_default_firmware_image().await.is_err() { - defmt::error!("failed to flash BL"); - } else { - defmt::info!("BL flashed"); - } + // wait for the switch state to be read + while !self.shared_robot_state.hw_init_state_valid() { + Timer::after_millis(10).await; + } + + self.flash_motor_firmware( + self.shared_robot_state.hw_in_debug_mode()); + + + embassy_futures::join::join5( + self.motor_fl.leave_reset(), + self.motor_bl.leave_reset(), + self.motor_br.leave_reset(), + self.motor_fr.leave_reset(), + self.motor_drib.leave_reset(), + ) + .await; + + + self.motor_fl.set_telemetry_enabled(true); + self.motor_bl.set_telemetry_enabled(true); + self.motor_br.set_telemetry_enabled(true); + self.motor_fr.set_telemetry_enabled(true); + self.motor_drib.set_telemetry_enabled(true); + + Timer::after_millis(10).await; + + let robot_model = self.get_robot_model(); + let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + + let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - if motor_br.load_default_firmware_image().await.is_err() { - defmt::error!("failed to flash BR"); - } else { - defmt::info!("BR flashed"); - } + let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); + let mut drib_vel = 0.0; + let mut ticks_since_packet = 0; - if motor_fr.load_default_firmware_image().await.is_err() { - defmt::error!("failed to flash FR"); - } else { - defmt::info!("FR flashed"); - } - if motor_drib.load_default_firmware_image().await.is_err() { - defmt::error!("failed to flash DRIB"); - } else { - defmt::info!("DRIB flashed"); - } - } else { - let _res = embassy_futures::join::join5( - motor_fl.load_default_firmware_image(), - motor_bl.load_default_firmware_image(), - motor_br.load_default_firmware_image(), - motor_fr.load_default_firmware_image(), - motor_drib.load_default_firmware_image(), - ) - .await; - - defmt::debug!("motor firmware flashed"); - } + loop { + self.motor_fl.process_packets(); + self.motor_bl.process_packets(); + self.motor_br.process_packets(); + self.motor_fr.process_packets(); + self.motor_drib.process_packets(); + self.motor_fl.log_reset("FL"); + self.motor_bl.log_reset("BL"); + self.motor_br.log_reset("BR"); + self.motor_fr.log_reset("FR"); + self.motor_drib.log_reset("DRIB"); - embassy_futures::join::join5( - motor_fl.leave_reset(), - motor_bl.leave_reset(), - motor_br.leave_reset(), - motor_fr.leave_reset(), - motor_drib.leave_reset(), - ) - .await; - - motor_fl.set_telemetry_enabled(true); - motor_bl.set_telemetry_enabled(true); - motor_br.set_telemetry_enabled(true); - motor_fr.set_telemetry_enabled(true); - motor_drib.set_telemetry_enabled(true); - - Timer::after_millis(10).await; - - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); - - let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); - let mut drib_vel = 0.0; - let mut ticks_since_packet = 0; - loop { - motor_fl.process_packets(); - motor_bl.process_packets(); - motor_br.process_packets(); - motor_fr.process_packets(); - motor_drib.process_packets(); - - motor_fl.log_reset("FL"); - motor_bl.log_reset("BL"); - motor_br.log_reset("BR"); - motor_fr.log_reset("FR"); - motor_drib.log_reset("DRIB"); - - if motor_drib.ball_detected() { - defmt::info!("ball detected"); - } + if self.motor_drib.ball_detected() { + defmt::info!("ball detected"); + } - if let Some(latest_packet) = command_subscriber.try_next_message_pure() { - match latest_packet { - ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + if let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { + match latest_packet { + ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + + let new_cmd_vel = Vector3::new( + latest_control.vel_x_linear, + latest_control.vel_y_linear, + latest_control.vel_z_angular, + ); + + defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); + + cmd_vel = new_cmd_vel; + drib_vel = latest_control.dribbler_speed; + ticks_since_packet = 0; + }, + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { + defmt::warn!("param updates aren't supported yet"); + }, + } + } else { + ticks_since_packet += 1; + if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + // ticks_since_packet = 0; + } + } - let new_cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, + defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); + + + // now we have setpoint r(t) in self.cmd_vel + // let battery_v = battery_sub.next_message_pure().await as f32; + let battery_v = 25.0; + let controls_enabled = false; + let gyro_rads = (self.gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; + let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + if controls_enabled + { + // TODO check order + let wheel_vels = Vector4::new( + self.motor_fr.read_rads(), + self.motor_fl.read_rads(), + self.motor_bl.read_rads(), + self.motor_br.read_rads() ); - defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); - - cmd_vel = new_cmd_vel; - drib_vel = latest_control.dribbler_speed; - ticks_since_packet = 0; - }, - ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { - defmt::warn!("param updates aren't supported yet"); - }, - } - } else { - ticks_since_packet += 1; - if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - cmd_vel = Vector3::new(0., 0., 0.); - // ticks_since_packet = 0; + // torque values are computed on the spin but put in the current variable + // TODO update this when packet/var names are updated to match software + let wheel_torques = Vector4::new( + self.motor_fr.read_current(), + self.motor_fl.read_current(), + self.motor_bl.read_current(), + self.motor_br.read_current() + ); + + // TODO read from channel or something + + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + robot_controller.get_wheel_velocities() + } else { + robot_model.robot_vel_to_wheel_vel(cmd_vel) + } + } else { + // Battery is too low, set velocity to zero + Vector4::new( + 0.0, + 0.0, + 0.0, + 0.0) + }; + + self.motor_fr.set_setpoint(wheel_vels[0]); + self.motor_fl.set_setpoint(wheel_vels[1]); + self.motor_bl.set_setpoint(wheel_vels[2]); + self.motor_br.set_setpoint(wheel_vels[3]); + + let drib_dc = -1.0 * drib_vel / 1000.0; + self.motor_drib.set_setpoint(drib_dc); + + self.motor_fr.send_motion_command(); + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_drib.send_motion_command(); + + + let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + sequence_number: 0, + robot_revision_major: 0, + robot_revision_minor: 0, + battery_level: battery_v, + battery_temperature: 0., + _bitfield_align_1: [], + _bitfield_1: BasicTelemetry::new_bitfield_1( + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ), + motor_0_temperature: 0., + motor_1_temperature: 0., + motor_2_temperature: 0., + motor_3_temperature: 0., + motor_4_temperature: 0., + kicker_charge_level: 0., + }); + self.telemetry_publisher.publish_immediate(basic_telem); + + let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); + self.telemetry_publisher.publish_immediate(control_debug_telem); + + loop_rate_ticker.next().await; } } - defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); - - - // now we have setpoint r(t) in self.cmd_vel - // let battery_v = battery_sub.next_message_pure().await as f32; - let battery_v = 25.0; - let controls_enabled = false; - let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; - let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - if controls_enabled - { - // TODO check order - let wheel_vels = Vector4::new( - motor_fr.read_rads(), - motor_fl.read_rads(), - motor_bl.read_rads(), - motor_br.read_rads() - ); - - // torque values are computed on the spin but put in the current variable - // TODO update this when packet/var names are updated to match software - let wheel_torques = Vector4::new( - motor_fr.read_current(), - motor_fl.read_current(), - motor_bl.read_current(), - motor_br.read_current() - ); - - // TODO read from channel or something - - robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); - robot_controller.get_wheel_velocities() + async fn flash_motor_firmware(&mut self, debug: bool) { + defmt::info!("flashing firmware"); + if debug { + if self.motor_fl.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash FL"); + } else { + defmt::info!("FL flashed"); + } + + if self.motor_bl.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash BL"); + } else { + defmt::info!("BL flashed"); + } + + if self.motor_br.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash BR"); + } else { + defmt::info!("BR flashed"); + } + + if self.motor_fr.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash FR"); + } else { + defmt::info!("FR flashed"); + } + + if self.motor_drib.load_default_firmware_image().await.is_err() { + defmt::error!("failed to flash DRIB"); + } else { + defmt::info!("DRIB flashed"); + } } else { - robot_model.robot_vel_to_wheel_vel(cmd_vel) + let _res = embassy_futures::join::join5( + self.motor_fl.load_default_firmware_image(), + self.motor_bl.load_default_firmware_image(), + self.motor_br.load_default_firmware_image(), + self.motor_fr.load_default_firmware_image(), + self.motor_drib.load_default_firmware_image(), + ) + .await; + + defmt::debug!("motor firmware flashed"); } - } else { - // Battery is too low, set velocity to zero - Vector4::new( - 0.0, - 0.0, - 0.0, - 0.0) - }; - - motor_fr.set_setpoint(wheel_vels[0]); - motor_fl.set_setpoint(wheel_vels[1]); - motor_bl.set_setpoint(wheel_vels[2]); - motor_br.set_setpoint(wheel_vels[3]); - - let drib_dc = -1.0 * drib_vel / 1000.0; - motor_drib.set_setpoint(drib_dc); - - motor_fr.send_motion_command(); - motor_fl.send_motion_command(); - motor_bl.send_motion_command(); - motor_br.send_motion_command(); - motor_drib.send_motion_command(); - - - let basic_telem = TelemetryPacket::Basic(BasicTelemetry { - sequence_number: 0, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: battery_v, - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }); - telemetry_publisher.publish_immediate(basic_telem); - - let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); - telemetry_publisher.publish_immediate(control_debug_telem); - - loop_rate_ticker.next().await; - } - - loop { - motor_fl.process_packets(); - - let rads = motor_fl.read_rads(); - defmt::info!("read motor rads {}", rads); - motor_fl.set_setpoint(3.1415 * 10.0); - - motor_fl.send_motion_command(); + } - Timer::after_millis(10).await; + fn get_robot_model(&mut self) -> motion::robot_model::RobotModel{ + let robot_model_constants: RobotConstants = RobotConstants { + wheel_angles_rad: Vector4::new( + WHEEL_ANGLES_DEG[0].to_radians(), + WHEEL_ANGLES_DEG[1].to_radians(), + WHEEL_ANGLES_DEG[2].to_radians(), + WHEEL_ANGLES_DEG[3].to_radians(), + ), + wheel_radius_m: Vector4::new( + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + ), + wheel_dist_to_cent_m: Vector4::new( + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + ), + }; + + let robot_model: RobotModel = RobotModel::new(robot_model_constants); + + return robot_model; + } } -} + +// #[embassy_executor::task] +// async fn control_task_entry( +// robot_state: &'static SharedRobotState, +// mut command_subscriber: CommandsSubscriber, +// telemetry_publisher: TelemetryPublisher, +// mut gyro_subscriber: GyroDataSubscriber, +// mut motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, +// mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, +// mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, +// mut motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, +// mut motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> +// ) { + + // wait for the switch state to be read + // while !robot_state.hw_init_state_valid() { + // Timer::after_millis(10).await; + // } + + // if robot_state.hw_in_debug_mode() { + // defmt::info!("flashing firmware"); + + // if motor_fl.load_default_firmware_image().await.is_err() { + // defmt::error!("failed to flash FL"); + // } else { + // defmt::info!("FL flashed"); + // } + + // if motor_bl.load_default_firmware_image().await.is_err() { + // defmt::error!("failed to flash BL"); + // } else { + // defmt::info!("BL flashed"); + // } + + // if motor_br.load_default_firmware_image().await.is_err() { + // defmt::error!("failed to flash BR"); + // } else { + // defmt::info!("BR flashed"); + // } + + // if motor_fr.load_default_firmware_image().await.is_err() { + // defmt::error!("failed to flash FR"); + // } else { + // defmt::info!("FR flashed"); + // } + + // if motor_drib.load_default_firmware_image().await.is_err() { + // defmt::error!("failed to flash DRIB"); + // } else { + // defmt::info!("DRIB flashed"); + // } + // } else { + // let _res = embassy_futures::join::join5( + // motor_fl.load_default_firmware_image(), + // motor_bl.load_default_firmware_image(), + // motor_br.load_default_firmware_image(), + // motor_fr.load_default_firmware_image(), + // motor_drib.load_default_firmware_image(), + // ) + // .await; + + // defmt::debug!("motor firmware flashed"); + // } + + + // embassy_futures::join::join5( + // motor_fl.leave_reset(), + // motor_bl.leave_reset(), + // motor_br.leave_reset(), + // motor_fr.leave_reset(), + // motor_drib.leave_reset(), + // ) + // .await; + + // motor_fl.set_telemetry_enabled(true); + // motor_bl.set_telemetry_enabled(true); + // motor_br.set_telemetry_enabled(true); + // motor_fr.set_telemetry_enabled(true); + // motor_drib.set_telemetry_enabled(true); + + // Timer::after_millis(10).await; + + // let robot_model_constants: RobotConstants = RobotConstants { + // wheel_angles_rad: Vector4::new( + // WHEEL_ANGLES_DEG[0].to_radians(), + // WHEEL_ANGLES_DEG[1].to_radians(), + // WHEEL_ANGLES_DEG[2].to_radians(), + // WHEEL_ANGLES_DEG[3].to_radians(), + // ), + // wheel_radius_m: Vector4::new( + // WHEEL_RADIUS_M, + // WHEEL_RADIUS_M, + // WHEEL_RADIUS_M, + // WHEEL_RADIUS_M, + // ), + // wheel_dist_to_cent_m: Vector4::new( + // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + // ), + // }; + + // let robot_model: RobotModel = RobotModel::new(robot_model_constants); + // let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + + // let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); + + // let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); + // let mut drib_vel = 0.0; + // let mut ticks_since_packet = 0; + // loop { + // motor_fl.process_packets(); + // motor_bl.process_packets(); + // motor_br.process_packets(); + // motor_fr.process_packets(); + // motor_drib.process_packets(); + + // motor_fl.log_reset("FL"); + // motor_bl.log_reset("BL"); + // motor_br.log_reset("BR"); + // motor_fr.log_reset("FR"); + // motor_drib.log_reset("DRIB"); + + // if motor_drib.ball_detected() { + // defmt::info!("ball detected"); + // } + + // if let Some(latest_packet) = command_subscriber.try_next_message_pure() { + // match latest_packet { + // ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + + // let new_cmd_vel = Vector3::new( + // latest_control.vel_x_linear, + // latest_control.vel_y_linear, + // latest_control.vel_z_angular, + // ); + + // defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); + + // cmd_vel = new_cmd_vel; + // drib_vel = latest_control.dribbler_speed; + // ticks_since_packet = 0; + // }, + // ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { + // defmt::warn!("param updates aren't supported yet"); + // }, + // } + // } else { + // ticks_since_packet += 1; + // if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { + // cmd_vel = Vector3::new(0., 0., 0.); + // // ticks_since_packet = 0; + // } + // } + + // defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); + + + // // now we have setpoint r(t) in self.cmd_vel + // // let battery_v = battery_sub.next_message_pure().await as f32; + // let battery_v = 25.0; + // let controls_enabled = false; + // let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; + // let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + // if controls_enabled + // { + // // TODO check order + // let wheel_vels = Vector4::new( + // motor_fr.read_rads(), + // motor_fl.read_rads(), + // motor_bl.read_rads(), + // motor_br.read_rads() + // ); + + // // torque values are computed on the spin but put in the current variable + // // TODO update this when packet/var names are updated to match software + // let wheel_torques = Vector4::new( + // motor_fr.read_current(), + // motor_fl.read_current(), + // motor_bl.read_current(), + // motor_br.read_current() + // ); + + // // TODO read from channel or something + + // robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + // robot_controller.get_wheel_velocities() + // } else { + // robot_model.robot_vel_to_wheel_vel(cmd_vel) + // } + // } else { + // // Battery is too low, set velocity to zero + // Vector4::new( + // 0.0, + // 0.0, + // 0.0, + // 0.0) + // }; + + // motor_fr.set_setpoint(wheel_vels[0]); + // motor_fl.set_setpoint(wheel_vels[1]); + // motor_bl.set_setpoint(wheel_vels[2]); + // motor_br.set_setpoint(wheel_vels[3]); + + // let drib_dc = -1.0 * drib_vel / 1000.0; + // motor_drib.set_setpoint(drib_dc); + + // motor_fr.send_motion_command(); + // motor_fl.send_motion_command(); + // motor_bl.send_motion_command(); + // motor_br.send_motion_command(); + // motor_drib.send_motion_command(); + + + // let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + // sequence_number: 0, + // robot_revision_major: 0, + // robot_revision_minor: 0, + // battery_level: battery_v, + // battery_temperature: 0., + // _bitfield_align_1: [], + // _bitfield_1: BasicTelemetry::new_bitfield_1( + // 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + // ), + // motor_0_temperature: 0., + // motor_1_temperature: 0., + // motor_2_temperature: 0., + // motor_3_temperature: 0., + // motor_4_temperature: 0., + // kicker_charge_level: 0., + // }); + // telemetry_publisher.publish_immediate(basic_telem); + + // let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); + // telemetry_publisher.publish_immediate(control_debug_telem); + + // loop_rate_ticker.next().await; + // } + + // UNUSED!!! + // loop { + // motor_fl.process_packets(); + + // let rads = motor_fl.read_rads(); + // defmt::info!("read motor rads {}", rads); + // motor_fl.set_setpoint(3.1415 * 10.0); + + // motor_fl.send_motion_command(); + + // Timer::after_millis(10).await; + // } +// } pub async fn start_control_task( uart_queue_spawner: SendSpawner, From 8aa9a80d1e3fabad743db10a467fc7055f2007bd Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 9 Jun 2024 22:08:20 -0400 Subject: [PATCH 067/157] Will should look at this --- control-board/src/tasks/control_task.rs | 260 +++++++++++++----------- 1 file changed, 141 insertions(+), 119 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 67239809..433e33d7 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,3 +1,4 @@ +use alloc::vec::Vec; use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; @@ -5,7 +6,7 @@ use embassy_stm32::usart::Uart; use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{self, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{self, robot_controller::{self, BodyVelocityController}, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -99,6 +100,72 @@ impl < motor_drib: motor_drib } } + + fn do_control_update(&mut self, + robot_controller: &mut BodyVelocityController, + cmd_vel: Vector3 + ) -> Vector4 + /* + Provide the motion controller with the current wheel velocities + and torques from the appropriate sensors, then get a set of wheel + velocities to apply based on the controller's current state. + */ + { + let wheel_vels = Vector4::new( + self.motor_fr.read_rads(), + self.motor_fl.read_rads(), + self.motor_bl.read_rads(), + self.motor_br.read_rads() + ); + + // torque values are computed on the spin but put in the current variable + // TODO update this when packet/var names are updated to match software + let wheel_torques = Vector4::new( + self.motor_fr.read_current(), + self.motor_fl.read_current(), + self.motor_bl.read_current(), + self.motor_br.read_current() + ); + + // TODO read from channel or something + + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + robot_controller.get_wheel_velocities() + } + + fn send_motor_commands_and_telemetry(&mut self, + robot_controller: &mut BodyVelocityController, + battery_voltage: &f32) + { + self.motor_fr.send_motion_command(); + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_drib.send_motion_command(); + + + let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + sequence_number: 0, + robot_revision_major: 0, + robot_revision_minor: 0, + battery_level: *battery_voltage, + battery_temperature: 0., + _bitfield_align_1: [], + _bitfield_1: BasicTelemetry::new_bitfield_1( + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + ), + motor_0_temperature: 0., + motor_1_temperature: 0., + motor_2_temperature: 0., + motor_3_temperature: 0., + motor_4_temperature: 0., + kicker_charge_level: 0., + }); + self.telemetry_publisher.publish_immediate(basic_telem); + + let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); + self.telemetry_publisher.publish_immediate(control_debug_telem); + } async fn control_task_entry(&mut self) { defmt::info!("control task init."); @@ -140,131 +207,86 @@ impl < let mut ticks_since_packet = 0; - loop { - self.motor_fl.process_packets(); - self.motor_bl.process_packets(); - self.motor_br.process_packets(); - self.motor_fr.process_packets(); - self.motor_drib.process_packets(); + loop { + self.motor_fl.process_packets(); + self.motor_bl.process_packets(); + self.motor_br.process_packets(); + self.motor_fr.process_packets(); + self.motor_drib.process_packets(); - self.motor_fl.log_reset("FL"); - self.motor_bl.log_reset("BL"); - self.motor_br.log_reset("BR"); - self.motor_fr.log_reset("FR"); - self.motor_drib.log_reset("DRIB"); + self.motor_fl.log_reset("FL"); + self.motor_bl.log_reset("BL"); + self.motor_br.log_reset("BR"); + self.motor_fr.log_reset("FR"); + self.motor_drib.log_reset("DRIB"); - if self.motor_drib.ball_detected() { - defmt::info!("ball detected"); - } - - if let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { - match latest_packet { - ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { - - let new_cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); - - defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); - - cmd_vel = new_cmd_vel; - drib_vel = latest_control.dribbler_speed; - ticks_since_packet = 0; - }, - ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { - defmt::warn!("param updates aren't supported yet"); - }, + if self.motor_drib.ball_detected() { + defmt::info!("ball detected"); } - } else { - ticks_since_packet += 1; - if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - cmd_vel = Vector3::new(0., 0., 0.); - // ticks_since_packet = 0; - } - } - defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); - - - // now we have setpoint r(t) in self.cmd_vel - // let battery_v = battery_sub.next_message_pure().await as f32; - let battery_v = 25.0; - let controls_enabled = false; - let gyro_rads = (self.gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; - let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - if controls_enabled - { - // TODO check order - let wheel_vels = Vector4::new( - self.motor_fr.read_rads(), - self.motor_fl.read_rads(), - self.motor_bl.read_rads(), - self.motor_br.read_rads() - ); - - // torque values are computed on the spin but put in the current variable - // TODO update this when packet/var names are updated to match software - let wheel_torques = Vector4::new( - self.motor_fr.read_current(), - self.motor_fl.read_current(), - self.motor_bl.read_current(), - self.motor_br.read_current() - ); - - // TODO read from channel or something - - robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); - robot_controller.get_wheel_velocities() + if let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { + match latest_packet { + ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + + let new_cmd_vel = Vector3::new( + latest_control.vel_x_linear, + latest_control.vel_y_linear, + latest_control.vel_z_angular, + ); + + defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); + + cmd_vel = new_cmd_vel; + drib_vel = latest_control.dribbler_speed; + ticks_since_packet = 0; + }, + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { + defmt::warn!("param updates aren't supported yet"); + }, + } } else { - robot_model.robot_vel_to_wheel_vel(cmd_vel) + ticks_since_packet += 1; + if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + // ticks_since_packet = 0; + } } - } else { - // Battery is too low, set velocity to zero - Vector4::new( - 0.0, - 0.0, - 0.0, - 0.0) - }; - - self.motor_fr.set_setpoint(wheel_vels[0]); - self.motor_fl.set_setpoint(wheel_vels[1]); - self.motor_bl.set_setpoint(wheel_vels[2]); - self.motor_br.set_setpoint(wheel_vels[3]); - - let drib_dc = -1.0 * drib_vel / 1000.0; - self.motor_drib.set_setpoint(drib_dc); - - self.motor_fr.send_motion_command(); - self.motor_fl.send_motion_command(); - self.motor_bl.send_motion_command(); - self.motor_br.send_motion_command(); - self.motor_drib.send_motion_command(); - - let basic_telem = TelemetryPacket::Basic(BasicTelemetry { - sequence_number: 0, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: battery_v, - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }); - self.telemetry_publisher.publish_immediate(basic_telem); - - let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); - self.telemetry_publisher.publish_immediate(control_debug_telem); + defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); + + + // now we have setpoint r(t) in self.cmd_vel + // let battery_v = battery_sub.next_message_pure().await as f32; + let battery_v = 25.0; + let controls_enabled = false; + let gyro_rads = (self.gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; + let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + if controls_enabled + { + // TODO check order + self.do_control_update(&mut robot_controller, cmd_vel) + } else { + robot_model.robot_vel_to_wheel_vel(cmd_vel) + } + } else { + // Battery is too low, set velocity to zero + Vector4::new( + 0.0, + 0.0, + 0.0, + 0.0) + }; + + self.motor_fr.set_setpoint(wheel_vels[0]); + self.motor_fl.set_setpoint(wheel_vels[1]); + self.motor_bl.set_setpoint(wheel_vels[2]); + self.motor_br.set_setpoint(wheel_vels[3]); + + let drib_dc = -1.0 * drib_vel / 1000.0; + self.motor_drib.set_setpoint(drib_dc); + + self.send_motor_commands_and_telemetry( + &mut robot_controller, &battery_v); loop_rate_ticker.next().await; } From 81abeeabbbc47394a2e61cd60ca1e8f560ebec67 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 11 Jun 2024 19:50:27 -0700 Subject: [PATCH 068/157] Added Battery pin, updated embassy --- control-board/Cargo.lock | 166 ++++--- control-board/src/bin/control-old/main.rs | 531 ---------------------- control-board/src/drivers/kicker.rs | 4 +- control-board/src/drivers/radio_robot.rs | 6 +- control-board/src/pins.rs | 7 +- control-board/src/stm32_interface.rs | 4 +- control-board/src/stspin_motor.rs | 8 +- control-board/src/tasks/control_task.rs | 4 + control-board/src/tasks/imu_task.rs | 2 +- control-board/src/tasks/radio_task.rs | 4 +- control-board/src/tasks/user_io_task.rs | 22 +- lib-stm32-test/Cargo.lock | 113 +++-- lib-stm32/Cargo.lock | 99 ++-- lib-stm32/src/drivers/flash/at25df041b.rs | 10 +- lib-stm32/src/drivers/imu/bmi085.rs | 12 +- lib-stm32/src/drivers/imu/bmi323.rs | 12 +- lib-stm32/src/drivers/mod.rs | 3 +- lib-stm32/src/drivers/other/adc_helper.rs | 39 ++ lib-stm32/src/drivers/other/mod.rs | 1 + lib-stm32/src/drivers/radio/odin_w26x.rs | 4 +- lib-stm32/src/uart/queue.rs | 30 +- 21 files changed, 328 insertions(+), 753 deletions(-) delete mode 100644 control-board/src/bin/control-old/main.rs create mode 100644 lib-stm32/src/drivers/other/adc_helper.rs create mode 100644 lib-stm32/src/drivers/other/mod.rs diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 41cd4a44..438a0ab0 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -56,7 +56,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "futures-util", "heapless 0.7.17", @@ -77,7 +77,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless 0.8.0", "paste", @@ -173,9 +173,9 @@ checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" [[package]] name = "bytemuck" -version = "1.15.0" +version = "1.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" +checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" [[package]] name = "byteorder" @@ -209,9 +209,9 @@ dependencies = [ [[package]] name = "clang-sys" -version = "1.7.0" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" dependencies = [ "glob", "libc", @@ -228,7 +228,7 @@ dependencies = [ "bitflags 1.3.2", "clap_lex", "indexmap", - "strsim", + "strsim 0.10.0", "termcolor", "textwrap", ] @@ -277,9 +277,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -326,9 +326,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -336,27 +336,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", - "strsim", - "syn 2.0.60", + "strsim 0.11.1", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -371,15 +371,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -421,7 +421,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -435,18 +435,18 @@ dependencies = [ [[package]] name = "either" -version = "1.11.0" +version = "1.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" +checksum = "3dca9240753cf90908d7e4aac30f630662b02aebaa1b58a3cadabdb23385b58b" [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -459,7 +459,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -473,23 +473,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -500,7 +500,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -508,7 +508,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -523,7 +523,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embassy-time-driver", "embassy-usb-driver", @@ -553,7 +553,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -565,8 +578,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -584,7 +597,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "document-features", ] @@ -592,12 +605,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -605,10 +618,10 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -705,9 +718,9 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", "windows-sys", @@ -850,9 +863,9 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.154" +version = "0.2.155" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" +checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" [[package]] name = "libloading" @@ -872,9 +885,9 @@ checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" [[package]] name = "linux-raw-sys" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" +checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" [[package]] name = "litrs" @@ -963,9 +976,9 @@ dependencies = [ [[package]] name = "num-complex" -version = "0.4.5" +version = "0.4.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "23c6602fda94a57c990fe0df199a035d83576b496aa29f4e634a8ac6004e68a6" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" dependencies = [ "num-traits", ] @@ -981,11 +994,10 @@ dependencies = [ [[package]] name = "num-rational" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" dependencies = [ - "autocfg", "num-integer", "num-traits", ] @@ -1014,9 +1026,9 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -1078,9 +1090,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -1102,9 +1114,9 @@ checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" [[package]] name = "regex" -version = "1.10.4" +version = "1.10.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +checksum = "b91213439dad192326a0d7c6ee3955910425f441d7038e0d6933b0aec5c4517f" dependencies = [ "aho-corasick", "memchr", @@ -1114,9 +1126,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.6" +version = "0.4.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +checksum = "38caf58cc5ef2fed281f89292ef23f6365465ed9a41b7a7754eb4e26496c92df" dependencies = [ "aho-corasick", "memchr", @@ -1125,9 +1137,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.3" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" +checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] name = "rgb" @@ -1159,7 +1171,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.22", + "semver 1.0.23", ] [[package]] @@ -1198,9 +1210,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.22" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -1286,7 +1298,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1298,6 +1310,12 @@ version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + [[package]] name = "syn" version = "1.0.109" @@ -1311,9 +1329,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -1337,22 +1355,22 @@ checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/control-board/src/bin/control-old/main.rs b/control-board/src/bin/control-old/main.rs deleted file mode 100644 index 3a3fe7a7..00000000 --- a/control-board/src/bin/control-old/main.rs +++ /dev/null @@ -1,531 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(async_closure)] -#![feature(sync_unsafe_cell)] - -use apa102_spi::Apa102; -use ateam_control_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ - kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator - }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE -}; -use control::Control; -use defmt::info; -use embassy_stm32::{ - adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog -}; -use embassy_executor::InterruptExecutor; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_control_board::pins::{ - PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, - ShutdownCompletePin, -}; - -use smart_leds::{SmartLedsWrite, RGB8}; - -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::PubSubChannel; - -use ateam_lib_stm32::make_uart_queue_pair; - -use ateam_control_board::motion::tasks::control; -use ateam_control_board::pins::*; -use ateam_control_board::radio::RadioTest; - -#[cfg(not(feature = "no-private-credentials"))] -use credentials::private_credentials::wifi::wifi_credentials; -#[cfg(feature = "no-private-credentials")] -use credentials::public_credentials::wifi::wifi_credentials; -use static_cell::ConstStaticCell; - -// Uncomment for testing: -// use panic_probe as _; - -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - defmt::error!("{}", defmt::Display2Format(info)); - // Delay to give it a change to print - cortex_m::asm::delay(10_000_000); - cortex_m::peripheral::SCB::sys_reset(); -} - -include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} - -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; -pub const RADIO_TX_BUF_DEPTH: usize = 4; -pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; -pub const RADIO_RX_BUF_DEPTH: usize = 4; - -pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; -pub const KICKER_TX_BUF_DEPTH: usize = 4; -pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; -pub const KICKER_RX_BUF_DEPTH: usize = 4; - -make_uart_queue_pair!(RADIO, - RadioUART, RadioRxDMA, RadioTxDMA, - RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, - RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(KICKER, - KickerUart, KickerRxDma, KickerTxDma, - KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, - KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; - -static RADIO_TEST: ConstStaticCell> = - ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); -// static RADIO_TEST: RadioTest< -// RADIO_MAX_TX_PACKET_SIZE, -// RADIO_MAX_RX_PACKET_SIZE, -// RADIO_TX_BUF_DEPTH, -// RADIO_RX_BUF_DEPTH, -// RadioUART, -// RadioRxDMA, -// RadioTxDMA, -// RadioReset, -// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); - -// pub sub channel for the gyro vals -// CAP queue size, n_subs, n_pubs -static GYRO_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// pub sub channel for the battery raw adc vals -// CAP queue size, n_subs, n_pubs -static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// static RADIO: Radio = Radio::new(); -static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); - -#[interrupt] -unsafe fn CEC() { - EXECUTOR_UART_QUEUE.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART10 => usart::InterruptHandler; - USART6 => usart::InterruptHandler; - USART1 => usart::InterruptHandler; - UART4 => usart::InterruptHandler; - UART7 => usart::InterruptHandler; - UART8 => usart::InterruptHandler; - UART5 => usart::InterruptHandler; -}); - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let sys_config = get_system_config(); - let p = embassy_stm32::init(sys_config); - - let config = usart::Config::default(); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); - let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); - - let mut dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = hz(1_000_000); - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - p.DMA2_CH6, - dotstar_spi_config, - ); - - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - info!("booted"); - - let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, - ).unwrap(); - - let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); - let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); - let kicker_det = Input::new(p.PG8, Pull::Down); - let dip1 = Input::new(p.PG7, Pull::Down); - let dip2 = Input::new(p.PG6, Pull::Down); - let dip3 = Input::new(p.PG5, Pull::Down); - let dip4 = Input::new(p.PG4, Pull::Down); - let dip5 = Input::new(p.PG3, Pull::Down); - let _dip6 = Input::new(p.PG2, Pull::Down); - let dip7 = Input::new(p.PD15, Pull::Down); - - // let robot_id = rotary.read(); - - //////////////////////// - // Dip Switch Inputs // - //////////////////////// - let robot_id = (dip1.is_high() as u8) << 3 - | (dip2.is_high() as u8) << 2 - | (dip3.is_high() as u8) << 1 - | (dip4.is_high() as u8); - info!("id: {}", robot_id); - shell_indicator.set(robot_id); - - let wifi_network = WifiNetwork::Team; - // let wifi_network = if dip5.is_high() & dip6.is_high() { - // WifiNetwork::Team - // } else if dip5.is_low() & dip6.is_high() { - // WifiNetwork::CompMain - // } else if dip5.is_high() & dip6.is_low() { - // WifiNetwork::CompPractice - // } else { - // WifiNetwork::Team - // }; - - let control_debug_telemetry_enabled = dip5.is_high(); - if control_debug_telemetry_enabled { - info!("Debug control telemetry transmission enabled"); - } - - let team = if dip7.is_high() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - - ////////////////////// - // Battery Voltage // - ////////////////////// - - let mut adc3 = Adc::new(p.ADC3); - adc3.set_sample_time(SampleTime::CYCLES1_5); - let mut battery_pin = p.PF5; - let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = - [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; - let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); - - let mut imu_spi_config = spi::Config::default(); - imu_spi_config.frequency = mhz(1); - let mut imu_spi = spi::Spi::new( - p.SPI6, - p.PA5, - p.PA7, - p.PA6, - p.BDMA_CH0, - p.BDMA_CH1, - imu_spi_config, - ); - - // acceleromter - let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // gyro - let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - Timer::after(Duration::from_millis(1)).await; - - let gyro_pub = GYRO_CHANNEL.publisher().unwrap(); - unsafe { - SPI6_BUF[0] = 0x80; - // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - imu_cs1.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs1.set_high(); - let accel_id = SPI6_BUF[1]; - info!("accelerometer id: 0x{:x}", accel_id); - - SPI6_BUF[0] = 0x80; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs2.set_high(); - let gyro_id = SPI6_BUF[1]; - info!("gyro id: 0x{:x}", gyro_id); - } - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - Irqs, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ).unwrap(); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - Irqs, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ).unwrap(); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - Irqs, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ).unwrap(); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - Irqs, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ).unwrap(); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - Irqs, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ).unwrap(); - - let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); - let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); - - if kicker_det.is_low() { - defmt::warn!("kicker appears unplugged!"); - } - - let kicker_usart = Uart::new( - p.USART6, - p.PC7, - p.PC6, - Irqs, - p.DMA2_CH4, - p.DMA2_CH5, - get_bootloader_uart_config(), - ).unwrap(); - - let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); - spawner - .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) - .unwrap(); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - gyro_sub, - battery_sub, - ); - - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - control.load_firmware().await; - - info!("flashing kicker..."); - - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_RX_UART_QUEUE, - &KICKER_TX_UART_QUEUE, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), - ); - let kicker_firmware_image = KICKER_FW_IMG; - let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; - - if res.is_err() { - defmt::warn!("kicker flashing failed."); - loop {} - } else { - info!("kicker flash complete"); - } - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }] - .iter() - .cloned(), - ); - - // let token = unsafe { - // (&mut *(&RADIO_TEST as *const _ - // as *mut RadioTest< - // RADIO_MAX_TX_PACKET_SIZE, - // RADIO_MAX_RX_PACKET_SIZE, - // RADIO_TX_BUF_DEPTH, - // RADIO_RX_BUF_DEPTH, - // RadioUART, - // RadioRxDMA, - // RadioTxDMA, - // RadioReset, - // >)) - // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - // .await - // }; - let radio_test = RADIO_TEST.take(); - let token = unsafe { - (&mut *(&RADIO_TEST as *const _ - as *mut RadioTest< - RADIO_MAX_TX_PACKET_SIZE, - RADIO_MAX_RX_PACKET_SIZE, - RADIO_TX_BUF_DEPTH, - RADIO_RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, - >)) - .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - .await - }; - // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; - spawner.spawn(token).unwrap(); - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] - .iter() - .cloned(), - ); - - let mut wdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); - unsafe { wdg.unleash() } - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - kicker.set_telemetry_enabled(true); - kicker.send_command(); - - defmt::info!("using SSID: {}", wifi_credentials[0].get_ssid()); - - loop { - unsafe { wdg.pet() }; - - unsafe { - SPI6_BUF[0] = 0x86; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - imu_cs2.set_high(); - let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - // info!("z rate: {}", rate_z); - let gyro_conversion = 2000.0 / 32767.0; - gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion); - } - - // could just feed gyro in here but the comment in control said to use a channel - - /////////////////////// - // Battery reading // - /////////////////////// - - let current_battery_v = - adc_v_to_battery_voltage(adc_raw_to_v(adc3.read(&mut battery_pin) as f32)); - // Shift buffer through - for i in (0..(BATTERY_BUFFER_SIZE - 1)).rev() { - battery_voltage_buffer[i + 1] = battery_voltage_buffer[i]; - } - // Add new battery read - battery_voltage_buffer[0] = current_battery_v; - let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); - let filter_battery_v = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); - battery_pub.publish_immediate(filter_battery_v); - - if filter_battery_v < BATTERY_MIN_VOLTAGE { - let _ = dotstar.write( - [RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 10, g: 0, b: 0 }] - .iter() - .cloned(), - ); - defmt::error!("Error filtered battery voltage too low"); - critical_section::with(|_| loop { - cortex_m::asm::delay(1_000_000); - unsafe { wdg.pet() }; - }); - } - - ///////////////////////// - // Parameter Updates // - ///////////////////////// - - let latest_param_cmd = radio_test.get_latest_params_cmd(); - - if let Some(latest_param_cmd) = latest_param_cmd { - let param_cmd_resp = control.apply_command(&latest_param_cmd); - - // if param_cmd_resp is Err, then the requested parameter update had no submodule acceping the - // field, or the type was invalid, or the update code is unimplemented - // if param_cmd_resp is Ok, then the read/write was successful - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - radio_test.send_parameter_response(resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - radio_test.send_parameter_response(resp).await; - } - } - - //////////////// - // Telemtry // - //////////////// - - let latest_control_cmd = radio_test.get_latest_control(); - - let telemetry = control.tick(latest_control_cmd).await; - if let (Some(mut telemetry), control_debug_telem) = telemetry { - // info!("{:?}", defmt::Debug2Format(&telemetry)); - - telemetry.kicker_charge_level = kicker.hv_rail_voltage(); - telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); - - radio_test.send_telemetry(telemetry).await; - - if control_debug_telemetry_enabled { - radio_test.send_control_debug_telemetry(control_debug_telem).await; - } - } - - kicker.process_telemetry(); - - if let Some(control) = latest_control_cmd { - kicker.set_kick_strength(control.kick_vel); - kicker.request_kick(control.kick_request); - kicker.send_command(); - } - - main_loop_rate_ticker.next().await; - } -} - -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); - loop {} -} diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 43e5acad..f37a2b86 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -7,7 +7,7 @@ use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, Kick pub struct Kicker< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -35,7 +35,7 @@ pub struct Kicker< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index ed7ac3ce..48bef498 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -52,7 +52,7 @@ pub enum RobotRadioError { unsafe impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -64,7 +64,7 @@ unsafe impl< pub struct RobotRadio< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_TX: usize, @@ -88,7 +88,7 @@ pub struct RobotRadio< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_TX: usize, diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index b8141c9c..85b37eda 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -14,6 +14,7 @@ const COMMANDS_PUBSUB_DEPTH: usize = 4; const TELEMETRY_PUBSUB_DEPTH: usize = 4; const GYRO_DATA_PUBSUB_DEPTH: usize = 1; const ACCEL_DATA_PUBSUB_DEPTH: usize = 1; +const BATTERY_VOLT_PUBSUB_DEPTH: usize = 1; pub type CommandsPubSub = PubSubChannel; pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; @@ -30,6 +31,9 @@ pub type AccelDataPubSub = PubSubChannel, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; pub type AccelDataSubscriber = Subscriber<'static, ThreadModeRawMutex, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type BatteryVoltPubSub = PubSubChannel; +pub type BatteryVoltPublisher = Publisher<'static, ThreadModeRawMutex, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; + ///////////// // Radio // ///////////// @@ -64,7 +68,8 @@ pub type ImuSpiInt2Pin = PB2; pub type ImuSpiInt1Exti = EXTI1; pub type ImuSpiInt2Exti = EXTI2; pub type ExtImuNDetPin = PF11; - +pub type BatteryAdcPin = PF5; +pub type BatteryAdc = ADC3; ////////////// // Kicker // diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 0f90bc8f..976afd9d 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -40,7 +40,7 @@ pub fn get_bootloader_uart_config() -> Config { pub struct Stm32Interface< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -60,7 +60,7 @@ pub struct Stm32Interface< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 5df713e6..8aa71e72 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -23,7 +23,7 @@ use crate::stm32_interface::Stm32Interface; pub struct WheelMotor< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -63,7 +63,7 @@ pub struct WheelMotor< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -326,7 +326,7 @@ impl< pub struct DribblerMotor< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -368,7 +368,7 @@ pub struct DribblerMotor< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index b3bbae38..e0cee2aa 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -222,6 +222,10 @@ async fn control_task_entry( loop { motor_fl.process_packets(); + motor_bl.process_packets(); + motor_br.process_packets(); + motor_fr.process_packets(); + let rads = motor_fl.read_rads(); defmt::info!("read motor rads {}", rads); diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 12dba3ff..95383fe8 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -34,7 +34,7 @@ static IMU_BUFFER_CELL: ConstStaticCell<[u8; bmi323::SPI_MIN_BUF_LEN]> = ConstSt async fn imu_task_entry( accel_pub: AccelDataPublisher, gyro_pub: GyroDataPublisher, - mut imu: Bmi323<'static, 'static, ImuSpi>, + mut imu: Bmi323<'static, 'static>, mut _accel_int: ExtiInput<'static>, mut gyro_int: ExtiInput<'static>) { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 1e862f2e..14d45b2e 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -60,7 +60,7 @@ enum RadioConnectionState { } pub struct RadioTask< - UartPeripherial: usart::BasicInstance, + UartPeripherial: usart::Instance, UartRxDma: usart::RxDma, UartTxDma: usart::TxDma, const RADIO_MAX_TX_PACKET_SIZE: usize, @@ -78,7 +78,7 @@ pub struct RadioTask< } impl< - UartPeripherial: usart::BasicInstance, + UartPeripherial: usart::Instance, UartRxDma: usart::RxDma, UartTxDma: usart::TxDma, const RADIO_MAX_TX_PACKET_SIZE: usize, diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 8b8db7af..bb0261f1 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -1,7 +1,10 @@ use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; +use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; + use embassy_executor::Spawner; use embassy_stm32::gpio::{AnyPin, Level, Output, Pull, Speed}; +use embassy_stm32::adc::{SampleTime, Resolution}; use embassy_time::Timer; use crate::drivers::shell_indicator::ShellIndicator; @@ -23,7 +26,10 @@ macro_rules! create_io_task { } #[embassy_executor::task] -async fn user_io_task_entry(robot_state: &'static SharedRobotState, +async fn user_io_task_entry( + robot_state: &'static SharedRobotState, + battery_volt_publisher: BatteryVoltPublisher, + mut battery_volt_adc: AdcHelper<'static, BatteryAdc, BatteryAdcPin>, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, @@ -62,6 +68,8 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); } + battery_volt_publisher.publish_immediate(battery_volt_adc.read_f32()); + // TODO read messages // update indicators @@ -82,8 +90,11 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, } } -pub fn start_io_task(spawner: Spawner, +pub fn start_io_task( + spawner: Spawner, robot_state: &'static SharedRobotState, + battery_volt_publisher: BatteryVoltPublisher, + battery_adc: BatteryAdc, battery_adc_pin: BatteryAdcPin, _usr_btn0_pin: UsrBtn0Pin, _usr_btn1_pin: UsrBtn1Pin, _usr_btn0_exti: UsrBtn0Exti, _usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, @@ -92,8 +103,7 @@ pub fn start_io_task(spawner: Spawner, usr_led0_pin: UsrLed0Pin, _usr_led1_pin: UsrLed1Pin, _usr_led2_pin: UsrLed2Pin, _usr_led3_pin: UsrLed3Pin, robot_id_indicator_fl: RobotIdIndicator0FlPin, robot_id_indicator_bl: RobotIdIndicator1BlPin, robot_id_indicator_br: RobotIdIndicator2BrPin, robot_id_indicator_fr: RobotIdIndicator3FrPin, - robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin, - ) { + robot_id_indicator_isblue: RobotIdIndicator4TeamIsBluePin) { let dip_sw_pins: [AnyPin; 7] = [usr_dip7_pin.into(), usr_dip6_pin.into(), usr_dip5_pin.into(), usr_dip4_pin.into(), usr_dip3_pin.into(), usr_dip2_pin.into(), usr_dip1_pin.into()]; let dip_switch = DipSwitch::new_from_pins(dip_sw_pins, Pull::None, None); @@ -105,5 +115,7 @@ pub fn start_io_task(spawner: Spawner, let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); - spawner.spawn(user_io_task_entry(robot_state, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); + let battery_volt_adc = AdcHelper::new(battery_adc, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); + + spawner.spawn(user_io_task_entry(robot_state, battery_volt_publisher, battery_volt_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); } \ No newline at end of file diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index c7726f86..3b9ee13e 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -12,7 +12,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless 0.8.0", "paste", @@ -33,7 +33,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "futures-util", "heapless 0.7.17", @@ -152,9 +152,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -197,9 +197,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -207,27 +207,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -242,15 +242,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -292,7 +292,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -307,11 +307,11 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -324,7 +324,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -338,23 +338,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -365,7 +365,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", ] @@ -373,7 +373,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -388,7 +388,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embassy-time-driver", "embassy-usb-driver", @@ -418,7 +418,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -431,7 +444,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -449,7 +462,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "document-features", ] @@ -457,12 +470,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", ] @@ -470,10 +483,10 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -680,9 +693,9 @@ dependencies = [ [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -738,9 +751,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -775,7 +788,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.22", + "semver 1.0.23", ] [[package]] @@ -801,9 +814,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.22" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -853,7 +866,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", "cortex-m-rt", @@ -861,9 +874,9 @@ dependencies = [ [[package]] name = "strsim" -version = "0.10.0" +version = "0.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" @@ -878,9 +891,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -889,22 +902,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 28d67430..5866696e 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -12,7 +12,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless", "paste", @@ -89,9 +89,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -125,9 +125,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -135,27 +135,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -170,15 +170,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -212,10 +212,10 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -228,7 +228,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "critical-section 1.1.2", "document-features", @@ -238,23 +238,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -264,12 +264,12 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -282,7 +282,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", "embassy-usb-synopsys-otg", "embedded-can", @@ -310,7 +310,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -322,7 +335,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -339,7 +352,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "document-features", ] @@ -347,20 +360,20 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -567,9 +580,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -643,16 +656,16 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", ] [[package]] name = "strsim" -version = "0.10.0" +version = "0.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" @@ -667,9 +680,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -678,22 +691,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index dad54dc7..8f1205de 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -4,18 +4,18 @@ use embassy_stm32::{ spi::{self, Error} }; -pub struct AT25DF041B<'buf, T: spi::Instance, const CS_POL_N: bool> { - spi: spi::Spi<'static, T, Async>, +pub struct AT25DF041B<'buf, const CS_POL_N: bool> { + spi: spi::Spi<'static, Async>, chip_select: Output<'static>, tx_buf: &'buf mut [u8; 256], rx_buf: &'buf mut [u8; 256], } -impl<'buf, T: spi::Instance, const CS_POL_N: bool> AT25DF041B<'buf, T, CS_POL_N> { - pub fn new(spi: spi::Spi<'static, T, Async>, +impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { + pub fn new(spi: spi::Spi<'static, Async>, chip_select: impl Pin, tx_buf: &'buf mut [u8; 256], - rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, T, CS_POL_N> { + rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, CS_POL_N> { AT25DF041B { spi, chip_select: Output::new(chip_select, Level::High, Speed::High), diff --git a/lib-stm32/src/drivers/imu/bmi085.rs b/lib-stm32/src/drivers/imu/bmi085.rs index ea6bbc12..32f75d49 100644 --- a/lib-stm32/src/drivers/imu/bmi085.rs +++ b/lib-stm32/src/drivers/imu/bmi085.rs @@ -22,8 +22,8 @@ use defmt::*; pub const SPI_MIN_BUF_LEN: usize = 8; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro -pub struct Bmi085<'a, 'buf, SpiPeri: spi::Instance> { - spi: spi::Spi<'a, SpiPeri, Async>, +pub struct Bmi085<'a, 'buf> { + spi: spi::Spi<'a, Async>, accel_cs: Output<'a>, gyro_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], @@ -200,11 +200,11 @@ const GYRO_CHIP_ID: u8 = 0x0F; const READ_BIT: u8 = 0x80; -impl<'a, 'buf, SpiPeri: spi::Instance> - Bmi085<'a, 'buf, SpiPeri> { +impl<'a, 'buf> + Bmi085<'a, 'buf> { /// creates a new BMI085 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, accel_cs: Output<'a>, gyro_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], @@ -220,7 +220,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } ///t creates a new BMI085 instance from uninitialized pins - pub fn new_from_pins( + pub fn new_from_pins( peri: impl Peripheral

+ 'a, sck: impl Peripheral

> + 'a, mosi: impl Peripheral

> + 'a, diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 69973866..e910d9bc 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -17,8 +17,8 @@ use embassy_stm32::{ pub const SPI_MIN_BUF_LEN: usize = 14; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro -pub struct Bmi323<'a, 'buf, SpiPeri: spi::Instance> { - spi: spi::Spi<'a, SpiPeri, Async>, +pub struct Bmi323<'a, 'buf> { + spi: spi::Spi<'a, Async>, spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], accel_mode: AccelMode, @@ -228,11 +228,11 @@ pub enum GyroMode { const BMI323_CHIP_ID: u16 = 0x0043; const READ_BIT: u8 = 0x80; -impl<'a, 'buf, SpiPeri: spi::Instance> - Bmi323<'a, 'buf, SpiPeri> { +impl<'a, 'buf> + Bmi323<'a, 'buf> { /// creates a new BMI323 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { @@ -254,7 +254,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } ///t creates a new BMI085 instance from uninitialized pins - pub fn new_from_pins( + pub fn new_from_pins( peri: impl Peripheral

+ 'a, sck_pin: impl Peripheral

> + 'a, mosi_pin: impl Peripheral

> + 'a, diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index 8a7859bb..a70041b5 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -2,4 +2,5 @@ pub mod flash; pub mod imu; pub mod radio; -pub mod switches; \ No newline at end of file +pub mod switches; +pub mod other; \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs new file mode 100644 index 00000000..c9129ebb --- /dev/null +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -0,0 +1,39 @@ + +use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime, VrefInt, VREF_DEFAULT_MV}; +use embassy_stm32::Peripheral; + +pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { + inst: Adc<'a, T>, + channel_pin: Ch, + channel_vref: VrefInt +} + +impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> { + pub fn new( + peri: impl Peripheral

+ 'a, + channel_pin: Ch, + sample_time: SampleTime, + resolution: Resolution + ) -> Self { + + let mut adc_inst = Adc::new(peri); + + adc_inst.set_sample_time(sample_time); + adc_inst.set_resolution(resolution); + + let channel_vref = adc_inst.enable_vrefint(); + + AdcHelper { + inst: adc_inst, + channel_pin: channel_pin, + channel_vref: channel_vref + } + } + + pub fn read_f32(&mut self) -> f32 { + // Gets the Vref since it changes based on chip class. + let vref_cur_mv = self.inst.read(&mut self.channel_vref) as f32; + // Scale by Vref to convert to absolute voltage. + return ((VREF_DEFAULT_MV as f32) / vref_cur_mv) * (self.inst.read(&mut self.channel_pin) as f32); + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/mod.rs b/lib-stm32/src/drivers/other/mod.rs new file mode 100644 index 00000000..1e9398cf --- /dev/null +++ b/lib-stm32/src/drivers/other/mod.rs @@ -0,0 +1 @@ +pub mod adc_helper; \ No newline at end of file diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index ba193e5b..6032eef6 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -45,7 +45,7 @@ pub struct PeerConnection { pub struct Radio< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, TxDma: usart::TxDma, RxDma: usart::RxDma, const LEN_TX: usize, @@ -60,7 +60,7 @@ pub struct Radio< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, TxDma: usart::TxDma, RxDma: usart::RxDma, const LEN_TX: usize, diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index 6f5b965b..66f52595 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -110,7 +110,7 @@ pub enum UartTaskCommand { } pub struct UartReadQueue< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -123,7 +123,7 @@ pub struct UartReadQueue< // TODO: pretty sure shouldn't do this unsafe impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -132,7 +132,7 @@ unsafe impl< } pub type ReadTaskFuture< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -140,7 +140,7 @@ pub type ReadTaskFuture< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -158,7 +158,7 @@ impl< fn read_task( &'static self, queue_rx: &'static Queue, - mut rx: UartRx<'static, UART, Async>, + mut rx: UartRx<'static, Async>, mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> ReadTaskFuture { async move { @@ -237,7 +237,7 @@ impl< pub fn spawn_task( &'static self, - rx: UartRx<'static, UART, Async>, + rx: UartRx<'static, Async>, uart_config_signal_subscriber: UartQueueConfigSyncSub ) -> SpawnToken { self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_subscriber)) @@ -245,7 +245,7 @@ impl< pub fn spawn_task_with_pubsub( &'static self, - rx: UartRx<'static, UART, Async>, + rx: UartRx<'static, Async>, uart_config_signal_pubsub: &'static UartQueueSyncPubSub ) -> SpawnToken { self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_pubsub.subscriber().unwrap())) @@ -262,7 +262,7 @@ impl< } pub struct UartWriteQueue< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -276,7 +276,7 @@ pub struct UartWriteQueue< } type WriteTaskFuture< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -284,7 +284,7 @@ type WriteTaskFuture< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -317,7 +317,7 @@ impl< fn write_task( &'static self, queue_tx: &'static Queue, - mut tx: UartTx<'static, UART, Async>, + mut tx: UartTx<'static, Async>, uart_config_signal_publisher: UartQueueConfigSyncPub, mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> WriteTaskFuture { @@ -400,7 +400,7 @@ impl< } pub fn spawn_task(&'static self, - tx: UartTx<'static, UART, Async>, + tx: UartTx<'static, Async>, uart_config_signal_publisher: UartQueueConfigSyncPub, uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> SpawnToken { @@ -408,7 +408,7 @@ impl< } pub fn spawn_task_with_pubsub(&'static self, - tx: UartTx<'static, UART, Async>, + tx: UartTx<'static, Async>, uart_config_signal_pubsub: &'static UartQueueSyncPubSub ) -> SpawnToken { self.task.spawn(|| self.write_task(&self.queue_tx, tx, uart_config_signal_pubsub.publisher().unwrap(), uart_config_signal_pubsub.subscriber().unwrap())) @@ -486,7 +486,7 @@ pub trait Writer { } impl< - UART: usart::BasicInstance, + UART: usart::Instance, Dma: usart::RxDma, const LEN: usize, const DEPTH: usize, @@ -498,7 +498,7 @@ impl< } impl< - UART: usart::BasicInstance, + UART: usart::Instance, Dma: usart::TxDma, const LEN: usize, const DEPTH: usize, From aa696fce2d90c184d639e462ec030382b143c6a1 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 11 Jun 2024 19:50:27 -0700 Subject: [PATCH 069/157] Added Battery pin, updated embassy --- control-board/Cargo.lock | 157 ++++--- control-board/src/bin/control-old/main.rs | 531 ---------------------- control-board/src/drivers/kicker.rs | 4 +- control-board/src/drivers/radio_robot.rs | 6 +- control-board/src/pins.rs | 7 +- control-board/src/stm32_interface.rs | 4 +- control-board/src/stspin_motor.rs | 8 +- control-board/src/tasks/control_task.rs | 4 + control-board/src/tasks/imu_task.rs | 2 +- control-board/src/tasks/radio_task.rs | 4 +- control-board/src/tasks/user_io_task.rs | 16 +- lib-stm32-test/Cargo.lock | 113 +++-- lib-stm32/Cargo.lock | 99 ++-- lib-stm32/src/drivers/flash/at25df041b.rs | 10 +- lib-stm32/src/drivers/imu/bmi085.rs | 12 +- lib-stm32/src/drivers/imu/bmi323.rs | 12 +- lib-stm32/src/drivers/mod.rs | 3 +- lib-stm32/src/drivers/other/adc_helper.rs | 39 ++ lib-stm32/src/drivers/other/mod.rs | 1 + lib-stm32/src/drivers/radio/odin_w26x.rs | 4 +- lib-stm32/src/uart/queue.rs | 30 +- 21 files changed, 320 insertions(+), 746 deletions(-) delete mode 100644 control-board/src/bin/control-old/main.rs create mode 100644 lib-stm32/src/drivers/other/adc_helper.rs create mode 100644 lib-stm32/src/drivers/other/mod.rs diff --git a/control-board/Cargo.lock b/control-board/Cargo.lock index 889c00f7..1a3deea2 100644 --- a/control-board/Cargo.lock +++ b/control-board/Cargo.lock @@ -56,7 +56,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "futures-util", "heapless 0.7.17", @@ -77,7 +77,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless 0.8.0", "num-traits", @@ -175,9 +175,9 @@ checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" [[package]] name = "bytemuck" -version = "1.15.0" +version = "1.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" +checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" [[package]] name = "byteorder" @@ -211,9 +211,9 @@ dependencies = [ [[package]] name = "clang-sys" -version = "1.7.0" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" dependencies = [ "glob", "libc", @@ -230,7 +230,7 @@ dependencies = [ "bitflags 1.3.2", "clap_lex", "indexmap", - "strsim", + "strsim 0.10.0", "termcolor", "textwrap", ] @@ -279,9 +279,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -328,9 +328,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -338,27 +338,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", - "strsim", - "syn 2.0.60", + "strsim 0.11.1", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -373,15 +373,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -423,7 +423,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -437,18 +437,18 @@ dependencies = [ [[package]] name = "either" -version = "1.11.0" +version = "1.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" +checksum = "3dca9240753cf90908d7e4aac30f630662b02aebaa1b58a3cadabdb23385b58b" [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -461,7 +461,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -475,23 +475,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -502,7 +502,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -510,7 +510,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -525,7 +525,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embassy-time-driver", "embassy-usb-driver", @@ -555,7 +555,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -567,8 +580,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -586,7 +599,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "document-features", ] @@ -594,12 +607,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -607,10 +620,10 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -707,9 +720,9 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", "windows-sys", @@ -852,9 +865,9 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.154" +version = "0.2.155" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" +checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" [[package]] name = "libloading" @@ -874,9 +887,9 @@ checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" [[package]] name = "linux-raw-sys" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" +checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" [[package]] name = "litrs" @@ -1015,9 +1028,9 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -1079,9 +1092,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -1103,9 +1116,9 @@ checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" [[package]] name = "regex" -version = "1.10.4" +version = "1.10.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +checksum = "b91213439dad192326a0d7c6ee3955910425f441d7038e0d6933b0aec5c4517f" dependencies = [ "aho-corasick", "memchr", @@ -1115,9 +1128,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.6" +version = "0.4.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +checksum = "38caf58cc5ef2fed281f89292ef23f6365465ed9a41b7a7754eb4e26496c92df" dependencies = [ "aho-corasick", "memchr", @@ -1126,9 +1139,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.3" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" +checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] name = "rgb" @@ -1160,7 +1173,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.22", + "semver 1.0.23", ] [[package]] @@ -1199,9 +1212,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.22" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -1305,7 +1318,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1317,6 +1330,12 @@ version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + [[package]] name = "syn" version = "1.0.109" @@ -1330,9 +1349,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -1356,22 +1375,22 @@ checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/control-board/src/bin/control-old/main.rs b/control-board/src/bin/control-old/main.rs deleted file mode 100644 index 3a3fe7a7..00000000 --- a/control-board/src/bin/control-old/main.rs +++ /dev/null @@ -1,531 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(async_closure)] -#![feature(sync_unsafe_cell)] - -use apa102_spi::Apa102; -use ateam_control_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ - kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator - }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE -}; -use control::Control; -use defmt::info; -use embassy_stm32::{ - adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog -}; -use embassy_executor::InterruptExecutor; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_control_board::pins::{ - PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, - ShutdownCompletePin, -}; - -use smart_leds::{SmartLedsWrite, RGB8}; - -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::PubSubChannel; - -use ateam_lib_stm32::make_uart_queue_pair; - -use ateam_control_board::motion::tasks::control; -use ateam_control_board::pins::*; -use ateam_control_board::radio::RadioTest; - -#[cfg(not(feature = "no-private-credentials"))] -use credentials::private_credentials::wifi::wifi_credentials; -#[cfg(feature = "no-private-credentials")] -use credentials::public_credentials::wifi::wifi_credentials; -use static_cell::ConstStaticCell; - -// Uncomment for testing: -// use panic_probe as _; - -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - defmt::error!("{}", defmt::Display2Format(info)); - // Delay to give it a change to print - cortex_m::asm::delay(10_000_000); - cortex_m::peripheral::SCB::sys_reset(); -} - -include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} - -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; -pub const RADIO_TX_BUF_DEPTH: usize = 4; -pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; -pub const RADIO_RX_BUF_DEPTH: usize = 4; - -pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; -pub const KICKER_TX_BUF_DEPTH: usize = 4; -pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; -pub const KICKER_RX_BUF_DEPTH: usize = 4; - -make_uart_queue_pair!(RADIO, - RadioUART, RadioRxDMA, RadioTxDMA, - RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, - RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(KICKER, - KickerUart, KickerRxDma, KickerTxDma, - KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, - KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; - -static RADIO_TEST: ConstStaticCell> = - ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); -// static RADIO_TEST: RadioTest< -// RADIO_MAX_TX_PACKET_SIZE, -// RADIO_MAX_RX_PACKET_SIZE, -// RADIO_TX_BUF_DEPTH, -// RADIO_RX_BUF_DEPTH, -// RadioUART, -// RadioRxDMA, -// RadioTxDMA, -// RadioReset, -// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); - -// pub sub channel for the gyro vals -// CAP queue size, n_subs, n_pubs -static GYRO_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// pub sub channel for the battery raw adc vals -// CAP queue size, n_subs, n_pubs -static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// static RADIO: Radio = Radio::new(); -static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); - -#[interrupt] -unsafe fn CEC() { - EXECUTOR_UART_QUEUE.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART10 => usart::InterruptHandler; - USART6 => usart::InterruptHandler; - USART1 => usart::InterruptHandler; - UART4 => usart::InterruptHandler; - UART7 => usart::InterruptHandler; - UART8 => usart::InterruptHandler; - UART5 => usart::InterruptHandler; -}); - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let sys_config = get_system_config(); - let p = embassy_stm32::init(sys_config); - - let config = usart::Config::default(); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); - let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); - - let mut dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = hz(1_000_000); - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - p.DMA2_CH6, - dotstar_spi_config, - ); - - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - info!("booted"); - - let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, - ).unwrap(); - - let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); - let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); - let kicker_det = Input::new(p.PG8, Pull::Down); - let dip1 = Input::new(p.PG7, Pull::Down); - let dip2 = Input::new(p.PG6, Pull::Down); - let dip3 = Input::new(p.PG5, Pull::Down); - let dip4 = Input::new(p.PG4, Pull::Down); - let dip5 = Input::new(p.PG3, Pull::Down); - let _dip6 = Input::new(p.PG2, Pull::Down); - let dip7 = Input::new(p.PD15, Pull::Down); - - // let robot_id = rotary.read(); - - //////////////////////// - // Dip Switch Inputs // - //////////////////////// - let robot_id = (dip1.is_high() as u8) << 3 - | (dip2.is_high() as u8) << 2 - | (dip3.is_high() as u8) << 1 - | (dip4.is_high() as u8); - info!("id: {}", robot_id); - shell_indicator.set(robot_id); - - let wifi_network = WifiNetwork::Team; - // let wifi_network = if dip5.is_high() & dip6.is_high() { - // WifiNetwork::Team - // } else if dip5.is_low() & dip6.is_high() { - // WifiNetwork::CompMain - // } else if dip5.is_high() & dip6.is_low() { - // WifiNetwork::CompPractice - // } else { - // WifiNetwork::Team - // }; - - let control_debug_telemetry_enabled = dip5.is_high(); - if control_debug_telemetry_enabled { - info!("Debug control telemetry transmission enabled"); - } - - let team = if dip7.is_high() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - - ////////////////////// - // Battery Voltage // - ////////////////////// - - let mut adc3 = Adc::new(p.ADC3); - adc3.set_sample_time(SampleTime::CYCLES1_5); - let mut battery_pin = p.PF5; - let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = - [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; - let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); - - let mut imu_spi_config = spi::Config::default(); - imu_spi_config.frequency = mhz(1); - let mut imu_spi = spi::Spi::new( - p.SPI6, - p.PA5, - p.PA7, - p.PA6, - p.BDMA_CH0, - p.BDMA_CH1, - imu_spi_config, - ); - - // acceleromter - let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // gyro - let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - Timer::after(Duration::from_millis(1)).await; - - let gyro_pub = GYRO_CHANNEL.publisher().unwrap(); - unsafe { - SPI6_BUF[0] = 0x80; - // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - imu_cs1.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs1.set_high(); - let accel_id = SPI6_BUF[1]; - info!("accelerometer id: 0x{:x}", accel_id); - - SPI6_BUF[0] = 0x80; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs2.set_high(); - let gyro_id = SPI6_BUF[1]; - info!("gyro id: 0x{:x}", gyro_id); - } - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - Irqs, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ).unwrap(); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - Irqs, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ).unwrap(); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - Irqs, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ).unwrap(); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - Irqs, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ).unwrap(); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - Irqs, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ).unwrap(); - - let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); - let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); - - if kicker_det.is_low() { - defmt::warn!("kicker appears unplugged!"); - } - - let kicker_usart = Uart::new( - p.USART6, - p.PC7, - p.PC6, - Irqs, - p.DMA2_CH4, - p.DMA2_CH5, - get_bootloader_uart_config(), - ).unwrap(); - - let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); - spawner - .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) - .unwrap(); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - gyro_sub, - battery_sub, - ); - - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - control.load_firmware().await; - - info!("flashing kicker..."); - - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_RX_UART_QUEUE, - &KICKER_TX_UART_QUEUE, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), - ); - let kicker_firmware_image = KICKER_FW_IMG; - let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; - - if res.is_err() { - defmt::warn!("kicker flashing failed."); - loop {} - } else { - info!("kicker flash complete"); - } - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }] - .iter() - .cloned(), - ); - - // let token = unsafe { - // (&mut *(&RADIO_TEST as *const _ - // as *mut RadioTest< - // RADIO_MAX_TX_PACKET_SIZE, - // RADIO_MAX_RX_PACKET_SIZE, - // RADIO_TX_BUF_DEPTH, - // RADIO_RX_BUF_DEPTH, - // RadioUART, - // RadioRxDMA, - // RadioTxDMA, - // RadioReset, - // >)) - // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - // .await - // }; - let radio_test = RADIO_TEST.take(); - let token = unsafe { - (&mut *(&RADIO_TEST as *const _ - as *mut RadioTest< - RADIO_MAX_TX_PACKET_SIZE, - RADIO_MAX_RX_PACKET_SIZE, - RADIO_TX_BUF_DEPTH, - RADIO_RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, - >)) - .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - .await - }; - // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; - spawner.spawn(token).unwrap(); - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] - .iter() - .cloned(), - ); - - let mut wdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); - unsafe { wdg.unleash() } - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - kicker.set_telemetry_enabled(true); - kicker.send_command(); - - defmt::info!("using SSID: {}", wifi_credentials[0].get_ssid()); - - loop { - unsafe { wdg.pet() }; - - unsafe { - SPI6_BUF[0] = 0x86; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - imu_cs2.set_high(); - let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - // info!("z rate: {}", rate_z); - let gyro_conversion = 2000.0 / 32767.0; - gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion); - } - - // could just feed gyro in here but the comment in control said to use a channel - - /////////////////////// - // Battery reading // - /////////////////////// - - let current_battery_v = - adc_v_to_battery_voltage(adc_raw_to_v(adc3.read(&mut battery_pin) as f32)); - // Shift buffer through - for i in (0..(BATTERY_BUFFER_SIZE - 1)).rev() { - battery_voltage_buffer[i + 1] = battery_voltage_buffer[i]; - } - // Add new battery read - battery_voltage_buffer[0] = current_battery_v; - let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); - let filter_battery_v = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); - battery_pub.publish_immediate(filter_battery_v); - - if filter_battery_v < BATTERY_MIN_VOLTAGE { - let _ = dotstar.write( - [RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 10, g: 0, b: 0 }] - .iter() - .cloned(), - ); - defmt::error!("Error filtered battery voltage too low"); - critical_section::with(|_| loop { - cortex_m::asm::delay(1_000_000); - unsafe { wdg.pet() }; - }); - } - - ///////////////////////// - // Parameter Updates // - ///////////////////////// - - let latest_param_cmd = radio_test.get_latest_params_cmd(); - - if let Some(latest_param_cmd) = latest_param_cmd { - let param_cmd_resp = control.apply_command(&latest_param_cmd); - - // if param_cmd_resp is Err, then the requested parameter update had no submodule acceping the - // field, or the type was invalid, or the update code is unimplemented - // if param_cmd_resp is Ok, then the read/write was successful - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - radio_test.send_parameter_response(resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - radio_test.send_parameter_response(resp).await; - } - } - - //////////////// - // Telemtry // - //////////////// - - let latest_control_cmd = radio_test.get_latest_control(); - - let telemetry = control.tick(latest_control_cmd).await; - if let (Some(mut telemetry), control_debug_telem) = telemetry { - // info!("{:?}", defmt::Debug2Format(&telemetry)); - - telemetry.kicker_charge_level = kicker.hv_rail_voltage(); - telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); - - radio_test.send_telemetry(telemetry).await; - - if control_debug_telemetry_enabled { - radio_test.send_control_debug_telemetry(control_debug_telem).await; - } - } - - kicker.process_telemetry(); - - if let Some(control) = latest_control_cmd { - kicker.set_kick_strength(control.kick_vel); - kicker.request_kick(control.kick_request); - kicker.send_command(); - } - - main_loop_rate_ticker.next().await; - } -} - -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); - loop {} -} diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 7b1ce8b9..15ca3a96 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -8,7 +8,7 @@ use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, Kick pub struct Kicker< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -36,7 +36,7 @@ pub struct Kicker< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index f4b78086..20d9263f 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -52,7 +52,7 @@ pub enum RobotRadioError { unsafe impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -64,7 +64,7 @@ unsafe impl< pub struct RobotRadio< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_TX: usize, @@ -88,7 +88,7 @@ pub struct RobotRadio< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_TX: usize, diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 943ab19e..c2316765 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -14,6 +14,7 @@ const COMMANDS_PUBSUB_DEPTH: usize = 4; const TELEMETRY_PUBSUB_DEPTH: usize = 4; const GYRO_DATA_PUBSUB_DEPTH: usize = 1; const ACCEL_DATA_PUBSUB_DEPTH: usize = 1; +const BATTERY_VOLT_PUBSUB_DEPTH: usize = 1; pub type CommandsPubSub = PubSubChannel; pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; @@ -30,6 +31,9 @@ pub type AccelDataPubSub = PubSubChannel, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; pub type AccelDataSubscriber = Subscriber<'static, ThreadModeRawMutex, nalgebra::Vector3, ACCEL_DATA_PUBSUB_DEPTH, 1, 1>; +pub type BatteryVoltPubSub = PubSubChannel; +pub type BatteryVoltPublisher = Publisher<'static, ThreadModeRawMutex, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; + ///////////// // Radio // ///////////// @@ -64,7 +68,8 @@ pub type ImuSpiInt2Pin = PB2; pub type ImuSpiInt1Exti = EXTI1; pub type ImuSpiInt2Exti = EXTI2; pub type ExtImuNDetPin = PF11; - +pub type BatteryAdcPin = PF5; +pub type BatteryAdc = ADC3; ////////////// // Kicker // diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 0f90bc8f..976afd9d 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -40,7 +40,7 @@ pub fn get_bootloader_uart_config() -> Config { pub struct Stm32Interface< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -60,7 +60,7 @@ pub struct Stm32Interface< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index c3a04eeb..554e19c9 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -23,7 +23,7 @@ use crate::stm32_interface::Stm32Interface; pub struct WheelMotor< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -63,7 +63,7 @@ pub struct WheelMotor< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -326,7 +326,7 @@ impl< pub struct DribblerMotor< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -368,7 +368,7 @@ pub struct DribblerMotor< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 8221dde8..8670dc67 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -295,6 +295,10 @@ async fn control_task_entry( loop { motor_fl.process_packets(); + motor_bl.process_packets(); + motor_br.process_packets(); + motor_fr.process_packets(); + let rads = motor_fl.read_rads(); defmt::info!("read motor rads {}", rads); diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index f3962a7d..896d87b7 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -41,7 +41,7 @@ async fn imu_task_entry( robot_state: &'static SharedRobotState, accel_pub: AccelDataPublisher, gyro_pub: GyroDataPublisher, - mut imu: Bmi323<'static, 'static, ImuSpi>, + mut imu: Bmi323<'static, 'static>, mut _accel_int: ExtiInput<'static>, mut gyro_int: ExtiInput<'static>) { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 63d4250a..90efe3e4 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -62,7 +62,7 @@ enum RadioConnectionState { } pub struct RadioTask< - UartPeripherial: usart::BasicInstance, + UartPeripherial: usart::Instance, UartRxDma: usart::RxDma, UartTxDma: usart::TxDma, const RADIO_MAX_TX_PACKET_SIZE: usize, @@ -82,7 +82,7 @@ pub struct RadioTask< } impl< - UartPeripherial: usart::BasicInstance, + UartPeripherial: usart::Instance, UartRxDma: usart::RxDma, UartTxDma: usart::TxDma, const RADIO_MAX_TX_PACKET_SIZE: usize, diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index c2667589..f66b0204 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -15,6 +15,8 @@ use static_cell::ConstStaticCell; use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; +use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; +use embassy_stm32::adc::{SampleTime, Resolution}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; @@ -43,9 +45,12 @@ macro_rules! create_io_task { } #[embassy_executor::task] -async fn user_io_task_entry(robot_state: &'static SharedRobotState, +async fn user_io_task_entry( + robot_state: &'static SharedRobotState, mut usr_btn0: AdvExtiButton, mut usr_btn1: AdvExtiButton, + battery_volt_publisher: BatteryVoltPublisher, + mut battery_volt_adc: AdcHelper<'static, BatteryAdc, BatteryAdcPin>, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, @@ -125,6 +130,8 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); } + battery_volt_publisher.publish_immediate(battery_volt_adc.read_f32()); + // TODO read messages // update indicators @@ -158,7 +165,9 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, pub async fn start_io_task(spawner: &Spawner, robot_state: &'static SharedRobotState, - usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, + battery_volt_publisher: BatteryVoltPublisher, + battery_adc: BatteryAdc, battery_adc_pin: BatteryAdcPin, + _usr_btn0_pin: UsrBtn0Pin, _usr_btn1_pin: UsrBtn1Pin, _usr_btn0_exti: UsrBtn0Exti, _usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, @@ -193,6 +202,7 @@ pub async fn start_io_task(spawner: &Spawner, let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); + let battery_volt_adc = AdcHelper::new(battery_adc, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); - spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); + spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); } \ No newline at end of file diff --git a/lib-stm32-test/Cargo.lock b/lib-stm32-test/Cargo.lock index cfc13b71..dd2bb425 100644 --- a/lib-stm32-test/Cargo.lock +++ b/lib-stm32-test/Cargo.lock @@ -12,7 +12,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless 0.8.0", "num-traits", @@ -35,7 +35,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "futures-util", "heapless 0.7.17", @@ -160,9 +160,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -205,9 +205,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -215,27 +215,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -250,15 +250,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -300,7 +300,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -315,11 +315,11 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -332,7 +332,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -346,23 +346,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -373,7 +373,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", ] @@ -381,7 +381,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -396,7 +396,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embassy-time-driver", "embassy-usb-driver", @@ -426,7 +426,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -439,7 +452,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -457,7 +470,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "document-features", ] @@ -465,12 +478,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "defmt", ] @@ -478,10 +491,10 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5f9bc6def7ea8698a6ce45d8e12e1d1bd8cce876" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -688,9 +701,9 @@ dependencies = [ [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -746,9 +759,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -792,7 +805,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.22", + "semver 1.0.23", ] [[package]] @@ -818,9 +831,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.22" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -888,7 +901,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", "cortex-m-rt", @@ -896,9 +909,9 @@ dependencies = [ [[package]] name = "strsim" -version = "0.10.0" +version = "0.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" @@ -913,9 +926,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -924,22 +937,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index a26debe8..9c10e482 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -12,7 +12,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless", "num-traits", @@ -97,9 +97,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -133,9 +133,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -143,27 +143,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", "strsim", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -178,15 +178,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -220,10 +220,10 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -236,7 +236,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "critical-section 1.1.2", "document-features", @@ -246,23 +246,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -272,12 +272,12 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -290,7 +290,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", "embassy-usb-synopsys-otg", "embedded-can", @@ -318,7 +318,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -330,7 +343,7 @@ dependencies = [ [[package]] name = "embassy-time" version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -347,7 +360,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "document-features", ] @@ -355,20 +368,20 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#ecd7888ba937b74560d3a1975445441d7c07be17" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -575,9 +588,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -678,16 +691,16 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", ] [[package]] name = "strsim" -version = "0.10.0" +version = "0.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "syn" @@ -702,9 +715,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -713,22 +726,22 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/lib-stm32/src/drivers/flash/at25df041b.rs b/lib-stm32/src/drivers/flash/at25df041b.rs index dad54dc7..8f1205de 100644 --- a/lib-stm32/src/drivers/flash/at25df041b.rs +++ b/lib-stm32/src/drivers/flash/at25df041b.rs @@ -4,18 +4,18 @@ use embassy_stm32::{ spi::{self, Error} }; -pub struct AT25DF041B<'buf, T: spi::Instance, const CS_POL_N: bool> { - spi: spi::Spi<'static, T, Async>, +pub struct AT25DF041B<'buf, const CS_POL_N: bool> { + spi: spi::Spi<'static, Async>, chip_select: Output<'static>, tx_buf: &'buf mut [u8; 256], rx_buf: &'buf mut [u8; 256], } -impl<'buf, T: spi::Instance, const CS_POL_N: bool> AT25DF041B<'buf, T, CS_POL_N> { - pub fn new(spi: spi::Spi<'static, T, Async>, +impl<'buf, const CS_POL_N: bool> AT25DF041B<'buf, CS_POL_N> { + pub fn new(spi: spi::Spi<'static, Async>, chip_select: impl Pin, tx_buf: &'buf mut [u8; 256], - rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, T, CS_POL_N> { + rx_buf: &'buf mut [u8; 256]) -> AT25DF041B<'buf, CS_POL_N> { AT25DF041B { spi, chip_select: Output::new(chip_select, Level::High, Speed::High), diff --git a/lib-stm32/src/drivers/imu/bmi085.rs b/lib-stm32/src/drivers/imu/bmi085.rs index ea6bbc12..32f75d49 100644 --- a/lib-stm32/src/drivers/imu/bmi085.rs +++ b/lib-stm32/src/drivers/imu/bmi085.rs @@ -22,8 +22,8 @@ use defmt::*; pub const SPI_MIN_BUF_LEN: usize = 8; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro -pub struct Bmi085<'a, 'buf, SpiPeri: spi::Instance> { - spi: spi::Spi<'a, SpiPeri, Async>, +pub struct Bmi085<'a, 'buf> { + spi: spi::Spi<'a, Async>, accel_cs: Output<'a>, gyro_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], @@ -200,11 +200,11 @@ const GYRO_CHIP_ID: u8 = 0x0F; const READ_BIT: u8 = 0x80; -impl<'a, 'buf, SpiPeri: spi::Instance> - Bmi085<'a, 'buf, SpiPeri> { +impl<'a, 'buf> + Bmi085<'a, 'buf> { /// creates a new BMI085 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, accel_cs: Output<'a>, gyro_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], @@ -220,7 +220,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } ///t creates a new BMI085 instance from uninitialized pins - pub fn new_from_pins( + pub fn new_from_pins( peri: impl Peripheral

+ 'a, sck: impl Peripheral

> + 'a, mosi: impl Peripheral

> + 'a, diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index 69973866..e910d9bc 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -17,8 +17,8 @@ use embassy_stm32::{ pub const SPI_MIN_BUF_LEN: usize = 14; /// SPI driver for the Bosch BMI085 IMU: Accel + Gyro -pub struct Bmi323<'a, 'buf, SpiPeri: spi::Instance> { - spi: spi::Spi<'a, SpiPeri, Async>, +pub struct Bmi323<'a, 'buf> { + spi: spi::Spi<'a, Async>, spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], accel_mode: AccelMode, @@ -228,11 +228,11 @@ pub enum GyroMode { const BMI323_CHIP_ID: u16 = 0x0043; const READ_BIT: u8 = 0x80; -impl<'a, 'buf, SpiPeri: spi::Instance> - Bmi323<'a, 'buf, SpiPeri> { +impl<'a, 'buf> + Bmi323<'a, 'buf> { /// creates a new BMI323 instance from a pre-existing Spi peripheral pub fn new_from_spi( - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, spi_cs: Output<'a>, spi_buf: &'buf mut [u8; SPI_MIN_BUF_LEN], ) -> Self { @@ -254,7 +254,7 @@ impl<'a, 'buf, SpiPeri: spi::Instance> } ///t creates a new BMI085 instance from uninitialized pins - pub fn new_from_pins( + pub fn new_from_pins( peri: impl Peripheral

+ 'a, sck_pin: impl Peripheral

> + 'a, mosi_pin: impl Peripheral

> + 'a, diff --git a/lib-stm32/src/drivers/mod.rs b/lib-stm32/src/drivers/mod.rs index a6106cbf..71322485 100644 --- a/lib-stm32/src/drivers/mod.rs +++ b/lib-stm32/src/drivers/mod.rs @@ -3,4 +3,5 @@ pub mod flash; pub mod imu; pub mod led; pub mod radio; -pub mod switches; \ No newline at end of file +pub mod switches; +pub mod other; \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs new file mode 100644 index 00000000..c9129ebb --- /dev/null +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -0,0 +1,39 @@ + +use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime, VrefInt, VREF_DEFAULT_MV}; +use embassy_stm32::Peripheral; + +pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { + inst: Adc<'a, T>, + channel_pin: Ch, + channel_vref: VrefInt +} + +impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> { + pub fn new( + peri: impl Peripheral

+ 'a, + channel_pin: Ch, + sample_time: SampleTime, + resolution: Resolution + ) -> Self { + + let mut adc_inst = Adc::new(peri); + + adc_inst.set_sample_time(sample_time); + adc_inst.set_resolution(resolution); + + let channel_vref = adc_inst.enable_vrefint(); + + AdcHelper { + inst: adc_inst, + channel_pin: channel_pin, + channel_vref: channel_vref + } + } + + pub fn read_f32(&mut self) -> f32 { + // Gets the Vref since it changes based on chip class. + let vref_cur_mv = self.inst.read(&mut self.channel_vref) as f32; + // Scale by Vref to convert to absolute voltage. + return ((VREF_DEFAULT_MV as f32) / vref_cur_mv) * (self.inst.read(&mut self.channel_pin) as f32); + } +} \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/mod.rs b/lib-stm32/src/drivers/other/mod.rs new file mode 100644 index 00000000..1e9398cf --- /dev/null +++ b/lib-stm32/src/drivers/other/mod.rs @@ -0,0 +1 @@ +pub mod adc_helper; \ No newline at end of file diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 51d252bd..84d227ef 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -46,7 +46,7 @@ pub struct PeerConnection { pub struct OdinW262< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, TxDma: usart::TxDma, RxDma: usart::RxDma, const LEN_TX: usize, @@ -61,7 +61,7 @@ pub struct OdinW262< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, TxDma: usart::TxDma, RxDma: usart::RxDma, const LEN_TX: usize, diff --git a/lib-stm32/src/uart/queue.rs b/lib-stm32/src/uart/queue.rs index b4eda235..f254c64f 100644 --- a/lib-stm32/src/uart/queue.rs +++ b/lib-stm32/src/uart/queue.rs @@ -110,7 +110,7 @@ pub enum UartTaskCommand { } pub struct UartReadQueue< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -123,7 +123,7 @@ pub struct UartReadQueue< // TODO: pretty sure shouldn't do this unsafe impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -132,7 +132,7 @@ unsafe impl< } pub type ReadTaskFuture< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -140,7 +140,7 @@ pub type ReadTaskFuture< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::RxDma, const LENGTH: usize, const DEPTH: usize, @@ -158,7 +158,7 @@ impl< fn read_task( &'static self, queue_rx: &'static Queue, - mut rx: UartRx<'static, UART, Async>, + mut rx: UartRx<'static, Async>, mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> ReadTaskFuture { async move { @@ -237,7 +237,7 @@ impl< pub fn spawn_task( &'static self, - rx: UartRx<'static, UART, Async>, + rx: UartRx<'static, Async>, uart_config_signal_subscriber: UartQueueConfigSyncSub ) -> SpawnToken { self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_subscriber)) @@ -245,7 +245,7 @@ impl< pub fn spawn_task_with_pubsub( &'static self, - rx: UartRx<'static, UART, Async>, + rx: UartRx<'static, Async>, uart_config_signal_pubsub: &'static UartQueueSyncPubSub ) -> SpawnToken { self.task.spawn(|| self.read_task(&self.queue_rx, rx, uart_config_signal_pubsub.subscriber().unwrap())) @@ -266,7 +266,7 @@ impl< } pub struct UartWriteQueue< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -280,7 +280,7 @@ pub struct UartWriteQueue< } type WriteTaskFuture< - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -288,7 +288,7 @@ type WriteTaskFuture< impl< 'a, - UART: usart::BasicInstance, + UART: usart::Instance, DMA: usart::TxDma, const LENGTH: usize, const DEPTH: usize, @@ -321,7 +321,7 @@ impl< fn write_task( &'static self, queue_tx: &'static Queue, - mut tx: UartTx<'static, UART, Async>, + mut tx: UartTx<'static, Async>, uart_config_signal_publisher: UartQueueConfigSyncPub, mut uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> WriteTaskFuture { @@ -404,7 +404,7 @@ impl< } pub fn spawn_task(&'static self, - tx: UartTx<'static, UART, Async>, + tx: UartTx<'static, Async>, uart_config_signal_publisher: UartQueueConfigSyncPub, uart_config_signal_subscriber: UartQueueConfigSyncSub, ) -> SpawnToken { @@ -412,7 +412,7 @@ impl< } pub fn spawn_task_with_pubsub(&'static self, - tx: UartTx<'static, UART, Async>, + tx: UartTx<'static, Async>, uart_config_signal_pubsub: &'static UartQueueSyncPubSub ) -> SpawnToken { self.task.spawn(|| self.write_task(&self.queue_tx, tx, uart_config_signal_pubsub.publisher().unwrap(), uart_config_signal_pubsub.subscriber().unwrap())) @@ -490,7 +490,7 @@ pub trait Writer { } impl< - UART: usart::BasicInstance, + UART: usart::Instance, Dma: usart::RxDma, const LEN: usize, const DEPTH: usize, @@ -502,7 +502,7 @@ impl< } impl< - UART: usart::BasicInstance, + UART: usart::Instance, Dma: usart::TxDma, const LEN: usize, const DEPTH: usize, From b00dcf9623554ba7eb93b297d4c15cfb2db68ddc Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 11 Jun 2024 21:07:56 -0700 Subject: [PATCH 070/157] Got compile --- control-board/src/bin/control/main.rs | 9 +- control-board/src/pins.rs | 4 +- control-board/src/tasks/kicker_task.rs | 4 +- control-board/src/tasks/user_io_task.rs | 12 +- kicker-board/Cargo.lock | 157 ++++++++++++---------- lib-stm32/src/drivers/led/apa102.rs | 18 +-- lib-stm32/src/drivers/other/adc_helper.rs | 5 +- 7 files changed, 119 insertions(+), 90 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index eeeb48e1..c4fcff4d 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -9,7 +9,7 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; +use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; // load credentials from correct crate @@ -29,6 +29,7 @@ static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -77,6 +78,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); + // Battery Channel + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + let mut battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); + // TODO imu channel let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); @@ -104,6 +109,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_io_task!(main_spawner, robot_state, + battery_volt_publisher, p); create_shutdown_task!(main_spawner, @@ -132,6 +138,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; loop { + defmt::info!("{}", battery_volt_subscriber.next_message_pure().await); Timer::after_millis(10).await; } } \ No newline at end of file diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index c2316765..9d9fbd30 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -68,8 +68,8 @@ pub type ImuSpiInt2Pin = PB2; pub type ImuSpiInt1Exti = EXTI1; pub type ImuSpiInt2Exti = EXTI2; pub type ExtImuNDetPin = PF11; -pub type BatteryAdcPin = PF5; -pub type BatteryAdc = ADC3; +pub type BatteryAdcPin = PF14; +pub type BatteryAdc = ADC2; ////////////// // Kicker // diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 2e664332..fedef902 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -31,7 +31,7 @@ enum KickerTaskState { } pub struct KickerTask<'a, - UART: usart::BasicInstance, + UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, @@ -46,7 +46,7 @@ pub struct KickerTask<'a, } impl<'a, -UART: usart::BasicInstance, +UART: usart::Instance, DmaRx: usart::RxDma, DmaTx: usart::TxDma, const LEN_RX: usize, diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index f66b0204..95709f60 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -32,9 +32,11 @@ static mut DOTSTAR_SPI_BUFFER_CELL: [u8; 16] = [0; 16]; #[macro_export] macro_rules! create_io_task { - ($spawner:ident, $robot_state:ident, $p:ident) => { + ($spawner:ident, $robot_state:ident, $battery_volt_publisher:ident, $p:ident) => { ateam_control_board::tasks::user_io_task::start_io_task(&$spawner, $robot_state, + $battery_volt_publisher, + $p.ADC2, $p.PF14, $p.PD6, $p.PD5, $p.EXTI6, $p.EXTI5, $p.PG7, $p.PG6, $p.PG5, $p.PG4, $p.PG3, $p.PG2, $p.PD15, $p.PG12, $p.PG11, $p.PG10, $p.PG9, @@ -55,7 +57,7 @@ async fn user_io_task_entry( robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, mut robot_id_indicator: ShellIndicator<'static>, - mut dotstars: Apa102<'static, 'static, DotstarSpi, 2>, + mut dotstars: Apa102<'static, 'static, 2>, ) { defmt::info!("user io task initialized"); @@ -167,7 +169,7 @@ pub async fn start_io_task(spawner: &Spawner, robot_state: &'static SharedRobotState, battery_volt_publisher: BatteryVoltPublisher, battery_adc: BatteryAdc, battery_adc_pin: BatteryAdcPin, - _usr_btn0_pin: UsrBtn0Pin, _usr_btn1_pin: UsrBtn1Pin, _usr_btn0_exti: UsrBtn0Exti, _usr_btn1_exti: UsrBtn1Exti, + usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, robot_id_selector3_pin: RobotIdSelector3Pin, robot_id_selector2_pin: RobotIdSelector2Pin, @@ -187,7 +189,7 @@ pub async fn start_io_task(spawner: &Spawner, // defmt::info!("took buf"); let dotstar_spi_buf: &'static mut [u8; 16] = unsafe { &mut DOTSTAR_SPI_BUFFER_CELL }; - let dotstars = Apa102::<_, 2>::new_from_pins(dotstar_peri, dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, dotstar_spi_buf.into()); + let dotstars = Apa102::<2>::new_from_pins(dotstar_peri, dotstar_sck_pin, dotstar_mosi_pin, dotstar_tx_dma, dotstar_spi_buf.into()); let adv_usr_btn0: AdvExtiButton = AdvExtiButton::new_from_pins(usr_btn0_pin, usr_btn0_exti, false); let adv_usr_btn1: AdvExtiButton = AdvExtiButton::new_from_pins(usr_btn1_pin, usr_btn1_exti, false); @@ -204,5 +206,5 @@ pub async fn start_io_task(spawner: &Spawner, let battery_volt_adc = AdcHelper::new(battery_adc, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); - spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator)).unwrap(); + spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); } \ No newline at end of file diff --git a/kicker-board/Cargo.lock b/kicker-board/Cargo.lock index cf4bdad3..eb1297cf 100644 --- a/kicker-board/Cargo.lock +++ b/kicker-board/Cargo.lock @@ -46,7 +46,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "futures-util", "heapless 0.7.17", @@ -66,7 +66,7 @@ dependencies = [ "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync", + "embassy-sync 0.5.0", "embassy-time", "heapless 0.8.0", "num-traits", @@ -164,9 +164,9 @@ checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" [[package]] name = "bytemuck" -version = "1.15.0" +version = "1.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" +checksum = "78834c15cb5d5efe3452d58b1e8ba890dd62d21907f867f383358198e56ebca5" [[package]] name = "byteorder" @@ -200,9 +200,9 @@ dependencies = [ [[package]] name = "clang-sys" -version = "1.7.0" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" dependencies = [ "glob", "libc", @@ -219,7 +219,7 @@ dependencies = [ "bitflags 1.3.2", "clap_lex", "indexmap", - "strsim", + "strsim 0.10.0", "termcolor", "textwrap", ] @@ -268,9 +268,9 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.4" +version = "0.7.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2722f5b7d6ea8583cffa4d247044e280ccbb9fe501bed56552e2ba48b02d5f3d" +checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" dependencies = [ "cortex-m-rt-macros", ] @@ -313,9 +313,9 @@ checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" [[package]] name = "darling" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54e36fcd13ed84ffdfda6f5be89b31287cbb80c439841fe69e04841435464391" +checksum = "83b2eb4d90d12bdda5ed17de686c2acb4c57914f8f921b8da7e112b5a36f3fe1" dependencies = [ "darling_core", "darling_macro", @@ -323,27 +323,27 @@ dependencies = [ [[package]] name = "darling_core" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c2cf1c23a687a1feeb728783b993c4e1ad83d99f351801977dd809b48d0a70f" +checksum = "622687fe0bac72a04e5599029151f5796111b90f1baaa9b544d807a5e31cd120" dependencies = [ "fnv", "ident_case", "proc-macro2", "quote", - "strsim", - "syn 2.0.60", + "strsim 0.11.1", + "syn 2.0.66", ] [[package]] name = "darling_macro" -version = "0.20.8" +version = "0.20.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a668eda54683121533a393014d8692171709ff57a7d61f187b6e782719f8933f" +checksum = "733cabb43482b1a1b53eee8583c2b9e8684d592215ea83efd305dd31bc2f0178" dependencies = [ "darling_core", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -358,15 +358,15 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.7" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "18bdc7a7b92ac413e19e95240e75d3a73a8d8e78aa24a594c22cbb4d44b4bbda" +checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" dependencies = [ "defmt-parser", "proc-macro-error", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -408,7 +408,7 @@ checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] @@ -422,18 +422,18 @@ dependencies = [ [[package]] name = "either" -version = "1.11.0" +version = "1.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a47c1c47d2f5964e29c61246e81db715514cd532db6b5116a25ea3c03d6780a2" +checksum = "3dca9240753cf90908d7e4aac30f630662b02aebaa1b58a3cadabdb23385b58b" [[package]] name = "embassy-embedded-hal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", "embassy-futures", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -446,7 +446,7 @@ dependencies = [ [[package]] name = "embassy-executor" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -460,23 +460,23 @@ dependencies = [ [[package]] name = "embassy-executor-macros" version = "0.4.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "darling", "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] name = "embassy-futures" version = "0.1.1" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-hal-internal" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cortex-m", "critical-section 1.1.2", @@ -487,7 +487,7 @@ dependencies = [ [[package]] name = "embassy-net-driver" version = "0.2.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -495,7 +495,7 @@ dependencies = [ [[package]] name = "embassy-stm32" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "bit_field", "bitflags 2.5.0", @@ -510,7 +510,7 @@ dependencies = [ "embassy-futures", "embassy-hal-internal", "embassy-net-driver", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-time", "embassy-time-driver", "embassy-usb-driver", @@ -540,7 +540,20 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.5.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd938f25c0798db4280fcd8026bf4c2f48789aebf8f77b6e5cf8a7693ba114ec" +dependencies = [ + "cfg-if", + "critical-section 1.1.2", + "embedded-io-async", + "futures-util", + "heapless 0.8.0", +] + +[[package]] +name = "embassy-sync" +version = "0.6.0" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -552,8 +565,8 @@ dependencies = [ [[package]] name = "embassy-time" -version = "0.3.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +version = "0.3.1" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "cfg-if", "critical-section 1.1.2", @@ -571,7 +584,7 @@ dependencies = [ [[package]] name = "embassy-time-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "document-features", ] @@ -579,12 +592,12 @@ dependencies = [ [[package]] name = "embassy-time-queue-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" [[package]] name = "embassy-usb-driver" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "defmt", ] @@ -592,10 +605,10 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" version = "0.1.0" -source = "git+https://github.com/embassy-rs/embassy#15c3ae8ef6abaf37704e3278a1de6b2ae259aa15" +source = "git+https://github.com/embassy-rs/embassy#5154de3b7e5068b227e48480b8ac71c54447c264" dependencies = [ "critical-section 1.1.2", - "embassy-sync", + "embassy-sync 0.6.0", "embassy-usb-driver", ] @@ -692,9 +705,9 @@ dependencies = [ [[package]] name = "errno" -version = "0.3.8" +version = "0.3.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245" +checksum = "534c5cf6194dfab3db3242765c03bbe257cf92f22b38f6bc0c58d59108a820ba" dependencies = [ "libc", "windows-sys", @@ -837,9 +850,9 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.154" +version = "0.2.155" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" +checksum = "97b3888a4aecf77e811145cadf6eef5901f4782c53886191b2f693f24761847c" [[package]] name = "libloading" @@ -859,9 +872,9 @@ checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" [[package]] name = "linux-raw-sys" -version = "0.4.13" +version = "0.4.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c" +checksum = "78b3ae25bc7c8c38cec158d1f2757ee79e9b3740fbc7ccf0e59e4b08d793fa89" [[package]] name = "litrs" @@ -945,9 +958,9 @@ checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" [[package]] name = "panic-probe" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", "defmt", @@ -1009,9 +1022,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.81" +version = "1.0.85" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d1597b0c024618f09a9c3b8655b7e430397a36d23fdafec26d6965e9eec3eba" +checksum = "22244ce15aa966053a896d1accb3a6e68469b97c7f33f284b99f0d576879fc23" dependencies = [ "unicode-ident", ] @@ -1033,9 +1046,9 @@ checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" [[package]] name = "regex" -version = "1.10.4" +version = "1.10.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +checksum = "b91213439dad192326a0d7c6ee3955910425f441d7038e0d6933b0aec5c4517f" dependencies = [ "aho-corasick", "memchr", @@ -1045,9 +1058,9 @@ dependencies = [ [[package]] name = "regex-automata" -version = "0.4.6" +version = "0.4.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +checksum = "38caf58cc5ef2fed281f89292ef23f6365465ed9a41b7a7754eb4e26496c92df" dependencies = [ "aho-corasick", "memchr", @@ -1056,9 +1069,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.8.3" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" +checksum = "7a66a03ae7c801facd77a29370b4faec201768915ac14a721ba36f20bc9c209b" [[package]] name = "rgb" @@ -1090,7 +1103,7 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" dependencies = [ - "semver 1.0.22", + "semver 1.0.23", ] [[package]] @@ -1129,9 +1142,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.22" +version = "1.0.23" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "92d43fe69e652f3df9bdc2b85b2854a0825b86e4fb76bc44d945137d053639ca" +checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" [[package]] name = "semver-parser" @@ -1205,7 +1218,7 @@ dependencies = [ [[package]] name = "stm32-metapac" version = "15.0.0" -source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-f1297385e91f061fcb6134bb25f51e12d8abff93#acfef07196084706a9621a434d927229b616951f" +source = "git+https://github.com/embassy-rs/stm32-data-generated?tag=stm32-data-59b1f65bd109c3ef35782e6c44062208d0ef3d0e#2ad339796e0f40bd6fa784dbfe30a071d93cc886" dependencies = [ "cortex-m", "cortex-m-rt", @@ -1217,6 +1230,12 @@ version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + [[package]] name = "syn" version = "1.0.109" @@ -1230,9 +1249,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.60" +version = "2.0.66" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "909518bc7b1c9b779f1bbf07f2929d35af9f0f37e47c6e9ef7f9dddc1e1821f3" +checksum = "c42f3f41a2de00b01c0aaad383c5a45241efc8b2d1eda5661812fda5f3cdcff5" dependencies = [ "proc-macro2", "quote", @@ -1256,22 +1275,22 @@ checksum = "23d434d3f8967a09480fb04132ebe0a3e088c173e6d0ee7897abbdf4eab0f8b9" [[package]] name = "thiserror" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" +checksum = "c546c80d6be4bc6a00c0f01730c08df82eaa7a7a61f11d656526506112cc1709" dependencies = [ "thiserror-impl", ] [[package]] name = "thiserror-impl" -version = "1.0.59" +version = "1.0.61" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" +checksum = "46c3384250002a6d5af4d114f2845d37b57521033f30d5c3f46c4d70e1197533" dependencies = [ "proc-macro2", "quote", - "syn 2.0.60", + "syn 2.0.66", ] [[package]] diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index dc86c1eb..27a52c48 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -7,16 +7,16 @@ use smart_leds::RGB8; use crate::anim::{AnimInterface, Blink, CompositeAnimation, Lerp}; -pub struct Apa102<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> +pub struct Apa102<'a, 'buf, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], } -impl<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102<'a, 'buf, SpiPeri, NUM_LEDS> +impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> where [(); (NUM_LEDS * 4) + 8]: { pub fn new( - spi: spi::Spi<'a, SpiPeri, Async>, + spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], ) -> Self { // set start frame @@ -37,7 +37,7 @@ where [(); (NUM_LEDS * 4) + 8]: { } } - pub fn new_from_pins( + pub fn new_from_pins( peri: impl Peripheral

+ 'a, sck_pin: impl Peripheral

> + 'a, mosi_pin: impl Peripheral

> + 'a, @@ -114,15 +114,15 @@ where [(); (NUM_LEDS * 4) + 8]: { } } -pub struct Apa102Anim<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> +pub struct Apa102Anim<'a, 'buf, 'ca, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { - apa102_driver: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>, + apa102_driver: Apa102<'a, 'buf, NUM_LEDS>, animation_buf: [Option<&'ca mut CompositeAnimation<'ca, u8, RGB8>>; NUM_LEDS], } -impl<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, SpiPeri, NUM_LEDS> +impl<'a, 'buf, 'ca, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, NUM_LEDS> where [(); (NUM_LEDS * 4) + 8]: { - pub fn new(apa102: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>) -> Self { + pub fn new(apa102: Apa102<'a, 'buf, NUM_LEDS>) -> Self { Apa102Anim { apa102_driver: apa102, animation_buf: [const { None }; NUM_LEDS], diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index c9129ebb..4a6dc94d 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -2,13 +2,14 @@ use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime, VrefInt, VREF_DEFAULT_MV}; use embassy_stm32::Peripheral; -pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { +pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> where VrefInt: AdcChannel { inst: Adc<'a, T>, channel_pin: Ch, channel_vref: VrefInt } -impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> { +impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> +where VrefInt: AdcChannel { pub fn new( peri: impl Peripheral

+ 'a, channel_pin: Ch, From dc77cc2246699ab916e86f3e7cd39ff62b75672e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 12 Jun 2024 00:08:32 -0400 Subject: [PATCH 071/157] kicker bootloader works --- control-board/.cargo/config.toml | 4 +- .../src/bin/hwtest-kicker-coms/main.rs | 33 ++- control-board/src/stm32_interface.rs | 42 +++- kicker-board/Cargo.toml | 5 + kicker-board/src/bin/hwtest-coms/main.rs | 4 +- kicker-board/src/bin/kicker-no-uart/main.rs | 209 ++++++++++++++++++ 6 files changed, 263 insertions(+), 34 deletions(-) create mode 100644 kicker-board/src/bin/kicker-no-uart/main.rs diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index 6e4a803b..e487328e 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -2,7 +2,9 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] -runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' +# runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' +runner = 'probe-rs run --chip STM32H723ZGTx' + [env] # DEFMT_LOG = "error,ateam-control-board::tasks::control_task=trace" diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index ae669a09..34c4a658 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -17,7 +17,7 @@ use embassy_stm32::{ use embassy_time::{Duration, Ticker, Timer}; use panic_probe as _; -include_kicker_bin! {KICKER_FW_IMG, "hwtest-blinky.bin"} +include_kicker_bin! {KICKER_FW_IMG, "hwtest-coms.bin"} const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; @@ -55,12 +55,18 @@ async fn main(_spawner: embassy_executor::Spawner) { let mut kicker_pwr_pin = Output::new(p.PG8, Level::Low, Speed::Medium); - // defmt::info!("attempting to power on kicker."); - // Timer::after_millis(1000).await; - // kicker_pwr_pin.set_high(); - // Timer::after_millis(200).await; - // kicker_pwr_pin.set_low(); - // defmt::info!("power on attempt done"); + kicker_pwr_pin.set_high(); + defmt::info!("force power off kicker"); + Timer::after_millis(2000).await; + kicker_pwr_pin.set_low(); + defmt::info!("kicker force power off done"); + + defmt::info!("attempting to power on kicker."); + Timer::after_millis(1000).await; + kicker_pwr_pin.set_high(); + Timer::after_millis(200).await; + kicker_pwr_pin.set_low(); + defmt::info!("power on attempt done"); // loop { // Timer::after_millis(1000).await; @@ -104,19 +110,6 @@ async fn main(_spawner: embassy_executor::Spawner) { info!("kicker flash complete"); } - Timer::after_millis(3000).await; - - // defmt::info!("attempting to power off kicker."); - // Timer::after_millis(1000).await; - // kicker_pwr_pin.set_high(); - // Timer::after_millis(2000).await; - // kicker_pwr_pin.set_low(); - // defmt::info!("power off attempt done"); - - loop { - Timer::after_millis(1000).await; - } - kicker.set_telemetry_enabled(true); let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 0f90bc8f..43027bf2 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -5,7 +5,7 @@ use defmt::*; use embassy_stm32::gpio::{Level, Output, Pin, Speed}; use embassy_stm32::pac; -use embassy_stm32::usart::{self, Parity, Config}; +use embassy_stm32::usart::{self, Config, Parity, StopBits}; use embassy_time::{Duration, Timer}; use embassy_time::with_timeout; @@ -35,6 +35,7 @@ pub fn get_bootloader_uart_config() -> Config { let mut config = usart::Config::default(); config.baudrate = 115_200; // max officially support baudrate config.parity = Parity::ParityEven; + config.stop_bits = StopBits::STOP1; config } @@ -136,10 +137,19 @@ impl< pub async fn hard_reset(&mut self) { self.enter_reset().await; + // Timer::after_millis(1).await; self.leave_reset().await; + // Timer::after_millis(1).await; } pub async fn reset_into_bootloader(&mut self) -> Result<(), ()> { + // ensure UART is in the expected config for the bootloader + // this operation is unsafe because it takes the uart module offline + // when the executor may be relying on rx interrupts to unblock a thread + self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven).await; + Timer::after_millis(100).await; + + // set the boot0 line high to enter the UART bootloader upon reset self.boot0_pin.set_high(); Timer::after_millis(1).await; @@ -151,21 +161,23 @@ impl< // this time isn't documented and can possibly be lowered. Timer::after(Duration::from_millis(10)).await; - // ensure UART is in the expected config for the bootloader - // this operation is unsafe because it takes the uart module offline - // when the executor may be relying on rx interrupts to unblock a thread - self.update_uart_config(STM32_BOOTLOADER_MAX_BAUD_RATE, Parity::ParityEven).await; + defmt::debug!("sending the bootloader baud calibration command..."); - self.writer + Timer::after_millis(1000).await; + if self.writer .write(|buf| { buf[0] = STM32_BOOTLOADER_CODE_SEQUENCE_BYTE; 1 }) - .await?; + .await.is_err() { + defmt::debug!("failed to send bootloader start seq"); + return Err(()); + } + Timer::after_millis(10).await; let mut res = Err(()); - let sync_res = with_timeout(Duration::from_millis(100), self.reader.read(|buf| { + let sync_res = with_timeout(Duration::from_millis(5000), self.reader.read(|buf| { if buf.len() >= 1 { if buf[0] == STM32_BOOTLOADER_ACK { defmt::debug!("bootloader replied with ACK after calibration."); @@ -510,10 +522,20 @@ impl< return Err(err); } - // let device_id = self.get_device_id().await; match self.get_device_id().await { Err(err) => return Err(err), - Ok(device_id) => if device_id != 68 { return Err(()) } + Ok(device_id) => match device_id { + 68 => { + defmt::trace!("found stm32f1 device"); + } + 19 => { + defmt::trace!("found stm32f40xxx device"); + } + _ => { + defmt::trace!("found unknown device id {}", device_id); + return Err(()); + } + } }; // erase part diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index 03125c00..c12513b3 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -95,6 +95,11 @@ name = "kicker" test = false harness = false +[[bin]] +name = "kicker-no-uart" +test = false +harness = false + [patch.crates-io] embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 75dc93f0..e9118b23 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -9,8 +9,6 @@ use static_cell::StaticCell; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m_rt::entry; - use embassy_executor::{Executor, InterruptExecutor, Spawner}; use embassy_stm32::{ adc::{Adc, Resolution, SampleTime}, @@ -47,7 +45,7 @@ make_uart_queue_pair!(COMS, ComsUartModule, ComsUartRxDma, ComsUartTxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); + #[link_section = ".bss"]); fn get_empty_control_packet() -> KickerControl { diff --git a/kicker-board/src/bin/kicker-no-uart/main.rs b/kicker-board/src/bin/kicker-no-uart/main.rs new file mode 100644 index 00000000..5ddb5cb2 --- /dev/null +++ b/kicker-board/src/bin/kicker-no-uart/main.rs @@ -0,0 +1,209 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] + +use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::get_system_config}; +use static_cell::StaticCell; + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use libm::{fmaxf, fminf}; + +use embassy_executor::{Executor, InterruptExecutor, Spawner}; +use embassy_stm32::{ + adc::{Adc, SampleTime}, + bind_interrupts, + gpio::{Input, Level, Output, Pull, Speed}, + interrupt, + pac::Interrupt, + peripherals, +}; +use embassy_time::{Duration, Instant, Ticker, Timer}; + +use ateam_kicker_board::{ + adc_raw_to_v, adc_200v_to_rail_voltage, + kick_manager::{KickManager, KickType}, + pins::{ + BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, + PowerRail200vReadPin, KickPin, RedStatusLedPin, + }, +}; + + +use ateam_common_packets::bindings_kicker::{ + KickRequest::{self, KR_ARM, KR_DISABLE}, + KickerControl, KickerTelemetry, +}; + +const MAX_TX_PACKET_SIZE: usize = 16; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 16; +const RX_BUF_DEPTH: usize = 3; + +const MAX_KICK_SPEED: f32 = 5.5; +const SHUTDOWN_KICK_DUTY: f32 = 0.20; + +pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; +pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; +pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; +pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; + +fn get_empty_control_packet() -> KickerControl { + KickerControl { + _bitfield_align_1: [], + _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), + kick_request: KickRequest::KR_DISABLE, + kick_speed: 0.0, + } +} + +fn get_empty_telem_packet() -> KickerTelemetry { + KickerTelemetry { + _bitfield_align_1: [], + _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), + rail_voltage: 0.0, + battery_voltage: 0.0, + } +} + +#[embassy_executor::task] +async fn high_pri_kick_task( + mut ball_detected_output: Output<'static>, + kick_commanded_input: Input<'static>, + mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, + charge_pin: ChargePin, + kick_pin: KickPin, + chip_pin: ChipPin, + breakbeam_tx: BreakbeamLeftAgpioPin, + breakbeam_rx: BreakbeamRightAgpioPin, + mut rail_pin: PowerRail200vReadPin, + err_led_pin: RedStatusLedPin, + ball_detected_led1_pin: BlueStatusLed1Pin, + ball_detected_led2_pin: BlueStatusLed2Pin, +) -> ! { + // pins/safety management + let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); + let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); + let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); + let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); + + // debug LEDs + let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); + let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); + + // TODO dotstars + + let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); + + // coms buffers + let mut telemetry_enabled: bool; // = false; + let mut kicker_control_packet: KickerControl = get_empty_control_packet(); + + // bookkeeping for latched state + let mut kick_command_cleared: bool = false; + let mut latched_command = KickRequest::KR_DISABLE; + let mut error_latched: bool = false; + let mut charge_hv_rail: bool = false; + + // power down status + let mut shutdown_requested: bool = false; + let mut shutdown_completed: bool = false; + + // loop rate control + let mut ticker = Ticker::every(Duration::from_millis(1)); + + breakbeam.enable_tx(); + loop { + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint); + + let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + // let battery_voltage = + // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); + // optionally pre-flag errors? + let battery_voltage = 22.5; + + ///////////////////////////////////// + // process any available packets // + ///////////////////////////////////// + + let kick_commanded = kick_commanded_input.is_high(); + + // TODO: read breakbeam + let ball_detected = breakbeam.read(); + + /////////////////////////////////////////////// + // manage repetitive kick commands + state // + /////////////////////////////////////////////// + + let kick_command = + + kick_manager.command(battery_voltage, rail_voltage, charge_hv_rail, kick_command, kick_strength).await; + + // TODO write ball detected GPIO + + // LEDs + if error_latched { + err_led.set_high(); + } else { + err_led.set_low(); + } + + if ball_detected { + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); + + } else { + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); + } + // TODO Dotstar + + // loop rate control @1KHz + ticker.next().await; + } +} + +static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[interrupt] +unsafe fn TIM2() { + EXECUTOR_HIGH.on_interrupt(); +} + +bind_interrupts!(struct Irqs { + USART1 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) -> ! { + let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); + let p = embassy_stm32::init(sys_cfg); + + info!("kicker startup!"); + + let _status_led = Output::new(p.PA11, Level::High, Speed::Low); + + let ball_detected_output = Output::new(p.PA1, Level::Low, Speed::Low); + let kick_commanded_input = Input::new(p.PA1, Pull::Down); + + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); + + // high priority executor handles kicking system + // High-priority executor: I2C1, priority level 6 + // TODO CHECK THIS IS THE HIGHEST PRIORITY + embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); + let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + unwrap!(spawner.spawn(high_pri_kick_task(kick_commanded_input, ball_detected_output, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); + + loop { + Timer::after_millis(1000).await; + } +} From f1fe3bef70077f471206d2059c27278d538b50d9 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 14 Jun 2024 22:12:07 -0400 Subject: [PATCH 072/157] clean up control firmware --- control-board/Cargo.toml | 5 - control-board/src/bin/control-old/main.rs | 531 ------------------ control-board/src/bin/control/main.rs | 17 +- .../src/bin/hwtest-kicker-coms/main.rs | 4 +- control-board/src/bin/hwtest-piezo/main.rs | 4 +- control-board/src/bin/kicker/main.rs | 40 -- 6 files changed, 5 insertions(+), 596 deletions(-) delete mode 100644 control-board/src/bin/control-old/main.rs delete mode 100644 control-board/src/bin/kicker/main.rs diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index eba17d69..57877990 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -111,11 +111,6 @@ name = "hwtest-radio" test = false harness = false -[[bin]] -name = "kicker" -test = false -harness = false - [[test]] name = "basic-test" harness = false diff --git a/control-board/src/bin/control-old/main.rs b/control-board/src/bin/control-old/main.rs deleted file mode 100644 index 3a3fe7a7..00000000 --- a/control-board/src/bin/control-old/main.rs +++ /dev/null @@ -1,531 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(async_closure)] -#![feature(sync_unsafe_cell)] - -use apa102_spi::Apa102; -use ateam_control_board::{ - adc_raw_to_v, adc_v_to_battery_voltage, drivers::{ - kicker::Kicker, radio::{TeamColor, WifiNetwork}, rotary::Rotary, shell_indicator::ShellIndicator - }, get_system_config, include_kicker_bin, parameter_interface::ParameterInterface, stm32_interface::{get_bootloader_uart_config, Stm32Interface}, BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE -}; -use control::Control; -use defmt::info; -use embassy_stm32::{ - adc::{Adc, SampleTime}, bind_interrupts, exti::ExtiInput, gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, peripherals::{self, BDMA_CH0}, spi, time::{hz, mhz}, usart::{self, Uart}, wdg::IndependentWatchdog -}; -use embassy_executor::InterruptExecutor; -use embassy_time::{Duration, Ticker, Timer}; -use ateam_control_board::pins::{ - PowerStateExti, PowerStatePin, RadioReset, RadioRxDMA, RadioTxDMA, RadioUART, - ShutdownCompletePin, -}; - -use smart_leds::{SmartLedsWrite, RGB8}; - -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_sync::pubsub::PubSubChannel; - -use ateam_lib_stm32::make_uart_queue_pair; - -use ateam_control_board::motion::tasks::control; -use ateam_control_board::pins::*; -use ateam_control_board::radio::RadioTest; - -#[cfg(not(feature = "no-private-credentials"))] -use credentials::private_credentials::wifi::wifi_credentials; -#[cfg(feature = "no-private-credentials")] -use credentials::public_credentials::wifi::wifi_credentials; -use static_cell::ConstStaticCell; - -// Uncomment for testing: -// use panic_probe as _; - -#[panic_handler] -fn panic(info: &core::panic::PanicInfo) -> ! { - defmt::error!("{}", defmt::Display2Format(info)); - // Delay to give it a change to print - cortex_m::asm::delay(10_000_000); - cortex_m::peripheral::SCB::sys_reset(); -} - -include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} - -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; -pub const RADIO_TX_BUF_DEPTH: usize = 4; -pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; -pub const RADIO_RX_BUF_DEPTH: usize = 4; - -pub const KICKER_MAX_TX_PACKET_SIZE: usize = 256; -pub const KICKER_TX_BUF_DEPTH: usize = 4; -pub const KICKER_MAX_RX_PACKET_SIZE: usize = 256; -pub const KICKER_RX_BUF_DEPTH: usize = 4; - -make_uart_queue_pair!(RADIO, - RadioUART, RadioRxDMA, RadioTxDMA, - RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, - RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(KICKER, - KickerUart, KickerRxDma, KickerTxDma, - KICKER_MAX_RX_PACKET_SIZE, KICKER_RX_BUF_DEPTH, - KICKER_MAX_TX_PACKET_SIZE, KICKER_TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -#[link_section = ".sram4"] -static mut SPI6_BUF: [u8; 4] = [0x0; 4]; - -static RADIO_TEST: ConstStaticCell> = - ConstStaticCell::new(RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE)); -// static RADIO_TEST: RadioTest< -// RADIO_MAX_TX_PACKET_SIZE, -// RADIO_MAX_RX_PACKET_SIZE, -// RADIO_TX_BUF_DEPTH, -// RADIO_RX_BUF_DEPTH, -// RadioUART, -// RadioRxDMA, -// RadioTxDMA, -// RadioReset, -// > = RadioTest::new(&RADIO_TX_UART_QUEUE, &RADIO_RX_UART_QUEUE); - -// pub sub channel for the gyro vals -// CAP queue size, n_subs, n_pubs -static GYRO_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// pub sub channel for the battery raw adc vals -// CAP queue size, n_subs, n_pubs -static BATTERY_CHANNEL: PubSubChannel = PubSubChannel::new(); - -// static RADIO: Radio = Radio::new(); -static EXECUTOR_UART_QUEUE: InterruptExecutor = InterruptExecutor::new(); - -#[interrupt] -unsafe fn CEC() { - EXECUTOR_UART_QUEUE.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART10 => usart::InterruptHandler; - USART6 => usart::InterruptHandler; - USART1 => usart::InterruptHandler; - UART4 => usart::InterruptHandler; - UART7 => usart::InterruptHandler; - UART8 => usart::InterruptHandler; - UART5 => usart::InterruptHandler; -}); - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let sys_config = get_system_config(); - let p = embassy_stm32::init(sys_config); - - let config = usart::Config::default(); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - interrupt::InterruptExt::set_priority(interrupt::CEC, interrupt::Priority::P6); - let spawner = EXECUTOR_UART_QUEUE.start(Interrupt::CEC); - - let mut dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = hz(1_000_000); - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - p.DMA2_CH6, - dotstar_spi_config, - ); - - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - info!("booted"); - - let radio_usart = Uart::new( - p.USART10, p.PE2, p.PE3, Irqs, p.DMA2_CH0, p.DMA2_CH1, config, - ).unwrap(); - - let _rotary = Rotary::new(p.PG9, p.PG10, p.PG11, p.PG12); - let mut shell_indicator = ShellIndicator::new(p.PD0, p.PD1, p.PD3, p.PD4); - let kicker_det = Input::new(p.PG8, Pull::Down); - let dip1 = Input::new(p.PG7, Pull::Down); - let dip2 = Input::new(p.PG6, Pull::Down); - let dip3 = Input::new(p.PG5, Pull::Down); - let dip4 = Input::new(p.PG4, Pull::Down); - let dip5 = Input::new(p.PG3, Pull::Down); - let _dip6 = Input::new(p.PG2, Pull::Down); - let dip7 = Input::new(p.PD15, Pull::Down); - - // let robot_id = rotary.read(); - - //////////////////////// - // Dip Switch Inputs // - //////////////////////// - let robot_id = (dip1.is_high() as u8) << 3 - | (dip2.is_high() as u8) << 2 - | (dip3.is_high() as u8) << 1 - | (dip4.is_high() as u8); - info!("id: {}", robot_id); - shell_indicator.set(robot_id); - - let wifi_network = WifiNetwork::Team; - // let wifi_network = if dip5.is_high() & dip6.is_high() { - // WifiNetwork::Team - // } else if dip5.is_low() & dip6.is_high() { - // WifiNetwork::CompMain - // } else if dip5.is_high() & dip6.is_low() { - // WifiNetwork::CompPractice - // } else { - // WifiNetwork::Team - // }; - - let control_debug_telemetry_enabled = dip5.is_high(); - if control_debug_telemetry_enabled { - info!("Debug control telemetry transmission enabled"); - } - - let team = if dip7.is_high() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - - ////////////////////// - // Battery Voltage // - ////////////////////// - - let mut adc3 = Adc::new(p.ADC3); - adc3.set_sample_time(SampleTime::CYCLES1_5); - let mut battery_pin = p.PF5; - let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = - [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; - let battery_pub = BATTERY_CHANNEL.publisher().unwrap(); - - let mut imu_spi_config = spi::Config::default(); - imu_spi_config.frequency = mhz(1); - let mut imu_spi = spi::Spi::new( - p.SPI6, - p.PA5, - p.PA7, - p.PA6, - p.BDMA_CH0, - p.BDMA_CH1, - imu_spi_config, - ); - - // acceleromter - let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh); - // gyro - let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh); - - Timer::after(Duration::from_millis(1)).await; - - let gyro_pub = GYRO_CHANNEL.publisher().unwrap(); - unsafe { - SPI6_BUF[0] = 0x80; - // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]); - imu_cs1.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs1.set_high(); - let accel_id = SPI6_BUF[1]; - info!("accelerometer id: 0x{:x}", accel_id); - - SPI6_BUF[0] = 0x80; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await; - imu_cs2.set_high(); - let gyro_id = SPI6_BUF[1]; - info!("gyro id: 0x{:x}", gyro_id); - } - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - Irqs, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ).unwrap(); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - Irqs, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ).unwrap(); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - Irqs, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ).unwrap(); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - Irqs, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ).unwrap(); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - Irqs, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ).unwrap(); - - let gyro_sub = GYRO_CHANNEL.subscriber().unwrap(); - let battery_sub = BATTERY_CHANNEL.subscriber().unwrap(); - - if kicker_det.is_low() { - defmt::warn!("kicker appears unplugged!"); - } - - let kicker_usart = Uart::new( - p.USART6, - p.PC7, - p.PC6, - Irqs, - p.DMA2_CH4, - p.DMA2_CH5, - get_bootloader_uart_config(), - ).unwrap(); - - let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); - spawner - .spawn(KICKER_RX_UART_QUEUE.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_TX_UART_QUEUE.spawn_task(kicker_tx)) - .unwrap(); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - gyro_sub, - battery_sub, - ); - - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - control.load_firmware().await; - - info!("flashing kicker..."); - - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_RX_UART_QUEUE, - &KICKER_TX_UART_QUEUE, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), - ); - let kicker_firmware_image = KICKER_FW_IMG; - let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; - - if res.is_err() { - defmt::warn!("kicker flashing failed."); - loop {} - } else { - info!("kicker flash complete"); - } - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }] - .iter() - .cloned(), - ); - - // let token = unsafe { - // (&mut *(&RADIO_TEST as *const _ - // as *mut RadioTest< - // RADIO_MAX_TX_PACKET_SIZE, - // RADIO_MAX_RX_PACKET_SIZE, - // RADIO_TX_BUF_DEPTH, - // RADIO_RX_BUF_DEPTH, - // RadioUART, - // RadioRxDMA, - // RadioTxDMA, - // RadioReset, - // >)) - // .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - // .await - // }; - let radio_test = RADIO_TEST.take(); - let token = unsafe { - (&mut *(&RADIO_TEST as *const _ - as *mut RadioTest< - RADIO_MAX_TX_PACKET_SIZE, - RADIO_MAX_RX_PACKET_SIZE, - RADIO_TX_BUF_DEPTH, - RADIO_RX_BUF_DEPTH, - RadioUART, - RadioRxDMA, - RadioTxDMA, - RadioReset, - >)) - .setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network) - .await - }; - // let token = radio_test.setup(&spawner, radio_usart, p.PC13, robot_id, team, wifi_network).await; - spawner.spawn(token).unwrap(); - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] - .iter() - .cloned(), - ); - - let mut wdg = IndependentWatchdog::new(p.IWDG1, 1_000_000); - unsafe { wdg.unleash() } - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - kicker.set_telemetry_enabled(true); - kicker.send_command(); - - defmt::info!("using SSID: {}", wifi_credentials[0].get_ssid()); - - loop { - unsafe { wdg.pet() }; - - unsafe { - SPI6_BUF[0] = 0x86; - imu_cs2.set_low(); - let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await; - imu_cs2.set_high(); - let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16; - // info!("z rate: {}", rate_z); - let gyro_conversion = 2000.0 / 32767.0; - gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion); - } - - // could just feed gyro in here but the comment in control said to use a channel - - /////////////////////// - // Battery reading // - /////////////////////// - - let current_battery_v = - adc_v_to_battery_voltage(adc_raw_to_v(adc3.read(&mut battery_pin) as f32)); - // Shift buffer through - for i in (0..(BATTERY_BUFFER_SIZE - 1)).rev() { - battery_voltage_buffer[i + 1] = battery_voltage_buffer[i]; - } - // Add new battery read - battery_voltage_buffer[0] = current_battery_v; - let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); - let filter_battery_v = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); - battery_pub.publish_immediate(filter_battery_v); - - if filter_battery_v < BATTERY_MIN_VOLTAGE { - let _ = dotstar.write( - [RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 10, g: 0, b: 0 }] - .iter() - .cloned(), - ); - defmt::error!("Error filtered battery voltage too low"); - critical_section::with(|_| loop { - cortex_m::asm::delay(1_000_000); - unsafe { wdg.pet() }; - }); - } - - ///////////////////////// - // Parameter Updates // - ///////////////////////// - - let latest_param_cmd = radio_test.get_latest_params_cmd(); - - if let Some(latest_param_cmd) = latest_param_cmd { - let param_cmd_resp = control.apply_command(&latest_param_cmd); - - // if param_cmd_resp is Err, then the requested parameter update had no submodule acceping the - // field, or the type was invalid, or the update code is unimplemented - // if param_cmd_resp is Ok, then the read/write was successful - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - radio_test.send_parameter_response(resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - radio_test.send_parameter_response(resp).await; - } - } - - //////////////// - // Telemtry // - //////////////// - - let latest_control_cmd = radio_test.get_latest_control(); - - let telemetry = control.tick(latest_control_cmd).await; - if let (Some(mut telemetry), control_debug_telem) = telemetry { - // info!("{:?}", defmt::Debug2Format(&telemetry)); - - telemetry.kicker_charge_level = kicker.hv_rail_voltage(); - telemetry.set_breakbeam_ball_detected(kicker.ball_detected() as u32); - - radio_test.send_telemetry(telemetry).await; - - if control_debug_telemetry_enabled { - radio_test.send_control_debug_telemetry(control_debug_telem).await; - } - } - - kicker.process_telemetry(); - - if let Some(control) = latest_control_cmd { - kicker.set_kick_strength(control.kick_vel); - kicker.request_kick(control.kick_request); - kicker.send_command(); - } - - main_loop_rate_ticker.next().await; - } -} - -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state_pin, exti, Pull::None); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); - loop {} -} diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index eeeb48e1..b55696bb 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -3,7 +3,7 @@ use embassy_executor::InterruptExecutor; use embassy_stm32::{ - interrupt, pac::Interrupt, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} + interrupt, pac::Interrupt }; use embassy_sync::pubsub::PubSubChannel; @@ -87,21 +87,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - // let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); - // let mut pwm = SimplePwm::new(p.TIM15, None, Some(ch2), None, None, khz(2), Default::default()); - // let max = pwm.get_max_duty(); - // pwm.enable(Channel::Ch2); - - - // pwm.set_duty(Channel::Ch2, max / 2); - - // loop { - // pwm.set_frequency(hz(500)); - // Timer::after_millis(1000).await; - // pwm.set_frequency(hz(2000)); - // Timer::after_millis(1000).await; - // } - create_io_task!(main_spawner, robot_state, p); diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index 34c4a658..2cbf5819 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -6,13 +6,13 @@ use ateam_control_board::{ - drivers::kicker::Kicker, get_system_config, include_kicker_bin, pins::{KickerRxDma, KickerTxDma, KickerUart}, stm32_interface::{self, get_bootloader_uart_config, Stm32Interface}, + drivers::kicker::Kicker, get_system_config, include_kicker_bin, pins::{KickerRxDma, KickerTxDma, KickerUart}, stm32_interface::{self, Stm32Interface}, }; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use defmt::info; use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::{Input, Level, Output, Pull, Speed}, interrupt, pac::Interrupt, usart::Uart + gpio::{Level, Output, Speed}, interrupt, pac::Interrupt, usart::Uart }; use embassy_time::{Duration, Ticker, Timer}; use panic_probe as _; diff --git a/control-board/src/bin/hwtest-piezo/main.rs b/control-board/src/bin/hwtest-piezo/main.rs index 70d90bfc..22123c9c 100644 --- a/control-board/src/bin/hwtest-piezo/main.rs +++ b/control-board/src/bin/hwtest-piezo/main.rs @@ -1,10 +1,10 @@ #![no_std] #![no_main] -use ateam_lib_stm32::{audio::tone_player::{self, TonePlayer}, drivers::audio::buzzer::Buzzer}; +use ateam_lib_stm32::{audio::tone_player::TonePlayer, drivers::audio::buzzer::Buzzer}; use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::OutputType, interrupt, peripherals::TIM15, time::{hz, khz}, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} + gpio::OutputType, interrupt, time::khz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel} }; use defmt_rtt as _; diff --git a/control-board/src/bin/kicker/main.rs b/control-board/src/bin/kicker/main.rs deleted file mode 100644 index adfaff1c..00000000 --- a/control-board/src/bin/kicker/main.rs +++ /dev/null @@ -1,40 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(async_closure)] -#![feature(const_mut_refs)] -#![feature(ptr_metadata)] - -use defmt::*; -use defmt_rtt as _; - -use embassy_stm32::gpio::{Output, Level, Speed, Input, Pull}; -use embassy_stm32::time::mhz; -use embassy_stm32::{self as _,}; -use embassy_time::{Duration, Timer}; -use panic_probe as _; - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - info!("Startup"); - - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - - let btn = Input::new(p.PF5, Pull::Up); - let mut output = Output::new(p.PF9, Level::Low, Speed::High); - while btn.is_high() {} - - info!("kick"); - - // PF9: MOTOR_AUX_RST - output.set_high(); - Timer::after(Duration::from_micros(8000)).await; - output.set_low(); - info!("done"); - loop {} -} From 5d1ebe94200b8e9461dcff74db59b37ebcdfe76b Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 15 Jun 2024 16:08:48 -0400 Subject: [PATCH 073/157] Yay control task builds :D --- control-board/src/tasks/control_task.rs | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 433e33d7..f2a4ff8f 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,4 +1,3 @@ -use alloc::vec::Vec; use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; @@ -6,7 +5,7 @@ use embassy_stm32::usart::Uart; use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{self, robot_controller::{self, BodyVelocityController}, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{self, robot_controller::{self, BodyVelocityController}, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::{self, SharedRobotState}, stm32_interface::{self, Stm32Interface}, stspin_motor::{DribblerMotor, WheelMotor}, tasks::control_task, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -87,7 +86,7 @@ impl < motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> - ) -> Self { + ) -> Self { ControlTask { shared_robot_state: robot_state, command_subscriber: command_subscriber, @@ -103,7 +102,8 @@ impl < fn do_control_update(&mut self, robot_controller: &mut BodyVelocityController, - cmd_vel: Vector3 + cmd_vel: Vector3, + gyro_rads: f32 ) -> Vector4 /* Provide the motion controller with the current wheel velocities @@ -264,7 +264,7 @@ impl < if controls_enabled { // TODO check order - self.do_control_update(&mut robot_controller, cmd_vel) + self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads) } else { robot_model.robot_vel_to_wheel_vel(cmd_vel) } @@ -366,6 +366,13 @@ impl < } } +#[embassy_executor::task] +async fn control_task_entry(mut control_task: ControlTask) { + loop { + control_task.control_task_entry().await; + defmt::error!("control task returned"); + } +} // #[embassy_executor::task] // async fn control_task_entry( // robot_state: &'static SharedRobotState, @@ -671,7 +678,9 @@ pub async fn start_control_task( let motor_fr = WheelMotor::new_from_pins(&FRONT_RIGHT_RX_UART_QUEUE, &FRONT_RIGHT_TX_UART_QUEUE, motor_fr_boot0_pin, motor_fr_nrst_pin, WHEEL_FW_IMG); let motor_drib = DribblerMotor::new_from_pins(&DRIB_RX_UART_QUEUE, &DRIB_TX_UART_QUEUE, motor_d_boot0_pin, motor_d_nrst_pin, DRIB_FW_IMG, 1.0); - control_task_spawner.spawn(control_task_entry(robot_state, - command_subscriber, telemetry_publisher, gyro_subscriber, - motor_fl, motor_bl, motor_br, motor_fr, motor_drib)).unwrap(); + let control_task = ControlTask::new( + robot_state, command_subscriber, telemetry_publisher, + gyro_subscriber, motor_fl, motor_bl, motor_br, motor_fr, motor_drib); + + control_task_spawner.spawn(control_task_entry(control_task)).unwrap(); } \ No newline at end of file From b96ba3f9eb63ba47731e9712cc0a87e51d9350ab Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sat, 15 Jun 2024 16:11:26 -0400 Subject: [PATCH 074/157] Remove commented code --- control-board/src/tasks/control_task.rs | 252 ------------------------ 1 file changed, 252 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index f2a4ff8f..48a741dd 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -373,258 +373,6 @@ async fn control_task_entry(mut control_task: ControlTask, -// mut motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, -// mut motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, -// mut motor_fr: WheelMotor<'static, MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, -// mut motor_drib: DribblerMotor<'static, MotorDUart, MotorDDmaRx, MotorDDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> -// ) { - - // wait for the switch state to be read - // while !robot_state.hw_init_state_valid() { - // Timer::after_millis(10).await; - // } - - // if robot_state.hw_in_debug_mode() { - // defmt::info!("flashing firmware"); - - // if motor_fl.load_default_firmware_image().await.is_err() { - // defmt::error!("failed to flash FL"); - // } else { - // defmt::info!("FL flashed"); - // } - - // if motor_bl.load_default_firmware_image().await.is_err() { - // defmt::error!("failed to flash BL"); - // } else { - // defmt::info!("BL flashed"); - // } - - // if motor_br.load_default_firmware_image().await.is_err() { - // defmt::error!("failed to flash BR"); - // } else { - // defmt::info!("BR flashed"); - // } - - // if motor_fr.load_default_firmware_image().await.is_err() { - // defmt::error!("failed to flash FR"); - // } else { - // defmt::info!("FR flashed"); - // } - - // if motor_drib.load_default_firmware_image().await.is_err() { - // defmt::error!("failed to flash DRIB"); - // } else { - // defmt::info!("DRIB flashed"); - // } - // } else { - // let _res = embassy_futures::join::join5( - // motor_fl.load_default_firmware_image(), - // motor_bl.load_default_firmware_image(), - // motor_br.load_default_firmware_image(), - // motor_fr.load_default_firmware_image(), - // motor_drib.load_default_firmware_image(), - // ) - // .await; - - // defmt::debug!("motor firmware flashed"); - // } - - - // embassy_futures::join::join5( - // motor_fl.leave_reset(), - // motor_bl.leave_reset(), - // motor_br.leave_reset(), - // motor_fr.leave_reset(), - // motor_drib.leave_reset(), - // ) - // .await; - - // motor_fl.set_telemetry_enabled(true); - // motor_bl.set_telemetry_enabled(true); - // motor_br.set_telemetry_enabled(true); - // motor_fr.set_telemetry_enabled(true); - // motor_drib.set_telemetry_enabled(true); - - // Timer::after_millis(10).await; - - // let robot_model_constants: RobotConstants = RobotConstants { - // wheel_angles_rad: Vector4::new( - // WHEEL_ANGLES_DEG[0].to_radians(), - // WHEEL_ANGLES_DEG[1].to_radians(), - // WHEEL_ANGLES_DEG[2].to_radians(), - // WHEEL_ANGLES_DEG[3].to_radians(), - // ), - // wheel_radius_m: Vector4::new( - // WHEEL_RADIUS_M, - // WHEEL_RADIUS_M, - // WHEEL_RADIUS_M, - // WHEEL_RADIUS_M, - // ), - // wheel_dist_to_cent_m: Vector4::new( - // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - // WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - // ), - // }; - - // let robot_model: RobotModel = RobotModel::new(robot_model_constants); - // let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); - - // let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - // let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); - // let mut drib_vel = 0.0; - // let mut ticks_since_packet = 0; - // loop { - // motor_fl.process_packets(); - // motor_bl.process_packets(); - // motor_br.process_packets(); - // motor_fr.process_packets(); - // motor_drib.process_packets(); - - // motor_fl.log_reset("FL"); - // motor_bl.log_reset("BL"); - // motor_br.log_reset("BR"); - // motor_fr.log_reset("FR"); - // motor_drib.log_reset("DRIB"); - - // if motor_drib.ball_detected() { - // defmt::info!("ball detected"); - // } - - // if let Some(latest_packet) = command_subscriber.try_next_message_pure() { - // match latest_packet { - // ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { - - // let new_cmd_vel = Vector3::new( - // latest_control.vel_x_linear, - // latest_control.vel_y_linear, - // latest_control.vel_z_angular, - // ); - - // defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); - - // cmd_vel = new_cmd_vel; - // drib_vel = latest_control.dribbler_speed; - // ticks_since_packet = 0; - // }, - // ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param) => { - // defmt::warn!("param updates aren't supported yet"); - // }, - // } - // } else { - // ticks_since_packet += 1; - // if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - // cmd_vel = Vector3::new(0., 0., 0.); - // // ticks_since_packet = 0; - // } - // } - - // defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); - - - // // now we have setpoint r(t) in self.cmd_vel - // // let battery_v = battery_sub.next_message_pure().await as f32; - // let battery_v = 25.0; - // let controls_enabled = false; - // let gyro_rads = (gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; - // let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - // if controls_enabled - // { - // // TODO check order - // let wheel_vels = Vector4::new( - // motor_fr.read_rads(), - // motor_fl.read_rads(), - // motor_bl.read_rads(), - // motor_br.read_rads() - // ); - - // // torque values are computed on the spin but put in the current variable - // // TODO update this when packet/var names are updated to match software - // let wheel_torques = Vector4::new( - // motor_fr.read_current(), - // motor_fl.read_current(), - // motor_bl.read_current(), - // motor_br.read_current() - // ); - - // // TODO read from channel or something - - // robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); - // robot_controller.get_wheel_velocities() - // } else { - // robot_model.robot_vel_to_wheel_vel(cmd_vel) - // } - // } else { - // // Battery is too low, set velocity to zero - // Vector4::new( - // 0.0, - // 0.0, - // 0.0, - // 0.0) - // }; - - // motor_fr.set_setpoint(wheel_vels[0]); - // motor_fl.set_setpoint(wheel_vels[1]); - // motor_bl.set_setpoint(wheel_vels[2]); - // motor_br.set_setpoint(wheel_vels[3]); - - // let drib_dc = -1.0 * drib_vel / 1000.0; - // motor_drib.set_setpoint(drib_dc); - - // motor_fr.send_motion_command(); - // motor_fl.send_motion_command(); - // motor_bl.send_motion_command(); - // motor_br.send_motion_command(); - // motor_drib.send_motion_command(); - - - // let basic_telem = TelemetryPacket::Basic(BasicTelemetry { - // sequence_number: 0, - // robot_revision_major: 0, - // robot_revision_minor: 0, - // battery_level: battery_v, - // battery_temperature: 0., - // _bitfield_align_1: [], - // _bitfield_1: BasicTelemetry::new_bitfield_1( - // 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - // ), - // motor_0_temperature: 0., - // motor_1_temperature: 0., - // motor_2_temperature: 0., - // motor_3_temperature: 0., - // motor_4_temperature: 0., - // kicker_charge_level: 0., - // }); - // telemetry_publisher.publish_immediate(basic_telem); - - // let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); - // telemetry_publisher.publish_immediate(control_debug_telem); - - // loop_rate_ticker.next().await; - // } - - // UNUSED!!! - // loop { - // motor_fl.process_packets(); - - // let rads = motor_fl.read_rads(); - // defmt::info!("read motor rads {}", rads); - // motor_fl.set_setpoint(3.1415 * 10.0); - - // motor_fl.send_motion_command(); - - // Timer::after_millis(10).await; - // } -// } pub async fn start_control_task( uart_queue_spawner: SendSpawner, From f2f6ef87b6aad6900bd7183d65ccecb1f19d3934 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 16 Jun 2024 18:03:11 -0400 Subject: [PATCH 075/157] led refactor --- control-board/src/tasks/audio_task.rs | 6 +- control-board/src/tasks/control_task.rs | 12 -- control-board/src/tasks/user_io_task.rs | 62 ++++++---- lib-stm32/src/anim/mod.rs | 146 +++++++++++++++++++++++- lib-stm32/src/drivers/led/apa102.rs | 131 +++++++++++++++------ lib-stm32/src/math/lerp.rs | 4 +- 6 files changed, 288 insertions(+), 73 deletions(-) diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index a0809f64..814191ce 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -1,7 +1,7 @@ use ateam_lib_stm32::{audio::tone_player::TonePlayer, drivers::audio::buzzer::Buzzer}; use embassy_executor::Spawner; use embassy_stm32::{gpio::OutputType, time::hz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}}; -use embassy_time::Timer; +use embassy_time::{Duration, Ticker}; use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::TIPPED_WARNING_SONG}; @@ -19,6 +19,8 @@ async fn audio_task_entry( robot_state: &'static SharedRobotState, mut tone_player: TonePlayer<'static, Buzzer<'static, BuzzerTimer>>, ) { + let mut audio_ticker = Ticker::every(Duration::from_millis(100)); + loop { let cur_robot_state = robot_state.get_state(); @@ -28,7 +30,7 @@ async fn audio_task_entry( tone_player.play_song().await; } - Timer::after_millis(100).await; + audio_ticker.next().await; } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 8221dde8..0060516c 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -292,18 +292,6 @@ async fn control_task_entry( loop_rate_ticker.next().await; } - - loop { - motor_fl.process_packets(); - - let rads = motor_fl.read_rads(); - defmt::info!("read motor rads {}", rads); - motor_fl.set_setpoint(3.1415 * 10.0); - - motor_fl.send_motion_command(); - - Timer::after_millis(10).await; - } } pub async fn start_control_task( diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index c2667589..cd7c81e9 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -28,6 +28,8 @@ use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; #[link_section = ".sram4"] static mut DOTSTAR_SPI_BUFFER_CELL: [u8; 16] = [0; 16]; + + #[macro_export] macro_rules! create_io_task { ($spawner:ident, $robot_state:ident, $p:ident) => { @@ -54,35 +56,55 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, ) { defmt::info!("user io task initialized"); - let shutdown_anim = anim::Animation::Lerp(Lerp::new(WHITE, BLACK, Duration::from_millis(HARD_SHUTDOWN_TIME_MS), AnimRepeatMode::None)); - let mut sd_anim_seq = [shutdown_anim]; - let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); + // let shutdown_anim = anim::Animation::Lerp(Lerp::new(WHITE, BLACK, Duration::from_millis(HARD_SHUTDOWN_TIME_MS), AnimRepeatMode::None)); + // let mut sd_anim_seq = [shutdown_anim]; + // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); - let anim0 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim2 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim0 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim1 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let anim2 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); let mut anim_seq = [anim0, anim1, anim2]; - let mut composite_anim0 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); - composite_anim0.start_animation(); - - let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim1_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 255 }, RGB8 { r: 255, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim2_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 255 }, RGB8 { r: 255, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let mut anim_seq = [anim0_1, anim1_1, anim2_1]; - let mut composite_anim1 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); - composite_anim1.start_animation(); + const RGB_LERP_ID: usize = 1; + let composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut anim_seq, AnimRepeatMode::Forever); + // let mut composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut [anim0, anim1, anim2], AnimRepeatMode::Forever); + + let mut led0_anim_playbook = [composite_anim0]; + + // composite_anim0.start_animation(); + + // let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ + // Some(&mut [ + // CompositeAnimation::new(RGB_LERP_ID, &mut [ + // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), + // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)), + // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), + // ], AnimRepeatMode::Forever) + // ]), + // None, + // ]; + + let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ + Some(&mut led0_anim_playbook), + None, + ]; + + // let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + // let anim1_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 255 }, RGB8 { r: 255, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + // let anim2_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 255 }, RGB8 { r: 255, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + // let mut anim_seq = [anim0_1, anim1_1, anim2_1]; + // let mut composite_anim1 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); + // composite_anim1.start_animation(); dotstars.set_drv_str_all(32); - let mut dotstars_anim = Apa102Anim::new(dotstars); + let mut dotstars_anim = Apa102Anim::new(dotstars, animation_playbooks); + + dotstars_anim.enable_animation(0, RGB_LERP_ID); + dotstars_anim.enable_animation(1, RGB_LERP_ID); // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); // color_lerp.start(); - - dotstars_anim.set_animation(&mut composite_anim0, 0); - dotstars_anim.set_animation(&mut composite_anim1, 1); - // let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); // anim1.start_animation(); // dotstars_anim.set_animation(anim1, 1); diff --git a/lib-stm32/src/anim/mod.rs b/lib-stm32/src/anim/mod.rs index e251f93b..67d6d4fc 100644 --- a/lib-stm32/src/anim/mod.rs +++ b/lib-stm32/src/anim/mod.rs @@ -6,6 +6,7 @@ use crate::math::lerp::LerpNumeric; #[derive(Clone, Copy, PartialEq, Eq, Debug)] enum AnimState { + Disabled, Waiting, Running, Completed, @@ -19,11 +20,17 @@ pub enum AnimRepeatMode { } pub trait AnimInterface: Sized { + fn get_id(&self) -> usize; + fn enable(&mut self); + fn disable(&mut self); + fn enabled(&self) -> bool; + fn start_animation(&mut self); fn reset_animation(&mut self); fn animation_running(&self) -> bool; fn animation_completed(&self) -> bool; fn update(&mut self); + fn get_value(&self) -> T; } @@ -45,6 +52,50 @@ where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { + fn get_id(&self) -> usize { + match self { + Animation::Blink(a) => { + a.get_id() + }, + Animation::Lerp(a) => { + a.get_id() + }, + } + } + + fn enable(&mut self) { + match self { + Animation::Blink(a) => { + a.enable() + }, + Animation::Lerp(a) => { + a.enable() + }, + } + } + + fn disable(&mut self) { + match self { + Animation::Blink(a) => { + a.disable() + }, + Animation::Lerp(a) => { + a.disable() + }, + } + } + + fn enabled(&self) -> bool { + match self { + Animation::Blink(a) => { + a.enabled() + }, + Animation::Lerp(a) => { + a.enabled() + }, + } + } + fn start_animation(&mut self) { match self { Animation::Blink(a) => { @@ -116,6 +167,8 @@ pub struct CompositeAnimation<'a, N, L> where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { + id: usize, + animations: &'a mut [Animation], active_animation: usize, @@ -131,13 +184,14 @@ where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { - pub fn new(animations: &'a mut [Animation], repeat_mode: AnimRepeatMode) -> Self { + pub fn new(id: usize, animations: &'a mut [Animation], repeat_mode: AnimRepeatMode) -> Self { let first_val = animations[0].get_value(); CompositeAnimation { + id, animations: animations, active_animation: 0, repeat_mode: repeat_mode, - anim_state: AnimState::Waiting, + anim_state: AnimState::Disabled, repeat_counter: 0, last_value: first_val, } @@ -149,7 +203,27 @@ where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { + fn get_id(&self) -> usize { + return self.id; + } + + fn enable(&mut self) { + self.anim_state = AnimState::Waiting; + } + + fn disable(&mut self) { + self.anim_state = AnimState::Disabled; + } + + fn enabled(&self) -> bool { + self.anim_state != AnimState::Disabled + } + fn start_animation(&mut self) { + if !self.enabled() { + return; + } + self.active_animation = 0; self.anim_state = AnimState::Running; self.animations[self.active_animation].start_animation(); @@ -160,6 +234,10 @@ L: crate::math::lerp::Lerp } fn reset_animation(&mut self) { + if !self.enabled() { + return; + } + self.anim_state = AnimState::Waiting; } @@ -172,6 +250,10 @@ L: crate::math::lerp::Lerp } fn update(&mut self) { + if self.anim_state != AnimState::Running { + return; + } + // update the current animation self.animations[self.active_animation].update(); self.last_value = self.animations[self.active_animation].get_value(); @@ -219,6 +301,8 @@ L: crate::math::lerp::Lerp #[derive(Clone, Copy, Debug)] pub struct Blink { + id: usize, + val_one: T, val_two: T, v1_time: u64, @@ -232,8 +316,10 @@ pub struct Blink { } impl Blink { - pub fn new(val_one: T, val_two: T, v1_time: Duration, v2_time: Duration, repeat_style: AnimRepeatMode) -> Self { + pub fn new(id: usize, val_one: T, val_two: T, v1_time: Duration, v2_time: Duration, repeat_style: AnimRepeatMode) -> Self { Blink { + id, + val_one, val_two, v1_time: v1_time.as_millis(), @@ -249,7 +335,27 @@ impl Blink { } impl AnimInterface for Blink { + fn get_id(&self) -> usize { + return self.id; + } + + fn enable(&mut self) { + self.anim_state = AnimState::Disabled; + } + + fn disable(&mut self) { + self.anim_state = AnimState::Waiting; + } + + fn enabled(&self) -> bool { + self.anim_state != AnimState::Disabled + } + fn start_animation(&mut self) { + if !self.enabled() { + return; + } + self.start_time = Instant::now(); self.anim_state = AnimState::Running; @@ -259,6 +365,10 @@ impl AnimInterface for Blink { } fn reset_animation(&mut self) { + if !self.enabled() { + return; + } + self.anim_state = AnimState::Waiting; } @@ -314,6 +424,8 @@ pub struct Lerp where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { + id: usize, + val_one: L, val_two: L, lerp_duration_ms: u64, @@ -332,8 +444,10 @@ where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { - pub fn new(val_one: L, val_two: L, lerp_duration_ms: Duration, repeat_style: AnimRepeatMode) -> Self { + pub fn new(id: usize, val_one: L, val_two: L, lerp_duration_ms: Duration, repeat_style: AnimRepeatMode) -> Self { Lerp { + id, + val_one, val_two, lerp_duration_ms: lerp_duration_ms.as_millis(), @@ -354,7 +468,27 @@ where N: LerpNumeric, f32: core::convert::From, L: crate::math::lerp::Lerp { + fn get_id(&self) -> usize { + return self.id; + } + + fn enable(&mut self) { + self.anim_state = AnimState::Waiting + } + + fn disable(&mut self) { + self.anim_state = AnimState::Disabled + } + + fn enabled(&self) -> bool { + self.anim_state != AnimState::Disabled + } + fn start_animation(&mut self) { + if !self.enabled() { + return; + } + self.start_time = Instant::now(); self.anim_state = AnimState::Running; @@ -364,6 +498,10 @@ L: crate::math::lerp::Lerp } fn reset_animation(&mut self) { + if !self.enabled() { + return; + } + self.anim_state = AnimState::Waiting; } diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index dc86c1eb..73f050ac 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -2,10 +2,9 @@ use core::ops::Range; use embassy_stm32::{mode::Async, spi::{self, Config, MosiPin, SckPin, Spi}, time::mhz, Peripheral}; -use embassy_time::{Duration, Instant}; use smart_leds::RGB8; -use crate::anim::{AnimInterface, Blink, CompositeAnimation, Lerp}; +use crate::anim::{AnimInterface, CompositeAnimation}; pub struct Apa102<'a, 'buf, SpiPeri: spi::Instance, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { @@ -117,58 +116,124 @@ where [(); (NUM_LEDS * 4) + 8]: { pub struct Apa102Anim<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> where [(); (NUM_LEDS * 4) + 8]: { apa102_driver: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>, - animation_buf: [Option<&'ca mut CompositeAnimation<'ca, u8, RGB8>>; NUM_LEDS], + active_animation: [usize; NUM_LEDS], + animation_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS], + // animation_buf: [Option<&'ca mut [(u8, bool, &'ca mut CompositeAnimation<'ca, u8, RGB8>)]>; NUM_LEDS] + // animation_buf: [Option<&'ca mut CompositeAnimation<'ca, u8, RGB8>>; NUM_LEDS], } impl<'a, 'buf, 'ca, SpiPeri: spi::Instance, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, SpiPeri, NUM_LEDS> where [(); (NUM_LEDS * 4) + 8]: { - pub fn new(apa102: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>) -> Self { + pub fn new(apa102: Apa102<'a, 'buf, SpiPeri, NUM_LEDS>, anim_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS]) -> Self { Apa102Anim { apa102_driver: apa102, - animation_buf: [const { None }; NUM_LEDS], + active_animation: [0; NUM_LEDS], + animation_playbook_buf: anim_playbook_buf, } } - pub async fn update(&mut self) { - // update animations - for anim in self.animation_buf.iter_mut() { - if let Some(anim) = anim { - anim.update(); + pub fn next_valid_anim(search_start_ind: usize, anim_playbook: &[CompositeAnimation<'ca, u8, RGB8>]) -> usize { + if anim_playbook.len() == 1 { + return 0; + } + + let mut cur_index = search_start_ind; + loop { + // inc and loop the index + cur_index = cur_index + 1 % anim_playbook.len(); + + // we incremented and found an enabled animation + // return the index + if anim_playbook[cur_index].enabled() { + return cur_index; + } + + // we've searched everything and made it all the way around + // return the search start index + if cur_index == search_start_ind { + return search_start_ind; } } + } - // set colors from animations - for (i, anim) in self.animation_buf.iter().enumerate() { - if let Some(anim) = anim { - self.apa102_driver.set_color(anim.get_value(), i); + pub async fn update(&mut self) { + // update animations + for (led_index, anim_arr_opt) in self.animation_playbook_buf.iter_mut().enumerate() { + let mut led_color = RGB8 { r: 0, g: 0, b: 0 }; + + // see if a playbook is loaded + if let Some(anim_playbook) = anim_arr_opt { + // check if any animation are enabled + if anim_playbook.iter().any(|comp_anim| comp_anim.enabled()) { + // confirm the current animation is enabled + let mut cur_active_anim = self.active_animation[led_index]; + if !anim_playbook[cur_active_anim].enabled() { + // select and start the next animation + let next_valid_anim = Self::next_valid_anim(cur_active_anim, anim_playbook); + self.active_animation[led_index] = next_valid_anim; + anim_playbook[next_valid_anim].start_animation(); + + cur_active_anim = next_valid_anim; + }; + + // update the selected animation + anim_playbook[cur_active_anim].update(); + // check if animation is completed + if anim_playbook[cur_active_anim].animation_completed() { + // reset the animation + anim_playbook[cur_active_anim].reset_animation(); + + // select and start the next animation + let next_active_anim = Self::next_valid_anim(cur_active_anim, anim_playbook); + anim_playbook[next_active_anim].start_animation(); + self.active_animation[led_index] = next_active_anim; + + cur_active_anim = next_active_anim; + } + + // update led color + led_color = anim_playbook[cur_active_anim].get_value(); + } } + + // set the color + self.apa102_driver.set_color(led_color, led_index); } self.apa102_driver.update().await; } - pub fn set_animation(&mut self, anim: &'ca mut CompositeAnimation<'ca, u8, smart_leds::RGB>, led_index: usize) { - assert!(led_index < NUM_LEDS); - self.animation_buf[led_index] = Some(anim); + fn set_animation_enabled(&mut self, led_index: usize, anim_id: usize, enable: bool) -> Result<(), ()> { + if led_index >= NUM_LEDS { + return Err(()) + } + + if let Some(animation_playbook) = self.animation_playbook_buf[led_index].as_deref_mut() { + for composite_animation in animation_playbook.iter_mut() { + defmt::info!("evaluating animation {}", composite_animation.get_id()); + + if composite_animation.get_id() == anim_id { + if enable { + defmt::info!("enabling animation"); + composite_animation.enable(); + composite_animation.start_animation(); + } else { + composite_animation.reset_animation(); + composite_animation.disable(); + } + } + } + } + + Ok(()) } - pub fn clear_animation(&mut self, led_index: usize) { - assert!(led_index < NUM_LEDS); - self.animation_buf[led_index] = None; + pub fn enable_animation(&mut self, led_index: usize, anim_id: usize) -> Result<(), ()> { + self.set_animation_enabled(led_index, anim_id, true) } - // pub fn start_animation(&mut self, led_index: usize) { // pub fn start_animation(&mut self, led_index: usize) { - // assert!(led_index < NUM_LEDS); - - // if let Some(anim) = self.animation_buf[led_index].as_mut() { - // anim.start_animation(); - // } - // } - // assert!(led_index < NUM_LEDS); - - // if let Some(anim) = self.animation_buf[led_index].as_mut() { - // anim.start_animation(); - // } - // } + pub fn disable_animation(&mut self, led_index: usize, anim_id: usize) -> Result<(), ()> { + self.set_animation_enabled(led_index, anim_id, false) + } } diff --git a/lib-stm32/src/math/lerp.rs b/lib-stm32/src/math/lerp.rs index 62390f7a..2d0200f2 100644 --- a/lib-stm32/src/math/lerp.rs +++ b/lib-stm32/src/math/lerp.rs @@ -79,12 +79,12 @@ where N: FromPrimitive + Copy + Bounded, f32: core::convert::From, L: Lerp { - pub fn new(a: L, b: L, duration: Duration) -> TimeLerp<'a, N, L> { + pub const fn new(a: L, b: L, duration: Duration) -> TimeLerp<'a, N, L> { TimeLerp { a: a, b: b, duration: duration, - start_time: Instant::now(), + start_time: Instant::MIN, pd2: PhantomData, } } From 2961f9ae673cdb1868de85607daf41296375f70c Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 16 Jun 2024 19:10:23 -0700 Subject: [PATCH 076/157] Updated for Vref understanding --- control-board/src/pins.rs | 1 + control-board/src/tasks/user_io_task.rs | 26 ++++++++++--- lib-stm32/src/drivers/other/adc_helper.rs | 45 ++++++++++++++--------- 3 files changed, 49 insertions(+), 23 deletions(-) diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 9d9fbd30..6d0af0ab 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -70,6 +70,7 @@ pub type ImuSpiInt2Exti = EXTI2; pub type ExtImuNDetPin = PF11; pub type BatteryAdcPin = PF14; pub type BatteryAdc = ADC2; +pub type VrefIntAdc = ADC3; ////////////// // Kicker // diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 95709f60..aa833303 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -16,7 +16,7 @@ use ateam_lib_stm32::drivers::switches::button::AdvExtiButton; use ateam_lib_stm32::drivers::switches::dip::DipSwitch; use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; use ateam_lib_stm32::drivers::other::adc_helper::AdcHelper; -use embassy_stm32::adc::{SampleTime, Resolution}; +use embassy_stm32::adc::{Adc, SampleTime, Resolution}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; @@ -27,6 +27,10 @@ use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; // #[link_section = ".sram4"] // static DOTSTAR_SPI_BUFFER_CELL: ConstStaticCell<[u8; 16]> = ConstStaticCell::new([0; 16]); +fn get_vref_int_cal() -> u16 { + unsafe { *(0x1FF1_E860 as *const u16) } +} + #[link_section = ".sram4"] static mut DOTSTAR_SPI_BUFFER_CELL: [u8; 16] = [0; 16]; @@ -37,6 +41,7 @@ macro_rules! create_io_task { $robot_state, $battery_volt_publisher, $p.ADC2, $p.PF14, + $p.ADC3, $p.PD6, $p.PD5, $p.EXTI6, $p.EXTI5, $p.PG7, $p.PG6, $p.PG5, $p.PG4, $p.PG3, $p.PG2, $p.PD15, $p.PG12, $p.PG11, $p.PG10, $p.PG9, @@ -53,6 +58,7 @@ async fn user_io_task_entry( mut usr_btn1: AdvExtiButton, battery_volt_publisher: BatteryVoltPublisher, mut battery_volt_adc: AdcHelper<'static, BatteryAdc, BatteryAdcPin>, + mut vref_int_adc: Adc<'static, VrefIntAdc>, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, @@ -85,7 +91,10 @@ async fn user_io_task_entry( // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); // color_lerp.start(); - + // Battery ADC Setup + // Get the Vref_int calibration values. + let vref_int_cal = get_vref_int_cal() as f32; + let mut vref_int_ch = vref_int_adc.enable_vrefint(); dotstars_anim.set_animation(&mut composite_anim0, 0); dotstars_anim.set_animation(&mut composite_anim1, 1); @@ -132,7 +141,7 @@ async fn user_io_task_entry( defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); } - battery_volt_publisher.publish_immediate(battery_volt_adc.read_f32()); + battery_volt_publisher.publish_immediate(battery_volt_adc.read_volt_raw_f32(vref_int_adc.read(&mut vref_int_ch) as f32, vref_int_cal)); // TODO read messages @@ -168,7 +177,8 @@ async fn user_io_task_entry( pub async fn start_io_task(spawner: &Spawner, robot_state: &'static SharedRobotState, battery_volt_publisher: BatteryVoltPublisher, - battery_adc: BatteryAdc, battery_adc_pin: BatteryAdcPin, + battery_adc_peri: BatteryAdc, battery_adc_pin: BatteryAdcPin, + vref_int_adc_peri: VrefIntAdc, usr_btn0_pin: UsrBtn0Pin, usr_btn1_pin: UsrBtn1Pin, usr_btn0_exti: UsrBtn0Exti, usr_btn1_exti: UsrBtn1Exti, usr_dip7_pin: UsrDip7IsBluePin, usr_dip6_pin: UsrDip6Pin, usr_dip5_pin: UsrDip5Pin, usr_dip4_pin: UsrDip4Pin, usr_dip3_pin: UsrDip3Pin, usr_dip2_pin: UsrDip2Pin, usr_dip1_pin: UsrDip1Pin, @@ -204,7 +214,11 @@ pub async fn start_io_task(spawner: &Spawner, let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); - let battery_volt_adc = AdcHelper::new(battery_adc, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); + let battery_volt_adc = AdcHelper::new(battery_adc_peri, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); + let mut vref_int_adc = Adc::new(vref_int_adc_peri); + // Set the Vref_int ADC settings to the same as the battery. + vref_int_adc.set_resolution(Resolution::BITS8); + vref_int_adc.set_sample_time(SampleTime::CYCLES810_5); - spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); + spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, vref_int_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); } \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 4a6dc94d..6828a691 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -1,40 +1,51 @@ -use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime, VrefInt, VREF_DEFAULT_MV}; +use embassy_stm32::adc::{self, Adc, AdcChannel, Resolution, SampleTime}; use embassy_stm32::Peripheral; -pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> where VrefInt: AdcChannel { +// The voltage which the internal ADC were calibrated at. +// For the H743 and F407 +const V_CAL_MV: f32 = 3.3; + +pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { inst: Adc<'a, T>, - channel_pin: Ch, - channel_vref: VrefInt + pin: Ch, + adc_bins: u32, } -impl<'a, T: adc::Instance, Ch: AdcChannel> AdcHelper<'a, T, Ch> -where VrefInt: AdcChannel { +impl< + 'a, + T: adc::Instance, + Ch: AdcChannel> + AdcHelper<'a, T, Ch> { + + // NOTE: vref_int_peri is not checked by compiler and needs to + // be the peripheral connected to Vref_int. pub fn new( peri: impl Peripheral

+ 'a, - channel_pin: Ch, + pin: Ch, sample_time: SampleTime, resolution: Resolution - ) -> Self { + ) -> Self { let mut adc_inst = Adc::new(peri); adc_inst.set_sample_time(sample_time); adc_inst.set_resolution(resolution); - let channel_vref = adc_inst.enable_vrefint(); - + // Use resolution to calculate the max ADC min quantity. + let adc_bins = u32::pow(2, resolution.to_bits() as u32) - 1; AdcHelper { inst: adc_inst, - channel_pin: channel_pin, - channel_vref: channel_vref + pin: pin, + adc_bins: adc_bins } } - pub fn read_f32(&mut self) -> f32 { - // Gets the Vref since it changes based on chip class. - let vref_cur_mv = self.inst.read(&mut self.channel_vref) as f32; - // Scale by Vref to convert to absolute voltage. - return ((VREF_DEFAULT_MV as f32) / vref_cur_mv) * (self.inst.read(&mut self.channel_pin) as f32); + // vref_int_mv has to be passed in because the ADC peripheral that + // it is connected to depends on the chip. + pub fn read_volt_raw_f32(&mut self, vref_int_read_mv: f32, vref_int_cal: f32) -> f32 { + // Based off of this: http://www.efton.sk/STM32/STM32_VREF.pdf + // vmeas = vcal * MEAS / MAX * CAL / REFINT (4) + return V_CAL_MV * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32) * vref_int_cal / vref_int_read_mv; } } \ No newline at end of file From 0132e70f2906f64f74ad6ba25909a631fb42ce83 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 16 Jun 2024 22:11:59 -0400 Subject: [PATCH 077/157] radio debug work --- control-board/src/drivers/radio_robot.rs | 4 ++ control-board/src/tasks/control_task.rs | 7 +-- control-board/src/tasks/kicker_task.rs | 5 +- control-board/src/tasks/radio_task.rs | 75 ++++-------------------- control-board/src/tasks/user_io_task.rs | 27 ++++++--- lib-stm32/src/drivers/radio/odin_w26x.rs | 2 + 6 files changed, 41 insertions(+), 79 deletions(-) diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index f4b78086..67817e04 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -340,6 +340,7 @@ impl< Ok(Some(ret)) }, Err(_) => { + defmt::trace!("try read data failed after can read data reported data ready"); Err(()) }, } @@ -347,6 +348,7 @@ impl< Ok(None) } } else { + defmt::trace!("peer was none"); Err(()) } } @@ -575,6 +577,7 @@ impl< Err(_) => { // we got data that was a valid EDM DataPacket, but couldn't parse it // into any known A-Team packet format + defmt::trace!("got EDM packet but wasn't A-Team"); return Err(()); }, } @@ -585,6 +588,7 @@ impl< } }, Err(_) => { + defmt::trace!("radio in invalid state"); // read_data_nonblocking failed because the radio was in an invalid state return Err(()) }, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 094b2c2d..a7adae32 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -45,7 +45,7 @@ make_uart_queue_pair!(DRIB, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); -const TICKS_WITHOUT_PACKET_STOP: usize = 10; +const TICKS_WITHOUT_PACKET_STOP: usize = 20; const BATTERY_MIN_VOLTAGE: f32 = 18.0; const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); @@ -234,8 +234,6 @@ impl < latest_control.vel_z_angular, ); - defmt::info!("ControlTask - got c2 packet ({}, {}, {})", new_cmd_vel.x, new_cmd_vel.y, new_cmd_vel.z); - cmd_vel = new_cmd_vel; drib_vel = latest_control.dribbler_speed; ticks_since_packet = 0; @@ -252,9 +250,6 @@ impl < } } - defmt::info!("ControlTask - cmd_vel ({}, {}, {})", cmd_vel.x, cmd_vel.y, cmd_vel.z); - - // now we have setpoint r(t) in self.cmd_vel // let battery_v = battery_sub.next_message_pure().await as f32; let battery_v = 25.0; diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 2e664332..704ae2ee 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -95,11 +95,12 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ match self.kicker_task_state { KickerTaskState::PoweredOff => { - if cur_robot_state.hw_init_state_valid && cur_robot_state.shutdown_requested { + if cur_robot_state.hw_init_state_valid && !cur_robot_state.shutdown_requested { self.kicker_task_state = KickerTaskState::PowerOn; } }, KickerTaskState::PowerOn => { + self.remote_power_btn_hold().await; self.remote_power_btn_press().await; // power should be coming on, attempt connection self.kicker_task_state = KickerTaskState::ConnectUart; @@ -146,7 +147,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ async fn remote_power_btn_hold(&mut self) { self.remote_power_btn.set_high(); - Timer::after_millis(1500).await; + Timer::after_millis(1000).await; self.remote_power_btn.set_low(); Timer::after_millis(10).await; } diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 63d4250a..820218ef 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,5 +1,5 @@ -use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; +use ateam_common_packets::{bindings_radio::{BasicControl, BasicTelemetry}, radio::{DataPacket, TelemetryPacket}}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; @@ -251,7 +251,7 @@ impl< } }, RadioConnectionState::Connected => { - let _ = self.process_packets2().await; + let _ = self.process_packets().await; // if we're stably connected, process packets at 100Hz }, } @@ -331,14 +331,18 @@ impl< } } - async fn process_packets2(&mut self) -> Result<(), ()> { + async fn process_packets(&mut self) -> Result<(), ()> { // read any packets - if let Ok(pkt) = self.radio.read_packet_nonblocking() { - if let Some(c2_pkt) = pkt { - self.command_publisher.publish_immediate(c2_pkt); - } - } else { - defmt::warn!("RadioTask - error reading data packet"); + loop { + if let Ok(pkt) = self.radio.read_packet_nonblocking() { + if let Some(c2_pkt) = pkt { + self.command_publisher.publish_immediate(c2_pkt); + } else { + break; + } + } else { + defmt::warn!("RadioTask - error reading data packet"); + } } if let Some(telemetry) = self.telemetry_subscriber.try_next_message_pure() { @@ -366,59 +370,6 @@ impl< return Ok(()) } - - async fn process_packets(&mut self) -> Result<(), ()> { - match select(self.radio.read_packet(), self.telemetry_subscriber.next_message()).await { - Either::First(res) => { - if let Ok(c2_pkt) = res { - self.command_publisher.publish_immediate(c2_pkt); - Ok(()) - } else { - defmt::warn!("radio read packet returned an error"); - Err(()) - } - } - Either::Second(telem_msg) => { - match telem_msg { - WaitResult::Lagged(num_missed_pkts) => { - defmt::warn!("radio task missed sending {} outbound packets. Should channel have higher capacity?", num_missed_pkts); - Ok(()) - }, - WaitResult::Message(msg) => { - match msg { - TelemetryPacket::Basic(basic_telem_pkt) => { - let res = self.radio.send_telemetry(basic_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send basic telemetry packet. Is the backing queue too small?"); - Err(()) - } else { - Ok(()) - } - } - TelemetryPacket::Control(control_telem_pkt) => { - let res = self.radio.send_control_debug_telemetry(control_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); - Err(()) - } else { - Ok(()) - } - } - TelemetryPacket::ParameterCommandResponse(param_resp) => { - let res = self.radio.send_parameter_response(param_resp).await; - if res.is_err() { - defmt::warn!("radio task failed to send param resp packet. Is the backing queue too small?"); - Err(()) - } else { - Ok(()) - } - } - } - } - } - } - } - } } pub fn startup_uart_config() -> usart::Config { diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index cd7c81e9..724f5193 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -19,7 +19,7 @@ use ateam_lib_stm32::drivers::switches::rotary_encoder::RotaryEncoder; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; -use crate::pins::*; +use crate::{pins::*, stm32_interface}; use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; // #[link_section = ".sram4"] @@ -60,15 +60,22 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, // let mut sd_anim_seq = [shutdown_anim]; // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); + const SHUTDOWN_ANIM_ID: usize = 2; + let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(10000), AnimRepeatMode::None)); + let mut shutdown_dimmer_arr = [shutdown_dimmer]; + let shutdown_ca = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_dimmer_arr, AnimRepeatMode::None); + + let mut led0_anim_playbook = [shutdown_ca]; + + const RGB_LERP_ID: usize = 1; let anim0 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); let anim1 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); let anim2 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); let mut anim_seq = [anim0, anim1, anim2]; - const RGB_LERP_ID: usize = 1; let composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut anim_seq, AnimRepeatMode::Forever); // let mut composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut [anim0, anim1, anim2], AnimRepeatMode::Forever); - let mut led0_anim_playbook = [composite_anim0]; + let mut led1_anim_playbook = [composite_anim0]; // composite_anim0.start_animation(); @@ -84,8 +91,8 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, // ]; let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ + Some(&mut led1_anim_playbook), Some(&mut led0_anim_playbook), - None, ]; // let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); @@ -99,7 +106,7 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, let mut dotstars_anim = Apa102Anim::new(dotstars, animation_playbooks); dotstars_anim.enable_animation(0, RGB_LERP_ID); - dotstars_anim.enable_animation(1, RGB_LERP_ID); + // dotstars_anim.enable_animation(1, RGB_LERP_ID); // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); // color_lerp.start(); @@ -158,10 +165,10 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, debug_led0.set_low(); } - // if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { - // shutdown_anim.start_animation(); - // // dotstars_anim.set_animation(&shutdown_anim, 0); - // } + if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { + dotstars_anim.disable_animation(0, RGB_LERP_ID); + dotstars_anim.enable_animation(1, SHUTDOWN_ANIM_ID); + } // let color = color_lerp.update(); // dotstars.set_color(color, 0); @@ -174,6 +181,8 @@ async fn user_io_task_entry(robot_state: &'static SharedRobotState, robot_state.set_hw_init_state_valid(true); } + prev_robot_state = cur_robot_state; + Timer::after_millis(50).await; } } diff --git a/lib-stm32/src/drivers/radio/odin_w26x.rs b/lib-stm32/src/drivers/radio/odin_w26x.rs index 51d252bd..a1f34f97 100644 --- a/lib-stm32/src/drivers/radio/odin_w26x.rs +++ b/lib-stm32/src/drivers/radio/odin_w26x.rs @@ -467,10 +467,12 @@ impl< if let EdmPacket::DataEvent { channel: _ , data } = pkt { return Ok(fn_read(data)) } else { + // defmt::trace!("got non data event"); return Err(()); } }, Err(_) => { + // defmt::trace!("got data that wasn't an edm packet: {}", buf.data()); return Err(()); }, } From 69fd0a8cef10350da406e2f711205e34169ac5bc Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 16 Jun 2024 23:40:56 -0400 Subject: [PATCH 078/157] fix firmware images --- control-board/Cargo.toml | 2 +- control-board/src/bin/hwtest-adc/main.rs | 60 +++ control-board/src/bin/hwtest-enc/control.rs | 435 ------------------ control-board/src/bin/hwtest-enc/main.rs | 257 ----------- control-board/src/bin/hwtest-imu/main.rs | 8 +- control-board/src/bin/hwtest-piezo/main.rs | 9 +- control-board/src/bin/hwtest-radio/main.rs | 8 +- control-board/src/bin/hwtest-shutdown/main.rs | 10 +- control-board/src/lib.rs | 9 +- control-board/src/tasks/user_io_task.rs | 12 +- lib-stm32/src/drivers/other/adc_helper.rs | 27 +- 11 files changed, 123 insertions(+), 714 deletions(-) create mode 100644 control-board/src/bin/hwtest-adc/main.rs delete mode 100644 control-board/src/bin/hwtest-enc/control.rs delete mode 100644 control-board/src/bin/hwtest-enc/main.rs diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index 57877990..0839eeda 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -82,7 +82,7 @@ test = false harness = false [[bin]] -name = "hwtest-enc" +name = "hwtest-adc" test = false harness = false diff --git a/control-board/src/bin/hwtest-adc/main.rs b/control-board/src/bin/hwtest-adc/main.rs new file mode 100644 index 00000000..b0c5ffbc --- /dev/null +++ b/control-board/src/bin/hwtest-adc/main.rs @@ -0,0 +1,60 @@ +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_stm32::adc::{Adc, SampleTime}; +use embassy_stm32::Config; +use embassy_time::Timer; +use {defmt_rtt as _, panic_probe as _}; + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let mut config = Config::default(); + { + use embassy_stm32::rcc::*; + config.rcc.hsi = Some(HSIPrescaler::DIV1); + config.rcc.csi = true; + config.rcc.pll1 = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV4, + mul: PllMul::MUL50, + divp: Some(PllDiv::DIV2), + divq: Some(PllDiv::DIV8), // SPI1 cksel defaults to pll1_q + divr: None, + }); + config.rcc.pll2 = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV4, + mul: PllMul::MUL50, + divp: Some(PllDiv::DIV20), // 100mhz + divq: None, + divr: None, + }); + config.rcc.sys = Sysclk::PLL1_P; // 400 Mhz + config.rcc.ahb_pre = AHBPrescaler::DIV2; // 200 Mhz + config.rcc.apb1_pre = APBPrescaler::DIV2; // 100 Mhz + config.rcc.apb2_pre = APBPrescaler::DIV2; // 100 Mhz + config.rcc.apb3_pre = APBPrescaler::DIV2; // 100 Mhz + config.rcc.apb4_pre = APBPrescaler::DIV2; // 100 Mhz + config.rcc.voltage_scale = VoltageScale::Scale1; + config.rcc.mux.adcsel = mux::Adcsel::PLL2_P; + } + let mut p = embassy_stm32::init(config); + + info!("Hello World!"); + + let mut adc = Adc::new(p.ADC3); + + adc.set_sample_time(SampleTime::CYCLES32_5); + + let mut vrefint_channel = adc.enable_vrefint(); + + loop { + let vrefint = adc.read(&mut vrefint_channel); + info!("vrefint: {}", vrefint); + let measured = adc.read(&mut p.PC0); + info!("measured: {}", measured); + Timer::after_millis(500).await; + } +} diff --git a/control-board/src/bin/hwtest-enc/control.rs b/control-board/src/bin/hwtest-enc/control.rs deleted file mode 100644 index 538679ec..00000000 --- a/control-board/src/bin/hwtest-enc/control.rs +++ /dev/null @@ -1,435 +0,0 @@ -use ateam_control_board::{ - include_external_cpp_bin, - motion::robot_model::{RobotConstants, RobotModel}, - stm32_interface::Stm32Interface, - stspin_motor::{DribblerMotor, WheelMotor}, -}; -use ateam_lib_stm32::{ - queue::Buffer, - uart::queue::{UartReadQueue, UartWriteQueue}, -}; -use embassy_executor::SendSpawner; -use embassy_stm32::{ - gpio::{Level, Output, Speed}, - usart::Uart, -}; -use embassy_time::{Duration, Timer}; -use nalgebra::{Vector3, Vector4}; - -use ateam_control_board::pins::*; - -include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} -include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} - -// motor pinout -// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 -// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 -// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 -// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 -// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 - -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 20; - -create_uart_ - -// buffers for front right -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_TX: UartWriteQueue< - MotorFRUart, - MotorFRDmaTx, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_RX: UartReadQueue< - MotorFRUart, - MotorFRDmaRx, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_RX }); - -// buffers for front left -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_TX: UartWriteQueue< - MotorFLUart, - MotorFLDmaTx, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_RX: UartReadQueue< - MotorFLUart, - MotorFLDmaRx, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_RX }); - -// buffers for back left -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_TX: UartWriteQueue< - MotorBLUart, - MotorBLDmaTx, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_RX: UartReadQueue< - MotorBLUart, - MotorBLDmaRx, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_RX }); - -// buffers for back right -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_TX: UartWriteQueue< - MotorBRUart, - MotorBRDmaTx, - MAX_TX_PACKET_SIZE, - TX_BUF_DEPTH, -> = UartWriteQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_RX: UartReadQueue< - MotorBRUart, - MotorBRDmaRx, - MAX_RX_PACKET_SIZE, - RX_BUF_DEPTH, -> = UartReadQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_RX }); - -// buffers for dribbler -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static DRIB_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut DRIB_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static DRIB_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut DRIB_BUFFERS_RX }); - -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -pub struct MotorsError { - pub front_right: T, - pub front_left: T, - pub back_right: T, - pub back_left: T, - pub drib: T, -} - -pub struct Control { - robot_model: RobotModel, - cmd_vel: Vector3, - drib_vel: f32, - pub front_right_motor: WheelMotor< - 'static, - MotorFRUart, - MotorFRDmaRx, - MotorFRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFRBootPin, - MotorFRResetPin, - >, - pub front_left_motor: WheelMotor< - 'static, - MotorFLUart, - MotorFLDmaRx, - MotorFLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFLBootPin, - MotorFLResetPin, - >, - pub back_left_motor: WheelMotor< - 'static, - MotorBLUart, - MotorBLDmaRx, - MotorBLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBLBootPin, - MotorBLResetPin, - >, - pub back_right_motor: WheelMotor< - 'static, - MotorBRUart, - MotorBRDmaRx, - MotorBRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBRBootPin, - MotorBRResetPin, - >, - pub drib_motor: DribblerMotor< - 'static, - MotorDUart, - MotorDDmaRx, - MotorDDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorDBootPin, - MotorDResetPin, - >, -} - -impl Control { - pub fn new( - spawner: &SendSpawner, - front_right_usart: Uart<'static, MotorFRUart, MotorFRDmaTx, MotorFRDmaRx>, - front_left_usart: Uart<'static, MotorFLUart, MotorFLDmaTx, MotorFLDmaRx>, - back_left_usart: Uart<'static, MotorBLUart, MotorBLDmaTx, MotorBLDmaRx>, - back_right_usart: Uart<'static, MotorBRUart, MotorBRDmaTx, MotorBRDmaRx>, - drib_usart: Uart<'static, MotorDUart, MotorDDmaTx, MotorDDmaRx>, - front_right_boot0_pin: MotorFRBootPin, - front_left_boot0_pin: MotorFLBootPin, - back_left_boot0_pin: MotorBLBootPin, - back_right_boot0_pin: MotorBRBootPin, - drib_boot0_pin: MotorDBootPin, - front_right_reset_pin: MotorFRResetPin, - front_left_reset_pin: MotorFLResetPin, - back_left_reset_pin: MotorBLResetPin, - back_right_reset_pin: MotorBRResetPin, - drib_reset_pin: MotorDResetPin, - ball_detected_thresh: f32, - ) -> Control { - let wheel_firmware_image = WHEEL_FW_IMG; - let drib_firmware_image = DRIB_FW_IMG; - - let (front_right_tx, front_right_rx) = front_right_usart.split(); - let (front_left_tx, front_left_rx) = front_left_usart.split(); - let (back_left_tx, back_left_rx) = back_left_usart.split(); - let (back_right_tx, back_right_rx) = back_right_usart.split(); - let (drib_tx, drib_rx) = drib_usart.split(); - - let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - - let front_right_reset_pin = Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active - let front_left_reset_pin = Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_left_reset_pin = Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_right_reset_pin = Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active - let drib_reset_pin = Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active - - spawner - .spawn(FRONT_RIGHT_QUEUE_RX.spawn_task(front_right_rx)) - .unwrap(); - spawner - .spawn(FRONT_RIGHT_QUEUE_TX.spawn_task(front_right_tx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_RX.spawn_task(front_left_rx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_TX.spawn_task(front_left_tx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_QUEUE_RX.spawn_task(back_left_rx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_QUEUE_TX.spawn_task(back_left_tx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_RX.spawn_task(back_right_rx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_TX.spawn_task(back_right_tx)) - .unwrap(); - spawner.spawn(DRIB_QUEUE_RX.spawn_task(drib_rx)).unwrap(); - spawner.spawn(DRIB_QUEUE_TX.spawn_task(drib_tx)).unwrap(); - - let front_right_stm32_interface = Stm32Interface::new( - &FRONT_RIGHT_QUEUE_RX, - &FRONT_RIGHT_QUEUE_TX, - Some(front_right_boot0_pin), - Some(front_right_reset_pin), - ); - let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); - - let front_left_stm32_interface = Stm32Interface::new( - &FRONT_LEFT_QUEUE_RX, - &FRONT_LEFT_QUEUE_TX, - Some(front_left_boot0_pin), - Some(front_left_reset_pin), - ); - let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); - - let back_left_stm32_interface = Stm32Interface::new( - &BACK_LEFT_QUEUE_RX, - &BACK_LEFT_QUEUE_TX, - Some(back_left_boot0_pin), - Some(back_left_reset_pin), - ); - let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); - - let back_right_stm32_interface = Stm32Interface::new( - &BACK_RIGHT_QUEUE_RX, - &BACK_RIGHT_QUEUE_TX, - Some(back_right_boot0_pin), - Some(back_right_reset_pin), - ); - let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); - - let drib_stm32_interface = Stm32Interface::new( - &DRIB_QUEUE_RX, - &DRIB_QUEUE_TX, - Some(drib_boot0_pin), - Some(drib_reset_pin), - ); - let drib_motor = DribblerMotor::new( - drib_stm32_interface, - drib_firmware_image, - ball_detected_thresh, - ); - - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - Control { - robot_model, - cmd_vel: Vector3::new(0., 0., 0.), - drib_vel: 0.0, - front_right_motor, - front_left_motor, - back_left_motor, - back_right_motor, - drib_motor, - } - } - - pub async fn load_firmware(&mut self) -> Result<(), MotorsError> { - let ret = embassy_futures::join::join5( - self.front_right_motor.load_default_firmware_image(), - self.front_left_motor.load_default_firmware_image(), - self.back_left_motor.load_default_firmware_image(), - self.back_right_motor.load_default_firmware_image(), - self.drib_motor.load_default_firmware_image(), - ) - .await; - - if ret.0.is_err() || ret.1.is_err() || ret.2.is_err() || ret.3.is_err() || ret.4.is_err() { - return Err(MotorsError { - front_right: ret.0.is_err(), - front_left: ret.1.is_err(), - back_left: ret.2.is_err(), - back_right: ret.3.is_err(), - drib: ret.4.is_err(), - }); - } - - embassy_futures::join::join5( - self.front_right_motor.leave_reset(), - self.front_left_motor.leave_reset(), - self.back_left_motor.leave_reset(), - self.back_right_motor.leave_reset(), - self.drib_motor.leave_reset(), - ) - .await; - - self.front_right_motor.set_telemetry_enabled(true); - self.front_left_motor.set_telemetry_enabled(true); - self.back_left_motor.set_telemetry_enabled(true); - self.back_right_motor.set_telemetry_enabled(true); - self.drib_motor.set_telemetry_enabled(true); - - Timer::after(Duration::from_millis(10)).await; - - Ok(()) - } - - pub fn tick(&mut self, vel_angular: f32, vel_drib: f32) { - self.front_right_motor.process_packets(); - self.front_left_motor.process_packets(); - self.back_left_motor.process_packets(); - self.back_right_motor.process_packets(); - self.drib_motor.process_packets(); - - self.front_right_motor.log_reset("FR"); - self.front_left_motor.log_reset("RL"); - self.back_left_motor.log_reset("BL"); - self.back_right_motor.log_reset("BR"); - self.drib_motor.log_reset("DRIB"); - - if self.drib_motor.ball_detected() { - defmt::info!("ball detected"); - } - - let cmd_vel = Vector3::new(0., 0., vel_angular); - self.cmd_vel = cmd_vel; - self.drib_vel = vel_drib; - let wheel_vels = self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel); - - self.front_right_motor.set_setpoint(wheel_vels[0]); - self.front_left_motor.set_setpoint(wheel_vels[1]); - self.back_left_motor.set_setpoint(wheel_vels[2]); - self.back_right_motor.set_setpoint(wheel_vels[3]); - let drib_dc = self.drib_vel / 1000.0; - self.drib_motor.set_setpoint(drib_dc); - - self.front_right_motor.send_motion_command(); - self.front_left_motor.send_motion_command(); - self.back_left_motor.send_motion_command(); - self.back_right_motor.send_motion_command(); - self.drib_motor.send_motion_command(); - } -} diff --git a/control-board/src/bin/hwtest-enc/main.rs b/control-board/src/bin/hwtest-enc/main.rs deleted file mode 100644 index 2b3a6a06..00000000 --- a/control-board/src/bin/hwtest-enc/main.rs +++ /dev/null @@ -1,257 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] - -use control::Control; -use defmt_rtt as _; -use embassy_time::{Duration, Ticker, Timer}; -use panic_probe as _; - -use apa102_spi::Apa102; -use ateam_control_board::{ - colors::*, - stm32_interface::get_bootloader_uart_config}; -use embassy_stm32::{ - dma::NoDma, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt, - spi, - time::{hz, mhz}, - usart::Uart, -}; -use embassy_executor::InterruptExecutor; -use smart_leds::SmartLedsWrite; -use static_cell::StaticCell; - -mod control; - -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); - -// Angular velocity commanded in rads/sec -const ROBOT_VEL_ANGULAR: f32 = 10.; -// Expected RPM read from encoder for commanded velocity when wheels have no load -const EXPECTED_RPM_NO_LOAD: f32 = 370.; -// Allowed tolerance for expected RPM in percent -const EXPECTED_RPM_TOLERANCE: f32 = 0.20; - -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - - // Delay so dotstar and STSPIN can turn on - Timer::after(Duration::from_millis(50)).await; - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - - let dot_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), - ); - let mut dotstar = Apa102::new(dot_spi); - dotstar - .write([COLOR_BLUE, COLOR_OFF].iter().cloned()) - .unwrap(); - - let btn0 = Input::new(p.PD5, Pull::None); - - let mut led0 = Output::new(p.PF3, Level::Low, Speed::Low); - let mut led1 = Output::new(p.PF2, Level::Low, Speed::Low); - let mut led2 = Output::new(p.PF1, Level::Low, Speed::Low); - let mut led3 = Output::new(p.PF0, Level::Low, Speed::Low); - - let front_right_int = interrupt::take!(USART1); - let front_left_int = interrupt::take!(UART4); - let back_left_int = interrupt::take!(UART7); - let back_right_int = interrupt::take!(UART8); - let drib_int = interrupt::take!(UART5); - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - front_right_int, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - front_left_int, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - back_left_int, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - back_right_int, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - drib_int, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - ); - - let ret = control.load_firmware().await; - if let Err(err) = ret { - if err.front_right { - defmt::error!("Error flashing FR"); - } else { - led3.set_high(); - } - if err.front_left { - defmt::error!("Error flashing FL"); - } else { - led0.set_high(); - } - if err.back_right { - defmt::error!("Error flashing BR"); - } else { - led2.set_high(); - } - if err.back_left { - defmt::error!("Error flashing BL"); - } else { - led1.set_high(); - } - if err.drib { - defmt::error!("Error flashing DRIB"); - } - dotstar - .write([COLOR_RED, COLOR_RED].iter().cloned()) - .unwrap(); - defmt::panic!("Error flashing STSPINs") - } else { - defmt::info!("All STSPINs flashed correctly") - } - - dotstar.write([COLOR_YELLOW].iter().cloned()).unwrap(); - - loop { - while btn0.is_high() {} - while btn0.is_low() {} - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); - - loop { - main_loop_rate_ticker.next().await; - control.tick(ROBOT_VEL_ANGULAR, 64.); - - let err_fr = control.front_right_motor.read_is_error(); - let err_fl = control.front_left_motor.read_is_error(); - let err_br = control.back_right_motor.read_is_error(); - let err_bl = control.back_left_motor.read_is_error(); - let err_drib = control.drib_motor.read_is_error(); - - let rpm_fr = control.front_right_motor.read_rpm(); - let rpm_fl = control.front_left_motor.read_rpm(); - let rpm_br = control.back_right_motor.read_rpm(); - let rpm_bl = control.back_left_motor.read_rpm(); - - let rpm_ok_fr = rpm_fr > EXPECTED_RPM_NO_LOAD * (1. - EXPECTED_RPM_TOLERANCE) - && rpm_fr < EXPECTED_RPM_NO_LOAD * (1. + EXPECTED_RPM_TOLERANCE); - let rpm_ok_fl = rpm_fl > EXPECTED_RPM_NO_LOAD * (1. - EXPECTED_RPM_TOLERANCE) - && rpm_fl < EXPECTED_RPM_NO_LOAD * (1. + EXPECTED_RPM_TOLERANCE); - let rpm_ok_br = rpm_br > EXPECTED_RPM_NO_LOAD * (1. - EXPECTED_RPM_TOLERANCE) - && rpm_br < EXPECTED_RPM_NO_LOAD * (1. + EXPECTED_RPM_TOLERANCE); - let rpm_ok_bl = rpm_bl > EXPECTED_RPM_NO_LOAD * (1. - EXPECTED_RPM_TOLERANCE) - && rpm_bl < EXPECTED_RPM_NO_LOAD * (1. + EXPECTED_RPM_TOLERANCE); - - if err_fr || err_fl || err_br || err_bl || err_drib { - dotstar - .write([COLOR_BLUE, COLOR_RED].iter().cloned()) - .unwrap(); - } else if !rpm_ok_fr || !rpm_ok_fl || !rpm_ok_br || !rpm_ok_bl { - dotstar - .write([COLOR_BLUE, COLOR_YELLOW].iter().cloned()) - .unwrap(); - } else { - dotstar - .write([COLOR_BLUE, COLOR_GREEN].iter().cloned()) - .unwrap(); - } - - if !err_fl && rpm_ok_fl { - led0.set_high(); - } else { - led0.set_low(); - } - if !err_bl && rpm_ok_bl { - led1.set_high(); - } else { - led1.set_low(); - } - if !err_br && rpm_ok_br { - led2.set_high(); - } else { - led2.set_low(); - } - if !err_fr && rpm_ok_fr { - led3.set_high(); - } else { - led3.set_low(); - } - - if btn0.is_low() { - while btn0.is_low() {} - break; - } - } - - dotstar.write([COLOR_YELLOW].iter().cloned()).unwrap(); - } -} diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index 01797460..0fabea2d 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -8,7 +8,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{create_audio_task, create_imu_task, create_io_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, GyroDataPubSub}, robot_state::SharedRobotState}; +use ateam_control_board::{create_audio_task, create_imu_task, create_io_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, GyroDataPubSub}, robot_state::SharedRobotState}; use embassy_time::Timer; // provide embedded panic probe @@ -19,6 +19,7 @@ static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(Sha static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -51,11 +52,14 @@ async fn main(main_spawner: embassy_executor::Spawner) { let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); let mut imu_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + + /////////////////// // start tasks // /////////////////// - create_io_task!(main_spawner, robot_state, p); + create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); create_shutdown_task!(main_spawner, robot_state, p); diff --git a/control-board/src/bin/hwtest-piezo/main.rs b/control-board/src/bin/hwtest-piezo/main.rs index 22123c9c..2652a788 100644 --- a/control-board/src/bin/hwtest-piezo/main.rs +++ b/control-board/src/bin/hwtest-piezo/main.rs @@ -9,9 +9,10 @@ use embassy_stm32::{ use defmt_rtt as _; -use ateam_control_board::{create_io_task, get_system_config, robot_state::SharedRobotState, songs::TEST_SONG}; +use ateam_control_board::{create_io_task, get_system_config, pins::BatteryVoltPubSub, robot_state::SharedRobotState, songs::TEST_SONG}; +use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; @@ -19,6 +20,8 @@ use static_cell::ConstStaticCell; static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); + static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[interrupt] @@ -44,6 +47,9 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup inter-task coms channels // ////////////////////////////////////// + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + + /////////////////// // start tasks // /////////////////// @@ -52,6 +58,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_io_task!(main_spawner, robot_state, + battery_volt_publisher, p); let ch2 = PwmPin::new_ch2(p.PE6, OutputType::PushPull); diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index a191e196..dfe61337 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -11,7 +11,7 @@ use embassy_sync::pubsub::{PubSubChannel, WaitResult}; use defmt_rtt as _; -use ateam_control_board::{create_io_task, create_radio_task, get_system_config, pins::{CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; +use ateam_control_board::{create_io_task, create_radio_task, get_system_config, pins::{BatteryVoltPubSub, CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; // load credentials from correct crate @@ -29,6 +29,7 @@ static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(Sha static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -68,6 +69,9 @@ async fn main(main_spawner: embassy_executor::Spawner) { let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + + /////////////////// // start tasks // /////////////////// @@ -77,7 +81,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { radio_command_publisher, radio_telemetry_subscriber, wifi_credentials, p); - create_io_task!(main_spawner, robot_state, p); + create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); loop { match select::select(control_command_subscriber.next_message(), Timer::after_millis(1000)).await { diff --git a/control-board/src/bin/hwtest-shutdown/main.rs b/control-board/src/bin/hwtest-shutdown/main.rs index f03079f0..71f5e437 100644 --- a/control-board/src/bin/hwtest-shutdown/main.rs +++ b/control-board/src/bin/hwtest-shutdown/main.rs @@ -6,9 +6,10 @@ use embassy_stm32::interrupt; use defmt_rtt as _; -use ateam_control_board::{create_io_task, create_shutdown_task, get_system_config, robot_state::SharedRobotState}; +use ateam_control_board::{create_io_task, create_shutdown_task, get_system_config, pins::BatteryVoltPubSub, robot_state::SharedRobotState}; +use embassy_sync::pubsub::PubSubChannel; use embassy_time::Timer; // provide embedded panic probe use panic_probe as _; @@ -16,6 +17,8 @@ use static_cell::ConstStaticCell; static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); + static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); #[interrupt] @@ -41,11 +44,14 @@ async fn main(main_spawner: embassy_executor::Spawner) { // setup inter-task coms channels // ////////////////////////////////////// + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + + /////////////////// // start tasks // /////////////////// - create_io_task!(main_spawner, robot_state, p); + create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); create_shutdown_task!(main_spawner, robot_state, p); diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 884fe776..cffc5169 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -82,8 +82,8 @@ pub const fn adc_raw_to_v(adc_raw: f32) -> f32 { adc_raw / ADC_VREFINT_NOMINAL } -pub const fn adc_v_to_battery_voltage(adc_mv: f32) -> f32 { - (adc_mv / 2.762) * BATTERY_MAX_VOLTAGE +pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { + (adc_v / 1.966) * BATTERY_MAX_VOLTAGE } pub fn get_system_config() -> Config { @@ -117,7 +117,7 @@ pub fn get_system_config() -> Config { source: PllSource::HSE, prediv: PllPreDiv::DIV1, mul: PllMul::MUL31, - divp: Some(PllDiv::DIV5), // 49.6 MHz + divp: Some(PllDiv::DIV7), // 35.8 MHz divq: Some(PllDiv::DIV2), // 124 MHz divr: Some(PllDiv::DIV1) // 248 MHz }); @@ -149,7 +149,8 @@ pub fn get_system_config() -> Config { config.rcc.mux.usart16910sel = Usart16910sel::PCLK2; // 136 MHz config.rcc.mux.spi6sel = Spi6sel::PCLK4; // 136 MHz config.rcc.mux.sdmmcsel = Sdmmcsel::PLL2_R; // 248 MHz - config.rcc.mux.adcsel = Adcsel::PLL3_R; // 124 MHz + // config.rcc.mux.adcsel = Adcsel::PLL3_R; // 124 MHz + config.rcc.mux.adcsel = Adcsel::PLL2_P; config.rcc.mux.usbsel = Usbsel::PLL3_Q; // 124 MHz config.rcc.voltage_scale = VoltageScale::Scale0; diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 6c919bf1..7ff4255a 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -21,7 +21,7 @@ use embassy_stm32::adc::{Adc, SampleTime, Resolution}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; -use crate::{pins::*, stm32_interface}; +use crate::{adc_v_to_battery_voltage, pins::*, stm32_interface}; use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; // #[link_section = ".sram4"] @@ -171,7 +171,9 @@ async fn user_io_task_entry( defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); } - battery_volt_publisher.publish_immediate(battery_volt_adc.read_volt_raw_f32(vref_int_adc.read(&mut vref_int_ch) as f32, vref_int_cal)); + let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); + let batt_res_div_v = battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal); + battery_volt_publisher.publish_immediate(adc_v_to_battery_voltage(batt_res_div_v)); // TODO read messages @@ -246,11 +248,11 @@ pub async fn start_io_task(spawner: &Spawner, let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); - let battery_volt_adc = AdcHelper::new(battery_adc_peri, battery_adc_pin, SampleTime::CYCLES810_5, Resolution::BITS8); + let battery_volt_adc = AdcHelper::new(battery_adc_peri, battery_adc_pin, SampleTime::CYCLES32_5, Resolution::BITS12); let mut vref_int_adc = Adc::new(vref_int_adc_peri); // Set the Vref_int ADC settings to the same as the battery. - vref_int_adc.set_resolution(Resolution::BITS8); - vref_int_adc.set_sample_time(SampleTime::CYCLES810_5); + vref_int_adc.set_resolution(Resolution::BITS12); + vref_int_adc.set_sample_time(SampleTime::CYCLES32_5); spawner.spawn(user_io_task_entry(robot_state, adv_usr_btn0, adv_usr_btn1, battery_volt_publisher, battery_volt_adc, vref_int_adc, dip_switch, robot_id_rotary, debug_led0, robot_id_indicator, dotstars)).unwrap(); } \ No newline at end of file diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 6828a691..37c624e3 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -4,7 +4,7 @@ use embassy_stm32::Peripheral; // The voltage which the internal ADC were calibrated at. // For the H743 and F407 -const V_CAL_MV: f32 = 3.3; +const V_CAL_V: f32 = 3.3; pub struct AdcHelper<'a, T: adc::Instance, Ch: AdcChannel> { inst: Adc<'a, T>, @@ -32,8 +32,14 @@ impl< adc_inst.set_sample_time(sample_time); adc_inst.set_resolution(resolution); - // Use resolution to calculate the max ADC min quantity. - let adc_bins = u32::pow(2, resolution.to_bits() as u32) - 1; + // Use resolution to calculate the max ADC min quantity.' + let res_bits: u32 = match resolution { + Resolution::BITS12 => 12, + Resolution::BITS10 => 10, + Resolution::BITS8 => 8, + _ => 0, + }; + let adc_bins = u32::pow(2, res_bits) - 1; AdcHelper { inst: adc_inst, pin: pin, @@ -43,9 +49,20 @@ impl< // vref_int_mv has to be passed in because the ADC peripheral that // it is connected to depends on the chip. - pub fn read_volt_raw_f32(&mut self, vref_int_read_mv: f32, vref_int_cal: f32) -> f32 { + pub fn read_volt_raw_f32(&mut self, mut vref_int_read_mv: f32, vref_int_cal: f32) -> f32 { // Based off of this: http://www.efton.sk/STM32/STM32_VREF.pdf // vmeas = vcal * MEAS / MAX * CAL / REFINT (4) - return V_CAL_MV * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32) * vref_int_cal / vref_int_read_mv; + + defmt::info!("V_CAL_MV: {}", V_CAL_V); + defmt::info!("inst.read(): {}", self.inst.read(&mut self.pin) as f32); + defmt::info!("adc_bins: {}", self.adc_bins as f32); + defmt::info!("vref_int_cal: {}", vref_int_cal); + defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); + + // FIXME, embassy API appears to not correctly load vref_int_read + // override for now + vref_int_read_mv = 1509.0; + + return V_CAL_V * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32) * vref_int_cal / vref_int_read_mv; } } \ No newline at end of file From 3cb89dea417f1e93ba0376b4510b699294913454 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 18 Jun 2024 16:22:22 -0700 Subject: [PATCH 079/157] Manually moved changes --- control-board/src/bin/control/main.rs | 9 +- control-board/src/bin/hwtest-io/main.rs | 2 + control-board/src/lib.rs | 5 + .../src/motion/constant_gain_kalman_filter.rs | 97 +++--- .../motion/params/body_vel_filter_params.rs | 75 +++-- .../src/motion/params/body_vel_pid_params.rs | 15 +- control-board/src/motion/params/mod.rs | 4 +- .../motion/params/robot_physical_params.rs | 4 + control-board/src/motion/pid.rs | 44 +-- control-board/src/motion/robot_controller.rs | 301 +++++++----------- control-board/src/motion/robot_model.rs | 41 +-- control-board/src/stspin_motor.rs | 8 + control-board/src/tasks/control_task.rs | 32 +- motor-controller/bin/dribbler/main.c | 10 +- motor-controller/bin/dribbler/system.h | 1 + motor-controller/bin/wheel/main.c | 30 +- motor-controller/bin/wheel/main.h | 2 + motor-controller/bin/wheel/system.h | 2 +- motor-controller/common/6step.h | 2 +- motor-controller/common/motor_model.c | 15 +- motor-controller/common/motor_model.h | 2 + motor-controller/common/pid.c | 6 +- motor-controller/common/pid.h | 2 +- motor-controller/common/quadrature_encoder.c | 30 +- motor-controller/common/quadrature_encoder.h | 1 - 25 files changed, 356 insertions(+), 384 deletions(-) create mode 100644 control-board/src/motion/params/robot_physical_params.rs diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 9e0658f3..70716f93 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -9,8 +9,12 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; - +use ateam_control_board::{ + create_imu_task, create_io_task, create_radio_task, create_shutdown_task, + get_system_config, + pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, + robot_state::SharedRobotState, + tasks::control_task::start_control_task}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -123,7 +127,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; loop { - defmt::info!("{}", battery_volt_subscriber.next_message_pure().await); Timer::after_millis(10).await; } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-io/main.rs b/control-board/src/bin/hwtest-io/main.rs index c63bb9bb..fd15808a 100644 --- a/control-board/src/bin/hwtest-io/main.rs +++ b/control-board/src/bin/hwtest-io/main.rs @@ -46,6 +46,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + let mut battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); /////////////////// // start tasks // @@ -54,6 +55,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); loop { + defmt::info!("Battery Voltage: {}", battery_volt_subscriber.next_message_pure().await); Timer::after_millis(1000).await; } } \ No newline at end of file diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index cffc5169..fcd14021 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -13,6 +13,7 @@ #![feature(const_fn_floating_point_arithmetic)] #![feature(sync_unsafe_cell)] #![feature(inherent_associated_types)] +#![feature(const_float_bits_conv)] use embassy_stm32::{ bind_interrupts, peripherals, rcc::{ @@ -86,6 +87,10 @@ pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { (adc_v / 1.966) * BATTERY_MAX_VOLTAGE } +pub const fn abs_f32(x: f32) -> f32 { + f32::from_bits(x.to_bits() & (i32::MAX as u32)) +} + pub fn get_system_config() -> Config { let mut config = Config::default(); diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs index bf5977e7..bc8fec50 100644 --- a/control-board/src/motion/constant_gain_kalman_filter.rs +++ b/control-board/src/motion/constant_gain_kalman_filter.rs @@ -1,81 +1,68 @@ use nalgebra::base::SMatrix; -#[allow(non_camel_case_types)] pub struct CgKalmanFilter<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBSERVATIONS: usize> { - F_k: &'a SMatrix, - B_k: &'a SMatrix, - H_k: &'a SMatrix, - Q_k: &'a SMatrix, - R_k: &'a SMatrix, - P_k: SMatrix, - K_k: &'a SMatrix, - x_hat: SMatrix, + state_transition: &'a SMatrix, + control_input: &'a SMatrix, + observation_model: &'a SMatrix, + process_cov: &'a SMatrix, + kalman_gain: &'a SMatrix, + estimate_cov: SMatrix, + state_estimate: SMatrix, + pred_state_estimate: SMatrix, + pred_estimate_cov: SMatrix, + measurement_residual: SMatrix } impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBSERVATIONS: usize> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { - pub fn new(F_k: &'a SMatrix, - B_k: &'a SMatrix, - H_k: &'a SMatrix, - Q_k: &'a SMatrix, - R_k: &'a SMatrix, - P_k: &'a SMatrix, - K_k: &'a SMatrix, - // TODO: ? accept starting state? - ) -> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { + pub fn new(state_transition: &'a SMatrix, + control_input: &'a SMatrix, + observation_model: &'a SMatrix, + process_cov: &'a SMatrix, + kalman_gain: &'a SMatrix, + estimate_cov: &'a SMatrix::, + ) -> CgKalmanFilter<'a, NUM_STATES, NUM_CONTROL_INPUTS, NUM_OBSERVATIONS> { let mut filter = CgKalmanFilter { - F_k: F_k, - B_k: B_k, - H_k: H_k, - Q_k: Q_k, - R_k: R_k, - P_k: SMatrix::::zeros(), - K_k: K_k, - x_hat: SMatrix::::zeros(), + state_transition: state_transition, + control_input: control_input, + observation_model: observation_model, + process_cov: process_cov, + kalman_gain: kalman_gain, + estimate_cov: SMatrix::::zeros(), + state_estimate: SMatrix::::zeros(), + pred_state_estimate: SMatrix::::zeros(), + pred_estimate_cov: SMatrix::::zeros(), + measurement_residual: SMatrix::::zeros() }; - filter.P_k.copy_from(P_k); + // Just initializing to non-zero, so don't need a permanent reference in struct + filter.estimate_cov.copy_from(estimate_cov); filter } pub fn predict(&mut self, u: &SMatrix) { - self.x_hat = self.F_k * self.x_hat + self.B_k * u; - let P_k_hat = self.F_k * self.P_k * self.F_k.transpose() + self.Q_k; - self.P_k.copy_from(&P_k_hat); + self.pred_state_estimate = self.state_transition * self.state_estimate + self.control_input * u; + self.pred_estimate_cov = self.state_transition * self.estimate_cov * self.state_transition.transpose() + self.process_cov; } - pub fn update(&mut self, z: &SMatrix) { - // y = z - H*x_hat - //let S: SMatrix = self.H_k * self.P_k * self.H_k.transpose() + self.R_k; - //let K: SMatrix = self.P_k * self.H_k.transpose() * S.try_inverse().unwrap(); - // x_hat += K*y - // P = (I - K*H)*P + pub fn update(&mut self, measurement: &SMatrix) { + let innovation_residual = measurement - self.observation_model * self.pred_state_estimate; - /* - defmt::info!("start K"); - for r in 0..3 { - for c in 0..5 { - defmt::info!("{:?}, ", K.row(r)[c]); - } - defmt::info!("___________"); - } - defmt::info!("end"); - */ - + // Constant gain so don't need Innovation Covariance / Optimal Kalman gain calculation - let z_hat: SMatrix = self.H_k * self.x_hat; - let y: SMatrix = z - z_hat; - self.x_hat += self.K_k * y; + self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; - // defmt::info!("x predictor: {:?}, {:?}, {:?}", self.x_hat[0], self.x_hat[1], self.x_hat[2]); - } + // self.estimate_cov = (SMatrix::::identity() - self.kalman_gain * self.observation_model) * self.pred_estimate_cov; - pub fn read_state(&self, x_hat: &mut SMatrix) { - x_hat.copy_from(&self.x_hat); + self.measurement_residual = measurement - self.observation_model * self.state_estimate; } pub fn get_state(&self) -> SMatrix { - self.x_hat + self.state_estimate + } + + pub fn get_measurement_residual(&self) -> SMatrix { + self.measurement_residual } } diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 8d7216fb..10b29218 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -1,56 +1,55 @@ use nalgebra::{matrix, Matrix3, Matrix3x4, Matrix3x5, Matrix5, Matrix5x3}; -pub const KfNumStates: usize = 3; -pub const KfNumControlInputs: usize = 4; -pub const KfNumObservations: usize = 5; +pub const KF_NUM_STATES: usize = 3; +pub const KF_NUM_CONTROL_INPUTS: usize = 4; +pub const KF_NUM_OBSERVATIONS: usize = 5; -const ExpectedDt: f32 = 10000.0; // 10mS = 10000uS -const ExpectedDt2: f32 = ExpectedDt * ExpectedDt; -const EncoderNoise: f32 = 0.11; // noise in rad / sampling time -const GyroNoise: f32 = 0.002; // BMI085 compensated for spectral noise at 100Hz sample rate -const ProcessNoise: f32 = 0.05; -const InitialCovariance: f32 = 0.11; +const EXPECTED_DT: f32 = 10000.0; // 10mS = 10000uS +const EXPECTED_DT_2: f32 = EXPECTED_DT * EXPECTED_DT; +const ENCODER_NOISE: f32 = 0.11; // noise in rad / sampling time +const GYRO_NOISE: f32 = 0.002; // BMI085 compensated for spectral noise at 100Hz sample rate +const PROCESS_NOISE: f32 = 0.05; +const INITIAL_COV: f32 = 0.11; // Assume constant velocity as a valid linearization of the transition system -pub static F: Matrix3 = +pub static STATE_TRANSITION: Matrix3 = matrix![1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0]; // Assume no input for the moment -pub static B: Matrix3x4 = +pub static CONTROL_INPUT: Matrix3x4 = matrix![0.0, 0.0, 0.0, 0.0; 0.0, 0.0, 0.0, 0.0; 0.0, 0.0, 0.0, 0.0]; - -pub static H: Matrix5x3 = - matrix![35.34797566, 20.40816327, 3.46938776; - -35.34797566, 20.40816327, 3.46938776; - -28.86150127, -28.86150127, 3.46938776; - 28.86150127, -28.86150127, 3.46938776; +pub static OBSERVATION_MODEL: Matrix5x3 = + matrix![-20.2429, 35.0618, 3.29555; + -28.6278, -28.6278, 3.29555; + 28.6278, -28.6278, 3.29555; + 20.2429, 35.0618, 3.29555; 0.0, 0.0, 1.0]; -pub static Q: Matrix3 = - matrix![KfNumStates as f32 * ProcessNoise / ExpectedDt2, 0.0, 0.0; - 0.0, KfNumStates as f32 * ProcessNoise / ExpectedDt2, 0.0; - 0.0, 0.0, KfNumStates as f32 * ProcessNoise / ExpectedDt2]; - -pub static R: Matrix5 = - matrix![EncoderNoise, 0.0, 0.0, 0.0, 0.0; - 0.0, EncoderNoise, 0.0, 0.0, 0.0; - 0.0, 0.0, EncoderNoise, 0.0, 0.0; - 0.0, 0.0, 0.0, EncoderNoise, 0.0; - 0.0, 0.0, 0.0, 0.0, GyroNoise]; - -pub static P: Matrix3 = - matrix![InitialCovariance, 0.0, 0.0; - 0.0, InitialCovariance, 0.0; - 0.0, 0.0, InitialCovariance]; - -pub static K: Matrix3x5 = - matrix![0.008483887, -0.008483887, -0.006927967, 0.006928444, 0.0; - 0.009064674, 0.009062767, -0.01090765, -0.010907173, 0.012550354; - 0.038414717, 0.038418293, 0.027166963, 0.027170658, 0.53518677]; +pub static PROCESS_COV: Matrix3 = + matrix![KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0, 0.0; + 0.0, KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0; + 0.0, 0.0, KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2]; + +pub static OBSERVATION_COV: Matrix5 = + matrix![ENCODER_NOISE, 0.0, 0.0, 0.0, 0.0; + 0.0, ENCODER_NOISE, 0.0, 0.0, 0.0; + 0.0, 0.0, ENCODER_NOISE, 0.0, 0.0; + 0.0, 0.0, 0.0, ENCODER_NOISE, 0.0; + 0.0, 0.0, 0.0, 0.0, GYRO_NOISE]; + +pub static INIT_ESTIMATE_COV: Matrix3 = + matrix![INITIAL_COV, 0.0, 0.0; + 0.0, INITIAL_COV, 0.0; + 0.0, 0.0, INITIAL_COV]; + +pub static KALMAN_GAIN: Matrix3x5 = + matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; + 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; + 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 143a28ae..9115c621 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -1,12 +1,17 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; // Kp, Ki, Kd, Ki_err_min, Ki_err_max -pub static K_pid: Matrix3x5 = - matrix![6.0, 1.5, 0.0, -1.0, 1.0; - 6.0, 1.5, 0.0, -1.0, 1.0; - 6.0, 0.5, 0.0, -3.0, 3.0]; +pub static PID_GAIN: Matrix3x5 = + matrix![1.0, 0.0, 0.0, -1.0, 1.0; + 1.0, 0.0, 0.0, -1.0, 1.0; + 1.0, 0.0, 0.0, -3.0, 3.0]; // x, y, theta (m/s, m/s, rad/s) pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore -pub static WHEEL_ACC_LIM: Vector4 = vector![2000.0, 2000.0, 2000.0, 2000.0]; // TODO calibrate/ignore \ No newline at end of file + +// FL, BL, BR, FR (rad/s^2) +// Rough estimate for peak rating +// alpha = rated torque / interia +// 8.4 Ncm^2 / (270 gcm^2) = 3110 rad/s^2 actual max +pub static WHEEL_ACC_LIM: Vector4 = vector![2000.0, 2000.0, 2000.0, 2000.0]; \ No newline at end of file diff --git a/control-board/src/motion/params/mod.rs b/control-board/src/motion/params/mod.rs index c0bf6193..23d4d6a7 100644 --- a/control-board/src/motion/params/mod.rs +++ b/control-board/src/motion/params/mod.rs @@ -1,2 +1,4 @@ pub mod body_vel_filter_params; -pub mod body_vel_pid_params; \ No newline at end of file +pub mod body_vel_pid_params; + +pub mod robot_physical_params; \ No newline at end of file diff --git a/control-board/src/motion/params/robot_physical_params.rs b/control-board/src/motion/params/robot_physical_params.rs new file mode 100644 index 00000000..abe319d1 --- /dev/null +++ b/control-board/src/motion/params/robot_physical_params.rs @@ -0,0 +1,4 @@ +use nalgebra::Vector4; +pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(300.0, 45.0, 135.0, 240.0); // Degrees from y+ +pub const WHEEL_RADIUS_M: f32 = 0.0247; // wheel dia 49mm +pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot \ No newline at end of file diff --git a/control-board/src/motion/pid.rs b/control-board/src/motion/pid.rs index e72a9dec..69fbc30c 100644 --- a/control-board/src/motion/pid.rs +++ b/control-board/src/motion/pid.rs @@ -4,56 +4,44 @@ use nalgebra::{ }; pub struct PidController { - K: SMatrix, - u: SVector, + gain: SMatrix, prev_error: SVector, integrated_error: SVector, } impl<'a, const NUM_STATES: usize> PidController { - pub fn from_gains_matrix(K: &'a SMatrix) -> PidController { + pub fn from_gains_matrix(gain: &'a SMatrix) -> PidController { PidController { - K: K.clone(), - u: SVector::::zeros(), + gain: gain.clone(), prev_error: SVector::::zeros(), integrated_error: SVector::::zeros(), } } - pub fn calculate(&mut self, r: SVector, y: SVector, _dt: f32) { - let error = r - y; + pub fn calculate(&mut self, setpoint: &SVector, process_variable: &SVector, _dt: f32) -> SVector { + let error = setpoint - process_variable; - // calculate integrated error - let ie = self.integrated_error + error; + // Calculate integrated error. + let cur_integrated_error = self.integrated_error + error; // clamp error // is there a better way to do this? - self.integrated_error = ie.zip_zip_map(&self.K.column(3), &self.K.column(4), |err, min_err, max_err| clamp(err, min_err, max_err)); + self.integrated_error = cur_integrated_error.zip_zip_map(&self.gain.column(3), &self.gain.column(4), |err, min_err, max_err| clamp(err, min_err, max_err)); // calculate derivative error let de_dt = error - self.prev_error; self.prev_error = error; - let p = self.K.column(0).component_mul(&error); - let i = self.K.column(1).component_mul(&self.integrated_error); - let d = self.K.column(2).component_mul(&de_dt); - self.u = r + (p + i + d); + let p = self.gain.column(0).component_mul(&error); + let i = self.gain.column(1).component_mul(&self.integrated_error); + let d = self.gain.column(2).component_mul(&de_dt); + setpoint + (p + i + d) } - pub fn read_u(&self, u: &mut SVector) { - *u = self.u; + pub fn get_gain(&self) -> SMatrix { + return self.gain; } - pub fn get_u(&self) -> SVector { - self.u - } - - #[allow(non_snake_case)] - pub fn get_K(&self) -> SMatrix { - return self.K; - } - - #[allow(non_snake_case)] - pub fn set_K(&mut self, new_K: SMatrix) { - self.K.copy_from(&new_K) + pub fn set_gain(&mut self, new_gain: SMatrix) { + self.gain.copy_from(&new_gain) } } \ No newline at end of file diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index ada9fc7a..94418a9a 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -2,7 +2,7 @@ use ateam_common_packets::bindings_radio::{ParameterCommandCode::*, ParameterNam use ateam_common_packets::bindings_radio::ParameterDataFormat::{PID_LIMITED_INTEGRAL_F32, VEC3_F32, VEC4_F32}; use ateam_common_packets::bindings_radio::ParameterName::{VEL_PID_X, RC_BODY_VEL_LIMIT, RC_BODY_ACC_LIMIT, VEL_PID_Y, ANGULAR_VEL_PID_Z, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_PROCESS_NOISE, VEL_CGKF_GYRO_NOISE, VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_K_MATRIX, RC_WHEEL_ACC_LIMIT}; use nalgebra::{SVector, Vector3, Vector4, Vector5}; - +use crate::abs_f32; use crate::parameter_interface::ParameterInterface; use super::constant_gain_kalman_filter::CgKalmanFilter; @@ -10,12 +10,12 @@ use super::pid::PidController; use super::robot_model::RobotModel; use super::params::body_vel_filter_params::{ - F, B, H, Q, R, P, K, - KfNumStates, KfNumControlInputs, KfNumObservations, + KF_NUM_STATES, KF_NUM_CONTROL_INPUTS, KF_NUM_OBSERVATIONS, + STATE_TRANSITION, CONTROL_INPUT, OBSERVATION_MODEL, PROCESS_COV, INIT_ESTIMATE_COV, KALMAN_GAIN, }; use super::params::body_vel_pid_params::{ - K_pid, + PID_GAIN, BODY_VEL_LIM, BODY_ACC_LIM, WHEEL_ACC_LIM }; @@ -25,9 +25,12 @@ use ateam_common_packets::bindings_radio::{ }; // TODO find some general numeric type trait(s) for D -pub fn clamp_scale_vector(vec: &SVector, cabs: &SVector) -> SVector { - let scales: SVector = vec.abs().component_div(cabs).map(|val| f32::max(1.0, val)); - vec.component_div(&scales) +// Clamp the vector, but keep the direction consistent. +pub fn clamp_vector_keep_dir(vec: &SVector, limits_abs: &SVector) -> SVector { + // Applies the clamping in a sign-less way by getting the scale of the vector compared to absolute value clamp region. + let scales: SVector = vec.abs().component_div(limits_abs); + // To maintain direction, do a scalar divison of the max of the scales. Limit to make sure to only scale down. + vec / f32::max(1.0, scales.max()) } pub struct BodyVelocityController<'a> { @@ -38,19 +41,18 @@ pub struct BodyVelocityController<'a> { body_velocity_limit: Vector3, body_acceleration_limit: Vector3, wheel_acceleration_limits: Vector4, - prev_setpoint: Vector3, - prev_wheel_u: Vector4, + prev_output: Vector3, cmd_wheel_velocities: Vector4, debug_telemetry: ControlDebugTelemetry, } impl<'a> BodyVelocityController<'a> { pub fn new_from_global_params(loop_dt_s: f32, robot_model: RobotModel) -> BodyVelocityController<'a> { - let body_vel_filter: CgKalmanFilter = - CgKalmanFilter::new(&F, &B, &H, &Q, &R, &P, &K); + let body_vel_filter: CgKalmanFilter = + CgKalmanFilter::new(&STATE_TRANSITION, &CONTROL_INPUT, &OBSERVATION_MODEL, &PROCESS_COV, &KALMAN_GAIN, &INIT_ESTIMATE_COV); - let body_vel_controller: PidController = - PidController::from_gains_matrix(&K_pid); + let body_vel_controller: PidController = + PidController::from_gains_matrix(&PID_GAIN); let mut bvc = BodyVelocityController { loop_dt_s: loop_dt_s, @@ -60,8 +62,7 @@ impl<'a> BodyVelocityController<'a> { body_velocity_limit: Vector3::zeros(), body_acceleration_limit: Vector3::zeros(), wheel_acceleration_limits: Vector4::zeros(), - prev_setpoint: Vector3::zeros(), - prev_wheel_u: Vector4::zeros(), + prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), debug_telemetry: ControlDebugTelemetry { motor_fl: MotorDebugTelemetry { @@ -69,7 +70,7 @@ impl<'a> BodyVelocityController<'a> { wheel_velocity: 0.0, wheel_torque: 0.0 }, - motor_fr: MotorDebugTelemetry { + motor_bl: MotorDebugTelemetry { wheel_setpoint: 0.0, wheel_velocity: 0.0, wheel_torque: 0.0 @@ -79,7 +80,7 @@ impl<'a> BodyVelocityController<'a> { wheel_velocity: 0.0, wheel_torque: 0.0 }, - motor_bl: MotorDebugTelemetry { + motor_fr: MotorDebugTelemetry { wheel_setpoint: 0.0, wheel_velocity: 0.0, wheel_torque: 0.0 @@ -118,8 +119,7 @@ impl<'a> BodyVelocityController<'a> { body_velocity_limit: bv_limit, body_acceleration_limit: ba_limit, wheel_acceleration_limits: wa_limit, - prev_setpoint: Vector3::zeros(), - prev_wheel_u: Vector4::zeros(), + prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), debug_telemetry: ControlDebugTelemetry { motor_fl: MotorDebugTelemetry { @@ -127,7 +127,7 @@ impl<'a> BodyVelocityController<'a> { wheel_velocity: 0.0, wheel_torque: 0.0 }, - motor_fr: MotorDebugTelemetry { + motor_bl: MotorDebugTelemetry { wheel_setpoint: 0.0, wheel_velocity: 0.0, wheel_torque: 0.0 @@ -137,7 +137,7 @@ impl<'a> BodyVelocityController<'a> { wheel_velocity: 0.0, wheel_torque: 0.0 }, - motor_bl: MotorDebugTelemetry { + motor_fr: MotorDebugTelemetry { wheel_setpoint: 0.0, wheel_velocity: 0.0, wheel_torque: 0.0 @@ -154,82 +154,81 @@ impl<'a> BodyVelocityController<'a> { } } - pub fn control_update(&mut self, setpoint: &Vector3, wheel_velocities: &Vector4, wheel_torques: &Vector4, gyro_theta: f32) { - self.debug_telemetry.motor_fr.wheel_torque = wheel_torques[1]; + pub fn control_update(&mut self, body_vel_setpoint: &Vector3, wheel_velocities: &Vector4, wheel_torques: &Vector4, gyro_theta: f32) { + // Assign telemetry data + // TODO pass all of the gyro data up, not just theta + self.debug_telemetry.imu_gyro[2] = gyro_theta; self.debug_telemetry.motor_fl.wheel_torque = wheel_torques[0]; - self.debug_telemetry.motor_bl.wheel_torque = wheel_torques[3]; + self.debug_telemetry.motor_bl.wheel_torque = wheel_torques[1]; self.debug_telemetry.motor_br.wheel_torque = wheel_torques[2]; + self.debug_telemetry.motor_fr.wheel_torque = wheel_torques[3]; + self.debug_telemetry.motor_fl.wheel_velocity = wheel_velocities[0]; + self.debug_telemetry.motor_bl.wheel_velocity = wheel_velocities[1]; + self.debug_telemetry.motor_br.wheel_velocity = wheel_velocities[2]; + self.debug_telemetry.motor_fr.wheel_velocity = wheel_velocities[3]; + self.debug_telemetry.commanded_body_velocity.copy_from_slice(body_vel_setpoint.as_slice()); - // construct the observation vector the KF expects - let z: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); - self.control_update_z(setpoint, z); - } - - pub fn control_update_z(&mut self, setpoint: &Vector3, z: Vector5) { // TODO there are a few discrete time intergrals and derivatives in here // these should probably be genericized/templated some how + // Build measurement vector for CGKF. + let mut measurement: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); + // Convert encoder velocity to body velocity for comparison to IMU. + let enc_body_vel = self.robot_model.wheel_vel_to_robot_vel(wheel_velocities); - // clamp/scale setpoint vel - let setpoint = clamp_scale_vector(setpoint, &BODY_VEL_LIM); - self.debug_telemetry.commanded_body_velocity.copy_from_slice(setpoint.as_slice()); + // If the encoder estimate is small enough, then replace IMU value with + // encoder value to reduce the jittery noise. + if abs_f32(enc_body_vel[2]) < 0.1 { + measurement[4] = enc_body_vel[2] + } - // Firmware does FR, FL, BL, BR ordering like graph quadrants - // Software does FL, FR, BR, BL ordering "clockwise" - // we'll do the mapping here for now - // FIX ME - self.debug_telemetry.imu_gyro[2] = z.a; - self.debug_telemetry.motor_fr.wheel_velocity = z[1]; - self.debug_telemetry.motor_fl.wheel_velocity = z[0]; - self.debug_telemetry.motor_bl.wheel_velocity = z[3]; - self.debug_telemetry.motor_br.wheel_velocity = z[2]; + // Update measurements process observation input into CGKF. + self.body_vel_filter.update(&measurement); - // determine commanded body accleration, and clamp-scale the the control input - let sp_body_acc = (setpoint - self.prev_setpoint) / self.loop_dt_s; - let sp_body_acc_limited = clamp_scale_vector(&sp_body_acc, &BODY_ACC_LIM); - let setpoint_limited = self.prev_setpoint + (sp_body_acc_limited * self.loop_dt_s); - self.prev_setpoint = setpoint_limited; - self.debug_telemetry.clamped_commanded_body_velocity.copy_from_slice(setpoint_limited.as_slice()); + // Read the current body velocity state estimate from the CGKF. + let body_vel_estimate = self.body_vel_filter.get_state(); + self.debug_telemetry.cgkf_body_velocity_state_estimate.copy_from_slice(body_vel_estimate.as_slice()); - // get latest wheel vals and gyro (observe the state) - // process observation input into CGKF - self.body_vel_filter.update(&z); + // Apply control policy. + let body_vel_control_pid = self.body_vel_controller.calculate(&body_vel_setpoint, &body_vel_estimate, self.loop_dt_s); - // read the current body velocity state estimate from the CGKF - let y = self.body_vel_filter.get_state(); - self.debug_telemetry.cgkf_body_velocity_state_estimate.copy_from_slice(y.as_slice()); + // Add the commanded setpoint as a feedforward component. + let body_vel_output = body_vel_control_pid + body_vel_setpoint; + self.debug_telemetry.body_velocity_u.copy_from_slice(body_vel_output.as_slice()); - // apply control policy - self.body_vel_controller.calculate(setpoint_limited, y, self.loop_dt_s); - let u = self.body_vel_controller.get_u(); - self.debug_telemetry.body_velocity_u.copy_from_slice(u.as_slice()); + // Determine commanded body acceleration based on previous control output, and clamp and maintain the direction of acceleration. + // NOTE: Using previous control output instead of estimate so that collision disturbances would not impact. + let body_acc_output = (body_vel_output - self.prev_output) / self.loop_dt_s; + let body_acc_output_clamp = clamp_vector_keep_dir(&body_acc_output, &self.body_acceleration_limit); - // transform body velocity commands into the wheel velocity domain - let wheel_u = self.robot_model.robot_vel_to_wheel_vel(u); - self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_u.as_slice()); + // Convert back to body velocity + let body_vel_output_acc_clamp = self.prev_output + (body_acc_output_clamp * self.loop_dt_s); - // use control law adjusted value to predict the next cycle's state - // call CGKF.predict - self.body_vel_filter.predict(&wheel_u); + // Clamp and maintain direction of control body velocity. + let body_vel_output_full_clamp = clamp_vector_keep_dir(&body_vel_output_acc_clamp, &self.body_velocity_limit); + self.prev_output = body_vel_output_full_clamp; + self.debug_telemetry.clamped_commanded_body_velocity.copy_from_slice(body_vel_output_full_clamp.as_slice()); + + // Transform body velocity commands into the wheel velocity domain. + let wheel_vel_output = self.robot_model.robot_vel_to_wheel_vel(&body_vel_output_full_clamp); + self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output.as_slice()); + + // Use control law adjusted value to predict the next cycle's state. + self.body_vel_filter.predict(&wheel_vel_output); // determine commanded wheel accleration, and clamp-scale the the control input // TODO a linear clamp after the non-linear domain transformation is probably locally valid // and globally invalid. Investiage this later. If problems are suspected, disable this section // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) // TODO cross check in the future against wheel angle plots and analysis - let sp_wheel_acc = (wheel_u - self.prev_wheel_u) / self.loop_dt_s; - let sp_wheel_acc_limited = clamp_scale_vector(&sp_wheel_acc, &WHEEL_ACC_LIM); - let wheel_u_limited = self.prev_wheel_u + (sp_wheel_acc_limited * self.loop_dt_s); - self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_u_limited.as_slice()); - - // update previous u - self.prev_wheel_u = wheel_u_limited; - - // save command state - // self.cmd_wheel_velocities = wheel_u; - self.cmd_wheel_velocities = wheel_u_limited; + let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; + let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); + let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); + self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); + // Save command state. + self.cmd_wheel_velocities = wheel_vel_output_clamp; } pub fn get_wheel_velocities(&self) -> Vector4 { @@ -280,18 +279,17 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { reply_cmd.data_format = PID_LIMITED_INTEGRAL_F32; // readback the data - #[allow(non_snake_case)] // K is the mathematical symbol for this matrix - let current_K = self.body_vel_controller.get_K(); + let current_pid_gain = self.body_vel_controller.get_gain(); // can't slice copy b/c backing storage is column-major // so a row slice isn't contiguous in backing memory and // therefore you can't do a slice copy unsafe { - reply_cmd.data.pidii_f32[0] = current_K.row(0)[0]; - reply_cmd.data.pidii_f32[1] = current_K.row(0)[1]; - reply_cmd.data.pidii_f32[2] = current_K.row(0)[2]; - reply_cmd.data.pidii_f32[3] = current_K.row(0)[3]; - reply_cmd.data.pidii_f32[4] = current_K.row(0)[4]; + reply_cmd.data.pidii_f32[0] = current_pid_gain.row(0)[0]; + reply_cmd.data.pidii_f32[1] = current_pid_gain.row(0)[1]; + reply_cmd.data.pidii_f32[2] = current_pid_gain.row(0)[2]; + reply_cmd.data.pidii_f32[3] = current_pid_gain.row(0)[3]; + reply_cmd.data.pidii_f32[4] = current_pid_gain.row(0)[4]; } reply_cmd.command_code = PCC_ACK; @@ -299,14 +297,13 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { VEL_PID_Y => { reply_cmd.data_format = PID_LIMITED_INTEGRAL_F32; - #[allow(non_snake_case)] - let current_K = self.body_vel_controller.get_K(); + let current_pid_gain = self.body_vel_controller.get_gain(); unsafe { - reply_cmd.data.pidii_f32[0] = current_K.row(1)[0]; - reply_cmd.data.pidii_f32[1] = current_K.row(1)[1]; - reply_cmd.data.pidii_f32[2] = current_K.row(1)[2]; - reply_cmd.data.pidii_f32[3] = current_K.row(1)[3]; - reply_cmd.data.pidii_f32[4] = current_K.row(1)[4]; + reply_cmd.data.pidii_f32[0] = current_pid_gain.row(1)[0]; + reply_cmd.data.pidii_f32[1] = current_pid_gain.row(1)[1]; + reply_cmd.data.pidii_f32[2] = current_pid_gain.row(1)[2]; + reply_cmd.data.pidii_f32[3] = current_pid_gain.row(1)[3]; + reply_cmd.data.pidii_f32[4] = current_pid_gain.row(1)[4]; } reply_cmd.command_code = PCC_ACK; @@ -314,14 +311,13 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { ANGULAR_VEL_PID_Z => { reply_cmd.data_format = PID_LIMITED_INTEGRAL_F32; - #[allow(non_snake_case)] - let current_K = self.body_vel_controller.get_K(); + let current_pid_gain = self.body_vel_controller.get_gain(); unsafe { - reply_cmd.data.pidii_f32[0] = current_K.row(2)[0]; - reply_cmd.data.pidii_f32[1] = current_K.row(2)[1]; - reply_cmd.data.pidii_f32[2] = current_K.row(2)[2]; - reply_cmd.data.pidii_f32[3] = current_K.row(2)[3]; - reply_cmd.data.pidii_f32[4] = current_K.row(2)[4]; + reply_cmd.data.pidii_f32[0] = current_pid_gain.row(2)[0]; + reply_cmd.data.pidii_f32[1] = current_pid_gain.row(2)[1]; + reply_cmd.data.pidii_f32[2] = current_pid_gain.row(2)[2]; + reply_cmd.data.pidii_f32[3] = current_pid_gain.row(2)[3]; + reply_cmd.data.pidii_f32[4] = current_pid_gain.row(2)[4]; } reply_cmd.command_code = PCC_ACK; @@ -370,23 +366,21 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { VEL_PID_X => { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { // read data into matrix, modify the matrix row, then write the whole thing back - #[allow(non_snake_case)] - let mut current_K = self.body_vel_controller.get_K(); - current_K.row_mut(0).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); - self.body_vel_controller.set_K(current_K); + let mut current_pid_gain = self.body_vel_controller.get_gain(); + current_pid_gain.row_mut(0).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + self.body_vel_controller.set_gain(current_pid_gain); - #[allow(non_snake_case)] - let updated_K = self.body_vel_controller.get_K(); + let updated_pid_gain = self.body_vel_controller.get_gain(); // can't slice copy b/c backing storage is column-major // so a row slice isn't contiguous in backing memory and // therefore you can't do a slice copy unsafe { - reply_cmd.data.pidii_f32[0] = updated_K.row(0)[0]; - reply_cmd.data.pidii_f32[1] = updated_K.row(0)[1]; - reply_cmd.data.pidii_f32[2] = updated_K.row(0)[2]; - reply_cmd.data.pidii_f32[3] = updated_K.row(0)[3]; - reply_cmd.data.pidii_f32[4] = updated_K.row(0)[4]; + reply_cmd.data.pidii_f32[0] = current_pid_gain.row(0)[0]; + reply_cmd.data.pidii_f32[1] = current_pid_gain.row(0)[1]; + reply_cmd.data.pidii_f32[2] = current_pid_gain.row(0)[2]; + reply_cmd.data.pidii_f32[3] = current_pid_gain.row(0)[3]; + reply_cmd.data.pidii_f32[4] = current_pid_gain.row(0)[4]; } reply_cmd.command_code = PCC_ACK; @@ -397,19 +391,17 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { }, VEL_PID_Y => { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { - #[allow(non_snake_case)] - let mut current_K = self.body_vel_controller.get_K(); - current_K.row_mut(1).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); - self.body_vel_controller.set_K(current_K); + let mut current_pid_gain = self.body_vel_controller.get_gain(); + current_pid_gain.row_mut(1).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + self.body_vel_controller.set_gain(current_pid_gain); - #[allow(non_snake_case)] - let updated_K = self.body_vel_controller.get_K(); + let updated_pid_gain = self.body_vel_controller.get_gain(); unsafe { - reply_cmd.data.pidii_f32[0] = updated_K.row(1)[0]; - reply_cmd.data.pidii_f32[1] = updated_K.row(1)[1]; - reply_cmd.data.pidii_f32[2] = updated_K.row(1)[2]; - reply_cmd.data.pidii_f32[3] = updated_K.row(1)[3]; - reply_cmd.data.pidii_f32[4] = updated_K.row(1)[4]; + reply_cmd.data.pidii_f32[0] = updated_pid_gain.row(1)[0]; + reply_cmd.data.pidii_f32[1] = updated_pid_gain.row(1)[1]; + reply_cmd.data.pidii_f32[2] = updated_pid_gain.row(1)[2]; + reply_cmd.data.pidii_f32[3] = updated_pid_gain.row(1)[3]; + reply_cmd.data.pidii_f32[4] = updated_pid_gain.row(1)[4]; } reply_cmd.command_code = PCC_ACK; @@ -420,19 +412,17 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { }, ANGULAR_VEL_PID_Z => { if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { - #[allow(non_snake_case)] - let mut current_K = self.body_vel_controller.get_K(); - current_K.row_mut(2).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); - self.body_vel_controller.set_K(current_K); + let mut current_pid_gain = self.body_vel_controller.get_gain(); + current_pid_gain.row_mut(2).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); + self.body_vel_controller.set_gain(current_pid_gain); - #[allow(non_snake_case)] - let updated_K = self.body_vel_controller.get_K(); + let updated_pid_gain = self.body_vel_controller.get_gain(); unsafe { - reply_cmd.data.pidii_f32[0] = updated_K.row(2)[0]; - reply_cmd.data.pidii_f32[1] = updated_K.row(2)[1]; - reply_cmd.data.pidii_f32[2] = updated_K.row(2)[2]; - reply_cmd.data.pidii_f32[3] = updated_K.row(2)[3]; - reply_cmd.data.pidii_f32[4] = updated_K.row(2)[4]; + reply_cmd.data.pidii_f32[0] = updated_pid_gain.row(2)[0]; + reply_cmd.data.pidii_f32[1] = updated_pid_gain.row(2)[1]; + reply_cmd.data.pidii_f32[2] = updated_pid_gain.row(2)[2]; + reply_cmd.data.pidii_f32[3] = updated_pid_gain.row(2)[3]; + reply_cmd.data.pidii_f32[4] = updated_pid_gain.row(2)[4]; } reply_cmd.command_code = PCC_ACK; @@ -500,59 +490,6 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { } } - // match param_cmd.parameter_name { - // RC_BODY_VEL_LIMIT => { - // if param_cmd.data_format == VEC3_F32 { - // // if commanded to write, do the write - // if param_cmd.command_code == PCC_WRITE { - // self.body_velocity_limit.as_mut_slice().copy_from_slice(unsafe { ¶m_cmd.data.vec3_f32 }); - // } - - // // write back - // unsafe { reply_cmd.data.vec3_f32.copy_from_slice(self.body_velocity_limit.as_slice()); } - - // reply_cmd.command_code = PCC_ACK; - // } else { - // reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; - // return Err(reply_cmd); - // } - // }, - // VEL_PID_X => { - // if param_cmd.data_format == PID_LIMITED_INTEGRAL_F32 { - // defmt::trace!("hit VEL_PID_X update block"); - // #[allow(non_snake_case)] // K is the mathematical symbol for this matrix - // let mut current_K = self.body_vel_controller.get_K(); - - // // if commanded to write, do the write - // if param_cmd.command_code == PCC_WRITE { - // defmt::trace!("hit VEL_PID_X write update block"); - // current_K.row_mut(0).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); - // } - - // // can't slice copy b/c backing storage is column-major - // // so a row slice isn't contiguous in backing memory and - // // therefore you can't do a slice copy - // unsafe { - // reply_cmd.data.pidii_f32[0] = current_K.row(0)[0]; - // reply_cmd.data.pidii_f32[1] = current_K.row(0)[1]; - // reply_cmd.data.pidii_f32[2] = current_K.row(0)[2]; - // reply_cmd.data.pidii_f32[3] = current_K.row(0)[3]; - // reply_cmd.data.pidii_f32[4] = current_K.row(0)[4]; - // } - - // reply_cmd.command_code = PCC_ACK; - // } else { - // reply_cmd.command_code = PCC_NACK_INVALID_TYPE_FOR_NAME; - // return Err(reply_cmd); - // } - // }, - // _ => { - // defmt::debug!("unimplement key update in RobotController"); - // reply_cmd.command_code = PCC_NACK_INVALID_NAME; - // return Err(reply_cmd); - // } - // } - return Ok(reply_cmd); } } \ No newline at end of file diff --git a/control-board/src/motion/robot_model.rs b/control-board/src/motion/robot_model.rs index 5b158b10..46eeb25c 100644 --- a/control-board/src/motion/robot_model.rs +++ b/control-board/src/motion/robot_model.rs @@ -11,44 +11,45 @@ pub struct RobotConstants { #[derive(Clone, Copy)] pub struct RobotModel { - t_robot_to_wheel: Matrix4x3, - t_wheel_to_robot: Matrix3x4, + t_body_to_wheel: Matrix4x3, + t_wheel_to_body: Matrix3x4, } impl RobotModel { - fn T_body_wheel_from_model(robot_constants: RobotConstants) -> Matrix4x3 { + fn t_body_to_wheel_from_model(robot_constants: RobotConstants) -> Matrix4x3 { let theta = &robot_constants.wheel_angles_rad; - let l = &robot_constants.wheel_dist_to_cent_m; - let r = &robot_constants.wheel_radius_m; + let wheel_dist = &robot_constants.wheel_dist_to_cent_m; + + let neg_r = -(&robot_constants.wheel_radius_m); Matrix4x3::new( - cosf(theta[0]) / r[0], sinf(theta[0]) / r[0], l[0] / r[0], - cosf(theta[1]) / r[1], sinf(theta[1]) / r[1], l[1] / r[1], - cosf(theta[2]) / r[2], sinf(theta[2]) / r[2], l[2] / r[2], - cosf(theta[3]) / r[3], sinf(theta[3]) / r[3], l[3] / r[3], + cosf(theta[0]) / neg_r[0], sinf(theta[0]) / neg_r[0], -wheel_dist[0] / neg_r[0], + cosf(theta[1]) / neg_r[1], sinf(theta[1]) / neg_r[1], -wheel_dist[1] / neg_r[1], + cosf(theta[2]) / neg_r[2], sinf(theta[2]) / neg_r[2], -wheel_dist[2] / neg_r[2], + cosf(theta[3]) / neg_r[3], sinf(theta[3]) / neg_r[3], -wheel_dist[3] / neg_r[3], ) } pub fn new(robot_constants: RobotConstants) -> RobotModel { - // construct the backward (bot to wheel) and forward (wheel to bot) dynamics - let t_robot_to_wheel = Self::T_body_wheel_from_model(robot_constants); - let ta = t_robot_to_wheel.transpose() * t_robot_to_wheel; + // Construct the backward (body velocity to wheel angular velocity) and forward (wheel to body) dynamics. + let t_body_to_wheel = Self::t_body_to_wheel_from_model(robot_constants); + let ta = t_body_to_wheel.transpose() * t_body_to_wheel; if !ta.is_invertible() { - defmt::error!("robot to wheel TA matrix is not invertible, cannot calculate the forward dynamics") + defmt::error!("Body velocity to wheel TA matrix is not invertible, cannot calculate the forward dynamics!") } - let t_wheel_to_robot = ta.try_inverse().unwrap() * t_robot_to_wheel.transpose(); + let t_wheel_to_body = ta.try_inverse().unwrap() * t_body_to_wheel.transpose(); RobotModel { - t_robot_to_wheel: t_robot_to_wheel, - t_wheel_to_robot: t_wheel_to_robot, + t_body_to_wheel: t_body_to_wheel, + t_wheel_to_body: t_wheel_to_body, } } - pub fn robot_vel_to_wheel_vel(&self, robot_vel: Vector3) -> Vector4 { - self.t_robot_to_wheel * robot_vel + pub fn robot_vel_to_wheel_vel(&self, robot_vel: &Vector3) -> Vector4 { + self.t_body_to_wheel * robot_vel } - pub fn wheel_vel_to_robot_vel(&self, wheel_vels: Vector4) -> Vector3 { - self.t_wheel_to_robot * wheel_vels + pub fn wheel_vel_to_robot_vel(&self, wheel_vels: &Vector4) -> Vector3 { + self.t_wheel_to_body * wheel_vels } } \ No newline at end of file diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 554e19c9..dd59409c 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -322,6 +322,14 @@ impl< pub fn read_rpm(&self) -> f32 { return self.current_state.vel_enc_estimate * 60.0 / (2.0 * PI); } + + pub fn read_vel_setpoint(&self) -> f32 { + return self.current_state.vel_setpoint; + } + + pub fn read_vel_computed_setpoint(&self) -> f32 { + return self.current_state.vel_computed_setpoint; + } } pub struct DribblerMotor< diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index a7adae32..1d8fc927 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -5,7 +5,13 @@ use embassy_stm32::usart::Uart; use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{self, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, pins::*, robot_state::SharedRobotState, stm32_interface, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{self, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, + motion::params::robot_physical_params::{ + WHEEL_ANGLES_DEG, + WHEEL_RADIUS_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M + }, + pins::*, robot_state::SharedRobotState, stm32_interface, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -48,10 +54,6 @@ make_uart_queue_pair!(DRIB, const TICKS_WITHOUT_PACKET_STOP: usize = 20; const BATTERY_MIN_VOLTAGE: f32 = 18.0; -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - pub struct ControlTask< const MAX_RX_PACKET_SIZE: usize, const MAX_TX_PACKET_SIZE: usize, @@ -112,19 +114,19 @@ impl < */ { let wheel_vels = Vector4::new( - self.motor_fr.read_rads(), self.motor_fl.read_rads(), self.motor_bl.read_rads(), - self.motor_br.read_rads() + self.motor_br.read_rads(), + self.motor_fr.read_rads() ); // torque values are computed on the spin but put in the current variable // TODO update this when packet/var names are updated to match software let wheel_torques = Vector4::new( - self.motor_fr.read_current(), self.motor_fl.read_current(), self.motor_bl.read_current(), - self.motor_br.read_current() + self.motor_br.read_current(), + self.motor_fr.read_current() ); // TODO read from channel or something @@ -137,10 +139,10 @@ impl < robot_controller: &mut BodyVelocityController, battery_voltage: &f32) { - self.motor_fr.send_motion_command(); self.motor_fl.send_motion_command(); self.motor_bl.send_motion_command(); self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); self.motor_drib.send_motion_command(); @@ -261,7 +263,7 @@ impl < // TODO check order self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads) } else { - robot_model.robot_vel_to_wheel_vel(cmd_vel) + robot_model.robot_vel_to_wheel_vel(&cmd_vel) } } else { // Battery is too low, set velocity to zero @@ -272,10 +274,10 @@ impl < 0.0) }; - self.motor_fr.set_setpoint(wheel_vels[0]); - self.motor_fl.set_setpoint(wheel_vels[1]); - self.motor_bl.set_setpoint(wheel_vels[2]); - self.motor_br.set_setpoint(wheel_vels[3]); + self.motor_fl.set_setpoint(wheel_vels[0]); + self.motor_bl.set_setpoint(wheel_vels[1]); + self.motor_br.set_setpoint(wheel_vels[2]); + self.motor_fr.set_setpoint(wheel_vels[3]); let drib_dc = -1.0 * drib_vel / 1000.0; self.motor_drib.set_setpoint(drib_dc); diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 908480f0..fe4c13be 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -79,7 +79,9 @@ int main() { bool params_return_packet_requested = false; SyncTimer_t torque_loop_timer; - time_sync_init(&torque_loop_timer, 1); + time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); + SyncTimer_t telemetry_timer; + time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); IIRFilter_t torque_filter; iir_filter_init(&torque_filter, iir_filter_alpha_from_Tf(TORQUE_IIR_TF_MS, TORQUE_LOOP_RATE_MS)); @@ -174,6 +176,7 @@ int main() { } bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); + bool run_telemetry = time_sync_ready_rst(&telemetry_timer); if (run_torque_loop) { float cur_measurement = ((float) res.I_motor / (float) UINT16_MAX) * AVDD_V; @@ -259,7 +262,10 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + if (run_telemetry) + { + uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + } // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h index acbd893d..0fff3837 100644 --- a/motor-controller/bin/dribbler/system.h +++ b/motor-controller/bin/dribbler/system.h @@ -46,6 +46,7 @@ #define VELOCITY_LOOP_RATE_S ((float) VELOCITY_LOOP_RATE_MS / (float) MS_PER_S) #define TORQUE_LOOP_RATE_MS 1 #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) +#define TELEMETRY_LOOP_RATE_MS 1 //////////////////////// // FILTERING/TUNING // diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 7d2bbc27..a72adaf1 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -101,8 +101,10 @@ int main() { // setup the loop rate regulators SyncTimer_t vel_loop_timer; SyncTimer_t torque_loop_timer; - time_sync_init(&vel_loop_timer, 10); - time_sync_init(&torque_loop_timer, 1); + SyncTimer_t telemetry_timer; + time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); + time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); + time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); // setup the velocity filter IIRFilter_t encoder_filter; @@ -136,6 +138,8 @@ int main() { df45_model.torque_to_current_linear_model_b = DF45_TORQUE_TO_CURRENT_LINEAR_B; df45_model.current_to_torque_linear_model_m = DF45_CURRENT_TO_TORQUE_LINEAR_M; df45_model.current_to_torque_linear_model_b = DF45_CURRENT_TO_TORQUE_LINEAR_B; + df45_model.rads_to_dc_linear_map_m = DF45_RADS_TO_DC_LINEAR_M; + df45_model.rads_to_dc_linear_map_b = DF45_RADS_TO_DC_LINEAR_B; // setup the velocity PID PidConstants_t vel_pid_constants; @@ -144,10 +148,10 @@ int main() { pid_initialize(&vel_pid, &vel_pid_constants); vel_pid_constants.kP = 1.0f; - // vel_pid_constants.kI = 0.0001; - // vel_pid_constants.kD = 0.1; - // vel_pid_constants.kI_max = 550.0; - // vel_pid_constants.kI_min = -550.0; + vel_pid_constants.kI = 0.0001f; + // vel_pid_constants.kD = 0.1f; + vel_pid_constants.kI_max = 1.0; + vel_pid_constants.kI_min = -1.0; // setup the torque PID PidConstants_t torque_pid_constants; @@ -199,6 +203,7 @@ int main() { telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; r_motor_board = motor_command_packet.data.motion.setpoint; + motion_control_type = motor_command_packet.data.motion.motion_control_type; } else if (motor_command_packet.type == MCP_PARAMS) { // a params update is issued (or the upstream just wants to readback current params) @@ -263,6 +268,7 @@ int main() { // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); bool run_vel_loop = time_sync_ready_rst(&vel_loop_timer); + bool run_telemtry = time_sync_ready_rst(&telemetry_timer); // run the torque loop if applicable if (run_torque_loop) { @@ -325,18 +331,18 @@ int main() { enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); // compute the velcoity PID - float vel_setpoint_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt); + float vel_setpoint_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); // back convert rads to duty cycle u_vel_loop = mm_rads_to_dc(&df45_model, vel_setpoint_rads); // u_vel_loop = vel_setpoint / DF45_MAX_MOTOR_RAD_PER_S; // velocity control data - response_packet.data.motion.vel_setpoint = vel_setpoint_rads; + response_packet.data.motion.vel_setpoint = r_motor_board; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; response_packet.data.motion.vel_hall_estimate = 0U; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = vel_setpoint_rads; + response_packet.data.motion.vel_computed_setpoint = u_vel_loop; } @@ -347,6 +353,8 @@ int main() { // set the motor duty cycle if (motion_control_type == OPEN_LOOP) { float r_motor = mm_rads_to_dc(&df45_model, r_motor_board); + response_packet.data.motion.vel_setpoint = r_motor_board; + response_packet.data.motion.vel_computed_setpoint = r_motor; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { pwm6step_set_duty_cycle_f(u_vel_loop); @@ -407,9 +415,9 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - // if (telemetry_enabled) { + if (telemetry_enabled) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); - // } + } // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h index baa186ac..c89e19cf 100644 --- a/motor-controller/bin/wheel/main.h +++ b/motor-controller/bin/wheel/main.h @@ -46,6 +46,8 @@ #define DF45_TORQUE_TO_CURRENT_LINEAR_B (-0.0558f) #define DF45_CURRENT_TO_TORQUE_LINEAR_M 0.03477f #define DF45_CURRENT_TO_TORQUE_LINEAR_B 0.00242f +#define DF45_RADS_TO_DC_LINEAR_M 0.00144873f +#define DF45_RADS_TO_DC_LINEAR_B 0.0057431f //////////////////////// // FILTERING/TUNING // diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 654e5485..82c09880 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -43,7 +43,7 @@ #define VELOCITY_LOOP_RATE_S ((float) VELOCITY_LOOP_RATE_MS / (float) MS_PER_S) #define TORQUE_LOOP_RATE_MS 1 #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) - +#define TELEMETRY_LOOP_RATE_MS 5 diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index 0e5662c0..e32acce9 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -51,7 +51,7 @@ typedef struct MotorErrors { #define PWM_TIM_PRESCALER 0 -// period 20000ns +// period 20833ns // be conscious of dead time ratio #define PWM_FREQ_HZ 48000 diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c index a99f017e..734e05e4 100644 --- a/motor-controller/common/motor_model.c +++ b/motor-controller/common/motor_model.c @@ -13,11 +13,24 @@ void mm_initialize(MotorModel_t *mm) { mm->torque_to_current_linear_model_b = 0.0f; mm->current_to_torque_linear_model_m = 0.0f; mm->current_to_torque_linear_model_b = 0.0f; + mm->rads_to_dc_linear_map_m = 0.0f; + mm->rads_to_dc_linear_map_b = 0.0f; } float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) { + // Need to exceed coefficient to prevent b from inverting the directions. + // So just return 0.0 at small values + if (avel_rads > -(mm->rads_to_dc_linear_map_b/mm->rads_to_dc_linear_map_m) && + avel_rads < (mm->rads_to_dc_linear_map_b/mm->rads_to_dc_linear_map_m)) { + return 0.0f; + } + + // Linear mapping from 'motor_model.py' in hw-analysis + float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + + ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); + // bound DC [-1, 1] - return fmax(fmin(avel_rads / mm->max_vel_rads, 1.0f), -1.0f); + return fmax(fmin(map_value, 1.0f), -1.0f); } float mm_enc_ticks_to_rev(MotorModel_t *mm, float enc_ticks) { diff --git a/motor-controller/common/motor_model.h b/motor-controller/common/motor_model.h index 4231669d..90bf66bf 100644 --- a/motor-controller/common/motor_model.h +++ b/motor-controller/common/motor_model.h @@ -11,6 +11,8 @@ typedef struct MotorModel { float torque_to_current_linear_model_b; float current_to_torque_linear_model_m; float current_to_torque_linear_model_b; + float rads_to_dc_linear_map_m; + float rads_to_dc_linear_map_b; } MotorModel_t; void mm_initialize(MotorModel_t *mm); diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c index 299a6d6d..7edcb4d6 100644 --- a/motor-controller/common/pid.c +++ b/motor-controller/common/pid.c @@ -23,12 +23,12 @@ void pid_initialize(Pid_t *pid, PidConstants_t *pid_constants) { // this graphic might be helpful // https://upload.wikimedia.org/wikipedia/commons/4/43/PID_en.svg -float pid_calculate(Pid_t *pid, float r, float y) { +float pid_calculate(Pid_t *pid, float r, float y, float dt) { float err = r - y; - + float termP = err * pid->pid_constants->kP; - float termI = pid->eI + (err * pid->pid_constants->kI); + float termI = (pid->eI + (err * dt)) * pid->pid_constants->kI; if (termI > pid->pid_constants->kI_max) { termI = pid->pid_constants->kI_max; } else if (termI < pid->pid_constants->kI_min) { diff --git a/motor-controller/common/pid.h b/motor-controller/common/pid.h index e3b382ba..8b2a30b9 100644 --- a/motor-controller/common/pid.h +++ b/motor-controller/common/pid.h @@ -23,4 +23,4 @@ void pid_constants_initialize(PidConstants_t *pid_constants); void pid_initialize(Pid_t *pid, PidConstants_t *pid_constants); -float pid_calculate(Pid_t *pid, float r, float y); \ No newline at end of file +float pid_calculate(Pid_t *pid, float r, float y, float dt); \ No newline at end of file diff --git a/motor-controller/common/quadrature_encoder.c b/motor-controller/common/quadrature_encoder.c index 98300961..836eab00 100644 --- a/motor-controller/common/quadrature_encoder.c +++ b/motor-controller/common/quadrature_encoder.c @@ -65,20 +65,6 @@ void quadenc_setup() { TIM3->CR1 |= (TIM_CR1_CEN); } -/** - * @brief gets the raw counter value - * - * @return uint16_t - */ -uint16_t quadenc_get_counter() { - // counter clockwise counts down - // - underflow resets to 65535 - // clockwise counts up - // - overflow to 0 - - return TIM3->CNT; -} - /** * @brief resets the counter to the median value * @@ -97,13 +83,25 @@ void quadenc_reset_encoder_delta() { * NOTE: make sure its infeasible for 32k tick to occur * in either direction between calls to get the * delta + * + * Currently the encoder is set to 4000 step per revolution. + * The motor can go 5260 rpm -> 87.66 rps. + * The steps per second is 4000*87.66 = 350,666.66. + * + * * TODO: investigate OF/UF detection so we know if the delta is bad * * @return int32_t number of encoder ticks since the last call */ int32_t quadenc_get_encoder_delta() { - uint16_t cur_val = quadenc_get_counter(); - quadenc_reset_encoder_delta(); + // counter clockwise counts down + // - underflow resets to 65535 + // clockwise counts up + // - overflow to 0 + // Gets the current count + uint16_t cur_val = TIM3->CNT; + // Resets the count back to the center value + TIM3->CNT = 0x8000; int32_t enc_delta = (int32_t) cur_val - 0x8000; return enc_delta; diff --git a/motor-controller/common/quadrature_encoder.h b/motor-controller/common/quadrature_encoder.h index a1b771e1..16e4c55e 100644 --- a/motor-controller/common/quadrature_encoder.h +++ b/motor-controller/common/quadrature_encoder.h @@ -15,7 +15,6 @@ /////////////////////////////////// void quadenc_setup(); -uint16_t quadenc_get_counter(); void quadenc_reset_encoder_delta(); int32_t quadenc_get_encoder_delta(); float quadenc_delta_to_w(float encoder_delta, float deltat_s); From 64546f5ed2efbe12368cccd48321cded555b04ba Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 18 Jun 2024 16:28:00 -0700 Subject: [PATCH 080/157] Gain fix --- control-board/src/motion/robot_controller.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 94418a9a..7bd89755 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -370,17 +370,17 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { current_pid_gain.row_mut(0).copy_from_slice(unsafe { ¶m_cmd.data.pidii_f32 }); self.body_vel_controller.set_gain(current_pid_gain); - let updated_pid_gain = self.body_vel_controller.get_gain(); - + // can't slice copy b/c backing storage is column-major // so a row slice isn't contiguous in backing memory and // therefore you can't do a slice copy + let updated_pid_gain = self.body_vel_controller.get_gain(); unsafe { - reply_cmd.data.pidii_f32[0] = current_pid_gain.row(0)[0]; - reply_cmd.data.pidii_f32[1] = current_pid_gain.row(0)[1]; - reply_cmd.data.pidii_f32[2] = current_pid_gain.row(0)[2]; - reply_cmd.data.pidii_f32[3] = current_pid_gain.row(0)[3]; - reply_cmd.data.pidii_f32[4] = current_pid_gain.row(0)[4]; + reply_cmd.data.pidii_f32[0] = updated_pid_gain.row(0)[0]; + reply_cmd.data.pidii_f32[1] = updated_pid_gain.row(0)[1]; + reply_cmd.data.pidii_f32[2] = updated_pid_gain.row(0)[2]; + reply_cmd.data.pidii_f32[3] = updated_pid_gain.row(0)[3]; + reply_cmd.data.pidii_f32[4] = updated_pid_gain.row(0)[4]; } reply_cmd.command_code = PCC_ACK; From dcd153e1d3b582468721d43e10ecc8276b6f2303 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 18 Jun 2024 19:55:43 -0400 Subject: [PATCH 081/157] add param updates --- control-board/src/tasks/control_task.rs | 25 +++++++++++++++-------- lib-stm32/src/drivers/other/adc_helper.rs | 10 ++++----- 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 1d8fc927..c3e4132b 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -5,13 +5,9 @@ use embassy_stm32::usart::Uart; use embassy_time::{Duration, Ticker, Timer}; use nalgebra::{Vector3, Vector4}; -use crate::{include_external_cpp_bin, motion::{self, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, - motion::params::robot_physical_params::{ - WHEEL_ANGLES_DEG, - WHEEL_RADIUS_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M - }, - pins::*, robot_state::SharedRobotState, stm32_interface, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; +use crate::{include_external_cpp_bin, motion::{self, params::robot_physical_params::{ + WHEEL_ANGLES_DEG, WHEEL_DISTANCE_TO_ROBOT_CENTER_M, WHEEL_RADIUS_M + }, robot_controller::BodyVelocityController, robot_model::{RobotConstants, RobotModel}}, parameter_interface::ParameterInterface, pins::*, robot_state::SharedRobotState, stm32_interface, stspin_motor::{DribblerMotor, WheelMotor}, SystemIrqs}; include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} @@ -240,8 +236,19 @@ impl < drib_vel = latest_control.dribbler_speed; ticks_since_packet = 0; }, - ateam_common_packets::radio::DataPacket::ParameterCommand(_latest_param) => { - defmt::warn!("param updates aren't supported yet"); + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { + // defmt::warn!("param updates aren't supported yet"); + let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); + + if let Ok(resp) = param_cmd_resp { + defmt::info!("sending successful parameter update command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } else if let Err(resp) = param_cmd_resp { + defmt::warn!("sending failed parameter updated command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } }, } } else { diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 37c624e3..eb233d63 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -53,11 +53,11 @@ impl< // Based off of this: http://www.efton.sk/STM32/STM32_VREF.pdf // vmeas = vcal * MEAS / MAX * CAL / REFINT (4) - defmt::info!("V_CAL_MV: {}", V_CAL_V); - defmt::info!("inst.read(): {}", self.inst.read(&mut self.pin) as f32); - defmt::info!("adc_bins: {}", self.adc_bins as f32); - defmt::info!("vref_int_cal: {}", vref_int_cal); - defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); + // defmt::info!("V_CAL_MV: {}", V_CAL_V); + // defmt::info!("inst.read(): {}", self.inst.read(&mut self.pin) as f32); + // defmt::info!("adc_bins: {}", self.adc_bins as f32); + // defmt::info!("vref_int_cal: {}", vref_int_cal); + // defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); // FIXME, embassy API appears to not correctly load vref_int_read // override for now From 5bc9c18148f0657f4d5e71215aa6862a57280d9c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 18 Jun 2024 20:10:28 -0400 Subject: [PATCH 082/157] fix radio tx loop with extra packets --- control-board/src/tasks/radio_task.rs | 184 +++----------------------- 1 file changed, 19 insertions(+), 165 deletions(-) diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 606c8b76..2ceb75bd 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -345,21 +345,25 @@ impl< } } - if let Some(telemetry) = self.telemetry_subscriber.try_next_message_pure() { - match telemetry { - TelemetryPacket::Basic(basic) => { - self.last_basic_telemetry = basic; - }, - TelemetryPacket::Control(control) => { - if self.radio.send_control_debug_telemetry(control).await.is_err() { - defmt::warn!("RadioTask - failed to send control debug telemetry packet"); - } - }, - TelemetryPacket::ParameterCommandResponse(pc_resp) => { - if self.radio.send_parameter_response(pc_resp).await.is_err() { - defmt::warn!("RadioTask - failed to send control parameter response packet"); - } - }, + loop { + if let Some(telemetry) = self.telemetry_subscriber.try_next_message_pure() { + match telemetry { + TelemetryPacket::Basic(basic) => { + self.last_basic_telemetry = basic; + }, + TelemetryPacket::Control(control) => { + if self.radio.send_control_debug_telemetry(control).await.is_err() { + defmt::warn!("RadioTask - failed to send control debug telemetry packet"); + } + }, + TelemetryPacket::ParameterCommandResponse(pc_resp) => { + if self.radio.send_parameter_response(pc_resp).await.is_err() { + defmt::warn!("RadioTask - failed to send control parameter response packet"); + } + }, + } + } else { + break; } } @@ -390,154 +394,6 @@ async fn radio_task_entry(mut radio_task: RadioTask = - RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin); - - let mut startup_radio_uart_config = usart::Config::default(); - startup_radio_uart_config.baudrate = 115_200; - startup_radio_uart_config.data_bits = DataBits::DataBits8; - startup_radio_uart_config.stop_bits = StopBits::STOP1; - startup_radio_uart_config.parity = Parity::ParityNone; - - #[allow(unused_labels)] - 'connect_loop: - loop { - if radio_ndet.is_high() { - defmt::error!("radio appears unplugged."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } - - if RADIO_TX_UART_QUEUE.update_uart_config(startup_radio_uart_config).await.is_err() { - defmt::error!("failed to reset radio config."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } - - defmt::info!("connecting radio uart"); - match select(radio.connect_uart(), Timer::after_millis(10000)).await { - Either::First(res) => { - if res.is_err() { - defmt::error!("failed to establish radio UART connection."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } else { - defmt::debug!("established radio uart coms"); - } - } - Either::Second(_) => { - defmt::error!("initial radio uart connection timed out"); - continue 'connect_loop; - } - } - - while radio.connect_to_network(wifi_network, robot_state.get_hw_robot_id()).await.is_err() { - defmt::error!("failed to connect to wifi network."); - Timer::after_millis(1000).await; - } - defmt::info!("radio connected"); - - let res = radio.open_multicast().await; - if res.is_err() { - defmt::error!("failed to establish multicast socket to network."); - continue 'connect_loop; - } - defmt::info!("multicast open"); - - 'connect_hello: - loop { - defmt::info!("sending hello"); - - let robot_id = robot_state.get_hw_robot_id(); - let team_color = if robot_state.hw_robot_team_is_blue() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - radio.send_hello(robot_id, team_color).await.unwrap(); - let hello = radio.wait_hello(Duration::from_millis(1000)).await; - - match hello { - Ok(hello) => { - defmt::info!( - "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port - ); - radio.close_peer().await.unwrap(); - defmt::info!("multicast peer closed"); - - radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); - defmt::info!("unicast open"); - - break 'connect_hello; - } - Err(_) => {} - } - } - - // TODO add inbound timeout somewhere, maybe not here. - 'process_packets: - loop { - match select(radio.read_packet(), telemetry_subscriber.next_message()).await { - Either::First(res) => { - if let Ok(c2_pkt) = res { - command_publisher.publish_immediate(c2_pkt); - } else { - defmt::warn!("radio read packet returned an error"); - } - } - Either::Second(telem_msg) => { - match telem_msg { - WaitResult::Lagged(num_missed_pkts) => { - defmt::warn!("radio task missed sending {} outbound packets. Should channel have higher capacity?", num_missed_pkts); - }, - WaitResult::Message(msg) => { - match msg { - TelemetryPacket::Basic(basic_telem_pkt) => { - let res = radio.send_telemetry(basic_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send basic telemetry packet. Is the backing queue too small?"); - } - } - TelemetryPacket::Control(control_telem_pkt) => { - let res = radio.send_control_debug_telemetry(control_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); - } - } - TelemetryPacket::ParameterCommandResponse(param_resp) => { - let res = radio.send_parameter_response(param_resp).await; - if res.is_err() { - defmt::warn!("radio task failed to send param resp packet. Is the backing queue too small?") - } - } - } - } - } - } - } - - if radio_ndet.is_high() { - defmt::error!("radio was unplugged."); - break 'process_packets; - } - } - } -} - pub async fn start_radio_task(radio_task_spawner: Spawner, queue_spawner: SendSpawner, robot_state: &'static SharedRobotState, @@ -565,6 +421,4 @@ pub async fn start_radio_task(radio_task_spawner: Spawner, let radio_task = RadioTask::new_from_pins(robot_state, command_publisher, telemetry_subscriber, &RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin, radio_ndet_pin, wifi_credentials); radio_task_spawner.spawn(radio_task_entry(radio_task)).unwrap(); - - // radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); } \ No newline at end of file From 98724cb85ba78657c1901dbe643f96aa9ceebf9c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 18 Jun 2024 22:04:05 -0400 Subject: [PATCH 083/157] disable adc helper logging --- lib-stm32/src/drivers/other/adc_helper.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 37c624e3..eb233d63 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -53,11 +53,11 @@ impl< // Based off of this: http://www.efton.sk/STM32/STM32_VREF.pdf // vmeas = vcal * MEAS / MAX * CAL / REFINT (4) - defmt::info!("V_CAL_MV: {}", V_CAL_V); - defmt::info!("inst.read(): {}", self.inst.read(&mut self.pin) as f32); - defmt::info!("adc_bins: {}", self.adc_bins as f32); - defmt::info!("vref_int_cal: {}", vref_int_cal); - defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); + // defmt::info!("V_CAL_MV: {}", V_CAL_V); + // defmt::info!("inst.read(): {}", self.inst.read(&mut self.pin) as f32); + // defmt::info!("adc_bins: {}", self.adc_bins as f32); + // defmt::info!("vref_int_cal: {}", vref_int_cal); + // defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); // FIXME, embassy API appears to not correctly load vref_int_read // override for now From 815b39f391d91c0e8abddf5830fa1f64f94a840b Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 19 Jun 2024 00:46:42 -0400 Subject: [PATCH 084/157] wheel PID prelim working --- control-board/.cargo/config.toml | 4 ++-- .../src/motion/params/body_vel_pid_params.rs | 6 +++--- control-board/src/motion/pid.rs | 9 +++++---- control-board/src/motion/robot_controller.rs | 2 ++ control-board/src/stspin_motor.rs | 4 ++++ control-board/src/tasks/control_task.rs | 18 +++++++++++++++--- motor-controller/bin/wheel/main.c | 18 +++++++++--------- motor-controller/common/pid.c | 14 ++++++++------ software-communication | 2 +- 9 files changed, 49 insertions(+), 28 deletions(-) diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index e487328e..09a3c3aa 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -2,8 +2,8 @@ target = "thumbv7em-none-eabihf" [target.thumbv7em-none-eabihf] -# runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' -runner = 'probe-rs run --chip STM32H723ZGTx' +runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' +# runner = 'probe-rs run --chip STM32H723ZGTx' [env] diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 9115c621..5a1303f6 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -2,9 +2,9 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; // Kp, Ki, Kd, Ki_err_min, Ki_err_max pub static PID_GAIN: Matrix3x5 = - matrix![1.0, 0.0, 0.0, -1.0, 1.0; - 1.0, 0.0, 0.0, -1.0, 1.0; - 1.0, 0.0, 0.0, -3.0, 3.0]; + matrix![0.1, 0.0, 0.0, -1.0, 1.0; + 0.1, 0.0, 0.0, -1.0, 1.0; + 0.5, 0.0, 0.0, -3.0, 3.0]; // x, y, theta (m/s, m/s, rad/s) pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate diff --git a/control-board/src/motion/pid.rs b/control-board/src/motion/pid.rs index 69fbc30c..ac17c94b 100644 --- a/control-board/src/motion/pid.rs +++ b/control-board/src/motion/pid.rs @@ -18,23 +18,24 @@ impl<'a, const NUM_STATES: usize> PidController { } } - pub fn calculate(&mut self, setpoint: &SVector, process_variable: &SVector, _dt: f32) -> SVector { + pub fn calculate(&mut self, setpoint: &SVector, process_variable: &SVector, dt_s: f32) -> SVector { let error = setpoint - process_variable; // Calculate integrated error. - let cur_integrated_error = self.integrated_error + error; + let cur_integrated_error = self.integrated_error + (error * dt_s); // clamp error // is there a better way to do this? self.integrated_error = cur_integrated_error.zip_zip_map(&self.gain.column(3), &self.gain.column(4), |err, min_err, max_err| clamp(err, min_err, max_err)); // calculate derivative error - let de_dt = error - self.prev_error; + let de_dt = (error - self.prev_error) / dt_s; self.prev_error = error; let p = self.gain.column(0).component_mul(&error); let i = self.gain.column(1).component_mul(&self.integrated_error); let d = self.gain.column(2).component_mul(&de_dt); - setpoint + (p + i + d) + + p + i + d } pub fn get_gain(&self) -> SMatrix { diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 7bd89755..bd77ec02 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -195,6 +195,8 @@ impl<'a> BodyVelocityController<'a> { // Add the commanded setpoint as a feedforward component. let body_vel_output = body_vel_control_pid + body_vel_setpoint; + // let body_vel_output = body_vel_setpoint; + self.debug_telemetry.body_velocity_u.copy_from_slice(body_vel_output.as_slice()); // Determine commanded body acceleration based on previous control output, and clamp and maintain the direction of acceleration. diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index dd59409c..56e076d5 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -330,6 +330,10 @@ impl< pub fn read_vel_computed_setpoint(&self) -> f32 { return self.current_state.vel_computed_setpoint; } + + pub fn read_hall_vel(&self) -> f32 { + return self.current_state.vel_hall_estimate; + } } pub struct DribblerMotor< diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index c3e4132b..477725d9 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,4 +1,4 @@ -use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; +use ateam_common_packets::{bindings_radio::BasicTelemetry, bindings_stspin::MotorCommand_MotionType, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; @@ -161,7 +161,13 @@ impl < }); self.telemetry_publisher.publish_immediate(basic_telem); - let control_debug_telem = TelemetryPacket::Control(robot_controller.get_control_debug_telem()); + let mut control_debug_telem = robot_controller.get_control_debug_telem(); + control_debug_telem.motor_fl.wheel_torque = self.motor_fl.read_hall_vel(); + control_debug_telem.motor_bl.wheel_torque = self.motor_bl.read_hall_vel(); + control_debug_telem.motor_br.wheel_torque = self.motor_br.read_hall_vel(); + control_debug_telem.motor_fr.wheel_torque = self.motor_fr.read_hall_vel(); + + let control_debug_telem = TelemetryPacket::Control(control_debug_telem); self.telemetry_publisher.publish_immediate(control_debug_telem); } @@ -193,6 +199,12 @@ impl < self.motor_fr.set_telemetry_enabled(true); self.motor_drib.set_telemetry_enabled(true); + self.motor_fl.set_motion_type(MotorCommand_MotionType::VELOCITY); + self.motor_bl.set_motion_type(MotorCommand_MotionType::VELOCITY); + self.motor_br.set_motion_type(MotorCommand_MotionType::VELOCITY); + self.motor_fr.set_motion_type(MotorCommand_MotionType::VELOCITY); + + Timer::after_millis(10).await; let robot_model = self.get_robot_model(); @@ -262,7 +274,7 @@ impl < // now we have setpoint r(t) in self.cmd_vel // let battery_v = battery_sub.next_message_pure().await as f32; let battery_v = 25.0; - let controls_enabled = false; + let controls_enabled = true; let gyro_rads = (self.gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { if controls_enabled diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index a72adaf1..da8e83b7 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -147,11 +147,11 @@ int main() { Pid_t vel_pid; pid_initialize(&vel_pid, &vel_pid_constants); - vel_pid_constants.kP = 1.0f; - vel_pid_constants.kI = 0.0001f; - // vel_pid_constants.kD = 0.1f; - vel_pid_constants.kI_max = 1.0; - vel_pid_constants.kI_min = -1.0; + vel_pid_constants.kP = 2.0f; + vel_pid_constants.kI = 2.0f; + vel_pid_constants.kD = 0.0f; + vel_pid_constants.kI_max = 20.0; + vel_pid_constants.kI_min = -20.0; // setup the torque PID PidConstants_t torque_pid_constants; @@ -198,7 +198,7 @@ int main() { if (motor_command_packet.data.motion.reset) { // TODO handle hardware reset - while (true); // block, hardware reset flagged + // while (true); // block, hardware reset flagged } telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; @@ -300,7 +300,7 @@ int main() { } // calculate PID on the torque in Nm - float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm); + float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S); // convert desired torque to desired current float current_setpoint = mm_torque_to_current(&df45_model, fabs(torque_setpoint_Nm)); @@ -340,7 +340,7 @@ int main() { response_packet.data.motion.vel_setpoint = r_motor_board; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; - response_packet.data.motion.vel_hall_estimate = 0U; + response_packet.data.motion.vel_hall_estimate = vel_setpoint_rads; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; response_packet.data.motion.vel_computed_setpoint = u_vel_loop; } @@ -415,7 +415,7 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (telemetry_enabled) { + if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); } // GPIOB->BSRR |= GPIO_BSRR_BR_8; diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c index 7edcb4d6..e36cc37a 100644 --- a/motor-controller/common/pid.c +++ b/motor-controller/common/pid.c @@ -28,13 +28,15 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { float termP = err * pid->pid_constants->kP; - float termI = (pid->eI + (err * dt)) * pid->pid_constants->kI; - if (termI > pid->pid_constants->kI_max) { - termI = pid->pid_constants->kI_max; - } else if (termI < pid->pid_constants->kI_min) { - termI = pid->pid_constants->kI_min; + // float alpha = 0.37f; + // pid->eI = (pid->eI * alpha) + (err * dt * (1.0f - alpha)); + pid->eI = pid->eI + (err * dt); + if (pid->eI > pid->pid_constants->kI_max) { + pid->eI = pid->pid_constants->kI_max; + } else if (pid->eI < pid->pid_constants->kI_min) { + pid->eI = pid->pid_constants->kI_min; } - pid->eI = termI; + float termI = pid->eI * pid->pid_constants->kI; float termD = (err - pid->prev_err) * pid->pid_constants->kD; // flip err and prev_err??? pid->prev_err = err; diff --git a/software-communication b/software-communication index f1517dde..5aae1e34 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit f1517dde41ea5940894d484489b1888623f74f78 +Subproject commit 5aae1e348f5a60a2279cbbff66ecb66d468620fd From 563644bc6a3081c4f2869b6f5ea3629fb6e4d144 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 23 Jun 2024 18:53:20 -0400 Subject: [PATCH 085/157] fix imu data rate and filtering config --- control-board/src/tasks/control_task.rs | 2 +- control-board/src/tasks/imu_task.rs | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 477725d9..a37d9fc4 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -275,7 +275,7 @@ impl < // let battery_v = battery_sub.next_message_pure().await as f32; let battery_v = 25.0; let controls_enabled = true; - let gyro_rads = (self.gyro_subscriber.next_message_pure().await[2] as f32) * 2.0 * core::f32::consts::PI / 360.0; + let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { if controls_enabled { diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 896d87b7..ed1673a9 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -39,8 +39,8 @@ static mut IMU_BUFFER_CELL: [u8; bmi323::SPI_MIN_BUF_LEN] = [0; bmi323::SPI_MIN_ #[embassy_executor::task] async fn imu_task_entry( robot_state: &'static SharedRobotState, - accel_pub: AccelDataPublisher, gyro_pub: GyroDataPublisher, + accel_pub: AccelDataPublisher, mut imu: Bmi323<'static, 'static>, mut _accel_int: ExtiInput<'static>, mut gyro_int: ExtiInput<'static>) { @@ -61,8 +61,8 @@ async fn imu_task_entry( let gyro_config_res = imu.set_gyro_config(GyroMode::ContinuousHighPerformance, GyroRange::PlusMinus2000DegPerSec, Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr6400p0, - DataAveragingWindow::Average64Samples).await; + OutputDataRate::Odr100p0, + DataAveragingWindow::Average2Samples).await; imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2).await; if gyro_config_res.is_err() { @@ -73,8 +73,8 @@ async fn imu_task_entry( let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance, AccelRange::Range2g, Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr6400p0, - DataAveragingWindow::Average64Samples).await; + OutputDataRate::Odr100p0, + DataAveragingWindow::Average2Samples).await; imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1).await; if acc_config_res.is_err() { @@ -153,6 +153,6 @@ pub fn start_imu_task( let accel_int = ExtiInput::new(accel_int_pin, accel_int, Pull::None); let gyro_int = ExtiInput::new(gyro_int_pin, gyro_int, Pull::None); - imu_task_spawner.spawn(imu_task_entry(robot_state, accel_data_publisher, gyro_data_publisher, imu, accel_int, gyro_int)).unwrap(); + imu_task_spawner.spawn(imu_task_entry(robot_state, gyro_data_publisher, accel_data_publisher, imu, accel_int, gyro_int)).unwrap(); } From 05dd2501e3432ac9816937d0122e444d030eee67 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 23 Jun 2024 16:20:27 -0700 Subject: [PATCH 086/157] Update parameters --- control-board/src/motion/params/body_vel_filter_params.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 10b29218..57857ec3 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -49,7 +49,7 @@ pub static INIT_ESTIMATE_COV: Matrix3 = 0.0, 0.0, INITIAL_COV]; pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; - 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; - 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; + matrix![-0.00798417141, -0.112913235, 0.0112913235, 0.00798417141, 0.0; + 0.00804295890, -0.00728930537, -0.00728930537, 0.00804295890, 0.0; + 0.0345676344, 0.0422518217, 0.0422518217, 0.0345676344, 0.0114364484]; From 79b97c81c84d4915f07de47cc932506f287ab541 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 23 Jun 2024 16:26:15 -0700 Subject: [PATCH 087/157] Fix parameter --- control-board/src/motion/params/body_vel_filter_params.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 57857ec3..4c9d72b5 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -49,7 +49,7 @@ pub static INIT_ESTIMATE_COV: Matrix3 = 0.0, 0.0, INITIAL_COV]; pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.00798417141, -0.112913235, 0.0112913235, 0.00798417141, 0.0; + matrix![-0.00798417141, -0.0112913235, 0.0112913235, 0.00798417141, 0.0; 0.00804295890, -0.00728930537, -0.00728930537, 0.00804295890, 0.0; 0.0345676344, 0.0422518217, 0.0422518217, 0.0345676344, 0.0114364484]; From 5b8fee6cd58e2973746abb5450a81316c4c4566b Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 23 Jun 2024 19:38:10 -0700 Subject: [PATCH 088/157] Adds accel limiting --- motor-controller/bin/wheel/main.c | 28 ++++++++++++++++++++-------- motor-controller/bin/wheel/system.h | 2 +- 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index da8e83b7..71a26447 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -118,7 +118,9 @@ int main() { // define the control points the loops use to interact float r_motor_board = 0.0f; - float u_vel_loop = 0.0f; + float control_setpoint_vel_duty = 0.0f; + float control_setpoint_vel_rads = 0.0f; + float control_setpoint_vel_rads_prev = 0.0f; float u_torque_loop = 0.0f; float cur_limit = 0.0f; @@ -296,7 +298,7 @@ int main() { if (motion_control_type == TORQUE) { r_Nm = r_motor_board; } else { - r_Nm = u_vel_loop; + r_Nm = control_setpoint_vel_duty; } // calculate PID on the torque in Nm @@ -331,18 +333,28 @@ int main() { enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); // compute the velcoity PID - float vel_setpoint_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); + control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); + + // Clamp setpoint acceleration + float setpoint_accel_rads_2 = (control_setpoint_vel_rads - control_setpoint_vel_rads_prev)/VELOCITY_LOOP_RATE_S; + if (setpoint_accel_rads_2 > MOTOR_MAXIMUM_ACCEL) { + setpoint_accel_rads_2 = MOTOR_MAXIMUM_ACCEL; + } else if (setpoint_accel_rads_2 < -MOTOR_MAXIMUM_ACCEL) { + setpoint_accel_rads_2 = -MOTOR_MAXIMUM_ACCEL; + } + + control_setpoint_vel_rads = control_setpoint_vel_rads_prev + (setpoint_accel_rads_2 * VELOCITY_LOOP_RATE_S); + // back convert rads to duty cycle - u_vel_loop = mm_rads_to_dc(&df45_model, vel_setpoint_rads); - // u_vel_loop = vel_setpoint / DF45_MAX_MOTOR_RAD_PER_S; + control_setpoint_vel_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; - response_packet.data.motion.vel_hall_estimate = vel_setpoint_rads; + response_packet.data.motion.vel_hall_estimate = control_setpoint_vel_rads; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = u_vel_loop; + response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; } @@ -357,7 +369,7 @@ int main() { response_packet.data.motion.vel_computed_setpoint = r_motor; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { - pwm6step_set_duty_cycle_f(u_vel_loop); + pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); } else { pwm6step_set_duty_cycle_f(u_torque_loop); } diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 82c09880..6ec82b36 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -45,7 +45,7 @@ #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) #define TELEMETRY_LOOP_RATE_MS 5 - +#define MOTOR_MAXIMUM_ACCEL 2000 // rad/s^2 From 909707347fcf1aec8ee96e464887a9f29a84c5ec Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 23 Jun 2024 20:10:21 -0700 Subject: [PATCH 089/157] Update kalman gain --- control-board/src/motion/params/body_vel_filter_params.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 4c9d72b5..0cb03f03 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -49,7 +49,7 @@ pub static INIT_ESTIMATE_COV: Matrix3 = 0.0, 0.0, INITIAL_COV]; pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.00798417141, -0.0112913235, 0.0112913235, 0.00798417141, 0.0; - 0.00804295890, -0.00728930537, -0.00728930537, 0.00804295890, 0.0; - 0.0345676344, 0.0422518217, 0.0422518217, 0.0345676344, 0.0114364484]; + matrix![-0.006554, -0.009269, 0.009269, 0.0065542, 0.0; + 0.007210, -0.006147, -0.006147, 0.007214, 0.0; + 0.01268, 0.015415, 0.015415, 0.01268, 0.04181]; From 4d1e9c710c85efe3cd10663cd7330f0c79a13e13 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 23 Jun 2024 23:53:28 -0400 Subject: [PATCH 090/157] controller patches --- control-board/src/motion/robot_controller.rs | 9 ++++++--- control-board/src/tasks/control_task.rs | 1 + control-board/src/tasks/kicker_task.rs | 1 + motor-controller/bin/wheel/system.h | 2 +- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index bd77ec02..ce4813d3 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -179,9 +179,12 @@ impl<'a> BodyVelocityController<'a> { // If the encoder estimate is small enough, then replace IMU value with // encoder value to reduce the jittery noise. - if abs_f32(enc_body_vel[2]) < 0.1 { - measurement[4] = enc_body_vel[2] - } + // TODO at least need to change the conditional here + // if abs_f32(enc_body_vel[2]) < 0.1 { + // if libm::fabsf(enc_body_vel[2]) < 0.1 as f32 { + // defmt::warn!("remove gyro val"); + // measurement[4] = enc_body_vel[2] + // } // Update measurements process observation input into CGKF. self.body_vel_filter.update(&measurement); diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index a37d9fc4..03a6d943 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -276,6 +276,7 @@ impl < let battery_v = 25.0; let controls_enabled = true; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; + defmt::warn!("gyro rads: {}", gyro_rads); let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { if controls_enabled { diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 1f8bbcaf..5db11798 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -125,6 +125,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ }, KickerTaskState::WaitShutdown => { if self.kicker_driver.shutdown_completed() { + defmt::info!("kicker finished shutdown"); self.kicker_task_state = KickerTaskState::PoweredOff; } }, diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 6ec82b36..5a1511b9 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -45,7 +45,7 @@ #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) #define TELEMETRY_LOOP_RATE_MS 5 -#define MOTOR_MAXIMUM_ACCEL 2000 // rad/s^2 +#define MOTOR_MAXIMUM_ACCEL 60000 // rad/s^2 From 78fd968f0da5ff8e4e3d70bc2faedf5538834c74 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 23 Jun 2024 20:58:10 -0700 Subject: [PATCH 091/157] Reverted gain changes --- control-board/src/motion/params/body_vel_filter_params.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 0cb03f03..10b29218 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -49,7 +49,7 @@ pub static INIT_ESTIMATE_COV: Matrix3 = 0.0, 0.0, INITIAL_COV]; pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.006554, -0.009269, 0.009269, 0.0065542, 0.0; - 0.007210, -0.006147, -0.006147, 0.007214, 0.0; - 0.01268, 0.015415, 0.015415, 0.01268, 0.04181]; + matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; + 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; + 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; From 8767937369708e48d326b892e3eef547052168c3 Mon Sep 17 00:00:00 2001 From: "H.M. Murdock" Date: Sun, 30 Jun 2024 16:38:47 -0400 Subject: [PATCH 092/157] Captures changes from on-robot testing from coach laptop. --- .../src/motion/constant_gain_kalman_filter.rs | 2 +- control-board/src/motion/control.rs | 494 ------------- .../motion/params/body_vel_filter_params.rs | 3 +- .../src/motion/params/body_vel_pid_params.rs | 8 +- control-board/src/motion/robot_controller.rs | 22 +- control-board/src/stspin_motor.rs | 2 +- git_diff_firmware.txt | 651 ++++++++++++++++++ motor-controller/bin/wheel/system.h | 2 +- motor-controller/common/pid.c | 6 + software-communication | 2 +- 10 files changed, 681 insertions(+), 511 deletions(-) delete mode 100644 control-board/src/motion/control.rs create mode 100644 git_diff_firmware.txt diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs index bc8fec50..34f11c20 100644 --- a/control-board/src/motion/constant_gain_kalman_filter.rs +++ b/control-board/src/motion/constant_gain_kalman_filter.rs @@ -51,7 +51,7 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS // Constant gain so don't need Innovation Covariance / Optimal Kalman gain calculation self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; - + // self.estimate_cov = (SMatrix::::identity() - self.kalman_gain * self.observation_model) * self.pred_estimate_cov; self.measurement_residual = measurement - self.observation_model * self.state_estimate; diff --git a/control-board/src/motion/control.rs b/control-board/src/motion/control.rs deleted file mode 100644 index f8b96549..00000000 --- a/control-board/src/motion/control.rs +++ /dev/null @@ -1,494 +0,0 @@ -use embassy_executor::SendSpawner; -use embassy_stm32::{ - gpio::{Level, Output, Speed}, mode::Async, usart::Uart -}; -use embassy_sync::pubsub::Subscriber; -use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; -use embassy_time::{Duration, Timer}; -use crate::{ - include_external_cpp_bin, - stm32_interface::Stm32Interface, - stspin_motor::{WheelMotor, DribblerMotor}, - motion::{ - robot_model::{RobotConstants, RobotModel}, - robot_controller::BodyVelocityController - }, - BATTERY_MIN_VOLTAGE, - parameter_interface::ParameterInterface -}; - -use nalgebra::{Vector3, Vector4}; - -use ateam_lib_stm32::make_uart_queue_pair; - -use ateam_common_packets::bindings_radio::{ - BasicControl, - BasicTelemetry, - ControlDebugTelemetry, - ParameterCommand, - ParameterName -}; - -use crate::pins::*; - - -include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} -include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} - -// motor pinout -// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 -// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 -// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 -// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 -// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 - -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 20; - -const TICKS_WITHOUT_PACKET_STOP: u16 = 25; - -make_uart_queue_pair!(FRONT_LEFT, - MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, - MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, - MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(BACK_LEFT, - MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, - MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, - MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(BACK_RIGHT, - MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, - MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, - MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(FRONT_RIGHT, - MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, - MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, - MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -make_uart_queue_pair!(DRIB, - MotorDUart, MotorDDmaRx, MotorDDmaTx, - MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, - MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, - #[link_section = ".axisram.buffers"]); - -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -pub struct Control<'a> { - robot_model: RobotModel, - robot_controller: BodyVelocityController<'a>, - cmd_vel: Vector3, - drib_vel: f32, - front_right_motor: WheelMotor< - 'static, - MotorFRUart, - MotorFRDmaRx, - MotorFRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH - >, - front_left_motor: WheelMotor< - 'static, - MotorFLUart, - MotorFLDmaRx, - MotorFLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH - >, - back_left_motor: WheelMotor< - 'static, - MotorBLUart, - MotorBLDmaRx, - MotorBLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH - >, - back_right_motor: WheelMotor< - 'static, - MotorBRUart, - MotorBRDmaRx, - MotorBRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH - >, - drib_motor: DribblerMotor< - 'static, - MotorDUart, - MotorDDmaRx, - MotorDDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH - >, - ticks_since_packet: u16, - gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, - battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> -} - -// Uart - -impl<'a> Control<'a> { - pub fn new( - spawner: &SendSpawner, - front_right_usart: Uart<'static, MotorFRUart, Async>, - front_left_usart: Uart<'static, MotorFLUart, Async>, - back_left_usart: Uart<'static, MotorBLUart, Async>, - back_right_usart: Uart<'static, MotorBRUart, Async>, - drib_usart: Uart<'static, MotorDUart, Async>, - front_right_boot0_pin: MotorFRBootPin, - front_left_boot0_pin: MotorFLBootPin, - back_left_boot0_pin: MotorBLBootPin, - back_right_boot0_pin: MotorBRBootPin, - drib_boot0_pin: MotorDBootPin, - front_right_reset_pin: MotorFRResetPin, - front_left_reset_pin: MotorFLResetPin, - back_left_reset_pin: MotorBLResetPin, - back_right_reset_pin: MotorBRResetPin, - drib_reset_pin: MotorDResetPin, - ball_detected_thresh: f32, - gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, - battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> - ) -> Control<'a> { - let wheel_firmware_image = WHEEL_FW_IMG; - let drib_firmware_image = DRIB_FW_IMG; - - let (front_right_tx, front_right_rx) = front_right_usart.split(); - let (front_left_tx, front_left_rx) = front_left_usart.split(); - let (back_left_tx, back_left_rx) = back_left_usart.split(); - let (back_right_tx, back_right_rx) = back_right_usart.split(); - let (drib_tx, drib_rx) = drib_usart.split(); - - let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - - let front_right_reset_pin = - Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active - let front_left_reset_pin = - Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_left_reset_pin = - Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_right_reset_pin = - Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active - let drib_reset_pin = - Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active - - spawner - .spawn(FRONT_RIGHT_RX_UART_QUEUE.spawn_task(front_right_rx)) - .unwrap(); - spawner - .spawn(FRONT_RIGHT_TX_UART_QUEUE.spawn_task(front_right_tx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_RX_UART_QUEUE.spawn_task(front_left_rx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_TX_UART_QUEUE.spawn_task(front_left_tx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_RX_UART_QUEUE.spawn_task(back_left_rx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_TX_UART_QUEUE.spawn_task(back_left_tx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_RX_UART_QUEUE.spawn_task(back_right_rx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_TX_UART_QUEUE.spawn_task(back_right_tx)) - .unwrap(); - spawner - .spawn(DRIB_RX_UART_QUEUE.spawn_task(drib_rx)) - .unwrap(); - spawner - .spawn(DRIB_TX_UART_QUEUE.spawn_task(drib_tx)) - .unwrap(); - - let front_right_stm32_interface = Stm32Interface::new( - &FRONT_RIGHT_RX_UART_QUEUE, - &FRONT_RIGHT_TX_UART_QUEUE, - Some(front_right_boot0_pin), - Some(front_right_reset_pin), - ); - let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); - - let front_left_stm32_interface = Stm32Interface::new( - &FRONT_LEFT_RX_UART_QUEUE, - &FRONT_LEFT_TX_UART_QUEUE, - Some(front_left_boot0_pin), - Some(front_left_reset_pin), - ); - let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); - - let back_left_stm32_interface = Stm32Interface::new( - &BACK_LEFT_RX_UART_QUEUE, - &BACK_LEFT_TX_UART_QUEUE, - Some(back_left_boot0_pin), - Some(back_left_reset_pin), - ); - let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); - - let back_right_stm32_interface = Stm32Interface::new( - &BACK_RIGHT_RX_UART_QUEUE, - &BACK_RIGHT_TX_UART_QUEUE, - Some(back_right_boot0_pin), - Some(back_right_reset_pin), - ); - let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); - - let drib_stm32_interface = Stm32Interface::new( - &DRIB_RX_UART_QUEUE, - &DRIB_TX_UART_QUEUE, - Some(drib_boot0_pin), - Some(drib_reset_pin), - ); - let drib_motor = DribblerMotor::new(drib_stm32_interface, drib_firmware_image, ball_detected_thresh); - - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - let body_velocity_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); - - Control { - robot_model, - robot_controller: body_velocity_controller, - cmd_vel: Vector3::new(0., 0., 0.), - drib_vel: 0.0, - front_right_motor, - front_left_motor, - back_left_motor, - back_right_motor, - drib_motor, - ticks_since_packet: 0, - gyro_sub, - battery_sub - } - } - - pub async fn load_firmware(&mut self) { - let _res = embassy_futures::join::join5( - self.front_right_motor.load_default_firmware_image(), - self.front_left_motor.load_default_firmware_image(), - self.back_left_motor.load_default_firmware_image(), - self.back_right_motor.load_default_firmware_image(), - self.drib_motor.load_default_firmware_image(), - ) - .await; - - // defmt::info!("flashing firmware"); - - // self.front_right_motor.load_default_firmware_image().await; - // defmt::info!("FR flashed"); - - // self.front_left_motor.load_default_firmware_image().await; - // defmt::info!("FL flashed"); - - // self.back_left_motor.load_default_firmware_image().await; - // defmt::info!("BL flashed"); - - // self.back_right_motor.load_default_firmware_image().await; - // defmt::info!("BR flashed"); - - // self.drib_motor.load_default_firmware_image().await; - // defmt::info!("DRIB flashed"); - - - - defmt::info!("flashed"); - - // leave reset - // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue - embassy_futures::join::join5( - self.front_right_motor.leave_reset(), - self.front_left_motor.leave_reset(), - self.back_left_motor.leave_reset(), - self.back_right_motor.leave_reset(), - self.drib_motor.leave_reset(), - ) - .await; - - self.front_right_motor.set_telemetry_enabled(true); - self.front_left_motor.set_telemetry_enabled(true); - self.back_left_motor.set_telemetry_enabled(true); - self.back_right_motor.set_telemetry_enabled(true); - self.drib_motor.set_telemetry_enabled(true); - - // need to have telem off by default and enabled later - // theres a race condition to begin processing packets from the first part out - // of reset and waiting for the last part to boot up - Timer::after(Duration::from_millis(10)).await; - } - - fn process_mc_packets(&mut self) { - self.front_right_motor.process_packets(); - self.front_left_motor.process_packets(); - self.back_left_motor.process_packets(); - self.back_right_motor.process_packets(); - self.drib_motor.process_packets(); - } - - pub async fn tick(&mut self, latest_control: Option) -> (Option, ControlDebugTelemetry) { - self.process_mc_packets(); - - self.front_right_motor.log_reset("FR"); - self.front_left_motor.log_reset("RL"); - self.back_left_motor.log_reset("BL"); - self.back_right_motor.log_reset("BR"); - self.drib_motor.log_reset("DRIB"); - - if self.drib_motor.ball_detected() { - defmt::info!("ball detected"); - } - - if let Some(latest_control) = &latest_control { - let cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); - self.cmd_vel = cmd_vel; - self.drib_vel = latest_control.dribbler_speed; - self.ticks_since_packet = 0; - } else { - self.ticks_since_packet += 1; - if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - self.cmd_vel = Vector3::new(0., 0., 0.); - self.ticks_since_packet = 0; - } - } - - // now we have setpoint r(t) in self.cmd_vel - let battery_v = self.battery_sub.next_message_pure().await as f32; - let controls_enabled = true; - let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0; - let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - if controls_enabled - { - // TODO check order - let wheel_vels = Vector4::new( - self.front_right_motor.read_rads(), - self.front_left_motor.read_rads(), - self.back_left_motor.read_rads(), - self.back_right_motor.read_rads() - ); - - // torque values are computed on the spin but put in the current variable - // TODO update this when packet/var names are updated to match software - let wheel_torques = Vector4::new( - self.front_right_motor.read_current(), - self.front_left_motor.read_current(), - self.back_left_motor.read_current(), - self.back_right_motor.read_current() - ); - - // TODO read from channel or something - - self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); - - self.robot_controller.get_wheel_velocities() - } - else - { - self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel) - } - } - else - { - // Battery is too low, set velocity to zero - Vector4::new( - 0.0, - 0.0, - 0.0, - 0.0) - }; - - self.front_right_motor.set_setpoint(wheel_vels[0]); - self.front_left_motor.set_setpoint(wheel_vels[1]); - self.back_left_motor.set_setpoint(wheel_vels[2]); - self.back_right_motor.set_setpoint(wheel_vels[3]); - - let drib_dc = -1.0 * self.drib_vel / 1000.0; - self.drib_motor.set_setpoint(drib_dc); - - self.front_right_motor.send_motion_command(); - self.front_left_motor.send_motion_command(); - self.back_left_motor.send_motion_command(); - self.back_right_motor.send_motion_command(); - self.drib_motor.send_motion_command(); - - (Some(BasicTelemetry { - sequence_number: 0, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: battery_v, - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }), - self.robot_controller.get_control_debug_telem()) - } - - -} - -impl<'a> ParameterInterface for Control<'a> { - fn processes_cmd(&self, param_cmd: &ParameterCommand) -> bool { - return self.robot_controller.processes_cmd(param_cmd); - } - - fn has_name(&self, param_name: ParameterName::Type) -> bool { - return self.robot_controller.has_name(param_name); - } - - fn apply_command(&mut self, param_cmd: &ParameterCommand) -> Result { - return self.robot_controller.apply_command(param_cmd); - } -} diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index 10b29218..c0daedd8 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -51,5 +51,4 @@ pub static INIT_ESTIMATE_COV: Matrix3 = pub static KALMAN_GAIN: Matrix3x5 = matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; - 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; - + 0.02092594, 0.02562894, 0.02562894, 0.02092594, 0.6931516]; diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 5a1303f6..c383e11d 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -2,13 +2,13 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; // Kp, Ki, Kd, Ki_err_min, Ki_err_max pub static PID_GAIN: Matrix3x5 = - matrix![0.1, 0.0, 0.0, -1.0, 1.0; - 0.1, 0.0, 0.0, -1.0, 1.0; - 0.5, 0.0, 0.0, -3.0, 3.0]; + matrix![1.0, 0.0, 0.0, -1.0, 1.0; + 1.0, 0.0, 0.0, -1.0, 1.0; + 1.0, 0.0, 0.0, -3.0, 3.0]; // x, y, theta (m/s, m/s, rad/s) pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate -pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore +pub static BODY_ACC_LIM: Vector3 = vector![8.0, 8.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) // Rough estimate for peak rating diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index bd77ec02..9e0f037d 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -179,9 +179,17 @@ impl<'a> BodyVelocityController<'a> { // If the encoder estimate is small enough, then replace IMU value with // encoder value to reduce the jittery noise. - if abs_f32(enc_body_vel[2]) < 0.1 { + + /* + if abs_f32(enc_body_vel[0]) < 0.1 + && abs_f32(enc_body_vel[1]) < 0.1 + && abs_f32(enc_body_vel[2]) < 0.1 { + measurement[4] = enc_body_vel[2] } + */ + + //defmt::debug!("{}", measurement[4]); // Update measurements process observation input into CGKF. self.body_vel_filter.update(&measurement); @@ -224,13 +232,13 @@ impl<'a> BodyVelocityController<'a> { // and globally invalid. Investiage this later. If problems are suspected, disable this section // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) // TODO cross check in the future against wheel angle plots and analysis - let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; - let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); - let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); - self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); + //let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; + //let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); + //let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); + self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output.as_slice()); // Save command state. - self.cmd_wheel_velocities = wheel_vel_output_clamp; + self.cmd_wheel_velocities = wheel_vel_output; } pub fn get_wheel_velocities(&self) -> Vector4 { @@ -494,4 +502,4 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { return Ok(reply_cmd); } -} \ No newline at end of file +} diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 56e076d5..7eabe72a 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -219,7 +219,7 @@ impl< // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); if mrp.data.motion.master_error() != 0 { - error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + //error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); } // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); diff --git a/git_diff_firmware.txt b/git_diff_firmware.txt new file mode 100644 index 00000000..043f1c66 --- /dev/null +++ b/git_diff_firmware.txt @@ -0,0 +1,651 @@ +diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs +index bc8fec5..34f11c2 100644 +--- a/control-board/src/motion/constant_gain_kalman_filter.rs ++++ b/control-board/src/motion/constant_gain_kalman_filter.rs +@@ -51,7 +51,7 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS + // Constant gain so don't need Innovation Covariance / Optimal Kalman gain calculation + + self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; +- ++ + // self.estimate_cov = (SMatrix::::identity() - self.kalman_gain * self.observation_model) * self.pred_estimate_cov; + + self.measurement_residual = measurement - self.observation_model * self.state_estimate; +diff --git a/control-board/src/motion/control.rs b/control-board/src/motion/control.rs +deleted file mode 100644 +index f8b9654..0000000 +--- a/control-board/src/motion/control.rs ++++ /dev/null +@@ -1,494 +0,0 @@ +-use embassy_executor::SendSpawner; +-use embassy_stm32::{ +- gpio::{Level, Output, Speed}, mode::Async, usart::Uart +-}; +-use embassy_sync::pubsub::Subscriber; +-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; +-use embassy_time::{Duration, Timer}; +-use crate::{ +- include_external_cpp_bin, +- stm32_interface::Stm32Interface, +- stspin_motor::{WheelMotor, DribblerMotor}, +- motion::{ +- robot_model::{RobotConstants, RobotModel}, +- robot_controller::BodyVelocityController +- }, +- BATTERY_MIN_VOLTAGE, +- parameter_interface::ParameterInterface +-}; +- +-use nalgebra::{Vector3, Vector4}; +- +-use ateam_lib_stm32::make_uart_queue_pair; +- +-use ateam_common_packets::bindings_radio::{ +- BasicControl, +- BasicTelemetry, +- ControlDebugTelemetry, +- ParameterCommand, +- ParameterName +-}; +- +-use crate::pins::*; +- +- +-include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} +-include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} +- +-// motor pinout +-// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 +-// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 +-// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 +-// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 +-// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 +- +-const MAX_TX_PACKET_SIZE: usize = 64; +-const TX_BUF_DEPTH: usize = 3; +-const MAX_RX_PACKET_SIZE: usize = 64; +-const RX_BUF_DEPTH: usize = 20; +- +-const TICKS_WITHOUT_PACKET_STOP: u16 = 25; +- +-make_uart_queue_pair!(FRONT_LEFT, +- MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, +- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, +- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, +- #[link_section = ".axisram.buffers"]); +- +-make_uart_queue_pair!(BACK_LEFT, +- MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, +- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, +- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, +- #[link_section = ".axisram.buffers"]); +- +-make_uart_queue_pair!(BACK_RIGHT, +- MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, +- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, +- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, +- #[link_section = ".axisram.buffers"]); +- +-make_uart_queue_pair!(FRONT_RIGHT, +- MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, +- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, +- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, +- #[link_section = ".axisram.buffers"]); +- +-make_uart_queue_pair!(DRIB, +- MotorDUart, MotorDDmaRx, MotorDDmaTx, +- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, +- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, +- #[link_section = ".axisram.buffers"]); +- +-const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); +-const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm +-const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot +- +-pub struct Control<'a> { +- robot_model: RobotModel, +- robot_controller: BodyVelocityController<'a>, +- cmd_vel: Vector3, +- drib_vel: f32, +- front_right_motor: WheelMotor< +- 'static, +- MotorFRUart, +- MotorFRDmaRx, +- MotorFRDmaTx, +- MAX_RX_PACKET_SIZE, +- MAX_TX_PACKET_SIZE, +- RX_BUF_DEPTH, +- TX_BUF_DEPTH +- >, +- front_left_motor: WheelMotor< +- 'static, +- MotorFLUart, +- MotorFLDmaRx, +- MotorFLDmaTx, +- MAX_RX_PACKET_SIZE, +- MAX_TX_PACKET_SIZE, +- RX_BUF_DEPTH, +- TX_BUF_DEPTH +- >, +- back_left_motor: WheelMotor< +- 'static, +- MotorBLUart, +- MotorBLDmaRx, +- MotorBLDmaTx, +- MAX_RX_PACKET_SIZE, +- MAX_TX_PACKET_SIZE, +- RX_BUF_DEPTH, +- TX_BUF_DEPTH +- >, +- back_right_motor: WheelMotor< +- 'static, +- MotorBRUart, +- MotorBRDmaRx, +- MotorBRDmaTx, +- MAX_RX_PACKET_SIZE, +- MAX_TX_PACKET_SIZE, +- RX_BUF_DEPTH, +- TX_BUF_DEPTH +- >, +- drib_motor: DribblerMotor< +- 'static, +- MotorDUart, +- MotorDDmaRx, +- MotorDDmaTx, +- MAX_RX_PACKET_SIZE, +- MAX_TX_PACKET_SIZE, +- RX_BUF_DEPTH, +- TX_BUF_DEPTH +- >, +- ticks_since_packet: u16, +- gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, +- battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> +-} +- +-// Uart +- +-impl<'a> Control<'a> { +- pub fn new( +- spawner: &SendSpawner, +- front_right_usart: Uart<'static, MotorFRUart, Async>, +- front_left_usart: Uart<'static, MotorFLUart, Async>, +- back_left_usart: Uart<'static, MotorBLUart, Async>, +- back_right_usart: Uart<'static, MotorBRUart, Async>, +- drib_usart: Uart<'static, MotorDUart, Async>, +- front_right_boot0_pin: MotorFRBootPin, +- front_left_boot0_pin: MotorFLBootPin, +- back_left_boot0_pin: MotorBLBootPin, +- back_right_boot0_pin: MotorBRBootPin, +- drib_boot0_pin: MotorDBootPin, +- front_right_reset_pin: MotorFRResetPin, +- front_left_reset_pin: MotorFLResetPin, +- back_left_reset_pin: MotorBLResetPin, +- back_right_reset_pin: MotorBRResetPin, +- drib_reset_pin: MotorDResetPin, +- ball_detected_thresh: f32, +- gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, +- battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> +- ) -> Control<'a> { +- let wheel_firmware_image = WHEEL_FW_IMG; +- let drib_firmware_image = DRIB_FW_IMG; +- +- let (front_right_tx, front_right_rx) = front_right_usart.split(); +- let (front_left_tx, front_left_rx) = front_left_usart.split(); +- let (back_left_tx, back_left_rx) = back_left_usart.split(); +- let (back_right_tx, back_right_rx) = back_right_usart.split(); +- let (drib_tx, drib_rx) = drib_usart.split(); +- +- let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active +- let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active +- let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active +- let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active +- let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active +- +- let front_right_reset_pin = +- Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active +- let front_left_reset_pin = +- Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active +- let back_left_reset_pin = +- Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active +- let back_right_reset_pin = +- Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active +- let drib_reset_pin = +- Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active +- +- spawner +- .spawn(FRONT_RIGHT_RX_UART_QUEUE.spawn_task(front_right_rx)) +- .unwrap(); +- spawner +- .spawn(FRONT_RIGHT_TX_UART_QUEUE.spawn_task(front_right_tx)) +- .unwrap(); +- spawner +- .spawn(FRONT_LEFT_RX_UART_QUEUE.spawn_task(front_left_rx)) +- .unwrap(); +- spawner +- .spawn(FRONT_LEFT_TX_UART_QUEUE.spawn_task(front_left_tx)) +- .unwrap(); +- spawner +- .spawn(BACK_LEFT_RX_UART_QUEUE.spawn_task(back_left_rx)) +- .unwrap(); +- spawner +- .spawn(BACK_LEFT_TX_UART_QUEUE.spawn_task(back_left_tx)) +- .unwrap(); +- spawner +- .spawn(BACK_RIGHT_RX_UART_QUEUE.spawn_task(back_right_rx)) +- .unwrap(); +- spawner +- .spawn(BACK_RIGHT_TX_UART_QUEUE.spawn_task(back_right_tx)) +- .unwrap(); +- spawner +- .spawn(DRIB_RX_UART_QUEUE.spawn_task(drib_rx)) +- .unwrap(); +- spawner +- .spawn(DRIB_TX_UART_QUEUE.spawn_task(drib_tx)) +- .unwrap(); +- +- let front_right_stm32_interface = Stm32Interface::new( +- &FRONT_RIGHT_RX_UART_QUEUE, +- &FRONT_RIGHT_TX_UART_QUEUE, +- Some(front_right_boot0_pin), +- Some(front_right_reset_pin), +- ); +- let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); +- +- let front_left_stm32_interface = Stm32Interface::new( +- &FRONT_LEFT_RX_UART_QUEUE, +- &FRONT_LEFT_TX_UART_QUEUE, +- Some(front_left_boot0_pin), +- Some(front_left_reset_pin), +- ); +- let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); +- +- let back_left_stm32_interface = Stm32Interface::new( +- &BACK_LEFT_RX_UART_QUEUE, +- &BACK_LEFT_TX_UART_QUEUE, +- Some(back_left_boot0_pin), +- Some(back_left_reset_pin), +- ); +- let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); +- +- let back_right_stm32_interface = Stm32Interface::new( +- &BACK_RIGHT_RX_UART_QUEUE, +- &BACK_RIGHT_TX_UART_QUEUE, +- Some(back_right_boot0_pin), +- Some(back_right_reset_pin), +- ); +- let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); +- +- let drib_stm32_interface = Stm32Interface::new( +- &DRIB_RX_UART_QUEUE, +- &DRIB_TX_UART_QUEUE, +- Some(drib_boot0_pin), +- Some(drib_reset_pin), +- ); +- let drib_motor = DribblerMotor::new(drib_stm32_interface, drib_firmware_image, ball_detected_thresh); +- +- let robot_model_constants: RobotConstants = RobotConstants { +- wheel_angles_rad: Vector4::new( +- WHEEL_ANGLES_DEG[0].to_radians(), +- WHEEL_ANGLES_DEG[1].to_radians(), +- WHEEL_ANGLES_DEG[2].to_radians(), +- WHEEL_ANGLES_DEG[3].to_radians(), +- ), +- wheel_radius_m: Vector4::new( +- WHEEL_RADIUS_M, +- WHEEL_RADIUS_M, +- WHEEL_RADIUS_M, +- WHEEL_RADIUS_M, +- ), +- wheel_dist_to_cent_m: Vector4::new( +- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, +- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, +- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, +- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, +- ), +- }; +- +- let robot_model: RobotModel = RobotModel::new(robot_model_constants); +- +- let body_velocity_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); +- +- Control { +- robot_model, +- robot_controller: body_velocity_controller, +- cmd_vel: Vector3::new(0., 0., 0.), +- drib_vel: 0.0, +- front_right_motor, +- front_left_motor, +- back_left_motor, +- back_right_motor, +- drib_motor, +- ticks_since_packet: 0, +- gyro_sub, +- battery_sub +- } +- } +- +- pub async fn load_firmware(&mut self) { +- let _res = embassy_futures::join::join5( +- self.front_right_motor.load_default_firmware_image(), +- self.front_left_motor.load_default_firmware_image(), +- self.back_left_motor.load_default_firmware_image(), +- self.back_right_motor.load_default_firmware_image(), +- self.drib_motor.load_default_firmware_image(), +- ) +- .await; +- +- // defmt::info!("flashing firmware"); +- +- // self.front_right_motor.load_default_firmware_image().await; +- // defmt::info!("FR flashed"); +- +- // self.front_left_motor.load_default_firmware_image().await; +- // defmt::info!("FL flashed"); +- +- // self.back_left_motor.load_default_firmware_image().await; +- // defmt::info!("BL flashed"); +- +- // self.back_right_motor.load_default_firmware_image().await; +- // defmt::info!("BR flashed"); +- +- // self.drib_motor.load_default_firmware_image().await; +- // defmt::info!("DRIB flashed"); +- +- +- +- defmt::info!("flashed"); +- +- // leave reset +- // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue +- embassy_futures::join::join5( +- self.front_right_motor.leave_reset(), +- self.front_left_motor.leave_reset(), +- self.back_left_motor.leave_reset(), +- self.back_right_motor.leave_reset(), +- self.drib_motor.leave_reset(), +- ) +- .await; +- +- self.front_right_motor.set_telemetry_enabled(true); +- self.front_left_motor.set_telemetry_enabled(true); +- self.back_left_motor.set_telemetry_enabled(true); +- self.back_right_motor.set_telemetry_enabled(true); +- self.drib_motor.set_telemetry_enabled(true); +- +- // need to have telem off by default and enabled later +- // theres a race condition to begin processing packets from the first part out +- // of reset and waiting for the last part to boot up +- Timer::after(Duration::from_millis(10)).await; +- } +- +- fn process_mc_packets(&mut self) { +- self.front_right_motor.process_packets(); +- self.front_left_motor.process_packets(); +- self.back_left_motor.process_packets(); +- self.back_right_motor.process_packets(); +- self.drib_motor.process_packets(); +- } +- +- pub async fn tick(&mut self, latest_control: Option) -> (Option, ControlDebugTelemetry) { +- self.process_mc_packets(); +- +- self.front_right_motor.log_reset("FR"); +- self.front_left_motor.log_reset("RL"); +- self.back_left_motor.log_reset("BL"); +- self.back_right_motor.log_reset("BR"); +- self.drib_motor.log_reset("DRIB"); +- +- if self.drib_motor.ball_detected() { +- defmt::info!("ball detected"); +- } +- +- if let Some(latest_control) = &latest_control { +- let cmd_vel = Vector3::new( +- latest_control.vel_x_linear, +- latest_control.vel_y_linear, +- latest_control.vel_z_angular, +- ); +- self.cmd_vel = cmd_vel; +- self.drib_vel = latest_control.dribbler_speed; +- self.ticks_since_packet = 0; +- } else { +- self.ticks_since_packet += 1; +- if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { +- self.cmd_vel = Vector3::new(0., 0., 0.); +- self.ticks_since_packet = 0; +- } +- } +- +- // now we have setpoint r(t) in self.cmd_vel +- let battery_v = self.battery_sub.next_message_pure().await as f32; +- let controls_enabled = true; +- let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0; +- let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { +- if controls_enabled +- { +- // TODO check order +- let wheel_vels = Vector4::new( +- self.front_right_motor.read_rads(), +- self.front_left_motor.read_rads(), +- self.back_left_motor.read_rads(), +- self.back_right_motor.read_rads() +- ); +- +- // torque values are computed on the spin but put in the current variable +- // TODO update this when packet/var names are updated to match software +- let wheel_torques = Vector4::new( +- self.front_right_motor.read_current(), +- self.front_left_motor.read_current(), +- self.back_left_motor.read_current(), +- self.back_right_motor.read_current() +- ); +- +- // TODO read from channel or something +- +- self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); +- +- self.robot_controller.get_wheel_velocities() +- } +- else +- { +- self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel) +- } +- } +- else +- { +- // Battery is too low, set velocity to zero +- Vector4::new( +- 0.0, +- 0.0, +- 0.0, +- 0.0) +- }; +- +- self.front_right_motor.set_setpoint(wheel_vels[0]); +- self.front_left_motor.set_setpoint(wheel_vels[1]); +- self.back_left_motor.set_setpoint(wheel_vels[2]); +- self.back_right_motor.set_setpoint(wheel_vels[3]); +- +- let drib_dc = -1.0 * self.drib_vel / 1000.0; +- self.drib_motor.set_setpoint(drib_dc); +- +- self.front_right_motor.send_motion_command(); +- self.front_left_motor.send_motion_command(); +- self.back_left_motor.send_motion_command(); +- self.back_right_motor.send_motion_command(); +- self.drib_motor.send_motion_command(); +- +- (Some(BasicTelemetry { +- sequence_number: 0, +- robot_revision_major: 0, +- robot_revision_minor: 0, +- battery_level: battery_v, +- battery_temperature: 0., +- _bitfield_align_1: [], +- _bitfield_1: BasicTelemetry::new_bitfield_1( +- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, +- ), +- motor_0_temperature: 0., +- motor_1_temperature: 0., +- motor_2_temperature: 0., +- motor_3_temperature: 0., +- motor_4_temperature: 0., +- kicker_charge_level: 0., +- }), +- self.robot_controller.get_control_debug_telem()) +- } +- +- +-} +- +-impl<'a> ParameterInterface for Control<'a> { +- fn processes_cmd(&self, param_cmd: &ParameterCommand) -> bool { +- return self.robot_controller.processes_cmd(param_cmd); +- } +- +- fn has_name(&self, param_name: ParameterName::Type) -> bool { +- return self.robot_controller.has_name(param_name); +- } +- +- fn apply_command(&mut self, param_cmd: &ParameterCommand) -> Result { +- return self.robot_controller.apply_command(param_cmd); +- } +-} +diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs +index 10b2921..c0daedd 100644 +--- a/control-board/src/motion/params/body_vel_filter_params.rs ++++ b/control-board/src/motion/params/body_vel_filter_params.rs +@@ -51,5 +51,4 @@ pub static INIT_ESTIMATE_COV: Matrix3 = + pub static KALMAN_GAIN: Matrix3x5 = + matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; + 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; +- 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; +- ++ 0.02092594, 0.02562894, 0.02562894, 0.02092594, 0.6931516]; +diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs +index 5a1303f..c383e11 100644 +--- a/control-board/src/motion/params/body_vel_pid_params.rs ++++ b/control-board/src/motion/params/body_vel_pid_params.rs +@@ -2,13 +2,13 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; + + // Kp, Ki, Kd, Ki_err_min, Ki_err_max + pub static PID_GAIN: Matrix3x5 = +- matrix![0.1, 0.0, 0.0, -1.0, 1.0; +- 0.1, 0.0, 0.0, -1.0, 1.0; +- 0.5, 0.0, 0.0, -3.0, 3.0]; ++ matrix![1.0, 0.0, 0.0, -1.0, 1.0; ++ 1.0, 0.0, 0.0, -1.0, 1.0; ++ 1.0, 0.0, 0.0, -3.0, 3.0]; + + // x, y, theta (m/s, m/s, rad/s) + pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +-pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore ++pub static BODY_ACC_LIM: Vector3 = vector![8.0, 8.0, 36.0]; // TODO calibrate/ignore + + // FL, BL, BR, FR (rad/s^2) + // Rough estimate for peak rating +diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs +index bd77ec0..9e0f037 100644 +--- a/control-board/src/motion/robot_controller.rs ++++ b/control-board/src/motion/robot_controller.rs +@@ -179,9 +179,17 @@ impl<'a> BodyVelocityController<'a> { + + // If the encoder estimate is small enough, then replace IMU value with + // encoder value to reduce the jittery noise. +- if abs_f32(enc_body_vel[2]) < 0.1 { ++ ++ /* ++ if abs_f32(enc_body_vel[0]) < 0.1 ++ && abs_f32(enc_body_vel[1]) < 0.1 ++ && abs_f32(enc_body_vel[2]) < 0.1 { ++ + measurement[4] = enc_body_vel[2] + } ++ */ ++ ++ //defmt::debug!("{}", measurement[4]); + + // Update measurements process observation input into CGKF. + self.body_vel_filter.update(&measurement); +@@ -224,13 +232,13 @@ impl<'a> BodyVelocityController<'a> { + // and globally invalid. Investiage this later. If problems are suspected, disable this section + // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) + // TODO cross check in the future against wheel angle plots and analysis +- let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; +- let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); +- let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); +- self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); ++ //let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; ++ //let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); ++ //let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); ++ self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output.as_slice()); + + // Save command state. +- self.cmd_wheel_velocities = wheel_vel_output_clamp; ++ self.cmd_wheel_velocities = wheel_vel_output; + } + + pub fn get_wheel_velocities(&self) -> Vector4 { +@@ -494,4 +502,4 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { + + return Ok(reply_cmd); + } +-} +\ No newline at end of file ++} +diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs +index 56e076d..7eabe72 100644 +--- a/control-board/src/stspin_motor.rs ++++ b/control-board/src/stspin_motor.rs +@@ -219,7 +219,7 @@ impl< + // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); + // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); + if mrp.data.motion.master_error() != 0 { +- error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); ++ //error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + } + // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); + // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); +diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h +index 6ec82b3..3b9d55c 100644 +--- a/motor-controller/bin/wheel/system.h ++++ b/motor-controller/bin/wheel/system.h +@@ -45,7 +45,7 @@ + #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) + #define TELEMETRY_LOOP_RATE_MS 5 + +-#define MOTOR_MAXIMUM_ACCEL 2000 // rad/s^2 ++#define MOTOR_MAXIMUM_ACCEL 5500 // rad/s^2 + + + +diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c +index e36cc37..208e950 100644 +--- a/motor-controller/common/pid.c ++++ b/motor-controller/common/pid.c +@@ -1,5 +1,6 @@ + + #include ++#include + + #include "pid.h" + +@@ -31,6 +32,11 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { + // float alpha = 0.37f; + // pid->eI = (pid->eI * alpha) + (err * dt * (1.0f - alpha)); + pid->eI = pid->eI + (err * dt); ++ ++ if (fabs(r) < 3.0) { ++ pid->eI = 0.0; ++ } ++ + if (pid->eI > pid->pid_constants->kI_max) { + pid->eI = pid->pid_constants->kI_max; + } else if (pid->eI < pid->pid_constants->kI_min) { +diff --git a/software-communication b/software-communication +index 5aae1e3..cb445b0 160000 +--- a/software-communication ++++ b/software-communication +@@ -1 +1 @@ +-Subproject commit 5aae1e348f5a60a2279cbbff66ecb66d468620fd ++Subproject commit cb445b0c305e7033d7c05be1f2ee1695c461252e diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 6ec82b36..3b9d55cd 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -45,7 +45,7 @@ #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) #define TELEMETRY_LOOP_RATE_MS 5 -#define MOTOR_MAXIMUM_ACCEL 2000 // rad/s^2 +#define MOTOR_MAXIMUM_ACCEL 5500 // rad/s^2 diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c index e36cc37a..208e950c 100644 --- a/motor-controller/common/pid.c +++ b/motor-controller/common/pid.c @@ -1,5 +1,6 @@ #include +#include #include "pid.h" @@ -31,6 +32,11 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { // float alpha = 0.37f; // pid->eI = (pid->eI * alpha) + (err * dt * (1.0f - alpha)); pid->eI = pid->eI + (err * dt); + + if (fabs(r) < 3.0) { + pid->eI = 0.0; + } + if (pid->eI > pid->pid_constants->kI_max) { pid->eI = pid->pid_constants->kI_max; } else if (pid->eI < pid->pid_constants->kI_min) { diff --git a/software-communication b/software-communication index 5aae1e34..cb445b0c 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit 5aae1e348f5a60a2279cbbff66ecb66d468620fd +Subproject commit cb445b0c305e7033d7c05be1f2ee1695c461252e From 8aad7cd9d198fdcb004c0e9819b61ef0700497a9 Mon Sep 17 00:00:00 2001 From: "H.M. Murdock" Date: Sun, 30 Jun 2024 17:58:24 -0400 Subject: [PATCH 093/157] move controls enabled into control function --- control-board/src/motion/robot_controller.rs | 8 ++++++-- control-board/src/tasks/control_task.rs | 16 ++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 9e0f037d..57757a62 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -154,7 +154,7 @@ impl<'a> BodyVelocityController<'a> { } } - pub fn control_update(&mut self, body_vel_setpoint: &Vector3, wheel_velocities: &Vector4, wheel_torques: &Vector4, gyro_theta: f32) { + pub fn control_update(&mut self, body_vel_setpoint: &Vector3, wheel_velocities: &Vector4, wheel_torques: &Vector4, gyro_theta: f32, controls_enabled: bool) { // Assign telemetry data // TODO pass all of the gyro data up, not just theta self.debug_telemetry.imu_gyro[2] = gyro_theta; @@ -238,7 +238,11 @@ impl<'a> BodyVelocityController<'a> { self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output.as_slice()); // Save command state. - self.cmd_wheel_velocities = wheel_vel_output; + if controls_enabled { + self.cmd_wheel_velocities = wheel_vel_output; + } else { + self.cmd_wheel_velocities = self.robot_model.robot_vel_to_wheel_vel(&body_vel_setpoint); + } } pub fn get_wheel_velocities(&self) -> Vector4 { diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index a37d9fc4..050a570f 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -101,7 +101,8 @@ impl < fn do_control_update(&mut self, robot_controller: &mut BodyVelocityController, cmd_vel: Vector3, - gyro_rads: f32 + gyro_rads: f32, + controls_enabled: bool ) -> Vector4 /* Provide the motion controller with the current wheel velocities @@ -127,7 +128,7 @@ impl < // TODO read from channel or something - robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); robot_controller.get_wheel_velocities() } @@ -274,16 +275,11 @@ impl < // now we have setpoint r(t) in self.cmd_vel // let battery_v = battery_sub.next_message_pure().await as f32; let battery_v = 25.0; - let controls_enabled = true; + let controls_enabled = false; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { - if controls_enabled - { - // TODO check order - self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads) - } else { - robot_model.robot_vel_to_wheel_vel(&cmd_vel) - } + // TODO check order + self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) } else { // Battery is too low, set velocity to zero Vector4::new( From a3c90647d465ca73d01732effba54001f19539ca Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 30 Jun 2024 22:15:07 -0400 Subject: [PATCH 094/157] robot beeps for OV and UV --- control-board/src/bin/control/main.rs | 1 - control-board/src/bin/hwtest-io/main.rs | 6 +- control-board/src/bin/hwtest-motor/control.rs | 452 ------------------ control-board/src/bin/hwtest-motor/main.rs | 242 ++++------ control-board/src/robot_state.rs | 9 +- control-board/src/songs.rs | 17 +- control-board/src/tasks/audio_task.rs | 9 +- control-board/src/tasks/imu_task.rs | 8 +- control-board/src/tasks/user_io_task.rs | 10 +- 9 files changed, 135 insertions(+), 619 deletions(-) delete mode 100644 control-board/src/bin/hwtest-motor/control.rs diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 9e0658f3..e8aec7cb 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -123,7 +123,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; loop { - defmt::info!("{}", battery_volt_subscriber.next_message_pure().await); Timer::after_millis(10).await; } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-io/main.rs b/control-board/src/bin/hwtest-io/main.rs index c63bb9bb..70226357 100644 --- a/control-board/src/bin/hwtest-io/main.rs +++ b/control-board/src/bin/hwtest-io/main.rs @@ -6,7 +6,7 @@ use embassy_stm32::interrupt; use defmt_rtt as _; -use ateam_control_board::{create_io_task, get_system_config, pins::BatteryVoltPubSub, robot_state::SharedRobotState}; +use ateam_control_board::{create_audio_task, create_io_task, create_shutdown_task, get_system_config, pins::BatteryVoltPubSub, robot_state::SharedRobotState}; use embassy_sync::pubsub::PubSubChannel; @@ -53,6 +53,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); + create_shutdown_task!(main_spawner, robot_state, p); + + create_audio_task!(main_spawner, robot_state, p); + loop { Timer::after_millis(1000).await; } diff --git a/control-board/src/bin/hwtest-motor/control.rs b/control-board/src/bin/hwtest-motor/control.rs deleted file mode 100644 index f19750d6..00000000 --- a/control-board/src/bin/hwtest-motor/control.rs +++ /dev/null @@ -1,452 +0,0 @@ -use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry}; -use embassy_executor::SendSpawner; -use embassy_stm32::{ - gpio::{Level, Output, Speed}, - usart::Uart, -}; -use embassy_time::{Duration, Timer}; -use ateam_control_board::{ - include_external_cpp_bin, - queue::Buffer, - motion::robot_model::{RobotConstants, RobotModel}, - stm32_interface::Stm32Interface, - stspin_motor::{WheelMotor, DribblerMotor}, - uart_queue::{UartReadQueue, UartWriteQueue}, -}; -use nalgebra::{Vector3, Vector4}; - -use crate::pins::*; - -include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} -include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} - -// motor pinout -// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 -// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 -// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 -// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 -// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 - -const MAX_TX_PACKET_SIZE: usize = 64; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 64; -const RX_BUF_DEPTH: usize = 20; - -const TICKS_WITHOUT_PACKET_STOP: u16 = 25; - -// buffers for front right -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_RIGHT_BUFFERS_RX }); - -// buffers for front left -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut FRONT_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static FRONT_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut FRONT_LEFT_BUFFERS_RX }); - -// buffers for back left -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_LEFT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_LEFT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_LEFT_BUFFERS_RX }); - -// buffers for back right -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut BACK_RIGHT_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static BACK_RIGHT_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut BACK_RIGHT_BUFFERS_RX }); - -// buffers for dribbler -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static DRIB_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut DRIB_BUFFERS_TX }); - -#[link_section = ".axisram.buffers"] -static mut DRIB_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static DRIB_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut DRIB_BUFFERS_RX }); - -const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); -const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm -const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot - -pub struct Control { - robot_model: RobotModel, - cmd_vel: Vector3, - drib_vel: f32, - front_right_motor: WheelMotor< - 'static, - MotorFRUart, - MotorFRDmaRx, - MotorFRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFRBootPin, - MotorFRResetPin, - >, - front_left_motor: WheelMotor< - 'static, - MotorFLUart, - MotorFLDmaRx, - MotorFLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorFLBootPin, - MotorFLResetPin, - >, - back_left_motor: WheelMotor< - 'static, - MotorBLUart, - MotorBLDmaRx, - MotorBLDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBLBootPin, - MotorBLResetPin, - >, - back_right_motor: WheelMotor< - 'static, - MotorBRUart, - MotorBRDmaRx, - MotorBRDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorBRBootPin, - MotorBRResetPin, - >, - drib_motor: DribblerMotor< - 'static, - MotorDUart, - MotorDDmaRx, - MotorDDmaTx, - MAX_RX_PACKET_SIZE, - MAX_TX_PACKET_SIZE, - RX_BUF_DEPTH, - TX_BUF_DEPTH, - MotorDBootPin, - MotorDResetPin, - >, - ticks_since_packet: u16, -} - -// Uart - -impl Control { - pub fn new( - spawner: &SendSpawner, - front_right_usart: Uart<'static, MotorFRUart, MotorFRDmaTx, MotorFRDmaRx>, - front_left_usart: Uart<'static, MotorFLUart, MotorFLDmaTx, MotorFLDmaRx>, - back_left_usart: Uart<'static, MotorBLUart, MotorBLDmaTx, MotorBLDmaRx>, - back_right_usart: Uart<'static, MotorBRUart, MotorBRDmaTx, MotorBRDmaRx>, - drib_usart: Uart<'static, MotorDUart, MotorDDmaTx, MotorDDmaRx>, - front_right_boot0_pin: MotorFRBootPin, - front_left_boot0_pin: MotorFLBootPin, - back_left_boot0_pin: MotorBLBootPin, - back_right_boot0_pin: MotorBRBootPin, - drib_boot0_pin: MotorDBootPin, - front_right_reset_pin: MotorFRResetPin, - front_left_reset_pin: MotorFLResetPin, - back_left_reset_pin: MotorBLResetPin, - back_right_reset_pin: MotorBRResetPin, - drib_reset_pin: MotorDResetPin, - ball_detected_thresh: f32, - ) -> Control { - let wheel_firmware_image = WHEEL_FW_IMG; - let drib_firmware_image = DRIB_FW_IMG; - - let (front_right_tx, front_right_rx) = front_right_usart.split(); - let (front_left_tx, front_left_rx) = front_left_usart.split(); - let (back_left_tx, back_left_rx) = back_left_usart.split(); - let (back_right_tx, back_right_rx) = back_right_usart.split(); - let (drib_tx, drib_rx) = drib_usart.split(); - - let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active - - let front_right_reset_pin = - Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active - let front_left_reset_pin = - Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_left_reset_pin = - Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active - let back_right_reset_pin = - Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active - let drib_reset_pin = - Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active - - spawner - .spawn(FRONT_RIGHT_QUEUE_RX.spawn_task(front_right_rx)) - .unwrap(); - spawner - .spawn(FRONT_RIGHT_QUEUE_TX.spawn_task(front_right_tx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_RX.spawn_task(front_left_rx)) - .unwrap(); - spawner - .spawn(FRONT_LEFT_QUEUE_TX.spawn_task(front_left_tx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_QUEUE_RX.spawn_task(back_left_rx)) - .unwrap(); - spawner - .spawn(BACK_LEFT_QUEUE_TX.spawn_task(back_left_tx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_RX.spawn_task(back_right_rx)) - .unwrap(); - spawner - .spawn(BACK_RIGHT_QUEUE_TX.spawn_task(back_right_tx)) - .unwrap(); - spawner - .spawn(DRIB_QUEUE_RX.spawn_task(drib_rx)) - .unwrap(); - spawner - .spawn(DRIB_QUEUE_TX.spawn_task(drib_tx)) - .unwrap(); - - let front_right_stm32_interface = Stm32Interface::new( - &FRONT_RIGHT_QUEUE_RX, - &FRONT_RIGHT_QUEUE_TX, - Some(front_right_boot0_pin), - Some(front_right_reset_pin), - ); - let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); - - let front_left_stm32_interface = Stm32Interface::new( - &FRONT_LEFT_QUEUE_RX, - &FRONT_LEFT_QUEUE_TX, - Some(front_left_boot0_pin), - Some(front_left_reset_pin), - ); - let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); - - let back_left_stm32_interface = Stm32Interface::new( - &BACK_LEFT_QUEUE_RX, - &BACK_LEFT_QUEUE_TX, - Some(back_left_boot0_pin), - Some(back_left_reset_pin), - ); - let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); - - let back_right_stm32_interface = Stm32Interface::new( - &BACK_RIGHT_QUEUE_RX, - &BACK_RIGHT_QUEUE_TX, - Some(back_right_boot0_pin), - Some(back_right_reset_pin), - ); - let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); - - let drib_stm32_interface = Stm32Interface::new( - &DRIB_QUEUE_RX, - &DRIB_QUEUE_TX, - Some(drib_boot0_pin), - Some(drib_reset_pin), - ); - let drib_motor = DribblerMotor::new(drib_stm32_interface, drib_firmware_image, ball_detected_thresh); - - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - Control { - robot_model, - cmd_vel: Vector3::new(0., 0., 0.), - drib_vel: 0.0, - front_right_motor, - front_left_motor, - back_left_motor, - back_right_motor, - drib_motor, - ticks_since_packet: 0, - } - } - - pub async fn load_firmware(&mut self) { - defmt::info!("flashing firmware"); - - let _ = self.front_right_motor.load_default_firmware_image().await; - defmt::info!("FR flashed"); - - let _ = self.front_left_motor.load_default_firmware_image().await; - defmt::info!("FL flashed"); - - let _ = self.back_left_motor.load_default_firmware_image().await; - defmt::info!("BL flashed"); - - let _ = self.back_right_motor.load_default_firmware_image().await; - defmt::info!("BR flashed"); - - let _ = self.drib_motor.load_default_firmware_image().await; - defmt::info!("DRIB flashed"); - - - - defmt::info!("all motor controller firmware flashed"); - - // leave reset - // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue - embassy_futures::join::join5( - self.front_right_motor.leave_reset(), - self.front_left_motor.leave_reset(), - self.back_left_motor.leave_reset(), - self.back_right_motor.leave_reset(), - self.drib_motor.leave_reset(), - ) - .await; - - self.front_right_motor.set_telemetry_enabled(true); - self.front_left_motor.set_telemetry_enabled(true); - self.back_left_motor.set_telemetry_enabled(true); - self.back_right_motor.set_telemetry_enabled(true); - self.drib_motor.set_telemetry_enabled(true); - - // need to have telem off by default and enabled later - // theres a race condition to begin processing packets from the first part out - // of reset and waiting for the last part to boot up - Timer::after(Duration::from_millis(10)).await; - } - - pub fn tick(&mut self, latest_control: Option) -> Option { - self.front_right_motor.process_packets(); - self.front_left_motor.process_packets(); - self.back_left_motor.process_packets(); - self.back_right_motor.process_packets(); - self.drib_motor.process_packets(); - - self.front_right_motor.log_reset("FR"); - self.front_left_motor.log_reset("RL"); - self.back_left_motor.log_reset("BL"); - self.back_right_motor.log_reset("BR"); - self.drib_motor.log_reset("DRIB"); - - if self.drib_motor.ball_detected() { - defmt::info!("ball detected"); - } - - // let vel = 0.0005; // DC - // let angle: f32 = core::f32::consts::PI / 4.0; - // let cmd_vel: Vector3 = - // Vector3::new(libm::sinf(angle) * vel, libm::cosf(angle) * vel, 0.0); - if let Some(latest_control) = &latest_control { - let cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); - self.cmd_vel = cmd_vel; - self.drib_vel = latest_control.dribbler_speed; - self.ticks_since_packet = 0; - } else { - self.ticks_since_packet += 1; - if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - self.cmd_vel = Vector3::new(0., 0., 0.); - self.ticks_since_packet = 0; - } - } - let wheel_vels = self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel); - - // let c_vel = libm::sinf(angle) / 2.0; - // let c_vel = 0.2; - self.front_right_motor.set_setpoint(wheel_vels[0]); - self.front_left_motor.set_setpoint(wheel_vels[1]); - self.back_left_motor.set_setpoint(wheel_vels[2]); - self.back_right_motor.set_setpoint(wheel_vels[3]); - // angle += core::f32::consts::FRAC_2_PI / 200.0; - let drib_dc = self.drib_vel / 1000.0; - self.drib_motor.set_setpoint(drib_dc); - - // let c_vel = 0.2; - // front_right_motor.set_setpoint(c_vel); - // front_left_motor.set_setpoint(c_vel); - // back_left_motor.set_setpoint(c_vel); - // back_right_motor.set_setpoint(c_vel); - - self.front_right_motor.send_motion_command(); - self.front_left_motor.send_motion_command(); - self.back_left_motor.send_motion_command(); - self.back_right_motor.send_motion_command(); - self.drib_motor.send_motion_command(); - - Some(BasicTelemetry { - sequence_number: 0, - robot_revision_major: 0, - robot_revision_minor: 0, - battery_level: 0., - battery_temperature: 0., - _bitfield_align_1: [], - _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - ), - motor_0_temperature: 0., - motor_1_temperature: 0., - motor_2_temperature: 0., - motor_3_temperature: 0., - motor_4_temperature: 0., - kicker_charge_level: 0., - }) - } -} diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 128a1a6b..f5b8eca6 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -1,168 +1,100 @@ #![no_std] #![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -use apa102_spi::Apa102; -use ateam_control_board::stm32_interface::get_bootloader_uart_config; -use control::Control; -use defmt::info; +use embassy_executor::InterruptExecutor; use embassy_stm32::{ - dma::NoDma, - executor::InterruptExecutor, - exti::ExtiInput, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, - spi, - time::{hz, mhz}, - usart::Uart, + interrupt, pac::Interrupt }; -use embassy_time::{Duration, Ticker, Timer}; -use futures_util::StreamExt; -use panic_probe as _; -use pins::{ - PowerStateExti, PowerStatePin, - ShutdownCompletePin, -}; -use smart_leds::{SmartLedsWrite, RGB8}; -use static_cell::StaticCell; +use embassy_sync::pubsub::PubSubChannel; -mod control; -mod pins; +use defmt_rtt as _; -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); +use ateam_control_board::{create_io_task, create_shutdown_task, get_system_config, pins::{BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; -#[embassy_executor::main] -async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - let p = embassy_stm32::init(stm32_config); - - info!("system core initialized"); - - // Delay so dotstar can turn on - Timer::after(Duration::from_millis(50)).await; - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); - - let imu_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - NoDma, - hz(1_000_000), - spi::Config::default(), - ); - - let mut dotstar = Apa102::new(imu_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); - - let front_right_int = interrupt::take!(USART1); - let front_left_int = interrupt::take!(UART4); - let back_left_int = interrupt::take!(UART7); - let back_right_int = interrupt::take!(UART8); - let drib_int = interrupt::take!(UART5); - - let front_right_usart = Uart::new( - p.USART1, - p.PB15, - p.PB14, - front_right_int, - p.DMA1_CH0, - p.DMA1_CH1, - get_bootloader_uart_config(), - ); - let front_left_usart = Uart::new( - p.UART4, - p.PA1, - p.PA0, - front_left_int, - p.DMA1_CH2, - p.DMA1_CH3, - get_bootloader_uart_config(), - ); - let back_left_usart = Uart::new( - p.UART7, - p.PF6, - p.PF7, - back_left_int, - p.DMA1_CH4, - p.DMA1_CH5, - get_bootloader_uart_config(), - ); - let back_right_usart = Uart::new( - p.UART8, - p.PE0, - p.PE1, - back_right_int, - p.DMA1_CH6, - p.DMA1_CH7, - get_bootloader_uart_config(), - ); - let drib_usart = Uart::new( - p.UART5, - p.PB12, - p.PB13, - drib_int, - p.DMA2_CH2, - p.DMA2_CH3, - get_bootloader_uart_config(), - ); - - let ball_detected_thresh = 1.0; - let mut control = Control::new( - &spawner, - front_right_usart, - front_left_usart, - back_left_usart, - back_right_usart, - drib_usart, - p.PD8, - p.PC1, - p.PF8, - p.PB9, - p.PD13, - p.PD9, - p.PC0, - p.PF9, - p.PB8, - p.PD12, - ball_detected_thresh, - ); - - control.load_firmware().await; - - let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - let _ = dotstar.write([RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 0, b: 10 }].iter().cloned()); - - let _ = dotstar.write([RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }].iter().cloned()); - - let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; - loop { +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); - main_loop_rate_ticker.next().await; - } +static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); +static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); + +static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); } -#[embassy_executor::task] -async fn power_off_task( - power_state_pin: PowerStatePin, - exti: PowerStateExti, - shutdown_pin: ShutdownCompletePin, -) { - let power_state = Input::new(power_state_pin, Pull::None); - let mut shutdown = Output::new(shutdown_pin, Level::Low, Speed::Low); - let mut power_state = ExtiInput::new(power_state, exti); - power_state.wait_for_falling_edge().await; - shutdown.set_high(); - loop {} +#[interrupt] +unsafe fn CORDIC() { + RADIO_UART_QUEUE_EXECUTOR.on_interrupt(); } + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + // uart queue executor should be highest priority + // NOTE: maybe this should be all DMA tasks? No computation tasks here + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + // commands channel + let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + + // telemetry channel + let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + + // Battery Channel + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + + // TODO imu channel + let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + + /////////////////// + // start tasks // + /////////////////// + + create_io_task!(main_spawner, + robot_state, + battery_volt_publisher, + p); + + create_shutdown_task!(main_spawner, + robot_state, + p); + + start_control_task( + uart_queue_spawner, main_spawner, + robot_state, + control_command_subscriber, control_telemetry_publisher, control_gyro_data_subscriber, + p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, + p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, + p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, + p.USART1, p.PB15, p.PB14, p.DMA1_CH1, p.DMA1_CH0, p.PD8, p.PD9, + p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; + + loop { + Timer::after_millis(10).await; + } +} \ No newline at end of file diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index e194b055..6d0fa316 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -67,7 +67,7 @@ impl SharedRobotState { last_packet_receive_time_ticks_ms: 0, radio_connection_ok: false, - battery_pct: 0, + battery_pct: self.battery_pct(), battery_ok: false, robot_tipped: self.robot_tipped(), @@ -132,6 +132,13 @@ impl SharedRobotState { self.shutdown_requested.store(true, Ordering::Relaxed); } + pub fn battery_pct(&self) -> u8 { + self.battery_pct.load(Ordering::Relaxed) + } + + pub fn set_battery_pct(&self, battery_pct: u8) { + self.battery_pct.store(battery_pct, Ordering::Relaxed); + } } #[derive(Clone, Copy, PartialEq, Debug)] diff --git a/control-board/src/songs.rs b/control-board/src/songs.rs index 9c25ab16..1be5c576 100644 --- a/control-board/src/songs.rs +++ b/control-board/src/songs.rs @@ -1,20 +1,31 @@ use ateam_lib_stm32::audio::note::Beat; -pub const TIPPED_WARNING_SONG: [Beat; 14] = [ +pub const TIPPED_WARNING_SONG: [Beat; 18] = [ Beat::Note { tone: 392, duration: 125_000 }, Beat::Rest(125_000), Beat::Note { tone: 392, duration: 125_000 }, Beat::Rest(125_000), Beat::Note { tone: 392, duration: 125_000 }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { tone: 392, duration: 300_000 }, Beat::Rest(125_000), - Beat::Note { tone: 392, duration: 125_000 }, + Beat::Note { tone: 392, duration: 300_000 }, + Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 300_000 }, Beat::Rest(125_000), Beat::Note { tone: 392, duration: 125_000 }, Beat::Rest(125_000), Beat::Note { tone: 392, duration: 125_000 }, Beat::Rest(125_000), + Beat::Note { tone: 392, duration: 125_000 }, + Beat::Rest(1_000_000), +]; + +pub const BATTERY_WARNING_SONG: [Beat; 4] = [ + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Rest(250_000), + Beat::Note { tone: 392, duration: 250_000 }, + Beat::Rest(250_000), ]; pub const TEST_SONG: [Beat; 17] = [ diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index 814191ce..78a5916f 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -3,7 +3,7 @@ use embassy_executor::Spawner; use embassy_stm32::{gpio::OutputType, time::hz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}}; use embassy_time::{Duration, Ticker}; -use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::TIPPED_WARNING_SONG}; +use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::{BATTERY_WARNING_SONG, TIPPED_WARNING_SONG}}; #[macro_export] macro_rules! create_audio_task { @@ -30,6 +30,13 @@ async fn audio_task_entry( tone_player.play_song().await; } + defmt::info!("battery pct {}", cur_robot_state.battery_pct); + if cur_robot_state.battery_pct == 0 || cur_robot_state.battery_pct > 110 { + defmt::warn!("robot critical battery pct {}", cur_robot_state.battery_pct); + let _ = tone_player.load_song(&BATTERY_WARNING_SONG); + tone_player.play_song().await; + } + audio_ticker.next().await; } } diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 896d87b7..743b730a 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -61,8 +61,8 @@ async fn imu_task_entry( let gyro_config_res = imu.set_gyro_config(GyroMode::ContinuousHighPerformance, GyroRange::PlusMinus2000DegPerSec, Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr6400p0, - DataAveragingWindow::Average64Samples).await; + OutputDataRate::Odr200p0, + DataAveragingWindow::Average2Samples).await; imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2).await; if gyro_config_res.is_err() { @@ -73,8 +73,8 @@ async fn imu_task_entry( let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance, AccelRange::Range2g, Bandwidth3DbCutoffFreq::AccOdrOver2, - OutputDataRate::Odr6400p0, - DataAveragingWindow::Average64Samples).await; + OutputDataRate::Odr200p0, + DataAveragingWindow::Average2Samples).await; imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1).await; if acc_config_res.is_err() { diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 7ff4255a..bf59c848 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -173,7 +173,15 @@ async fn user_io_task_entry( let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); let batt_res_div_v = battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal); - battery_volt_publisher.publish_immediate(adc_v_to_battery_voltage(batt_res_div_v)); + let battery_voltage = adc_v_to_battery_voltage(batt_res_div_v); + // publish votlage + battery_volt_publisher.publish_immediate(battery_voltage); + // set pct in state + const LIPO_MIN_VOLTAGE: f32 = 6.0 * 3.2; + const LIPO_MAX_VOLTAGE: f32 = 6.0 * 4.2; + const LIPO_VOTLAGE_RANGE: f32 = LIPO_MAX_VOLTAGE - LIPO_MIN_VOLTAGE; + let battery_pct = (((battery_voltage - LIPO_MIN_VOLTAGE) / LIPO_VOTLAGE_RANGE) * 100.0) as u8; + robot_state.set_battery_pct(battery_pct); // TODO read messages From 179d1604bf8587ef949178f598645ba48fa97e42 Mon Sep 17 00:00:00 2001 From: "H.M. Murdock" Date: Sun, 30 Jun 2024 23:33:09 -0400 Subject: [PATCH 095/157] mostly working firmware control --- .../src/motion/params/body_vel_pid_params.rs | 6 +++--- control-board/src/motion/robot_controller.rs | 15 ++++++++++++++- control-board/src/tasks/control_task.rs | 8 +++++--- motor-controller/bin/wheel/main.c | 1 + 4 files changed, 23 insertions(+), 7 deletions(-) diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index c383e11d..535e07ad 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -4,11 +4,11 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; pub static PID_GAIN: Matrix3x5 = matrix![1.0, 0.0, 0.0, -1.0, 1.0; 1.0, 0.0, 0.0, -1.0, 1.0; - 1.0, 0.0, 0.0, -3.0, 3.0]; + 2.0, 0.5, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) -pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate -pub static BODY_ACC_LIM: Vector3 = vector![8.0, 8.0, 36.0]; // TODO calibrate/ignore +pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 22.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) // Rough estimate for peak rating diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 57757a62..5cad6906 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -195,7 +195,19 @@ impl<'a> BodyVelocityController<'a> { self.body_vel_filter.update(&measurement); // Read the current body velocity state estimate from the CGKF. - let body_vel_estimate = self.body_vel_filter.get_state(); + let mut body_vel_estimate = self.body_vel_filter.get_state(); + + // Deadzone the velocity estimate + if libm::fabsf(body_vel_estimate[0]) < 0.005 { + body_vel_estimate[0] = 0.0; + } + if libm::fabsf(body_vel_estimate[1]) < 0.005 { + body_vel_estimate[1] = 0.0; + } + if libm::fabsf(body_vel_estimate[2]) < 0.005 { + body_vel_estimate[2] = 0.0; + } + self.debug_telemetry.cgkf_body_velocity_state_estimate.copy_from_slice(body_vel_estimate.as_slice()); // Apply control policy. @@ -242,6 +254,7 @@ impl<'a> BodyVelocityController<'a> { self.cmd_wheel_velocities = wheel_vel_output; } else { self.cmd_wheel_velocities = self.robot_model.robot_vel_to_wheel_vel(&body_vel_setpoint); + self.debug_telemetry.wheel_velocity_u.copy_from_slice(self.cmd_wheel_velocities.as_slice()); } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 050a570f..68b0d163 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -128,8 +128,8 @@ impl < // TODO read from channel or something - robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); - robot_controller.get_wheel_velocities() + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); + robot_controller.get_wheel_velocities() } fn send_motor_commands_and_telemetry(&mut self, @@ -275,7 +275,7 @@ impl < // now we have setpoint r(t) in self.cmd_vel // let battery_v = battery_sub.next_message_pure().await as f32; let battery_v = 25.0; - let controls_enabled = false; + let controls_enabled = true; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { // TODO check order @@ -289,6 +289,8 @@ impl < 0.0) }; + // Scale the wheel velocities to + self.motor_fl.set_setpoint(wheel_vels[0]); self.motor_bl.set_setpoint(wheel_vels[1]); self.motor_br.set_setpoint(wheel_vels[2]); diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 71a26447..7f409095 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -344,6 +344,7 @@ int main() { } control_setpoint_vel_rads = control_setpoint_vel_rads_prev + (setpoint_accel_rads_2 * VELOCITY_LOOP_RATE_S); + control_setpoint_vel_rads_prev = control_setpoint_vel_rads; // back convert rads to duty cycle control_setpoint_vel_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); From 8b83d30f47addff6e01d923a46a3b2973e9eea27 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 30 Jun 2024 21:24:55 -0700 Subject: [PATCH 096/157] Clean up --- control-board/src/lib.rs | 5 ----- .../src/motion/constant_gain_kalman_filter.rs | 2 -- control-board/src/motion/robot_controller.rs | 21 +++---------------- control-board/src/stspin_motor.rs | 2 +- control-board/src/tasks/control_task.rs | 13 ++---------- motor-controller/common/pid.c | 3 --- software-communication | 2 +- 7 files changed, 7 insertions(+), 41 deletions(-) diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index fcd14021..cffc5169 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -13,7 +13,6 @@ #![feature(const_fn_floating_point_arithmetic)] #![feature(sync_unsafe_cell)] #![feature(inherent_associated_types)] -#![feature(const_float_bits_conv)] use embassy_stm32::{ bind_interrupts, peripherals, rcc::{ @@ -87,10 +86,6 @@ pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { (adc_v / 1.966) * BATTERY_MAX_VOLTAGE } -pub const fn abs_f32(x: f32) -> f32 { - f32::from_bits(x.to_bits() & (i32::MAX as u32)) -} - pub fn get_system_config() -> Config { let mut config = Config::default(); diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs index 34f11c20..83a7fc24 100644 --- a/control-board/src/motion/constant_gain_kalman_filter.rs +++ b/control-board/src/motion/constant_gain_kalman_filter.rs @@ -52,8 +52,6 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; - // self.estimate_cov = (SMatrix::::identity() - self.kalman_gain * self.observation_model) * self.pred_estimate_cov; - self.measurement_residual = measurement - self.observation_model * self.state_estimate; } diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 5cad6906..ebe8222e 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -172,24 +172,7 @@ impl<'a> BodyVelocityController<'a> { // these should probably be genericized/templated some how // Build measurement vector for CGKF. - let mut measurement: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); - - // Convert encoder velocity to body velocity for comparison to IMU. - let enc_body_vel = self.robot_model.wheel_vel_to_robot_vel(wheel_velocities); - - // If the encoder estimate is small enough, then replace IMU value with - // encoder value to reduce the jittery noise. - - /* - if abs_f32(enc_body_vel[0]) < 0.1 - && abs_f32(enc_body_vel[1]) < 0.1 - && abs_f32(enc_body_vel[2]) < 0.1 { - - measurement[4] = enc_body_vel[2] - } - */ - - //defmt::debug!("{}", measurement[4]); + let measurement: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); // Update measurements process observation input into CGKF. self.body_vel_filter.update(&measurement); @@ -201,9 +184,11 @@ impl<'a> BodyVelocityController<'a> { if libm::fabsf(body_vel_estimate[0]) < 0.005 { body_vel_estimate[0] = 0.0; } + if libm::fabsf(body_vel_estimate[1]) < 0.005 { body_vel_estimate[1] = 0.0; } + if libm::fabsf(body_vel_estimate[2]) < 0.005 { body_vel_estimate[2] = 0.0; } diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 7eabe72a..56e076d5 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -219,7 +219,7 @@ impl< // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); if mrp.data.motion.master_error() != 0 { - //error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); } // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 68b0d163..13486e12 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -182,7 +182,6 @@ impl < self.flash_motor_firmware( self.shared_robot_state.hw_in_debug_mode()).await; - embassy_futures::join::join5( self.motor_fl.leave_reset(), @@ -190,8 +189,7 @@ impl < self.motor_br.leave_reset(), self.motor_fr.leave_reset(), self.motor_drib.leave_reset(), - ) - .await; + ).await; self.motor_fl.set_telemetry_enabled(true); @@ -250,7 +248,6 @@ impl < ticks_since_packet = 0; }, ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { - // defmt::warn!("param updates aren't supported yet"); let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); if let Ok(resp) = param_cmd_resp { @@ -282,15 +279,9 @@ impl < self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) } else { // Battery is too low, set velocity to zero - Vector4::new( - 0.0, - 0.0, - 0.0, - 0.0) + Vector4::new(0.0, 0.0, 0.0, 0.0) }; - // Scale the wheel velocities to - self.motor_fl.set_setpoint(wheel_vels[0]); self.motor_bl.set_setpoint(wheel_vels[1]); self.motor_br.set_setpoint(wheel_vels[2]); diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c index 208e950c..82e26983 100644 --- a/motor-controller/common/pid.c +++ b/motor-controller/common/pid.c @@ -29,8 +29,6 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { float termP = err * pid->pid_constants->kP; - // float alpha = 0.37f; - // pid->eI = (pid->eI * alpha) + (err * dt * (1.0f - alpha)); pid->eI = pid->eI + (err * dt); if (fabs(r) < 3.0) { @@ -48,6 +46,5 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { pid->prev_err = err; float u = r + (termP + termI + termD); - //pid->prev_u = u; return u; } \ No newline at end of file diff --git a/software-communication b/software-communication index cb445b0c..c5fe2864 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit cb445b0c305e7033d7c05be1f2ee1695c461252e +Subproject commit c5fe28648faaeb42043e89b74e806b0db6de5858 From 02e99953764e5b1abf6dfeacd65459df5b8352a2 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Jul 2024 18:30:37 -0400 Subject: [PATCH 097/157] update and fix uart on main kicker image --- control-board/src/tasks/kicker_task.rs | 1 + kicker-board/src/bin/kicker/main.rs | 75 ++++++++++---------------- 2 files changed, 29 insertions(+), 47 deletions(-) diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 1f8bbcaf..5db11798 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -125,6 +125,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ }, KickerTaskState::WaitShutdown => { if self.kicker_driver.shutdown_completed() { + defmt::info!("kicker finished shutdown"); self.kicker_task_state = KickerTaskState::PoweredOff; } }, diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index d6ba9388..ce165636 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -4,30 +4,24 @@ #![feature(const_mut_refs)] #![feature(sync_unsafe_cell)] -use core::cell::SyncUnsafeCell; -use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::get_system_config}; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; -use static_cell::StaticCell; +use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::{get_system_config, ClkSource}}; use defmt::*; use {defmt_rtt as _, panic_probe as _}; -use cortex_m_rt::entry; - use libm::{fmaxf, fminf}; -use embassy_executor::{Executor, InterruptExecutor}; +use embassy_executor::{InterruptExecutor, Spawner}; use embassy_stm32::{ adc::{Adc, SampleTime}, bind_interrupts, gpio::{Level, Output, Speed}, interrupt, interrupt::InterruptExt, - pac::Interrupt, - peripherals, - usart::{self, Config, Parity, StopBits, Uart}, + pac::Interrupt,peripherals, + usart::{self, Config, Parity, StopBits, Uart} }; -use embassy_time::{Duration, Instant, Ticker}; +use embassy_time::{Duration, Instant, Ticker, Timer}; use ateam_kicker_board::{ adc_raw_to_v, adc_200v_to_rail_voltage, @@ -38,7 +32,7 @@ use ateam_kicker_board::{ }, }; -use ateam_lib_stm32::queue::Buffer; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use ateam_common_packets::bindings_kicker::{ @@ -46,11 +40,6 @@ use ateam_common_packets::bindings_kicker::{ KickerControl, KickerTelemetry, }; -const MAX_TX_PACKET_SIZE: usize = 16; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 16; -const RX_BUF_DEPTH: usize = 3; - const MAX_KICK_SPEED: f32 = 5.5; const SHUTDOWN_KICK_DUTY: f32 = 0.20; @@ -59,22 +48,16 @@ pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; -// control communications tx buffer -const COMS_BUFFER_MUTEX: Mutex = Mutex::new(false); -const COMS_BUFFER_VAL_TX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -static COMS_BUFFERS_TX: [SyncUnsafeCell>; TX_BUF_DEPTH] = - [COMS_BUFFER_VAL_TX; TX_BUF_DEPTH]; -static COMS_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(&COMS_BUFFERS_TX, COMS_BUFFER_MUTEX); - -// control communications rx buffer -const COMS_BUFFER_VAL_RX: SyncUnsafeCell> = - SyncUnsafeCell::new(Buffer::EMPTY); -static COMS_BUFFERS_RX: [SyncUnsafeCell>; RX_BUF_DEPTH] = - [COMS_BUFFER_VAL_RX; RX_BUF_DEPTH]; -static COMS_QUEUE_RX: UartReadQueue = - UartReadQueue::new(&COMS_BUFFERS_RX, COMS_BUFFER_MUTEX); +const MAX_TX_PACKET_SIZE: usize = 16; +const TX_BUF_DEPTH: usize = 3; +const MAX_RX_PACKET_SIZE: usize = 16; +const RX_BUF_DEPTH: usize = 3; + +make_uart_queue_pair!(COMS, + ComsUartModule, ComsUartRxDma, ComsUartTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".bss"]); fn get_empty_control_packet() -> KickerControl { KickerControl { @@ -389,7 +372,6 @@ async fn high_pri_kick_task( } static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); -static EXECUTOR_LOW: StaticCell = StaticCell::new(); #[interrupt] unsafe fn TIM2() { @@ -400,10 +382,11 @@ bind_interrupts!(struct Irqs { USART1 => usart::InterruptHandler; }); -#[entry] -fn main() -> ! { - let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_cfg); + +#[embassy_executor::main] +async fn main(spawner: Spawner) -> ! { + let stm32_config = get_system_config(ClkSource::InternalOscillator); + let p = embassy_stm32::init(stm32_config); info!("kicker startup!"); @@ -417,8 +400,9 @@ fn main() -> ! { // High-priority executor: I2C1, priority level 6 // TODO CHECK THIS IS THE HIGHEST PRIORITY embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); - let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(spawner.spawn(high_pri_kick_task(&COMS_QUEUE_RX, &COMS_QUEUE_TX, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); + let hp_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); + + unwrap!(hp_spawner.spawn(high_pri_kick_task(&COMS_RX_UART_QUEUE, &COMS_TX_UART_QUEUE, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// @@ -441,12 +425,9 @@ fn main() -> ! { ).unwrap(); let (coms_uart_tx, coms_uart_rx) = Uart::split(coms_usart); + queue_pair_register_and_spawn!(spawner, COMS, coms_uart_rx, coms_uart_tx); - // low priority executor handles coms and user IO - // Low priority executor: runs in thread mode, using WFE/SEV - let lp_executor = EXECUTOR_LOW.init(Executor::new()); - lp_executor.run(|spawner| { - unwrap!(spawner.spawn(COMS_QUEUE_TX.spawn_task(coms_uart_tx))); - unwrap!(spawner.spawn(COMS_QUEUE_RX.spawn_task(coms_uart_rx))); - }); + loop { + Timer::after_millis(1000).await; + } } From c26e81c0c4a182e186b9d3a697580f383d5b18f1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Jul 2024 20:38:30 -0400 Subject: [PATCH 098/157] add radio timeout --- control-board/src/bin/control/main.rs | 1 - control-board/src/bin/hwtest-kicker/main.rs | 193 ++++++-------------- control-board/src/tasks/radio_task.rs | 164 ++--------------- 3 files changed, 70 insertions(+), 288 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index e8aec7cb..4ff41e54 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -80,7 +80,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { // Battery Channel let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); - let mut battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); // TODO imu channel let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index 719faef9..d6b16728 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -2,128 +2,99 @@ #![no_main] #![feature(type_alias_impl_trait)] #![feature(const_mut_refs)] +#![feature(sync_unsafe_cell)] + -use apa102_spi::Apa102; use ateam_control_board::{ - drivers::kicker::Kicker, - include_kicker_bin, - queue::Buffer, - stm32_interface::{get_bootloader_uart_config, Stm32Interface}, - uart_queue::{UartReadQueue, UartWriteQueue}, + drivers::kicker::Kicker, get_system_config, include_kicker_bin, pins::{KickerRxDma, KickerTxDma, KickerUart}, stm32_interface::{self, Stm32Interface}, }; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use defmt::info; +use embassy_executor::InterruptExecutor; use embassy_stm32::{ - dma::NoDma, - executor::InterruptExecutor, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt::{self, InterruptExt}, - peripherals::{DMA2_CH4, DMA2_CH5, USART6}, - spi, - time::{hz, mhz}, - usart::Uart, + gpio::{Level, Output, Speed}, interrupt, pac::Interrupt, usart::Uart }; use embassy_time::{Duration, Ticker, Timer}; -use futures_util::StreamExt; use panic_probe as _; -use smart_leds::{SmartLedsWrite, RGB8}; -use static_cell::StaticCell; - -use ateam_common_packets::bindings_kicker::KickRequest; -include_kicker_bin! {KICKER_FW_IMG, "kicker.bin"} +include_kicker_bin! {KICKER_FW_IMG, "hwtest-kick.bin"} const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 20; -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_TX: [Buffer; TX_BUF_DEPTH] = - [Buffer::EMPTY; TX_BUF_DEPTH]; -static KICKER_QUEUE_TX: UartWriteQueue = - UartWriteQueue::new(unsafe { &mut KICKER_BUFFERS_TX }); +make_uart_queue_pair!(KICKER, + KickerUart, KickerRxDma, KickerTxDma, + MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, + MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, + #[link_section = ".axisram.buffers"]); -#[link_section = ".axisram.buffers"] -static mut KICKER_BUFFERS_RX: [Buffer; RX_BUF_DEPTH] = - [Buffer::EMPTY; RX_BUF_DEPTH]; -static KICKER_QUEUE_RX: UartReadQueue = - UartReadQueue::new(unsafe { &mut KICKER_BUFFERS_RX }); -static EXECUTOR_UART_QUEUE: StaticCell> = StaticCell::new(); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} #[embassy_executor::main] async fn main(_spawner: embassy_executor::Spawner) { - let mut stm32_config: embassy_stm32::Config = Default::default(); - stm32_config.rcc.hse = Some(mhz(8)); - stm32_config.rcc.sys_ck = Some(mhz(400)); - stm32_config.rcc.hclk = Some(mhz(200)); - stm32_config.rcc.pclk1 = Some(mhz(100)); - + let stm32_config = get_system_config(); let p = embassy_stm32::init(stm32_config); - // Delay so dotstar can turn on - Timer::after(Duration::from_millis(50)).await; - - let irq = interrupt::take!(CEC); - irq.set_priority(interrupt::Priority::P6); - let executor = EXECUTOR_UART_QUEUE.init(InterruptExecutor::new(irq)); - let spawner = executor.start(); + defmt::info!("Kicker system init"); - let dotstar_spi_config = spi::Config::default(); - dotstar_spi_config.frequency = mhz(1); + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P5); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); - let dotstar_spi = spi::Spi::new_txonly( - p.SPI3, - p.PB3, - p.PB5, - NoDma, - dotstar_spi_config, - ); - - let mut dotstar = Apa102::new(dotstar_spi); - let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned()); + // let kicker_det = Input::new(p.PG8, Pull::Up); + // if kicker_det.is_high() { + // defmt::warn!("kicker appears unplugged!"); + // } - info!("booted"); + let mut kicker_pwr_pin = Output::new(p.PG8, Level::Low, Speed::Medium); - let btn0 = Input::new(p.PD5, Pull::None); - let btn1 = Input::new(p.PD6, Pull::None); + kicker_pwr_pin.set_high(); + defmt::info!("force power off kicker"); + Timer::after_millis(2000).await; + kicker_pwr_pin.set_low(); + defmt::info!("kicker force power off done"); - let mut led0 = Output::new(p.PF3, Level::Low, Speed::Low); - let mut led1 = Output::new(p.PF2, Level::Low, Speed::Low); - let mut led2 = Output::new(p.PF1, Level::Low, Speed::Low); - let mut led3 = Output::new(p.PF0, Level::Low, Speed::Low); + defmt::info!("attempting to power on kicker."); + Timer::after_millis(1000).await; + kicker_pwr_pin.set_high(); + Timer::after_millis(200).await; + kicker_pwr_pin.set_low(); + defmt::info!("power on attempt done"); - let kicker_det = Input::new(p.PG8, Pull::Up); - if kicker_det.is_high() { - defmt::warn!("kicker appears unplugged!"); - } + // loop { + // Timer::after_millis(1000).await; + // } - let kicker_int = interrupt::take!(USART6); let kicker_usart = Uart::new( p.USART6, p.PC7, p.PC6, - kicker_int, + ateam_control_board::SystemIrqs, p.DMA2_CH4, p.DMA2_CH5, - get_bootloader_uart_config(), - ); - let (kicker_tx, kicker_rx) = kicker_usart.split(); - let kicker_boot0_pin = Output::new(p.PA8, Level::Low, Speed::Medium); - let kicker_reset_pin = Output::new(p.PA9, Level::Low, Speed::Medium); - - spawner - .spawn(KICKER_QUEUE_RX.spawn_task(kicker_rx)) - .unwrap(); - spawner - .spawn(KICKER_QUEUE_TX.spawn_task(kicker_tx)) - .unwrap(); - - let kicker_stm32_interface = Stm32Interface::new_noninverted_reset( - &KICKER_QUEUE_RX, - &KICKER_QUEUE_TX, - Some(kicker_boot0_pin), - Some(kicker_reset_pin), + stm32_interface::get_bootloader_uart_config(), + ).unwrap(); + + defmt::info!("init uart"); + + let (kicker_tx, kicker_rx) = Uart::split(kicker_usart); + queue_pair_register_and_spawn!(uart_queue_spawner, KICKER, kicker_rx, kicker_tx); + + defmt::info!("start qs"); + + let kicker_stm32_interface = Stm32Interface::new_from_pins( + &KICKER_RX_UART_QUEUE, + &KICKER_TX_UART_QUEUE, + p.PA8, + p.PA9, + true ); info!("flashing kicker..."); @@ -140,59 +111,13 @@ async fn main(_spawner: embassy_executor::Spawner) { } kicker.set_telemetry_enabled(true); - kicker.set_kick_strength(2.25); - - let _ = dotstar.write( - [RGB8 { r: 0, g: 10, b: 0 }, RGB8 { r: 0, g: 10, b: 0 }] - .iter() - .cloned(), - ); let mut main_loop_rate_ticker = Ticker::every(Duration::from_millis(10)); loop { kicker.process_telemetry(); // TODO print some telemetry or something - defmt::info!( - "high voltage: {}, battery voltage: {}", - kicker.hv_rail_voltage(), - kicker.battery_voltage() - ); - - if btn0.is_low() { - kicker.request_kick(KickRequest::KR_ARM); - - led0.set_high(); - } else { - kicker.request_kick(KickRequest::KR_DISABLE); - - led0.set_low(); - } - - if btn1.is_low() { - kicker.request_kick(KickRequest::KR_KICK_NOW); - - led1.set_high(); - } else { - led1.set_low(); - } - - if btn0.is_low() && btn1.is_low() { - kicker.request_kick(KickRequest::KR_DISABLE); - kicker.request_shutdown(); - } - - if kicker.shutdown_acknowledge() { - led2.set_high(); - } else { - led2.set_low(); - } - - if kicker.shutdown_completed() { - led3.set_high(); - } else { - led2.set_low(); - } + defmt::info!("high voltage: {}, battery voltage: {}", kicker.hv_rail_voltage(), kicker.battery_voltage()); kicker.send_command(); diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 606c8b76..54580815 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,5 +1,5 @@ -use ateam_common_packets::{bindings_radio::{BasicControl, BasicTelemetry}, radio::{DataPacket, TelemetryPacket}}; +use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; @@ -8,8 +8,7 @@ use embassy_stm32::{ gpio::{Input, Pin, Pull}, usart::{self, DataBits, Parity, StopBits, Uart} }; -use embassy_sync::pubsub::WaitResult; -use embassy_time::{Duration, Ticker, Timer}; +use embassy_time::{Duration, Instant, Ticker, Timer}; use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state::SharedRobotState, SystemIrqs}; @@ -78,6 +77,7 @@ pub struct RadioTask< connection_state: RadioConnectionState, wifi_credentials: &'static [WifiCredential], + last_software_packet: Instant, last_basic_telemetry: BasicTelemetry, } @@ -109,6 +109,7 @@ impl< radio_ndet_input: radio_ndet_input, connection_state: RadioConnectionState::Unconnected, wifi_credentials: wifi_credentials, + last_software_packet: Instant::now(), last_basic_telemetry: BasicTelemetry { sequence_number: 0, robot_revision_major: 0, @@ -253,6 +254,11 @@ impl< RadioConnectionState::Connected => { let _ = self.process_packets().await; // if we're stably connected, process packets at 100Hz + + // if 1000ms have elapsed since we last got a packet, return to software connection state + if Instant::now() - self.last_software_packet > Duration::from_millis(1000) { + self.connection_state = RadioConnectionState::ConnectSoftware; + } }, } @@ -336,6 +342,8 @@ impl< loop { if let Ok(pkt) = self.radio.read_packet_nonblocking() { if let Some(c2_pkt) = pkt { + // update the last packet timestamp + self.last_software_packet = Instant::now(); self.command_publisher.publish_immediate(c2_pkt); } else { break; @@ -390,154 +398,6 @@ async fn radio_task_entry(mut radio_task: RadioTask = - RobotRadio::new(&RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin); - - let mut startup_radio_uart_config = usart::Config::default(); - startup_radio_uart_config.baudrate = 115_200; - startup_radio_uart_config.data_bits = DataBits::DataBits8; - startup_radio_uart_config.stop_bits = StopBits::STOP1; - startup_radio_uart_config.parity = Parity::ParityNone; - - #[allow(unused_labels)] - 'connect_loop: - loop { - if radio_ndet.is_high() { - defmt::error!("radio appears unplugged."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } - - if RADIO_TX_UART_QUEUE.update_uart_config(startup_radio_uart_config).await.is_err() { - defmt::error!("failed to reset radio config."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } - - defmt::info!("connecting radio uart"); - match select(radio.connect_uart(), Timer::after_millis(10000)).await { - Either::First(res) => { - if res.is_err() { - defmt::error!("failed to establish radio UART connection."); - Timer::after_millis(1000).await; - continue 'connect_loop; - } else { - defmt::debug!("established radio uart coms"); - } - } - Either::Second(_) => { - defmt::error!("initial radio uart connection timed out"); - continue 'connect_loop; - } - } - - while radio.connect_to_network(wifi_network, robot_state.get_hw_robot_id()).await.is_err() { - defmt::error!("failed to connect to wifi network."); - Timer::after_millis(1000).await; - } - defmt::info!("radio connected"); - - let res = radio.open_multicast().await; - if res.is_err() { - defmt::error!("failed to establish multicast socket to network."); - continue 'connect_loop; - } - defmt::info!("multicast open"); - - 'connect_hello: - loop { - defmt::info!("sending hello"); - - let robot_id = robot_state.get_hw_robot_id(); - let team_color = if robot_state.hw_robot_team_is_blue() { - TeamColor::Blue - } else { - TeamColor::Yellow - }; - radio.send_hello(robot_id, team_color).await.unwrap(); - let hello = radio.wait_hello(Duration::from_millis(1000)).await; - - match hello { - Ok(hello) => { - defmt::info!( - "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port - ); - radio.close_peer().await.unwrap(); - defmt::info!("multicast peer closed"); - - radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); - defmt::info!("unicast open"); - - break 'connect_hello; - } - Err(_) => {} - } - } - - // TODO add inbound timeout somewhere, maybe not here. - 'process_packets: - loop { - match select(radio.read_packet(), telemetry_subscriber.next_message()).await { - Either::First(res) => { - if let Ok(c2_pkt) = res { - command_publisher.publish_immediate(c2_pkt); - } else { - defmt::warn!("radio read packet returned an error"); - } - } - Either::Second(telem_msg) => { - match telem_msg { - WaitResult::Lagged(num_missed_pkts) => { - defmt::warn!("radio task missed sending {} outbound packets. Should channel have higher capacity?", num_missed_pkts); - }, - WaitResult::Message(msg) => { - match msg { - TelemetryPacket::Basic(basic_telem_pkt) => { - let res = radio.send_telemetry(basic_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send basic telemetry packet. Is the backing queue too small?"); - } - } - TelemetryPacket::Control(control_telem_pkt) => { - let res = radio.send_control_debug_telemetry(control_telem_pkt).await; - if res.is_err() { - defmt::warn!("radio task failed to send control debug telemetry packet. Is the backing queue too small?"); - } - } - TelemetryPacket::ParameterCommandResponse(param_resp) => { - let res = radio.send_parameter_response(param_resp).await; - if res.is_err() { - defmt::warn!("radio task failed to send param resp packet. Is the backing queue too small?") - } - } - } - } - } - } - } - - if radio_ndet.is_high() { - defmt::error!("radio was unplugged."); - break 'process_packets; - } - } - } -} - pub async fn start_radio_task(radio_task_spawner: Spawner, queue_spawner: SendSpawner, robot_state: &'static SharedRobotState, @@ -565,6 +425,4 @@ pub async fn start_radio_task(radio_task_spawner: Spawner, let radio_task = RadioTask::new_from_pins(robot_state, command_publisher, telemetry_subscriber, &RADIO_RX_UART_QUEUE, &RADIO_TX_UART_QUEUE, radio_reset_pin, radio_ndet_pin, wifi_credentials); radio_task_spawner.spawn(radio_task_entry(radio_task)).unwrap(); - - // radio_task_spawner.spawn(radio_task_entry(robot_state, command_publisher, telemetry_subscriber, wifi_network, radio_reset_pin, radio_ndet_pin)).unwrap(); } \ No newline at end of file From af5370db593ef158aea0a07c6366c0b6e7d49f8d Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Jul 2024 20:43:33 -0400 Subject: [PATCH 099/157] close peer when falling back to hello messages --- control-board/src/tasks/radio_task.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 54580815..63955a3c 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -258,6 +258,7 @@ impl< // if 1000ms have elapsed since we last got a packet, return to software connection state if Instant::now() - self.last_software_packet > Duration::from_millis(1000) { self.connection_state = RadioConnectionState::ConnectSoftware; + self.radio.close_peer().await.unwrap(); } }, } From 43356330ce9e88c4e70351097aae4491fae993c7 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 1 Jul 2024 20:53:36 -0400 Subject: [PATCH 100/157] add connection flag to shared state --- control-board/src/robot_state.rs | 8 ++++++++ control-board/src/tasks/radio_task.rs | 3 +++ 2 files changed, 11 insertions(+) diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 6d0fa316..afffe709 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -139,6 +139,14 @@ impl SharedRobotState { pub fn set_battery_pct(&self, battery_pct: u8) { self.battery_pct.store(battery_pct, Ordering::Relaxed); } + + pub fn radio_connection_ok(&self) -> bool { + self.radio_connection_ok.load(Ordering::Relaxed) + } + + pub fn set_radio_connection_ok(&self, conn_ok: bool) { + self.radio_connection_ok.store(conn_ok, Ordering::Relaxed); + } } #[derive(Clone, Copy, PartialEq, Debug)] diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 63955a3c..6af18722 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -263,6 +263,9 @@ impl< }, } + // set global radio connected flag + self.shared_robot_state.set_radio_connection_ok(self.connection_state == RadioConnectionState::Connected); + radio_loop_rate_ticker.next().await; } From d331d49a49530da2cc9fd14698cc3819cc90d569 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Jul 2024 21:53:33 -0400 Subject: [PATCH 101/157] Putting motor error checking back in --- control-board/src/stspin_motor.rs | 8 ++++++++ control-board/src/tasks/control_task.rs | 13 ++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 554e19c9..3b8546b1 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -311,6 +311,10 @@ impl< return self.current_state.current_estimate; } + pub fn check_hall_error(&self) -> bool { + return self.current_state.hall_power_error() != 0 || self.current_state.hall_disconnected_error() != 0 || self.current_state.hall_enc_vel_disagreement_error() != 0; + } + pub fn read_encoder_delta(&self) -> i32 { return self.current_state.encoder_delta; } @@ -621,6 +625,10 @@ impl< return self.current_state.master_error() != 0; } + pub fn check_hall_error(&self) -> bool { + return self.current_state.hall_power_error() != 0 || self.current_state.hall_disconnected_error() != 0 || self.current_state.hall_enc_vel_disagreement_error() != 0; + } + pub fn read_current(&self) -> f32 { return self.current_state.current_estimate; } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index a7adae32..d0a1bd3a 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -143,6 +143,17 @@ impl < self.motor_br.send_motion_command(); self.motor_drib.send_motion_command(); + let err_fr = self.motor_fr.read_is_error() as u32; + let err_fl = self.motor_fl.read_is_error() as u32; + let err_br = self.motor_br.read_is_error() as u32; + let err_bl = self.motor_bl.read_is_error() as u32; + let err_drib = self.motor_drib.read_is_error() as u32; + + let hall_err_fr = self.motor_fr.check_hall_error() as u32; + let hall_err_fl = self.motor_fl.check_hall_error() as u32; + let hall_err_br = self.motor_br.check_hall_error() as u32; + let hall_err_bl = self.motor_bl.check_hall_error() as u32; + let hall_err_drib = self.motor_drib.check_hall_error() as u32; let basic_telem = TelemetryPacket::Basic(BasicTelemetry { sequence_number: 0, @@ -152,7 +163,7 @@ impl < battery_temperature: 0., _bitfield_align_1: [], _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, err_fr, hall_err_fr, err_fl, hall_err_fl, err_br, hall_err_br, err_bl, hall_err_bl, err_drib, hall_err_drib, 0, 0, 0, ), motor_0_temperature: 0., motor_1_temperature: 0., From 3c4094519c5568066766b1cbea4a7d2f7c2fd68e Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Jul 2024 22:48:33 -0400 Subject: [PATCH 102/157] Fix motor order --- control-board/src/tasks/control_task.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index d0a1bd3a..8a5edfd7 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -163,7 +163,7 @@ impl < battery_temperature: 0., _bitfield_align_1: [], _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, err_fr, hall_err_fr, err_fl, hall_err_fl, err_br, hall_err_br, err_bl, hall_err_bl, err_drib, hall_err_drib, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, err_fl, hall_err_fl, err_bl, hall_err_bl, err_br, hall_err_br, err_fr, hall_err_fr, err_drib, hall_err_drib, 0, 0, 0, ), motor_0_temperature: 0., motor_1_temperature: 0., From b892eb27d4cb92dfa5dcef313cb16598bafd89b8 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 2 Jul 2024 23:08:49 -0400 Subject: [PATCH 103/157] Kick test with multiple durations --- kicker-board/src/bin/hwtest-kickstr/main.rs | 140 ++++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 kicker-board/src/bin/hwtest-kickstr/main.rs diff --git a/kicker-board/src/bin/hwtest-kickstr/main.rs b/kicker-board/src/bin/hwtest-kickstr/main.rs new file mode 100644 index 00000000..30445e33 --- /dev/null +++ b/kicker-board/src/bin/hwtest-kickstr/main.rs @@ -0,0 +1,140 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::*; +use {defmt_rtt as _, panic_probe as _}; + +use cortex_m_rt::entry; + +use embassy_executor::Executor; +use embassy_stm32::{ + adc::{Adc, SampleTime}, + gpio::{Input, Level, Output, Pull, Speed}, +}; +use embassy_time::{Duration, Timer, Ticker}; + +use static_cell::StaticCell; + +use ateam_kicker_board::*; +use ateam_kicker_board::pins::*; + +#[embassy_executor::task] +async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, + mut hv_pin: PowerRail200vReadPin, + mut rail_12v0_pin: PowerRail12vReadPin, + reg_charge: ChargePin, + status_led_red: RedStatusLedPin, + status_led_green: GreenStatusLedPin, + usr_btn_pin: UserBtnPin, + kick_pin: KickPin) -> ! { + + let mut ticker = Ticker::every(Duration::from_millis(1)); + + let mut kick = Output::new(kick_pin, Level::Low, Speed::Medium); + + let mut reg_charge = Output::new(reg_charge, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(status_led_green, Level::Low, Speed::Medium); + let mut status_led_red = Output::new(status_led_red, Level::Low, Speed::Medium); + + let usr_btn = Input::new(usr_btn_pin, Pull::None); + + status_led_green.set_high(); + Timer::after(Duration::from_millis(500)).await; + status_led_green.set_low(); + Timer::after(Duration::from_millis(500)).await; + + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + let mut hv = adc.read(&mut hv_pin) as f32; + let mut regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, 12v reg mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + + let start_up_battery_voltage = adc_v_to_battery_voltage(adc_raw_to_v(regv, vrefint_sample)); + if start_up_battery_voltage < 11.5 { + status_led_red.set_high(); + warn!("regulator voltage is below 18.0 ({}), is the battery low or disconnected?", start_up_battery_voltage); + warn!("refusing to continue"); + loop { + reg_charge.set_low(); + + kick.set_high(); + Timer::after(Duration::from_micros(500)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; + } + } + + // in us + let durations = [500, 1000, 2000, 4000]; + + for d in durations { + 'outer: while usr_btn.is_low() { + while usr_btn.is_high() { + defmt::info!("btn pressed! - initiating kick for {} us", d); + break 'outer; + } + } + + Timer::after(Duration::from_millis(1000)).await; + + // Can't charge and kick at the same time + // Fully charges within ~400 ms + reg_charge.set_high(); + Timer::after(Duration::from_millis(450)).await; + reg_charge.set_low(); + + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + + Timer::after(Duration::from_millis(1000)).await; + + // High = discharge into the solenoid + kick.set_high(); + Timer::after(Duration::from_micros(d)).await; + kick.set_low(); + + Timer::after(Duration::from_millis(1000)).await; + + } + + loop { + let mut vrefint = adc.enable_vrefint(); + let vrefint_sample = adc.read(&mut vrefint) as f32; + + hv = adc.read(&mut hv_pin) as f32; + regv = adc.read(&mut rail_12v0_pin) as f32; + + info!("hv V: {}, batt mv: {}", adc_200v_to_rail_voltage(adc_raw_to_v(hv, vrefint_sample)), adc_12v_to_rail_voltage(adc_raw_to_v(regv, vrefint_sample))); + + reg_charge.set_low(); + kick.set_low(); + + ticker.next().await; + } +} + +static EXECUTOR_LOW: StaticCell = StaticCell::new(); + +#[entry] +fn main() -> ! { + let p = embassy_stm32::init(Default::default()); + + info!("kicker startup!"); + + let mut adc = Adc::new(p.ADC1); + adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); + adc.set_sample_time(SampleTime::CYCLES480); + + // Low priority executor: runs in thread mode, using WFE/SEV + let executor = EXECUTOR_LOW.init(Executor::new()); + executor.run(|spawner| { + unwrap!(spawner.spawn(run_kick(adc, p.PC0, p.PC1, p.PE4, p.PE1, p.PE0, p.PD4, p.PE5))); + }); +} \ No newline at end of file From 46b2697d72ac2b90587683e628bca215bab11837 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Wed, 3 Jul 2024 21:49:25 -0400 Subject: [PATCH 104/157] Add comments for future reference --- kicker-board/src/bin/hwtest-kickstr/main.rs | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/kicker-board/src/bin/hwtest-kickstr/main.rs b/kicker-board/src/bin/hwtest-kickstr/main.rs index 30445e33..dd621dd8 100644 --- a/kicker-board/src/bin/hwtest-kickstr/main.rs +++ b/kicker-board/src/bin/hwtest-kickstr/main.rs @@ -1,3 +1,11 @@ +// Test script for different kicker charge times +// Change the values in the `durations` array to the +// number of microseconds you'd like to charge for each +// trial. +// Push the button to charge and then kick. Afterwards, +// the bot will wait for the next button press for the +// current duration. + #![no_std] #![no_main] #![feature(type_alias_impl_trait)] @@ -70,6 +78,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, // in us let durations = [500, 1000, 2000, 4000]; + // For each duration, wait for button, charge, then kick for d in durations { 'outer: while usr_btn.is_low() { while usr_btn.is_high() { @@ -80,8 +89,8 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, Timer::after(Duration::from_millis(1000)).await; - // Can't charge and kick at the same time - // Fully charges within ~400 ms + // We can't charge and kick at the same time... + // The kicker fully charges within ~400 ms reg_charge.set_high(); Timer::after(Duration::from_millis(450)).await; reg_charge.set_low(); @@ -98,6 +107,7 @@ async fn run_kick(mut adc: Adc<'static, PowerRailAdc>, // High = discharge into the solenoid kick.set_high(); Timer::after(Duration::from_micros(d)).await; + // Low = don't discharge into solenoid kick.set_low(); Timer::after(Duration::from_millis(1000)).await; From 7b95c5a6f3569453d15969bced89d16145a152ea Mon Sep 17 00:00:00 2001 From: "H.M. Murdock" Date: Wed, 3 Jul 2024 22:10:38 -0400 Subject: [PATCH 105/157] working controls stuff --- control-board/src/motion/params/body_vel_pid_params.rs | 2 +- control-board/src/motion/robot_controller.rs | 1 - control-board/src/tasks/imu_task.rs | 4 ---- control-board/src/tasks/radio_task.rs | 6 ++++-- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 535e07ad..02339056 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -7,7 +7,7 @@ pub static PID_GAIN: Matrix3x5 = 2.0, 0.5, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) -pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 22.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +pub static BODY_VEL_LIM: Vector3 = vector![5.0, 3.0, 22.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index ebe8222e..e60f7823 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -2,7 +2,6 @@ use ateam_common_packets::bindings_radio::{ParameterCommandCode::*, ParameterNam use ateam_common_packets::bindings_radio::ParameterDataFormat::{PID_LIMITED_INTEGRAL_F32, VEC3_F32, VEC4_F32}; use ateam_common_packets::bindings_radio::ParameterName::{VEL_PID_X, RC_BODY_VEL_LIMIT, RC_BODY_ACC_LIMIT, VEL_PID_Y, ANGULAR_VEL_PID_Z, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_PROCESS_NOISE, VEL_CGKF_GYRO_NOISE, VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_K_MATRIX, RC_WHEEL_ACC_LIMIT}; use nalgebra::{SVector, Vector3, Vector4, Vector5}; -use crate::abs_f32; use crate::parameter_interface::ParameterInterface; use super::constant_gain_kalman_filter::CgKalmanFilter; diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 9227da22..d3432599 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -74,11 +74,7 @@ async fn imu_task_entry( let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance, AccelRange::Range2g, Bandwidth3DbCutoffFreq::AccOdrOver2, -<<<<<<< HEAD OutputDataRate::Odr100p0, -======= - OutputDataRate::Odr200p0, ->>>>>>> origin/support/generation2 DataAveragingWindow::Average2Samples).await; imu.set_accel_interrupt_mode(InterruptMode::MappedToInt1).await; diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 06e472eb..f22c61a9 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -240,6 +240,7 @@ impl< if let Ok(connected) = self.connect_software(cur_robot_state.hw_robot_id, cur_robot_state.hw_robot_team_is_blue).await { if connected { self.connection_state = RadioConnectionState::Connected; + self.last_software_packet = Instant::now(); } else { // software didn't respond to our hello, it may not be started yet Timer::after_millis(1000).await; @@ -257,8 +258,9 @@ impl< // if 1000ms have elapsed since we last got a packet, return to software connection state if Instant::now() - self.last_software_packet > Duration::from_millis(1000) { - self.connection_state = RadioConnectionState::ConnectSoftware; - self.radio.close_peer().await.unwrap(); + //self.connection_state = RadioConnectionState::ConnectSoftware; + //self.radio.close_peer().await.unwrap(); + cortex_m::peripheral::SCB::sys_reset(); } }, } From 21390bb43ca2fed01a76d677f304e0730e0c1fc4 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Wed, 3 Jul 2024 23:06:10 -0400 Subject: [PATCH 106/157] Linear mapping, brought to you by me and chatgpt --- lib-stm32/src/math/linear.rs | 30 ++++++++++++++++++++++++++++++ lib-stm32/src/math/mod.rs | 3 ++- 2 files changed, 32 insertions(+), 1 deletion(-) create mode 100644 lib-stm32/src/math/linear.rs diff --git a/lib-stm32/src/math/linear.rs b/lib-stm32/src/math/linear.rs new file mode 100644 index 00000000..cf7778bd --- /dev/null +++ b/lib-stm32/src/math/linear.rs @@ -0,0 +1,30 @@ +use num_traits::{FromPrimitive, ToPrimitive}; +use core::ops::{Add, Sub, Div, Mul}; + +pub trait BoundNumeric: Copy + Sub + Add + + Div + Mul + FromPrimitive + ToPrimitive {} + +// Take input max and/or min (bounds), output value within new bounds +struct Bounds +where + T: BoundNumeric +{ + min: T, + max: T, +} + +impl Bounds +where + T: BoundNumeric +{ + fn new(min: T, max: T) -> Self { + Bounds { min, max } + } + + fn linear_map_to_new_bounds(&self, val: T, new_bounds: Bounds) -> T { + let min_diff = self.min + new_bounds.min; + let scale = (new_bounds.max - new_bounds.min) / (self.max - self.min); + let mapped_val = (val - min_diff) * scale; + return mapped_val; + } +} \ No newline at end of file diff --git a/lib-stm32/src/math/mod.rs b/lib-stm32/src/math/mod.rs index 8eb51f5c..607812d0 100644 --- a/lib-stm32/src/math/mod.rs +++ b/lib-stm32/src/math/mod.rs @@ -1 +1,2 @@ -pub mod lerp; \ No newline at end of file +pub mod lerp; +pub mod linear; \ No newline at end of file From 7bbf6f76a817996a9631f078adf64af4e6b2ec75 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Wed, 3 Jul 2024 23:24:25 -0400 Subject: [PATCH 107/157] Better names --- lib-stm32/src/math/linear.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/lib-stm32/src/math/linear.rs b/lib-stm32/src/math/linear.rs index cf7778bd..f1a223a4 100644 --- a/lib-stm32/src/math/linear.rs +++ b/lib-stm32/src/math/linear.rs @@ -1,27 +1,27 @@ use num_traits::{FromPrimitive, ToPrimitive}; use core::ops::{Add, Sub, Div, Mul}; -pub trait BoundNumeric: Copy + Sub + Add + +pub trait MappingNumeric: Copy + Sub + Add + Div + Mul + FromPrimitive + ToPrimitive {} // Take input max and/or min (bounds), output value within new bounds -struct Bounds +struct LinearMap where - T: BoundNumeric + T: MappingNumeric { min: T, max: T, } -impl Bounds +impl LinearMap where - T: BoundNumeric + T: MappingNumeric { fn new(min: T, max: T) -> Self { - Bounds { min, max } + LinearMap { min, max } } - fn linear_map_to_new_bounds(&self, val: T, new_bounds: Bounds) -> T { + fn linear_map_to_new_bounds(&self, val: T, new_bounds: LinearMap) -> T { let min_diff = self.min + new_bounds.min; let scale = (new_bounds.max - new_bounds.min) / (self.max - self.min); let mapped_val = (val - min_diff) * scale; From 0b9c9b1009827b6a755f77496c9cc0e19cc08921 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 3 Jul 2024 23:43:43 -0400 Subject: [PATCH 108/157] integrated firmware, shutdown behaviors --- control-board/.cargo/config.toml | 2 +- control-board/src/bin/control/main.rs | 11 +++++- control-board/src/pins.rs | 6 ++-- control-board/src/robot_state.rs | 25 +++++++++++++ control-board/src/tasks/control_task.rs | 6 ++-- control-board/src/tasks/kicker_task.rs | 46 ++++++++++++++++++++++-- control-board/src/tasks/shutdown_task.rs | 9 ++--- software-communication | 2 +- 8 files changed, 91 insertions(+), 16 deletions(-) diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index 09a3c3aa..32127df8 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -8,4 +8,4 @@ runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' [env] # DEFMT_LOG = "error,ateam-control-board::tasks::control_task=trace" -DEFMT_LOG="trace" +DEFMT_LOG="debug" diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index dd1a29cc..ddf151cd 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -14,7 +14,7 @@ use ateam_control_board::{ get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, - tasks::control_task::start_control_task}; + tasks::{control_task::start_control_task, kicker_task::start_kicker_task}}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -77,6 +77,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // commands channel let radio_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + let kicker_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); // telemetry channel let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); @@ -125,6 +126,14 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.USART1, p.PB15, p.PB14, p.DMA1_CH1, p.DMA1_CH0, p.PD8, p.PD9, p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; + start_kicker_task( + main_spawner, uart_queue_spawner, + robot_state, + kicker_command_subscriber, + p.USART6, + p.PC7, p.PC6, p.DMA2_CH5, p.DMA2_CH4, p.PA8, p.PA9, p.PG8, + ).await; + loop { Timer::after_millis(10).await; } diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index 6d0af0ab..fc8c1c0f 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -16,9 +16,9 @@ const GYRO_DATA_PUBSUB_DEPTH: usize = 1; const ACCEL_DATA_PUBSUB_DEPTH: usize = 1; const BATTERY_VOLT_PUBSUB_DEPTH: usize = 1; -pub type CommandsPubSub = PubSubChannel; -pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; -pub type CommandsSubscriber = Subscriber<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; +pub type CommandsPubSub = PubSubChannel; +pub type CommandsPublisher = Publisher<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; +pub type CommandsSubscriber = Subscriber<'static, ThreadModeRawMutex, DataPacket, COMMANDS_PUBSUB_DEPTH, 2, 1>; pub type TelemetryPubSub = PubSubChannel; pub type TelemetryPublisher = Publisher<'static, ThreadModeRawMutex, TelemetryPacket, COMMANDS_PUBSUB_DEPTH, 1, 1>; diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index afffe709..e188c354 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -26,6 +26,9 @@ pub struct SharedRobotState { robot_tipped: AtomicBool, shutdown_requested: AtomicBool, + + ball_detected: AtomicBool, + kicker_shutdown_complete: AtomicBool, } impl SharedRobotState { @@ -47,6 +50,8 @@ impl SharedRobotState { battery_ok: AtomicBool::new(false), robot_tipped: AtomicBool::new(false), shutdown_requested: AtomicBool::new(false), + ball_detected: AtomicBool::new(false), + kicker_shutdown_complete: AtomicBool::new(false), } } @@ -73,6 +78,8 @@ impl SharedRobotState { robot_tipped: self.robot_tipped(), shutdown_requested: self.shutdown_requested(), + ball_detected: self.ball_detected(), + kicker_shutdown_complete: self.kicker_shutdown_complete(), } } @@ -147,6 +154,22 @@ impl SharedRobotState { pub fn set_radio_connection_ok(&self, conn_ok: bool) { self.radio_connection_ok.store(conn_ok, Ordering::Relaxed); } + + pub fn ball_detected(&self) -> bool { + self.ball_detected.load(Ordering::Relaxed) + } + + pub fn set_ball_detected(&self, ball_detected: bool) { + self.ball_detected.store(ball_detected, Ordering::Relaxed); + } + + pub fn kicker_shutdown_complete(&self) -> bool { + self.kicker_shutdown_complete.load(Ordering::Relaxed) + } + + pub fn set_kicker_shutdown_complete(&self, shutdown_complete: bool) { + self.kicker_shutdown_complete.store(shutdown_complete, Ordering::Relaxed); + } } #[derive(Clone, Copy, PartialEq, Debug)] @@ -173,4 +196,6 @@ pub struct RobotState { pub robot_tipped: bool, pub shutdown_requested: bool, + pub ball_detected: bool, + pub kicker_shutdown_complete: bool, } \ No newline at end of file diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index d2dcb2de..0528c9dc 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -162,7 +162,7 @@ impl < battery_temperature: 0., _bitfield_align_1: [], _bitfield_1: BasicTelemetry::new_bitfield_1( - 0, 0, 0, 0, 0, 0, 0, 0, err_fl, hall_err_fl, err_bl, hall_err_bl, err_br, hall_err_br, err_fr, hall_err_fr, err_drib, hall_err_drib, 0, 0, 0, + 0, 0, 0, self.shared_robot_state.ball_detected() as u32, 0, 0, 0, 0, err_fl, hall_err_fl, err_bl, hall_err_bl, err_br, hall_err_br, err_fr, hall_err_fr, err_drib, hall_err_drib, 0, 0, 0, ), motor_0_temperature: 0., motor_1_temperature: 0., @@ -285,8 +285,8 @@ impl < let battery_v = 25.0; let controls_enabled = true; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; - defmt::warn!("gyro rads: {}", gyro_rads); - let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { + // defmt::warn!("gyro rads: {}", gyro_rads); + let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { // TODO check order self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) } else { diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 6d590cb9..158fd82c 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -3,7 +3,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart: use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::{gpio::{Level, Output, Pin, Speed}, usart::{self, Uart}}; use embassy_sync::pubsub::WaitResult; -use embassy_time::Timer; +use embassy_time::{Duration, Ticker, Timer}; use crate::{drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, stm32_interface, SystemIrqs}; @@ -20,6 +20,22 @@ make_uart_queue_pair!(KICKER, MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, #[link_section = ".axisram.buffers"]); +#[macro_export] +macro_rules! create_kicker_task { + ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, + $radio_command_publisher:ident, $radio_telemetry_subscriber:ident, + $wifi_credentials:ident, $p:ident) => { + ateam_control_board::tasks::radio_task::start_radio_task( + $main_spawner, $uart_queue_spawner, + $robot_state, + $radio_command_publisher, $radio_telemetry_subscriber, + &$wifi_credentials, + $p.USART10, $p.PE2, $p.PE3, $p.PG13, $p.PG14, + $p.DMA2_CH1, $p.DMA2_CH0, + $p.PC13, $p.PE4).await; + }; +} + #[derive(PartialEq, PartialOrd, Debug)] enum KickerTaskState { PoweredOff, @@ -82,6 +98,8 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } pub async fn kicker_task_entry(&mut self) { + let mut main_loop_ticker = Ticker::every(Duration::from_hz(100)); + loop { let cur_robot_state = self.robot_state.get_state(); @@ -100,6 +118,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } }, KickerTaskState::PowerOn => { + defmt::debug!("power cycling kicker"); self.remote_power_btn_hold().await; self.remote_power_btn_press().await; // power should be coming on, attempt connection @@ -111,8 +130,12 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ // if the kicker is missing or bugged we'll stay in a power on -> attempt // uart loop forever self.kicker_task_state = KickerTaskState::PowerOn; + + defmt::error!("kicker firmware load failed, try power cycle"); } else { self.kicker_task_state = KickerTaskState::Connected; + + defmt::info!("kicker flashed!"); } }, KickerTaskState::Connected => { @@ -121,12 +144,18 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ }, KickerTaskState::InitiateShutdown => { self.kicker_driver.request_shutdown(); - self.kicker_task_state = KickerTaskState::WaitShutdown; + + // wait for kicker to ack shutdown + if self.kicker_driver.shutdown_acknowledge() { + defmt::info!("kicker acknowledged shutdown request"); + self.kicker_task_state = KickerTaskState::WaitShutdown; + } }, KickerTaskState::WaitShutdown => { if self.kicker_driver.shutdown_completed() { defmt::info!("kicker finished shutdown"); self.kicker_task_state = KickerTaskState::PoweredOff; + self.robot_state.set_kicker_shutdown_complete(true); } }, } @@ -135,7 +164,16 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ // commands to the kicker if self.kicker_task_state >= KickerTaskState::Connected { self.kicker_driver.send_command(); + + let ball_detected = self.kicker_driver.ball_detected(); + if ball_detected { + defmt::info!("ball detected!"); + } + + self.robot_state.set_ball_detected(ball_detected); } + + main_loop_ticker.next().await; } } @@ -143,7 +181,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ self.remote_power_btn.set_high(); Timer::after_millis(200).await; self.remote_power_btn.set_low(); - Timer::after_millis(10).await; + Timer::after_millis(200).await; } async fn remote_power_btn_hold(&mut self) { @@ -154,6 +192,8 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } async fn connected_poll_loop(&mut self) { + self.kicker_driver.set_telemetry_enabled(true); + if let Some(pkt) = self.commands_subscriber.try_next_message() { match pkt { WaitResult::Lagged(amnt) => { diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs index faa30978..9c62d7b7 100644 --- a/control-board/src/tasks/shutdown_task.rs +++ b/control-board/src/tasks/shutdown_task.rs @@ -4,7 +4,7 @@ use embassy_futures::select; use embassy_stm32::gpio::{Level, Output, OutputOpenDrain, Pull, Speed}; use embassy_time::Timer; -use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::SharedRobotState}; +use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::{self, SharedRobotState}}; pub const HARD_SHUTDOWN_TIME_MS: u64 = 10000; @@ -35,10 +35,11 @@ async fn shutdown_task_entry(robot_state: &'static SharedRobotState, // wait for tasks to flag shutdown complete, or hard temrinate after 10s select::select(async move { loop { + Timer::after_millis(100).await; - - // TODO wait for other tasks - Timer::after_millis(1000).await; + if robot_state.kicker_shutdown_complete() { + break; + } } }, Timer::after_millis(HARD_SHUTDOWN_TIME_MS)).await; diff --git a/software-communication b/software-communication index c5fe2864..099134d3 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit c5fe28648faaeb42043e89b74e806b0db6de5858 +Subproject commit 099134d3d866954aaeecd967ec515a73c54fa634 From 0fabc3c61064b5ee595ca37807375ca6e274adb3 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 3 Jul 2024 23:45:01 -0400 Subject: [PATCH 109/157] remove git diff text file --- git_diff_firmware.txt | 651 ------------------------------------------ 1 file changed, 651 deletions(-) delete mode 100644 git_diff_firmware.txt diff --git a/git_diff_firmware.txt b/git_diff_firmware.txt deleted file mode 100644 index 043f1c66..00000000 --- a/git_diff_firmware.txt +++ /dev/null @@ -1,651 +0,0 @@ -diff --git a/control-board/src/motion/constant_gain_kalman_filter.rs b/control-board/src/motion/constant_gain_kalman_filter.rs -index bc8fec5..34f11c2 100644 ---- a/control-board/src/motion/constant_gain_kalman_filter.rs -+++ b/control-board/src/motion/constant_gain_kalman_filter.rs -@@ -51,7 +51,7 @@ impl<'a, const NUM_STATES: usize, const NUM_CONTROL_INPUTS: usize, const NUM_OBS - // Constant gain so don't need Innovation Covariance / Optimal Kalman gain calculation - - self.state_estimate = self.pred_state_estimate + self.kalman_gain * innovation_residual; -- -+ - // self.estimate_cov = (SMatrix::::identity() - self.kalman_gain * self.observation_model) * self.pred_estimate_cov; - - self.measurement_residual = measurement - self.observation_model * self.state_estimate; -diff --git a/control-board/src/motion/control.rs b/control-board/src/motion/control.rs -deleted file mode 100644 -index f8b9654..0000000 ---- a/control-board/src/motion/control.rs -+++ /dev/null -@@ -1,494 +0,0 @@ --use embassy_executor::SendSpawner; --use embassy_stm32::{ -- gpio::{Level, Output, Speed}, mode::Async, usart::Uart --}; --use embassy_sync::pubsub::Subscriber; --use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; --use embassy_time::{Duration, Timer}; --use crate::{ -- include_external_cpp_bin, -- stm32_interface::Stm32Interface, -- stspin_motor::{WheelMotor, DribblerMotor}, -- motion::{ -- robot_model::{RobotConstants, RobotModel}, -- robot_controller::BodyVelocityController -- }, -- BATTERY_MIN_VOLTAGE, -- parameter_interface::ParameterInterface --}; -- --use nalgebra::{Vector3, Vector4}; -- --use ateam_lib_stm32::make_uart_queue_pair; -- --use ateam_common_packets::bindings_radio::{ -- BasicControl, -- BasicTelemetry, -- ControlDebugTelemetry, -- ParameterCommand, -- ParameterName --}; -- --use crate::pins::*; -- -- --include_external_cpp_bin! {DRIB_FW_IMG, "dribbler.bin"} --include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"} -- --// motor pinout --// FrontRight Wheel - UART5 - tx pb6, pb12, boot pb1, rst pb2, DMA1 0/1 --// FrontLeft Wheel - UART7 - tx pf7, rx pf6, boot pg2, rst pg3, DMA1 2/3 --// BackLeft Wheel - UART4 - tx pd1, rx pd0, boot pg0, rst pg1, DMA1 4/5 --// BackRight Wheel - USART3 - tx pb10, rx pb11, boot pf4, rst pa3, DMA1 6/7 --// Dribbler - USART6 - tx pc6, rx pc7, boot pc2, rst pd7, DMA2 2/3 -- --const MAX_TX_PACKET_SIZE: usize = 64; --const TX_BUF_DEPTH: usize = 3; --const MAX_RX_PACKET_SIZE: usize = 64; --const RX_BUF_DEPTH: usize = 20; -- --const TICKS_WITHOUT_PACKET_STOP: u16 = 25; -- --make_uart_queue_pair!(FRONT_LEFT, -- MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, -- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, -- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, -- #[link_section = ".axisram.buffers"]); -- --make_uart_queue_pair!(BACK_LEFT, -- MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, -- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, -- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, -- #[link_section = ".axisram.buffers"]); -- --make_uart_queue_pair!(BACK_RIGHT, -- MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, -- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, -- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, -- #[link_section = ".axisram.buffers"]); -- --make_uart_queue_pair!(FRONT_RIGHT, -- MotorFRUart, MotorFRDmaRx, MotorFRDmaTx, -- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, -- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, -- #[link_section = ".axisram.buffers"]); -- --make_uart_queue_pair!(DRIB, -- MotorDUart, MotorDDmaRx, MotorDDmaTx, -- MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, -- MAX_TX_PACKET_SIZE, TX_BUF_DEPTH, -- #[link_section = ".axisram.buffers"]); -- --const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(30.0, 150.0, 225.0, 315.0); --const WHEEL_RADIUS_M: f32 = 0.049 / 2.0; // wheel dia 49mm --const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.085; // 85mm from center of wheel body to center of robot -- --pub struct Control<'a> { -- robot_model: RobotModel, -- robot_controller: BodyVelocityController<'a>, -- cmd_vel: Vector3, -- drib_vel: f32, -- front_right_motor: WheelMotor< -- 'static, -- MotorFRUart, -- MotorFRDmaRx, -- MotorFRDmaTx, -- MAX_RX_PACKET_SIZE, -- MAX_TX_PACKET_SIZE, -- RX_BUF_DEPTH, -- TX_BUF_DEPTH -- >, -- front_left_motor: WheelMotor< -- 'static, -- MotorFLUart, -- MotorFLDmaRx, -- MotorFLDmaTx, -- MAX_RX_PACKET_SIZE, -- MAX_TX_PACKET_SIZE, -- RX_BUF_DEPTH, -- TX_BUF_DEPTH -- >, -- back_left_motor: WheelMotor< -- 'static, -- MotorBLUart, -- MotorBLDmaRx, -- MotorBLDmaTx, -- MAX_RX_PACKET_SIZE, -- MAX_TX_PACKET_SIZE, -- RX_BUF_DEPTH, -- TX_BUF_DEPTH -- >, -- back_right_motor: WheelMotor< -- 'static, -- MotorBRUart, -- MotorBRDmaRx, -- MotorBRDmaTx, -- MAX_RX_PACKET_SIZE, -- MAX_TX_PACKET_SIZE, -- RX_BUF_DEPTH, -- TX_BUF_DEPTH -- >, -- drib_motor: DribblerMotor< -- 'static, -- MotorDUart, -- MotorDDmaRx, -- MotorDDmaTx, -- MAX_RX_PACKET_SIZE, -- MAX_TX_PACKET_SIZE, -- RX_BUF_DEPTH, -- TX_BUF_DEPTH -- >, -- ticks_since_packet: u16, -- gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, -- battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> --} -- --// Uart -- --impl<'a> Control<'a> { -- pub fn new( -- spawner: &SendSpawner, -- front_right_usart: Uart<'static, MotorFRUart, Async>, -- front_left_usart: Uart<'static, MotorFLUart, Async>, -- back_left_usart: Uart<'static, MotorBLUart, Async>, -- back_right_usart: Uart<'static, MotorBRUart, Async>, -- drib_usart: Uart<'static, MotorDUart, Async>, -- front_right_boot0_pin: MotorFRBootPin, -- front_left_boot0_pin: MotorFLBootPin, -- back_left_boot0_pin: MotorBLBootPin, -- back_right_boot0_pin: MotorBRBootPin, -- drib_boot0_pin: MotorDBootPin, -- front_right_reset_pin: MotorFRResetPin, -- front_left_reset_pin: MotorFLResetPin, -- back_left_reset_pin: MotorBLResetPin, -- back_right_reset_pin: MotorBRResetPin, -- drib_reset_pin: MotorDResetPin, -- ball_detected_thresh: f32, -- gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>, -- battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2> -- ) -> Control<'a> { -- let wheel_firmware_image = WHEEL_FW_IMG; -- let drib_firmware_image = DRIB_FW_IMG; -- -- let (front_right_tx, front_right_rx) = front_right_usart.split(); -- let (front_left_tx, front_left_rx) = front_left_usart.split(); -- let (back_left_tx, back_left_rx) = back_left_usart.split(); -- let (back_right_tx, back_right_rx) = back_right_usart.split(); -- let (drib_tx, drib_rx) = drib_usart.split(); -- -- let front_right_boot0_pin = Output::new(front_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active -- let front_left_boot0_pin = Output::new(front_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active -- let back_left_boot0_pin = Output::new(back_left_boot0_pin, Level::Low, Speed::Medium); // boot0 not active -- let back_right_boot0_pin = Output::new(back_right_boot0_pin, Level::Low, Speed::Medium); // boot0 not active -- let drib_boot0_pin = Output::new(drib_boot0_pin, Level::Low, Speed::Medium); // boot0 not active -- -- let front_right_reset_pin = -- Output::new(front_right_reset_pin, Level::Low, Speed::Medium); // reset active -- let front_left_reset_pin = -- Output::new(front_left_reset_pin, Level::Low, Speed::Medium); // reset active -- let back_left_reset_pin = -- Output::new(back_left_reset_pin, Level::Low, Speed::Medium); // reset active -- let back_right_reset_pin = -- Output::new(back_right_reset_pin, Level::Low, Speed::Medium); // reset active -- let drib_reset_pin = -- Output::new(drib_reset_pin, Level::Low, Speed::Medium); // reset active -- -- spawner -- .spawn(FRONT_RIGHT_RX_UART_QUEUE.spawn_task(front_right_rx)) -- .unwrap(); -- spawner -- .spawn(FRONT_RIGHT_TX_UART_QUEUE.spawn_task(front_right_tx)) -- .unwrap(); -- spawner -- .spawn(FRONT_LEFT_RX_UART_QUEUE.spawn_task(front_left_rx)) -- .unwrap(); -- spawner -- .spawn(FRONT_LEFT_TX_UART_QUEUE.spawn_task(front_left_tx)) -- .unwrap(); -- spawner -- .spawn(BACK_LEFT_RX_UART_QUEUE.spawn_task(back_left_rx)) -- .unwrap(); -- spawner -- .spawn(BACK_LEFT_TX_UART_QUEUE.spawn_task(back_left_tx)) -- .unwrap(); -- spawner -- .spawn(BACK_RIGHT_RX_UART_QUEUE.spawn_task(back_right_rx)) -- .unwrap(); -- spawner -- .spawn(BACK_RIGHT_TX_UART_QUEUE.spawn_task(back_right_tx)) -- .unwrap(); -- spawner -- .spawn(DRIB_RX_UART_QUEUE.spawn_task(drib_rx)) -- .unwrap(); -- spawner -- .spawn(DRIB_TX_UART_QUEUE.spawn_task(drib_tx)) -- .unwrap(); -- -- let front_right_stm32_interface = Stm32Interface::new( -- &FRONT_RIGHT_RX_UART_QUEUE, -- &FRONT_RIGHT_TX_UART_QUEUE, -- Some(front_right_boot0_pin), -- Some(front_right_reset_pin), -- ); -- let front_right_motor = WheelMotor::new(front_right_stm32_interface, wheel_firmware_image); -- -- let front_left_stm32_interface = Stm32Interface::new( -- &FRONT_LEFT_RX_UART_QUEUE, -- &FRONT_LEFT_TX_UART_QUEUE, -- Some(front_left_boot0_pin), -- Some(front_left_reset_pin), -- ); -- let front_left_motor = WheelMotor::new(front_left_stm32_interface, wheel_firmware_image); -- -- let back_left_stm32_interface = Stm32Interface::new( -- &BACK_LEFT_RX_UART_QUEUE, -- &BACK_LEFT_TX_UART_QUEUE, -- Some(back_left_boot0_pin), -- Some(back_left_reset_pin), -- ); -- let back_left_motor = WheelMotor::new(back_left_stm32_interface, wheel_firmware_image); -- -- let back_right_stm32_interface = Stm32Interface::new( -- &BACK_RIGHT_RX_UART_QUEUE, -- &BACK_RIGHT_TX_UART_QUEUE, -- Some(back_right_boot0_pin), -- Some(back_right_reset_pin), -- ); -- let back_right_motor = WheelMotor::new(back_right_stm32_interface, wheel_firmware_image); -- -- let drib_stm32_interface = Stm32Interface::new( -- &DRIB_RX_UART_QUEUE, -- &DRIB_TX_UART_QUEUE, -- Some(drib_boot0_pin), -- Some(drib_reset_pin), -- ); -- let drib_motor = DribblerMotor::new(drib_stm32_interface, drib_firmware_image, ball_detected_thresh); -- -- let robot_model_constants: RobotConstants = RobotConstants { -- wheel_angles_rad: Vector4::new( -- WHEEL_ANGLES_DEG[0].to_radians(), -- WHEEL_ANGLES_DEG[1].to_radians(), -- WHEEL_ANGLES_DEG[2].to_radians(), -- WHEEL_ANGLES_DEG[3].to_radians(), -- ), -- wheel_radius_m: Vector4::new( -- WHEEL_RADIUS_M, -- WHEEL_RADIUS_M, -- WHEEL_RADIUS_M, -- WHEEL_RADIUS_M, -- ), -- wheel_dist_to_cent_m: Vector4::new( -- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, -- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, -- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, -- WHEEL_DISTANCE_TO_ROBOT_CENTER_M, -- ), -- }; -- -- let robot_model: RobotModel = RobotModel::new(robot_model_constants); -- -- let body_velocity_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); -- -- Control { -- robot_model, -- robot_controller: body_velocity_controller, -- cmd_vel: Vector3::new(0., 0., 0.), -- drib_vel: 0.0, -- front_right_motor, -- front_left_motor, -- back_left_motor, -- back_right_motor, -- drib_motor, -- ticks_since_packet: 0, -- gyro_sub, -- battery_sub -- } -- } -- -- pub async fn load_firmware(&mut self) { -- let _res = embassy_futures::join::join5( -- self.front_right_motor.load_default_firmware_image(), -- self.front_left_motor.load_default_firmware_image(), -- self.back_left_motor.load_default_firmware_image(), -- self.back_right_motor.load_default_firmware_image(), -- self.drib_motor.load_default_firmware_image(), -- ) -- .await; -- -- // defmt::info!("flashing firmware"); -- -- // self.front_right_motor.load_default_firmware_image().await; -- // defmt::info!("FR flashed"); -- -- // self.front_left_motor.load_default_firmware_image().await; -- // defmt::info!("FL flashed"); -- -- // self.back_left_motor.load_default_firmware_image().await; -- // defmt::info!("BL flashed"); -- -- // self.back_right_motor.load_default_firmware_image().await; -- // defmt::info!("BR flashed"); -- -- // self.drib_motor.load_default_firmware_image().await; -- // defmt::info!("DRIB flashed"); -- -- -- -- defmt::info!("flashed"); -- -- // leave reset -- // don't pull the chip out of reset until we're ready to read packets or we'll fill the queue -- embassy_futures::join::join5( -- self.front_right_motor.leave_reset(), -- self.front_left_motor.leave_reset(), -- self.back_left_motor.leave_reset(), -- self.back_right_motor.leave_reset(), -- self.drib_motor.leave_reset(), -- ) -- .await; -- -- self.front_right_motor.set_telemetry_enabled(true); -- self.front_left_motor.set_telemetry_enabled(true); -- self.back_left_motor.set_telemetry_enabled(true); -- self.back_right_motor.set_telemetry_enabled(true); -- self.drib_motor.set_telemetry_enabled(true); -- -- // need to have telem off by default and enabled later -- // theres a race condition to begin processing packets from the first part out -- // of reset and waiting for the last part to boot up -- Timer::after(Duration::from_millis(10)).await; -- } -- -- fn process_mc_packets(&mut self) { -- self.front_right_motor.process_packets(); -- self.front_left_motor.process_packets(); -- self.back_left_motor.process_packets(); -- self.back_right_motor.process_packets(); -- self.drib_motor.process_packets(); -- } -- -- pub async fn tick(&mut self, latest_control: Option) -> (Option, ControlDebugTelemetry) { -- self.process_mc_packets(); -- -- self.front_right_motor.log_reset("FR"); -- self.front_left_motor.log_reset("RL"); -- self.back_left_motor.log_reset("BL"); -- self.back_right_motor.log_reset("BR"); -- self.drib_motor.log_reset("DRIB"); -- -- if self.drib_motor.ball_detected() { -- defmt::info!("ball detected"); -- } -- -- if let Some(latest_control) = &latest_control { -- let cmd_vel = Vector3::new( -- latest_control.vel_x_linear, -- latest_control.vel_y_linear, -- latest_control.vel_z_angular, -- ); -- self.cmd_vel = cmd_vel; -- self.drib_vel = latest_control.dribbler_speed; -- self.ticks_since_packet = 0; -- } else { -- self.ticks_since_packet += 1; -- if self.ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { -- self.cmd_vel = Vector3::new(0., 0., 0.); -- self.ticks_since_packet = 0; -- } -- } -- -- // now we have setpoint r(t) in self.cmd_vel -- let battery_v = self.battery_sub.next_message_pure().await as f32; -- let controls_enabled = true; -- let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0; -- let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE { -- if controls_enabled -- { -- // TODO check order -- let wheel_vels = Vector4::new( -- self.front_right_motor.read_rads(), -- self.front_left_motor.read_rads(), -- self.back_left_motor.read_rads(), -- self.back_right_motor.read_rads() -- ); -- -- // torque values are computed on the spin but put in the current variable -- // TODO update this when packet/var names are updated to match software -- let wheel_torques = Vector4::new( -- self.front_right_motor.read_current(), -- self.front_left_motor.read_current(), -- self.back_left_motor.read_current(), -- self.back_right_motor.read_current() -- ); -- -- // TODO read from channel or something -- -- self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads); -- -- self.robot_controller.get_wheel_velocities() -- } -- else -- { -- self.robot_model.robot_vel_to_wheel_vel(self.cmd_vel) -- } -- } -- else -- { -- // Battery is too low, set velocity to zero -- Vector4::new( -- 0.0, -- 0.0, -- 0.0, -- 0.0) -- }; -- -- self.front_right_motor.set_setpoint(wheel_vels[0]); -- self.front_left_motor.set_setpoint(wheel_vels[1]); -- self.back_left_motor.set_setpoint(wheel_vels[2]); -- self.back_right_motor.set_setpoint(wheel_vels[3]); -- -- let drib_dc = -1.0 * self.drib_vel / 1000.0; -- self.drib_motor.set_setpoint(drib_dc); -- -- self.front_right_motor.send_motion_command(); -- self.front_left_motor.send_motion_command(); -- self.back_left_motor.send_motion_command(); -- self.back_right_motor.send_motion_command(); -- self.drib_motor.send_motion_command(); -- -- (Some(BasicTelemetry { -- sequence_number: 0, -- robot_revision_major: 0, -- robot_revision_minor: 0, -- battery_level: battery_v, -- battery_temperature: 0., -- _bitfield_align_1: [], -- _bitfield_1: BasicTelemetry::new_bitfield_1( -- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -- ), -- motor_0_temperature: 0., -- motor_1_temperature: 0., -- motor_2_temperature: 0., -- motor_3_temperature: 0., -- motor_4_temperature: 0., -- kicker_charge_level: 0., -- }), -- self.robot_controller.get_control_debug_telem()) -- } -- -- --} -- --impl<'a> ParameterInterface for Control<'a> { -- fn processes_cmd(&self, param_cmd: &ParameterCommand) -> bool { -- return self.robot_controller.processes_cmd(param_cmd); -- } -- -- fn has_name(&self, param_name: ParameterName::Type) -> bool { -- return self.robot_controller.has_name(param_name); -- } -- -- fn apply_command(&mut self, param_cmd: &ParameterCommand) -> Result { -- return self.robot_controller.apply_command(param_cmd); -- } --} -diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs -index 10b2921..c0daedd 100644 ---- a/control-board/src/motion/params/body_vel_filter_params.rs -+++ b/control-board/src/motion/params/body_vel_filter_params.rs -@@ -51,5 +51,4 @@ pub static INIT_ESTIMATE_COV: Matrix3 = - pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; - 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; -- 0.06698, 0.0820302, 0.0820302, 0.06698, 0.00011093]; -- -+ 0.02092594, 0.02562894, 0.02562894, 0.02092594, 0.6931516]; -diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs -index 5a1303f..c383e11 100644 ---- a/control-board/src/motion/params/body_vel_pid_params.rs -+++ b/control-board/src/motion/params/body_vel_pid_params.rs -@@ -2,13 +2,13 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; - - // Kp, Ki, Kd, Ki_err_min, Ki_err_max - pub static PID_GAIN: Matrix3x5 = -- matrix![0.1, 0.0, 0.0, -1.0, 1.0; -- 0.1, 0.0, 0.0, -1.0, 1.0; -- 0.5, 0.0, 0.0, -3.0, 3.0]; -+ matrix![1.0, 0.0, 0.0, -1.0, 1.0; -+ 1.0, 0.0, 0.0, -1.0, 1.0; -+ 1.0, 0.0, 0.0, -3.0, 3.0]; - - // x, y, theta (m/s, m/s, rad/s) - pub static BODY_VEL_LIM: Vector3 = vector![3.0, 3.0, 18.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate --pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore -+pub static BODY_ACC_LIM: Vector3 = vector![8.0, 8.0, 36.0]; // TODO calibrate/ignore - - // FL, BL, BR, FR (rad/s^2) - // Rough estimate for peak rating -diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs -index bd77ec0..9e0f037 100644 ---- a/control-board/src/motion/robot_controller.rs -+++ b/control-board/src/motion/robot_controller.rs -@@ -179,9 +179,17 @@ impl<'a> BodyVelocityController<'a> { - - // If the encoder estimate is small enough, then replace IMU value with - // encoder value to reduce the jittery noise. -- if abs_f32(enc_body_vel[2]) < 0.1 { -+ -+ /* -+ if abs_f32(enc_body_vel[0]) < 0.1 -+ && abs_f32(enc_body_vel[1]) < 0.1 -+ && abs_f32(enc_body_vel[2]) < 0.1 { -+ - measurement[4] = enc_body_vel[2] - } -+ */ -+ -+ //defmt::debug!("{}", measurement[4]); - - // Update measurements process observation input into CGKF. - self.body_vel_filter.update(&measurement); -@@ -224,13 +232,13 @@ impl<'a> BodyVelocityController<'a> { - // and globally invalid. Investiage this later. If problems are suspected, disable this section - // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) - // TODO cross check in the future against wheel angle plots and analysis -- let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; -- let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); -- let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); -- self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); -+ //let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; -+ //let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); -+ //let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); -+ self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output.as_slice()); - - // Save command state. -- self.cmd_wheel_velocities = wheel_vel_output_clamp; -+ self.cmd_wheel_velocities = wheel_vel_output; - } - - pub fn get_wheel_velocities(&self) -> Vector4 { -@@ -494,4 +502,4 @@ impl<'a> ParameterInterface for BodyVelocityController<'a> { - - return Ok(reply_cmd); - } --} -\ No newline at end of file -+} -diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs -index 56e076d..7eabe72 100644 ---- a/control-board/src/stspin_motor.rs -+++ b/control-board/src/stspin_motor.rs -@@ -219,7 +219,7 @@ impl< - // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); - // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); - if mrp.data.motion.master_error() != 0 { -- error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); -+ //error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); - } - // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); - // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); -diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h -index 6ec82b3..3b9d55c 100644 ---- a/motor-controller/bin/wheel/system.h -+++ b/motor-controller/bin/wheel/system.h -@@ -45,7 +45,7 @@ - #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) - #define TELEMETRY_LOOP_RATE_MS 5 - --#define MOTOR_MAXIMUM_ACCEL 2000 // rad/s^2 -+#define MOTOR_MAXIMUM_ACCEL 5500 // rad/s^2 - - - -diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c -index e36cc37..208e950 100644 ---- a/motor-controller/common/pid.c -+++ b/motor-controller/common/pid.c -@@ -1,5 +1,6 @@ - - #include -+#include - - #include "pid.h" - -@@ -31,6 +32,11 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { - // float alpha = 0.37f; - // pid->eI = (pid->eI * alpha) + (err * dt * (1.0f - alpha)); - pid->eI = pid->eI + (err * dt); -+ -+ if (fabs(r) < 3.0) { -+ pid->eI = 0.0; -+ } -+ - if (pid->eI > pid->pid_constants->kI_max) { - pid->eI = pid->pid_constants->kI_max; - } else if (pid->eI < pid->pid_constants->kI_min) { -diff --git a/software-communication b/software-communication -index 5aae1e3..cb445b0 160000 ---- a/software-communication -+++ b/software-communication -@@ -1 +1 @@ --Subproject commit 5aae1e348f5a60a2279cbbff66ecb66d468620fd -+Subproject commit cb445b0c305e7033d7c05be1f2ee1695c461252e From bd0cf1ebb471e10cc710ebbc5d87ac5413551d8d Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Wed, 3 Jul 2024 23:47:45 -0400 Subject: [PATCH 110/157] Add tests --- lib-stm32/src/math/linear.rs | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/lib-stm32/src/math/linear.rs b/lib-stm32/src/math/linear.rs index f1a223a4..ea95c7c5 100644 --- a/lib-stm32/src/math/linear.rs +++ b/lib-stm32/src/math/linear.rs @@ -4,6 +4,8 @@ use core::ops::{Add, Sub, Div, Mul}; pub trait MappingNumeric: Copy + Sub + Add + Div + Mul + FromPrimitive + ToPrimitive {} +impl MappingNumeric for T where T: Copy + Sub + Add + Div + Mul + FromPrimitive + ToPrimitive {} + // Take input max and/or min (bounds), output value within new bounds struct LinearMap where @@ -27,4 +29,31 @@ where let mapped_val = (val - min_diff) * scale; return mapped_val; } -} \ No newline at end of file +} + +#[test] +fn small_to_large() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(0f32, 50f32); + let test_val: f32 = 5f32; + let success_val: f32 = 25f32; + assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); +} + +#[test] +fn large_to_small() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(0f32, 50f32); + let test_val: f32 = 25f32; + let success_val: f32 = 5f32; + assert_eq!(large.linear_map_to_new_bounds(test_val, small), success_val); +} + +#[test] +fn move_zero_point() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(10f32, 50f32); + let test_val: f32 = 5f32; + let success_val: f32 = 30f32; + assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); +} From 1af4f680b2753094c0968558bf8d68b9d49ce06f Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 7 Jul 2024 19:02:37 -0400 Subject: [PATCH 111/157] No panic handler --- lib-stm32/Cargo.lock | 33 +++++++++++++++++++++++++++++++++ lib-stm32/Cargo.toml | 13 ++++++++++++- lib-stm32/build.rs | 6 +++--- lib-stm32/src/math/lerp.rs | 5 ----- lib-stm32/src/math/linear.rs | 32 +++----------------------------- lib-stm32/tests/linear-map.rs | 35 +++++++++++++++++++++++++++++++++++ 6 files changed, 86 insertions(+), 38 deletions(-) create mode 100644 lib-stm32/tests/linear-map.rs diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index 9c10e482..e69ba759 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -9,6 +9,7 @@ dependencies = [ "critical-section 1.1.2", "defmt", "defmt-rtt", + "defmt-test", "embassy-executor", "embassy-futures", "embassy-stm32", @@ -115,6 +116,15 @@ dependencies = [ "syn 1.0.109", ] +[[package]] +name = "cortex-m-semihosting" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" +dependencies = [ + "cortex-m", +] + [[package]] name = "critical-section" version = "0.2.8" @@ -208,6 +218,29 @@ dependencies = [ "defmt", ] +[[package]] +name = "defmt-test" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +dependencies = [ + "cortex-m-rt", + "cortex-m-semihosting", + "defmt", + "defmt-test-macros", +] + +[[package]] +name = "defmt-test-macros" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.66", +] + [[package]] name = "document-features" version = "0.2.8" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 9926ee85..292d1bcc 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -32,4 +32,15 @@ embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} embassy-time = { git = "https://github.com/embassy-rs/embassy"} embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} -embassy-futures = { git = "https://github.com/embassy-rs/embassy"} \ No newline at end of file +embassy-futures = { git = "https://github.com/embassy-rs/embassy"} + +[dev-dependencies] +defmt-test = "0.3.0" + +[lib] +test = false +harness = false + +[[test]] +name = "linear-map" +harness = false \ No newline at end of file diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index 2c5edafd..e607c5b3 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -30,9 +30,9 @@ fn generate_pitch_set(ref_freq: u16, octaves_below: i8, octaves_above: i8) -> Ve } fn write_pitches_to_file(pitch_set: Vec<(String, u16)>){ - /// Write each pitch name as a constant - /// in the format PITCH_NAME_OCTAVE, ex. - /// pub const A4: u16 = 440 + // Write each pitch name as a constant + // in the format PITCH_NAME_OCTAVE, ex. + // pub const A4: u16 = 440 let f = fs::File::create("./src/audio/pitches.rs").expect("Unable to create pitches.rs"); let mut f = BufWriter::new(f); for pitch in pitch_set { diff --git a/lib-stm32/src/math/lerp.rs b/lib-stm32/src/math/lerp.rs index 2d0200f2..6060799d 100644 --- a/lib-stm32/src/math/lerp.rs +++ b/lib-stm32/src/math/lerp.rs @@ -100,8 +100,3 @@ L: Lerp L::lerp_f(self.a, self.b, pct_complete) } } - -#[test] -fn test_lerp() { - print("hi") -} \ No newline at end of file diff --git a/lib-stm32/src/math/linear.rs b/lib-stm32/src/math/linear.rs index ea95c7c5..b78d9742 100644 --- a/lib-stm32/src/math/linear.rs +++ b/lib-stm32/src/math/linear.rs @@ -7,7 +7,7 @@ pub trait MappingNumeric: Copy + Sub + Add + impl MappingNumeric for T where T: Copy + Sub + Add + Div + Mul + FromPrimitive + ToPrimitive {} // Take input max and/or min (bounds), output value within new bounds -struct LinearMap +pub struct LinearMap where T: MappingNumeric { @@ -19,11 +19,11 @@ impl LinearMap where T: MappingNumeric { - fn new(min: T, max: T) -> Self { + pub fn new(min: T, max: T) -> Self { LinearMap { min, max } } - fn linear_map_to_new_bounds(&self, val: T, new_bounds: LinearMap) -> T { + pub fn linear_map_to_new_bounds(&self, val: T, new_bounds: LinearMap) -> T { let min_diff = self.min + new_bounds.min; let scale = (new_bounds.max - new_bounds.min) / (self.max - self.min); let mapped_val = (val - min_diff) * scale; @@ -31,29 +31,3 @@ where } } -#[test] -fn small_to_large() { - let small = LinearMap::new(0f32, 10f32); - let large = LinearMap::new(0f32, 50f32); - let test_val: f32 = 5f32; - let success_val: f32 = 25f32; - assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); -} - -#[test] -fn large_to_small() { - let small = LinearMap::new(0f32, 10f32); - let large = LinearMap::new(0f32, 50f32); - let test_val: f32 = 25f32; - let success_val: f32 = 5f32; - assert_eq!(large.linear_map_to_new_bounds(test_val, small), success_val); -} - -#[test] -fn move_zero_point() { - let small = LinearMap::new(0f32, 10f32); - let large = LinearMap::new(10f32, 50f32); - let test_val: f32 = 5f32; - let success_val: f32 = 30f32; - assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); -} diff --git a/lib-stm32/tests/linear-map.rs b/lib-stm32/tests/linear-map.rs new file mode 100644 index 00000000..06821b55 --- /dev/null +++ b/lib-stm32/tests/linear-map.rs @@ -0,0 +1,35 @@ +#![no_std] +#![no_main] + +#[defmt_test::tests] +mod tests{ + use ateam_lib_stm32::math::linear::LinearMap; + use defmt::*; + + #[test] + fn small_to_large() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(0f32, 50f32); + let test_val: f32 = 5f32; + let success_val: f32 = 25f32; + defmt::assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); + } + + #[test] + fn large_to_small() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(0f32, 50f32); + let test_val: f32 = 25f32; + let success_val: f32 = 5f32; + defmt::assert_eq!(large.linear_map_to_new_bounds(test_val, small), success_val); + } + + #[test] + fn move_zero_point() { + let small = LinearMap::new(0f32, 10f32); + let large = LinearMap::new(10f32, 50f32); + let test_val: f32 = 5f32; + let success_val: f32 = 30f32; + defmt::assert_eq!(small.linear_map_to_new_bounds(test_val, large), success_val); + } +} From 474799fb0f952e52de0405520e8251ef9492fde1 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Sun, 7 Jul 2024 19:06:55 -0400 Subject: [PATCH 112/157] need to run tests on device or set different target --- lib-stm32/Cargo.lock | 11 +++++++++++ lib-stm32/Cargo.toml | 1 + lib-stm32/tests/linear-map.rs | 3 +++ 3 files changed, 15 insertions(+) diff --git a/lib-stm32/Cargo.lock b/lib-stm32/Cargo.lock index e69ba759..b2400f32 100644 --- a/lib-stm32/Cargo.lock +++ b/lib-stm32/Cargo.lock @@ -17,6 +17,7 @@ dependencies = [ "embassy-time", "heapless", "num-traits", + "panic-probe", "paste", "smart-leds", ] @@ -577,6 +578,16 @@ dependencies = [ "autocfg", ] +[[package]] +name = "panic-probe" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" +dependencies = [ + "cortex-m", + "defmt", +] + [[package]] name = "paste" version = "1.0.15" diff --git a/lib-stm32/Cargo.toml b/lib-stm32/Cargo.toml index 292d1bcc..0b755c0b 100644 --- a/lib-stm32/Cargo.toml +++ b/lib-stm32/Cargo.toml @@ -36,6 +36,7 @@ embassy-futures = { git = "https://github.com/embassy-rs/embassy"} [dev-dependencies] defmt-test = "0.3.0" +panic-probe = { version = "0.3", features = ["print-defmt"] } [lib] test = false diff --git a/lib-stm32/tests/linear-map.rs b/lib-stm32/tests/linear-map.rs index 06821b55..43f085c9 100644 --- a/lib-stm32/tests/linear-map.rs +++ b/lib-stm32/tests/linear-map.rs @@ -1,6 +1,9 @@ #![no_std] #![no_main] +use panic_probe as _; +use defmt_rtt as _; + #[defmt_test::tests] mod tests{ use ateam_lib_stm32::math::linear::LinearMap; From 1c2efe4207203ba5de13fbcf1358d42491aa39d3 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 9 Jul 2024 21:41:43 -0400 Subject: [PATCH 113/157] Fix the clamping function to be good math --- lib-stm32/src/math/linear.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/lib-stm32/src/math/linear.rs b/lib-stm32/src/math/linear.rs index b78d9742..02ec948b 100644 --- a/lib-stm32/src/math/linear.rs +++ b/lib-stm32/src/math/linear.rs @@ -24,9 +24,8 @@ where } pub fn linear_map_to_new_bounds(&self, val: T, new_bounds: LinearMap) -> T { - let min_diff = self.min + new_bounds.min; let scale = (new_bounds.max - new_bounds.min) / (self.max - self.min); - let mapped_val = (val - min_diff) * scale; + let mapped_val = (val - self.min) * scale + new_bounds.min; return mapped_val; } } From 47c803fe94edd1114b1d387f5a4db297ab00e3b7 Mon Sep 17 00:00:00 2001 From: Christian Clark Date: Tue, 9 Jul 2024 23:02:24 -0400 Subject: [PATCH 114/157] Mapping speed to duration in kick_manager --- kicker-board/src/bin/kicker/main.rs | 6 +++--- kicker-board/src/kick_manager.rs | 16 ++++++++++------ 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index ce165636..e4a1d006 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -212,12 +212,12 @@ async fn high_pri_kick_task( // scale kick strength from m/s to duty for the critical section // if shutdown is requested and not complete, set kick discharge kick strength to 5% - let kick_strength = if shutdown_completed { + let kick_speed = if shutdown_completed { 0.0 } else if shutdown_requested { SHUTDOWN_KICK_DUTY } else { - fmaxf(0.0, fminf(MAX_KICK_SPEED, kicker_control_packet.kick_speed)) / MAX_KICK_SPEED + fmaxf(0.0, fminf(MAX_KICK_SPEED, kicker_control_packet.kick_speed)) }; // if control requests only an ARM or DISABLE, clear the active command @@ -304,7 +304,7 @@ async fn high_pri_kick_task( rail_voltage, charge_hv_rail, kick_command, - kick_strength, + kick_speed ) .await }; diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index b5a88894..869a71c8 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -25,8 +25,9 @@ use embassy_stm32::gpio::Output; use embassy_time::{Duration, Timer}; use libm::{fmaxf, fminf}; +use ateam_lib_stm32::math::linear::LinearMap; -const MAX_KICK_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick +const MAX_KICK_DURATION_US: f32 = 4500.0; // 10ms (10k us) max power kick const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off @@ -37,6 +38,7 @@ const MAX_SAFE_RAIL_VOLTAGE: f32 = 190.0; // rail is rated for 200V, and should const VBATT_OVERVOLTAGE_LOCKOUT: f32 = 27.2; const VBATT_UNDERVOLTAGE_LOCKOUT: f32 = 17.2; + #[derive(Copy, Clone, PartialEq, Eq, PartialOrd, Ord)] pub enum KickType { Kick, @@ -68,7 +70,7 @@ impl<'a> KickManager<'a> { } } - pub async fn command(&mut self, battery_voltage: f32, rail_voltage:f32, charge: bool, kick_type: KickType, kick_strength: f32) -> Result<(), ()> { + pub async fn command(&mut self, battery_voltage: f32, rail_voltage:f32, charge: bool, kick_type: KickType, kick_speed: f32) -> Result<(), ()> { // latch an error for invalid battery voltage if battery_voltage > VBATT_OVERVOLTAGE_LOCKOUT || battery_voltage < VBATT_UNDERVOLTAGE_LOCKOUT { self.error_latched = true; @@ -88,8 +90,10 @@ impl<'a> KickManager<'a> { return Err(()); } - // bound kick strength - let kick_strength = fmaxf(fminf(kick_strength, 1.0), 0.0); + // set charge duration via mapping from kick speed + let kick_speed_map = LinearMap::new(0f32, 5.5f32); // Max kick speed is approx 5.5m/s + let kick_duration_map = LinearMap::new(0f32, MAX_KICK_DURATION_US); + let charge_duration_us: f32 = kick_speed_map.linear_map_to_new_bounds(kick_speed, kick_duration_map); // handle charge, kick, and chip match kick_type { @@ -104,7 +108,7 @@ impl<'a> KickManager<'a> { // beign kick, wait time to determine power self.kick_pin.set_high(); - Timer::after(Duration::from_micros((MAX_KICK_DURATION_US * kick_strength) as u64)).await; + Timer::after(Duration::from_micros(charge_duration_us as u64)).await; // end kick self.kick_pin.set_low(); @@ -129,7 +133,7 @@ impl<'a> KickManager<'a> { // begin chip, wait time to determine power self.chip_pin.set_high(); - Timer::after(Duration::from_micros((MAX_CHIP_DURATION_US * kick_strength) as u64)).await; + Timer::after(Duration::from_micros((MAX_CHIP_DURATION_US * kick_speed) as u64)).await; // end chip self.chip_pin.set_low(); From c9611dfe323c2bcda075ac26bc8ef68c8fd7a8f7 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 9 Jul 2024 23:08:37 -0400 Subject: [PATCH 115/157] improve hwtests --- control-board/src/bin/control/main.rs | 10 ++++---- control-board/src/bin/hwtest-motor/main.rs | 25 +++++++++++++++++-- control-board/src/tasks/control_task.rs | 15 ++++++++++- kicker-board/src/bin/hwtest-breakbeam/main.rs | 19 ++++++++------ 4 files changed, 53 insertions(+), 16 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index ddf151cd..a6d9012d 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -10,11 +10,7 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; use ateam_control_board::{ - create_imu_task, create_io_task, create_radio_task, create_shutdown_task, - get_system_config, - pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, - robot_state::SharedRobotState, - tasks::{control_task::start_control_task, kicker_task::start_kicker_task}}; + create_audio_task, create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{control_task::start_control_task, kicker_task::start_kicker_task}}; // load credentials from correct crate #[cfg(not(feature = "no-private-credentials"))] @@ -105,6 +101,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); + create_audio_task!(main_spawner, + robot_state, + p); + create_radio_task!(main_spawner, radio_uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index f5b8eca6..098ce6ed 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -1,6 +1,7 @@ #![no_std] #![no_main] +use ateam_common_packets::{bindings_kicker::KickRequest, bindings_radio::{BasicControl, BasicTelemetry}, radio::DataPacket}; use embassy_executor::InterruptExecutor; use embassy_stm32::{ interrupt, pac::Interrupt @@ -9,7 +10,7 @@ use embassy_sync::pubsub::PubSubChannel; use defmt_rtt as _; -use ateam_control_board::{create_io_task, create_shutdown_task, get_system_config, pins::{BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; +use ateam_control_board::{create_imu_task, create_io_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; use embassy_time::Timer; // provide embedded panic probe @@ -21,6 +22,7 @@ static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(Sha static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); @@ -61,6 +63,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // commands channel let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + let test_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); // telemetry channel let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); @@ -69,8 +72,11 @@ async fn main(main_spawner: embassy_executor::Spawner) { let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); // TODO imu channel + let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + /////////////////// // start tasks // /////////////////// @@ -84,6 +90,11 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); + create_imu_task!(main_spawner, + robot_state, + imu_gyro_data_publisher, imu_accel_data_publisher, + p); + start_control_task( uart_queue_spawner, main_spawner, robot_state, @@ -94,7 +105,17 @@ async fn main(main_spawner: embassy_executor::Spawner) { p.USART1, p.PB15, p.PB14, p.DMA1_CH1, p.DMA1_CH0, p.PD8, p.PD9, p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; + loop { - Timer::after_millis(10).await; + Timer::after_millis(100).await; + + test_command_publisher.publish(DataPacket::BasicControl(BasicControl { + vel_x_linear: 0.2, + vel_y_linear: 0.0, + vel_z_angular: 0.0, + kick_vel: 0.0, + dribbler_speed: -0.1, + kick_request: KickRequest::KR_DISABLE, + })).await; } } \ No newline at end of file diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 0528c9dc..08ad6b9f 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -285,12 +285,13 @@ impl < let battery_v = 25.0; let controls_enabled = true; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; - // defmt::warn!("gyro rads: {}", gyro_rads); + defmt::warn!("gyro rads: {}", gyro_rads); let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { // TODO check order self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) } else { // Battery is too low, set velocity to zero + drib_vel = 0.0; Vector4::new(0.0, 0.0, 0.0, 0.0) }; @@ -312,35 +313,47 @@ impl < async fn flash_motor_firmware(&mut self, debug: bool) { defmt::info!("flashing firmware"); if debug { + let mut had_motor_error = false; if self.motor_fl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FL"); + had_motor_error = true; } else { defmt::info!("FL flashed"); } if self.motor_bl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash BL"); + had_motor_error = true; } else { defmt::info!("BL flashed"); } if self.motor_br.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash BR"); + had_motor_error = true; } else { defmt::info!("BR flashed"); } if self.motor_fr.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FR"); + had_motor_error = true; } else { defmt::info!("FR flashed"); } if self.motor_drib.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash DRIB"); + had_motor_error = true; } else { defmt::info!("DRIB flashed"); } + + if had_motor_error { + defmt::error!("one or more motors failed to flash.") + } else { + defmt::debug!("all motors flashed"); + } } else { let _res = embassy_futures::join::join5( self.motor_fl.load_default_firmware_image(), diff --git a/kicker-board/src/bin/hwtest-breakbeam/main.rs b/kicker-board/src/bin/hwtest-breakbeam/main.rs index a030a868..b8c37182 100644 --- a/kicker-board/src/bin/hwtest-breakbeam/main.rs +++ b/kicker-board/src/bin/hwtest-breakbeam/main.rs @@ -18,18 +18,19 @@ async fn main(_spawner: Spawner) { let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); let p = embassy_stm32::init(sys_cfg); - let _charge_pin = Output::new(p.PB3, Level::Low, Speed::Medium); - let _kick_pin = Output::new(p.PB0, Level::Low, Speed::Medium); - let _chip_pin = Output::new(p.PB1, Level::Low, Speed::Medium); + let _charge_pin = Output::new(p.PE4, Level::Low, Speed::Medium); + let _kick_pin = Output::new(p.PE5, Level::Low, Speed::Medium); + let _chip_pin = Output::new(p.PE6, Level::Low, Speed::Medium); info!("breakbeam startup!"); - let mut status_led_green = Output::new(p.PA11, Level::Low, Speed::Medium); - let mut status_led_blue = Output::new(p.PA8, Level::Low, Speed::Medium); + let mut status_led_green = Output::new(p.PE0, Level::Low, Speed::Medium); + let mut ball_detected_led1 = Output::new(p.PE2, Level::Low, Speed::Low); + let mut ball_detected_led2 = Output::new(p.PE3, Level::Low, Speed::Low); // Breakbeam // nets on schematic are inverted to silkscreen, sorry :/ -Will - let mut breakbeam = Breakbeam::new(p.PA3, p.PA2); + let mut breakbeam = Breakbeam::new(p.PA1, p.PA0); status_led_green.set_high(); Timer::after(Duration::from_millis(250)).await; @@ -47,11 +48,13 @@ async fn main(_spawner: Spawner) { // Timer::after(Duration::from_millis(100)).await; if breakbeam.read() { - status_led_blue.set_high(); + ball_detected_led1.set_high(); + ball_detected_led2.set_high(); } else { - status_led_blue.set_low(); + ball_detected_led1.set_low(); + ball_detected_led2.set_low(); } // Timer::after(Duration::from_millis(1000)).await; From 3296fd40ef09055525da9be87b10a6cc7d306442 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 10 Jul 2024 20:46:15 -0400 Subject: [PATCH 116/157] reduce log level --- kicker-board/.cargo/config.toml | 2 +- lib-stm32/.cargo/config.toml | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/kicker-board/.cargo/config.toml b/kicker-board/.cargo/config.toml index 2c1b4189..bdda3e75 100644 --- a/kicker-board/.cargo/config.toml +++ b/kicker-board/.cargo/config.toml @@ -6,4 +6,4 @@ runner = 'probe-rs run --chip STM32F407VETx --connect-under-reset' # runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' [env] -DEFMT_LOG = "trace" +DEFMT_LOG = "debug" diff --git a/lib-stm32/.cargo/config.toml b/lib-stm32/.cargo/config.toml index 5d10902a..8f1f6b57 100644 --- a/lib-stm32/.cargo/config.toml +++ b/lib-stm32/.cargo/config.toml @@ -1,2 +1,5 @@ [build] target = "thumbv7em-none-eabihf" + +[env] +DEFMT_LOG = "debug" From 3a61dba74a71c89cf6b18e3862d32c9e4f91b1c4 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Wed, 10 Jul 2024 18:20:02 -0700 Subject: [PATCH 117/157] Update software-communication --- software-communication | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/software-communication b/software-communication index 099134d3..d337d483 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit 099134d3d866954aaeecd967ec515a73c54fa634 +Subproject commit d337d48321aa453d15c9a93d8b5482c3b4213889 From fdc1aa78a2f0d049646ae021ae731e27ddc317f2 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Thu, 11 Jul 2024 00:56:35 -0400 Subject: [PATCH 118/157] fix dribbler compile --- motor-controller/bin/dribbler/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index fe4c13be..e2b5dd54 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -189,7 +189,7 @@ int main() { float r = r_motor_board; - float torque_setpoint = pid_calculate(&torque_pid, r, cur_measurement); + float torque_setpoint = pid_calculate(&torque_pid, r, cur_measurement, TORQUE_LOOP_RATE_S); // TODO: transform desired torque to current u_torque_loop = torque_setpoint; From e4521ff3a06ff85ce466fafe50dfb33342975521 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Wed, 10 Jul 2024 22:11:56 -0700 Subject: [PATCH 119/157] Battery updates, tipped filtering --- control-board/src/lib.rs | 10 ++--- control-board/src/robot_state.rs | 42 ++++++++++++++------- control-board/src/tasks/audio_task.rs | 4 +- control-board/src/tasks/imu_task.rs | 28 ++++++++++++-- control-board/src/tasks/user_io_task.rs | 45 +++++++++++++++++------ lib-stm32/src/drivers/other/adc_helper.rs | 8 +--- 6 files changed, 94 insertions(+), 43 deletions(-) diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index cffc5169..f203fc0f 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -73,17 +73,15 @@ macro_rules! include_kicker_bin { } } -pub const BATTERY_MIN_VOLTAGE: f32 = 19.0; +pub const BATTERY_MIN_SAFE_VOLTAGE: f32 = 21.0; +pub const BATTERY_MIN_CRIT_VOLTAGE: f32 = 19.5; pub const BATTERY_MAX_VOLTAGE: f32 = 25.2; pub const BATTERY_BUFFER_SIZE: usize = 10; pub const ADC_VREFINT_NOMINAL: f32 = 1050.0; // mV - -pub const fn adc_raw_to_v(adc_raw: f32) -> f32 { - adc_raw / ADC_VREFINT_NOMINAL -} +pub const ADC_TO_BATTERY_DIVIDER: f32 = (130_000.0 + 11_000.0) / 11_000.0; pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { - (adc_v / 1.966) * BATTERY_MAX_VOLTAGE + adc_v * ADC_TO_BATTERY_DIVIDER } pub fn get_system_config() -> Config { diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index e188c354..33ba0cf4 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -20,8 +20,8 @@ pub struct SharedRobotState { last_packet_receive_time_ticks_ms: AtomicU32, radio_connection_ok: AtomicBool, - battery_pct: AtomicU8, - battery_ok: AtomicBool, + battery_low: AtomicBool, + battery_crit: AtomicBool, robot_tipped: AtomicBool, @@ -46,8 +46,8 @@ impl SharedRobotState { dribbler_inop: AtomicBool::new(true), last_packet_receive_time_ticks_ms: AtomicU32::new(0), radio_connection_ok: AtomicBool::new(false), - battery_pct: AtomicU8::new(0), - battery_ok: AtomicBool::new(false), + battery_low: AtomicBool::new(false), + battery_crit: AtomicBool::new(false), robot_tipped: AtomicBool::new(false), shutdown_requested: AtomicBool::new(false), ball_detected: AtomicBool::new(false), @@ -64,7 +64,7 @@ impl SharedRobotState { hw_debug_mode: self.hw_in_debug_mode(), radio_inop: true, - imu_inop: true, + imu_inop: self.get_imu_inop(), kicker_inop: true, wheels_inop: 0xFF, dribbler_inop: true, @@ -72,8 +72,8 @@ impl SharedRobotState { last_packet_receive_time_ticks_ms: 0, radio_connection_ok: false, - battery_pct: self.battery_pct(), - battery_ok: false, + battery_low: self.get_battery_low(), + battery_crit: self.get_battery_crit(), robot_tipped: self.robot_tipped(), @@ -131,6 +131,14 @@ impl SharedRobotState { self.robot_tipped.store(tipped, Ordering::Relaxed); } + pub fn get_imu_inop(&self) -> bool { + self.imu_inop.load(Ordering::Relaxed) + } + + pub fn set_imu_inop(&self, imu_inop: bool) { + self.imu_inop.store(imu_inop, Ordering::Relaxed); + } + pub fn shutdown_requested(&self) -> bool { self.shutdown_requested.load(Ordering::Relaxed) } @@ -139,12 +147,20 @@ impl SharedRobotState { self.shutdown_requested.store(true, Ordering::Relaxed); } - pub fn battery_pct(&self) -> u8 { - self.battery_pct.load(Ordering::Relaxed) + pub fn get_battery_low(&self) -> bool { + self.battery_low.load(Ordering::Relaxed) + } + + pub fn set_battery_low(&self, battery_low: bool) { + self.battery_low.store(battery_low, Ordering::Relaxed); + } + + pub fn get_battery_crit(&self) -> bool { + self.battery_crit.load(Ordering::Relaxed) } - pub fn set_battery_pct(&self, battery_pct: u8) { - self.battery_pct.store(battery_pct, Ordering::Relaxed); + pub fn set_battery_crit(&self, battery_crit: bool) { + self.battery_crit.store(battery_crit, Ordering::Relaxed); } pub fn radio_connection_ok(&self) -> bool { @@ -190,8 +206,8 @@ pub struct RobotState { pub last_packet_receive_time_ticks_ms: u32, pub radio_connection_ok: bool, - pub battery_pct: u8, - pub battery_ok: bool, + pub battery_low: bool, + pub battery_crit: bool, pub robot_tipped: bool, diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index 78a5916f..683c8988 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -30,9 +30,7 @@ async fn audio_task_entry( tone_player.play_song().await; } - defmt::info!("battery pct {}", cur_robot_state.battery_pct); - if cur_robot_state.battery_pct == 0 || cur_robot_state.battery_pct > 110 { - defmt::warn!("robot critical battery pct {}", cur_robot_state.battery_pct); + if cur_robot_state.battery_low { let _ = tone_player.load_song(&BATTERY_WARNING_SONG); tone_player.play_song().await; } diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index d3432599..800e2b2c 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -5,7 +5,7 @@ use embassy_stm32::exti::ExtiInput; use embassy_stm32::gpio::Pull; use embassy_stm32::spi::{SckPin, MisoPin, MosiPin}; -use embassy_time::Timer; +use embassy_time::{Duration, Instant, Timer}; use nalgebra::Vector3; use static_cell::ConstStaticCell; @@ -15,6 +15,8 @@ use ateam_lib_stm32::drivers::imu::bmi323::{self, *}; use crate::pins::*; use crate::robot_state::SharedRobotState; +const TIPPED_MIN_DURATION_MS: u64 = 1000; + #[macro_export] macro_rules! create_imu_task { ($main_spawner:ident, $robot_state:ident, $imu_gyro_data_publisher:ident, $imu_accel_data_publisher:ident, $p:ident) => { @@ -46,9 +48,13 @@ async fn imu_task_entry( mut gyro_int: ExtiInput<'static>) { defmt::info!("imu start startup."); + let mut first_tipped_check_time = Instant::now(); + let mut first_tipped_seen = false; 'imu_configuration_loop: loop { + // At the beginning, assume IMU is not working yet. + robot_state.set_imu_inop(true); imu.init().await; let self_test_res = imu.self_test().await; if self_test_res.is_err() { @@ -62,7 +68,6 @@ async fn imu_task_entry( GyroRange::PlusMinus2000DegPerSec, Bandwidth3DbCutoffFreq::AccOdrOver2, OutputDataRate::Odr100p0, - DataAveragingWindow::Average2Samples).await; imu.set_gyro_interrupt_mode(InterruptMode::MappedToInt2).await; @@ -94,6 +99,9 @@ async fn imu_task_entry( // block on gyro interrupt, active low match select(gyro_int.wait_for_falling_edge(), Timer::after_millis(1000)).await { Either::First(_) => { + // Got an interrupt, so IMU should be working. + robot_state.set_imu_inop(false); + // read gyro data let imu_data = imu.gyro_get_data_rads().await; gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2])); @@ -105,8 +113,22 @@ async fn imu_task_entry( // TODO: magic number, fix after raw data conversion if accel_data[2] < 8000 { - robot_state.set_robot_tipped(true); + if !first_tipped_seen { + // If it's the first time a tipping occured, start tracking. + first_tipped_seen = true; + first_tipped_check_time = Instant::now(); + } else { + // After the first tipped is seen, then wait if it has been tipped for long enough. + if (Instant::now() - first_tipped_check_time) > Duration::from_millis(TIPPED_MIN_DURATION_MS) { + robot_state.set_robot_tipped(true); + } else { + // If it hasn't been long enough, clear the robot tipped. + robot_state.set_robot_tipped(false); + } + } } else { + // Not tipped so clear everything. + first_tipped_seen = false; robot_state.set_robot_tipped(false); } } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index bf59c848..58d8b1d6 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -21,7 +21,7 @@ use embassy_stm32::adc::{Adc, SampleTime, Resolution}; use crate::drivers::shell_indicator::ShellIndicator; use crate::robot_state::SharedRobotState; -use crate::{adc_v_to_battery_voltage, pins::*, stm32_interface}; +use crate::{adc_v_to_battery_voltage, pins::*, stm32_interface, BATTERY_MIN_CRIT_VOLTAGE, BATTERY_MIN_SAFE_VOLTAGE, BATTERY_MAX_VOLTAGE, BATTERY_BUFFER_SIZE}; use crate::tasks::shutdown_task::HARD_SHUTDOWN_TIME_MS; // #[link_section = ".sram4"] @@ -137,6 +137,11 @@ async fn user_io_task_entry( // blink_anim.start_animation(); // dotstars.set_animation(blink_anim, 1); + let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = + [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; + + let mut battery_voltage_filt_indx = 0; + let mut prev_robot_state = robot_state.get_state(); loop { let cur_robot_state = robot_state.get_state(); @@ -170,18 +175,34 @@ async fn user_io_task_entry( robot_state.set_hw_network_index(robot_network_index); defmt::info!("updated robot network index {} -> {}", cur_robot_state.hw_wifi_network_index, robot_network_index); } - - let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); + + /////////////////////// + // Battery reading // + /////////////////////// + + // FIXME: Vref_int is not returning valid value. Embassy issue. + // let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); + let vref_int_read_mv = 1509.0; let batt_res_div_v = battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal); - let battery_voltage = adc_v_to_battery_voltage(batt_res_div_v); - // publish votlage - battery_volt_publisher.publish_immediate(battery_voltage); - // set pct in state - const LIPO_MIN_VOLTAGE: f32 = 6.0 * 3.2; - const LIPO_MAX_VOLTAGE: f32 = 6.0 * 4.2; - const LIPO_VOTLAGE_RANGE: f32 = LIPO_MAX_VOLTAGE - LIPO_MIN_VOLTAGE; - let battery_pct = (((battery_voltage - LIPO_MIN_VOLTAGE) / LIPO_VOTLAGE_RANGE) * 100.0) as u8; - robot_state.set_battery_pct(battery_pct); + let battery_voltage_cur = adc_v_to_battery_voltage(batt_res_div_v); + + // Add new battery read to cyclical buffer. + battery_voltage_buffer[battery_voltage_filt_indx] = battery_voltage_cur; + + // Shift index for next run. + if battery_voltage_filt_indx == (BATTERY_BUFFER_SIZE - 1) { + battery_voltage_filt_indx = 0; + } else { + battery_voltage_filt_indx += 1; + } + + let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); + // Calculate battery average + let battery_voltage_ave = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); + + battery_volt_publisher.publish_immediate(battery_voltage_ave); + robot_state.set_battery_low(battery_voltage_ave < BATTERY_MIN_SAFE_VOLTAGE); + robot_state.set_battery_crit(battery_voltage_ave < BATTERY_MIN_CRIT_VOLTAGE || battery_voltage_ave > BATTERY_MAX_VOLTAGE); // TODO read messages diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index eb233d63..7496c227 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -47,9 +47,9 @@ impl< } } - // vref_int_mv has to be passed in because the ADC peripheral that + // vref_int_read_mv has to be passed in because the ADC peripheral that // it is connected to depends on the chip. - pub fn read_volt_raw_f32(&mut self, mut vref_int_read_mv: f32, vref_int_cal: f32) -> f32 { + pub fn read_volt_raw_f32(&mut self, vref_int_read_mv: f32, vref_int_cal: f32) -> f32 { // Based off of this: http://www.efton.sk/STM32/STM32_VREF.pdf // vmeas = vcal * MEAS / MAX * CAL / REFINT (4) @@ -59,10 +59,6 @@ impl< // defmt::info!("vref_int_cal: {}", vref_int_cal); // defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); - // FIXME, embassy API appears to not correctly load vref_int_read - // override for now - vref_int_read_mv = 1509.0; - return V_CAL_V * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32) * vref_int_cal / vref_int_read_mv; } } \ No newline at end of file From ce53b14c98d93ad6a82b6bf0bed0b77f784ae91a Mon Sep 17 00:00:00 2001 From: "H.M. Murdock" Date: Thu, 11 Jul 2024 21:47:46 -0400 Subject: [PATCH 120/157] Fix battery voltage pubsub --- control-board/src/bin/control/main.rs | 3 ++- control-board/src/lib.rs | 5 ++--- control-board/src/pins.rs | 1 + control-board/src/tasks/control_task.rs | 10 ++++++---- control-board/src/tasks/user_io_task.rs | 4 ++-- lib-stm32/src/drivers/other/adc_helper.rs | 2 +- 6 files changed, 14 insertions(+), 11 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index a6d9012d..7617e217 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -81,6 +81,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // Battery Channel let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + let battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); // TODO imu channel let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); @@ -119,7 +120,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { start_control_task( uart_queue_spawner, main_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, control_gyro_data_subscriber, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index f203fc0f..84ac29dc 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -76,12 +76,11 @@ macro_rules! include_kicker_bin { pub const BATTERY_MIN_SAFE_VOLTAGE: f32 = 21.0; pub const BATTERY_MIN_CRIT_VOLTAGE: f32 = 19.5; pub const BATTERY_MAX_VOLTAGE: f32 = 25.2; -pub const BATTERY_BUFFER_SIZE: usize = 10; -pub const ADC_VREFINT_NOMINAL: f32 = 1050.0; // mV +pub const BATTERY_BUFFER_SIZE: usize = 20; pub const ADC_TO_BATTERY_DIVIDER: f32 = (130_000.0 + 11_000.0) / 11_000.0; pub const fn adc_v_to_battery_voltage(adc_v: f32) -> f32 { - adc_v * ADC_TO_BATTERY_DIVIDER + (adc_v * ADC_TO_BATTERY_DIVIDER) - 0.5 // Conservatively offset the battery voltage } pub fn get_system_config() -> Config { diff --git a/control-board/src/pins.rs b/control-board/src/pins.rs index fc8c1c0f..a4073ef8 100644 --- a/control-board/src/pins.rs +++ b/control-board/src/pins.rs @@ -33,6 +33,7 @@ pub type AccelDataSubscriber = Subscriber<'static, ThreadModeRawMutex, nalgebra: pub type BatteryVoltPubSub = PubSubChannel; pub type BatteryVoltPublisher = Publisher<'static, ThreadModeRawMutex, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; +pub type BatteryVoltSubscriber = Subscriber<'static, ThreadModeRawMutex, f32, BATTERY_VOLT_PUBSUB_DEPTH, 1, 1>; ///////////// // Radio // diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 08ad6b9f..02849abe 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -57,6 +57,7 @@ pub struct ControlTask< const TX_BUF_DEPTH: usize> { shared_robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, + battery_subscriber: BatteryVoltSubscriber, gyro_subscriber: GyroDataSubscriber, telemetry_publisher: TelemetryPublisher, @@ -78,6 +79,7 @@ impl < pub fn new(robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, + battery_subscriber: BatteryVoltSubscriber, gyro_subscriber: GyroDataSubscriber, motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -89,6 +91,7 @@ impl < shared_robot_state: robot_state, command_subscriber: command_subscriber, telemetry_publisher: telemetry_publisher, + battery_subscriber: battery_subscriber, gyro_subscriber: gyro_subscriber, motor_fl: motor_fl, motor_bl: motor_bl, @@ -281,11 +284,9 @@ impl < } // now we have setpoint r(t) in self.cmd_vel - // let battery_v = battery_sub.next_message_pure().await as f32; - let battery_v = 25.0; + let battery_v = self.battery_subscriber.next_message_pure().await as f32; let controls_enabled = true; let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; - defmt::warn!("gyro rads: {}", gyro_rads); let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { // TODO check order self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) @@ -410,6 +411,7 @@ pub async fn start_control_task( robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, telemetry_publisher: TelemetryPublisher, + battery_subscriber: BatteryVoltSubscriber, gyro_subscriber: GyroDataSubscriber, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin, @@ -457,7 +459,7 @@ pub async fn start_control_task( let motor_drib = DribblerMotor::new_from_pins(&DRIB_RX_UART_QUEUE, &DRIB_TX_UART_QUEUE, motor_d_boot0_pin, motor_d_nrst_pin, DRIB_FW_IMG, 1.0); let control_task = ControlTask::new( - robot_state, command_subscriber, telemetry_publisher, + robot_state, command_subscriber, telemetry_publisher, battery_subscriber, gyro_subscriber, motor_fl, motor_bl, motor_br, motor_fr, motor_drib); control_task_spawner.spawn(control_task_entry(control_task)).unwrap(); diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 58d8b1d6..8ee2a027 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -182,7 +182,7 @@ async fn user_io_task_entry( // FIXME: Vref_int is not returning valid value. Embassy issue. // let vref_int_read_mv = vref_int_adc.read(&mut vref_int_ch); - let vref_int_read_mv = 1509.0; + let vref_int_read_mv = 1216.0; let batt_res_div_v = battery_volt_adc.read_volt_raw_f32(vref_int_read_mv as f32, vref_int_cal); let battery_voltage_cur = adc_v_to_battery_voltage(batt_res_div_v); @@ -199,7 +199,7 @@ async fn user_io_task_entry( let battery_voltage_sum: f32 = battery_voltage_buffer.iter().sum(); // Calculate battery average let battery_voltage_ave = battery_voltage_sum / (BATTERY_BUFFER_SIZE as f32); - + battery_volt_publisher.publish_immediate(battery_voltage_ave); robot_state.set_battery_low(battery_voltage_ave < BATTERY_MIN_SAFE_VOLTAGE); robot_state.set_battery_crit(battery_voltage_ave < BATTERY_MIN_CRIT_VOLTAGE || battery_voltage_ave > BATTERY_MAX_VOLTAGE); diff --git a/lib-stm32/src/drivers/other/adc_helper.rs b/lib-stm32/src/drivers/other/adc_helper.rs index 7496c227..09f98d90 100644 --- a/lib-stm32/src/drivers/other/adc_helper.rs +++ b/lib-stm32/src/drivers/other/adc_helper.rs @@ -59,6 +59,6 @@ impl< // defmt::info!("vref_int_cal: {}", vref_int_cal); // defmt::info!("vref_int_read_mv: {}", vref_int_read_mv); - return V_CAL_V * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32) * vref_int_cal / vref_int_read_mv; + return V_CAL_V * (self.inst.read(&mut self.pin) as f32) / (self.adc_bins as f32); // * vref_int_cal / vref_int_read_mv; } } \ No newline at end of file From f1fc2e40eb2edd0d93f840bb32702ef59db2dcaa Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Fri, 12 Jul 2024 21:54:54 -0400 Subject: [PATCH 121/157] fix control task loop rate, fix color display of shell ID --- control-board/src/tasks/control_task.rs | 26 ++++++++++++++++++------- control-board/src/tasks/user_io_task.rs | 2 +- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 02849abe..a8f3ce60 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -58,7 +58,9 @@ pub struct ControlTask< shared_robot_state: &'static SharedRobotState, command_subscriber: CommandsSubscriber, battery_subscriber: BatteryVoltSubscriber, + last_battery_v: f32, gyro_subscriber: GyroDataSubscriber, + last_gyro_rads: f32, telemetry_publisher: TelemetryPublisher, motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -92,7 +94,9 @@ impl < command_subscriber: command_subscriber, telemetry_publisher: telemetry_publisher, battery_subscriber: battery_subscriber, + last_battery_v: 0.0, gyro_subscriber: gyro_subscriber, + last_gyro_rads: 0.0, motor_fl: motor_fl, motor_bl: motor_bl, motor_br: motor_br, @@ -137,7 +141,7 @@ impl < fn send_motor_commands_and_telemetry(&mut self, robot_controller: &mut BodyVelocityController, - battery_voltage: &f32) + battery_voltage: f32) { self.motor_fl.send_motion_command(); self.motor_bl.send_motion_command(); @@ -161,7 +165,7 @@ impl < sequence_number: 0, robot_revision_major: 0, robot_revision_minor: 0, - battery_level: *battery_voltage, + battery_level: battery_voltage, battery_temperature: 0., _bitfield_align_1: [], _bitfield_1: BasicTelemetry::new_bitfield_1( @@ -284,12 +288,20 @@ impl < } // now we have setpoint r(t) in self.cmd_vel - let battery_v = self.battery_subscriber.next_message_pure().await as f32; + // let battery_v = self.battery_subscriber.next_message_pure().await as f32; + if let Some(battery_v) = self.battery_subscriber.try_next_message_pure() { + self.last_battery_v = battery_v; + } let controls_enabled = true; - let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; - let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { + // let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; + if let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { + self.last_gyro_rads = gyro_rads[2]; + } + + + let wheel_vels = if self.last_battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { // TODO check order - self.do_control_update(&mut robot_controller, cmd_vel, gyro_rads, controls_enabled) + self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_rads, controls_enabled) } else { // Battery is too low, set velocity to zero drib_vel = 0.0; @@ -305,7 +317,7 @@ impl < self.motor_drib.set_setpoint(drib_dc); self.send_motor_commands_and_telemetry( - &mut robot_controller, &battery_v); + &mut robot_controller, self.last_battery_v); loop_rate_ticker.next().await; } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 8ee2a027..0f71fcab 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -275,7 +275,7 @@ pub async fn start_io_task(spawner: &Spawner, let debug_led0 = Output::new(usr_led0_pin, Level::Low, Speed::Low); - let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_br, robot_id_indicator_bl, Some(robot_id_indicator_isblue)); + let robot_id_indicator = ShellIndicator::new(robot_id_indicator_fr, robot_id_indicator_fl, robot_id_indicator_bl, robot_id_indicator_br, Some(robot_id_indicator_isblue)); let battery_volt_adc = AdcHelper::new(battery_adc_peri, battery_adc_pin, SampleTime::CYCLES32_5, Resolution::BITS12); let mut vref_int_adc = Adc::new(vref_int_adc_peri); From cb1f9fd5dbdd5d18a9bb857cb1996d38c89434af Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 14 Jul 2024 16:13:20 -0400 Subject: [PATCH 122/157] update log level --- control-board/src/bin/hwtest-motor/main.rs | 3 ++- lib-stm32-test/.cargo/config.toml | 2 +- lib-stm32/.cargo/config.toml | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 098ce6ed..63326969 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -70,6 +70,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { // Battery Channel let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + let battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); // TODO imu channel let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); @@ -98,7 +99,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { start_control_task( uart_queue_spawner, main_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, control_gyro_data_subscriber, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, diff --git a/lib-stm32-test/.cargo/config.toml b/lib-stm32-test/.cargo/config.toml index b56b74be..2f5bd941 100644 --- a/lib-stm32-test/.cargo/config.toml +++ b/lib-stm32-test/.cargo/config.toml @@ -6,4 +6,4 @@ runner = 'probe-rs run --chip STM32H723ZGTx --connect-under-reset' # runner = 'probe-rs debug --chip STM32F407VETx --connect-under-reset --exe' [env] -DEFMT_LOG = "trace" +DEFMT_LOG = "debug" diff --git a/lib-stm32/.cargo/config.toml b/lib-stm32/.cargo/config.toml index 8f1f6b57..bd102333 100644 --- a/lib-stm32/.cargo/config.toml +++ b/lib-stm32/.cargo/config.toml @@ -2,4 +2,4 @@ target = "thumbv7em-none-eabihf" [env] -DEFMT_LOG = "debug" +DEFMT_LOG = "warn" From 82a35ec8b8ae04a65e4bafe4d309464160ae7a7d Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 14 Jul 2024 16:36:28 -0400 Subject: [PATCH 123/157] add voltage rail filtering --- kicker-board/src/bin/kicker/main.rs | 46 +++++++++++++++++++++-------- kicker-board/src/kick_manager.rs | 3 +- 2 files changed, 36 insertions(+), 13 deletions(-) diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index e4a1d006..2d39fad4 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -44,7 +44,7 @@ const MAX_KICK_SPEED: f32 = 5.5; const SHUTDOWN_KICK_DUTY: f32 = 0.20; pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; -pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; +pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 195.0; pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; @@ -53,6 +53,8 @@ const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 3; +const RAIL_BUFFER_SIZE: usize = 10; + make_uart_queue_pair!(COMS, ComsUartModule, ComsUartRxDma, ComsUartTxDma, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, @@ -132,6 +134,10 @@ async fn high_pri_kick_task( let mut shutdown_requested: bool = false; let mut shutdown_completed: bool = false; + let mut rail_voltage_buffer: [f32; RAIL_BUFFER_SIZE] = + [0.0; RAIL_BUFFER_SIZE]; + let mut rail_voltage_filt_indx: usize = 0; + // loop rate control let mut ticker = Ticker::every(Duration::from_millis(1)); let mut last_packet_sent_time = Instant::now(); @@ -141,7 +147,22 @@ async fn high_pri_kick_task( let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.read(&mut vrefint); - let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + let rail_voltage_cur = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); + + // Add new battery read to cyclical buffer. + rail_voltage_buffer[rail_voltage_filt_indx] = rail_voltage_cur; + + // Shift index for next run. + if rail_voltage_filt_indx == (RAIL_BUFFER_SIZE - 1) { + rail_voltage_filt_indx = 0; + } else { + rail_voltage_filt_indx += 1; + } + + let rail_voltage_sum: f32 = rail_voltage_buffer.iter().sum(); + // Calculate battery average + let rail_voltage_ave = rail_voltage_sum / (RAIL_BUFFER_SIZE as f32); + // let battery_voltage = // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); // optionally pre-flag errors? @@ -185,11 +206,11 @@ async fn high_pri_kick_task( } // check if we've met the criteria for completed shutdown - if shutdown_requested && rail_voltage < CHARGE_SAFE_VOLTAGE { + if shutdown_requested && rail_voltage_ave < CHARGE_SAFE_VOLTAGE { shutdown_completed = true; } - if rail_voltage > CHARGE_OVERVOLT_THRESH_VOLTAGE { + if rail_voltage_ave > CHARGE_OVERVOLT_THRESH_VOLTAGE { error_latched = true; } @@ -252,7 +273,7 @@ async fn high_pri_kick_task( } } KickRequest::KR_KICK_CAPTURED => { - if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { + if ball_detected && rail_voltage_ave > CHARGED_THRESH_VOLTAGE { KickType::Kick } else { KickType::None @@ -267,7 +288,7 @@ async fn high_pri_kick_task( } } KickRequest::KR_CHIP_CAPTURED => { - if ball_detected && rail_voltage > CHARGED_THRESH_VOLTAGE { + if ball_detected && rail_voltage_ave > CHARGED_THRESH_VOLTAGE { KickType::Chip } else { KickType::None @@ -289,7 +310,7 @@ async fn high_pri_kick_task( // if telemetry isn't enabled, the control board doesn't want to talk to us, don't permit any actions let res = if !telemetry_enabled || error_latched { kick_manager - .command(battery_voltage, rail_voltage, false, KickType::None, 0.0) + .command(battery_voltage, rail_voltage_ave, false, KickType::None, 0.0) .await } else { if kick_command == KickType::Kick || kick_command == KickType::Chip { @@ -298,10 +319,11 @@ async fn high_pri_kick_task( false => KickRequest::KR_DISABLE, } } + kick_manager .command( battery_voltage, - rail_voltage, + rail_voltage_ave, charge_hv_rail, kick_command, kick_speed @@ -312,9 +334,9 @@ async fn high_pri_kick_task( // this will permanently latch an error if the rail voltages are low // which we probably don't want on boot up? // maybe this error should be clearable and the HV rail OV should not be - // if res.is_err() { - // error_latched = true; - // } + if res.is_err() { + error_latched = true; + } // send telemetry packet if telemetry_enabled { @@ -330,7 +352,7 @@ async fn high_pri_kick_task( ball_detected as u32, res.is_err() as u32, ); - kicker_telemetry_packet.rail_voltage = rail_voltage; + kicker_telemetry_packet.rail_voltage = rail_voltage_ave; kicker_telemetry_packet.battery_voltage = battery_voltage; // raw interpretaion of a struct for wire transmission is unsafe diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index 869a71c8..986c8c33 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -27,6 +27,7 @@ use embassy_time::{Duration, Timer}; use libm::{fmaxf, fminf}; use ateam_lib_stm32::math::linear::LinearMap; +const MIN_KICK_DURATION_US: f32 = 500.0; const MAX_KICK_DURATION_US: f32 = 4500.0; // 10ms (10k us) max power kick const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick @@ -92,7 +93,7 @@ impl<'a> KickManager<'a> { // set charge duration via mapping from kick speed let kick_speed_map = LinearMap::new(0f32, 5.5f32); // Max kick speed is approx 5.5m/s - let kick_duration_map = LinearMap::new(0f32, MAX_KICK_DURATION_US); + let kick_duration_map = LinearMap::new(MIN_KICK_DURATION_US, MAX_KICK_DURATION_US); let charge_duration_us: f32 = kick_speed_map.linear_map_to_new_bounds(kick_speed, kick_duration_map); // handle charge, kick, and chip From 764f810b18e035435a1487255314d0b1998eab91 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Mon, 15 Jul 2024 12:14:16 -0400 Subject: [PATCH 124/157] make kicker green led turn on when telemetry is enabled --- control-board/src/tasks/user_io_task.rs | 6 +++--- kicker-board/src/bin/kicker/main.rs | 13 +++++++++---- kicker-board/src/kick_manager.rs | 1 - 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 0f71fcab..313b6055 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -56,11 +56,11 @@ macro_rules! create_io_task { #[embassy_executor::task] async fn user_io_task_entry( robot_state: &'static SharedRobotState, - mut usr_btn0: AdvExtiButton, - mut usr_btn1: AdvExtiButton, + mut _usr_btn0: AdvExtiButton, + mut _usr_btn1: AdvExtiButton, battery_volt_publisher: BatteryVoltPublisher, mut battery_volt_adc: AdcHelper<'static, BatteryAdc, BatteryAdcPin>, - mut vref_int_adc: Adc<'static, VrefIntAdc>, + vref_int_adc: Adc<'static, VrefIntAdc>, dip_switch: DipSwitch<'static, 7>, robot_id_rotary: RotaryEncoder<'static, 4>, mut debug_led0: Output<'static>, diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 2d39fad4..88a8c50b 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -4,7 +4,7 @@ #![feature(const_mut_refs)] #![feature(sync_unsafe_cell)] -use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::{get_system_config, ClkSource}}; +use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin, GreenStatusLedPin}, tasks::{get_system_config, ClkSource}}; use defmt::*; use {defmt_rtt as _, panic_probe as _}; @@ -100,6 +100,7 @@ async fn high_pri_kick_task( breakbeam_tx: BreakbeamLeftAgpioPin, breakbeam_rx: BreakbeamRightAgpioPin, mut rail_pin: PowerRail200vReadPin, + grn_led_pin: GreenStatusLedPin, err_led_pin: RedStatusLedPin, ball_detected_led1_pin: BlueStatusLed1Pin, ball_detected_led2_pin: BlueStatusLed2Pin, @@ -111,6 +112,7 @@ async fn high_pri_kick_task( let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); // debug LEDs + let mut status_led = Output::new(grn_led_pin, Level::Low, Speed::Low); let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); @@ -199,6 +201,11 @@ async fn high_pri_kick_task( // update telemetry requests telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; + if telemetry_enabled { + status_led.set_high(); + } else { + status_led.set_low(); + } // for now shutdown requests will be latched and a reboot is required to re-power if kicker_control_packet.request_power_down() != 0 { @@ -412,8 +419,6 @@ async fn main(spawner: Spawner) -> ! { info!("kicker startup!"); - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - let mut adc = Adc::new(p.ADC1); adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); adc.set_sample_time(SampleTime::CYCLES480); @@ -424,7 +429,7 @@ async fn main(spawner: Spawner) -> ! { embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); let hp_spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(hp_spawner.spawn(high_pri_kick_task(&COMS_RX_UART_QUEUE, &COMS_TX_UART_QUEUE, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); + unwrap!(hp_spawner.spawn(high_pri_kick_task(&COMS_RX_UART_QUEUE, &COMS_TX_UART_QUEUE, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE0, p.PE1, p.PE2, p.PE3))); ////////////////////////////////// diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index 986c8c33..955158ea 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -24,7 +24,6 @@ use embassy_stm32::gpio::Output; use embassy_time::{Duration, Timer}; -use libm::{fmaxf, fminf}; use ateam_lib_stm32::math::linear::LinearMap; const MIN_KICK_DURATION_US: f32 = 500.0; From 20e932cbdc05479f5c645dd0d45224fadf38265c Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Mon, 15 Jul 2024 19:55:43 +0200 Subject: [PATCH 125/157] Joe/estop2 (#72) * Audio and flag watching fixes * Added prints, audio checked working * Kicker discharge finger awareness, clean up --------- Co-authored-by: Joe Spall --- control-board/src/songs.rs | 5 + control-board/src/tasks/audio_task.rs | 18 +- control-board/src/tasks/control_task.rs | 9 +- kicker-board/Cargo.toml | 5 - kicker-board/src/bin/hwtest-flash/main.rs | 2 +- kicker-board/src/bin/kicker-no-uart/main.rs | 209 -------------------- kicker-board/src/bin/kicker/main.rs | 13 +- 7 files changed, 31 insertions(+), 230 deletions(-) delete mode 100644 kicker-board/src/bin/kicker-no-uart/main.rs diff --git a/control-board/src/songs.rs b/control-board/src/songs.rs index 1be5c576..283bbfe2 100644 --- a/control-board/src/songs.rs +++ b/control-board/src/songs.rs @@ -28,6 +28,11 @@ pub const BATTERY_WARNING_SONG: [Beat; 4] = [ Beat::Rest(250_000), ]; +pub const BATTERY_CRITICAL_SONG: [Beat; 2] = [ + Beat::Note { tone: 440, duration: 250_000 }, + Beat::Note { tone: 466, duration: 250_000 }, +]; + pub const TEST_SONG: [Beat; 17] = [ Beat::Note { tone: 392, duration: 250_000 }, Beat::Note { tone: 392, duration: 250_000 }, diff --git a/control-board/src/tasks/audio_task.rs b/control-board/src/tasks/audio_task.rs index 683c8988..ff2d65c8 100644 --- a/control-board/src/tasks/audio_task.rs +++ b/control-board/src/tasks/audio_task.rs @@ -3,7 +3,7 @@ use embassy_executor::Spawner; use embassy_stm32::{gpio::OutputType, time::hz, timer::{simple_pwm::{PwmPin, SimplePwm}, Channel}}; use embassy_time::{Duration, Ticker}; -use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::{BATTERY_WARNING_SONG, TIPPED_WARNING_SONG}}; +use crate::{pins::{BuzzerPin, BuzzerTimer}, robot_state::SharedRobotState, songs::{BATTERY_CRITICAL_SONG, BATTERY_WARNING_SONG, TIPPED_WARNING_SONG}}; #[macro_export] macro_rules! create_audio_task { @@ -24,15 +24,19 @@ async fn audio_task_entry( loop { let cur_robot_state = robot_state.get_state(); - if cur_robot_state.robot_tipped { - defmt::warn!("robot tipped audio"); - let _ = tone_player.load_song(&TIPPED_WARNING_SONG); + // Structure so only one song can play per + if cur_robot_state.battery_crit { + defmt::warn!("battery critical"); + let _ = tone_player.load_song(&BATTERY_CRITICAL_SONG); tone_player.play_song().await; - } - - if cur_robot_state.battery_low { + } else if cur_robot_state.battery_low { + defmt::warn!("battery low"); let _ = tone_player.load_song(&BATTERY_WARNING_SONG); tone_player.play_song().await; + } else if cur_robot_state.robot_tipped { + defmt::warn!("robot tipped audio"); + let _ = tone_player.load_song(&TIPPED_WARNING_SONG); + tone_player.play_song().await; } audio_ticker.next().await; diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index a8f3ce60..56ca6f64 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -288,18 +288,17 @@ impl < } // now we have setpoint r(t) in self.cmd_vel - // let battery_v = self.battery_subscriber.next_message_pure().await as f32; if let Some(battery_v) = self.battery_subscriber.try_next_message_pure() { self.last_battery_v = battery_v; } - let controls_enabled = true; - // let gyro_rads = self.gyro_subscriber.next_message_pure().await[2] as f32; + if let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { self.last_gyro_rads = gyro_rads[2]; } + + let controls_enabled = true; - - let wheel_vels = if self.last_battery_v > BATTERY_MIN_VOLTAGE && !self.shared_robot_state.shutdown_requested() { + let wheel_vels = if !(self.shared_robot_state.get_battery_low() || self.shared_robot_state.get_battery_crit()) && !self.shared_robot_state.shutdown_requested() { // TODO check order self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_rads, controls_enabled) } else { diff --git a/kicker-board/Cargo.toml b/kicker-board/Cargo.toml index c12513b3..03125c00 100644 --- a/kicker-board/Cargo.toml +++ b/kicker-board/Cargo.toml @@ -95,11 +95,6 @@ name = "kicker" test = false harness = false -[[bin]] -name = "kicker-no-uart" -test = false -harness = false - [patch.crates-io] embassy-executor = { git = "https://github.com/embassy-rs/embassy"} embassy-sync = { git = "https://github.com/embassy-rs/embassy"} diff --git a/kicker-board/src/bin/hwtest-flash/main.rs b/kicker-board/src/bin/hwtest-flash/main.rs index 73e200af..7efd150f 100644 --- a/kicker-board/src/bin/hwtest-flash/main.rs +++ b/kicker-board/src/bin/hwtest-flash/main.rs @@ -41,7 +41,7 @@ async fn main(_spawner: Spawner) -> ! { let tx_buf = FLASH_TX_BUF.init([0; 256]); - let mut flash: AT25DF041B<'static, embassy_stm32::peripherals::SPI2, true> = AT25DF041B::new(spi, p.PB12, rx_buf, tx_buf); + let mut flash: AT25DF041B<'static, true> = AT25DF041B::new(spi, p.PB12, rx_buf, tx_buf); let res = flash.verify_chip_id().await; if res.is_err() { defmt::error!("failed to verify flash chip ID"); diff --git a/kicker-board/src/bin/kicker-no-uart/main.rs b/kicker-board/src/bin/kicker-no-uart/main.rs deleted file mode 100644 index 5ddb5cb2..00000000 --- a/kicker-board/src/bin/kicker-no-uart/main.rs +++ /dev/null @@ -1,209 +0,0 @@ -#![no_std] -#![no_main] -#![feature(type_alias_impl_trait)] -#![feature(const_mut_refs)] -#![feature(sync_unsafe_cell)] - -use ateam_kicker_board::{drivers::breakbeam::Breakbeam, pins::{BreakbeamLeftAgpioPin, BreakbeamRightAgpioPin}, tasks::get_system_config}; -use static_cell::StaticCell; - -use defmt::*; -use {defmt_rtt as _, panic_probe as _}; - -use libm::{fmaxf, fminf}; - -use embassy_executor::{Executor, InterruptExecutor, Spawner}; -use embassy_stm32::{ - adc::{Adc, SampleTime}, - bind_interrupts, - gpio::{Input, Level, Output, Pull, Speed}, - interrupt, - pac::Interrupt, - peripherals, -}; -use embassy_time::{Duration, Instant, Ticker, Timer}; - -use ateam_kicker_board::{ - adc_raw_to_v, adc_200v_to_rail_voltage, - kick_manager::{KickManager, KickType}, - pins::{ - BlueStatusLed1Pin, BlueStatusLed2Pin, ChargePin, ChipPin, - PowerRail200vReadPin, KickPin, RedStatusLedPin, - }, -}; - - -use ateam_common_packets::bindings_kicker::{ - KickRequest::{self, KR_ARM, KR_DISABLE}, - KickerControl, KickerTelemetry, -}; - -const MAX_TX_PACKET_SIZE: usize = 16; -const TX_BUF_DEPTH: usize = 3; -const MAX_RX_PACKET_SIZE: usize = 16; -const RX_BUF_DEPTH: usize = 3; - -const MAX_KICK_SPEED: f32 = 5.5; -const SHUTDOWN_KICK_DUTY: f32 = 0.20; - -pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; -pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 190.0; -pub const CHARGED_THRESH_VOLTAGE: f32 = 170.0; -pub const CHARGE_SAFE_VOLTAGE: f32 = 5.0; - -fn get_empty_control_packet() -> KickerControl { - KickerControl { - _bitfield_align_1: [], - _bitfield_1: KickerControl::new_bitfield_1(0, 0, 0), - kick_request: KickRequest::KR_DISABLE, - kick_speed: 0.0, - } -} - -fn get_empty_telem_packet() -> KickerTelemetry { - KickerTelemetry { - _bitfield_align_1: [], - _bitfield_1: KickerTelemetry::new_bitfield_1(0, 0, 0, 0), - rail_voltage: 0.0, - battery_voltage: 0.0, - } -} - -#[embassy_executor::task] -async fn high_pri_kick_task( - mut ball_detected_output: Output<'static>, - kick_commanded_input: Input<'static>, - mut adc: Adc<'static, embassy_stm32::peripherals::ADC1>, - charge_pin: ChargePin, - kick_pin: KickPin, - chip_pin: ChipPin, - breakbeam_tx: BreakbeamLeftAgpioPin, - breakbeam_rx: BreakbeamRightAgpioPin, - mut rail_pin: PowerRail200vReadPin, - err_led_pin: RedStatusLedPin, - ball_detected_led1_pin: BlueStatusLed1Pin, - ball_detected_led2_pin: BlueStatusLed2Pin, -) -> ! { - // pins/safety management - let charge_pin = Output::new(charge_pin, Level::Low, Speed::Medium); - let kick_pin = Output::new(kick_pin, Level::Low, Speed::Medium); - let chip_pin = Output::new(chip_pin, Level::Low, Speed::Medium); - let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); - - // debug LEDs - let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); - let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); - let mut ball_detected_led2 = Output::new(ball_detected_led2_pin, Level::Low, Speed::Low); - - // TODO dotstars - - let mut breakbeam = Breakbeam::new(breakbeam_tx, breakbeam_rx); - - // coms buffers - let mut telemetry_enabled: bool; // = false; - let mut kicker_control_packet: KickerControl = get_empty_control_packet(); - - // bookkeeping for latched state - let mut kick_command_cleared: bool = false; - let mut latched_command = KickRequest::KR_DISABLE; - let mut error_latched: bool = false; - let mut charge_hv_rail: bool = false; - - // power down status - let mut shutdown_requested: bool = false; - let mut shutdown_completed: bool = false; - - // loop rate control - let mut ticker = Ticker::every(Duration::from_millis(1)); - - breakbeam.enable_tx(); - loop { - let mut vrefint = adc.enable_vrefint(); - let vrefint_sample = adc.read(&mut vrefint); - - let rail_voltage = adc_200v_to_rail_voltage(adc_raw_to_v(adc.read(&mut rail_pin) as f32, vrefint_sample as f32)); - // let battery_voltage = - // adc_v_to_battery_voltage(adc_raw_to_v(adc.read(&mut battery_voltage_pin) as f32, vrefint_sample as f32)); - // optionally pre-flag errors? - let battery_voltage = 22.5; - - ///////////////////////////////////// - // process any available packets // - ///////////////////////////////////// - - let kick_commanded = kick_commanded_input.is_high(); - - // TODO: read breakbeam - let ball_detected = breakbeam.read(); - - /////////////////////////////////////////////// - // manage repetitive kick commands + state // - /////////////////////////////////////////////// - - let kick_command = - - kick_manager.command(battery_voltage, rail_voltage, charge_hv_rail, kick_command, kick_strength).await; - - // TODO write ball detected GPIO - - // LEDs - if error_latched { - err_led.set_high(); - } else { - err_led.set_low(); - } - - if ball_detected { - ball_detected_led1.set_high(); - ball_detected_led2.set_high(); - - } else { - ball_detected_led1.set_low(); - ball_detected_led2.set_low(); - } - // TODO Dotstar - - // loop rate control @1KHz - ticker.next().await; - } -} - -static EXECUTOR_HIGH: InterruptExecutor = InterruptExecutor::new(); -static EXECUTOR_LOW: StaticCell = StaticCell::new(); - -#[interrupt] -unsafe fn TIM2() { - EXECUTOR_HIGH.on_interrupt(); -} - -bind_interrupts!(struct Irqs { - USART1 => usart::InterruptHandler; -}); - -#[embassy_executor::main] -async fn main(_spawner: Spawner) -> ! { - let sys_cfg = get_system_config(ateam_kicker_board::tasks::ClkSource::InternalOscillator); - let p = embassy_stm32::init(sys_cfg); - - info!("kicker startup!"); - - let _status_led = Output::new(p.PA11, Level::High, Speed::Low); - - let ball_detected_output = Output::new(p.PA1, Level::Low, Speed::Low); - let kick_commanded_input = Input::new(p.PA1, Pull::Down); - - let mut adc = Adc::new(p.ADC1); - adc.set_resolution(embassy_stm32::adc::Resolution::BITS12); - adc.set_sample_time(SampleTime::CYCLES480); - - // high priority executor handles kicking system - // High-priority executor: I2C1, priority level 6 - // TODO CHECK THIS IS THE HIGHEST PRIORITY - embassy_stm32::interrupt::TIM2.set_priority(embassy_stm32::interrupt::Priority::P6); - let spawner = EXECUTOR_HIGH.start(Interrupt::TIM2); - unwrap!(spawner.spawn(high_pri_kick_task(kick_commanded_input, ball_detected_output, adc, p.PE4, p.PE5, p.PE6, p.PA1, p.PA0, p.PC0, p.PE1, p.PE2, p.PE3))); - - loop { - Timer::after_millis(1000).await; - } -} diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 88a8c50b..73e0a0be 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -41,7 +41,7 @@ use ateam_common_packets::bindings_kicker::{ }; const MAX_KICK_SPEED: f32 = 5.5; -const SHUTDOWN_KICK_DUTY: f32 = 0.20; +const SHUTDOWN_KICK_SPEED: f32 = 0.10; pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 195.0; @@ -243,7 +243,7 @@ async fn high_pri_kick_task( let kick_speed = if shutdown_completed { 0.0 } else if shutdown_requested { - SHUTDOWN_KICK_DUTY + SHUTDOWN_KICK_SPEED } else { fmaxf(0.0, fminf(MAX_KICK_SPEED, kicker_control_packet.kick_speed)) }; @@ -266,7 +266,14 @@ async fn high_pri_kick_task( let kick_command = if shutdown_completed { KickType::None } else if shutdown_requested { - KickType::Kick + if ball_detected { + // If shutdown requested, person could be picking up the robot. + // Don't want to kick if ball detected since it might + // be someone's finger. + KickType::None + } else { + KickType::Kick + } } else { match latched_command { KickRequest::KR_DISABLE => KickType::None, From 67e59abcda180a6afb5ea933f628778c85c1ea7a Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 16 Jul 2024 17:09:45 +0200 Subject: [PATCH 126/157] Adds decel, reverts motor models --- .../src/motion/params/body_vel_pid_params.rs | 7 +-- control-board/src/motion/robot_controller.rs | 43 ++++++++++++++----- control-board/src/tasks/control_task.rs | 10 +++-- control-board/src/tasks/shutdown_task.rs | 2 +- control-board/src/tasks/user_io_task.rs | 2 +- kicker-board/src/bin/kicker/main.rs | 2 +- kicker-board/src/kick_manager.rs | 2 +- motor-controller/bin/wheel/main.c | 24 ++++++++--- motor-controller/bin/wheel/system.h | 3 +- motor-controller/common/motor_model.c | 10 +++-- 10 files changed, 73 insertions(+), 32 deletions(-) diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 02339056..1dd17146 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -4,11 +4,12 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; pub static PID_GAIN: Matrix3x5 = matrix![1.0, 0.0, 0.0, -1.0, 1.0; 1.0, 0.0, 0.0, -1.0, 1.0; - 2.0, 0.5, 0.0, -2.0, 2.0]; + 3.0, 0.0, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) -pub static BODY_VEL_LIM: Vector3 = vector![5.0, 3.0, 22.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate -pub static BODY_ACC_LIM: Vector3 = vector![5.0, 3.0, 36.0]; // TODO calibrate/ignore +pub static BODY_VEL_LIM: Vector3 = vector![5.0, 5.0, 30.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +pub static BODY_ACC_LIM: Vector3 = vector![5.0, 2.0, 36.0]; // TODO calibrate/ignore +pub static BODY_DEACC_LIM: Vector3 = vector![5.0, 5.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) // Rough estimate for peak rating diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index d502e2d5..59fb02c6 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -15,7 +15,7 @@ use super::params::body_vel_filter_params::{ use super::params::body_vel_pid_params::{ PID_GAIN, - BODY_VEL_LIM, BODY_ACC_LIM, WHEEL_ACC_LIM + BODY_VEL_LIM, BODY_ACC_LIM, BODY_DEACC_LIM, WHEEL_ACC_LIM }; use ateam_common_packets::bindings_radio::{ @@ -39,6 +39,7 @@ pub struct BodyVelocityController<'a> { body_vel_controller: PidController<3>, body_velocity_limit: Vector3, body_acceleration_limit: Vector3, + body_deceleration_limit: Vector3, wheel_acceleration_limits: Vector4, prev_output: Vector3, cmd_wheel_velocities: Vector4, @@ -60,6 +61,7 @@ impl<'a> BodyVelocityController<'a> { body_vel_controller: body_vel_controller, body_velocity_limit: Vector3::zeros(), body_acceleration_limit: Vector3::zeros(), + body_deceleration_limit: Vector3::zeros(), wheel_acceleration_limits: Vector4::zeros(), prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), @@ -97,6 +99,7 @@ impl<'a> BodyVelocityController<'a> { bvc.body_velocity_limit.copy_from(&BODY_VEL_LIM); bvc.body_acceleration_limit.copy_from(&BODY_ACC_LIM); + bvc.body_deceleration_limit.copy_from(&BODY_DEACC_LIM); bvc.wheel_acceleration_limits.copy_from(&WHEEL_ACC_LIM); bvc @@ -108,6 +111,7 @@ impl<'a> BodyVelocityController<'a> { pid_controller: PidController<3>, bv_limit: Vector3, ba_limit: Vector3, + bda_limit: Vector3, wa_limit: Vector4 ) -> BodyVelocityController<'a> { BodyVelocityController { @@ -117,6 +121,7 @@ impl<'a> BodyVelocityController<'a> { body_vel_controller: pid_controller, body_velocity_limit: bv_limit, body_acceleration_limit: ba_limit, + body_deceleration_limit: bda_limit, wheel_acceleration_limits: wa_limit, prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), @@ -176,15 +181,15 @@ impl<'a> BodyVelocityController<'a> { let mut body_vel_estimate = self.body_vel_filter.get_state(); // Deadzone the velocity estimate - if libm::fabsf(body_vel_estimate[0]) < 0.005 { + if libm::fabsf(body_vel_estimate[0]) < 0.05 { body_vel_estimate[0] = 0.0; } - if libm::fabsf(body_vel_estimate[1]) < 0.005 { + if libm::fabsf(body_vel_estimate[1]) < 0.05 { body_vel_estimate[1] = 0.0; } - if libm::fabsf(body_vel_estimate[2]) < 0.005 { + if libm::fabsf(body_vel_estimate[2]) < 0.05 { body_vel_estimate[2] = 0.0; } @@ -202,14 +207,30 @@ impl<'a> BodyVelocityController<'a> { // Determine commanded body acceleration based on previous control output, and clamp and maintain the direction of acceleration. // NOTE: Using previous control output instead of estimate so that collision disturbances would not impact. let body_acc_output = (body_vel_output - self.prev_output) / self.loop_dt_s; - let body_acc_output_clamp = clamp_vector_keep_dir(&body_acc_output, &self.body_acceleration_limit); + // Make a copy of acceleration to swap out with decel parts. + let mut temp_accel_decel_holder: Vector3 = Vector3::zeros(); + temp_accel_decel_holder.copy_from_slice(self.body_acceleration_limit.as_slice()); + // Check if each part is decel (sign of acceleration != est velocity) + if (body_acc_output[0] > 0.0) != (body_vel_estimate[0] > 0.0) { + temp_accel_decel_holder[0] = self.body_deceleration_limit[0]; + } + + if (body_acc_output[1] > 0.0) != (body_vel_estimate[1] > 0.0) { + temp_accel_decel_holder[1] = self.body_deceleration_limit[1]; + } + + if (body_acc_output[2] > 0.0) != (body_vel_estimate[2] > 0.0) { + temp_accel_decel_holder[2] = self.body_deceleration_limit[2]; + } + + let body_acc_output_clamp = clamp_vector_keep_dir(&body_acc_output, &temp_accel_decel_holder); // Convert back to body velocity let body_vel_output_acc_clamp = self.prev_output + (body_acc_output_clamp * self.loop_dt_s); // Clamp and maintain direction of control body velocity. let body_vel_output_full_clamp = clamp_vector_keep_dir(&body_vel_output_acc_clamp, &self.body_velocity_limit); - self.prev_output = body_vel_output_full_clamp; + self.prev_output.copy_from_slice(body_vel_output_full_clamp.as_slice()); self.debug_telemetry.clamped_commanded_body_velocity.copy_from_slice(body_vel_output_full_clamp.as_slice()); // Transform body velocity commands into the wheel velocity domain. @@ -224,17 +245,17 @@ impl<'a> BodyVelocityController<'a> { // and globally invalid. Investiage this later. If problems are suspected, disable this section // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) // TODO cross check in the future against wheel angle plots and analysis - //let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; - //let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); - //let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); - self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output.as_slice()); + let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; + let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); + let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); + self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); // Save command state. if controls_enabled { self.cmd_wheel_velocities = wheel_vel_output; } else { self.cmd_wheel_velocities = self.robot_model.robot_vel_to_wheel_vel(&body_vel_setpoint); - self.debug_telemetry.wheel_velocity_u.copy_from_slice(self.cmd_wheel_velocities.as_slice()); + self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 56ca6f64..0345f0f1 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -367,7 +367,7 @@ impl < defmt::debug!("all motors flashed"); } } else { - let _res = embassy_futures::join::join5( + let res = embassy_futures::join::join5( self.motor_fl.load_default_firmware_image(), self.motor_bl.load_default_firmware_image(), self.motor_br.load_default_firmware_image(), @@ -375,8 +375,12 @@ impl < self.motor_drib.load_default_firmware_image(), ) .await; - - defmt::debug!("motor firmware flashed"); + + if res.0.is_err() || res.1.is_err() || res.2.is_err() || res.3.is_err() { + defmt::error!("failed to flash drive motor: {}", res); + } else { + defmt::debug!("motor firmware flashed"); + } } } diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs index 9c62d7b7..e01e773f 100644 --- a/control-board/src/tasks/shutdown_task.rs +++ b/control-board/src/tasks/shutdown_task.rs @@ -6,7 +6,7 @@ use embassy_time::Timer; use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::{self, SharedRobotState}}; -pub const HARD_SHUTDOWN_TIME_MS: u64 = 10000; +pub const HARD_SHUTDOWN_TIME_MS: u64 = 20000; #[macro_export] macro_rules! create_shutdown_task { diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index 313b6055..bd54bb42 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -74,7 +74,7 @@ async fn user_io_task_entry( // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); const SHUTDOWN_ANIM_ID: usize = 2; - let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(10000), AnimRepeatMode::None)); + let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(20100), AnimRepeatMode::None)); let mut shutdown_dimmer_arr = [shutdown_dimmer]; let shutdown_ca = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_dimmer_arr, AnimRepeatMode::None); diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 73e0a0be..732aeeff 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -41,7 +41,7 @@ use ateam_common_packets::bindings_kicker::{ }; const MAX_KICK_SPEED: f32 = 5.5; -const SHUTDOWN_KICK_SPEED: f32 = 0.10; +const SHUTDOWN_KICK_SPEED: f32 = 0.15; pub const CHARGE_TARGET_VOLTAGE: f32 = 182.0; pub const CHARGE_OVERVOLT_THRESH_VOLTAGE: f32 = 195.0; diff --git a/kicker-board/src/kick_manager.rs b/kicker-board/src/kick_manager.rs index 955158ea..147ebb49 100644 --- a/kicker-board/src/kick_manager.rs +++ b/kicker-board/src/kick_manager.rs @@ -31,7 +31,7 @@ const MAX_KICK_DURATION_US: f32 = 4500.0; // 10ms (10k us) max power kick const MAX_CHIP_DURATION_US: f32 = 10000.0; // 10ms (10k us) max power kick const CHARGE_COOLDOWN: Duration = Duration::from_micros(50); // 50 micros (5 switching cycles) to confirm switching regulator is off -const KICK_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it +const KICK_COOLDOWN: Duration = Duration::from_millis(300); // TODO: get estiamted mechanical return time from Matt and pad it const CHIP_COOLDOWN: Duration = Duration::from_millis(500); // TODO: get estiamted mechanical return time from Matt and pad it const MAX_SAFE_RAIL_VOLTAGE: f32 = 190.0; // rail is rated for 200V, and should stop charging around 180V diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 7f409095..67bb4358 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -150,7 +150,7 @@ int main() { pid_initialize(&vel_pid, &vel_pid_constants); vel_pid_constants.kP = 2.0f; - vel_pid_constants.kI = 2.0f; + vel_pid_constants.kI = 0.0f; vel_pid_constants.kD = 0.0f; vel_pid_constants.kI_max = 20.0; vel_pid_constants.kI_min = -20.0; @@ -174,9 +174,6 @@ int main() { //GPIOB->BSRR |= GPIO_BSRR_BR_8; //GPIOB->BSRR |= GPIO_BSRR_BR_9; - // watchdog on receiving a command packet - ticks_since_last_command_packet++; - GPIOB->BSRR |= GPIO_BSRR_BS_9; #ifdef UART_ENABLED // increment the soft watchdog @@ -260,12 +257,23 @@ int main() { // if upstream isn't listening or its been too long since we got a command packet, turn off the motor if (!telemetry_enabled || ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; + // Warning pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + } else { + // Warning pin clear. + GPIOB->BSRR |= GPIO_BSRR_BR_8; } // if any critical error is latched, coast the motor if (response_packet.data.motion.master_error) { r_motor_board = 0.0f; - } + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_6; + } + // else { + // // Error pin clear. + // GPIOB->BSRR |= GPIO_BSRR_BR_6; + // } // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -368,6 +376,7 @@ int main() { float r_motor = mm_rads_to_dc(&df45_model, r_motor_board); response_packet.data.motion.vel_setpoint = r_motor_board; response_packet.data.motion.vel_computed_setpoint = r_motor; + response_packet.data.motion.vel_hall_estimate = r_motor_board; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); @@ -464,5 +473,10 @@ int main() { if (sync_systick()) { slipped_control_frame_count++; } + + if (slipped_control_frame_count > 10) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_6; + } } } diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 832ff479..a2ac31bc 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -45,7 +45,6 @@ #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) #define TELEMETRY_LOOP_RATE_MS 5 -#define MOTOR_MAXIMUM_ACCEL 5500 // rad/s^2 - +#define MOTOR_MAXIMUM_ACCEL 6000 // rad/s^2 // #define MOTOR_MAXIMUM_RAD_S 550.825911929f \ No newline at end of file diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c index 734e05e4..e3583cc6 100644 --- a/motor-controller/common/motor_model.c +++ b/motor-controller/common/motor_model.c @@ -3,6 +3,8 @@ #include "motor_model.h" +#define MOTOR_MINIMUM_VEL 4.0f // rad/s + void mm_initialize(MotorModel_t *mm) { mm->max_vel_rads = 0.0f; mm->enc_ticks_per_rev = 0.0f; @@ -20,15 +22,15 @@ void mm_initialize(MotorModel_t *mm) { float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) { // Need to exceed coefficient to prevent b from inverting the directions. // So just return 0.0 at small values - if (avel_rads > -(mm->rads_to_dc_linear_map_b/mm->rads_to_dc_linear_map_m) && - avel_rads < (mm->rads_to_dc_linear_map_b/mm->rads_to_dc_linear_map_m)) { + if (avel_rads > -MOTOR_MINIMUM_VEL && avel_rads < MOTOR_MINIMUM_VEL) { return 0.0f; } // Linear mapping from 'motor_model.py' in hw-analysis - float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + - ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); + //float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + + // ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); + float map_value = avel_rads / mm->max_vel_rads; // bound DC [-1, 1] return fmax(fmin(map_value, 1.0f), -1.0f); } From 0a59ab860deea63285c3b7913da17dc31f442dad Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 17 Jul 2024 03:21:23 -0400 Subject: [PATCH 127/157] add full control test image --- control-board/src/bin/hwtest-control/main.rs | 153 +++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 control-board/src/bin/hwtest-control/main.rs diff --git a/control-board/src/bin/hwtest-control/main.rs b/control-board/src/bin/hwtest-control/main.rs new file mode 100644 index 00000000..808ec6cc --- /dev/null +++ b/control-board/src/bin/hwtest-control/main.rs @@ -0,0 +1,153 @@ +#![no_std] +#![no_main] + +use ateam_common_packets::{bindings_kicker::KickRequest, bindings_radio::BasicControl, radio::DataPacket}; +use embassy_executor::InterruptExecutor; +use embassy_stm32::{ + interrupt, pac::Interrupt +}; +use embassy_sync::pubsub::PubSubChannel; + +use defmt_rtt as _; + +use ateam_control_board::{ + create_audio_task, create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{control_task::start_control_task, kicker_task::start_kicker_task}}; + +// load credentials from correct crate +#[cfg(not(feature = "no-private-credentials"))] +use credentials::private_credentials::wifi::wifi_credentials; +#[cfg(feature = "no-private-credentials")] +use credentials::public_credentials::wifi::wifi_credentials; + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); + +static RADIO_DUMMY_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); +static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); +static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new(); + +static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[interrupt] +unsafe fn CORDIC() { + RADIO_UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CORDIC, embassy_stm32::interrupt::Priority::P6); + let radio_uart_queue_spawner = RADIO_UART_QUEUE_EXECUTOR.start(Interrupt::CORDIC); + + // uart queue executor should be highest priority + // NOTE: maybe this should be all DMA tasks? No computation tasks here + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + // commands channel + let radio_dummy_command_publisher = RADIO_DUMMY_C2_CHANNEL.publisher().unwrap(); + let radio_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap(); + let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + let kicker_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + + // telemetry channel + let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + let radio_telemetry_subscriber = RADIO_TELEMETRY_CHANNEL.subscriber().unwrap(); + + // Battery Channel + let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap(); + let battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().unwrap(); + + // TODO imu channel + let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); + let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + + let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + + /////////////////// + // start tasks // + /////////////////// + + create_io_task!(main_spawner, + robot_state, + battery_volt_publisher, + p); + + create_shutdown_task!(main_spawner, + robot_state, + p); + + create_audio_task!(main_spawner, + robot_state, + p); + + create_radio_task!(main_spawner, radio_uart_queue_spawner, + robot_state, + radio_dummy_command_publisher, radio_telemetry_subscriber, + wifi_credentials, + p); + + create_imu_task!(main_spawner, + robot_state, + imu_gyro_data_publisher, imu_accel_data_publisher, + p); + + start_control_task( + uart_queue_spawner, main_spawner, + robot_state, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, + p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, + p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, + p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, + p.USART1, p.PB15, p.PB14, p.DMA1_CH1, p.DMA1_CH0, p.PD8, p.PD9, + p.UART5, p.PB12, p.PB13, p.DMA2_CH3, p.DMA2_CH2, p.PD13, p.PD12).await; + + start_kicker_task( + main_spawner, uart_queue_spawner, + robot_state, + kicker_command_subscriber, + p.USART6, + p.PC7, p.PC6, p.DMA2_CH5, p.DMA2_CH4, p.PA8, p.PA9, p.PG8, + ).await; + + loop { + Timer::after_millis(100).await; + + radio_command_publisher.publish(DataPacket::BasicControl(BasicControl { + vel_x_linear: 2.0, + vel_y_linear: 0.0, + vel_z_angular: 0.0, + kick_vel: 0.0, + dribbler_speed: -0.1, + kick_request: KickRequest::KR_ARM, + })).await; + } +} \ No newline at end of file From 89206558b18f388ffbd1f7dd86b5d8dcca90d3a1 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Wed, 17 Jul 2024 09:21:30 +0200 Subject: [PATCH 128/157] More testing adjustments --- .../src/motion/params/body_vel_pid_params.rs | 10 ++++----- control-board/src/motion/robot_controller.rs | 10 ++++----- control-board/src/tasks/control_task.rs | 8 +++---- motor-controller/bin/wheel/main.c | 21 +++++++++---------- 4 files changed, 24 insertions(+), 25 deletions(-) diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index 1dd17146..f92d12a4 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -2,14 +2,14 @@ use nalgebra::{matrix, vector, Matrix3x5, Vector3, Vector4}; // Kp, Ki, Kd, Ki_err_min, Ki_err_max pub static PID_GAIN: Matrix3x5 = - matrix![1.0, 0.0, 0.0, -1.0, 1.0; - 1.0, 0.0, 0.0, -1.0, 1.0; - 3.0, 0.0, 0.0, -2.0, 2.0]; + matrix![1.5, 0.0, 0.0, -1.0, 1.0; + 1.5, 0.0, 0.0, -1.0, 1.0; + 4.0, 0.0, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) pub static BODY_VEL_LIM: Vector3 = vector![5.0, 5.0, 30.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate -pub static BODY_ACC_LIM: Vector3 = vector![5.0, 2.0, 36.0]; // TODO calibrate/ignore -pub static BODY_DEACC_LIM: Vector3 = vector![5.0, 5.0, 36.0]; // TODO calibrate/ignore +pub static BODY_ACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore +pub static BODY_DEACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore // FL, BL, BR, FR (rad/s^2) // Rough estimate for peak rating diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index 59fb02c6..dba6f7d7 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -245,17 +245,17 @@ impl<'a> BodyVelocityController<'a> { // and globally invalid. Investiage this later. If problems are suspected, disable this section // and lower the body acc limit (maybe something anatgonist based on 45/30 deg wheel angles?) // TODO cross check in the future against wheel angle plots and analysis - let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; - let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); - let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); - self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); + //let wheel_acc_setpoint = (wheel_vel_output - self.cmd_wheel_velocities) / self.loop_dt_s; + //let wheel_acc_setpoint_clamp = clamp_vector_keep_dir(&wheel_acc_setpoint, &WHEEL_ACC_LIM); + //let wheel_vel_output_clamp = self.cmd_wheel_velocities + (wheel_acc_setpoint_clamp * self.loop_dt_s); + //self.debug_telemetry.wheel_velocity_clamped_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); // Save command state. if controls_enabled { self.cmd_wheel_velocities = wheel_vel_output; } else { self.cmd_wheel_velocities = self.robot_model.robot_vel_to_wheel_vel(&body_vel_setpoint); - self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output_clamp.as_slice()); + self.debug_telemetry.wheel_velocity_u.copy_from_slice(wheel_vel_output.as_slice()); } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 0345f0f1..8e679f74 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -216,10 +216,10 @@ impl < self.motor_fr.set_telemetry_enabled(true); self.motor_drib.set_telemetry_enabled(true); - self.motor_fl.set_motion_type(MotorCommand_MotionType::VELOCITY); - self.motor_bl.set_motion_type(MotorCommand_MotionType::VELOCITY); - self.motor_br.set_motion_type(MotorCommand_MotionType::VELOCITY); - self.motor_fr.set_motion_type(MotorCommand_MotionType::VELOCITY); + self.motor_fl.set_motion_type(MotorCommand_MotionType::OPEN_LOOP); + self.motor_bl.set_motion_type(MotorCommand_MotionType::OPEN_LOOP); + self.motor_br.set_motion_type(MotorCommand_MotionType::OPEN_LOOP); + self.motor_fr.set_motion_type(MotorCommand_MotionType::OPEN_LOOP); Timer::after_millis(10).await; diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 67bb4358..4020e5a0 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -255,25 +255,24 @@ int main() { #endif // if upstream isn't listening or its been too long since we got a command packet, turn off the motor - if (!telemetry_enabled || ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + if (!telemetry_enabled) { r_motor_board = 0.0f; - // Warning pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_8; - } else { - // Warning pin clear. - GPIOB->BSRR |= GPIO_BSRR_BR_8; + } + + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + r_motor_board = 0.0f; + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_7; + // HACK Will force the watchdog to trigger. + while(true); } // if any critical error is latched, coast the motor if (response_packet.data.motion.master_error) { r_motor_board = 0.0f; // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_6; + GPIOB->BSRR |= GPIO_BSRR_BS_7; } - // else { - // // Error pin clear. - // GPIOB->BSRR |= GPIO_BSRR_BR_6; - // } // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); From 9b77ec3e21f5368de7e72dde355cc20faac9338d Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 17 Jul 2024 09:08:20 -0400 Subject: [PATCH 129/157] kicker flashing updates, radio uart prio updates, and stspin led updates --- control-board/src/bin/control/main.rs | 13 +++++++-- control-board/src/bin/hwtest-control/main.rs | 21 +++++++++++---- .../src/bin/hwtest-kicker-coms/main.rs | 2 +- control-board/src/bin/hwtest-kicker/main.rs | 2 +- control-board/src/bin/hwtest-radio/main.rs | 9 ++++--- control-board/src/drivers/kicker.rs | 13 +++++---- control-board/src/tasks/control_task.rs | 25 ++++++++--------- control-board/src/tasks/kicker_task.rs | 15 +++++++---- control-board/src/tasks/radio_task.rs | 16 +++++++---- kicker-board/src/bin/kicker/main.rs | 2 +- kicker-board/src/kick_manager.rs | 4 +-- motor-controller/bin/wheel/main.c | 27 +++++++++---------- motor-controller/bin/wheel/setup.c | 2 ++ 13 files changed, 94 insertions(+), 57 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 7617e217..66ee3312 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -20,9 +20,17 @@ use credentials::public_credentials::wifi::wifi_credentials; use embassy_time::Timer; // provide embedded panic probe -use panic_probe as _; +// use panic_probe as _; use static_cell::ConstStaticCell; +#[panic_handler] +fn panic(info: &core::panic::PanicInfo) -> ! { + defmt::error!("{}", defmt::Display2Format(info)); + // Delay to give it a change to print + cortex_m::asm::delay(10_000_000); + cortex_m::peripheral::SCB::sys_reset(); +} + static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); @@ -106,7 +114,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); - create_radio_task!(main_spawner, radio_uart_queue_spawner, + create_radio_task!(main_spawner, radio_uart_queue_spawner, uart_queue_spawner, + // create_radio_task!(main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, wifi_credentials, diff --git a/control-board/src/bin/hwtest-control/main.rs b/control-board/src/bin/hwtest-control/main.rs index 808ec6cc..6f75cf65 100644 --- a/control-board/src/bin/hwtest-control/main.rs +++ b/control-board/src/bin/hwtest-control/main.rs @@ -21,9 +21,17 @@ use credentials::public_credentials::wifi::wifi_credentials; use embassy_time::Timer; // provide embedded panic probe -use panic_probe as _; +// use panic_probe as _; use static_cell::ConstStaticCell; +#[panic_handler] +fn panic(info: &core::panic::PanicInfo) -> ! { + defmt::error!("{}", defmt::Display2Format(info)); + // Delay to give it a change to print + cortex_m::asm::delay(10_000_000); + cortex_m::peripheral::SCB::sys_reset(); +} + static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); static RADIO_DUMMY_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); @@ -109,7 +117,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); - create_radio_task!(main_spawner, radio_uart_queue_spawner, + create_radio_task!(main_spawner, radio_uart_queue_spawner, uart_queue_spawner, + // create_radio_task!(main_spawner, uart_queue_spawner, robot_state, radio_dummy_command_publisher, radio_telemetry_subscriber, wifi_credentials, @@ -139,15 +148,17 @@ async fn main(main_spawner: embassy_executor::Spawner) { ).await; loop { - Timer::after_millis(100).await; + Timer::after_millis(10).await; + + defmt::info!("main loop"); - radio_command_publisher.publish(DataPacket::BasicControl(BasicControl { + radio_command_publisher.publish_immediate(DataPacket::BasicControl(BasicControl { vel_x_linear: 2.0, vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, dribbler_speed: -0.1, kick_request: KickRequest::KR_ARM, - })).await; + })); } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index 2cbf5819..d4ddb9d9 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -101,7 +101,7 @@ async fn main(_spawner: embassy_executor::Spawner) { let kicker_firmware_image = KICKER_FW_IMG; let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; + let res = kicker.init_default_firmware_image(true).await; if res.is_err() { defmt::warn!("kicker flashing failed."); diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index d6b16728..07008057 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -101,7 +101,7 @@ async fn main(_spawner: embassy_executor::Spawner) { let kicker_firmware_image = KICKER_FW_IMG; let mut kicker = Kicker::new(kicker_stm32_interface, kicker_firmware_image); - let res = kicker.load_default_firmware_image().await; + let res = kicker.init_default_firmware_image(true).await; if res.is_err() { defmt::warn!("kicker flashing failed."); diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index dfe61337..ec08695c 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -76,10 +76,11 @@ async fn main(main_spawner: embassy_executor::Spawner) { // start tasks // /////////////////// - create_radio_task!(main_spawner, uart_queue_spawner, - robot_state, - radio_command_publisher, radio_telemetry_subscriber, - wifi_credentials, p); + create_radio_task!(main_spawner, uart_queue_spawner, uart_queue_spawner, + robot_state, + radio_command_publisher, radio_telemetry_subscriber, + wifi_credentials, + p); create_io_task!(main_spawner, robot_state, battery_volt_publisher, p); diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 15ca3a96..e3b1abbc 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -176,9 +176,12 @@ impl< self.stm32_uart_interface.leave_reset().await; } - pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> { - // defmt::warn!("currently skipping kicker firmware flash"); - self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await?; + pub async fn init_firmware_image(&mut self, flash: bool, fw_image_bytes: &[u8]) -> Result<(), ()> { + if flash { + self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await?; + } else { + defmt::warn!("currently skipping kicker firmware flash"); + } self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; @@ -190,8 +193,8 @@ impl< return Ok(()); } - pub async fn load_default_firmware_image(&mut self) -> Result<(), ()> { - return self.load_firmware_image(self.firmware_image).await; + pub async fn init_default_firmware_image(&mut self, flash: bool) -> Result<(), ()> { + return self.init_firmware_image(flash, self.firmware_image).await; } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 8e679f74..ae2ae9c6 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -231,8 +231,7 @@ impl < let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); let mut drib_vel = 0.0; - let mut ticks_since_packet = 0; - + let mut ticks_since_control_packet = 0; loop { self.motor_fl.process_packets(); @@ -251,7 +250,8 @@ impl < defmt::info!("ball detected"); } - if let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { + ticks_since_control_packet += 1; + while let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { match latest_packet { ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { @@ -263,7 +263,7 @@ impl < cmd_vel = new_cmd_vel; drib_vel = latest_control.dribbler_speed; - ticks_since_packet = 0; + ticks_since_control_packet = 0; }, ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); @@ -279,20 +279,20 @@ impl < } }, } - } else { - ticks_since_packet += 1; - if ticks_since_packet >= TICKS_WITHOUT_PACKET_STOP { - cmd_vel = Vector3::new(0., 0., 0.); - // ticks_since_packet = 0; - } + } + + if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + drib_vel = 0.0; + defmt::warn!("ticks since packet lockout"); } // now we have setpoint r(t) in self.cmd_vel - if let Some(battery_v) = self.battery_subscriber.try_next_message_pure() { + while let Some(battery_v) = self.battery_subscriber.try_next_message_pure() { self.last_battery_v = battery_v; } - if let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { + while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { self.last_gyro_rads = gyro_rads[2]; } @@ -304,6 +304,7 @@ impl < } else { // Battery is too low, set velocity to zero drib_vel = 0.0; + defmt::warn!("CT - low batter command lockout"); Vector4::new(0.0, 0.0, 0.0, 0.0) }; diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 158fd82c..7e53ec52 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -118,14 +118,19 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } }, KickerTaskState::PowerOn => { - defmt::debug!("power cycling kicker"); - self.remote_power_btn_hold().await; + // lets power settle on kicker + Timer::after_millis(100).await; + defmt::debug!("turn on kicker"); self.remote_power_btn_press().await; + Timer::after_millis(50).await; + self.kicker_driver.enter_reset().await; + self.kicker_driver.leave_reset().await; // power should be coming on, attempt connection self.kicker_task_state = KickerTaskState::ConnectUart; }, KickerTaskState::ConnectUart => { - if self.kicker_driver.load_default_firmware_image().await.is_err() { + let flash_firmware = cur_robot_state.hw_debug_mode; + if self.kicker_driver.init_default_firmware_image(flash_firmware).await.is_err() { // attempt to power on the board again // if the kicker is missing or bugged we'll stay in a power on -> attempt // uart loop forever @@ -179,9 +184,9 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ async fn remote_power_btn_press(&mut self) { self.remote_power_btn.set_high(); - Timer::after_millis(200).await; + Timer::after_millis(250).await; self.remote_power_btn.set_low(); - Timer::after_millis(200).await; + Timer::after_millis(50).await; } async fn remote_power_btn_hold(&mut self) { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index f22c61a9..1bdfde0c 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,6 +1,6 @@ use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; -use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart::queue::{UartReadQueue, UartWriteQueue}}; +use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_signals, queue_pair_rx_task, queue_pair_tx_task, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; use embassy_futures::select::{select, Either}; @@ -14,11 +14,11 @@ use crate::{drivers::radio_robot::{RobotRadio, TeamColor}, pins::*, robot_state: #[macro_export] macro_rules! create_radio_task { - ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, + ($main_spawner:ident, $rx_uart_queue_spawner:ident, $tx_uart_queue_spawner:ident, $robot_state:ident, $radio_command_publisher:ident, $radio_telemetry_subscriber:ident, $wifi_credentials:ident, $p:ident) => { ateam_control_board::tasks::radio_task::start_radio_task( - $main_spawner, $uart_queue_spawner, + $main_spawner, $rx_uart_queue_spawner, $tx_uart_queue_spawner, $robot_state, $radio_command_publisher, $radio_telemetry_subscriber, &$wifi_credentials, @@ -261,6 +261,8 @@ impl< //self.connection_state = RadioConnectionState::ConnectSoftware; //self.radio.close_peer().await.unwrap(); cortex_m::peripheral::SCB::sys_reset(); + + // self.connection_state = RadioConnectionState::ConnectPhys; } }, } @@ -409,7 +411,8 @@ async fn radio_task_entry(mut radio_task: RadioTaskBSRR |= GPIO_BSRR_BR_6; GPIOB->BSRR |= GPIO_BSRR_BR_7; + GPIOB->BSRR |= GPIO_BSRR_BR_8; - // turn on Red LED + // turn on Red/Yl LED GPIOB->BSRR |= GPIO_BSRR_BS_6; + GPIOB->BSRR |= GPIO_BSRR_BS_8; + // Setups clocks setup(); @@ -55,11 +58,6 @@ int main() { while (IWDG->SR) {} // wait for value to take IWDG->KR = 0x0000AAAA; // feed the watchdog - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BR_7; - - GPIOB->BSRR |= GPIO_BSRR_BS_8; - uint32_t ticks_since_last_command_packet = 0; bool telemetry_enabled = false; @@ -163,18 +161,14 @@ int main() { torque_pid_constants.kP = 1.0f; - // turn off Red turn on Green + // turn off Red/Yl GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BS_7; + GPIOB->BSRR |= GPIO_BSRR_BR_8; // toggle J1-1 while (true) { IWDG->KR = 0x0000AAAA; // feed the watchdog - //GPIOB->BSRR |= GPIO_BSRR_BR_8; - //GPIOB->BSRR |= GPIO_BSRR_BR_9; - - GPIOB->BSRR |= GPIO_BSRR_BS_9; #ifdef UART_ENABLED // increment the soft watchdog ticks_since_last_command_packet++; @@ -257,14 +251,19 @@ int main() { // if upstream isn't listening or its been too long since we got a command packet, turn off the motor if (!telemetry_enabled) { r_motor_board = 0.0f; + GPIOB->BSRR |= GPIO_BSRR_BR_7; + } else { + GPIOB->BSRR |= GPIO_BSRR_BS_7; } if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_7; + GPIOB->BSRR |= GPIO_BSRR_BS_8; // HACK Will force the watchdog to trigger. - while(true); + // while(true); + } else { + GPIOB->BSRR |= GPIO_BSRR_BR_8; } // if any critical error is latched, coast the motor diff --git a/motor-controller/bin/wheel/setup.c b/motor-controller/bin/wheel/setup.c index 905c4c01..705cc499 100644 --- a/motor-controller/bin/wheel/setup.c +++ b/motor-controller/bin/wheel/setup.c @@ -161,6 +161,8 @@ inline void setup_io() { GPIOB->MODER |= GPIO_MODER_MODER6_0; // set output, Red LED, Err GPIOB->MODER |= GPIO_MODER_MODER7_0; // set output, Green LED, Fw Ready + GPIOB->MODER |= GPIO_MODER_MODER8_0; // set output, Yellow LED, Warn + } /** From 48de57270e097b037f07f7e7942a107307e2bc6c Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 06:35:37 +0200 Subject: [PATCH 130/157] THE HAIL MARY (remove LED, cleanup, remove STPIN timer) --- control-board/src/lib.rs | 2 +- control-board/src/radio.rs | 261 ------------------------ control-board/src/robot_state.rs | 41 +++- control-board/src/stm32_interface.rs | 4 +- control-board/src/tasks/radio_task.rs | 134 ++++++------ control-board/src/tasks/user_io_task.rs | 71 +++++-- lib-stm32/src/drivers/led/apa102.rs | 52 +++-- motor-controller/bin/wheel/main.c | 10 +- 8 files changed, 195 insertions(+), 380 deletions(-) delete mode 100644 control-board/src/radio.rs diff --git a/control-board/src/lib.rs b/control-board/src/lib.rs index 84ac29dc..27896d19 100644 --- a/control-board/src/lib.rs +++ b/control-board/src/lib.rs @@ -75,7 +75,7 @@ macro_rules! include_kicker_bin { pub const BATTERY_MIN_SAFE_VOLTAGE: f32 = 21.0; pub const BATTERY_MIN_CRIT_VOLTAGE: f32 = 19.5; -pub const BATTERY_MAX_VOLTAGE: f32 = 25.2; +pub const BATTERY_MAX_VOLTAGE: f32 = 26.0; pub const BATTERY_BUFFER_SIZE: usize = 20; pub const ADC_TO_BATTERY_DIVIDER: f32 = (130_000.0 + 11_000.0) / 11_000.0; diff --git a/control-board/src/radio.rs b/control-board/src/radio.rs deleted file mode 100644 index f8201ba9..00000000 --- a/control-board/src/radio.rs +++ /dev/null @@ -1,261 +0,0 @@ -use core::future::Future; - -use embassy_executor::{raw::TaskStorage, SendSpawner, SpawnToken}; -use embassy_futures::join::join; -use embassy_stm32::{gpio::Pin, usart}; -use embassy_stm32::mode::Async; -use embassy_stm32::usart::Uart; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex}; -use embassy_time::{Duration, Ticker}; - -use defmt::*; - -use ateam_lib_stm32::make_uart_queues; -use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; - -use ateam_common_packets::bindings_radio::{BasicControl, BasicTelemetry, ControlDebugTelemetry, ParameterCommand}; -use ateam_common_packets::radio::DataPacket; - -use crate::drivers::radio::{RobotRadio, TeamColor, WifiNetwork}; -use crate::pins::*; - -type ControlTaskFuture< - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - ResetPin: Pin + Send + Sync, -> where - UART::Interrupt: Send, -= impl Future + Sync; - -/* - * Uart tx (Radio) - * mut - write task - * Uart rx (Radio) - * mut - read task - * Write Queue - * [mut] - write task - * [mut] - control task (through radio) - * Read Queue - * [mut] - read task - * [mut] - radio read task - * Radio State - * mut - radio read task - * Latest Control - * mut - radio read task - * mut - control task - */ - -pub struct RadioTest< - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - ResetPin: Pin + Send + Sync, -> where - UART::Interrupt: Send, -{ - queue_tx: &'static UartWriteQueue, - queue_rx: &'static UartReadQueue, - task: TaskStorage< - ControlTaskFuture, - >, - latest_control: Mutex>, - latest_params: Mutex>, - radio: Option>, -} - -impl< - const LEN_TX: usize, - const LEN_RX: usize, - const DEPTH_TX: usize, - const DEPTH_RX: usize, - UART: usart::BasicInstance + Send, - RxDma: usart::RxDma + Send, - TxDma: usart::TxDma + Send, - ResetPin: Pin + Send + Sync, - > RadioTest -where - UART::Interrupt: Send, -{ - pub const fn new( - tx_queue: &'static UartWriteQueue, - rx_queue: &'static UartReadQueue, - ) -> Self { - RadioTest { - queue_rx: rx_queue, - queue_tx: tx_queue, - task: TaskStorage::new(), - latest_control: Mutex::new(None), - latest_params: Mutex::new(None), - radio: None, - } - } - - // setup uart - // setup tasks - pub async fn setup( - &'static mut self, - spawner: &SendSpawner, - uart: usart::Uart<'static, UART, Async>, - reset_pin: impl Pin, - id: u8, - team: TeamColor, - wifi_network: WifiNetwork, - ) -> SpawnToken { - let (tx, rx) = Uart::split(uart); - - spawner.spawn(self.queue_rx.spawn_task(rx)).unwrap(); - spawner.spawn(self.queue_tx.spawn_task(tx)).unwrap(); - - let mut radio = RobotRadio::new(&self.queue_rx, &self.queue_tx, reset_pin) - .await - .unwrap(); - - info!("radio created"); - defmt::panic!("radio temporarily left unconnected"); - // radio.connect_to_network(wifi_network).await.unwrap(); - info!("radio connected"); - - radio.open_multicast().await.unwrap(); - info!("multicast open"); - - loop { - info!("sending hello"); - radio.send_hello(id, team).await.unwrap(); - let hello = radio.wait_hello(Duration::from_millis(1000)).await; - - match hello { - Ok(hello) => { - info!( - "recieved hello resp to: {}.{}.{}.{}:{}", - hello.ipv4[0], hello.ipv4[1], hello.ipv4[2], hello.ipv4[3], hello.port - ); - radio.close_peer().await.unwrap(); - info!("multicast peer closed"); - radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); - info!("unicast open"); - break; - } - Err(_) => {} - } - } - - { - self.radio = Some(radio); - } - - self.task - .spawn(|| Self::control_task(self.radio.as_ref().unwrap(), &self.latest_control, &self.latest_params)) - } - - fn control_task( - radio: &'static RobotRadio< - 'static, - UART, - RxDma, - TxDma, - LEN_TX, - LEN_RX, - DEPTH_TX, - DEPTH_RX - >, - latest_control: &'static Mutex>, - latest_param_cmd: &'static Mutex>, - ) -> ControlTaskFuture { - let last_control: Mutex = Mutex::new(true); - async move { - join( - (async || loop { - let data_packet = radio.read_packet().await; - if let Ok(data_packet) = data_packet { - if let DataPacket::BasicControl(control) = data_packet { - let mut latest_control = latest_control.lock().await; - *latest_control = Some(control); - - // feed the disconnected watch-dog because we got a packet - { - let mut last_control = last_control.lock().await; - *last_control = true; - } - } else if let DataPacket::ParameterCommand(param_cmd) = data_packet { - let mut latest_param_cmd = latest_param_cmd.lock().await; - *latest_param_cmd = Some(param_cmd); - } - } - })(), - (async || { - // TODO: parameterize this timeout - let mut no_packet_timeout = Ticker::every(Duration::from_millis(3000)); - loop { - { - // TODO this can be an AtomicBool - let mut last_control = last_control.lock().await; - if !*last_control { - cortex_m::peripheral::SCB::sys_reset(); - } - *last_control = false; - } - no_packet_timeout.next().await; - } - })(), - ) - .await; - } - } - - // write telemetry to queue - pub async fn send_telemetry(&self, telemetry: BasicTelemetry) { - self.radio - .as_ref() - .unwrap() - .send_telemetry(telemetry) - .await - .unwrap(); - } - - pub async fn send_control_debug_telemetry(&self, control_debug_telemetry: ControlDebugTelemetry) { - self.radio - .as_ref() - .unwrap() - .send_control_debug_telemetry(control_debug_telemetry) - .await - .unwrap(); - } - - pub async fn send_parameter_response(&self, parameter_command: ParameterCommand) { - self.radio - .as_ref() - .unwrap() - .send_parameter_response(parameter_command) - .await - .unwrap(); - } - - // fetch latest stored control value - pub fn get_latest_control(&self) -> Option { - let mut latest_control = self.latest_control.try_lock(); - if let Ok(latest_control) = &mut latest_control { - latest_control.take() - } else { - None - } - } - - // fetch latest stored parameters packet - pub fn get_latest_params_cmd(&self) -> Option { - let mut latest_param_cmd = self.latest_params.try_lock(); - if let Ok(latest_param_cmd) = &mut latest_param_cmd { - latest_param_cmd.take() - } else { - None - } - } -} diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index 33ba0cf4..bf92e6e3 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -18,7 +18,9 @@ pub struct SharedRobotState { // 32bit Cortex M has no AtomicU64, so we'll sync on milliseconds // overflow occurs in ~49.7 days last_packet_receive_time_ticks_ms: AtomicU32, - radio_connection_ok: AtomicBool, + radio_network_ok: AtomicBool, + radio_bridge_ok: AtomicBool, + battery_low: AtomicBool, battery_crit: AtomicBool, @@ -45,7 +47,8 @@ impl SharedRobotState { wheels_inop: AtomicU8::new(0x0F), dribbler_inop: AtomicBool::new(true), last_packet_receive_time_ticks_ms: AtomicU32::new(0), - radio_connection_ok: AtomicBool::new(false), + radio_network_ok: AtomicBool::new(false), + radio_bridge_ok: AtomicBool::new(false), battery_low: AtomicBool::new(false), battery_crit: AtomicBool::new(false), robot_tipped: AtomicBool::new(false), @@ -63,15 +66,16 @@ impl SharedRobotState { hw_wifi_network_index: self.hw_wifi_network_index(), hw_debug_mode: self.hw_in_debug_mode(), - radio_inop: true, + radio_inop: self.get_radio_inop(), imu_inop: self.get_imu_inop(), kicker_inop: true, wheels_inop: 0xFF, dribbler_inop: true, last_packet_receive_time_ticks_ms: 0, - radio_connection_ok: false, - + radio_network_ok: self.get_radio_network_ok(), + radio_bridge_ok: self.get_radio_bridge_ok(), + battery_low: self.get_battery_low(), battery_crit: self.get_battery_crit(), @@ -131,6 +135,14 @@ impl SharedRobotState { self.robot_tipped.store(tipped, Ordering::Relaxed); } + pub fn get_radio_inop(&self) -> bool { + self.radio_inop.load(Ordering::Relaxed) + } + + pub fn set_radio_inop(&self, radio_inop: bool) { + self.radio_inop.store(radio_inop, Ordering::Relaxed); + } + pub fn get_imu_inop(&self) -> bool { self.imu_inop.load(Ordering::Relaxed) } @@ -163,12 +175,20 @@ impl SharedRobotState { self.battery_crit.store(battery_crit, Ordering::Relaxed); } - pub fn radio_connection_ok(&self) -> bool { - self.radio_connection_ok.load(Ordering::Relaxed) + pub fn get_radio_network_ok(&self) -> bool { + self.radio_network_ok.load(Ordering::Relaxed) + } + + pub fn set_radio_network_ok(&self, network_ok: bool) { + self.radio_network_ok.store(network_ok, Ordering::Relaxed); + } + + pub fn get_radio_bridge_ok(&self) -> bool { + self.radio_bridge_ok.load(Ordering::Relaxed) } - pub fn set_radio_connection_ok(&self, conn_ok: bool) { - self.radio_connection_ok.store(conn_ok, Ordering::Relaxed); + pub fn set_radio_bridge_ok(&self, bridge_ok: bool) { + self.radio_bridge_ok.store(bridge_ok, Ordering::Relaxed); } pub fn ball_detected(&self) -> bool { @@ -204,7 +224,8 @@ pub struct RobotState { pub dribbler_inop: bool, pub last_packet_receive_time_ticks_ms: u32, - pub radio_connection_ok: bool, + pub radio_network_ok: bool, + pub radio_bridge_ok: bool, pub battery_low: bool, pub battery_crit: bool, diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 30b59190..0703edc5 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -234,7 +234,9 @@ impl< } pub fn send_or_discard_data(&self, data: &[u8]) { - let _ = self.try_send_data(data); + if self.try_send_data(data).is_err() { + defmt::error!("Failed to send motor data"); + }; } //////////////////////////////////////////////// diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 1bdfde0c..c6ffa1c0 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -53,11 +53,10 @@ make_uart_queue_pair!(RADIO, #[derive(Clone, Copy, PartialEq, PartialOrd, Debug)] enum RadioConnectionState { Unconnected, - ConnectPhys, - ConnectUart, - ConnectNetwork, - ConnectSoftware, - Connected, + ConnectedPhys, + ConnectedUart, + ConnectedNetwork, + Connected } pub struct RadioTask< @@ -93,6 +92,7 @@ impl< pub type TaskRobotRadio = RobotRadio<'static, UartPeripherial, UartRxDma, UartTxDma, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH>; const RETRY_DELAY_MS: u64 = 1000; + const RESPONSE_FROM_PC_TIMEOUT_MS: u64 = 1000; const UART_CONNECT_TIMEOUT_MS: u64 = 5000; pub fn new(robot_state: &'static SharedRobotState, @@ -158,23 +158,25 @@ impl< #[allow(unused_assignments)] // let mut next_connection_state = self.connection_state; loop { - + // Keep a local flag of radio issues. + let mut radio_inop_flag_local = false; + let mut radio_network_fail_local = false; // check for hardware config changes that affect radio connection let cur_robot_state = self.shared_robot_state.get_state(); if cur_robot_state != last_robot_state { // we are connected to software and robot id or team was changed on hardware if self.connection_state == RadioConnectionState::Connected && - (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id - || cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) { - // enter software connect state (disconnecting) - self.connection_state = RadioConnectionState::ConnectSoftware; + (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id || + cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) { + // Enter connected network state (disconnecting) + self.connection_state = RadioConnectionState::ConnectedNetwork; defmt::info!("shell state change triggering software reconnect"); } - // we ar at least connected to Wifi and the wifi network id changed - if self.connection_state >= RadioConnectionState::ConnectNetwork - && cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { + // We are at least connected on UART and the wifi network id changed + if self.connection_state >= RadioConnectionState::ConnectedUart && + cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index { defmt::info!("dip state change triggering wifi network change"); if self.radio.disconnect_network().await.is_err() { @@ -184,42 +186,42 @@ impl< defmt::error!("failed to cleanly disconnect - consider radio reboot"); } - self.connection_state = RadioConnectionState::ConnectNetwork; + // Go back to reset the full network flow. + self.connection_state = RadioConnectionState::ConnectedUart; } } - last_robot_state = cur_robot_state; - - // check for missing radio + // Check for missing radio. Highest priority clear of state. if self.radio_ndet_input.is_high() { defmt::error!("radio appears unplugged."); - self.connection_state = RadioConnectionState::ConnectPhys; + radio_inop_flag_local = true; + self.connection_state = RadioConnectionState::Unconnected; } // execute on the connection state match self.connection_state { RadioConnectionState::Unconnected => { - self.connection_state = RadioConnectionState::ConnectPhys; - }, - RadioConnectionState::ConnectPhys => { - if self.radio_ndet_input.is_high() { - Timer::after_millis(Self::RETRY_DELAY_MS).await; - self.connection_state = RadioConnectionState::ConnectPhys; + // Radio detect pin says unplugged from above. + if radio_inop_flag_local { + self.connection_state = RadioConnectionState::Unconnected; } else { - self.connection_state = RadioConnectionState::ConnectUart; + // Pin is detected, so connected physically. + self.connection_state = RadioConnectionState::ConnectedPhys; } }, - RadioConnectionState::ConnectUart => { + RadioConnectionState::ConnectedPhys => { if self.connect_uart().await.is_err() { - Timer::after_millis(Self::RETRY_DELAY_MS).await; - // failed to connect, go back to physical connection check - self.connection_state = RadioConnectionState::ConnectPhys; + radio_inop_flag_local = true; + // If the pin is unconnected, will be overridden out of the state. + // So just check UART again. + self.connection_state = RadioConnectionState::ConnectedPhys; } else { - self.connection_state = RadioConnectionState::ConnectNetwork; + // UART is not in error, so good to go. + self.connection_state = RadioConnectionState::ConnectedUart; } }, - RadioConnectionState::ConnectNetwork => { + RadioConnectionState::ConnectedUart => { let wifi_network_ind = cur_robot_state.hw_wifi_network_index as usize; let wifi_credential = if wifi_network_ind >= self.wifi_credentials.len() { self.wifi_credentials[self.wifi_credentials.len() - 1] @@ -229,46 +231,57 @@ impl< defmt::debug!("connecting to network ({}): ssid: {}, password: {}", wifi_network_ind, wifi_credential.get_ssid(), wifi_credential.get_password()); if self.connect_network(wifi_credential, cur_robot_state.hw_robot_id).await.is_err() { - Timer::after_millis(Self::RETRY_DELAY_MS).await; - // TODO make error handling smarter, e.g. if we get a timeout or low level errors - // we should fall back to ConnectPhys or ConnectUart, not keep retrying forever + // If network connection failed, go back up to verify UART. + radio_network_fail_local = true; + self.connection_state = RadioConnectionState::ConnectedPhys; } else { - self.connection_state = RadioConnectionState::ConnectSoftware; + self.connection_state = RadioConnectionState::ConnectedNetwork; } }, - RadioConnectionState::ConnectSoftware => { + RadioConnectionState::ConnectedNetwork => { if let Ok(connected) = self.connect_software(cur_robot_state.hw_robot_id, cur_robot_state.hw_robot_team_is_blue).await { if connected { - self.connection_state = RadioConnectionState::Connected; + // Refresh last software packet on first connect. self.last_software_packet = Instant::now(); + self.connection_state = RadioConnectionState::Connected; } else { - // software didn't respond to our hello, it may not be started yet - Timer::after_millis(1000).await; + // Software didn't respond to our hello, it may not be started yet. + radio_network_fail_local = true; + self.connection_state = RadioConnectionState::ConnectedNetwork; } } else { - // a hard error occurred - Timer::after_millis(Self::RETRY_DELAY_MS).await; - - // TODO where should we retry? + radio_network_fail_local = true; + // If network connection failed, go back up to verify UART. + self.connection_state = RadioConnectionState::ConnectedPhys; } }, RadioConnectionState::Connected => { let _ = self.process_packets().await; // if we're stably connected, process packets at 100Hz - // if 1000ms have elapsed since we last got a packet, return to software connection state - if Instant::now() - self.last_software_packet > Duration::from_millis(1000) { - //self.connection_state = RadioConnectionState::ConnectSoftware; - //self.radio.close_peer().await.unwrap(); + // If timeout have elapsed since we last got a packet, + // reboot the robot (unless we had a shutdown request). + if !cur_robot_state.shutdown_requested && Instant::now() - self.last_software_packet > Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS) { cortex_m::peripheral::SCB::sys_reset(); - - // self.connection_state = RadioConnectionState::ConnectPhys; } }, } - // set global radio connected flag - self.shared_robot_state.set_radio_connection_ok(self.connection_state == RadioConnectionState::Connected); + // set global radio connected flag + self.shared_robot_state.set_radio_network_ok(self.connection_state >= RadioConnectionState::ConnectedNetwork); + self.shared_robot_state.set_radio_bridge_ok(self.connection_state == RadioConnectionState::Connected); + self.shared_robot_state.set_radio_inop(radio_inop_flag_local); + + if radio_inop_flag_local { + // If hardware problems is present, adds a delay. + Timer::after_millis(Self::RETRY_DELAY_MS).await; + } + else if radio_network_fail_local { + // If network problems is present, adds a delay. + Timer::after_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS).await; + } + + last_robot_state = cur_robot_state; radio_loop_rate_ticker.next().await; } @@ -281,15 +294,15 @@ impl< Either::First(res) => { if res.is_err() { defmt::error!("failed to establish radio UART connection."); - return Err(()) + return Err(()); } else { defmt::debug!("established radio uart coms"); - return Ok(()) + return Ok(()); } } Either::Second(_) => { defmt::error!("initial radio uart connection timed out"); - return Err(()) + return Err(()); } } } @@ -304,11 +317,11 @@ impl< let res = self.radio.open_multicast().await; if res.is_err() { defmt::error!("failed to establish multicast socket to network."); - return Err(()) + return Err(()); } defmt::info!("multicast open"); - return Ok(()) + return Ok(()); } async fn connect_software(&mut self, robot_id: u8, is_blue: bool) -> Result { @@ -321,10 +334,11 @@ impl< }; if self.radio.send_hello(robot_id, team_color).await.is_err() { - return Err(()) + defmt::error!("send hello failed."); + return Err(()); } - let hello = self.radio.wait_hello(Duration::from_millis(1000)).await; + let hello = self.radio.wait_hello(Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS)).await; match hello { Ok(hello) => { defmt::info!( @@ -337,10 +351,10 @@ impl< self.radio.open_unicast(hello.ipv4, hello.port).await.unwrap(); defmt::info!("unicast open"); - Ok(true) + return Ok(true); } Err(_) => { - Ok(false) + return Ok(false); } } } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index bd54bb42..c71db114 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -73,22 +73,36 @@ async fn user_io_task_entry( // let mut sd_anim_seq = [shutdown_anim]; // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); - const SHUTDOWN_ANIM_ID: usize = 2; + const SHUTDOWN_ANIM_ID: usize = 0; let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(20100), AnimRepeatMode::None)); - let mut shutdown_dimmer_arr = [shutdown_dimmer]; - let shutdown_ca = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_dimmer_arr, AnimRepeatMode::None); + let mut shutdown_anim_seq = [shutdown_dimmer]; + let shutdown_comp_anim = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_anim_seq, AnimRepeatMode::None); - let mut led0_anim_playbook = [shutdown_ca]; + const ERROR_ANIM_ID: usize = 1; + let error_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); + let mut error_blink_seq = [error_blink]; + let error_blink_comp_anim = CompositeAnimation::new(ERROR_ANIM_ID, &mut error_blink_seq, AnimRepeatMode::Forever); - const RGB_LERP_ID: usize = 1; - let anim0 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim1 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let anim2 = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let mut anim_seq = [anim0, anim1, anim2]; - let composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut anim_seq, AnimRepeatMode::Forever); - // let mut composite_anim0 = CompositeAnimation::new(RGB_LERP_ID, &mut [anim0, anim1, anim2], AnimRepeatMode::Forever); + const NETWORK_OK_ANIM_ID: usize = 2; + let network_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); + let mut network_blink_seq = [network_ok_blink]; + let network_ok_blink_comp_anim = CompositeAnimation::new(NETWORK_OK_ANIM_ID, &mut network_blink_seq, AnimRepeatMode::Forever); - let mut led1_anim_playbook = [composite_anim0]; + const BRIDGE_OK_ANIM_ID: usize = 3; + let bridge_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); + let mut bridge_ok_blink_seq = [bridge_ok_blink]; + let bridge_ok_blink_comp_anim = CompositeAnimation::new(BRIDGE_OK_ANIM_ID, &mut bridge_ok_blink_seq, AnimRepeatMode::Forever); + + let mut led0_anim_playbook = [shutdown_comp_anim, error_blink_comp_anim, network_ok_blink_comp_anim, bridge_ok_blink_comp_anim]; + + const RGB_LERP_ID: usize = 0; + let red_to_green_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let green_to_blue_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let blue_to_red_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); + let mut rgb_blend_seq = [red_to_green_anim, green_to_blue_anim, blue_to_red_anim]; + let rgb_blend_anim = CompositeAnimation::new(RGB_LERP_ID, &mut rgb_blend_seq, AnimRepeatMode::Forever); + + let mut led1_anim_playbook = [rgb_blend_anim]; // composite_anim0.start_animation(); @@ -124,11 +138,20 @@ async fn user_io_task_entry( // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); // color_lerp.start(); - // Battery ADC Setup + /////////////////////// + // Battery ADC Setup // + /////////////////////// + // Get the Vref_int calibration values. let vref_int_cal = get_vref_int_cal() as f32; let mut vref_int_ch = vref_int_adc.enable_vrefint(); + // Create the running buffers for averaging + let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = + [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; + + let mut battery_voltage_filt_indx = 0; + // let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); // anim1.start_animation(); // dotstars_anim.set_animation(anim1, 1); @@ -137,11 +160,6 @@ async fn user_io_task_entry( // blink_anim.start_animation(); // dotstars.set_animation(blink_anim, 1); - let mut battery_voltage_buffer: [f32; BATTERY_BUFFER_SIZE] = - [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE]; - - let mut battery_voltage_filt_indx = 0; - let mut prev_robot_state = robot_state.get_state(); loop { let cur_robot_state = robot_state.get_state(); @@ -215,15 +233,26 @@ async fn user_io_task_entry( debug_led0.set_low(); } + // LED states cascade down. if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { - dotstars_anim.disable_animation(0, RGB_LERP_ID); - dotstars_anim.enable_animation(1, SHUTDOWN_ANIM_ID); + let _ = dotstars_anim.disable_animation(0, RGB_LERP_ID); + let _ = dotstars_anim.enable_animation(1, SHUTDOWN_ANIM_ID); } + // TODO THESE DON'T WORK BUT LED BAD (for now) + else if cur_robot_state.radio_inop { + let _ = dotstars_anim.enable_animation(1, ERROR_ANIM_ID); + } else if cur_robot_state.radio_bridge_ok { + let _ = dotstars_anim.enable_animation(1, BRIDGE_OK_ANIM_ID); + } else if cur_robot_state.radio_network_ok { + let _ = dotstars_anim.enable_animation(1, NETWORK_OK_ANIM_ID); + } // let color = color_lerp.update(); // dotstars.set_color(color, 0); // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); - dotstars_anim.update().await; + // TODO THIS BRICKS EVERYTHING SOMETHING SOMETHING SPI + // SOMETHING SOMETHING UART + //dotstars_anim.update().await; if !robot_state.hw_init_state_valid() { diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index 4a0645eb..c04e8049 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -6,17 +6,23 @@ use smart_leds::RGB8; use crate::anim::{AnimInterface, CompositeAnimation}; +const START_FRAME_SIZE: usize = 4; +const COLOR_FRAME_SIZE: usize = 4; +const END_FRAME_SIZE: usize = 4; +const HEADER_FRAME_SIZE: usize = START_FRAME_SIZE + END_FRAME_SIZE; + + pub struct Apa102<'a, 'buf, const NUM_LEDS: usize> -where [(); (NUM_LEDS * 4) + 8]: { +where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { spi: spi::Spi<'a, Async>, - spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], } impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> -where [(); (NUM_LEDS * 4) + 8]: { +where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { pub fn new( spi: spi::Spi<'a, Async>, - spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], ) -> Self { // set start frame spi_buf[0] = 0x00; @@ -25,10 +31,10 @@ where [(); (NUM_LEDS * 4) + 8]: { spi_buf[3] = 0x00; // set end frame - spi_buf[8 + (NUM_LEDS * 4) - 4] = 0xFF; - spi_buf[8 + (NUM_LEDS * 4) - 3] = 0xFF; - spi_buf[8 + (NUM_LEDS * 4) - 2] = 0xFF; - spi_buf[8 + (NUM_LEDS * 4) - 1] = 0xFF; + spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 4] = 0xFF; + spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 3] = 0xFF; + spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 2] = 0xFF; + spi_buf[HEADER_FRAME_SIZE + (NUM_LEDS * COLOR_FRAME_SIZE) - 1] = 0xFF; Apa102 { spi: spi, @@ -41,7 +47,7 @@ where [(); (NUM_LEDS * 4) + 8]: { sck_pin: impl Peripheral

> + 'a, mosi_pin: impl Peripheral

> + 'a, tx_dma: impl Peripheral

> + 'a, - spi_buf: &'buf mut [u8; (NUM_LEDS * 4) + 8], + spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], ) -> Self { let mut dotstar_spi_config = Config::default(); dotstar_spi_config.frequency = mhz(1); @@ -58,26 +64,30 @@ where [(); (NUM_LEDS * 4) + 8]: { } const fn l2d(led_index: usize) -> usize { - 4 + (led_index * 4) + 0 + START_FRAME_SIZE + (led_index * COLOR_FRAME_SIZE) + 0 } + // Calculates the frame index for red based on LED index. const fn l2r(led_index: usize) -> usize { - 4 + (led_index * 4) + 3 + START_FRAME_SIZE + (led_index * COLOR_FRAME_SIZE) + 3 } + // Calculates the frame index for green based on LED index. const fn l2g(led_index: usize) -> usize { - 4 + (led_index * 4) + 2 + START_FRAME_SIZE + (led_index * COLOR_FRAME_SIZE) + 2 } + // Calculates the frame index for blue based on LED index. const fn l2b(led_index: usize) -> usize { - 4 + (led_index * 4) + 1 + START_FRAME_SIZE + (led_index * COLOR_FRAME_SIZE) + 1 } - pub fn set_drv_str(&mut self, str: u8, led_index: usize) { + pub fn set_drv_str(&mut self, strength: u8, led_index: usize) { assert!(led_index < NUM_LEDS); - - let str = ((str >> 3) & 0x1F) | 0xE0; - self.spi_buf[Self::l2d(led_index)] = str; + // The top 3 bits always high (0xE0) + // Bottom 5 bits are the intensity. + let strength_shift = ((strength >> 3) & 0x1F) | 0xE0; + self.spi_buf[Self::l2d(led_index)] = strength_shift; } pub fn set_drv_str_range(&mut self, str: u8, led_index_range: Range) { @@ -114,7 +124,7 @@ where [(); (NUM_LEDS * 4) + 8]: { } pub struct Apa102Anim<'a, 'buf, 'ca, const NUM_LEDS: usize> -where [(); (NUM_LEDS * 4) + 8]: { +where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { apa102_driver: Apa102<'a, 'buf, NUM_LEDS>, active_animation: [usize; NUM_LEDS], animation_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS], @@ -123,7 +133,7 @@ where [(); (NUM_LEDS * 4) + 8]: { } impl<'a, 'buf, 'ca, const NUM_LEDS: usize> Apa102Anim<'a, 'buf, 'ca, NUM_LEDS> -where [(); (NUM_LEDS * 4) + 8]: { +where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { pub fn new(apa102: Apa102<'a, 'buf, NUM_LEDS>, anim_playbook_buf: [Option<&'ca mut [CompositeAnimation<'ca, u8, RGB8>]>; NUM_LEDS]) -> Self { Apa102Anim { apa102_driver: apa102, @@ -210,11 +220,11 @@ where [(); (NUM_LEDS * 4) + 8]: { if let Some(animation_playbook) = self.animation_playbook_buf[led_index].as_deref_mut() { for composite_animation in animation_playbook.iter_mut() { - defmt::info!("evaluating animation {}", composite_animation.get_id()); + //defmt::info!("evaluating animation {}", composite_animation.get_id()); if composite_animation.get_id() == anim_id { if enable { - defmt::info!("enabling animation"); + //defmt::info!("enabling animation"); composite_animation.enable(); composite_animation.start_animation(); } else { diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index a888dc8e..62652204 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -99,10 +99,10 @@ int main() { // setup the loop rate regulators SyncTimer_t vel_loop_timer; SyncTimer_t torque_loop_timer; - SyncTimer_t telemetry_timer; + //SyncTimer_t telemetry_timer; time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); - time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); + //time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); // setup the velocity filter IIRFilter_t encoder_filter; @@ -276,7 +276,7 @@ int main() { // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); bool run_vel_loop = time_sync_ready_rst(&vel_loop_timer); - bool run_telemtry = time_sync_ready_rst(&telemetry_timer); + //bool run_telemtry = time_sync_ready_rst(&telemetry_timer); // run the torque loop if applicable if (run_torque_loop) { @@ -435,9 +435,9 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (run_telemtry) { + //if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); - } + //} // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif From a35b0a4acfe538b16f1b8414bb986459d1580c5d Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 06:42:12 +0200 Subject: [PATCH 131/157] Clarified a log of kicker connected --- control-board/src/tasks/kicker_task.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 7e53ec52..999170bf 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -140,7 +140,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } else { self.kicker_task_state = KickerTaskState::Connected; - defmt::info!("kicker flashed!"); + defmt::info!("kicker booted!"); } }, KickerTaskState::Connected => { From dd4e355bbb92742329debc1b124ae02ba625bdde Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 06:42:39 +0200 Subject: [PATCH 132/157] Uhh missed the change of the last commit? --- control-board/src/tasks/kicker_task.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 999170bf..9bcaf27e 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -140,7 +140,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ } else { self.kicker_task_state = KickerTaskState::Connected; - defmt::info!("kicker booted!"); + defmt::info!("kicker connected!"); } }, KickerTaskState::Connected => { From 982325fe41195f107ca4748ccb44c8765830fa2b Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 12:42:16 +0200 Subject: [PATCH 133/157] Code ran against RIONE --- control-board/src/bin/control/main.rs | 8 +- control-board/src/robot_state.rs | 30 ++++++- control-board/src/tasks/control_task.rs | 16 +++- control-board/src/tasks/kicker_task.rs | 6 +- control-board/src/tasks/user_io_task.rs | 115 +++++------------------- lib-stm32/src/drivers/led/apa102.rs | 15 +++- motor-controller/bin/wheel/main.c | 7 +- 7 files changed, 88 insertions(+), 109 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 66ee3312..52ca7f7b 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -110,12 +110,12 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); - create_audio_task!(main_spawner, - robot_state, - p); + // TODO Come back to. Extra compute. + //create_audio_task!(main_spawner, + // robot_state, + // p); create_radio_task!(main_spawner, radio_uart_queue_spawner, uart_queue_spawner, - // create_radio_task!(main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, wifi_credentials, diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index bf92e6e3..ec309783 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -68,9 +68,9 @@ impl SharedRobotState { radio_inop: self.get_radio_inop(), imu_inop: self.get_imu_inop(), - kicker_inop: true, - wheels_inop: 0xFF, - dribbler_inop: true, + kicker_inop: self.get_kicker_inop(), + wheels_inop: self.get_wheels_inop(), + dribbler_inop: self.get_dribbler_inop(), last_packet_receive_time_ticks_ms: 0, radio_network_ok: self.get_radio_network_ok(), @@ -151,6 +151,30 @@ impl SharedRobotState { self.imu_inop.store(imu_inop, Ordering::Relaxed); } + pub fn get_wheels_inop(&self) -> u8 { + self.wheels_inop.load(Ordering::Relaxed) + } + + pub fn set_wheels_inop(&self, wheels_inop: u8) { + self.wheels_inop.store(wheels_inop, Ordering::Relaxed); + } + + pub fn get_dribbler_inop(&self) -> bool { + self.dribbler_inop.load(Ordering::Relaxed) + } + + pub fn set_dribbler_inop(&self, drib_inop: bool) { + self.dribbler_inop.store(drib_inop, Ordering::Relaxed); + } + + pub fn get_kicker_inop(&self) -> bool { + self.kicker_inop.load(Ordering::Relaxed) + } + + pub fn set_kicker_inop(&self, kicker_inop: bool) { + self.kicker_inop.store(kicker_inop, Ordering::Relaxed); + } + pub fn shutdown_requested(&self) -> bool { self.shutdown_requested.load(Ordering::Relaxed) } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index ae2ae9c6..65b46500 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -304,7 +304,7 @@ impl < } else { // Battery is too low, set velocity to zero drib_vel = 0.0; - defmt::warn!("CT - low batter command lockout"); + defmt::warn!("CT - low battery / shutting down command lockout"); Vector4::new(0.0, 0.0, 0.0, 0.0) }; @@ -325,7 +325,7 @@ impl < async fn flash_motor_firmware(&mut self, debug: bool) { defmt::info!("flashing firmware"); - if debug { + if true { let mut had_motor_error = false; if self.motor_fl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FL"); @@ -377,8 +377,16 @@ impl < ) .await; - if res.0.is_err() || res.1.is_err() || res.2.is_err() || res.3.is_err() { - defmt::error!("failed to flash drive motor: {}", res); + let error_mask = res.0.is_err() as u8 + | ((res.1.is_err() as u8) & 0x01) << 1 + | ((res.2.is_err() as u8) & 0x01) << 2 + | ((res.3.is_err() as u8) & 0x01) << 3; + + self.shared_robot_state.set_wheels_inop(error_mask); + self.shared_robot_state.set_dribbler_inop(res.4.is_err()); + + if error_mask != 0 { + defmt::error!("failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", res); } else { defmt::debug!("motor firmware flashed"); } diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 9bcaf27e..bfe9aafa 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -99,10 +99,10 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ pub async fn kicker_task_entry(&mut self) { let mut main_loop_ticker = Ticker::every(Duration::from_hz(100)); - + loop { let cur_robot_state = self.robot_state.get_state(); - + // Assume not working, clear if connected. self.kicker_driver.process_telemetry(); // TODO global state overrides of kicker state @@ -178,6 +178,8 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ self.robot_state.set_ball_detected(ball_detected); } + self.robot_state.set_kicker_inop(self.kicker_task_state < KickerTaskState::Connected); + main_loop_ticker.next().await; } } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index c71db114..71a9dfc5 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -7,7 +7,7 @@ use embassy_stm32::spi::{Config, Spi}; use embassy_stm32::time::mhz; use embassy_time::{Duration, Timer}; -use smart_leds::colors::{BLACK, WHITE}; +use smart_leds::colors::{BLACK, GREEN, ORANGE, RED, WHITE, YELLOW}; use smart_leds::RGB8; use static_cell::ConstStaticCell; @@ -69,74 +69,7 @@ async fn user_io_task_entry( ) { defmt::info!("user io task initialized"); - // let shutdown_anim = anim::Animation::Lerp(Lerp::new(WHITE, BLACK, Duration::from_millis(HARD_SHUTDOWN_TIME_MS), AnimRepeatMode::None)); - // let mut sd_anim_seq = [shutdown_anim]; - // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); - - const SHUTDOWN_ANIM_ID: usize = 0; - let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(20100), AnimRepeatMode::None)); - let mut shutdown_anim_seq = [shutdown_dimmer]; - let shutdown_comp_anim = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_anim_seq, AnimRepeatMode::None); - - const ERROR_ANIM_ID: usize = 1; - let error_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut error_blink_seq = [error_blink]; - let error_blink_comp_anim = CompositeAnimation::new(ERROR_ANIM_ID, &mut error_blink_seq, AnimRepeatMode::Forever); - - const NETWORK_OK_ANIM_ID: usize = 2; - let network_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut network_blink_seq = [network_ok_blink]; - let network_ok_blink_comp_anim = CompositeAnimation::new(NETWORK_OK_ANIM_ID, &mut network_blink_seq, AnimRepeatMode::Forever); - - const BRIDGE_OK_ANIM_ID: usize = 3; - let bridge_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut bridge_ok_blink_seq = [bridge_ok_blink]; - let bridge_ok_blink_comp_anim = CompositeAnimation::new(BRIDGE_OK_ANIM_ID, &mut bridge_ok_blink_seq, AnimRepeatMode::Forever); - - let mut led0_anim_playbook = [shutdown_comp_anim, error_blink_comp_anim, network_ok_blink_comp_anim, bridge_ok_blink_comp_anim]; - - const RGB_LERP_ID: usize = 0; - let red_to_green_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let green_to_blue_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let blue_to_red_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let mut rgb_blend_seq = [red_to_green_anim, green_to_blue_anim, blue_to_red_anim]; - let rgb_blend_anim = CompositeAnimation::new(RGB_LERP_ID, &mut rgb_blend_seq, AnimRepeatMode::Forever); - - let mut led1_anim_playbook = [rgb_blend_anim]; - - // composite_anim0.start_animation(); - - // let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ - // Some(&mut [ - // CompositeAnimation::new(RGB_LERP_ID, &mut [ - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // ], AnimRepeatMode::Forever) - // ]), - // None, - // ]; - - let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ - Some(&mut led1_anim_playbook), - Some(&mut led0_anim_playbook), - ]; - - // let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let anim1_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 255 }, RGB8 { r: 255, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let anim2_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 255 }, RGB8 { r: 255, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let mut anim_seq = [anim0_1, anim1_1, anim2_1]; - // let mut composite_anim1 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); - // composite_anim1.start_animation(); - dotstars.set_drv_str_all(32); - let mut dotstars_anim = Apa102Anim::new(dotstars, animation_playbooks); - - dotstars_anim.enable_animation(0, RGB_LERP_ID); - // dotstars_anim.enable_animation(1, RGB_LERP_ID); - - // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); - // color_lerp.start(); /////////////////////// // Battery ADC Setup // @@ -152,14 +85,6 @@ async fn user_io_task_entry( let mut battery_voltage_filt_indx = 0; - // let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); - // anim1.start_animation(); - // dotstars_anim.set_animation(anim1, 1); - - // let mut blink_anim = Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), AnimRepeatMode::Forever); - // blink_anim.start_animation(); - // dotstars.set_animation(blink_anim, 1); - let mut prev_robot_state = robot_state.get_state(); loop { let cur_robot_state = robot_state.get_state(); @@ -233,27 +158,33 @@ async fn user_io_task_entry( debug_led0.set_low(); } - // LED states cascade down. - if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { - let _ = dotstars_anim.disable_animation(0, RGB_LERP_ID); - let _ = dotstars_anim.enable_animation(1, SHUTDOWN_ANIM_ID); - } - // TODO THESE DON'T WORK BUT LED BAD (for now) - else if cur_robot_state.radio_inop { - let _ = dotstars_anim.enable_animation(1, ERROR_ANIM_ID); + // LED 0 states cascade down. + if cur_robot_state.shutdown_requested { + let _ = dotstars.set_color(WHITE, 0); + } else if cur_robot_state.radio_inop { + let _ = dotstars.set_color(RED, 0); } else if cur_robot_state.radio_bridge_ok { - let _ = dotstars_anim.enable_animation(1, BRIDGE_OK_ANIM_ID); + let _ = dotstars.set_color(GREEN, 0); } else if cur_robot_state.radio_network_ok { - let _ = dotstars_anim.enable_animation(1, NETWORK_OK_ANIM_ID); + let _ = dotstars.set_color(ORANGE, 0); + } else { + let _ = dotstars.set_color(BLACK, 0); } - // let color = color_lerp.update(); - // dotstars.set_color(color, 0); - // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); - // TODO THIS BRICKS EVERYTHING SOMETHING SOMETHING SPI - // SOMETHING SOMETHING UART - //dotstars_anim.update().await; + // LED 1 states cascade down. + if cur_robot_state.shutdown_requested { + let _ = dotstars.set_color(WHITE, 1); + } else if cur_robot_state.wheels_inop != 0 { + let _ = dotstars.set_color(RED, 1); + } else if cur_robot_state.imu_inop { + let _ = dotstars.set_color(ORANGE, 1); + } else if cur_robot_state.kicker_inop { + let _ = dotstars.set_color(YELLOW, 1); + } else { + let _ = dotstars.set_color(BLACK, 1); + } + //dotstars.update().await; if !robot_state.hw_init_state_valid() { defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index c04e8049..60199926 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -16,6 +16,7 @@ pub struct Apa102<'a, 'buf, const NUM_LEDS: usize> where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], + needs_update: bool, } impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> @@ -39,6 +40,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { Apa102 { spi: spi, spi_buf: spi_buf, + needs_update: false, } } @@ -103,6 +105,12 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { pub fn set_color(&mut self, color: RGB8, led_index: usize) { assert!(led_index < NUM_LEDS); + if self.spi_buf[Self::l2r(led_index)] != color.r + || self.spi_buf[Self::l2g(led_index)] != color.g + || self.spi_buf[Self::l2b(led_index)] != color.b { + self.needs_update = true; + } + self.spi_buf[Self::l2r(led_index)] = color.r; self.spi_buf[Self::l2g(led_index)] = color.g; self.spi_buf[Self::l2b(led_index)] = color.b; @@ -119,7 +127,10 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { } pub async fn update(&mut self) { - let _ = self.spi.write(self.spi_buf).await; + if self.needs_update { + let _ = self.spi.write(self.spi_buf).await; + self.needs_update = false; + } } } @@ -150,7 +161,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { let mut cur_index = search_start_ind; loop { // inc and loop the index - cur_index = cur_index + 1 % anim_playbook.len(); + cur_index = (cur_index + 1) % anim_playbook.len(); // we incremented and found an enabled animation // return the index diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 62652204..59d1ca56 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -171,7 +171,7 @@ int main() { #ifdef UART_ENABLED // increment the soft watchdog - ticks_since_last_command_packet++; + //ticks_since_last_command_packet++; // process all available packets while (uart_can_read()) { @@ -261,7 +261,10 @@ int main() { // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; // HACK Will force the watchdog to trigger. - // while(true); + //while(true) { + // NVIC_SystemReset(); + // while(true); + //} } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; } From 0952eb88d16a3507a37748a946005c72368d7bb1 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 16:21:20 +0200 Subject: [PATCH 134/157] Firmware used against IT Androids --- .../src/motion/params/body_vel_pid_params.rs | 6 +- control-board/src/stm32_interface.rs | 8 +- control-board/src/tasks/control_task.rs | 2 +- control-board/src/tasks/shutdown_task.rs | 6 +- motor-controller/bin/wheel/main.c | 3 +- motor-controller/common/motor_model.c | 7 +- util/udev/99-jlink.rules | 106 +++++ util/udev/99-jlink.rules:Zone.Identifier | 0 util/win11/win11_setup.ps1 | 374 ++++++++++++++++++ util/win11/win11_setup.ps1:Zone.Identifier | 3 + util/win11/win11_usb_daemon.ps1 | 25 ++ .../win11_usb_daemon.ps1:Zone.Identifier | 3 + util/win11/win11_usb_daemon.py | 229 +++++++++++ .../win11/win11_usb_daemon.py:Zone.Identifier | 3 + 14 files changed, 761 insertions(+), 14 deletions(-) create mode 100644 util/udev/99-jlink.rules create mode 100644 util/udev/99-jlink.rules:Zone.Identifier create mode 100644 util/win11/win11_setup.ps1 create mode 100644 util/win11/win11_setup.ps1:Zone.Identifier create mode 100644 util/win11/win11_usb_daemon.ps1 create mode 100644 util/win11/win11_usb_daemon.ps1:Zone.Identifier create mode 100644 util/win11/win11_usb_daemon.py create mode 100644 util/win11/win11_usb_daemon.py:Zone.Identifier diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index f92d12a4..d0c962cb 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -7,7 +7,11 @@ pub static PID_GAIN: Matrix3x5 = 4.0, 0.0, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) -pub static BODY_VEL_LIM: Vector3 = vector![5.0, 5.0, 30.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +// NOTE: Because of (bad) motor model, need to give way +// higher constraints to allow controller to overcome +// static friction / loading. +// 8, 8, 34.9 maxes out motors/IMU measurement rate +pub static BODY_VEL_LIM: Vector3 = vector![8.0, 8.0, 40.0]; pub static BODY_ACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore pub static BODY_DEACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 0703edc5..e2561d42 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -3,7 +3,7 @@ use core::cmp::min; use defmt_rtt as _; use defmt::*; -use embassy_stm32::gpio::{Level, Output, Pin, Speed}; +use embassy_stm32::gpio::{Level, Output, OutputOpenDrain, Pin, Speed}; use embassy_stm32::pac; use embassy_stm32::usart::{self, Config, Parity, StopBits}; use embassy_time::{Duration, Timer}; @@ -52,7 +52,7 @@ pub struct Stm32Interface< reader: &'a UartReadQueue, writer: &'a UartWriteQueue, boot0_pin: Output<'a>, - reset_pin: Output<'a>, + reset_pin: OutputOpenDrain<'a>, reset_pin_noninverted: bool, @@ -74,7 +74,7 @@ impl< read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, boot0_pin: Output<'a>, - reset_pin: Output<'a>, + reset_pin: OutputOpenDrain<'a>, reset_polarity_high: bool, ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { Stm32Interface { @@ -101,7 +101,7 @@ impl< } else { Level::High }; - let reset_output = Output::new(reset_pin, initial_reset_level, Speed::Medium); + let reset_output = OutputOpenDrain::new(reset_pin, initial_reset_level, Speed::Medium, embassy_stm32::gpio::Pull::None); Stm32Interface { reader: read_queue, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 65b46500..dcee3c63 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -325,7 +325,7 @@ impl < async fn flash_motor_firmware(&mut self, debug: bool) { defmt::info!("flashing firmware"); - if true { + if debug { let mut had_motor_error = false; if self.motor_fl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FL"); diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs index e01e773f..70e99e8e 100644 --- a/control-board/src/tasks/shutdown_task.rs +++ b/control-board/src/tasks/shutdown_task.rs @@ -6,7 +6,7 @@ use embassy_time::Timer; use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::{self, SharedRobotState}}; -pub const HARD_SHUTDOWN_TIME_MS: u64 = 20000; +pub const HARD_SHUTDOWN_TIME_MS: u64 = 15000; #[macro_export] macro_rules! create_shutdown_task { @@ -32,12 +32,12 @@ async fn shutdown_task_entry(robot_state: &'static SharedRobotState, defmt::warn!("shutdown initiated via user btn! syncing..."); - // wait for tasks to flag shutdown complete, or hard temrinate after 10s + // wait for tasks to flag shutdown complete, or hard temrinate after hard shutdown time select::select(async move { loop { Timer::after_millis(100).await; - if robot_state.kicker_shutdown_complete() { + if robot_state.kicker_shutdown_complete() || robot_state.get_kicker_inop() { break; } } diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 59d1ca56..2c8ac3b8 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -171,7 +171,7 @@ int main() { #ifdef UART_ENABLED // increment the soft watchdog - //ticks_since_last_command_packet++; + ticks_since_last_command_packet++; // process all available packets while (uart_can_read()) { @@ -261,6 +261,7 @@ int main() { // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; // HACK Will force the watchdog to trigger. + while(true); //while(true) { // NVIC_SystemReset(); // while(true); diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c index e3583cc6..c3dd1ad8 100644 --- a/motor-controller/common/motor_model.c +++ b/motor-controller/common/motor_model.c @@ -27,12 +27,11 @@ float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) { } // Linear mapping from 'motor_model.py' in hw-analysis - //float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + - // ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); + float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + + ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); - float map_value = avel_rads / mm->max_vel_rads; // bound DC [-1, 1] - return fmax(fmin(map_value, 1.0f), -1.0f); + return fmax(fmin(2.0f * map_value, 1.0f), -1.0f); } float mm_enc_ticks_to_rev(MotorModel_t *mm, float enc_ticks) { diff --git a/util/udev/99-jlink.rules b/util/udev/99-jlink.rules new file mode 100644 index 00000000..6504b25f --- /dev/null +++ b/util/udev/99-jlink.rules @@ -0,0 +1,106 @@ +# +# This file is going to be stored at /etc/udev/rules.d on installation of the J-Link package +# It makes sure that non-superuser have access to the connected J-Links, so JLinkExe etc. can be executed as non-superuser and can work with J-Link +# +# +# Matches are AND combined, meaning: a==b,c==d,do stuff +# results in: if (a == b) && (c == d) -> do stuff +# +ACTION!="add", SUBSYSTEM!="usb_device", GOTO="jlink_rules_end" +# +# Give all users read and write access. +# Note: NOT all combinations are supported by J-Link right now. Some are reserved for future use, but already added here +# +# 0x0101 - J-Link (default) +# 0x0102 - J-Link USBAddr = 1 (obsolete) +# 0x0103 - J-Link USBAddr = 2 (obsolete) +# 0x0104 - J-Link USBAddr = 3 (obsolete) +# 0x0105 - CDC + J-Link +# 0x0106 - CDC +# 0x0107 - RNDIS + J-Link +# 0x0108 - J-Link + MSD +# 0x1000 - MSD +# +# ATTR{filename} +# Match sysfs attribute values of the event device. Trailing +# whitespace in the attribute values is ignored unless the specified +# match value itself contains trailing whitespace. +# +# ATTRS{filename} +# Search the devpath upwards for a device with matching sysfs +# attribute values. If multiple ATTRS matches are specified, all of +# them must match on the same device. Trailing whitespace in the +# attribute values is ignored unless the specified match value itself +# contains trailing whitespace. +# +# How to find out about udev attributes of device: +# Connect J-Link to PC +# Terminal: cat /var/log/syslog +# Find path to where J-Link device has been "mounted" +# sudo udevadm info --query=all --attribute-walk --path= +# +ATTR{idProduct}=="0101", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0102", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0103", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0104", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0105", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0107", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0108", ATTR{idVendor}=="1366", MODE="666" +# +# 0x1000 not added yet +# +# New PID scheme, for all possible combinations +# 0x1001: MSD +# 0x1002: RNDIS +# 0x1003: RNDIS + MSD +# 0x1004: CDC +# 0x1005: CDC + MSD +# 0x1006: RNDIS + CDC +# 0x1007: RNDIS + CDC + MSD +# 0x1008: HID +# 0x1009: MSD + HID +# 0x100a: RNDIS + HID +# 0x100b: RNDIS + MSD + HID +# 0x100c: CDC + HID +# 0x100d: CDC + MSD + HID +# 0x100e: RNDIS + CDC + HID +# 0x100f: RNDIS + CDC + MSD + HID +# 0x1010: J_LINK +# 0x1011: J_LINK + MSD +# 0x1012: RNDIS + J_LINK +# 0x1013: RNDIS + J_LINK + MSD +# 0x1014: CDC + J_LINK +# 0x1015: CDC + J_LINK + MSD +# 0x1016: RNDIS + CDC + J_LINK +# 0x1017: RNDIS + CDC + J_LINK + MSD +# 0x1018: J_LINK + HID +# 0x1019: J_LINK + MSD + HID +# 0x101a: RNDIS + J_LINK + HID +# 0x101b: RNDIS + J_LINK + MSD + HID +# 0x101c: CDC + J_LINK + HID +# 0x101d: CDC + J_LINK + MSD + HID +# 0x101e: RNDIS + CDC + J_LINK + HID +# 0x101f: RNDIS + CDC + J_LINK + MSD + HID +# +# 0x1001 - 0x100f not added yet +# +ATTR{idProduct}=="1010", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1011", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1012", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1013", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1014", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1015", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1016", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1017", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1018", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1019", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101A", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101B", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101C", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101D", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101E", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101F", ATTR{idVendor}=="1366", MODE="666" +# +# End of list +# +LABEL="jlink_rules_end" diff --git a/util/udev/99-jlink.rules:Zone.Identifier b/util/udev/99-jlink.rules:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/util/win11/win11_setup.ps1 b/util/win11/win11_setup.ps1 new file mode 100644 index 00000000..be1a5964 --- /dev/null +++ b/util/win11/win11_setup.ps1 @@ -0,0 +1,374 @@ + +param ( + [Switch]$help, + [Switch]$install +) + +$invocationPath = $MyInvocation.MyCommand.Path + +Remove-Variable WSL_REQUIRED_CORE_VERSION -force -ErrorAction SilentlyContinue +Remove-Variable WSL_MIN_KERNEL_VERSION -force -ErrorAction SilentlyContinue +Set-Variable WSL_REQUIRED_CORE_VERSION -option ReadOnly -scope Script -value ([String]"2") +Set-Variable WSL_MIN_KERNEL_VERSION -option ReadOnly -scope Script -value ([String]"5.10.16") + +Remove-Variable WINGET_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable WINGET_MIN_VERSION -option ReadOnly -scope Script -value ([String]"1.7.10861") + +Remove-Variable GIT_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable GIT_MIN_VERSION -option ReadOnly -scope Script -value ([String]"2.33.0") + +Remove-Variable PYTHON_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable PYTHON_MIN_VERSION -option ReadOnly -scope Script -value ([String]"3.9.0") + +Remove-Variable USBIPD_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable USBIPD_MIN_VERSION -option ReadOnly -scope Script -value ([String]"4.1.0") + + + +function Write-Help { + Write-Host "Usage: ./win11_setup.ps1 -help | -install" + Write-Host "" + Write-Host "-help: print this message" + Write-Host "-install: will attempt to install rather than notify of missing dependencies. Requires admin" + Write-Host "" +} + +#################### +# WSL2 Utilities # +#################### + +function Confirm-wslIsInstalled { + $wslStatusOutput = @(wsl --status) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput.Contains("Default Version")) { + Write-Host "OK-ACK. WSL is installed" + return $true + } else { + Write-Host "OK-NCK. WSL is not installed" + } + + return $false +} + +function Confirm-wslCoreVersion { + $wslStatusOutput = @(wsl --status) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput -like "*Default Version: $WSL_REQUIRED_CORE_VERSION*") { + Write-Host "OK-ACK. WSL Core Version is 2." + return $true + } else { + Write-Host "OK-NCK. WSL Core Version is not 2." + } + + return $false +} + +function Confirm-wslKernelVersion { + $wslStatusOutput = @(wsl --version) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput -match "Kernel version\: (\d+\.\d+\.\d+)") { + $kernelVersion = $Matches.1 + $kernelVersion = $kernelVersion.Trim() + if ([System.Version]($kernelVersion + ".0") -lt [System.Version]($WSL_MIN_KERNEL_VERSION + ".0")) { + Write-Host "OK-NCK. WSL Kernel Version is $kernelVersion" + } else { + Write-Host "OK-ACK. WSL Kernel Version is $kernelVersion" + return $true + } + } else { + Write-Host "ERROR. WSL Kernel Version not readable. This shouldn't be possible with a validated wsl install..." + Write-Host "Abort." + Exit 1 + } + + return $false +} + +############ +# Python # +############ + +function Confirm-Python3 { + if ($null -eq (Get-Command "python" -ErrorAction SilentlyContinue)) { + return $false + } + + # ughhh windows please don't alias a message script to a missing program + # existense of the program is how scripts check for it ya f*** + # on top of that, it sends it to stderr so it's not captured by other default checks + $output = @(python --version) 2>&1 | Out-String + $output = ([string]($output)).Replace("`0", "") + # Write-Host $output + + if (!([String]($output) -match "Python was not found")) { + $output = @(python --version) | Out-String + $output = $output.Replace("`0", "") + + $res = [Regex]::Match($output, ".*Python (\d+\.\d+\.\d+).*") + if (($null -eq $res) -or (!($res.Success))) { + Write-Host "ERROR. Unkown python version output format." + return $false + } + + $detectedVersionString = $res.Groups[1].Value + $detectedVersionString_WV = $detectedVersionString + ".0" + $pythonMinVersion_WV = $PYTHON_MIN_VERSION + ".0" + if (!([System.Version]($detectedVersionString_WV) -lt [System.Version]($pythonMinVersion_WV))) { + Write-Host "OK-ACK. Python version is $detectedVersionString." + return $true + } + } else { + Write-Host "OK-NCK. Python not installed. System probably has windows store shim." + } + + return $false +} + +######### +# Git # +######### + +function Confirm-gitIsInstalled { + $gitStatusOutput = @(git --version) | Out-String + $gitStatusOutput = $gitStatusOutput.Replace("`0", "") + if ($gitStatusOutput -match "git version (\d+\.\d+\.\d+).*") { + $gitVersion = $Matches.1 + $gitVersion = $gitVersion.Trim() + if ([System.Version]($gitVersion + ".0") -lt [System.Version]($GIT_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. Git version is $gitVersion" + } else { + Write-Host "OK-ACK. Git version is $gitVersion" + return $true + } + } + + return $false +} + +############ +# usbipd # +############ + +function Confirm-usbipd { + if (Get-Command -ErrorAction Ignore -Type Application usbipd) { + $usbipdStatusOutput = @(usbipd --version) | Out-String + $usbipdStatusOutput = $usbipdStatusOutput.Replace("`0", "") + if ($usbipdStatusOutput -match "(\d+\.\d+\.\d+).*") { + $usbipdVersion = $Matches.1 + $usbipdVersion = $usbipdVersion.Trim() + if ([System.Version]($usbipdVersion + ".0") -lt [System.Version]($USBIPD_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. usbipd version is $usbipdVersion" + } else { + Write-Host "OK-ACK. usbipd version is $usbipdVersion" + return $true + } + } else { + Write-Host "ERROR. cannot parse usbipd version" + } + } else { + Write-Host "OK-NCK. usbipd is not installed." + } + + return $false +} + +############ +# WINGET # +############ + +function Confirm-winget { + if (Get-Command -ErrorAction Ignore -Type Application winget) { + $wingetStatusOutput = @(winget --version) | Out-String + $wingetStatusOutput = $wingetStatusOutput.Replace("`0", "") + if ($wingetStatusOutput -match "v(\d+\.\d+\.\d+).*") { + $wingetVersion = $Matches.1 + $wingetVersion = $wingetVersion.Trim() + if ([System.Version]($wingetVersion + ".0") -lt [System.Version]($WINGET_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. Winget version is $wingetVersion" + } else { + Write-Host "OK-ACK. Winget version is $wingetVersion" + return $true + } + } else { + Write-Host "ERROR. cannot parse winget version" + } + } else { + Write-Host "ERROR. winget is not installed." + } + + return $false +} + +function Install-wingetPackage { + param ( + [string]$PackageName + ) + + $wingetInstallOutput = @(winget install --exact $PackageName) | Out-String + + if ($wingetInstallOutput -match "No package found") { + Write-Host "ERROR. Package not found." + return $false + } + + if ($wingetInstallOutput -match "Successfully installed") { + Write-Host "OK. Installed $PackageName via winget." + return $true + } + + if ($wingetInstallOutput -match "No newer package versions are available") { + Write-Host "OK. $PackageName already installed." + return $true + } + + Write-Host $wingetInstallOutput + Write-Host "ERROR. Unknown install status" + + return $false +} + +############ +# Script # +############ + +Write-Host "Checking critical versions..." + +$anyInstallPending = $false +$canInstallMissingItems = $true +$installWsl2 = $false +$installPython = $false +$installGit = $false +$installUsbipd = $false +$installWinget = $false + +if (!(Confirm-wslIsInstalled) -or !(Confirm-wslCoreVersion)) { + $installWsl2 = $true + $anyInstallPending = $true +} + +if (!(Confirm-wslKernelVersion)) { + Write-Host "CONFLICT. WSL kernel version is too old. Script cannot resolve." + $installWsl2 = $false + $canInstallMissingItems = $false + $anyInstallPending = $true +} + +if (!(Confirm-Python3)) { + $installPython = $true + $anyInstallPending = $true +} + +if (!(Confirm-gitIsInstalled)) { + $installGit = $true + $anyInstallPending = $true +} + +if (!(Confirm-usbipd)) { + $installUsbipd = $true + $anyInstallPending = $true +} + +if (!(Confirm-winget)) { + $installWinget = $true + $anyInstallPending = $true +} + +Write-Host "" +Write-Host "" + +if ($anyInstallPending) { + Write-Host "There are dependencies to install." + if ($installWsl2) { + Write-Host "`tWSL2" + } + + if ($installPython) { + Write-Host "`tPython" + } + + if ($installGit) { + Write-Host "`tGit" + } + + if ($installUsbipd) { + Write-Host "`tUsbipd" + } + + Write-Host "" +} else { + Write-Host "All dependencies valid." + Write-Host "" + + Exit 0 +} + + + +if (!($install)) { + Write-Host "Use the -install flag to install missing items." + Write-Host "" + + Exit 0 +} + +################## +# Install Step # +################## + +$installStepFailed = $false + +if ($installWsl2) { + Write-Host "UNWILLING. There are too many pitfalls to automate this easily." + Write-Host "Please install a WSL2 Ubuntu LTS distro from the store and re-run the script." + # NOTE: Will Stuckey has prior work here, but the hypervisor mode failures are esoteric + # and quite a pain to detect and support for arbitrary users. Make them do this manually. + Exit 1 +} + +if ($installWinget) { + Write-Host "UNABLE. Windows 11 should come with winget." + Write-Host "Unable to proceed with package installations." + Exit 1 +} + +if ($installPython) { + $success = Install-wingetPackage Python.Python.3.11 + if (!($success)) { + Write-Host "FAILED. Python install failed." + $installStepFailed = $true + } +} + +if ($installUsbipd) { + $success = Install-wingetPackage dorssel.usbipd-win + if (!($success)) { + Write-Host "FAILED. usbipd install failed." + $installStepFailed = $true + } +} + +if ($installGit) { + $success = Install-wingetPackage Git.Git + if (!($success)) { + Write-Host "FAILED. Git install failed." + $installStepFailed = $true + } +} + + +Write-Host "" +Write-Host "" + +if ($installStepFailed) { + Write-Host "ERROR. One or more install steps failed." + Write-Host "" + + Exit 1 +} + +Write-Host "OK-ACK. All dependencies installed." +Write-Host "NOTE. You will need to restart your shell." +Write-Host "" + +Exit 0 + diff --git a/util/win11/win11_setup.ps1:Zone.Identifier b/util/win11/win11_setup.ps1:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_setup.ps1:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip diff --git a/util/win11/win11_usb_daemon.ps1 b/util/win11/win11_usb_daemon.ps1 new file mode 100644 index 00000000..654522c3 --- /dev/null +++ b/util/win11/win11_usb_daemon.ps1 @@ -0,0 +1,25 @@ +$scriptFilePath = $MyInvocation.MyCommand.Path +$scriptDirPath = (Get-Item $scriptFilePath).DirectoryName + +function Confirm-AdministatorPrivledgesActive { + return ([Security.Principal.WindowsPrincipal] [Security.Principal.WindowsIdentity]::GetCurrent()).IsInRole([Security.Principal.WindowsBuiltInRole] 'Administrator') +} + +if (!(Confirm-AdministatorPrivledgesActive)) { + # user has not deliberately run the script as admin + # print text warning and pause so they know a UAC prompt is coming + Write-Host "Elevating to administrator runtime. You will prompted via UAC." + + # elevate + if ([int](Get-CimInstance -Class Win32_OperatingSystem | Select-Object -ExpandProperty BuildNumber) -ge 6000) { + Start-Process -FilePath PowerShell.exe -Verb Runas -ArgumentList $scriptFilePath + + Exit 0 + } else { + # this should only happen on like old Win 7/Vista machines... ether way WSL aint happening + Write-Host "Operating system build number is severely outdated. Install the latest patch set for windows 10 or 11. Unable to continue." + Exit 1 + } +} + +Start-Process -FilePath python -Verb Runas -ArgumentList "$scriptDirPath/win11_usb_daemon.py" diff --git a/util/win11/win11_usb_daemon.ps1:Zone.Identifier b/util/win11/win11_usb_daemon.ps1:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_usb_daemon.ps1:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip diff --git a/util/win11/win11_usb_daemon.py b/util/win11/win11_usb_daemon.py new file mode 100644 index 00000000..c7b2e1cd --- /dev/null +++ b/util/win11/win11_usb_daemon.py @@ -0,0 +1,229 @@ +import dataclasses +import json +import shutil +import signal +import subprocess +import time +import traceback + +WSL_CMD_LITE_WAKEUP = ["wsl", "--exec", "echo", "hi"] +WSL_CMD_LSUSB = ["wsl", "--exec", "lsusb"] + +USBIPD_CMD_LIST = ["usbipd", "list"] +USBIPD_CMD_STATE = ["usbipd", "state"] +USBIPD_CMD_BIND_BUSID = ["usbipd", "bind", "--busid", "ID"] +USBIPD_CMD_ATTACH_BUSID = ["usbipd", "attach", "--wsl", "--busid", "ID"] +USBIPD_CMD_DETACH_BUSID = ["usbipd", "detach", "--busid", "ID"] + + +DESCR_KEYS = ["ST-Link Debug", "J-Link driver"] +DEV_KEY = "Devices" + +running = True +current_state = None + +@dataclasses.dataclass +class Device: + DEV_BUSID_KEY = "BusId" + DEV_CLIENT_IP_KEY = "ClientIPAddress" + DEV_DESC_KEY = "Description" + DEV_INST_ID_KEY = "InstanceId" + DEV_FORCED_KEY = "IsForced" + DEV_PER_GUID_KEY = "PersistedGuid" + DEV_STUB_INST_ID_KEY = "StubInstanceId" + + bus_id: str + client_ip_address: str + description: str + instance_id: str + is_forced: bool + persisted_guid: str + stub_instance_id: str + + @staticmethod + def from_usbipd_dict(dict: dict): + dev = Device( + dict[Device.DEV_BUSID_KEY], + dict[Device.DEV_CLIENT_IP_KEY], + dict[Device.DEV_DESC_KEY], + dict[Device.DEV_INST_ID_KEY], + dict[Device.DEV_FORCED_KEY], + dict[Device.DEV_PER_GUID_KEY], + dict[Device.DEV_STUB_INST_ID_KEY]) + + return dev + + def __hash__(self): + hashed = hash(self.instance_id) + return hashed + + +def check_usbipd(): + return shutil.which("usbipd") is not None + +def check_wsl(): + return shutil.which("wsl") is not None + +def validate_env(): + valid_env = True + if not check_wsl(): + print("WSL not installed") + valid_env = False + + if not check_usbipd(): + print("USBIPD not installed") + valid_env = False + + return valid_env + +def process_json_devices(json_list: list[Device]): + dev_set = set() + for dev in json_list: + dev_set.add(Device.from_usbipd_dict(dev)) + + return dev_set + + +def attach_device_to_wsl(dev: Device): + # print(dev) + + if dev.bus_id is None: + # print("Bus Id invalid. This sometimes happens with reconnects or as the stlink VHUB is enumerated.") + return False + + print(f"New programmer device ({dev.description}) at id ({dev.bus_id}). Attaching to WSL") + + if dev.client_ip_address is not None: + print(f"Bus ({dev.bus_id}) is already attached") + return False + + output = subprocess.run(WSL_CMD_LITE_WAKEUP, capture_output=True) + if output.returncode != 0: + print("Failed to wakeup wsl.") + return False + + USBIPD_CMD_BIND_BUSID[3] = dev.bus_id + output = subprocess.run(USBIPD_CMD_BIND_BUSID, capture_output=True) + if output.returncode == 3: + print(f"Failed to bind device ({dev.bus_id}). Admin priviledges needed. Re-run via the powershell script or directly as admin.") + print(output) + + return False + + USBIPD_CMD_ATTACH_BUSID[4] = dev.bus_id + output = subprocess.run(USBIPD_CMD_ATTACH_BUSID, capture_output=True) + if output.returncode == 1: + print(f"Failed to attach to WSL via usbipd. Was the bind successful?") + print(output) + + return False + + # TODO verify by running lsusb? There doesn't seem to be a correlation between win host bus id + # and WSL bus id which makes sense.... not sure how to do this. Might need state tracking of some kind + # output = subprocess.run(WSL_CMD_LSUSB, capture_output=True) + # print(output) + + return True + + +def process_new_devices(devices: set[Device]): + for dev in devices: + # Iterates through all of the possible keys and adds if matches. + for key in DESCR_KEYS: + if key in dev.description: + attach_device_to_wsl(dev) + + +def cleanup_device(dev: Device): + if dev.bus_id is None: + # print("Bus Id invalid. This sometimes happens with reconnects or as the stlink VHUB is enumerated.") + # this also shows as a host disconn when attachment is successful + return False + + if dev.client_ip_address is not None: + print(f"Cleaning up programmer device ({dev.description}) at id ({dev.bus_id}).") + + USBIPD_CMD_DETACH_BUSID[3] = dev.bus_id + output = subprocess.run(USBIPD_CMD_DETACH_BUSID, capture_output=True) + if output.returncode == 1: + # print(f"Failed to detach to WSL via usbipd. The hardware host bus may have disconnected already.") + # print(output) + + return False + + +def process_removed_devices(devices: set[Device]): + for dev in devices: + # Iterates through all of the possible keys and adds if matches. + for key in DESCR_KEYS: + if key in dev.description: + cleanup_device(dev) + + +def shutdown_cleanup(): + if current_state is not None: + for dev in current_state: + if dev.bus_id is not None: + cleanup_device(dev) + + +def shutdown_handler(sig, frame): + print("received Ctrl+C shutting down...") + global running + running = False + + +if __name__ == "__main__": + print("Starting USBIPD Daemon") + try: + valid_env = validate_env() + if not valid_env: + print("environment is invalid.") + exit(1) + + signal.signal(signal.SIGINT, shutdown_handler) + + while running: + time.sleep(1) + + output = subprocess.run(USBIPD_CMD_STATE, capture_output=True) + if output.returncode != 0 or output.stderr != b'': + print("failed to query usb passthrough status via USBIPD STATE.") + print(output.stderr) + + continue + + latest_state = json.loads(output.stdout) + latest_state = process_json_devices(latest_state[DEV_KEY]) + if current_state is None: + process_new_devices(latest_state) + current_state = latest_state + continue + + added_devices = latest_state - current_state + removed_devices = current_state - latest_state + current_state = latest_state + + process_new_devices(added_devices) + process_removed_devices(removed_devices) + + print("cleaning up lingering devices...") + except KeyboardInterrupt: + print("received hard Ctrl+C~ shutting down...") + + running = False + except Exception as e: + traceback.print_exc() + print(e) + + exit(1) + + shutdown_cleanup() + + print("") + print("") + print("Daemon shutdown complete.") + print("") + + exit(0) + \ No newline at end of file diff --git a/util/win11/win11_usb_daemon.py:Zone.Identifier b/util/win11/win11_usb_daemon.py:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_usb_daemon.py:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip From f6cf78aee32ea4db3903a6035d2517b57e85baee Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 22:50:09 +0200 Subject: [PATCH 135/157] Fixed wheel angles --- control-board/src/motion/params/robot_physical_params.rs | 2 +- control-board/src/tasks/kicker_task.rs | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/control-board/src/motion/params/robot_physical_params.rs b/control-board/src/motion/params/robot_physical_params.rs index abe319d1..91bfe07a 100644 --- a/control-board/src/motion/params/robot_physical_params.rs +++ b/control-board/src/motion/params/robot_physical_params.rs @@ -1,4 +1,4 @@ use nalgebra::Vector4; -pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(300.0, 45.0, 135.0, 240.0); // Degrees from y+ +pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(330.0, 45.0, 135.0, 210.0); // Degrees from y+ pub const WHEEL_RADIUS_M: f32 = 0.0247; // wheel dia 49mm pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot \ No newline at end of file diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index bfe9aafa..475cea34 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -119,7 +119,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ }, KickerTaskState::PowerOn => { // lets power settle on kicker - Timer::after_millis(100).await; + Timer::after_millis(2000).await; defmt::debug!("turn on kicker"); self.remote_power_btn_press().await; Timer::after_millis(50).await; @@ -186,9 +186,9 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ async fn remote_power_btn_press(&mut self) { self.remote_power_btn.set_high(); - Timer::after_millis(250).await; + Timer::after_millis(500).await; self.remote_power_btn.set_low(); - Timer::after_millis(50).await; + Timer::after_millis(250).await; } async fn remote_power_btn_hold(&mut self) { From 0fb423ec378228a25ab1b62614790a58e6fc3208 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Fri, 19 Jul 2024 15:19:36 +0200 Subject: [PATCH 136/157] Firmware used against Delft --- control-board/src/drivers/kicker.rs | 7 +++++- control-board/src/stm32_interface.rs | 4 ++-- control-board/src/tasks/control_task.rs | 2 +- control-board/src/tasks/imu_task.rs | 3 ++- control-board/src/tasks/kicker_task.rs | 30 ++++++++++++++++++++----- control-board/src/tasks/radio_task.rs | 4 +++- kicker-board/src/bin/kicker/main.rs | 4 ++++ motor-controller/bin/dribbler/main.c | 24 +++++++++++++++++--- motor-controller/bin/dribbler/setup.c | 1 + motor-controller/bin/wheel/main.c | 14 +++++------- 10 files changed, 70 insertions(+), 23 deletions(-) diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index e3b1abbc..bcb55e72 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -80,7 +80,8 @@ impl< Self::new(stm32_interface, firmware_image) } - pub fn process_telemetry(&mut self) { + pub fn process_telemetry(&mut self) -> bool { + let mut received_packet = false; while let Ok(res) = self.stm32_uart_interface.try_read_data() { let buf = res.data(); @@ -89,6 +90,8 @@ impl< continue; } + received_packet = true; + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe unsafe { // copy receieved uart bytes into packet @@ -98,6 +101,8 @@ impl< } } } + + return received_packet; } pub fn send_command(&mut self) { diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index e2561d42..71260418 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -123,7 +123,7 @@ impl< } else { self.reset_pin.set_low(); } - Timer::after(Duration::from_micros(100)).await; + Timer::after(Duration::from_millis(50)).await; } pub async fn leave_reset(&mut self) { @@ -132,7 +132,7 @@ impl< } else { self.reset_pin.set_high(); } - Timer::after(Duration::from_micros(100)).await; + Timer::after(Duration::from_millis(50)).await; } pub async fn hard_reset(&mut self) { diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index dcee3c63..aab803dd 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -284,7 +284,7 @@ impl < if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { cmd_vel = Vector3::new(0., 0., 0.); drib_vel = 0.0; - defmt::warn!("ticks since packet lockout"); + //defmt::warn!("ticks since packet lockout"); } // now we have setpoint r(t) in self.cmd_vel diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 800e2b2c..41af3611 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -119,7 +119,8 @@ async fn imu_task_entry( first_tipped_check_time = Instant::now(); } else { // After the first tipped is seen, then wait if it has been tipped for long enough. - if (Instant::now() - first_tipped_check_time) > Duration::from_millis(TIPPED_MIN_DURATION_MS) { + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, first_tipped_check_time).unwrap().as_millis() > TIPPED_MIN_DURATION_MS { robot_state.set_robot_tipped(true); } else { // If it hasn't been long enough, clear the robot tipped. diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 475cea34..a977406e 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -3,7 +3,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart: use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::{gpio::{Level, Output, Pin, Speed}, usart::{self, Uart}}; use embassy_sync::pubsub::WaitResult; -use embassy_time::{Duration, Ticker, Timer}; +use embassy_time::{Duration, Ticker, Timer, Instant}; use crate::{drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, stm32_interface, SystemIrqs}; @@ -13,6 +13,7 @@ const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 20; +const TELEMETRY_TIMEOUT_MS: u64 = 2000; make_uart_queue_pair!(KICKER, KickerUart, KickerRxDma, KickerTxDma, @@ -99,11 +100,24 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ pub async fn kicker_task_entry(&mut self) { let mut main_loop_ticker = Ticker::every(Duration::from_hz(100)); - + let mut last_packet_sent_time = Instant::now(); loop { let cur_robot_state = self.robot_state.get_state(); - // Assume not working, clear if connected. - self.kicker_driver.process_telemetry(); + + if self.kicker_driver.process_telemetry() { + last_packet_sent_time = Instant::now(); + } + + let cur_time = Instant::now(); + if self.kicker_task_state == KickerTaskState::Connected && Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > TELEMETRY_TIMEOUT_MS { + defmt::error!("Kicker telemetry timed out! Will reset."); + self.kicker_driver.reset().await; + // Have a small delay for bring up to prevent boot looping. + Timer::after_millis(1000).await; + // Capture packet time to just in case UART is getting set up. + // TODO Remove this and actually get timing on bring up tuned in. + last_packet_sent_time = Instant::now(); + } // TODO global state overrides of kicker state // e.g. external shutdown requsts, battery votlage, etc @@ -121,6 +135,12 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ // lets power settle on kicker Timer::after_millis(2000).await; defmt::debug!("turn on kicker"); + // TODO Don't spam the power button. + // Spam power button press on to make sure it is on. + self.remote_power_btn_press().await; + Timer::after_millis(500).await; + self.remote_power_btn_press().await; + Timer::after_millis(500).await; self.remote_power_btn_press().await; Timer::after_millis(50).await; self.kicker_driver.enter_reset().await; @@ -186,7 +206,7 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ async fn remote_power_btn_press(&mut self) { self.remote_power_btn.set_high(); - Timer::after_millis(500).await; + Timer::after_millis(300).await; self.remote_power_btn.set_low(); Timer::after_millis(250).await; } diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index c6ffa1c0..967eaa5a 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -261,7 +261,9 @@ impl< // If timeout have elapsed since we last got a packet, // reboot the robot (unless we had a shutdown request). - if !cur_robot_state.shutdown_requested && Instant::now() - self.last_software_packet > Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS) { + let cur_time = Instant::now(); + if !cur_robot_state.shutdown_requested && + Instant::checked_duration_since(&cur_time, self.last_software_packet).unwrap().as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { cortex_m::peripheral::SCB::sys_reset(); } }, diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 19f23dbe..cd15f4c6 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -145,6 +145,7 @@ async fn high_pri_kick_task( let mut last_packet_sent_time = Instant::now(); breakbeam.enable_tx(); + loop { let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.read(&mut vrefint); @@ -424,6 +425,9 @@ async fn main(spawner: Spawner) -> ! { let stm32_config = get_system_config(ClkSource::InternalOscillator); let p = embassy_stm32::init(stm32_config); + // Drives power on high in case of floating. + let _power_on = Output::new(p.PD6, Level::High, Speed::Low); + info!("kicker startup!"); let mut adc = Adc::new(p.ADC1); diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index e2b5dd54..24e318d0 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -108,10 +108,10 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BR_8; // GPIOB->BSRR |= GPIO_BSRR_BR_9; +#ifdef UART_ENABLED // watchdog on receiving a command packet ticks_since_last_command_packet++; -#ifdef UART_ENABLED while (uart_can_read()) { MotorCommandPacket motor_command_packet; uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); @@ -170,9 +170,22 @@ int main() { } #endif - // if upstream isn't listening or its been too long since we got a packet, turn off the motor - if (!telemetry_enabled || ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + // if upstream isn't listening or its been too long since we got a command packet, turn off the motor + if (!telemetry_enabled) { + r_motor_board = 0.0f; + GPIOB->BSRR |= GPIO_BSRR_BR_7; + } else { + GPIOB->BSRR |= GPIO_BSRR_BS_7; + } + + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + // HACK Will force the watchdog to trigger. + while(true); + } else { + GPIOB->BSRR |= GPIO_BSRR_BR_8; } bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -294,5 +307,10 @@ int main() { if (sync_systick()) { slipped_control_frame_count++; } + + if (slipped_control_frame_count > 10) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_6; + } } } diff --git a/motor-controller/bin/dribbler/setup.c b/motor-controller/bin/dribbler/setup.c index 00bf4305..fe615064 100644 --- a/motor-controller/bin/dribbler/setup.c +++ b/motor-controller/bin/dribbler/setup.c @@ -157,6 +157,7 @@ inline void setup_io() { GPIOB->MODER |= GPIO_MODER_MODER6_0; // set output, Red LED, Err GPIOB->MODER |= GPIO_MODER_MODER7_0; // set output, Green LED, Fw Ready + GPIOB->MODER |= GPIO_MODER_MODER8_0; // set output, Yellow LED, Warn } /** diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 2c8ac3b8..e050043f 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -99,10 +99,10 @@ int main() { // setup the loop rate regulators SyncTimer_t vel_loop_timer; SyncTimer_t torque_loop_timer; - //SyncTimer_t telemetry_timer; + SyncTimer_t telemetry_timer; time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); - //time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); + time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); // setup the velocity filter IIRFilter_t encoder_filter; @@ -262,10 +262,6 @@ int main() { GPIOB->BSRR |= GPIO_BSRR_BS_8; // HACK Will force the watchdog to trigger. while(true); - //while(true) { - // NVIC_SystemReset(); - // while(true); - //} } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; } @@ -280,7 +276,7 @@ int main() { // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); bool run_vel_loop = time_sync_ready_rst(&vel_loop_timer); - //bool run_telemtry = time_sync_ready_rst(&telemetry_timer); + bool run_telemtry = time_sync_ready_rst(&telemetry_timer); // run the torque loop if applicable if (run_torque_loop) { @@ -439,9 +435,9 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - //if (run_telemtry) { + if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); - //} + } // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif From 0e0663a3c766e2598c0117930e5ff14c99140a7e Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sat, 20 Jul 2024 08:22:50 +0200 Subject: [PATCH 137/157] Will and Joe's adventures through inner uart --- motor-controller/common/io_queue.c | 76 ++++++++++++++++++++---------- motor-controller/common/io_queue.h | 8 ++-- motor-controller/common/uart.c | 57 +++++++++++----------- motor-controller/common/uart.h | 1 - util/cmake/stm32-defines.cmake | 2 +- 5 files changed, 86 insertions(+), 58 deletions(-) diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index 77502041..521f917f 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -18,6 +18,10 @@ void _increment_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind + 1) % IOQ_BUF_DEPTH; } +void _decrement_read_ind(IoQueue_t *q) { + q->read_ind = (q->read_ind - 1) % IOQ_BUF_DEPTH; +} + //////////////////////// // PUBLIC FUNCTIONS // //////////////////////// @@ -32,31 +36,39 @@ void ioq_initialize(IoQueue_t *q) { } } -bool ioq_empty(IoQueue_t *q) { +bool ioq_is_empty(IoQueue_t *q) { return (q->size == 0); } -bool ioq_full(IoQueue_t *q) { +bool ioq_is_full(IoQueue_t *q) { return (q->size == IOQ_BUF_DEPTH); } -uint8_t ioq_cur_size(IoQueue_t *q) { +uint8_t ioq_get_cur_size(IoQueue_t *q) { return (q->size); } bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { + // Can't add to a full queue. if (ioq_full(q)) { return false; } + // Can't write longer than our buffer size. if (len > IOQ_BUF_LENGTH) { return false; } - q->backing_sto[q->write_ind].len = len; + // Do copy first and then set the length to confirm data + // is valid in the buffer. memcpy(q->backing_sto[q->write_ind].buf, buf, len); + q->backing_sto[q->write_ind].len = len; + // Critical. A packet can be removed from the queue between assigning size. Should be fast. + __disable_irq(); q->size++; + __enable_irq(); + _increment_write_ind(q); return true; @@ -64,25 +76,30 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { if (ioq_full(q)) { + // If the queue is full, discard the back to maintain + // order and minimize data loss. + ioq_discard_back(q); + // TODO JOE FINISH return false; } + // Gets the pointer for the next buffer to write to. *buf = &q->backing_sto[q->write_ind]; return true; } bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { + // Can't add to a full queue. if (ioq_full(q)) { return false; } - // if (buf != &q->backing_sto[q->write_ind]) { - // return false; - // } - _increment_write_ind(q); + // Critical. A packet can be removed from the queue between assigning size. Should be fast. + __disable_irq(); q->size++; + __enable_irq(); return true; } @@ -91,55 +108,62 @@ bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { // READING // /////////////// -uint8_t ioq_read(IoQueue_t *q, void *dest, uint8_t len) { +bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) { + // Can't read an empty queue. if (ioq_empty(q)) { - return 0; + return false; } - uint8_t cpy_num_bytes = q->backing_sto[q->read_ind].len; + uint16_t cpy_num_bytes = q->backing_sto[q->read_ind].len; + // Intended read size is smaller than intended. if (len < cpy_num_bytes) { - return 0; + return false; } memcpy(dest, q->backing_sto[q->read_ind].buf, cpy_num_bytes); - _increment_read_ind(q); + + // Critical. A packet can be added to the queue between assigning size. Should be fast. + __disable_irq(); q->size--; + __enable_irq(); - return cpy_num_bytes; + *num_bytes_read = cpy_num_bytes; + return true; } bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { + // Can't read an empty queue. if (ioq_empty(q)) { return false; } + // Gets the pointer for the next buffer to be read from. *dest = &q->backing_sto[q->read_ind]; return true; } bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { + // Can't read an empty queue. if (ioq_empty(q)) { return false; } - // looks like the hardware might actually increment the - // mem addr, so we can't do this comparison without additional - // storage and idt that's worth it - // if (dest != &q->backing_sto[q->read_ind]) { - // return false; - // } - _increment_read_ind(q); + // This is probably fine without disable since called from interrupt with high enough + // priority, but just to be safe. + __disable_irq(); q->size--; + __enable_irq(); return true; } -void ioq_discard(IoQueue_t *q) { - while (q->size != 0) { - _increment_read_ind(q); - q->size--; - } +void ioq_discard_back(IoQueue_t *q) { + // Critical. Should be quick but we don't want to infinite loop + __disable_irq(); + _decrement_read_ind(q); + q->size--; + __enable_irq(); } \ No newline at end of file diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 7723cd5b..874b44a6 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -14,14 +14,16 @@ typedef struct IoBuf { uint8_t buf[IOQ_BUF_LENGTH]; - volatile uint8_t len; + // u16 is underlying register size. + volatile uint16_t len; } IoBuf_t; typedef struct IoQueue { volatile uint8_t size; volatile uint8_t read_ind; volatile uint8_t write_ind; - + // Holders are u8, so protect against bigger. + _Static_assert(IOQ_BUF_DEPTH < 255); IoBuf_t backing_sto[IOQ_BUF_DEPTH]; } IoQueue_t; @@ -39,7 +41,7 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf); -uint8_t ioq_read(IoQueue_t *q, void *dest, uint8_t len); +bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest); void ioq_discard(IoQueue_t *q); \ No newline at end of file diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index fc66d393..71ffd582 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -31,9 +31,9 @@ volatile IoBuf_t backing_sto[2]; // PRIVATE FUNCTIONS // ///////////////////////// -bool _uart_start_transmit_dma(); -void _uart_start_rx_dma(); -void _uart_rx_dma(); +void _uart_start_transmit_dma(); +void _uart_start_receive_dma(); +void _uart_receive_dma(); //////////////////////// // PUBLIC FUNCTIONS // @@ -43,18 +43,14 @@ void uart_initialize() { ioq_initialize(&uart_tx_queue); ioq_initialize(&uart_rx_queue); - _uart_start_rx_dma(); + _uart_start_receive_dma(); } ////////// // TX // ////////// -bool uart_can_transmit() { - return false; -} - -bool uart_transmit_dma_pending() { +bool uart_is_transmit_dma_pending() { return uart_dma_tx_active; } @@ -63,25 +59,32 @@ bool uart_wait_for_transmission() { } bool uart_transmit(uint8_t *data_buf, uint16_t len) { - ioq_write(&uart_tx_queue, data_buf, len); + if (!ioq_write(&uart_tx_queue, data_buf, len)) { + // Queue is either full or the data length is invalid. + return false; + } // dma transmission isn't in progress to keep scheduling dma writes // manually start the first/only transfer - if (!uart_dma_tx_active) { + if (!uart_is_transmit_dma_pending()) { _uart_start_transmit_dma(); } + + return true; } -bool _uart_start_transmit_dma() { - // get the next data to read and send down the line +void _uart_start_transmit_dma() { + // Get the next data to read and send down the line. IoBuf_t *tx_buf; - ioq_peek_read(&uart_tx_queue, &tx_buf); + if (!ioq_peek_read(&uart_tx_queue, &tx_buf)) { + // This would fail if the queue is empty, so just don't + // start the DMA. + return; + } - // prevent nested/concurrent transfers + // Prevent nested/concurrent transfers uart_dma_tx_active = true; - // TODO this should be done by the interrupt callback, - // maybe remove this // clear the transfer complete flag USART1->ICR = USART_ICR_TCCF; // clear all interrupt flags on the tx dma channel @@ -109,21 +112,21 @@ void uart_discard() { ioq_discard(&uart_rx_queue); } -uint8_t uart_read(void *dest, uint8_t len) { - return ioq_read(&uart_rx_queue, dest, len); +bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { + return ioq_read(&uart_rx_queue, dest, len, num_bytes_read); } -void _uart_start_rx_dma() { +void _uart_start_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; - ioq_peek_write(&uart_rx_queue, &rx_buf); + if (!ioq_peek_write(&uart_rx_queue, &rx_buf)) { + // Queue is full so don't set up to receive. + return; + } DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; - // DMA1_Channel3->CMAR = ( uint32_t) backing_sto[0].buf; - // DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; - // enable DMA DMA1_Channel3->CCR |= DMA_CCR_EN; @@ -131,7 +134,7 @@ void _uart_start_rx_dma() { USART1->CR1 |= USART_CR1_IDLEIE; } -void _uart_rx_dma() { +void _uart_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); @@ -190,7 +193,7 @@ void DMA1_Channel2_3_IRQHandler() { // max buffer length, which fires USART line idle. We probably got // two packets back to back and need to sort that out if (DMA1->ISR & DMA_ISR_TCIF3) { - // _uart_rx_dma(); + // _uart_receive_dma(); } DMA1->IFCR |= DMA_IFCR_CGIF3; @@ -243,7 +246,7 @@ void USART1_IRQHandler() { // clear idle line int flag USART1->ICR |= USART_ICR_IDLECF; - _uart_rx_dma(); + _uart_receive_dma(); // enable idle interrupts // USART1->CR1 |= USART_CR1_IDLEIE; diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index 72f05f61..5ab1f51b 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -17,7 +17,6 @@ void uart_initialize(); -bool uart_can_transmit(); bool uart_transmit_dma_pending(); bool uart_wait_for_transmission(); bool uart_transmit(uint8_t *data_buf, uint16_t len); diff --git a/util/cmake/stm32-defines.cmake b/util/cmake/stm32-defines.cmake index ebb22e5a..17e30a59 100644 --- a/util/cmake/stm32-defines.cmake +++ b/util/cmake/stm32-defines.cmake @@ -17,7 +17,7 @@ set(NUCLEO_F429ZI_OPENOCD_CFG "board/stm32f429discovery.cfg") ################# set(STSPIN32F0x_MACHINE_OPTIONS -mthumb -mcpu=cortex-m0 -mfloat-abi=soft CACHE STRING "") -set(STSPIN32F0x_C_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS}) +set(STSPIN32F0x_C_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wall) set(STSPIN32F0x_CXX_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wno-register -fno-exceptions -fno-rtti -ffunction-sections -fdata-sections CACHE STRING "") set(STSPIN32F0x_LINKER_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wl,--gc-sections,--no-warn-rwx-segments --specs=nosys.specs --specs=nano.specs CACHE STRING "") set(STSPIN32F0x_DEFINITIONS -DSTM32F031xx) From e45a383bcc4a898183cac6b523d18e10adf1d351 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 28 Jul 2024 13:10:16 +0200 Subject: [PATCH 138/157] Additional review with Will --- motor-controller/common/io_queue.c | 23 +++++++++++++---------- motor-controller/common/io_queue.h | 4 ++-- motor-controller/common/uart.c | 27 ++++++++++++--------------- 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index 521f917f..b2088b47 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -18,6 +18,10 @@ void _increment_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind + 1) % IOQ_BUF_DEPTH; } +void _decrement_write_ind(IoQueue_t *q) { + q->write_ind = (q->write_ind - 1) % IOQ_BUF_DEPTH; +} + void _decrement_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind - 1) % IOQ_BUF_DEPTH; } @@ -74,24 +78,22 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { return true; } -bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { +void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { if (ioq_full(q)) { // If the queue is full, discard the back to maintain // order and minimize data loss. - ioq_discard_back(q); - // TODO JOE FINISH - return false; + ioq_discard_write_back(q); } // Gets the pointer for the next buffer to write to. *buf = &q->backing_sto[q->write_ind]; - - return true; } -bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { +bool ioq_finalize_peek_write(IoQueue_t *q) { // Can't add to a full queue. if (ioq_full(q)) { + // Tried to clear with the peek write, so if still + // full probably filling too fast. return false; } @@ -160,10 +162,11 @@ bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { return true; } -void ioq_discard_back(IoQueue_t *q) { - // Critical. Should be quick but we don't want to infinite loop +void ioq_discard_write_back(IoQueue_t *q) { + // Critical. Should be quick but we don't want to infinite loop with + // additions and decrements. __disable_irq(); - _decrement_read_ind(q); + _decrement_write_ind(q); q->size--; __enable_irq(); } \ No newline at end of file diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 874b44a6..1135f476 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -38,8 +38,8 @@ bool ioq_full(IoQueue_t *q); uint8_t ioq_cur_size(IoQueue_t *q); bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); -bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); -bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf); +void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); +bool ioq_finalize_peek_write(IoQueue_t *q); bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 71ffd582..32a141a5 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -96,8 +96,6 @@ void _uart_start_transmit_dma() { // enable DMA DMA1_Channel2->CCR |= DMA_CCR_EN; - - return true; } ////////// @@ -119,10 +117,7 @@ bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { void _uart_start_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; - if (!ioq_peek_write(&uart_rx_queue, &rx_buf)) { - // Queue is full so don't set up to receive. - return; - } + ioq_peek_write(&uart_rx_queue, &rx_buf); DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; @@ -139,18 +134,20 @@ void _uart_receive_dma() { IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); - // check if were filling the last slot - if (ioq_cur_size(&uart_rx_queue) < (IOQ_BUF_DEPTH - 1)) { - uint8_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); - rx_buf->len = transmitted_bytes; + // Peak write discards the last entry if we are full, + // so we will always be good on transfers. - // data and len now correct, finalize write - ioq_finalize_peek_write(&uart_rx_queue, NULL); + // CNDTR is number of bytes left so max + // tranfer size - size left is transfer size. + uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); + rx_buf->len = transmitted_bytes; - // re-peek after potential finalization - ioq_peek_write(&uart_rx_queue, &rx_buf); - } + // data and len now correct, finalize write + // TODO JOE Need a hardware failure/indicator if this returns false. + // Implies RX happens twice without handling + ioq_finalize_peek_write(&uart_rx_queue); + // Assigns the place and length for the write. DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; } From d78a64a2a944b203c800f9119a8615370c3f9296 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 28 Jul 2024 13:12:37 +0200 Subject: [PATCH 139/157] Dev/comp fixes (#81) * Code ran against RIONE * Firmware used against IT Androids * Fixed wheel angles * Firmware used against Delft * What was ran against Delft --------- Co-authored-by: Joe Spall --- control-board/src/bin/control/main.rs | 8 +- control-board/src/bin/hwtest-motor/main.rs | 2 +- control-board/src/drivers/kicker.rs | 11 +- .../src/motion/params/body_vel_pid_params.rs | 6 +- .../motion/params/robot_physical_params.rs | 2 +- control-board/src/robot_state.rs | 30 +- control-board/src/stm32_interface.rs | 13 +- control-board/src/stspin_motor.rs | 8 +- control-board/src/tasks/control_task.rs | 16 +- control-board/src/tasks/imu_task.rs | 3 +- control-board/src/tasks/kicker_task.rs | 34 +- control-board/src/tasks/radio_task.rs | 4 +- control-board/src/tasks/shutdown_task.rs | 6 +- control-board/src/tasks/user_io_task.rs | 115 ++---- kicker-board/src/bin/kicker/main.rs | 4 + lib-stm32/src/drivers/led/apa102.rs | 15 +- motor-controller/bin/dribbler/main.c | 24 +- motor-controller/bin/dribbler/setup.c | 1 + motor-controller/bin/wheel/main.c | 12 +- motor-controller/common/motor_model.c | 7 +- util/udev/99-jlink.rules | 106 +++++ util/udev/99-jlink.rules:Zone.Identifier | 0 util/win11/win11_setup.ps1 | 374 ++++++++++++++++++ util/win11/win11_setup.ps1:Zone.Identifier | 3 + util/win11/win11_usb_daemon.ps1 | 25 ++ .../win11_usb_daemon.ps1:Zone.Identifier | 3 + util/win11/win11_usb_daemon.py | 229 +++++++++++ .../win11/win11_usb_daemon.py:Zone.Identifier | 3 + 28 files changed, 920 insertions(+), 144 deletions(-) create mode 100644 util/udev/99-jlink.rules create mode 100644 util/udev/99-jlink.rules:Zone.Identifier create mode 100644 util/win11/win11_setup.ps1 create mode 100644 util/win11/win11_setup.ps1:Zone.Identifier create mode 100644 util/win11/win11_usb_daemon.ps1 create mode 100644 util/win11/win11_usb_daemon.ps1:Zone.Identifier create mode 100644 util/win11/win11_usb_daemon.py create mode 100644 util/win11/win11_usb_daemon.py:Zone.Identifier diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 66ee3312..52ca7f7b 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -110,12 +110,12 @@ async fn main(main_spawner: embassy_executor::Spawner) { robot_state, p); - create_audio_task!(main_spawner, - robot_state, - p); + // TODO Come back to. Extra compute. + //create_audio_task!(main_spawner, + // robot_state, + // p); create_radio_task!(main_spawner, radio_uart_queue_spawner, uart_queue_spawner, - // create_radio_task!(main_spawner, uart_queue_spawner, robot_state, radio_command_publisher, radio_telemetry_subscriber, wifi_credentials, diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 63326969..e9c1fcab 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -115,7 +115,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, - dribbler_speed: -0.1, + dribbler_speed: 10.0, kick_request: KickRequest::KR_DISABLE, })).await; } diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index e3b1abbc..4c75e872 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -1,5 +1,5 @@ use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; -use embassy_stm32::{gpio::Pin, usart::{self, Parity}}; +use embassy_stm32::{gpio::{Pin, Pull}, usart::{self, Parity}}; use embassy_time::{Duration, Timer}; use crate::stm32_interface::Stm32Interface; @@ -75,12 +75,13 @@ impl< reset_pin: impl Pin, firmware_image: &'a [u8]) -> Self { - let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, true); + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, Pull::Up, true); Self::new(stm32_interface, firmware_image) } - pub fn process_telemetry(&mut self) { + pub fn process_telemetry(&mut self) -> bool { + let mut received_packet = false; while let Ok(res) = self.stm32_uart_interface.try_read_data() { let buf = res.data(); @@ -89,6 +90,8 @@ impl< continue; } + received_packet = true; + // reinterpreting/initializing packed ffi structs is nearly entirely unsafe unsafe { // copy receieved uart bytes into packet @@ -98,6 +101,8 @@ impl< } } } + + return received_packet; } pub fn send_command(&mut self) { diff --git a/control-board/src/motion/params/body_vel_pid_params.rs b/control-board/src/motion/params/body_vel_pid_params.rs index f92d12a4..d0c962cb 100644 --- a/control-board/src/motion/params/body_vel_pid_params.rs +++ b/control-board/src/motion/params/body_vel_pid_params.rs @@ -7,7 +7,11 @@ pub static PID_GAIN: Matrix3x5 = 4.0, 0.0, 0.0, -2.0, 2.0]; // x, y, theta (m/s, m/s, rad/s) -pub static BODY_VEL_LIM: Vector3 = vector![5.0, 5.0, 30.0]; // 8, 8, 34.9 maxes out motors/IMU measurement rate +// NOTE: Because of (bad) motor model, need to give way +// higher constraints to allow controller to overcome +// static friction / loading. +// 8, 8, 34.9 maxes out motors/IMU measurement rate +pub static BODY_VEL_LIM: Vector3 = vector![8.0, 8.0, 40.0]; pub static BODY_ACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore pub static BODY_DEACC_LIM: Vector3 = vector![7.0, 5.0, 36.0]; // TODO calibrate/ignore diff --git a/control-board/src/motion/params/robot_physical_params.rs b/control-board/src/motion/params/robot_physical_params.rs index abe319d1..91bfe07a 100644 --- a/control-board/src/motion/params/robot_physical_params.rs +++ b/control-board/src/motion/params/robot_physical_params.rs @@ -1,4 +1,4 @@ use nalgebra::Vector4; -pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(300.0, 45.0, 135.0, 240.0); // Degrees from y+ +pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(330.0, 45.0, 135.0, 210.0); // Degrees from y+ pub const WHEEL_RADIUS_M: f32 = 0.0247; // wheel dia 49mm pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot \ No newline at end of file diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs index bf92e6e3..ec309783 100644 --- a/control-board/src/robot_state.rs +++ b/control-board/src/robot_state.rs @@ -68,9 +68,9 @@ impl SharedRobotState { radio_inop: self.get_radio_inop(), imu_inop: self.get_imu_inop(), - kicker_inop: true, - wheels_inop: 0xFF, - dribbler_inop: true, + kicker_inop: self.get_kicker_inop(), + wheels_inop: self.get_wheels_inop(), + dribbler_inop: self.get_dribbler_inop(), last_packet_receive_time_ticks_ms: 0, radio_network_ok: self.get_radio_network_ok(), @@ -151,6 +151,30 @@ impl SharedRobotState { self.imu_inop.store(imu_inop, Ordering::Relaxed); } + pub fn get_wheels_inop(&self) -> u8 { + self.wheels_inop.load(Ordering::Relaxed) + } + + pub fn set_wheels_inop(&self, wheels_inop: u8) { + self.wheels_inop.store(wheels_inop, Ordering::Relaxed); + } + + pub fn get_dribbler_inop(&self) -> bool { + self.dribbler_inop.load(Ordering::Relaxed) + } + + pub fn set_dribbler_inop(&self, drib_inop: bool) { + self.dribbler_inop.store(drib_inop, Ordering::Relaxed); + } + + pub fn get_kicker_inop(&self) -> bool { + self.kicker_inop.load(Ordering::Relaxed) + } + + pub fn set_kicker_inop(&self, kicker_inop: bool) { + self.kicker_inop.store(kicker_inop, Ordering::Relaxed); + } + pub fn shutdown_requested(&self) -> bool { self.shutdown_requested.load(Ordering::Relaxed) } diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 0703edc5..754d8470 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -3,7 +3,7 @@ use core::cmp::min; use defmt_rtt as _; use defmt::*; -use embassy_stm32::gpio::{Level, Output, Pin, Speed}; +use embassy_stm32::gpio::{Level, Output, OutputOpenDrain, Pin, Speed, Pull}; use embassy_stm32::pac; use embassy_stm32::usart::{self, Config, Parity, StopBits}; use embassy_time::{Duration, Timer}; @@ -52,7 +52,7 @@ pub struct Stm32Interface< reader: &'a UartReadQueue, writer: &'a UartWriteQueue, boot0_pin: Output<'a>, - reset_pin: Output<'a>, + reset_pin: OutputOpenDrain<'a>, reset_pin_noninverted: bool, @@ -74,7 +74,7 @@ impl< read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, boot0_pin: Output<'a>, - reset_pin: Output<'a>, + reset_pin: OutputOpenDrain<'a>, reset_polarity_high: bool, ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { Stm32Interface { @@ -92,6 +92,7 @@ impl< write_queue: &'a UartWriteQueue, boot0_pin: impl Pin, reset_pin: impl Pin, + reset_pin_pull: Pull, reset_polarity_high: bool, ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { let boot0_output = Output::new(boot0_pin, Level::Low, Speed::Medium); @@ -101,7 +102,7 @@ impl< } else { Level::High }; - let reset_output = Output::new(reset_pin, initial_reset_level, Speed::Medium); + let reset_output = OutputOpenDrain::new(reset_pin, initial_reset_level, Speed::Medium, reset_pin_pull); Stm32Interface { reader: read_queue, @@ -123,7 +124,7 @@ impl< } else { self.reset_pin.set_low(); } - Timer::after(Duration::from_micros(100)).await; + Timer::after(Duration::from_millis(50)).await; } pub async fn leave_reset(&mut self) { @@ -132,7 +133,7 @@ impl< } else { self.reset_pin.set_high(); } - Timer::after(Duration::from_micros(100)).await; + Timer::after(Duration::from_millis(50)).await; } pub async fn hard_reset(&mut self) { diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 3734a91e..f8104c4d 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -3,7 +3,7 @@ use core::{mem::MaybeUninit, f32::consts::PI}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; use defmt::*; use embassy_stm32::{ - gpio::Pin, + gpio::{Pin, Pull}, usart::{self, Parity}, }; use embassy_time::{Duration, Timer}; @@ -122,7 +122,8 @@ impl< firmware_image: &'a [u8], ) -> WheelMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, false); + // Need a Pull None to allow for STSPIN watchdog usage. + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, Pull::None, false); let start_state: MotorResponse_Motion_Packet = { unsafe { MaybeUninit::zeroed().assume_init() } }; @@ -447,7 +448,8 @@ impl< ball_detected_thresh: f32, ) -> DribblerMotor<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, false); + // Need a Pull None to allow for STSPIN watchdog usage. + let stm32_interface = Stm32Interface::new_from_pins(read_queue, write_queue, boot0_pin, reset_pin, Pull::None, false); let start_state: MotorResponse_Motion_Packet = { unsafe { MaybeUninit::zeroed().assume_init() } }; diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index ae2ae9c6..aab803dd 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -284,7 +284,7 @@ impl < if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { cmd_vel = Vector3::new(0., 0., 0.); drib_vel = 0.0; - defmt::warn!("ticks since packet lockout"); + //defmt::warn!("ticks since packet lockout"); } // now we have setpoint r(t) in self.cmd_vel @@ -304,7 +304,7 @@ impl < } else { // Battery is too low, set velocity to zero drib_vel = 0.0; - defmt::warn!("CT - low batter command lockout"); + defmt::warn!("CT - low battery / shutting down command lockout"); Vector4::new(0.0, 0.0, 0.0, 0.0) }; @@ -377,8 +377,16 @@ impl < ) .await; - if res.0.is_err() || res.1.is_err() || res.2.is_err() || res.3.is_err() { - defmt::error!("failed to flash drive motor: {}", res); + let error_mask = res.0.is_err() as u8 + | ((res.1.is_err() as u8) & 0x01) << 1 + | ((res.2.is_err() as u8) & 0x01) << 2 + | ((res.3.is_err() as u8) & 0x01) << 3; + + self.shared_robot_state.set_wheels_inop(error_mask); + self.shared_robot_state.set_dribbler_inop(res.4.is_err()); + + if error_mask != 0 { + defmt::error!("failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", res); } else { defmt::debug!("motor firmware flashed"); } diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 800e2b2c..41af3611 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -119,7 +119,8 @@ async fn imu_task_entry( first_tipped_check_time = Instant::now(); } else { // After the first tipped is seen, then wait if it has been tipped for long enough. - if (Instant::now() - first_tipped_check_time) > Duration::from_millis(TIPPED_MIN_DURATION_MS) { + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, first_tipped_check_time).unwrap().as_millis() > TIPPED_MIN_DURATION_MS { robot_state.set_robot_tipped(true); } else { // If it hasn't been long enough, clear the robot tipped. diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 9bcaf27e..a977406e 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -3,7 +3,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn, uart: use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::{gpio::{Level, Output, Pin, Speed}, usart::{self, Uart}}; use embassy_sync::pubsub::WaitResult; -use embassy_time::{Duration, Ticker, Timer}; +use embassy_time::{Duration, Ticker, Timer, Instant}; use crate::{drivers::kicker::Kicker, include_kicker_bin, pins::*, robot_state::SharedRobotState, stm32_interface, SystemIrqs}; @@ -13,6 +13,7 @@ const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; const MAX_RX_PACKET_SIZE: usize = 16; const RX_BUF_DEPTH: usize = 20; +const TELEMETRY_TIMEOUT_MS: u64 = 2000; make_uart_queue_pair!(KICKER, KickerUart, KickerRxDma, KickerTxDma, @@ -99,11 +100,24 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ pub async fn kicker_task_entry(&mut self) { let mut main_loop_ticker = Ticker::every(Duration::from_hz(100)); - + let mut last_packet_sent_time = Instant::now(); loop { let cur_robot_state = self.robot_state.get_state(); - self.kicker_driver.process_telemetry(); + if self.kicker_driver.process_telemetry() { + last_packet_sent_time = Instant::now(); + } + + let cur_time = Instant::now(); + if self.kicker_task_state == KickerTaskState::Connected && Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > TELEMETRY_TIMEOUT_MS { + defmt::error!("Kicker telemetry timed out! Will reset."); + self.kicker_driver.reset().await; + // Have a small delay for bring up to prevent boot looping. + Timer::after_millis(1000).await; + // Capture packet time to just in case UART is getting set up. + // TODO Remove this and actually get timing on bring up tuned in. + last_packet_sent_time = Instant::now(); + } // TODO global state overrides of kicker state // e.g. external shutdown requsts, battery votlage, etc @@ -119,8 +133,14 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ }, KickerTaskState::PowerOn => { // lets power settle on kicker - Timer::after_millis(100).await; + Timer::after_millis(2000).await; defmt::debug!("turn on kicker"); + // TODO Don't spam the power button. + // Spam power button press on to make sure it is on. + self.remote_power_btn_press().await; + Timer::after_millis(500).await; + self.remote_power_btn_press().await; + Timer::after_millis(500).await; self.remote_power_btn_press().await; Timer::after_millis(50).await; self.kicker_driver.enter_reset().await; @@ -178,15 +198,17 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ self.robot_state.set_ball_detected(ball_detected); } + self.robot_state.set_kicker_inop(self.kicker_task_state < KickerTaskState::Connected); + main_loop_ticker.next().await; } } async fn remote_power_btn_press(&mut self) { self.remote_power_btn.set_high(); - Timer::after_millis(250).await; + Timer::after_millis(300).await; self.remote_power_btn.set_low(); - Timer::after_millis(50).await; + Timer::after_millis(250).await; } async fn remote_power_btn_hold(&mut self) { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index c6ffa1c0..967eaa5a 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -261,7 +261,9 @@ impl< // If timeout have elapsed since we last got a packet, // reboot the robot (unless we had a shutdown request). - if !cur_robot_state.shutdown_requested && Instant::now() - self.last_software_packet > Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS) { + let cur_time = Instant::now(); + if !cur_robot_state.shutdown_requested && + Instant::checked_duration_since(&cur_time, self.last_software_packet).unwrap().as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { cortex_m::peripheral::SCB::sys_reset(); } }, diff --git a/control-board/src/tasks/shutdown_task.rs b/control-board/src/tasks/shutdown_task.rs index e01e773f..70e99e8e 100644 --- a/control-board/src/tasks/shutdown_task.rs +++ b/control-board/src/tasks/shutdown_task.rs @@ -6,7 +6,7 @@ use embassy_time::Timer; use crate::{pins::{PowerBtnPressedIntExti, PowerBtnPressedIntPin, PowerKillPin, ShutdownInitiatedLedPin}, robot_state::{self, SharedRobotState}}; -pub const HARD_SHUTDOWN_TIME_MS: u64 = 20000; +pub const HARD_SHUTDOWN_TIME_MS: u64 = 15000; #[macro_export] macro_rules! create_shutdown_task { @@ -32,12 +32,12 @@ async fn shutdown_task_entry(robot_state: &'static SharedRobotState, defmt::warn!("shutdown initiated via user btn! syncing..."); - // wait for tasks to flag shutdown complete, or hard temrinate after 10s + // wait for tasks to flag shutdown complete, or hard temrinate after hard shutdown time select::select(async move { loop { Timer::after_millis(100).await; - if robot_state.kicker_shutdown_complete() { + if robot_state.kicker_shutdown_complete() || robot_state.get_kicker_inop() { break; } } diff --git a/control-board/src/tasks/user_io_task.rs b/control-board/src/tasks/user_io_task.rs index c71db114..71a9dfc5 100644 --- a/control-board/src/tasks/user_io_task.rs +++ b/control-board/src/tasks/user_io_task.rs @@ -7,7 +7,7 @@ use embassy_stm32::spi::{Config, Spi}; use embassy_stm32::time::mhz; use embassy_time::{Duration, Timer}; -use smart_leds::colors::{BLACK, WHITE}; +use smart_leds::colors::{BLACK, GREEN, ORANGE, RED, WHITE, YELLOW}; use smart_leds::RGB8; use static_cell::ConstStaticCell; @@ -69,74 +69,7 @@ async fn user_io_task_entry( ) { defmt::info!("user io task initialized"); - // let shutdown_anim = anim::Animation::Lerp(Lerp::new(WHITE, BLACK, Duration::from_millis(HARD_SHUTDOWN_TIME_MS), AnimRepeatMode::None)); - // let mut sd_anim_seq = [shutdown_anim]; - // let mut shutdown_anim = CompositeAnimation::new(&mut sd_anim_seq, AnimRepeatMode::None); - - const SHUTDOWN_ANIM_ID: usize = 0; - let shutdown_dimmer = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 255, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(20100), AnimRepeatMode::None)); - let mut shutdown_anim_seq = [shutdown_dimmer]; - let shutdown_comp_anim = CompositeAnimation::new(SHUTDOWN_ANIM_ID, &mut shutdown_anim_seq, AnimRepeatMode::None); - - const ERROR_ANIM_ID: usize = 1; - let error_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut error_blink_seq = [error_blink]; - let error_blink_comp_anim = CompositeAnimation::new(ERROR_ANIM_ID, &mut error_blink_seq, AnimRepeatMode::Forever); - - const NETWORK_OK_ANIM_ID: usize = 2; - let network_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut network_blink_seq = [network_ok_blink]; - let network_ok_blink_comp_anim = CompositeAnimation::new(NETWORK_OK_ANIM_ID, &mut network_blink_seq, AnimRepeatMode::Forever); - - const BRIDGE_OK_ANIM_ID: usize = 3; - let bridge_ok_blink = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0}, RGB8 { r: 0, g: 0, b:0 }, Duration::from_millis(500), AnimRepeatMode::Forever)); - let mut bridge_ok_blink_seq = [bridge_ok_blink]; - let bridge_ok_blink_comp_anim = CompositeAnimation::new(BRIDGE_OK_ANIM_ID, &mut bridge_ok_blink_seq, AnimRepeatMode::Forever); - - let mut led0_anim_playbook = [shutdown_comp_anim, error_blink_comp_anim, network_ok_blink_comp_anim, bridge_ok_blink_comp_anim]; - - const RGB_LERP_ID: usize = 0; - let red_to_green_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let green_to_blue_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let blue_to_red_anim = anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - let mut rgb_blend_seq = [red_to_green_anim, green_to_blue_anim, blue_to_red_anim]; - let rgb_blend_anim = CompositeAnimation::new(RGB_LERP_ID, &mut rgb_blend_seq, AnimRepeatMode::Forever); - - let mut led1_anim_playbook = [rgb_blend_anim]; - - // composite_anim0.start_animation(); - - // let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ - // Some(&mut [ - // CompositeAnimation::new(RGB_LERP_ID, &mut [ - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 255, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // anim::Animation::Lerp(Lerp::new(0, RGB8 { r: 0, g: 0, b: 255 }, RGB8 { r: 255, g: 0, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)), - // ], AnimRepeatMode::Forever) - // ]), - // None, - // ]; - - let animation_playbooks: [Option<&mut [CompositeAnimation]>; 2] = [ - Some(&mut led1_anim_playbook), - Some(&mut led0_anim_playbook), - ]; - - // let anim0_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 255, b: 0 }, RGB8 { r: 0, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let anim1_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 0, g: 255, b: 255 }, RGB8 { r: 255, g: 0, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let anim2_1 = anim::Animation::Lerp(Lerp::new(RGB8 { r: 255, g: 0, b: 255 }, RGB8 { r: 255, g: 255, b: 0 }, Duration::from_millis(1000), AnimRepeatMode::None)); - // let mut anim_seq = [anim0_1, anim1_1, anim2_1]; - // let mut composite_anim1 = CompositeAnimation::new(&mut anim_seq, AnimRepeatMode::Forever); - // composite_anim1.start_animation(); - dotstars.set_drv_str_all(32); - let mut dotstars_anim = Apa102Anim::new(dotstars, animation_playbooks); - - dotstars_anim.enable_animation(0, RGB_LERP_ID); - // dotstars_anim.enable_animation(1, RGB_LERP_ID); - - // let mut color_lerp = TimeLerp::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(10000)); - // color_lerp.start(); /////////////////////// // Battery ADC Setup // @@ -152,14 +85,6 @@ async fn user_io_task_entry( let mut battery_voltage_filt_indx = 0; - // let mut anim1 = Lerp::new(RGB8 { r: 0, g: 0, b: 0}, RGB8 { r: 255, g: 255, b: 255 }, Duration::from_millis(1000), AnimRepeatMode::Forever); - // anim1.start_animation(); - // dotstars_anim.set_animation(anim1, 1); - - // let mut blink_anim = Blink::new(RGB8 { r: 255, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 255 }, Duration::from_millis(800), Duration::from_millis(200), AnimRepeatMode::Forever); - // blink_anim.start_animation(); - // dotstars.set_animation(blink_anim, 1); - let mut prev_robot_state = robot_state.get_state(); loop { let cur_robot_state = robot_state.get_state(); @@ -233,27 +158,33 @@ async fn user_io_task_entry( debug_led0.set_low(); } - // LED states cascade down. - if !prev_robot_state.shutdown_requested && cur_robot_state.shutdown_requested { - let _ = dotstars_anim.disable_animation(0, RGB_LERP_ID); - let _ = dotstars_anim.enable_animation(1, SHUTDOWN_ANIM_ID); - } - // TODO THESE DON'T WORK BUT LED BAD (for now) - else if cur_robot_state.radio_inop { - let _ = dotstars_anim.enable_animation(1, ERROR_ANIM_ID); + // LED 0 states cascade down. + if cur_robot_state.shutdown_requested { + let _ = dotstars.set_color(WHITE, 0); + } else if cur_robot_state.radio_inop { + let _ = dotstars.set_color(RED, 0); } else if cur_robot_state.radio_bridge_ok { - let _ = dotstars_anim.enable_animation(1, BRIDGE_OK_ANIM_ID); + let _ = dotstars.set_color(GREEN, 0); } else if cur_robot_state.radio_network_ok { - let _ = dotstars_anim.enable_animation(1, NETWORK_OK_ANIM_ID); + let _ = dotstars.set_color(ORANGE, 0); + } else { + let _ = dotstars.set_color(BLACK, 0); } - // let color = color_lerp.update(); - // dotstars.set_color(color, 0); - // dotstars.set_color(RGB8 { r: 0, g: 0, b: 0 }, 1); - // TODO THIS BRICKS EVERYTHING SOMETHING SOMETHING SPI - // SOMETHING SOMETHING UART - //dotstars_anim.update().await; + // LED 1 states cascade down. + if cur_robot_state.shutdown_requested { + let _ = dotstars.set_color(WHITE, 1); + } else if cur_robot_state.wheels_inop != 0 { + let _ = dotstars.set_color(RED, 1); + } else if cur_robot_state.imu_inop { + let _ = dotstars.set_color(ORANGE, 1); + } else if cur_robot_state.kicker_inop { + let _ = dotstars.set_color(YELLOW, 1); + } else { + let _ = dotstars.set_color(BLACK, 1); + } + //dotstars.update().await; if !robot_state.hw_init_state_valid() { defmt::info!("loaded robot state: robot id: {}, team: {}", robot_id, robot_team_isblue); diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 19f23dbe..cd15f4c6 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -145,6 +145,7 @@ async fn high_pri_kick_task( let mut last_packet_sent_time = Instant::now(); breakbeam.enable_tx(); + loop { let mut vrefint = adc.enable_vrefint(); let vrefint_sample = adc.read(&mut vrefint); @@ -424,6 +425,9 @@ async fn main(spawner: Spawner) -> ! { let stm32_config = get_system_config(ClkSource::InternalOscillator); let p = embassy_stm32::init(stm32_config); + // Drives power on high in case of floating. + let _power_on = Output::new(p.PD6, Level::High, Speed::Low); + info!("kicker startup!"); let mut adc = Adc::new(p.ADC1); diff --git a/lib-stm32/src/drivers/led/apa102.rs b/lib-stm32/src/drivers/led/apa102.rs index c04e8049..60199926 100644 --- a/lib-stm32/src/drivers/led/apa102.rs +++ b/lib-stm32/src/drivers/led/apa102.rs @@ -16,6 +16,7 @@ pub struct Apa102<'a, 'buf, const NUM_LEDS: usize> where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { spi: spi::Spi<'a, Async>, spi_buf: &'buf mut [u8; (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE], + needs_update: bool, } impl<'a, 'buf, const NUM_LEDS: usize> Apa102<'a, 'buf, NUM_LEDS> @@ -39,6 +40,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { Apa102 { spi: spi, spi_buf: spi_buf, + needs_update: false, } } @@ -103,6 +105,12 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { pub fn set_color(&mut self, color: RGB8, led_index: usize) { assert!(led_index < NUM_LEDS); + if self.spi_buf[Self::l2r(led_index)] != color.r + || self.spi_buf[Self::l2g(led_index)] != color.g + || self.spi_buf[Self::l2b(led_index)] != color.b { + self.needs_update = true; + } + self.spi_buf[Self::l2r(led_index)] = color.r; self.spi_buf[Self::l2g(led_index)] = color.g; self.spi_buf[Self::l2b(led_index)] = color.b; @@ -119,7 +127,10 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { } pub async fn update(&mut self) { - let _ = self.spi.write(self.spi_buf).await; + if self.needs_update { + let _ = self.spi.write(self.spi_buf).await; + self.needs_update = false; + } } } @@ -150,7 +161,7 @@ where [(); (NUM_LEDS * COLOR_FRAME_SIZE) + HEADER_FRAME_SIZE]: { let mut cur_index = search_start_ind; loop { // inc and loop the index - cur_index = cur_index + 1 % anim_playbook.len(); + cur_index = (cur_index + 1) % anim_playbook.len(); // we incremented and found an enabled animation // return the index diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index e2b5dd54..24e318d0 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -108,10 +108,10 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BR_8; // GPIOB->BSRR |= GPIO_BSRR_BR_9; +#ifdef UART_ENABLED // watchdog on receiving a command packet ticks_since_last_command_packet++; -#ifdef UART_ENABLED while (uart_can_read()) { MotorCommandPacket motor_command_packet; uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); @@ -170,9 +170,22 @@ int main() { } #endif - // if upstream isn't listening or its been too long since we got a packet, turn off the motor - if (!telemetry_enabled || ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + // if upstream isn't listening or its been too long since we got a command packet, turn off the motor + if (!telemetry_enabled) { + r_motor_board = 0.0f; + GPIOB->BSRR |= GPIO_BSRR_BR_7; + } else { + GPIOB->BSRR |= GPIO_BSRR_BS_7; + } + + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + // HACK Will force the watchdog to trigger. + while(true); + } else { + GPIOB->BSRR |= GPIO_BSRR_BR_8; } bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -294,5 +307,10 @@ int main() { if (sync_systick()) { slipped_control_frame_count++; } + + if (slipped_control_frame_count > 10) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_6; + } } } diff --git a/motor-controller/bin/dribbler/setup.c b/motor-controller/bin/dribbler/setup.c index 00bf4305..fe615064 100644 --- a/motor-controller/bin/dribbler/setup.c +++ b/motor-controller/bin/dribbler/setup.c @@ -157,6 +157,7 @@ inline void setup_io() { GPIOB->MODER |= GPIO_MODER_MODER6_0; // set output, Red LED, Err GPIOB->MODER |= GPIO_MODER_MODER7_0; // set output, Green LED, Fw Ready + GPIOB->MODER |= GPIO_MODER_MODER8_0; // set output, Yellow LED, Warn } /** diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 62652204..e050043f 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -99,10 +99,10 @@ int main() { // setup the loop rate regulators SyncTimer_t vel_loop_timer; SyncTimer_t torque_loop_timer; - //SyncTimer_t telemetry_timer; + SyncTimer_t telemetry_timer; time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); - //time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); + time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); // setup the velocity filter IIRFilter_t encoder_filter; @@ -261,7 +261,7 @@ int main() { // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; // HACK Will force the watchdog to trigger. - // while(true); + while(true); } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; } @@ -276,7 +276,7 @@ int main() { // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); bool run_vel_loop = time_sync_ready_rst(&vel_loop_timer); - //bool run_telemtry = time_sync_ready_rst(&telemetry_timer); + bool run_telemtry = time_sync_ready_rst(&telemetry_timer); // run the torque loop if applicable if (run_torque_loop) { @@ -435,9 +435,9 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - //if (run_telemtry) { + if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); - //} + } // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c index e3583cc6..c3dd1ad8 100644 --- a/motor-controller/common/motor_model.c +++ b/motor-controller/common/motor_model.c @@ -27,12 +27,11 @@ float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) { } // Linear mapping from 'motor_model.py' in hw-analysis - //float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + - // ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); + float map_value = (avel_rads * mm->rads_to_dc_linear_map_m) + + ((avel_rads < 0) ? mm->rads_to_dc_linear_map_b : -mm->rads_to_dc_linear_map_b); - float map_value = avel_rads / mm->max_vel_rads; // bound DC [-1, 1] - return fmax(fmin(map_value, 1.0f), -1.0f); + return fmax(fmin(2.0f * map_value, 1.0f), -1.0f); } float mm_enc_ticks_to_rev(MotorModel_t *mm, float enc_ticks) { diff --git a/util/udev/99-jlink.rules b/util/udev/99-jlink.rules new file mode 100644 index 00000000..6504b25f --- /dev/null +++ b/util/udev/99-jlink.rules @@ -0,0 +1,106 @@ +# +# This file is going to be stored at /etc/udev/rules.d on installation of the J-Link package +# It makes sure that non-superuser have access to the connected J-Links, so JLinkExe etc. can be executed as non-superuser and can work with J-Link +# +# +# Matches are AND combined, meaning: a==b,c==d,do stuff +# results in: if (a == b) && (c == d) -> do stuff +# +ACTION!="add", SUBSYSTEM!="usb_device", GOTO="jlink_rules_end" +# +# Give all users read and write access. +# Note: NOT all combinations are supported by J-Link right now. Some are reserved for future use, but already added here +# +# 0x0101 - J-Link (default) +# 0x0102 - J-Link USBAddr = 1 (obsolete) +# 0x0103 - J-Link USBAddr = 2 (obsolete) +# 0x0104 - J-Link USBAddr = 3 (obsolete) +# 0x0105 - CDC + J-Link +# 0x0106 - CDC +# 0x0107 - RNDIS + J-Link +# 0x0108 - J-Link + MSD +# 0x1000 - MSD +# +# ATTR{filename} +# Match sysfs attribute values of the event device. Trailing +# whitespace in the attribute values is ignored unless the specified +# match value itself contains trailing whitespace. +# +# ATTRS{filename} +# Search the devpath upwards for a device with matching sysfs +# attribute values. If multiple ATTRS matches are specified, all of +# them must match on the same device. Trailing whitespace in the +# attribute values is ignored unless the specified match value itself +# contains trailing whitespace. +# +# How to find out about udev attributes of device: +# Connect J-Link to PC +# Terminal: cat /var/log/syslog +# Find path to where J-Link device has been "mounted" +# sudo udevadm info --query=all --attribute-walk --path= +# +ATTR{idProduct}=="0101", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0102", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0103", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0104", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0105", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0107", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="0108", ATTR{idVendor}=="1366", MODE="666" +# +# 0x1000 not added yet +# +# New PID scheme, for all possible combinations +# 0x1001: MSD +# 0x1002: RNDIS +# 0x1003: RNDIS + MSD +# 0x1004: CDC +# 0x1005: CDC + MSD +# 0x1006: RNDIS + CDC +# 0x1007: RNDIS + CDC + MSD +# 0x1008: HID +# 0x1009: MSD + HID +# 0x100a: RNDIS + HID +# 0x100b: RNDIS + MSD + HID +# 0x100c: CDC + HID +# 0x100d: CDC + MSD + HID +# 0x100e: RNDIS + CDC + HID +# 0x100f: RNDIS + CDC + MSD + HID +# 0x1010: J_LINK +# 0x1011: J_LINK + MSD +# 0x1012: RNDIS + J_LINK +# 0x1013: RNDIS + J_LINK + MSD +# 0x1014: CDC + J_LINK +# 0x1015: CDC + J_LINK + MSD +# 0x1016: RNDIS + CDC + J_LINK +# 0x1017: RNDIS + CDC + J_LINK + MSD +# 0x1018: J_LINK + HID +# 0x1019: J_LINK + MSD + HID +# 0x101a: RNDIS + J_LINK + HID +# 0x101b: RNDIS + J_LINK + MSD + HID +# 0x101c: CDC + J_LINK + HID +# 0x101d: CDC + J_LINK + MSD + HID +# 0x101e: RNDIS + CDC + J_LINK + HID +# 0x101f: RNDIS + CDC + J_LINK + MSD + HID +# +# 0x1001 - 0x100f not added yet +# +ATTR{idProduct}=="1010", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1011", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1012", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1013", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1014", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1015", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1016", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1017", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1018", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="1019", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101A", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101B", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101C", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101D", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101E", ATTR{idVendor}=="1366", MODE="666" +ATTR{idProduct}=="101F", ATTR{idVendor}=="1366", MODE="666" +# +# End of list +# +LABEL="jlink_rules_end" diff --git a/util/udev/99-jlink.rules:Zone.Identifier b/util/udev/99-jlink.rules:Zone.Identifier new file mode 100644 index 00000000..e69de29b diff --git a/util/win11/win11_setup.ps1 b/util/win11/win11_setup.ps1 new file mode 100644 index 00000000..be1a5964 --- /dev/null +++ b/util/win11/win11_setup.ps1 @@ -0,0 +1,374 @@ + +param ( + [Switch]$help, + [Switch]$install +) + +$invocationPath = $MyInvocation.MyCommand.Path + +Remove-Variable WSL_REQUIRED_CORE_VERSION -force -ErrorAction SilentlyContinue +Remove-Variable WSL_MIN_KERNEL_VERSION -force -ErrorAction SilentlyContinue +Set-Variable WSL_REQUIRED_CORE_VERSION -option ReadOnly -scope Script -value ([String]"2") +Set-Variable WSL_MIN_KERNEL_VERSION -option ReadOnly -scope Script -value ([String]"5.10.16") + +Remove-Variable WINGET_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable WINGET_MIN_VERSION -option ReadOnly -scope Script -value ([String]"1.7.10861") + +Remove-Variable GIT_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable GIT_MIN_VERSION -option ReadOnly -scope Script -value ([String]"2.33.0") + +Remove-Variable PYTHON_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable PYTHON_MIN_VERSION -option ReadOnly -scope Script -value ([String]"3.9.0") + +Remove-Variable USBIPD_MIN_VERSION -force -ErrorAction SilentlyContinue +Set-Variable USBIPD_MIN_VERSION -option ReadOnly -scope Script -value ([String]"4.1.0") + + + +function Write-Help { + Write-Host "Usage: ./win11_setup.ps1 -help | -install" + Write-Host "" + Write-Host "-help: print this message" + Write-Host "-install: will attempt to install rather than notify of missing dependencies. Requires admin" + Write-Host "" +} + +#################### +# WSL2 Utilities # +#################### + +function Confirm-wslIsInstalled { + $wslStatusOutput = @(wsl --status) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput.Contains("Default Version")) { + Write-Host "OK-ACK. WSL is installed" + return $true + } else { + Write-Host "OK-NCK. WSL is not installed" + } + + return $false +} + +function Confirm-wslCoreVersion { + $wslStatusOutput = @(wsl --status) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput -like "*Default Version: $WSL_REQUIRED_CORE_VERSION*") { + Write-Host "OK-ACK. WSL Core Version is 2." + return $true + } else { + Write-Host "OK-NCK. WSL Core Version is not 2." + } + + return $false +} + +function Confirm-wslKernelVersion { + $wslStatusOutput = @(wsl --version) | Out-String + $wslStatusOutput = $wslStatusOutput.Replace("`0", "") + if ($wslStatusOutput -match "Kernel version\: (\d+\.\d+\.\d+)") { + $kernelVersion = $Matches.1 + $kernelVersion = $kernelVersion.Trim() + if ([System.Version]($kernelVersion + ".0") -lt [System.Version]($WSL_MIN_KERNEL_VERSION + ".0")) { + Write-Host "OK-NCK. WSL Kernel Version is $kernelVersion" + } else { + Write-Host "OK-ACK. WSL Kernel Version is $kernelVersion" + return $true + } + } else { + Write-Host "ERROR. WSL Kernel Version not readable. This shouldn't be possible with a validated wsl install..." + Write-Host "Abort." + Exit 1 + } + + return $false +} + +############ +# Python # +############ + +function Confirm-Python3 { + if ($null -eq (Get-Command "python" -ErrorAction SilentlyContinue)) { + return $false + } + + # ughhh windows please don't alias a message script to a missing program + # existense of the program is how scripts check for it ya f*** + # on top of that, it sends it to stderr so it's not captured by other default checks + $output = @(python --version) 2>&1 | Out-String + $output = ([string]($output)).Replace("`0", "") + # Write-Host $output + + if (!([String]($output) -match "Python was not found")) { + $output = @(python --version) | Out-String + $output = $output.Replace("`0", "") + + $res = [Regex]::Match($output, ".*Python (\d+\.\d+\.\d+).*") + if (($null -eq $res) -or (!($res.Success))) { + Write-Host "ERROR. Unkown python version output format." + return $false + } + + $detectedVersionString = $res.Groups[1].Value + $detectedVersionString_WV = $detectedVersionString + ".0" + $pythonMinVersion_WV = $PYTHON_MIN_VERSION + ".0" + if (!([System.Version]($detectedVersionString_WV) -lt [System.Version]($pythonMinVersion_WV))) { + Write-Host "OK-ACK. Python version is $detectedVersionString." + return $true + } + } else { + Write-Host "OK-NCK. Python not installed. System probably has windows store shim." + } + + return $false +} + +######### +# Git # +######### + +function Confirm-gitIsInstalled { + $gitStatusOutput = @(git --version) | Out-String + $gitStatusOutput = $gitStatusOutput.Replace("`0", "") + if ($gitStatusOutput -match "git version (\d+\.\d+\.\d+).*") { + $gitVersion = $Matches.1 + $gitVersion = $gitVersion.Trim() + if ([System.Version]($gitVersion + ".0") -lt [System.Version]($GIT_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. Git version is $gitVersion" + } else { + Write-Host "OK-ACK. Git version is $gitVersion" + return $true + } + } + + return $false +} + +############ +# usbipd # +############ + +function Confirm-usbipd { + if (Get-Command -ErrorAction Ignore -Type Application usbipd) { + $usbipdStatusOutput = @(usbipd --version) | Out-String + $usbipdStatusOutput = $usbipdStatusOutput.Replace("`0", "") + if ($usbipdStatusOutput -match "(\d+\.\d+\.\d+).*") { + $usbipdVersion = $Matches.1 + $usbipdVersion = $usbipdVersion.Trim() + if ([System.Version]($usbipdVersion + ".0") -lt [System.Version]($USBIPD_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. usbipd version is $usbipdVersion" + } else { + Write-Host "OK-ACK. usbipd version is $usbipdVersion" + return $true + } + } else { + Write-Host "ERROR. cannot parse usbipd version" + } + } else { + Write-Host "OK-NCK. usbipd is not installed." + } + + return $false +} + +############ +# WINGET # +############ + +function Confirm-winget { + if (Get-Command -ErrorAction Ignore -Type Application winget) { + $wingetStatusOutput = @(winget --version) | Out-String + $wingetStatusOutput = $wingetStatusOutput.Replace("`0", "") + if ($wingetStatusOutput -match "v(\d+\.\d+\.\d+).*") { + $wingetVersion = $Matches.1 + $wingetVersion = $wingetVersion.Trim() + if ([System.Version]($wingetVersion + ".0") -lt [System.Version]($WINGET_MIN_VERSION + ".0")) { + Write-Host "OK-NCK. Winget version is $wingetVersion" + } else { + Write-Host "OK-ACK. Winget version is $wingetVersion" + return $true + } + } else { + Write-Host "ERROR. cannot parse winget version" + } + } else { + Write-Host "ERROR. winget is not installed." + } + + return $false +} + +function Install-wingetPackage { + param ( + [string]$PackageName + ) + + $wingetInstallOutput = @(winget install --exact $PackageName) | Out-String + + if ($wingetInstallOutput -match "No package found") { + Write-Host "ERROR. Package not found." + return $false + } + + if ($wingetInstallOutput -match "Successfully installed") { + Write-Host "OK. Installed $PackageName via winget." + return $true + } + + if ($wingetInstallOutput -match "No newer package versions are available") { + Write-Host "OK. $PackageName already installed." + return $true + } + + Write-Host $wingetInstallOutput + Write-Host "ERROR. Unknown install status" + + return $false +} + +############ +# Script # +############ + +Write-Host "Checking critical versions..." + +$anyInstallPending = $false +$canInstallMissingItems = $true +$installWsl2 = $false +$installPython = $false +$installGit = $false +$installUsbipd = $false +$installWinget = $false + +if (!(Confirm-wslIsInstalled) -or !(Confirm-wslCoreVersion)) { + $installWsl2 = $true + $anyInstallPending = $true +} + +if (!(Confirm-wslKernelVersion)) { + Write-Host "CONFLICT. WSL kernel version is too old. Script cannot resolve." + $installWsl2 = $false + $canInstallMissingItems = $false + $anyInstallPending = $true +} + +if (!(Confirm-Python3)) { + $installPython = $true + $anyInstallPending = $true +} + +if (!(Confirm-gitIsInstalled)) { + $installGit = $true + $anyInstallPending = $true +} + +if (!(Confirm-usbipd)) { + $installUsbipd = $true + $anyInstallPending = $true +} + +if (!(Confirm-winget)) { + $installWinget = $true + $anyInstallPending = $true +} + +Write-Host "" +Write-Host "" + +if ($anyInstallPending) { + Write-Host "There are dependencies to install." + if ($installWsl2) { + Write-Host "`tWSL2" + } + + if ($installPython) { + Write-Host "`tPython" + } + + if ($installGit) { + Write-Host "`tGit" + } + + if ($installUsbipd) { + Write-Host "`tUsbipd" + } + + Write-Host "" +} else { + Write-Host "All dependencies valid." + Write-Host "" + + Exit 0 +} + + + +if (!($install)) { + Write-Host "Use the -install flag to install missing items." + Write-Host "" + + Exit 0 +} + +################## +# Install Step # +################## + +$installStepFailed = $false + +if ($installWsl2) { + Write-Host "UNWILLING. There are too many pitfalls to automate this easily." + Write-Host "Please install a WSL2 Ubuntu LTS distro from the store and re-run the script." + # NOTE: Will Stuckey has prior work here, but the hypervisor mode failures are esoteric + # and quite a pain to detect and support for arbitrary users. Make them do this manually. + Exit 1 +} + +if ($installWinget) { + Write-Host "UNABLE. Windows 11 should come with winget." + Write-Host "Unable to proceed with package installations." + Exit 1 +} + +if ($installPython) { + $success = Install-wingetPackage Python.Python.3.11 + if (!($success)) { + Write-Host "FAILED. Python install failed." + $installStepFailed = $true + } +} + +if ($installUsbipd) { + $success = Install-wingetPackage dorssel.usbipd-win + if (!($success)) { + Write-Host "FAILED. usbipd install failed." + $installStepFailed = $true + } +} + +if ($installGit) { + $success = Install-wingetPackage Git.Git + if (!($success)) { + Write-Host "FAILED. Git install failed." + $installStepFailed = $true + } +} + + +Write-Host "" +Write-Host "" + +if ($installStepFailed) { + Write-Host "ERROR. One or more install steps failed." + Write-Host "" + + Exit 1 +} + +Write-Host "OK-ACK. All dependencies installed." +Write-Host "NOTE. You will need to restart your shell." +Write-Host "" + +Exit 0 + diff --git a/util/win11/win11_setup.ps1:Zone.Identifier b/util/win11/win11_setup.ps1:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_setup.ps1:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip diff --git a/util/win11/win11_usb_daemon.ps1 b/util/win11/win11_usb_daemon.ps1 new file mode 100644 index 00000000..654522c3 --- /dev/null +++ b/util/win11/win11_usb_daemon.ps1 @@ -0,0 +1,25 @@ +$scriptFilePath = $MyInvocation.MyCommand.Path +$scriptDirPath = (Get-Item $scriptFilePath).DirectoryName + +function Confirm-AdministatorPrivledgesActive { + return ([Security.Principal.WindowsPrincipal] [Security.Principal.WindowsIdentity]::GetCurrent()).IsInRole([Security.Principal.WindowsBuiltInRole] 'Administrator') +} + +if (!(Confirm-AdministatorPrivledgesActive)) { + # user has not deliberately run the script as admin + # print text warning and pause so they know a UAC prompt is coming + Write-Host "Elevating to administrator runtime. You will prompted via UAC." + + # elevate + if ([int](Get-CimInstance -Class Win32_OperatingSystem | Select-Object -ExpandProperty BuildNumber) -ge 6000) { + Start-Process -FilePath PowerShell.exe -Verb Runas -ArgumentList $scriptFilePath + + Exit 0 + } else { + # this should only happen on like old Win 7/Vista machines... ether way WSL aint happening + Write-Host "Operating system build number is severely outdated. Install the latest patch set for windows 10 or 11. Unable to continue." + Exit 1 + } +} + +Start-Process -FilePath python -Verb Runas -ArgumentList "$scriptDirPath/win11_usb_daemon.py" diff --git a/util/win11/win11_usb_daemon.ps1:Zone.Identifier b/util/win11/win11_usb_daemon.ps1:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_usb_daemon.ps1:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip diff --git a/util/win11/win11_usb_daemon.py b/util/win11/win11_usb_daemon.py new file mode 100644 index 00000000..c7b2e1cd --- /dev/null +++ b/util/win11/win11_usb_daemon.py @@ -0,0 +1,229 @@ +import dataclasses +import json +import shutil +import signal +import subprocess +import time +import traceback + +WSL_CMD_LITE_WAKEUP = ["wsl", "--exec", "echo", "hi"] +WSL_CMD_LSUSB = ["wsl", "--exec", "lsusb"] + +USBIPD_CMD_LIST = ["usbipd", "list"] +USBIPD_CMD_STATE = ["usbipd", "state"] +USBIPD_CMD_BIND_BUSID = ["usbipd", "bind", "--busid", "ID"] +USBIPD_CMD_ATTACH_BUSID = ["usbipd", "attach", "--wsl", "--busid", "ID"] +USBIPD_CMD_DETACH_BUSID = ["usbipd", "detach", "--busid", "ID"] + + +DESCR_KEYS = ["ST-Link Debug", "J-Link driver"] +DEV_KEY = "Devices" + +running = True +current_state = None + +@dataclasses.dataclass +class Device: + DEV_BUSID_KEY = "BusId" + DEV_CLIENT_IP_KEY = "ClientIPAddress" + DEV_DESC_KEY = "Description" + DEV_INST_ID_KEY = "InstanceId" + DEV_FORCED_KEY = "IsForced" + DEV_PER_GUID_KEY = "PersistedGuid" + DEV_STUB_INST_ID_KEY = "StubInstanceId" + + bus_id: str + client_ip_address: str + description: str + instance_id: str + is_forced: bool + persisted_guid: str + stub_instance_id: str + + @staticmethod + def from_usbipd_dict(dict: dict): + dev = Device( + dict[Device.DEV_BUSID_KEY], + dict[Device.DEV_CLIENT_IP_KEY], + dict[Device.DEV_DESC_KEY], + dict[Device.DEV_INST_ID_KEY], + dict[Device.DEV_FORCED_KEY], + dict[Device.DEV_PER_GUID_KEY], + dict[Device.DEV_STUB_INST_ID_KEY]) + + return dev + + def __hash__(self): + hashed = hash(self.instance_id) + return hashed + + +def check_usbipd(): + return shutil.which("usbipd") is not None + +def check_wsl(): + return shutil.which("wsl") is not None + +def validate_env(): + valid_env = True + if not check_wsl(): + print("WSL not installed") + valid_env = False + + if not check_usbipd(): + print("USBIPD not installed") + valid_env = False + + return valid_env + +def process_json_devices(json_list: list[Device]): + dev_set = set() + for dev in json_list: + dev_set.add(Device.from_usbipd_dict(dev)) + + return dev_set + + +def attach_device_to_wsl(dev: Device): + # print(dev) + + if dev.bus_id is None: + # print("Bus Id invalid. This sometimes happens with reconnects or as the stlink VHUB is enumerated.") + return False + + print(f"New programmer device ({dev.description}) at id ({dev.bus_id}). Attaching to WSL") + + if dev.client_ip_address is not None: + print(f"Bus ({dev.bus_id}) is already attached") + return False + + output = subprocess.run(WSL_CMD_LITE_WAKEUP, capture_output=True) + if output.returncode != 0: + print("Failed to wakeup wsl.") + return False + + USBIPD_CMD_BIND_BUSID[3] = dev.bus_id + output = subprocess.run(USBIPD_CMD_BIND_BUSID, capture_output=True) + if output.returncode == 3: + print(f"Failed to bind device ({dev.bus_id}). Admin priviledges needed. Re-run via the powershell script or directly as admin.") + print(output) + + return False + + USBIPD_CMD_ATTACH_BUSID[4] = dev.bus_id + output = subprocess.run(USBIPD_CMD_ATTACH_BUSID, capture_output=True) + if output.returncode == 1: + print(f"Failed to attach to WSL via usbipd. Was the bind successful?") + print(output) + + return False + + # TODO verify by running lsusb? There doesn't seem to be a correlation between win host bus id + # and WSL bus id which makes sense.... not sure how to do this. Might need state tracking of some kind + # output = subprocess.run(WSL_CMD_LSUSB, capture_output=True) + # print(output) + + return True + + +def process_new_devices(devices: set[Device]): + for dev in devices: + # Iterates through all of the possible keys and adds if matches. + for key in DESCR_KEYS: + if key in dev.description: + attach_device_to_wsl(dev) + + +def cleanup_device(dev: Device): + if dev.bus_id is None: + # print("Bus Id invalid. This sometimes happens with reconnects or as the stlink VHUB is enumerated.") + # this also shows as a host disconn when attachment is successful + return False + + if dev.client_ip_address is not None: + print(f"Cleaning up programmer device ({dev.description}) at id ({dev.bus_id}).") + + USBIPD_CMD_DETACH_BUSID[3] = dev.bus_id + output = subprocess.run(USBIPD_CMD_DETACH_BUSID, capture_output=True) + if output.returncode == 1: + # print(f"Failed to detach to WSL via usbipd. The hardware host bus may have disconnected already.") + # print(output) + + return False + + +def process_removed_devices(devices: set[Device]): + for dev in devices: + # Iterates through all of the possible keys and adds if matches. + for key in DESCR_KEYS: + if key in dev.description: + cleanup_device(dev) + + +def shutdown_cleanup(): + if current_state is not None: + for dev in current_state: + if dev.bus_id is not None: + cleanup_device(dev) + + +def shutdown_handler(sig, frame): + print("received Ctrl+C shutting down...") + global running + running = False + + +if __name__ == "__main__": + print("Starting USBIPD Daemon") + try: + valid_env = validate_env() + if not valid_env: + print("environment is invalid.") + exit(1) + + signal.signal(signal.SIGINT, shutdown_handler) + + while running: + time.sleep(1) + + output = subprocess.run(USBIPD_CMD_STATE, capture_output=True) + if output.returncode != 0 or output.stderr != b'': + print("failed to query usb passthrough status via USBIPD STATE.") + print(output.stderr) + + continue + + latest_state = json.loads(output.stdout) + latest_state = process_json_devices(latest_state[DEV_KEY]) + if current_state is None: + process_new_devices(latest_state) + current_state = latest_state + continue + + added_devices = latest_state - current_state + removed_devices = current_state - latest_state + current_state = latest_state + + process_new_devices(added_devices) + process_removed_devices(removed_devices) + + print("cleaning up lingering devices...") + except KeyboardInterrupt: + print("received hard Ctrl+C~ shutting down...") + + running = False + except Exception as e: + traceback.print_exc() + print(e) + + exit(1) + + shutdown_cleanup() + + print("") + print("") + print("Daemon shutdown complete.") + print("") + + exit(0) + \ No newline at end of file diff --git a/util/win11/win11_usb_daemon.py:Zone.Identifier b/util/win11/win11_usb_daemon.py:Zone.Identifier new file mode 100644 index 00000000..8da591fa --- /dev/null +++ b/util/win11/win11_usb_daemon.py:Zone.Identifier @@ -0,0 +1,3 @@ +[ZoneTransfer] +ZoneId=3 +ReferrerUrl=C:\Users\jspal\Downloads\firmware-main.zip From cab75cd2c4f0e4ec720057d4af4389d3eb0dee78 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 20 Oct 2024 22:19:19 -0700 Subject: [PATCH 140/157] WIP continued review code --- motor-controller/bin/wheel/setup.c | 62 ++++++++++++---------- motor-controller/common/uart.c | 84 +++++++++++++++++++++--------- motor-controller/common/uart.h | 15 ++++-- 3 files changed, 106 insertions(+), 55 deletions(-) diff --git a/motor-controller/bin/wheel/setup.c b/motor-controller/bin/wheel/setup.c index 705cc499..dd339dff 100644 --- a/motor-controller/bin/wheel/setup.c +++ b/motor-controller/bin/wheel/setup.c @@ -4,15 +4,15 @@ * @brief setup function for the STSPIN32 * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * * Relevant Documents: * stm32f031x4/stm32f031x6 reference "stspin reference" * ARM-Based 32-bit MCU with up to 32 Kbyte Flash, 9 timers, ADC and communication interfaces, 2.0 - 3.6V, 106 pages * stm32f0x1/stm32f0x2/stm32f0x8 reference "m0 reference" - * 1004 pages - * + * 1004 pages + * */ #include @@ -30,10 +30,10 @@ void HardFault_Handler() { /** * @brief Setup the clock tree - * + * * Clock tree diagram: pg 14 / 106 of the "stspin reference" * Register info: pg 108+ / 1004 of the "m0 reference" - * + * */ __attribute__((optimize("O0"))) __attribute__((always_inline)) @@ -102,17 +102,17 @@ inline void setup_clocks() { RCC->BDCR |= RCC_BDCR_RTCSEL_LSI; // HSI14 enable (for ADC) - // on by default?? + // on by default?? - // Setup TIM14 sysclk source + // Setup TIM14 sysclk source RCC->CFGR |= RCC_CFGR_MCO_SYSCLK; // MCOPRE -> 1 RCC->CFGR |= RCC_CFGR_MCOPRE_DIV1; // https://developer.arm.com/documentation/dui0552/a/cortex-m3-peripherals/system-timer--systick/systick-reload-value-register - // set source to sysclk (48MHz) + // set source to sysclk (48MHz) SysTick->CTRL |= SysTick_CTRL_CLKSOURCE_Msk; - // timer counts down to fire approximately every 1ms + // timer counts down to fire approximately every 1ms SysTick->LOAD = (F_SYS_CLK_HZ / 1000UL); // current value set to 0, e.g. no trigger event SysTick->VAL = (F_SYS_CLK_HZ / 1000UL) - 1; @@ -124,7 +124,7 @@ inline void setup_clocks() { /** * @brief sets up base IO - * + * */ __attribute__((optimize("O0"))) inline void setup_io() { @@ -167,7 +167,7 @@ inline void setup_io() { /** * @brief setups UART IO, UART, and UART DMA - * + * * TODO move to uart.c */ __attribute__((optimize("O0"))) @@ -189,7 +189,7 @@ void setup_uart() { // configure PA14 and PA15 AF mode GPIOA->MODER |= (GPIO_MODER_MODER14_1 | GPIO_MODER_MODER15_1); // configure PA14 and PA15 pin speed to 10Mhz - GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); + GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); // configure PA14 and PA15 pin pullup to UP GPIOA->PUPDR |= (GPIO_PUPDR_PUPDR14_0 | GPIO_PUPDR_PUPDR15_0); // configure PA14 and PA15 alternate function @@ -203,12 +203,12 @@ void setup_uart() { // UART Peripheral Setup // ///////////////////////////// - // make sure USART1 is disabled + // make sure USART1 is disabled while configuring. USART1->CR1 &= ~(USART_CR1_UE); // 9-bits with parity (PCE) insert parity bit at the 9th bit // defaults to even parity USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); - // enable transmision + // Enable transmit and receive functionality. USART1->CR1 |= (USART_CR1_TE | USART_CR1_RE); // we don't need anything here USART1->CR2 = 0; @@ -217,42 +217,50 @@ void setup_uart() { // set baud rate USART1->BRR = 0x18; // => 2 Mbaud/s - // enable the module + // Enable the module now that configuration is finished. USART1->CR1 |= USART_CR1_UE; - // Enable idle line interrupt + // Clear idle line interrupt USART1->ICR |= USART_ICR_IDLECF; + // FUTURE: We can probably enable the idle line interrupt here + // and leave it enabled after we add pullups to the bus. // USART1->CR1 |= USART_CR1_IDLEIE; ////////////////////////////////////// // Transmission DMA Channel Setup // ////////////////////////////////////// - // medium priority, memory increment, memory to peripheral - DMA1_Channel2->CCR = DMA_CCR_PL_0 | DMA_CCR_MINC | DMA_CCR_DIR; + // Memory increment, memory to peripheral + DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; + // DMA set to Medium Priority + DMA1_Channel2->CCR |= DMA_CCR_PL_0; // clear buffer base addr DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time // clear transmission length DMA1_Channel2->CNDTR = 0; // transmit length, set at transmission time // set destination address as UART periperal transmission shift register DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address - // enable the transfer complete (TCIE) and transfer error (TEIE) interrupts + // Enable the transfer complete (TCIE) and transfer error (TEIE) interrupts DMA1_Channel2->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); - // clear channel 2, global IF, transfer error IF, half-transfer IF, and transfer complete IF + // Clear the Global Ch2 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF2; ///////////////////////////////// // Receive DMA Channel Setup // ///////////////////////////////// - // USART1_RX, low priority, memory increment, peripheral to memory - // DMA1_Channel3->CCR = DMA_CCR_MINC | DMA_CCR_CIRC; - DMA1_Channel3->CCR = DMA_CCR_MINC; - DMA1_Channel3->CCR |= (0x3U << DMA_CCR_PL_Pos); + // USART1_RX memory increment, peripheral to memory + // Sets memory increment mode. + DMA1_Channel3->CCR = DMA_CCR_MINC; + // DMA set to High Priority + DMA1_Channel3->CCR |= DMA_CCR_PL_1; + // clear buffer base addr DMA1_Channel3->CMAR = (uint32_t) 0 ; + // Set destination address as UART periperal receive register DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; + // clear transmission length DMA1_Channel3->CNDTR = 0; - // DMA1_Channel3->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); + // Clear the Global Ch3 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF3; ///////////////// @@ -268,7 +276,7 @@ void setup_uart() { /** * @brief performs startup system configuration - * + * */ __attribute__((optimize("O0"))) //__attribute__((always_inline)) diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 32a141a5..99d5c4e8 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -1,12 +1,12 @@ /** * @file uart.c * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-30 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -40,9 +40,15 @@ void _uart_receive_dma(); //////////////////////// void uart_initialize() { + ioq_initialize(&uart_tx_queue); ioq_initialize(&uart_rx_queue); + // Make sure SYSCFG bit(s) are cleared for USART TX and RX + // to be on DMA Ch2 and Ch3, respectively. + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_USART1TX_DMA_RMP); + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_USART1RX_DMA_RMP); + _uart_start_receive_dma(); } @@ -123,7 +129,7 @@ void _uart_start_receive_dma() { DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; // enable DMA - DMA1_Channel3->CCR |= DMA_CCR_EN; + DMA1_Channel3->CCR |= DMA_CCR_EN; USART1->ICR |= USART_ICR_IDLECF; USART1->CR1 |= USART_CR1_IDLEIE; @@ -134,11 +140,11 @@ void _uart_receive_dma() { IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); - // Peak write discards the last entry if we are full, - // so we will always be good on transfers. + // Peak write discards the last entry if we are full, + // so we will always be good on transfers. - // CNDTR is number of bytes left so max - // tranfer size - size left is transfer size. + // CNDTR is number of bytes left so + // (max tranfer size - size left) is transfer size. uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); rx_buf->len = transmitted_bytes; @@ -157,64 +163,91 @@ void _uart_receive_dma() { ////////////////////////// /** - * @brief callback handler for DMA RX - * + * @brief callback handler for USART DMA1 Ch2 (TX) and Ch3 (RX) + * */ void DMA1_Channel2_3_IRQHandler() { - // check if DMA1 CH2 has any interrupt, this is for transmit + // Check if DMA1 Ch2 (TX) has any interrupts. if (DMA1->ISR & DMA_ISR_GIF2) { - // transmit had a DMA error + // Transmit had a DMA error. Occurs when the USART is + // reading / writing at a reserved address. if (DMA1->ISR & DMA_ISR_TEIF2) { - // TODO log error + uart_logging_status = UART_LOGGING_DMA_TX_ERROR; + // In COMP_MODE, try to just clear the error and then + // continue with transfers. + #ifdef COMP_MODE + // If TEIF2 is set, the CCR is disabled automatically. + // First need to clear Transfer Error Flag. + DMA1->IFCR |= DMA_IFCR_CTEIF2; + // Then reset the DMA channel control (CCR). + DMA1_Channel2->CCR |= DMA_CCR_EN; + #endif } // DMA finished transfer to USART if (DMA1->ISR & DMA_ISR_TCIF2) { - // expect the USART1 TC interrupt to fire once last byte is out of the FIFO/TR - // make sure tx complete inerrupts are on + // Expect the USART1 Transfer Complete interrupt to + // fire once last byte is out of + // the FIFO/Transmit Register. + // Since USART1 handler disables the Transfer Complete + // interrupt, we need to re-enable it here. USART1->CR1 |= USART_CR1_TCIE; } + // Clears the interrupt flags for Ch2. DMA1->IFCR |= DMA_IFCR_CGIF2; } - // check if DMA1 CH3 has any interrupt, this is for receive + // Check if DMA1 Ch3 (RX) has any interrupt. if (DMA1->ISR & DMA_ISR_GIF3) { - // receive had a dma error + // Receive had a DMA error if (DMA1->ISR & DMA_ISR_TEIF3) { - // TODO: log error + uart_logging_status = UART_LOGGING_DMA_RX_ERROR; + // In COMP_MODE, try to just clear the error and then + // continue with transfers. + #ifdef COMP_MODE + // If TEIF3 is set, the CCR is disabled automatically. + // First need to clear Transfer Error Flag. + DMA1->IFCR |= DMA_IFCR_CTEIF3; + // Then reset the DMA channel control (CCR). + DMA1_Channel3->CCR |= DMA_CCR_EN; + #endif } // receive, got a full buffer + // TODO JOE look into this. // this is sort of unexpected, we generally expect a packet less than - // max buffer length, which fires USART line idle. We probably got + // max buffer length, which fires USART line idle. We probably got // two packets back to back and need to sort that out if (DMA1->ISR & DMA_ISR_TCIF3) { // _uart_receive_dma(); } + // Clears the interrupt flags for Ch3. DMA1->IFCR |= DMA_IFCR_CGIF3; } } /** * @brief callback handler for uart - * + * */ __attribute((__optimize__("O0"))) void USART1_IRQHandler() { - uint32_t uart_status_register = USART1->ISR; + const uint32_t uart_status_register = USART1->ISR; //////////////////// // Transmission // //////////////////// + // If it's a USART transmit complete interrupt: if (uart_status_register & USART_ISR_TC) { - // disable DMA channel + // Disable TX DMA channel until we + // know we have a payload to send. DMA1_Channel2->CCR &= ~(DMA_CCR_EN); - // disable transfer complete interrupts + // Disable transfer complete interrupts. USART1->CR1 &= ~(USART_CR1_TCIE); - + // Clear transmission complete flag. USART1->ICR |= USART_ICR_TCCF; // finalize tx queue read @@ -232,7 +265,8 @@ void USART1_IRQHandler() { //////////////////// // Reception // //////////////////// - + // TODO Check parity? + // TODO Finish looking over this. // detected idle line before full buf if (uart_status_register & USART_ISR_IDLE) { // disable DMA diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index 5ab1f51b..f1d6a89c 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -1,15 +1,16 @@ /** * @file uart.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-30 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once +#define COMP_MODE //////////////////////// // PUBLIC FUNCTIONS // @@ -24,3 +25,11 @@ bool uart_transmit(uint8_t *data_buf, uint16_t len); bool uart_can_read(); uint8_t uart_read(void *dest, uint8_t len); void uart_discard(); + +volatile uart_logging_status_t uart_logging_status; + +typedef enum { + UART_LOGGING_OK = 0, + UART_LOGGING_DMA_TX_ERROR, + UART_LOGGING_DMA_RX_ERROR +} uart_logging_status_t; From fd5e2af9b7763a6bb3c6ecef9530c4f43c90f489 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 22 Oct 2024 19:56:53 -0700 Subject: [PATCH 141/157] Finished up UART review --- motor-controller/bin/dribbler/main.c | 20 +++++-- motor-controller/bin/wheel/main.c | 33 ++++++----- motor-controller/common/io_queue.c | 22 +++---- motor-controller/common/io_queue.h | 8 +-- motor-controller/common/uart.c | 87 ++++++++++++++++------------ motor-controller/common/uart.h | 21 +++---- 6 files changed, 110 insertions(+), 81 deletions(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 24e318d0..88199065 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -75,7 +75,7 @@ int main() { response_packet.data.motion.reset_low_power = rcc_csr & RCC_CSR_LPWRRSTF != 0; response_packet.data.motion.reset_software = rcc_csr & RCC_CSR_SFTRSTF != 0; response_packet.data.motion.reset_pin = rcc_csr & RCC_CSR_PINRSTF != 0; - + bool params_return_packet_requested = false; SyncTimer_t torque_loop_timer; @@ -87,7 +87,7 @@ int main() { iir_filter_init(&torque_filter, iir_filter_alpha_from_Tf(TORQUE_IIR_TF_MS, TORQUE_LOOP_RATE_MS)); MotorCommand_MotionType motion_control_type = OPEN_LOOP; - + float r_motor_board = 0.0f; float u_torque_loop; float cur_limit = 0.0f; @@ -114,14 +114,22 @@ int main() { while (uart_can_read()) { MotorCommandPacket motor_command_packet; - uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); - if (bytes_moved != sizeof(MotorCommandPacket)) { + uint16_t uart_bytes_read = 0; + bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); + if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { // something went wrong, just purge all of the data uart_discard(); continue; } + // If something goes wrong with the UART, we need to flag it. + if (uart_logging_status != UART_LOGGING_OK) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + } + + if (motor_command_packet.type == MCP_MOTION) { // GPIOB->BSRR |= GPIO_BSRR_BS_8; @@ -177,12 +185,12 @@ int main() { } else { GPIOB->BSRR |= GPIO_BSRR_BS_7; } - + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. + // HACK Will force the watchdog to trigger. while(true); } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index e050043f..86d9ad95 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -1,12 +1,12 @@ /** * @file main.c * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -113,7 +113,7 @@ int main() { // set the default command mode to open loop (no PID) MotorCommand_MotionType motion_control_type = OPEN_LOOP; - + // define the control points the loops use to interact float r_motor_board = 0.0f; float control_setpoint_vel_duty = 0.0f; @@ -175,14 +175,21 @@ int main() { // process all available packets while (uart_can_read()) { - uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); - if (bytes_moved != sizeof(MotorCommandPacket)) { + uint16_t uart_bytes_read = 0; + bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); + if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { // something went wrong, just purge all of the data uart_discard(); continue; } + // If something goes wrong with the UART, we need to flag it. + if (uart_logging_status != UART_LOGGING_OK) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + } + if (motor_command_packet.type == MCP_MOTION) { // we got a motion packet! @@ -255,12 +262,12 @@ int main() { } else { GPIOB->BSRR |= GPIO_BSRR_BS_7; } - + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. + // HACK Will force the watchdog to trigger. while(true); } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; @@ -271,7 +278,7 @@ int main() { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_7; - } + } // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -290,7 +297,7 @@ int main() { // filter torque measured_torque_Nm = iir_filter_update(&torque_filter, measured_torque_Nm); - // TODO: add filter? + // TODO: add filter? // correct torque sign from recovered velocity // TODO: this should probably be acceleration based not velocity @@ -337,10 +344,10 @@ int main() { // filter the recovered velocity enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); - + // compute the velcoity PID control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); - + // Clamp setpoint acceleration float setpoint_accel_rads_2 = (control_setpoint_vel_rads - control_setpoint_vel_rads_prev)/VELOCITY_LOOP_RATE_S; if (setpoint_accel_rads_2 > MOTOR_MAXIMUM_ACCEL) { @@ -435,7 +442,7 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (run_telemtry) { + if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); } // GPIOB->BSRR |= GPIO_BSRR_BR_8; diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index b2088b47..977000d6 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -1,4 +1,4 @@ - +#include #include #include #include @@ -54,7 +54,7 @@ uint8_t ioq_get_cur_size(IoQueue_t *q) { bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { // Can't add to a full queue. - if (ioq_full(q)) { + if (ioq_is_full(q)) { return false; } @@ -63,7 +63,7 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { return false; } - // Do copy first and then set the length to confirm data + // Do copy first and then set the length to confirm data // is valid in the buffer. memcpy(q->backing_sto[q->write_ind].buf, buf, len); q->backing_sto[q->write_ind].len = len; @@ -79,8 +79,8 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { } void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { - if (ioq_full(q)) { - // If the queue is full, discard the back to maintain + if (ioq_is_full(q)) { + // If the queue is full, discard the back to maintain // order and minimize data loss. ioq_discard_write_back(q); } @@ -91,7 +91,7 @@ void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { bool ioq_finalize_peek_write(IoQueue_t *q) { // Can't add to a full queue. - if (ioq_full(q)) { + if (ioq_is_full(q)) { // Tried to clear with the peek write, so if still // full probably filling too fast. return false; @@ -112,12 +112,12 @@ bool ioq_finalize_peek_write(IoQueue_t *q) { bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } uint16_t cpy_num_bytes = q->backing_sto[q->read_ind].len; - // Intended read size is smaller than intended. + // Intended read size is smaller than intended. if (len < cpy_num_bytes) { return false; } @@ -136,7 +136,7 @@ bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } @@ -148,12 +148,12 @@ bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } _increment_read_ind(q); - // This is probably fine without disable since called from interrupt with high enough + // This is probably fine without disable since called from interrupt with high enough // priority, but just to be safe. __disable_irq(); q->size--; diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 1135f476..7c5367a1 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -33,9 +33,9 @@ typedef struct IoQueue { void ioq_initialize(IoQueue_t *q); -bool ioq_empty(IoQueue_t *q); -bool ioq_full(IoQueue_t *q); -uint8_t ioq_cur_size(IoQueue_t *q); +bool ioq_is_empty(IoQueue_t *q); +bool ioq_is_full(IoQueue_t *q); +uint8_t ioq_get_cur_size(IoQueue_t *q); bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); @@ -44,4 +44,4 @@ bool ioq_finalize_peek_write(IoQueue_t *q); bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest); -void ioq_discard(IoQueue_t *q); \ No newline at end of file +void ioq_discard_write_back(IoQueue_t *q); \ No newline at end of file diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 99d5c4e8..4ad05baa 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -25,7 +25,6 @@ IoQueue_t uart_tx_queue; volatile bool uart_dma_rx_active = false; volatile int uart_dma_rx_num_bytes = 0; IoQueue_t uart_rx_queue; -volatile IoBuf_t backing_sto[2]; ///////////////////////// // PRIVATE FUNCTIONS // @@ -33,7 +32,7 @@ volatile IoBuf_t backing_sto[2]; void _uart_start_transmit_dma(); void _uart_start_receive_dma(); -void _uart_receive_dma(); +void _uart_receive_dma(bool parity_error); //////////////////////// // PUBLIC FUNCTIONS // @@ -60,8 +59,8 @@ bool uart_is_transmit_dma_pending() { return uart_dma_tx_active; } -bool uart_wait_for_transmission() { - while (uart_transmit_dma_pending()); +void uart_wait_for_transmission() { + while (uart_is_transmit_dma_pending()); } bool uart_transmit(uint8_t *data_buf, uint16_t len) { @@ -109,11 +108,11 @@ void _uart_start_transmit_dma() { ////////// bool uart_can_read() { - return (!ioq_empty(&uart_rx_queue)); + return (!ioq_is_empty(&uart_rx_queue)); } void uart_discard() { - ioq_discard(&uart_rx_queue); + ioq_discard_write_back(&uart_rx_queue); } bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { @@ -135,27 +134,40 @@ void _uart_start_receive_dma() { USART1->CR1 |= USART_CR1_IDLEIE; } -void _uart_receive_dma() { - // get the next data to read and send down the line - IoBuf_t *rx_buf; - ioq_peek_write(&uart_rx_queue, &rx_buf); - - // Peak write discards the last entry if we are full, - // so we will always be good on transfers. - - // CNDTR is number of bytes left so - // (max tranfer size - size left) is transfer size. - uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); - rx_buf->len = transmitted_bytes; +void _uart_receive_dma(bool parity_error) { + // If we have a parity error, we can just overwrite the queue + // with the next packet. + if (!parity_error) { + // Get the buffer location that was written into with + // last DMA contents. + IoBuf_t *rx_buf; + ioq_peek_write(&uart_rx_queue, &rx_buf); + + // Peek write discards the last entry if we are full, + // so we will always be good on transfers. + + // CNDTR is number of bytes left in the DMA buffer so + // (max transfer size - size left) is packet transfer size. + uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); + rx_buf->len = transmitted_bytes; + + // data and len now correct, finalize write + // If this returns false, implies RX happens twice without handling. + if (!ioq_finalize_peek_write(&uart_rx_queue)) { + uart_logging_status = UART_LOGGING_UART_RX_BUFFER_FULL; + } + } - // data and len now correct, finalize write - // TODO JOE Need a hardware failure/indicator if this returns false. - // Implies RX happens twice without handling - ioq_finalize_peek_write(&uart_rx_queue); + // Get the NEXT buffer for the DMA to write into. + IoBuf_t* rx_buf_next; + ioq_peek_write(&uart_rx_queue, &rx_buf_next); - // Assigns the place and length for the write. - DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; + // Assigns the location and length for the NEXT DMA write. + DMA1_Channel3->CMAR = (uint32_t) rx_buf_next->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; + + // DMA is now ready to receive the next payload, so enable DMA. + DMA1_Channel3->CCR |= DMA_CCR_EN; } ////////////////////////// @@ -214,13 +226,12 @@ void DMA1_Channel2_3_IRQHandler() { #endif } - // receive, got a full buffer - // TODO JOE look into this. - // this is sort of unexpected, we generally expect a packet less than + // Receive, got a full buffer + // This is sort of unexpected, we generally expect less than // max buffer length, which fires USART line idle. We probably got - // two packets back to back and need to sort that out + // two packets back to back and need to sort that out. if (DMA1->ISR & DMA_ISR_TCIF3) { - // _uart_receive_dma(); + uart_logging_status = UART_LOGGING_DMA_RX_BUFFER_FULL; } // Clears the interrupt flags for Ch3. @@ -254,7 +265,7 @@ void USART1_IRQHandler() { // check if queue is empty // conditionally recall transmit ioq_finalize_peek_read(&uart_tx_queue, NULL); - if (!ioq_empty(&uart_tx_queue)) { + if (!ioq_is_empty(&uart_tx_queue)) { _uart_start_transmit_dma(); } else { // restore the ready flag @@ -265,24 +276,26 @@ void USART1_IRQHandler() { //////////////////// // Reception // //////////////////// - // TODO Check parity? - // TODO Finish looking over this. // detected idle line before full buf if (uart_status_register & USART_ISR_IDLE) { // disable DMA DMA1_Channel3->CCR &= ~(DMA_CCR_EN); - // disable idle interrupts + // FUTURE: Need to fix pull-ups to disable idle interrupts // USART1->CR1 &= ~(USART_CR1_IDLEIE); // clear idle line int flag USART1->ICR |= USART_ICR_IDLECF; - _uart_receive_dma(); + // Moves the received DMA data to the queue + // and prepares the next DMA transfer. + bool parity_error = (uart_status_register & USART_ISR_PE); + _uart_receive_dma(parity_error); + + // Clear the parity error flag for the next transfer. + USART1->ICR |= USART_ICR_PECF; + // FUTURE: Need to fix pull-ups to enable idle interrupts // enable idle interrupts // USART1->CR1 |= USART_CR1_IDLEIE; - - // enable DMA - DMA1_Channel3->CCR |= DMA_CCR_EN; } } \ No newline at end of file diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index f1d6a89c..4cd59d8d 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -10,7 +10,14 @@ */ #pragma once -#define COMP_MODE + +typedef enum { + UART_LOGGING_OK = 0, + UART_LOGGING_DMA_TX_ERROR, + UART_LOGGING_DMA_RX_ERROR, + UART_LOGGING_DMA_RX_BUFFER_FULL, + UART_LOGGING_UART_RX_BUFFER_FULL +} uart_logging_status_t; //////////////////////// // PUBLIC FUNCTIONS // @@ -19,17 +26,11 @@ void uart_initialize(); bool uart_transmit_dma_pending(); -bool uart_wait_for_transmission(); +void uart_wait_for_transmission(); bool uart_transmit(uint8_t *data_buf, uint16_t len); bool uart_can_read(); -uint8_t uart_read(void *dest, uint8_t len); void uart_discard(); +bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read); -volatile uart_logging_status_t uart_logging_status; - -typedef enum { - UART_LOGGING_OK = 0, - UART_LOGGING_DMA_TX_ERROR, - UART_LOGGING_DMA_RX_ERROR -} uart_logging_status_t; +static volatile uart_logging_status_t uart_logging_status; From e02a908f094ef829347d1fcb7efc6a9615a3c156 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 29 Oct 2024 18:32:54 -0700 Subject: [PATCH 142/157] White space --- motor-controller/bin/dribbler/main.c | 1 - 1 file changed, 1 deletion(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 88199065..9ef9563c 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -129,7 +129,6 @@ int main() { GPIOB->BSRR |= GPIO_BSRR_BS_8; } - if (motor_command_packet.type == MCP_MOTION) { // GPIOB->BSRR |= GPIO_BSRR_BS_8; From 371ea40db683ce0dfe9083a8e15fb89da172b301 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 12:42:16 +0200 Subject: [PATCH 143/157] Code ran against RIONE --- control-board/src/tasks/control_task.rs | 2 +- control-board/src/tasks/kicker_task.rs | 6 +++--- motor-controller/bin/wheel/main.c | 24 ++++++++++++------------ 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index aab803dd..55fd3b6e 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -325,7 +325,7 @@ impl < async fn flash_motor_firmware(&mut self, debug: bool) { defmt::info!("flashing firmware"); - if debug { + if true { let mut had_motor_error = false; if self.motor_fl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FL"); diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index a977406e..28110cc8 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -33,7 +33,7 @@ macro_rules! create_kicker_task { &$wifi_credentials, $p.USART10, $p.PE2, $p.PE3, $p.PG13, $p.PG14, $p.DMA2_CH1, $p.DMA2_CH0, - $p.PC13, $p.PE4).await; + $p.PC13, $p.PE4).await; }; } @@ -112,12 +112,12 @@ const DEPTH_TX: usize> KickerTask<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_ if self.kicker_task_state == KickerTaskState::Connected && Instant::checked_duration_since(&cur_time, last_packet_sent_time).unwrap().as_millis() > TELEMETRY_TIMEOUT_MS { defmt::error!("Kicker telemetry timed out! Will reset."); self.kicker_driver.reset().await; - // Have a small delay for bring up to prevent boot looping. + // Have a small delay for bring up to prevent boot looping. Timer::after_millis(1000).await; // Capture packet time to just in case UART is getting set up. // TODO Remove this and actually get timing on bring up tuned in. last_packet_sent_time = Instant::now(); - } + } // TODO global state overrides of kicker state // e.g. external shutdown requsts, battery votlage, etc diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index e050043f..7fa34a13 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -1,12 +1,12 @@ /** * @file main.c * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -113,7 +113,7 @@ int main() { // set the default command mode to open loop (no PID) MotorCommand_MotionType motion_control_type = OPEN_LOOP; - + // define the control points the loops use to interact float r_motor_board = 0.0f; float control_setpoint_vel_duty = 0.0f; @@ -171,7 +171,7 @@ int main() { #ifdef UART_ENABLED // increment the soft watchdog - ticks_since_last_command_packet++; + //ticks_since_last_command_packet++; // process all available packets while (uart_can_read()) { @@ -255,12 +255,12 @@ int main() { } else { GPIOB->BSRR |= GPIO_BSRR_BS_7; } - + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. + // HACK Will force the watchdog to trigger. while(true); } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; @@ -271,7 +271,7 @@ int main() { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_7; - } + } // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -290,7 +290,7 @@ int main() { // filter torque measured_torque_Nm = iir_filter_update(&torque_filter, measured_torque_Nm); - // TODO: add filter? + // TODO: add filter? // correct torque sign from recovered velocity // TODO: this should probably be acceleration based not velocity @@ -337,10 +337,10 @@ int main() { // filter the recovered velocity enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); - + // compute the velcoity PID control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); - + // Clamp setpoint acceleration float setpoint_accel_rads_2 = (control_setpoint_vel_rads - control_setpoint_vel_rads_prev)/VELOCITY_LOOP_RATE_S; if (setpoint_accel_rads_2 > MOTOR_MAXIMUM_ACCEL) { @@ -435,7 +435,7 @@ int main() { // GPIOB->BSRR |= GPIO_BSRR_BS_8; uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (run_telemtry) { + if (run_telemtry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); } // GPIOB->BSRR |= GPIO_BSRR_BR_8; From 94d2f2b7415c055741c864cab7b9d6dd5886f97f Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Thu, 18 Jul 2024 16:21:20 +0200 Subject: [PATCH 144/157] Firmware used against IT Androids --- control-board/src/stm32_interface.rs | 26 ++++++++++++------------- control-board/src/tasks/control_task.rs | 2 +- motor-controller/bin/wheel/main.c | 2 +- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index 754d8470..b2696cd3 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -46,7 +46,7 @@ pub struct Stm32Interface< DmaTx: usart::TxDma, const LEN_RX: usize, const LEN_TX: usize, - const DEPTH_RX: usize, + const DEPTH_RX: usize, const DEPTH_TX: usize, > { reader: &'a UartReadQueue, @@ -66,17 +66,17 @@ impl< DmaTx: usart::TxDma, const LEN_RX: usize, const LEN_TX: usize, - const DEPTH_RX: usize, + const DEPTH_RX: usize, const DEPTH_TX: usize, > Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - pub fn new( + pub fn new( read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, boot0_pin: Output<'a>, reset_pin: OutputOpenDrain<'a>, reset_polarity_high: bool, - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { Stm32Interface { reader: read_queue, writer: write_queue, @@ -87,16 +87,16 @@ impl< } } - pub fn new_from_pins( + pub fn new_from_pins( read_queue: &'a UartReadQueue, write_queue: &'a UartWriteQueue, boot0_pin: impl Pin, reset_pin: impl Pin, reset_pin_pull: Pull, reset_polarity_high: bool, - ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { - let boot0_output = Output::new(boot0_pin, Level::Low, Speed::Medium); - + ) -> Stm32Interface<'a, UART, DmaRx, DmaTx, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX> { + let boot0_output = Output::new(boot0_pin, Level::Low, Speed::Medium); + let initial_reset_level = if reset_polarity_high { Level::Low } else { @@ -347,7 +347,7 @@ impl< return Err(()); } - // stm bl can only support 256 byte data payloads, right now we don't automatically handle + // stm bl can only support 256 byte data payloads, right now we don't automatically handle // larger chunks than the tx buffer if data.len() > 256 || data.len() + 1 > LEN_TX { return Err(()); @@ -520,11 +520,11 @@ impl< return Err(err); } } - + if let Err(err) = self.verify_bootloader().await { return Err(err); } - + match self.get_device_id().await { Err(err) => return Err(err), Ok(device_id) => match device_id { @@ -540,7 +540,7 @@ impl< } } }; - + // erase part if let Err(err) = self.erase_flash_memory().await { return Err(err); @@ -552,7 +552,7 @@ impl< } self.reset_into_program(false).await; - + Ok(()) } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 55fd3b6e..aab803dd 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -325,7 +325,7 @@ impl < async fn flash_motor_firmware(&mut self, debug: bool) { defmt::info!("flashing firmware"); - if true { + if debug { let mut had_motor_error = false; if self.motor_fl.load_default_firmware_image().await.is_err() { defmt::error!("failed to flash FL"); diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 7fa34a13..6e54e47c 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -171,7 +171,7 @@ int main() { #ifdef UART_ENABLED // increment the soft watchdog - //ticks_since_last_command_packet++; + ticks_since_last_command_packet++; // process all available packets while (uart_can_read()) { From 5fb8f2331a92c842f57dfe852c5c868bb450395b Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sat, 20 Jul 2024 08:22:50 +0200 Subject: [PATCH 145/157] Will and Joe's adventures through inner uart --- motor-controller/common/io_queue.c | 76 ++++++++++++++++++++---------- motor-controller/common/io_queue.h | 8 ++-- motor-controller/common/uart.c | 57 +++++++++++----------- motor-controller/common/uart.h | 1 - util/cmake/stm32-defines.cmake | 2 +- 5 files changed, 86 insertions(+), 58 deletions(-) diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index 77502041..521f917f 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -18,6 +18,10 @@ void _increment_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind + 1) % IOQ_BUF_DEPTH; } +void _decrement_read_ind(IoQueue_t *q) { + q->read_ind = (q->read_ind - 1) % IOQ_BUF_DEPTH; +} + //////////////////////// // PUBLIC FUNCTIONS // //////////////////////// @@ -32,31 +36,39 @@ void ioq_initialize(IoQueue_t *q) { } } -bool ioq_empty(IoQueue_t *q) { +bool ioq_is_empty(IoQueue_t *q) { return (q->size == 0); } -bool ioq_full(IoQueue_t *q) { +bool ioq_is_full(IoQueue_t *q) { return (q->size == IOQ_BUF_DEPTH); } -uint8_t ioq_cur_size(IoQueue_t *q) { +uint8_t ioq_get_cur_size(IoQueue_t *q) { return (q->size); } bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { + // Can't add to a full queue. if (ioq_full(q)) { return false; } + // Can't write longer than our buffer size. if (len > IOQ_BUF_LENGTH) { return false; } - q->backing_sto[q->write_ind].len = len; + // Do copy first and then set the length to confirm data + // is valid in the buffer. memcpy(q->backing_sto[q->write_ind].buf, buf, len); + q->backing_sto[q->write_ind].len = len; + // Critical. A packet can be removed from the queue between assigning size. Should be fast. + __disable_irq(); q->size++; + __enable_irq(); + _increment_write_ind(q); return true; @@ -64,25 +76,30 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { if (ioq_full(q)) { + // If the queue is full, discard the back to maintain + // order and minimize data loss. + ioq_discard_back(q); + // TODO JOE FINISH return false; } + // Gets the pointer for the next buffer to write to. *buf = &q->backing_sto[q->write_ind]; return true; } bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { + // Can't add to a full queue. if (ioq_full(q)) { return false; } - // if (buf != &q->backing_sto[q->write_ind]) { - // return false; - // } - _increment_write_ind(q); + // Critical. A packet can be removed from the queue between assigning size. Should be fast. + __disable_irq(); q->size++; + __enable_irq(); return true; } @@ -91,55 +108,62 @@ bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { // READING // /////////////// -uint8_t ioq_read(IoQueue_t *q, void *dest, uint8_t len) { +bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) { + // Can't read an empty queue. if (ioq_empty(q)) { - return 0; + return false; } - uint8_t cpy_num_bytes = q->backing_sto[q->read_ind].len; + uint16_t cpy_num_bytes = q->backing_sto[q->read_ind].len; + // Intended read size is smaller than intended. if (len < cpy_num_bytes) { - return 0; + return false; } memcpy(dest, q->backing_sto[q->read_ind].buf, cpy_num_bytes); - _increment_read_ind(q); + + // Critical. A packet can be added to the queue between assigning size. Should be fast. + __disable_irq(); q->size--; + __enable_irq(); - return cpy_num_bytes; + *num_bytes_read = cpy_num_bytes; + return true; } bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { + // Can't read an empty queue. if (ioq_empty(q)) { return false; } + // Gets the pointer for the next buffer to be read from. *dest = &q->backing_sto[q->read_ind]; return true; } bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { + // Can't read an empty queue. if (ioq_empty(q)) { return false; } - // looks like the hardware might actually increment the - // mem addr, so we can't do this comparison without additional - // storage and idt that's worth it - // if (dest != &q->backing_sto[q->read_ind]) { - // return false; - // } - _increment_read_ind(q); + // This is probably fine without disable since called from interrupt with high enough + // priority, but just to be safe. + __disable_irq(); q->size--; + __enable_irq(); return true; } -void ioq_discard(IoQueue_t *q) { - while (q->size != 0) { - _increment_read_ind(q); - q->size--; - } +void ioq_discard_back(IoQueue_t *q) { + // Critical. Should be quick but we don't want to infinite loop + __disable_irq(); + _decrement_read_ind(q); + q->size--; + __enable_irq(); } \ No newline at end of file diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 7723cd5b..874b44a6 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -14,14 +14,16 @@ typedef struct IoBuf { uint8_t buf[IOQ_BUF_LENGTH]; - volatile uint8_t len; + // u16 is underlying register size. + volatile uint16_t len; } IoBuf_t; typedef struct IoQueue { volatile uint8_t size; volatile uint8_t read_ind; volatile uint8_t write_ind; - + // Holders are u8, so protect against bigger. + _Static_assert(IOQ_BUF_DEPTH < 255); IoBuf_t backing_sto[IOQ_BUF_DEPTH]; } IoQueue_t; @@ -39,7 +41,7 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf); -uint8_t ioq_read(IoQueue_t *q, void *dest, uint8_t len); +bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest); void ioq_discard(IoQueue_t *q); \ No newline at end of file diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index fc66d393..71ffd582 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -31,9 +31,9 @@ volatile IoBuf_t backing_sto[2]; // PRIVATE FUNCTIONS // ///////////////////////// -bool _uart_start_transmit_dma(); -void _uart_start_rx_dma(); -void _uart_rx_dma(); +void _uart_start_transmit_dma(); +void _uart_start_receive_dma(); +void _uart_receive_dma(); //////////////////////// // PUBLIC FUNCTIONS // @@ -43,18 +43,14 @@ void uart_initialize() { ioq_initialize(&uart_tx_queue); ioq_initialize(&uart_rx_queue); - _uart_start_rx_dma(); + _uart_start_receive_dma(); } ////////// // TX // ////////// -bool uart_can_transmit() { - return false; -} - -bool uart_transmit_dma_pending() { +bool uart_is_transmit_dma_pending() { return uart_dma_tx_active; } @@ -63,25 +59,32 @@ bool uart_wait_for_transmission() { } bool uart_transmit(uint8_t *data_buf, uint16_t len) { - ioq_write(&uart_tx_queue, data_buf, len); + if (!ioq_write(&uart_tx_queue, data_buf, len)) { + // Queue is either full or the data length is invalid. + return false; + } // dma transmission isn't in progress to keep scheduling dma writes // manually start the first/only transfer - if (!uart_dma_tx_active) { + if (!uart_is_transmit_dma_pending()) { _uart_start_transmit_dma(); } + + return true; } -bool _uart_start_transmit_dma() { - // get the next data to read and send down the line +void _uart_start_transmit_dma() { + // Get the next data to read and send down the line. IoBuf_t *tx_buf; - ioq_peek_read(&uart_tx_queue, &tx_buf); + if (!ioq_peek_read(&uart_tx_queue, &tx_buf)) { + // This would fail if the queue is empty, so just don't + // start the DMA. + return; + } - // prevent nested/concurrent transfers + // Prevent nested/concurrent transfers uart_dma_tx_active = true; - // TODO this should be done by the interrupt callback, - // maybe remove this // clear the transfer complete flag USART1->ICR = USART_ICR_TCCF; // clear all interrupt flags on the tx dma channel @@ -109,21 +112,21 @@ void uart_discard() { ioq_discard(&uart_rx_queue); } -uint8_t uart_read(void *dest, uint8_t len) { - return ioq_read(&uart_rx_queue, dest, len); +bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { + return ioq_read(&uart_rx_queue, dest, len, num_bytes_read); } -void _uart_start_rx_dma() { +void _uart_start_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; - ioq_peek_write(&uart_rx_queue, &rx_buf); + if (!ioq_peek_write(&uart_rx_queue, &rx_buf)) { + // Queue is full so don't set up to receive. + return; + } DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; - // DMA1_Channel3->CMAR = ( uint32_t) backing_sto[0].buf; - // DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; - // enable DMA DMA1_Channel3->CCR |= DMA_CCR_EN; @@ -131,7 +134,7 @@ void _uart_start_rx_dma() { USART1->CR1 |= USART_CR1_IDLEIE; } -void _uart_rx_dma() { +void _uart_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); @@ -190,7 +193,7 @@ void DMA1_Channel2_3_IRQHandler() { // max buffer length, which fires USART line idle. We probably got // two packets back to back and need to sort that out if (DMA1->ISR & DMA_ISR_TCIF3) { - // _uart_rx_dma(); + // _uart_receive_dma(); } DMA1->IFCR |= DMA_IFCR_CGIF3; @@ -243,7 +246,7 @@ void USART1_IRQHandler() { // clear idle line int flag USART1->ICR |= USART_ICR_IDLECF; - _uart_rx_dma(); + _uart_receive_dma(); // enable idle interrupts // USART1->CR1 |= USART_CR1_IDLEIE; diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index 72f05f61..5ab1f51b 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -17,7 +17,6 @@ void uart_initialize(); -bool uart_can_transmit(); bool uart_transmit_dma_pending(); bool uart_wait_for_transmission(); bool uart_transmit(uint8_t *data_buf, uint16_t len); diff --git a/util/cmake/stm32-defines.cmake b/util/cmake/stm32-defines.cmake index ebb22e5a..17e30a59 100644 --- a/util/cmake/stm32-defines.cmake +++ b/util/cmake/stm32-defines.cmake @@ -17,7 +17,7 @@ set(NUCLEO_F429ZI_OPENOCD_CFG "board/stm32f429discovery.cfg") ################# set(STSPIN32F0x_MACHINE_OPTIONS -mthumb -mcpu=cortex-m0 -mfloat-abi=soft CACHE STRING "") -set(STSPIN32F0x_C_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS}) +set(STSPIN32F0x_C_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wall) set(STSPIN32F0x_CXX_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wno-register -fno-exceptions -fno-rtti -ffunction-sections -fdata-sections CACHE STRING "") set(STSPIN32F0x_LINKER_OPTIONS ${STSPIN32F0x_MACHINE_OPTIONS} -Wl,--gc-sections,--no-warn-rwx-segments --specs=nosys.specs --specs=nano.specs CACHE STRING "") set(STSPIN32F0x_DEFINITIONS -DSTM32F031xx) From 9fc6bdce562ac0015db031a832c11a79f96a3371 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 28 Jul 2024 13:10:16 +0200 Subject: [PATCH 146/157] Additional review with Will --- motor-controller/common/io_queue.c | 23 +++++++++++++---------- motor-controller/common/io_queue.h | 4 ++-- motor-controller/common/uart.c | 27 ++++++++++++--------------- 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index 521f917f..b2088b47 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -18,6 +18,10 @@ void _increment_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind + 1) % IOQ_BUF_DEPTH; } +void _decrement_write_ind(IoQueue_t *q) { + q->write_ind = (q->write_ind - 1) % IOQ_BUF_DEPTH; +} + void _decrement_read_ind(IoQueue_t *q) { q->read_ind = (q->read_ind - 1) % IOQ_BUF_DEPTH; } @@ -74,24 +78,22 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { return true; } -bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { +void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { if (ioq_full(q)) { // If the queue is full, discard the back to maintain // order and minimize data loss. - ioq_discard_back(q); - // TODO JOE FINISH - return false; + ioq_discard_write_back(q); } // Gets the pointer for the next buffer to write to. *buf = &q->backing_sto[q->write_ind]; - - return true; } -bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf) { +bool ioq_finalize_peek_write(IoQueue_t *q) { // Can't add to a full queue. if (ioq_full(q)) { + // Tried to clear with the peek write, so if still + // full probably filling too fast. return false; } @@ -160,10 +162,11 @@ bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { return true; } -void ioq_discard_back(IoQueue_t *q) { - // Critical. Should be quick but we don't want to infinite loop +void ioq_discard_write_back(IoQueue_t *q) { + // Critical. Should be quick but we don't want to infinite loop with + // additions and decrements. __disable_irq(); - _decrement_read_ind(q); + _decrement_write_ind(q); q->size--; __enable_irq(); } \ No newline at end of file diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 874b44a6..1135f476 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -38,8 +38,8 @@ bool ioq_full(IoQueue_t *q); uint8_t ioq_cur_size(IoQueue_t *q); bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); -bool ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); -bool ioq_finalize_peek_write(IoQueue_t *q, IoBuf_t *buf); +void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); +bool ioq_finalize_peek_write(IoQueue_t *q); bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 71ffd582..32a141a5 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -96,8 +96,6 @@ void _uart_start_transmit_dma() { // enable DMA DMA1_Channel2->CCR |= DMA_CCR_EN; - - return true; } ////////// @@ -119,10 +117,7 @@ bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { void _uart_start_receive_dma() { // get the next data to read and send down the line IoBuf_t *rx_buf; - if (!ioq_peek_write(&uart_rx_queue, &rx_buf)) { - // Queue is full so don't set up to receive. - return; - } + ioq_peek_write(&uart_rx_queue, &rx_buf); DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; @@ -139,18 +134,20 @@ void _uart_receive_dma() { IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); - // check if were filling the last slot - if (ioq_cur_size(&uart_rx_queue) < (IOQ_BUF_DEPTH - 1)) { - uint8_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); - rx_buf->len = transmitted_bytes; + // Peak write discards the last entry if we are full, + // so we will always be good on transfers. - // data and len now correct, finalize write - ioq_finalize_peek_write(&uart_rx_queue, NULL); + // CNDTR is number of bytes left so max + // tranfer size - size left is transfer size. + uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); + rx_buf->len = transmitted_bytes; - // re-peek after potential finalization - ioq_peek_write(&uart_rx_queue, &rx_buf); - } + // data and len now correct, finalize write + // TODO JOE Need a hardware failure/indicator if this returns false. + // Implies RX happens twice without handling + ioq_finalize_peek_write(&uart_rx_queue); + // Assigns the place and length for the write. DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; } From 49d92eea5b7026544058c81352f37460f253de80 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 20 Oct 2024 22:19:19 -0700 Subject: [PATCH 147/157] WIP continued review code --- motor-controller/bin/wheel/setup.c | 62 ++++++++++++---------- motor-controller/common/uart.c | 84 +++++++++++++++++++++--------- motor-controller/common/uart.h | 15 ++++-- 3 files changed, 106 insertions(+), 55 deletions(-) diff --git a/motor-controller/bin/wheel/setup.c b/motor-controller/bin/wheel/setup.c index 705cc499..dd339dff 100644 --- a/motor-controller/bin/wheel/setup.c +++ b/motor-controller/bin/wheel/setup.c @@ -4,15 +4,15 @@ * @brief setup function for the STSPIN32 * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * * Relevant Documents: * stm32f031x4/stm32f031x6 reference "stspin reference" * ARM-Based 32-bit MCU with up to 32 Kbyte Flash, 9 timers, ADC and communication interfaces, 2.0 - 3.6V, 106 pages * stm32f0x1/stm32f0x2/stm32f0x8 reference "m0 reference" - * 1004 pages - * + * 1004 pages + * */ #include @@ -30,10 +30,10 @@ void HardFault_Handler() { /** * @brief Setup the clock tree - * + * * Clock tree diagram: pg 14 / 106 of the "stspin reference" * Register info: pg 108+ / 1004 of the "m0 reference" - * + * */ __attribute__((optimize("O0"))) __attribute__((always_inline)) @@ -102,17 +102,17 @@ inline void setup_clocks() { RCC->BDCR |= RCC_BDCR_RTCSEL_LSI; // HSI14 enable (for ADC) - // on by default?? + // on by default?? - // Setup TIM14 sysclk source + // Setup TIM14 sysclk source RCC->CFGR |= RCC_CFGR_MCO_SYSCLK; // MCOPRE -> 1 RCC->CFGR |= RCC_CFGR_MCOPRE_DIV1; // https://developer.arm.com/documentation/dui0552/a/cortex-m3-peripherals/system-timer--systick/systick-reload-value-register - // set source to sysclk (48MHz) + // set source to sysclk (48MHz) SysTick->CTRL |= SysTick_CTRL_CLKSOURCE_Msk; - // timer counts down to fire approximately every 1ms + // timer counts down to fire approximately every 1ms SysTick->LOAD = (F_SYS_CLK_HZ / 1000UL); // current value set to 0, e.g. no trigger event SysTick->VAL = (F_SYS_CLK_HZ / 1000UL) - 1; @@ -124,7 +124,7 @@ inline void setup_clocks() { /** * @brief sets up base IO - * + * */ __attribute__((optimize("O0"))) inline void setup_io() { @@ -167,7 +167,7 @@ inline void setup_io() { /** * @brief setups UART IO, UART, and UART DMA - * + * * TODO move to uart.c */ __attribute__((optimize("O0"))) @@ -189,7 +189,7 @@ void setup_uart() { // configure PA14 and PA15 AF mode GPIOA->MODER |= (GPIO_MODER_MODER14_1 | GPIO_MODER_MODER15_1); // configure PA14 and PA15 pin speed to 10Mhz - GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); + GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); // configure PA14 and PA15 pin pullup to UP GPIOA->PUPDR |= (GPIO_PUPDR_PUPDR14_0 | GPIO_PUPDR_PUPDR15_0); // configure PA14 and PA15 alternate function @@ -203,12 +203,12 @@ void setup_uart() { // UART Peripheral Setup // ///////////////////////////// - // make sure USART1 is disabled + // make sure USART1 is disabled while configuring. USART1->CR1 &= ~(USART_CR1_UE); // 9-bits with parity (PCE) insert parity bit at the 9th bit // defaults to even parity USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); - // enable transmision + // Enable transmit and receive functionality. USART1->CR1 |= (USART_CR1_TE | USART_CR1_RE); // we don't need anything here USART1->CR2 = 0; @@ -217,42 +217,50 @@ void setup_uart() { // set baud rate USART1->BRR = 0x18; // => 2 Mbaud/s - // enable the module + // Enable the module now that configuration is finished. USART1->CR1 |= USART_CR1_UE; - // Enable idle line interrupt + // Clear idle line interrupt USART1->ICR |= USART_ICR_IDLECF; + // FUTURE: We can probably enable the idle line interrupt here + // and leave it enabled after we add pullups to the bus. // USART1->CR1 |= USART_CR1_IDLEIE; ////////////////////////////////////// // Transmission DMA Channel Setup // ////////////////////////////////////// - // medium priority, memory increment, memory to peripheral - DMA1_Channel2->CCR = DMA_CCR_PL_0 | DMA_CCR_MINC | DMA_CCR_DIR; + // Memory increment, memory to peripheral + DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; + // DMA set to Medium Priority + DMA1_Channel2->CCR |= DMA_CCR_PL_0; // clear buffer base addr DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time // clear transmission length DMA1_Channel2->CNDTR = 0; // transmit length, set at transmission time // set destination address as UART periperal transmission shift register DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address - // enable the transfer complete (TCIE) and transfer error (TEIE) interrupts + // Enable the transfer complete (TCIE) and transfer error (TEIE) interrupts DMA1_Channel2->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); - // clear channel 2, global IF, transfer error IF, half-transfer IF, and transfer complete IF + // Clear the Global Ch2 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF2; ///////////////////////////////// // Receive DMA Channel Setup // ///////////////////////////////// - // USART1_RX, low priority, memory increment, peripheral to memory - // DMA1_Channel3->CCR = DMA_CCR_MINC | DMA_CCR_CIRC; - DMA1_Channel3->CCR = DMA_CCR_MINC; - DMA1_Channel3->CCR |= (0x3U << DMA_CCR_PL_Pos); + // USART1_RX memory increment, peripheral to memory + // Sets memory increment mode. + DMA1_Channel3->CCR = DMA_CCR_MINC; + // DMA set to High Priority + DMA1_Channel3->CCR |= DMA_CCR_PL_1; + // clear buffer base addr DMA1_Channel3->CMAR = (uint32_t) 0 ; + // Set destination address as UART periperal receive register DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; + // clear transmission length DMA1_Channel3->CNDTR = 0; - // DMA1_Channel3->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); + // Clear the Global Ch3 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF3; ///////////////// @@ -268,7 +276,7 @@ void setup_uart() { /** * @brief performs startup system configuration - * + * */ __attribute__((optimize("O0"))) //__attribute__((always_inline)) diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 32a141a5..99d5c4e8 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -1,12 +1,12 @@ /** * @file uart.c * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-30 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -40,9 +40,15 @@ void _uart_receive_dma(); //////////////////////// void uart_initialize() { + ioq_initialize(&uart_tx_queue); ioq_initialize(&uart_rx_queue); + // Make sure SYSCFG bit(s) are cleared for USART TX and RX + // to be on DMA Ch2 and Ch3, respectively. + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_USART1TX_DMA_RMP); + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_USART1RX_DMA_RMP); + _uart_start_receive_dma(); } @@ -123,7 +129,7 @@ void _uart_start_receive_dma() { DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; // enable DMA - DMA1_Channel3->CCR |= DMA_CCR_EN; + DMA1_Channel3->CCR |= DMA_CCR_EN; USART1->ICR |= USART_ICR_IDLECF; USART1->CR1 |= USART_CR1_IDLEIE; @@ -134,11 +140,11 @@ void _uart_receive_dma() { IoBuf_t *rx_buf; ioq_peek_write(&uart_rx_queue, &rx_buf); - // Peak write discards the last entry if we are full, - // so we will always be good on transfers. + // Peak write discards the last entry if we are full, + // so we will always be good on transfers. - // CNDTR is number of bytes left so max - // tranfer size - size left is transfer size. + // CNDTR is number of bytes left so + // (max tranfer size - size left) is transfer size. uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); rx_buf->len = transmitted_bytes; @@ -157,64 +163,91 @@ void _uart_receive_dma() { ////////////////////////// /** - * @brief callback handler for DMA RX - * + * @brief callback handler for USART DMA1 Ch2 (TX) and Ch3 (RX) + * */ void DMA1_Channel2_3_IRQHandler() { - // check if DMA1 CH2 has any interrupt, this is for transmit + // Check if DMA1 Ch2 (TX) has any interrupts. if (DMA1->ISR & DMA_ISR_GIF2) { - // transmit had a DMA error + // Transmit had a DMA error. Occurs when the USART is + // reading / writing at a reserved address. if (DMA1->ISR & DMA_ISR_TEIF2) { - // TODO log error + uart_logging_status = UART_LOGGING_DMA_TX_ERROR; + // In COMP_MODE, try to just clear the error and then + // continue with transfers. + #ifdef COMP_MODE + // If TEIF2 is set, the CCR is disabled automatically. + // First need to clear Transfer Error Flag. + DMA1->IFCR |= DMA_IFCR_CTEIF2; + // Then reset the DMA channel control (CCR). + DMA1_Channel2->CCR |= DMA_CCR_EN; + #endif } // DMA finished transfer to USART if (DMA1->ISR & DMA_ISR_TCIF2) { - // expect the USART1 TC interrupt to fire once last byte is out of the FIFO/TR - // make sure tx complete inerrupts are on + // Expect the USART1 Transfer Complete interrupt to + // fire once last byte is out of + // the FIFO/Transmit Register. + // Since USART1 handler disables the Transfer Complete + // interrupt, we need to re-enable it here. USART1->CR1 |= USART_CR1_TCIE; } + // Clears the interrupt flags for Ch2. DMA1->IFCR |= DMA_IFCR_CGIF2; } - // check if DMA1 CH3 has any interrupt, this is for receive + // Check if DMA1 Ch3 (RX) has any interrupt. if (DMA1->ISR & DMA_ISR_GIF3) { - // receive had a dma error + // Receive had a DMA error if (DMA1->ISR & DMA_ISR_TEIF3) { - // TODO: log error + uart_logging_status = UART_LOGGING_DMA_RX_ERROR; + // In COMP_MODE, try to just clear the error and then + // continue with transfers. + #ifdef COMP_MODE + // If TEIF3 is set, the CCR is disabled automatically. + // First need to clear Transfer Error Flag. + DMA1->IFCR |= DMA_IFCR_CTEIF3; + // Then reset the DMA channel control (CCR). + DMA1_Channel3->CCR |= DMA_CCR_EN; + #endif } // receive, got a full buffer + // TODO JOE look into this. // this is sort of unexpected, we generally expect a packet less than - // max buffer length, which fires USART line idle. We probably got + // max buffer length, which fires USART line idle. We probably got // two packets back to back and need to sort that out if (DMA1->ISR & DMA_ISR_TCIF3) { // _uart_receive_dma(); } + // Clears the interrupt flags for Ch3. DMA1->IFCR |= DMA_IFCR_CGIF3; } } /** * @brief callback handler for uart - * + * */ __attribute((__optimize__("O0"))) void USART1_IRQHandler() { - uint32_t uart_status_register = USART1->ISR; + const uint32_t uart_status_register = USART1->ISR; //////////////////// // Transmission // //////////////////// + // If it's a USART transmit complete interrupt: if (uart_status_register & USART_ISR_TC) { - // disable DMA channel + // Disable TX DMA channel until we + // know we have a payload to send. DMA1_Channel2->CCR &= ~(DMA_CCR_EN); - // disable transfer complete interrupts + // Disable transfer complete interrupts. USART1->CR1 &= ~(USART_CR1_TCIE); - + // Clear transmission complete flag. USART1->ICR |= USART_ICR_TCCF; // finalize tx queue read @@ -232,7 +265,8 @@ void USART1_IRQHandler() { //////////////////// // Reception // //////////////////// - + // TODO Check parity? + // TODO Finish looking over this. // detected idle line before full buf if (uart_status_register & USART_ISR_IDLE) { // disable DMA diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index 5ab1f51b..f1d6a89c 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -1,15 +1,16 @@ /** * @file uart.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-30 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once +#define COMP_MODE //////////////////////// // PUBLIC FUNCTIONS // @@ -24,3 +25,11 @@ bool uart_transmit(uint8_t *data_buf, uint16_t len); bool uart_can_read(); uint8_t uart_read(void *dest, uint8_t len); void uart_discard(); + +volatile uart_logging_status_t uart_logging_status; + +typedef enum { + UART_LOGGING_OK = 0, + UART_LOGGING_DMA_TX_ERROR, + UART_LOGGING_DMA_RX_ERROR +} uart_logging_status_t; From e1ff9297946f128df65b8305be9e1cbc0291e40b Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 22 Oct 2024 19:56:53 -0700 Subject: [PATCH 148/157] Finished up UART review --- motor-controller/bin/dribbler/main.c | 20 +++++-- motor-controller/bin/wheel/main.c | 11 +++- motor-controller/common/io_queue.c | 22 +++---- motor-controller/common/io_queue.h | 8 +-- motor-controller/common/uart.c | 87 ++++++++++++++++------------ motor-controller/common/uart.h | 21 +++---- 6 files changed, 99 insertions(+), 70 deletions(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 24e318d0..88199065 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -75,7 +75,7 @@ int main() { response_packet.data.motion.reset_low_power = rcc_csr & RCC_CSR_LPWRRSTF != 0; response_packet.data.motion.reset_software = rcc_csr & RCC_CSR_SFTRSTF != 0; response_packet.data.motion.reset_pin = rcc_csr & RCC_CSR_PINRSTF != 0; - + bool params_return_packet_requested = false; SyncTimer_t torque_loop_timer; @@ -87,7 +87,7 @@ int main() { iir_filter_init(&torque_filter, iir_filter_alpha_from_Tf(TORQUE_IIR_TF_MS, TORQUE_LOOP_RATE_MS)); MotorCommand_MotionType motion_control_type = OPEN_LOOP; - + float r_motor_board = 0.0f; float u_torque_loop; float cur_limit = 0.0f; @@ -114,14 +114,22 @@ int main() { while (uart_can_read()) { MotorCommandPacket motor_command_packet; - uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); - if (bytes_moved != sizeof(MotorCommandPacket)) { + uint16_t uart_bytes_read = 0; + bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); + if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { // something went wrong, just purge all of the data uart_discard(); continue; } + // If something goes wrong with the UART, we need to flag it. + if (uart_logging_status != UART_LOGGING_OK) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + } + + if (motor_command_packet.type == MCP_MOTION) { // GPIOB->BSRR |= GPIO_BSRR_BS_8; @@ -177,12 +185,12 @@ int main() { } else { GPIOB->BSRR |= GPIO_BSRR_BS_7; } - + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { r_motor_board = 0.0f; // Error pin enable. GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. + // HACK Will force the watchdog to trigger. while(true); } else { GPIOB->BSRR |= GPIO_BSRR_BR_8; diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 6e54e47c..86d9ad95 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -175,14 +175,21 @@ int main() { // process all available packets while (uart_can_read()) { - uint8_t bytes_moved = uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); - if (bytes_moved != sizeof(MotorCommandPacket)) { + uint16_t uart_bytes_read = 0; + bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); + if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { // something went wrong, just purge all of the data uart_discard(); continue; } + // If something goes wrong with the UART, we need to flag it. + if (uart_logging_status != UART_LOGGING_OK) { + // Error pin enable. + GPIOB->BSRR |= GPIO_BSRR_BS_8; + } + if (motor_command_packet.type == MCP_MOTION) { // we got a motion packet! diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index b2088b47..977000d6 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -1,4 +1,4 @@ - +#include #include #include #include @@ -54,7 +54,7 @@ uint8_t ioq_get_cur_size(IoQueue_t *q) { bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { // Can't add to a full queue. - if (ioq_full(q)) { + if (ioq_is_full(q)) { return false; } @@ -63,7 +63,7 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { return false; } - // Do copy first and then set the length to confirm data + // Do copy first and then set the length to confirm data // is valid in the buffer. memcpy(q->backing_sto[q->write_ind].buf, buf, len); q->backing_sto[q->write_ind].len = len; @@ -79,8 +79,8 @@ bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len) { } void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { - if (ioq_full(q)) { - // If the queue is full, discard the back to maintain + if (ioq_is_full(q)) { + // If the queue is full, discard the back to maintain // order and minimize data loss. ioq_discard_write_back(q); } @@ -91,7 +91,7 @@ void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf) { bool ioq_finalize_peek_write(IoQueue_t *q) { // Can't add to a full queue. - if (ioq_full(q)) { + if (ioq_is_full(q)) { // Tried to clear with the peek write, so if still // full probably filling too fast. return false; @@ -112,12 +112,12 @@ bool ioq_finalize_peek_write(IoQueue_t *q) { bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } uint16_t cpy_num_bytes = q->backing_sto[q->read_ind].len; - // Intended read size is smaller than intended. + // Intended read size is smaller than intended. if (len < cpy_num_bytes) { return false; } @@ -136,7 +136,7 @@ bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } @@ -148,12 +148,12 @@ bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest) { bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest) { // Can't read an empty queue. - if (ioq_empty(q)) { + if (ioq_is_empty(q)) { return false; } _increment_read_ind(q); - // This is probably fine without disable since called from interrupt with high enough + // This is probably fine without disable since called from interrupt with high enough // priority, but just to be safe. __disable_irq(); q->size--; diff --git a/motor-controller/common/io_queue.h b/motor-controller/common/io_queue.h index 1135f476..7c5367a1 100644 --- a/motor-controller/common/io_queue.h +++ b/motor-controller/common/io_queue.h @@ -33,9 +33,9 @@ typedef struct IoQueue { void ioq_initialize(IoQueue_t *q); -bool ioq_empty(IoQueue_t *q); -bool ioq_full(IoQueue_t *q); -uint8_t ioq_cur_size(IoQueue_t *q); +bool ioq_is_empty(IoQueue_t *q); +bool ioq_is_full(IoQueue_t *q); +uint8_t ioq_get_cur_size(IoQueue_t *q); bool ioq_write(IoQueue_t *q, uint8_t *buf, uint16_t len); void ioq_peek_write(IoQueue_t *q, IoBuf_t **buf); @@ -44,4 +44,4 @@ bool ioq_finalize_peek_write(IoQueue_t *q); bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t *num_bytes_read); bool ioq_peek_read(IoQueue_t *q, IoBuf_t **dest); bool ioq_finalize_peek_read(IoQueue_t *q, IoBuf_t *dest); -void ioq_discard(IoQueue_t *q); \ No newline at end of file +void ioq_discard_write_back(IoQueue_t *q); \ No newline at end of file diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 99d5c4e8..4ad05baa 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -25,7 +25,6 @@ IoQueue_t uart_tx_queue; volatile bool uart_dma_rx_active = false; volatile int uart_dma_rx_num_bytes = 0; IoQueue_t uart_rx_queue; -volatile IoBuf_t backing_sto[2]; ///////////////////////// // PRIVATE FUNCTIONS // @@ -33,7 +32,7 @@ volatile IoBuf_t backing_sto[2]; void _uart_start_transmit_dma(); void _uart_start_receive_dma(); -void _uart_receive_dma(); +void _uart_receive_dma(bool parity_error); //////////////////////// // PUBLIC FUNCTIONS // @@ -60,8 +59,8 @@ bool uart_is_transmit_dma_pending() { return uart_dma_tx_active; } -bool uart_wait_for_transmission() { - while (uart_transmit_dma_pending()); +void uart_wait_for_transmission() { + while (uart_is_transmit_dma_pending()); } bool uart_transmit(uint8_t *data_buf, uint16_t len) { @@ -109,11 +108,11 @@ void _uart_start_transmit_dma() { ////////// bool uart_can_read() { - return (!ioq_empty(&uart_rx_queue)); + return (!ioq_is_empty(&uart_rx_queue)); } void uart_discard() { - ioq_discard(&uart_rx_queue); + ioq_discard_write_back(&uart_rx_queue); } bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { @@ -135,27 +134,40 @@ void _uart_start_receive_dma() { USART1->CR1 |= USART_CR1_IDLEIE; } -void _uart_receive_dma() { - // get the next data to read and send down the line - IoBuf_t *rx_buf; - ioq_peek_write(&uart_rx_queue, &rx_buf); - - // Peak write discards the last entry if we are full, - // so we will always be good on transfers. - - // CNDTR is number of bytes left so - // (max tranfer size - size left) is transfer size. - uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); - rx_buf->len = transmitted_bytes; +void _uart_receive_dma(bool parity_error) { + // If we have a parity error, we can just overwrite the queue + // with the next packet. + if (!parity_error) { + // Get the buffer location that was written into with + // last DMA contents. + IoBuf_t *rx_buf; + ioq_peek_write(&uart_rx_queue, &rx_buf); + + // Peek write discards the last entry if we are full, + // so we will always be good on transfers. + + // CNDTR is number of bytes left in the DMA buffer so + // (max transfer size - size left) is packet transfer size. + uint16_t transmitted_bytes = (IOQ_BUF_LENGTH - DMA1_Channel3->CNDTR); + rx_buf->len = transmitted_bytes; + + // data and len now correct, finalize write + // If this returns false, implies RX happens twice without handling. + if (!ioq_finalize_peek_write(&uart_rx_queue)) { + uart_logging_status = UART_LOGGING_UART_RX_BUFFER_FULL; + } + } - // data and len now correct, finalize write - // TODO JOE Need a hardware failure/indicator if this returns false. - // Implies RX happens twice without handling - ioq_finalize_peek_write(&uart_rx_queue); + // Get the NEXT buffer for the DMA to write into. + IoBuf_t* rx_buf_next; + ioq_peek_write(&uart_rx_queue, &rx_buf_next); - // Assigns the place and length for the write. - DMA1_Channel3->CMAR = (uint32_t) rx_buf->buf; + // Assigns the location and length for the NEXT DMA write. + DMA1_Channel3->CMAR = (uint32_t) rx_buf_next->buf; DMA1_Channel3->CNDTR = IOQ_BUF_LENGTH; + + // DMA is now ready to receive the next payload, so enable DMA. + DMA1_Channel3->CCR |= DMA_CCR_EN; } ////////////////////////// @@ -214,13 +226,12 @@ void DMA1_Channel2_3_IRQHandler() { #endif } - // receive, got a full buffer - // TODO JOE look into this. - // this is sort of unexpected, we generally expect a packet less than + // Receive, got a full buffer + // This is sort of unexpected, we generally expect less than // max buffer length, which fires USART line idle. We probably got - // two packets back to back and need to sort that out + // two packets back to back and need to sort that out. if (DMA1->ISR & DMA_ISR_TCIF3) { - // _uart_receive_dma(); + uart_logging_status = UART_LOGGING_DMA_RX_BUFFER_FULL; } // Clears the interrupt flags for Ch3. @@ -254,7 +265,7 @@ void USART1_IRQHandler() { // check if queue is empty // conditionally recall transmit ioq_finalize_peek_read(&uart_tx_queue, NULL); - if (!ioq_empty(&uart_tx_queue)) { + if (!ioq_is_empty(&uart_tx_queue)) { _uart_start_transmit_dma(); } else { // restore the ready flag @@ -265,24 +276,26 @@ void USART1_IRQHandler() { //////////////////// // Reception // //////////////////// - // TODO Check parity? - // TODO Finish looking over this. // detected idle line before full buf if (uart_status_register & USART_ISR_IDLE) { // disable DMA DMA1_Channel3->CCR &= ~(DMA_CCR_EN); - // disable idle interrupts + // FUTURE: Need to fix pull-ups to disable idle interrupts // USART1->CR1 &= ~(USART_CR1_IDLEIE); // clear idle line int flag USART1->ICR |= USART_ICR_IDLECF; - _uart_receive_dma(); + // Moves the received DMA data to the queue + // and prepares the next DMA transfer. + bool parity_error = (uart_status_register & USART_ISR_PE); + _uart_receive_dma(parity_error); + + // Clear the parity error flag for the next transfer. + USART1->ICR |= USART_ICR_PECF; + // FUTURE: Need to fix pull-ups to enable idle interrupts // enable idle interrupts // USART1->CR1 |= USART_CR1_IDLEIE; - - // enable DMA - DMA1_Channel3->CCR |= DMA_CCR_EN; } } \ No newline at end of file diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index f1d6a89c..4cd59d8d 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -10,7 +10,14 @@ */ #pragma once -#define COMP_MODE + +typedef enum { + UART_LOGGING_OK = 0, + UART_LOGGING_DMA_TX_ERROR, + UART_LOGGING_DMA_RX_ERROR, + UART_LOGGING_DMA_RX_BUFFER_FULL, + UART_LOGGING_UART_RX_BUFFER_FULL +} uart_logging_status_t; //////////////////////// // PUBLIC FUNCTIONS // @@ -19,17 +26,11 @@ void uart_initialize(); bool uart_transmit_dma_pending(); -bool uart_wait_for_transmission(); +void uart_wait_for_transmission(); bool uart_transmit(uint8_t *data_buf, uint16_t len); bool uart_can_read(); -uint8_t uart_read(void *dest, uint8_t len); void uart_discard(); +bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read); -volatile uart_logging_status_t uart_logging_status; - -typedef enum { - UART_LOGGING_OK = 0, - UART_LOGGING_DMA_TX_ERROR, - UART_LOGGING_DMA_RX_ERROR -} uart_logging_status_t; +static volatile uart_logging_status_t uart_logging_status; From 69a7949da8d66859c7a7fcb8be979dba72abb07f Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 29 Oct 2024 18:32:54 -0700 Subject: [PATCH 149/157] White space --- motor-controller/bin/dribbler/main.c | 1 - 1 file changed, 1 deletion(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 88199065..9ef9563c 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -129,7 +129,6 @@ int main() { GPIOB->BSRR |= GPIO_BSRR_BS_8; } - if (motor_command_packet.type == MCP_MOTION) { // GPIOB->BSRR |= GPIO_BSRR_BS_8; From 7e45e7bda9f171d239d0efa03e9ed62e7af9ea97 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 29 Oct 2024 19:12:10 -0700 Subject: [PATCH 150/157] Copy setup formatting --- motor-controller/bin/dribbler/setup.c | 86 ++++++++++++++------------- motor-controller/bin/wheel/setup.c | 34 +++++------ 2 files changed, 63 insertions(+), 57 deletions(-) diff --git a/motor-controller/bin/dribbler/setup.c b/motor-controller/bin/dribbler/setup.c index fe615064..42acf1e3 100644 --- a/motor-controller/bin/dribbler/setup.c +++ b/motor-controller/bin/dribbler/setup.c @@ -4,15 +4,15 @@ * @brief setup function for the STSPIN32 * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * * Relevant Documents: * stm32f031x4/stm32f031x6 reference "stspin reference" * ARM-Based 32-bit MCU with up to 32 Kbyte Flash, 9 timers, ADC and communication interfaces, 2.0 - 3.6V, 106 pages * stm32f0x1/stm32f0x2/stm32f0x8 reference "m0 reference" - * 1004 pages - * + * 1004 pages + * */ #include @@ -30,10 +30,10 @@ void HardFault_Handler() { /** * @brief Setup the clock tree - * + * * Clock tree diagram: pg 14 / 106 of the "stspin reference" * Register info: pg 108+ / 1004 of the "m0 reference" - * + * */ __attribute__((optimize("O0"))) __attribute__((always_inline)) @@ -42,9 +42,9 @@ inline void setup_clocks() { RCC->CR |= RCC_CR_HSION; RCC->CFGR = 0x00000000; RCC->CR &= ~(RCC_CR_PLLON | RCC_CR_CSSON | RCC_CR_HSEBYP | RCC_CR_HSEON); - RCC->CFGR2 = 0; - RCC->CFGR3 = 0; - RCC->CIR = 0; + RCC->CFGR2 = 0; + RCC->CFGR3 = 0; + RCC->CIR = 0; // wait for HSI to be stable while ((RCC->CR & RCC_CR_HSIRDY) == 0) { @@ -102,17 +102,17 @@ inline void setup_clocks() { RCC->BDCR |= RCC_BDCR_RTCSEL_LSI; // HSI14 enable (for ADC) - // on by default?? + // on by default?? - // Setup TIM14 sysclk source + // Setup TIM14 sysclk source RCC->CFGR |= RCC_CFGR_MCO_SYSCLK; // MCOPRE -> 1 RCC->CFGR |= RCC_CFGR_MCOPRE_DIV1; // https://developer.arm.com/documentation/dui0552/a/cortex-m3-peripherals/system-timer--systick/systick-reload-value-register - // set source to sysclk (48MHz) + // set source to sysclk (48MHz) SysTick->CTRL |= SysTick_CTRL_CLKSOURCE_Msk; - // timer counts down to fire approximately every 1ms + // timer counts down to fire approximately every 1ms SysTick->LOAD = (F_SYS_CLK_HZ / 1000UL); // current value set to 0, e.g. no trigger event SysTick->VAL = (F_SYS_CLK_HZ / 1000UL) - 1; @@ -124,7 +124,7 @@ inline void setup_clocks() { /** * @brief sets up base IO - * + * */ __attribute__((optimize("O0"))) inline void setup_io() { @@ -162,7 +162,7 @@ inline void setup_io() { /** * @brief setups UART IO, UART, and UART DMA - * + * * TODO move to uart.c */ __attribute__((optimize("O0"))) @@ -184,7 +184,7 @@ void setup_uart() { // configure PA14 and PA15 AF mode GPIOA->MODER |= (GPIO_MODER_MODER14_1 | GPIO_MODER_MODER15_1); // configure PA14 and PA15 pin speed to 10Mhz - GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); + GPIOA->OSPEEDR |= (GPIO_OSPEEDR_OSPEEDR14_0 | GPIO_OSPEEDR_OSPEEDR15_0); // configure PA14 and PA15 pin pullup to UP GPIOA->PUPDR |= (GPIO_PUPDR_PUPDR14_0 | GPIO_PUPDR_PUPDR15_0); // configure PA14 and PA15 alternate function @@ -198,23 +198,23 @@ void setup_uart() { // UART Peripheral Setup // ///////////////////////////// - // make sure USART1 is disabled + // make sure USART1 is disabled while configuring. USART1->CR1 &= ~(USART_CR1_UE); // 9-bits with parity (PCE) inserst parity bit at the 9th bit - USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); - // enable transmision + USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); + // Enable transmit and receive functionality. USART1->CR1 |= (USART_CR1_TE | USART_CR1_RE); // we don't need anything here - USART1->CR2 = 0; + USART1->CR2 = 0; // enable DMA for transmission and receive - USART1->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; + USART1->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; // set baud rate - USART1->BRR = 0x18; // => 2 Mbaud/s + USART1->BRR = 0x18; // => 2 Mbaud/s - // enable the module - USART1->CR1 |= USART_CR1_UE; + // Enable the module now that configuration is finished. + USART1->CR1 |= USART_CR1_UE; - // Enable idle line interrupt + // Clear idle line interrupt USART1->ICR |= USART_ICR_IDLECF; // USART1->CR1 |= USART_CR1_IDLEIE; @@ -222,31 +222,37 @@ void setup_uart() { // Transmission DMA Channel Setup // ////////////////////////////////////// - // medium priority, memory increment, memory to peripheral - DMA1_Channel2->CCR = DMA_CCR_PL_0 | DMA_CCR_MINC | DMA_CCR_DIR; + // Memory increment, memory to peripheral + DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; + // DMA set to Medium Priority + DMA1_Channel2->CCR |= DMA_CCR_PL_0; // clear buffer base addr - DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time + DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time // clear transmission length DMA1_Channel2->CNDTR = 0; // transmit length, set at transmission time // set destination address as UART periperal transmission shift register - DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address - // enable the transfer complete (TCIE) and transfer error (TEIE) interrupts + DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address + // Enable the transfer complete (TCIE) and transfer error (TEIE) interrupts DMA1_Channel2->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); - // clear channel 2, global IF, transfer error IF, half-transfer IF, and transfer complete IF + // Clear the Global Ch2 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF2; ///////////////////////////////// // Receive DMA Channel Setup // ///////////////////////////////// - // USART1_RX, low priority, memory increment, peripheral to memory - // DMA1_Channel3->CCR = DMA_CCR_MINC | DMA_CCR_CIRC; - DMA1_Channel3->CCR = DMA_CCR_MINC; - DMA1_Channel3->CCR |= (0x3U << DMA_CCR_PL_Pos); - DMA1_Channel3->CMAR = (uint32_t) 0 ; - DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; - DMA1_Channel3->CNDTR = 0; - // DMA1_Channel3->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); + // USART1_RX memory increment, peripheral to memory + // Sets memory increment mode. + DMA1_Channel3->CCR = DMA_CCR_MINC; + // DMA set to High Priority + DMA1_Channel3->CCR |= DMA_CCR_PL_1; + // clear buffer base addr + DMA1_Channel3->CMAR = (uint32_t) 0 ; + // Set destination address as UART periperal receive register + DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; + // clear transmission length + DMA1_Channel3->CNDTR = 0; + // Clear the Global Ch3 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF3; ///////////////// @@ -262,7 +268,7 @@ void setup_uart() { /** * @brief performs startup system configuration - * + * */ __attribute__((optimize("O0"))) //__attribute__((always_inline)) diff --git a/motor-controller/bin/wheel/setup.c b/motor-controller/bin/wheel/setup.c index dd339dff..f91717fe 100644 --- a/motor-controller/bin/wheel/setup.c +++ b/motor-controller/bin/wheel/setup.c @@ -42,9 +42,9 @@ inline void setup_clocks() { RCC->CR |= RCC_CR_HSION; RCC->CFGR = 0x00000000; RCC->CR &= ~(RCC_CR_PLLON | RCC_CR_CSSON | RCC_CR_HSEBYP | RCC_CR_HSEON); - RCC->CFGR2 = 0; - RCC->CFGR3 = 0; - RCC->CIR = 0; + RCC->CFGR2 = 0; + RCC->CFGR3 = 0; + RCC->CIR = 0; // wait for HSI to be stable while ((RCC->CR & RCC_CR_HSIRDY) == 0) { @@ -207,18 +207,18 @@ void setup_uart() { USART1->CR1 &= ~(USART_CR1_UE); // 9-bits with parity (PCE) insert parity bit at the 9th bit // defaults to even parity - USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); + USART1->CR1 |= (USART_CR1_M | USART_CR1_PCE); // Enable transmit and receive functionality. USART1->CR1 |= (USART_CR1_TE | USART_CR1_RE); // we don't need anything here - USART1->CR2 = 0; + USART1->CR2 = 0; // enable DMA for transmission and receive - USART1->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; + USART1->CR3 = USART_CR3_DMAT | USART_CR3_DMAR; // set baud rate - USART1->BRR = 0x18; // => 2 Mbaud/s + USART1->BRR = 0x18; // => 2 Mbaud/s // Enable the module now that configuration is finished. - USART1->CR1 |= USART_CR1_UE; + USART1->CR1 |= USART_CR1_UE; // Clear idle line interrupt USART1->ICR |= USART_ICR_IDLECF; @@ -230,16 +230,16 @@ void setup_uart() { // Transmission DMA Channel Setup // ////////////////////////////////////// - // Memory increment, memory to peripheral - DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; + // Memory increment, memory to peripheral + DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; // DMA set to Medium Priority DMA1_Channel2->CCR |= DMA_CCR_PL_0; // clear buffer base addr - DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time + DMA1_Channel2->CMAR = 0; // transmit buffer base addr, set at transmission time // clear transmission length DMA1_Channel2->CNDTR = 0; // transmit length, set at transmission time // set destination address as UART periperal transmission shift register - DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address + DMA1_Channel2->CPAR = (uint32_t) &USART1->TDR; // USART1 data transmit register address // Enable the transfer complete (TCIE) and transfer error (TEIE) interrupts DMA1_Channel2->CCR |= (DMA_CCR_TEIE | DMA_CCR_TCIE); // Clear the Global Ch2 interrupt flag. @@ -249,17 +249,17 @@ void setup_uart() { // Receive DMA Channel Setup // ///////////////////////////////// - // USART1_RX memory increment, peripheral to memory - // Sets memory increment mode. + // USART1_RX memory increment, peripheral to memory + // Sets memory increment mode. DMA1_Channel3->CCR = DMA_CCR_MINC; // DMA set to High Priority DMA1_Channel3->CCR |= DMA_CCR_PL_1; // clear buffer base addr - DMA1_Channel3->CMAR = (uint32_t) 0 ; + DMA1_Channel3->CMAR = (uint32_t) 0 ; // Set destination address as UART periperal receive register - DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; + DMA1_Channel3->CPAR = (uint32_t) &USART1->RDR; // clear transmission length - DMA1_Channel3->CNDTR = 0; + DMA1_Channel3->CNDTR = 0; // Clear the Global Ch3 interrupt flag. DMA1->IFCR |= DMA_IFCR_CGIF3; From 9afdca79a2059ef8e72e029c7bfb074fdc9503ae Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 3 Nov 2024 19:42:02 -0800 Subject: [PATCH 151/157] Add while discard loop again, fix warning --- motor-controller/bin/dribbler/main.c | 10 +++++----- motor-controller/bin/wheel/main.c | 10 +++++----- motor-controller/common/uart.c | 12 +++++++++++- 3 files changed, 21 insertions(+), 11 deletions(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 9ef9563c..aa20375b 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -70,11 +70,11 @@ int main() { MotorResponsePacket response_packet; memset(&response_packet, 0, sizeof(MotorResponsePacket)); - response_packet.data.motion.reset_watchdog_independent = rcc_csr & RCC_CSR_IWDGRSTF != 0; - response_packet.data.motion.reset_watchdog_window = rcc_csr & RCC_CSR_WWDGRSTF != 0; - response_packet.data.motion.reset_low_power = rcc_csr & RCC_CSR_LPWRRSTF != 0; - response_packet.data.motion.reset_software = rcc_csr & RCC_CSR_SFTRSTF != 0; - response_packet.data.motion.reset_pin = rcc_csr & RCC_CSR_PINRSTF != 0; + response_packet.data.motion.reset_watchdog_independent = (rcc_csr & RCC_CSR_IWDGRSTF) != 0; + response_packet.data.motion.reset_watchdog_window = (rcc_csr & RCC_CSR_WWDGRSTF) != 0; + response_packet.data.motion.reset_low_power = (rcc_csr & RCC_CSR_LPWRRSTF) != 0; + response_packet.data.motion.reset_software = (rcc_csr & RCC_CSR_SFTRSTF) != 0; + response_packet.data.motion.reset_pin = (rcc_csr & RCC_CSR_PINRSTF) != 0; bool params_return_packet_requested = false; diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 86d9ad95..6fa89532 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -88,11 +88,11 @@ int main() { MotorResponsePacket response_packet; memset(&response_packet, 0, sizeof(MotorResponsePacket)); - response_packet.data.motion.reset_watchdog_independent = rcc_csr & RCC_CSR_IWDGRSTF != 0; - response_packet.data.motion.reset_watchdog_window = rcc_csr & RCC_CSR_WWDGRSTF != 0; - response_packet.data.motion.reset_low_power = rcc_csr & RCC_CSR_LPWRRSTF != 0; - response_packet.data.motion.reset_software = rcc_csr & RCC_CSR_SFTRSTF != 0; - response_packet.data.motion.reset_pin = rcc_csr & RCC_CSR_PINRSTF != 0; + response_packet.data.motion.reset_watchdog_independent = (rcc_csr & RCC_CSR_IWDGRSTF) != 0; + response_packet.data.motion.reset_watchdog_window = (rcc_csr & RCC_CSR_WWDGRSTF) != 0; + response_packet.data.motion.reset_low_power = (rcc_csr & RCC_CSR_LPWRRSTF) != 0; + response_packet.data.motion.reset_software = (rcc_csr & RCC_CSR_SFTRSTF) != 0; + response_packet.data.motion.reset_pin = (rcc_csr & RCC_CSR_PINRSTF) != 0; bool params_return_packet_requested = false; diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 4ad05baa..7054b48b 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -112,7 +112,17 @@ bool uart_can_read() { } void uart_discard() { - ioq_discard_write_back(&uart_rx_queue); + uint8_t discard_runs = 0; + // Try to empty the current queue. If it's empty, we're done. + while (!ioq_is_empty(&uart_rx_queue)) { + ioq_discard_write_back(&uart_rx_queue); + + // Don't want to get caught in an infinite loop, + // so just clear at least the buffer depth. + if (discard_runs++ > IOQ_BUF_DEPTH) { + break; + } + } } bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { From 3e6815a5d5023350d47fa2421d699f4a830c2f68 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sun, 17 Nov 2024 20:08:50 -0800 Subject: [PATCH 152/157] LED and logging fixes --- .../src/bin/hwtest-kicker-coms/main.rs | 3 +- control-board/src/bin/hwtest-kicker/main.rs | 3 +- motor-controller/bin/dribbler/main.c | 175 +++++++++++----- motor-controller/bin/wheel/main.c | 195 ++++++++++++------ motor-controller/common/debug.c | 58 ++++-- motor-controller/common/debug.h | 7 + motor-controller/common/io_queue.c | 9 +- motor-controller/common/uart.c | 33 ++- motor-controller/common/uart.h | 19 +- 9 files changed, 342 insertions(+), 160 deletions(-) diff --git a/control-board/src/bin/hwtest-kicker-coms/main.rs b/control-board/src/bin/hwtest-kicker-coms/main.rs index d4ddb9d9..29df6069 100644 --- a/control-board/src/bin/hwtest-kicker-coms/main.rs +++ b/control-board/src/bin/hwtest-kicker-coms/main.rs @@ -12,7 +12,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use defmt::info; use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::{Level, Output, Speed}, interrupt, pac::Interrupt, usart::Uart + gpio::{Level, Output, Speed, Pull}, interrupt, pac::Interrupt, usart::Uart }; use embassy_time::{Duration, Ticker, Timer}; use panic_probe as _; @@ -94,6 +94,7 @@ async fn main(_spawner: embassy_executor::Spawner) { &KICKER_TX_UART_QUEUE, p.PA8, p.PA9, + Pull::Up, true ); diff --git a/control-board/src/bin/hwtest-kicker/main.rs b/control-board/src/bin/hwtest-kicker/main.rs index 07008057..eb02ce3c 100644 --- a/control-board/src/bin/hwtest-kicker/main.rs +++ b/control-board/src/bin/hwtest-kicker/main.rs @@ -12,7 +12,7 @@ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use defmt::info; use embassy_executor::InterruptExecutor; use embassy_stm32::{ - gpio::{Level, Output, Speed}, interrupt, pac::Interrupt, usart::Uart + gpio::{Level, Output, Speed, Pull}, interrupt, pac::Interrupt, usart::Uart }; use embassy_time::{Duration, Ticker, Timer}; use panic_probe as _; @@ -94,6 +94,7 @@ async fn main(_spawner: embassy_executor::Spawner) { &KICKER_TX_UART_QUEUE, p.PA8, p.PA9, + Pull::Up, true ); diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index aa20375b..b9af604b 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -7,9 +7,9 @@ #include "ateam-common-packets/include/stspin.h" +#include "debug.h" #include "6step.h" #include "current_sensing.h" -#include "debug.h" #include "iir.h" #include "io_queue.h" #include "main.h" @@ -28,11 +28,13 @@ int main() { RCC->CSR |= RCC_CSR_RMVF; // turn off LEDs - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BR_7; + turn_off_red_led(); + turn_off_yellow_led(); + turn_off_green_led(); // turn on Red LED - GPIOB->BSRR |= GPIO_BSRR_BS_6; + turn_on_red_led(); + turn_on_yellow_led(); // Setups clocks setup(); @@ -45,10 +47,6 @@ int main() { while (IWDG->SR) {} // wait for value to take IWDG->KR = 0x0000AAAA; // feed the watchdog - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BR_7; - - GPIOB->BSRR |= GPIO_BSRR_BS_8; uint32_t ticks_since_last_command_packet = 0; bool telemetry_enabled = false; @@ -64,9 +62,8 @@ int main() { // enable ADC hardware trigger (tied to 6step timer) currsen_enable_ht(); - MotorCommandPacket motor_command_packet; - memset(&motor_command_packet, 0, sizeof(MotorCommandPacket)); + // Initialized response_packet here to capture the reset method. MotorResponsePacket response_packet; memset(&response_packet, 0, sizeof(MotorResponsePacket)); @@ -97,56 +94,66 @@ int main() { Pid_t torque_pid; pid_initialize(&torque_pid, &torque_pid_constants); - // turn off Red turn on Green - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BS_7; + // Turn off Red/Yellow LED after booting. + turn_off_red_led(); + turn_off_yellow_led(); + // UART logging status. + uart_logging_status_t uart_logging_status_receive; + uart_logging_status_t uart_logging_status_send; // toggle J1-1 while (true) { IWDG->KR = 0x0000AAAA; // feed the watchdog - // GPIOB->BSRR |= GPIO_BSRR_BR_8; - // GPIOB->BSRR |= GPIO_BSRR_BR_9; #ifdef UART_ENABLED // watchdog on receiving a command packet ticks_since_last_command_packet++; while (uart_can_read()) { + // Make a new packet and clear out before reading + // in the new data. MotorCommandPacket motor_command_packet; - uint16_t uart_bytes_read = 0; - bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); - if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { - // something went wrong, just purge all of the data - uart_discard(); + memset(&motor_command_packet, 0, sizeof(MotorCommandPacket)); - continue; - } + // Read in the new packet data. + uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); // If something goes wrong with the UART, we need to flag it. if (uart_logging_status != UART_LOGGING_OK) { - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_8; + // Capture the status of the UART. + uart_logging_status_receive = uart_logging_status; + + // If something went wrong, just purge all of the data. + uart_discard(); + + // Go through the loop again to get the next packet. + continue; + } else { + // If we are in COMP_MODE, don't latch the status + // and clear it out if we get UART working. + #ifdef COMP_MODE + uart_logging_status_receive = UART_LOGGING_OK; + #endif } - if (motor_command_packet.type == MCP_MOTION) { - // GPIOB->BSRR |= GPIO_BSRR_BS_8; + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); + if (motor_command_packet.type == MCP_MOTION) { // we got a motion packet! ticks_since_last_command_packet = 0; if (motor_command_packet.data.motion.reset) { - // TODO handle hardware reset - - while (true); // block, hardware reset flagged + // Block, watchdog will trigger reset. + while (true); } telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; r_motor_board = motor_command_packet.data.motion.setpoint; + motion_control_type = motor_command_packet.data.motion.motion_control_type; } else if (motor_command_packet.type == MCP_PARAMS) { - // GPIOB->BSRR |= GPIO_BSRR_BS_9; - - // we got some params + // Every time a params packet is received, we echo back the current params state params_return_packet_requested = true; if (motor_command_packet.data.params.update_timestamp) { @@ -177,22 +184,19 @@ int main() { } #endif - // if upstream isn't listening or its been too long since we got a command packet, turn off the motor - if (!telemetry_enabled) { - r_motor_board = 0.0f; - GPIOB->BSRR |= GPIO_BSRR_BR_7; - } else { - GPIOB->BSRR |= GPIO_BSRR_BS_7; - } - - if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + // Coast motor based on failure states. + if (!telemetry_enabled || + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS || + response_packet.data.motion.master_error || + uart_logging_status_receive != UART_LOGGING_OK) { + // If telemetry is disabled, that means the upstream + // isn't ready. + // If we haven't received a command packet in a while, + // that means the upstream isn't ready or locked up. + // If we have a master error, means the motor is in a bad state. + // If we have a UART error, means something happened coming down + // stream from the upstream or something locally went wrong. r_motor_board = 0.0f; - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. - while(true); - } else { - GPIOB->BSRR |= GPIO_BSRR_BR_8; } bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); @@ -206,7 +210,6 @@ int main() { cur_measurement = iir_filter_update(&torque_filter, cur_measurement); - float r = r_motor_board; float torque_setpoint = pid_calculate(&torque_pid, r, cur_measurement, TORQUE_LOOP_RATE_S); @@ -258,7 +261,7 @@ int main() { response_packet.data.motion.torque_limited = false; // loop time - response_packet.data.motion.control_loop_time_error = false; + response_packet.data.motion.control_loop_time_error = slipped_control_frame_count > 10; // timestamp response_packet.data.motion.timestamp = time_local_epoch_s(); @@ -279,14 +282,26 @@ int main() { // transmit packets #ifdef UART_ENABLED - // GPIOB->BSRR |= GPIO_BSRR_BS_8; + // If previous UART transmit is still occurring, + // wait for it to finish. uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (run_telemetry) - { + if (run_telemetry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + // Capture the status for the response packet / LED. + if (uart_logging_status != UART_LOGGING_OK) { + uart_logging_status_send = uart_logging_status; + } else { + // If we are in COMP_MODE, don't latch the status if + // we are able to send a packet successfully later. + #ifdef COMP_MODE + uart_logging_status_send = UART_LOGGING_OK; + #endif + } + + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); } - // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif if (params_return_packet_requested) { @@ -306,18 +321,66 @@ int main() { #ifdef UART_ENABLED uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + // Capture the status for the response packet / LED. + if (uart_logging_status != UART_LOGGING_OK) { + uart_logging_status_send = uart_logging_status; + } else { + // If we are in COMP_MODE, don't latch the status if + // we are able to send a packet successfully later. + #ifdef COMP_MODE + uart_logging_status_send = UART_LOGGING_OK; + #endif + } + + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); #endif } } // limit loop rate to smallest time step if (sync_systick()) { + // Track if we are slipping control frames. slipped_control_frame_count++; } - if (slipped_control_frame_count > 10) { - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_6; + // Set all status LEDs + + // Red LED means we are in an error state. + // This latches and requires resetting the robot to clear. + if (response_packet.data.motion.master_error || + (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + turn_on_red_led(); + } + + // Yellow LED means we are in an warning state. + // Will clear if resolved. + if (uart_logging_status_receive != UART_LOGGING_OK || + uart_logging_status_send != UART_LOGGING_OK) { + turn_on_yellow_led(); + } else { + turn_off_yellow_led(); + } + + // Green LED means we are able to send telemetry upstream. + // This means the upstream sent a packet downstream with telemetry enabled. + if (!telemetry_enabled) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + #ifdef COMP_MODE + if (telemetry_enabled && + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + // If have telemetry enabled (meaning we at one + // point received a message from upstream) and haven't + // received a command packet in a while, we need to reset + // the system when in COMP_MODE. + // This is a safety feature to prevent the robot from + // running away if the upstream controller locks up. + while (true); } + #endif } } diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 6fa89532..806be928 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -16,6 +16,7 @@ #include "ateam-common-packets/include/stspin.h" +#include "debug.h" #include "6step.h" #include "conversions.h" #include "current_sensing.h" @@ -38,14 +39,13 @@ int main() { RCC->CSR |= RCC_CSR_RMVF; // turn off LEDs - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BR_7; - GPIOB->BSRR |= GPIO_BSRR_BR_8; - - // turn on Red/Yl LED - GPIOB->BSRR |= GPIO_BSRR_BS_6; - GPIOB->BSRR |= GPIO_BSRR_BS_8; + turn_off_red_led(); + turn_off_yellow_led(); + turn_off_green_led(); + // turn on Red/Yellow LED + turn_on_red_led(); + turn_on_yellow_led(); // Setups clocks setup(); @@ -76,15 +76,7 @@ int main() { quadenc_setup(); quadenc_reset_encoder_delta(); - // pwm6step_set_duty_cycle_f(0.05f); - - // while (1) { - // IWDG->KR = 0x0000AAAA; // feed the watchdog - // } - - MotorCommandPacket motor_command_packet; - memset(&motor_command_packet, 0, sizeof(MotorCommandPacket)); - + // Initialized response_packet here to capture the reset method. MotorResponsePacket response_packet; memset(&response_packet, 0, sizeof(MotorResponsePacket)); @@ -98,10 +90,10 @@ int main() { // setup the loop rate regulators SyncTimer_t vel_loop_timer; - SyncTimer_t torque_loop_timer; - SyncTimer_t telemetry_timer; time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); + SyncTimer_t torque_loop_timer; time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); + SyncTimer_t telemetry_timer; time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); // setup the velocity filter @@ -161,9 +153,13 @@ int main() { torque_pid_constants.kP = 1.0f; - // turn off Red/Yl - GPIOB->BSRR |= GPIO_BSRR_BR_6; - GPIOB->BSRR |= GPIO_BSRR_BR_8; + // Turn off Red/Yellow LED after booting. + turn_off_red_led(); + turn_off_yellow_led(); + + // UART logging status. + uart_logging_status_t uart_logging_status_receive; + uart_logging_status_t uart_logging_status_send; // toggle J1-1 while (true) { @@ -175,39 +171,52 @@ int main() { // process all available packets while (uart_can_read()) { - uint16_t uart_bytes_read = 0; - bool uart_read_success = uart_read(&motor_command_packet, sizeof(MotorCommandPacket), &uart_bytes_read); - if (!uart_read_success || uart_bytes_read != sizeof(MotorCommandPacket)) { - // something went wrong, just purge all of the data - uart_discard(); + // Make a new packet and clear out before reading + // in the new data. + MotorCommandPacket motor_command_packet; + memset(&motor_command_packet, 0, sizeof(MotorCommandPacket)); - continue; - } + // Read in the new packet data. + uart_read(&motor_command_packet, sizeof(MotorCommandPacket)); // If something goes wrong with the UART, we need to flag it. if (uart_logging_status != UART_LOGGING_OK) { - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_8; + // Capture the status of the UART. + uart_logging_status_receive = uart_logging_status; + + // If something went wrong, just purge all of the data. + uart_discard(); + + // Go through the loop again to get the next packet. + continue; + } else { + // If we are in COMP_MODE, don't latch the status + // and clear it out if we get UART working. + #ifdef COMP_MODE + uart_logging_status_receive = UART_LOGGING_OK; + #endif } - if (motor_command_packet.type == MCP_MOTION) { + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); - // we got a motion packet! + if (motor_command_packet.type == MCP_MOTION) { + // We got a motion packet! ticks_since_last_command_packet = 0; if (motor_command_packet.data.motion.reset) { - // TODO handle hardware reset + // Block, watchdog will trigger reset. - // while (true); // block, hardware reset flagged + while (true); } telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; r_motor_board = motor_command_packet.data.motion.setpoint; motion_control_type = motor_command_packet.data.motion.motion_control_type; } else if (motor_command_packet.type == MCP_PARAMS) { - // a params update is issued (or the upstream just wants to readback current params) + // a params update is issued (or the upstream just wants to read back current params) - // everytime a params packet is received, we echo back the current params state + // Every time a params packet is received, we echo back the current params state params_return_packet_requested = true; if (motor_command_packet.data.params.update_timestamp) { @@ -255,35 +264,25 @@ int main() { } #endif - // if upstream isn't listening or its been too long since we got a command packet, turn off the motor - if (!telemetry_enabled) { + // Coast motor based on failure states. + if (!telemetry_enabled || + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS || + response_packet.data.motion.master_error || + uart_logging_status_receive != UART_LOGGING_OK) { + // If telemetry is disabled, that means the upstream + // isn't ready. + // If we haven't received a command packet in a while, + // that means the upstream isn't ready or locked up. + // If we have a master error, means the motor is in a bad state. + // If we have a UART error, means something happened coming down + // stream from the upstream or something locally went wrong. r_motor_board = 0.0f; - GPIOB->BSRR |= GPIO_BSRR_BR_7; - } else { - GPIOB->BSRR |= GPIO_BSRR_BS_7; - } - - if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { - r_motor_board = 0.0f; - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_8; - // HACK Will force the watchdog to trigger. - while(true); - } else { - GPIOB->BSRR |= GPIO_BSRR_BR_8; - } - - // if any critical error is latched, coast the motor - if (response_packet.data.motion.master_error) { - r_motor_board = 0.0f; - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_7; } // determine which loops need to be run bool run_torque_loop = time_sync_ready_rst(&torque_loop_timer); bool run_vel_loop = time_sync_ready_rst(&vel_loop_timer); - bool run_telemtry = time_sync_ready_rst(&telemetry_timer); + bool run_telemetry = time_sync_ready_rst(&telemetry_timer); // run the torque loop if applicable if (run_torque_loop) { @@ -345,7 +344,7 @@ int main() { // filter the recovered velocity enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); - // compute the velcoity PID + // compute the velocity PID control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); // Clamp setpoint acceleration @@ -371,7 +370,6 @@ int main() { response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; } - if (run_torque_loop || run_vel_loop) { // detect if the encoder is not pulling the detect pin down bool encoder_disconnected = (GPIOA->IDR & GPIO_IDR_5) != 0; @@ -418,7 +416,7 @@ int main() { response_packet.data.motion.torque_limited = false; // loop time - response_packet.data.motion.control_loop_time_error = false; + response_packet.data.motion.control_loop_time_error = slipped_control_frame_count > 10; // master error response_packet.data.motion.master_error = response_packet.data.motion.hall_power_error @@ -439,13 +437,26 @@ int main() { // transmit packets #ifdef UART_ENABLED - // GPIOB->BSRR |= GPIO_BSRR_BS_8; + // If previous UART transmit is still occurring, + // wait for it to finish. uart_wait_for_transmission(); // takes ~270uS, mostly hardware DMA - if (run_telemtry) { + if (run_telemetry) { uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + // Capture the status for the response packet / LED. + if (uart_logging_status != UART_LOGGING_OK) { + uart_logging_status_send = uart_logging_status; + } else { + // If we are in COMP_MODE, don't latch the status if + // we are able to send a packet successfully later. + #ifdef COMP_MODE + uart_logging_status_send = UART_LOGGING_OK; + #endif + } + + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); } - // GPIOB->BSRR |= GPIO_BSRR_BR_8; #endif if (params_return_packet_requested) { @@ -470,18 +481,66 @@ int main() { #ifdef UART_ENABLED uart_transmit((uint8_t *) &response_packet, sizeof(MotorResponsePacket)); + // Capture the status for the response packet / LED. + if (uart_logging_status != UART_LOGGING_OK) { + uart_logging_status_send = uart_logging_status; + } else { + // If we are in COMP_MODE, don't latch the status if + // we are able to send a packet successfully later. + #ifdef COMP_MODE + uart_logging_status_send = UART_LOGGING_OK; + #endif + } + + // Clear the logging for the next UART transmit / receive. + uart_clear_logging_status(); #endif } } // limit loop rate to smallest time step if (sync_systick()) { + // Track if we are slipping control frames. slipped_control_frame_count++; } - if (slipped_control_frame_count > 10) { - // Error pin enable. - GPIOB->BSRR |= GPIO_BSRR_BS_6; + // Set all status LEDs + + // Red LED means we are in an error state. + // This latches and requires resetting the robot to clear. + if (response_packet.data.motion.master_error || + (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + turn_on_red_led(); + } + + // Yellow LED means we are in an warning state. + // Will clear if resolved. + if (uart_logging_status_receive != UART_LOGGING_OK || + uart_logging_status_send != UART_LOGGING_OK) { + turn_on_yellow_led(); + } else { + turn_off_yellow_led(); + } + + // Green LED means we are able to send telemetry upstream. + // This means the upstream sent a packet downstream with telemetry enabled. + if (!telemetry_enabled) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + #ifdef COMP_MODE + if (telemetry_enabled && + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + // If have telemetry enabled (meaning we at one + // point received a message from upstream) and haven't + // received a command packet in a while, we need to reset + // the system when in COMP_MODE. + // This is a safety feature to prevent the robot from + // running away if the upstream controller locks up. + while (true); } + #endif } } diff --git a/motor-controller/common/debug.c b/motor-controller/common/debug.c index f980d883..c6f93c70 100644 --- a/motor-controller/common/debug.c +++ b/motor-controller/common/debug.c @@ -1,20 +1,44 @@ -#include -#include "time.h" #include "debug.h" +#include "time.h" +#include -void _debug_value_manchester_8(uint8_t val) { +void turn_on_red_led() { + GPIOB->BSRR |= GPIO_BSRR_BS_6; +} + +void turn_off_red_led() { + GPIOB->BSRR |= GPIO_BSRR_BR_6; +} + +void turn_on_yellow_led() { + GPIOB->BSRR |= GPIO_BSRR_BS_8; +} + +void turn_off_yellow_led() { GPIOB->BSRR |= GPIO_BSRR_BR_8; +} + +void turn_on_green_led() { + GPIOB->BSRR |= GPIO_BSRR_BS_7; +} + +void turn_off_green_led() { + GPIOB->BSRR |= GPIO_BSRR_BR_7; +} + +void _debug_value_manchester_8(uint8_t val) { + turn_off_yellow_led(); wait_ms(1); for (int i = 8; i >= 0; i--) { - GPIOB->BSRR |= GPIO_BSRR_BS_8; + turn_on_yellow_led(); wait_ms(1); - if ((val >> i) & 0x1 != 0) { + if (((val >> i) & 0x1) != 0) { wait_ms(1); - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); } else { - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); wait_ms(1); } @@ -23,17 +47,17 @@ void _debug_value_manchester_8(uint8_t val) { } void _debug_value_manchester(uint16_t val) { - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); wait_ms(1); for (int i = 15; i >= 0; i--) { - GPIOB->BSRR |= GPIO_BSRR_BS_8; + turn_on_yellow_led(); wait_ms(1); - if ((val >> i) & 0x1 != 0) { + if (((val >> i) & 0x1) != 0) { wait_ms(1); - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); } else { - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); wait_ms(1); } @@ -42,17 +66,17 @@ void _debug_value_manchester(uint16_t val) { } void _debug_value_manchester_32(int32_t val) { - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); wait_ms(1); for (int i = 31; i >= 0; i--) { - GPIOB->BSRR |= GPIO_BSRR_BS_8; + turn_on_yellow_led(); wait_ms(1); - if ((val >> i) & 0x1 != 0) { + if (((val >> i) & 0x1) != 0) { wait_ms(1); - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); } else { - GPIOB->BSRR |= GPIO_BSRR_BR_8; + turn_off_yellow_led(); wait_ms(1); } diff --git a/motor-controller/common/debug.h b/motor-controller/common/debug.h index 9d324f30..a9db2567 100644 --- a/motor-controller/common/debug.h +++ b/motor-controller/common/debug.h @@ -2,6 +2,13 @@ #include +void turn_on_red_led(); +void turn_off_red_led(); +void turn_on_yellow_led(); +void turn_off_yellow_led(); +void turn_on_green_led(); +void turn_off_green_led(); + void _debug_value_manchester_8(uint8_t val); void _debug_value_manchester(uint16_t val); void _debug_value_manchester_32(int32_t val); \ No newline at end of file diff --git a/motor-controller/common/io_queue.c b/motor-controller/common/io_queue.c index 977000d6..2b2ecce3 100644 --- a/motor-controller/common/io_queue.c +++ b/motor-controller/common/io_queue.c @@ -110,14 +110,18 @@ bool ioq_finalize_peek_write(IoQueue_t *q) { // READING // /////////////// -bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) { +bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_to_read) { // Can't read an empty queue. if (ioq_is_empty(q)) { return false; } uint16_t cpy_num_bytes = q->backing_sto[q->read_ind].len; - // Intended read size is smaller than intended. + // Updates the number of bytes to read so we can tell + // that we didn't have length to read the full packet. + *num_bytes_to_read = cpy_num_bytes; + + // Intended read size is smaller than storage contents. if (len < cpy_num_bytes) { return false; } @@ -130,7 +134,6 @@ bool ioq_read(IoQueue_t *q, void *dest, uint16_t len, uint16_t* num_bytes_read) q->size--; __enable_irq(); - *num_bytes_read = cpy_num_bytes; return true; } diff --git a/motor-controller/common/uart.c b/motor-controller/common/uart.c index 7054b48b..56648908 100644 --- a/motor-controller/common/uart.c +++ b/motor-controller/common/uart.c @@ -63,10 +63,14 @@ void uart_wait_for_transmission() { while (uart_is_transmit_dma_pending()); } -bool uart_transmit(uint8_t *data_buf, uint16_t len) { +void uart_transmit(uint8_t *data_buf, uint16_t len) { if (!ioq_write(&uart_tx_queue, data_buf, len)) { - // Queue is either full or the data length is invalid. - return false; + // Queue is either full or the length is too long. + uart_logging_status = UART_LOGGING_UART_TX_BUFFER_FULL; + if (len > IOQ_BUF_LENGTH) { + uart_logging_status = UART_LOGGING_UART_TX_SIZE_MISMATCH; + } + return; } // dma transmission isn't in progress to keep scheduling dma writes @@ -74,8 +78,6 @@ bool uart_transmit(uint8_t *data_buf, uint16_t len) { if (!uart_is_transmit_dma_pending()) { _uart_start_transmit_dma(); } - - return true; } void _uart_start_transmit_dma() { @@ -125,8 +127,21 @@ void uart_discard() { } } -bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read) { - return ioq_read(&uart_rx_queue, dest, len, num_bytes_read); +void uart_clear_logging_status() { + uart_logging_status = UART_LOGGING_OK; +} + +void uart_read(void *dest, uint16_t len) { + uint16_t num_bytes_to_read = 0; + if (!ioq_read(&uart_rx_queue, dest, len, &num_bytes_to_read)) { + // Can fail from empty queue or size mismatch. + uart_logging_status = UART_LOGGING_UART_RX_BUFFER_EMPTY; + // If the size number of bytes to read is not zero and + // not matching, then we have a size mismatch. + if (num_bytes_to_read != 0 && len != num_bytes_to_read) { + uart_logging_status = UART_LOGGING_UART_RX_SIZE_MISMATCH; + } + } } void _uart_start_receive_dma() { @@ -166,6 +181,10 @@ void _uart_receive_dma(bool parity_error) { if (!ioq_finalize_peek_write(&uart_rx_queue)) { uart_logging_status = UART_LOGGING_UART_RX_BUFFER_FULL; } + } else { + // If we have a parity error, we can just overwrite the queue + // with the next packet. + uart_logging_status = UART_LOGGING_UART_RX_PARITY_ERROR; } // Get the NEXT buffer for the DMA to write into. diff --git a/motor-controller/common/uart.h b/motor-controller/common/uart.h index 4cd59d8d..e8c5411a 100644 --- a/motor-controller/common/uart.h +++ b/motor-controller/common/uart.h @@ -13,10 +13,15 @@ typedef enum { UART_LOGGING_OK = 0, - UART_LOGGING_DMA_TX_ERROR, - UART_LOGGING_DMA_RX_ERROR, - UART_LOGGING_DMA_RX_BUFFER_FULL, - UART_LOGGING_UART_RX_BUFFER_FULL + UART_LOGGING_DMA_TX_ERROR = 1, + UART_LOGGING_DMA_RX_ERROR = 2, + UART_LOGGING_DMA_RX_BUFFER_FULL = 3, + UART_LOGGING_UART_RX_BUFFER_FULL = 4, + UART_LOGGING_UART_RX_PARITY_ERROR = 5, + UART_LOGGING_UART_RX_BUFFER_EMPTY = 6, + UART_LOGGING_UART_RX_SIZE_MISMATCH = 7, + UART_LOGGING_UART_TX_BUFFER_FULL = 8, + UART_LOGGING_UART_TX_SIZE_MISMATCH = 9 } uart_logging_status_t; //////////////////////// @@ -25,12 +30,12 @@ typedef enum { void uart_initialize(); -bool uart_transmit_dma_pending(); void uart_wait_for_transmission(); -bool uart_transmit(uint8_t *data_buf, uint16_t len); +void uart_transmit(uint8_t *data_buf, uint16_t len); bool uart_can_read(); void uart_discard(); -bool uart_read(void *dest, uint16_t len, uint16_t* num_bytes_read); +void uart_read(void *dest, uint16_t len); static volatile uart_logging_status_t uart_logging_status; +void uart_clear_logging_status(); \ No newline at end of file From 76457c33011c4c8f791734807e24882c63da8f89 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 19 Nov 2024 20:44:55 -0500 Subject: [PATCH 153/157] add accel data to imu, update control params --- control-board/src/bin/control/main.rs | 3 ++- control-board/src/bin/hwtest-imu/main.rs | 2 +- .../motion/params/body_vel_filter_params.rs | 16 ++++++------ .../motion/params/robot_physical_params.rs | 2 +- control-board/src/tasks/control_task.rs | 17 +++++++++++- control-board/src/tasks/imu_task.rs | 4 +-- lib-stm32/src/drivers/imu/bmi323.rs | 26 +++++++++---------- 7 files changed, 42 insertions(+), 28 deletions(-) diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs index 52ca7f7b..08938178 100644 --- a/control-board/src/bin/control/main.rs +++ b/control-board/src/bin/control/main.rs @@ -96,6 +96,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); /////////////////// // start tasks // @@ -129,7 +130,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { start_control_task( uart_queue_spawner, main_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, control_accel_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, diff --git a/control-board/src/bin/hwtest-imu/main.rs b/control-board/src/bin/hwtest-imu/main.rs index 0fabea2d..4f6d1912 100644 --- a/control-board/src/bin/hwtest-imu/main.rs +++ b/control-board/src/bin/hwtest-imu/main.rs @@ -89,7 +89,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { defmt::warn!("publishing accel data lagged by {}", amnt); } WaitResult::Message(msg) => { - defmt::info!("got accel data (x: {}, y: {}, z: {})", msg[1], msg[1], msg[2]); + defmt::info!("got accel data (x: {}, y: {}, z: {})", msg[0], msg[1], msg[2]); } } diff --git a/control-board/src/motion/params/body_vel_filter_params.rs b/control-board/src/motion/params/body_vel_filter_params.rs index c0daedd8..1ad16de5 100644 --- a/control-board/src/motion/params/body_vel_filter_params.rs +++ b/control-board/src/motion/params/body_vel_filter_params.rs @@ -25,11 +25,11 @@ pub static CONTROL_INPUT: Matrix3x4 = 0.0, 0.0, 0.0, 0.0]; pub static OBSERVATION_MODEL: Matrix5x3 = - matrix![-20.2429, 35.0618, 3.29555; - -28.6278, -28.6278, 3.29555; - 28.6278, -28.6278, 3.29555; - 20.2429, 35.0618, 3.29555; - 0.0, 0.0, 1.0]; + matrix![-28.86751346, 16.66666667, 2.71333333; + -23.57022604, -23.57022604, 2.71333333; + 23.57022604, -23.57022604, 2.71333333; + 28.86751346, 16.66666667, 2.71333333; + 0.0, 0.0, 1.0 ]; pub static PROCESS_COV: Matrix3 = matrix![KF_NUM_STATES as f32 * PROCESS_NOISE / EXPECTED_DT_2, 0.0, 0.0; @@ -49,6 +49,6 @@ pub static INIT_ESTIMATE_COV: Matrix3 = 0.0, 0.0, INITIAL_COV]; pub static KALMAN_GAIN: Matrix3x5 = - matrix![-0.00823069, -0.0116399, 0.0116399, 0.00823069, 0.0; - 0.00786165, -0.00783362, -0.00783362, 0.00786165, 0.0; - 0.02092594, 0.02562894, 0.02562894, 0.02092594, 0.6931516]; + matrix![-1.03923044e-02, -8.48528101e-03, 8.48528101e-03, 1.03923044e-02, -8.82095698e-11; + 1.05519794e-02, -1.37518257e-02, -1.37518257e-02, 1.05519794e-02, 1.73644781e-02; + 2.45564740e-02, 1.73640497e-02, 1.73640497e-02, 2.45564740e-02, 7.72510348e-01 ]; \ No newline at end of file diff --git a/control-board/src/motion/params/robot_physical_params.rs b/control-board/src/motion/params/robot_physical_params.rs index 91bfe07a..0b887525 100644 --- a/control-board/src/motion/params/robot_physical_params.rs +++ b/control-board/src/motion/params/robot_physical_params.rs @@ -1,4 +1,4 @@ use nalgebra::Vector4; pub const WHEEL_ANGLES_DEG: Vector4 = Vector4::new(330.0, 45.0, 135.0, 210.0); // Degrees from y+ -pub const WHEEL_RADIUS_M: f32 = 0.0247; // wheel dia 49mm +pub const WHEEL_RADIUS_M: f32 = 0.030; // was 0.0247 // wheel dia 49mm pub const WHEEL_DISTANCE_TO_ROBOT_CENTER_M: f32 = 0.0814; // from center of wheel body to center of robot \ No newline at end of file diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index aab803dd..7ec8291e 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -60,7 +60,10 @@ pub struct ControlTask< battery_subscriber: BatteryVoltSubscriber, last_battery_v: f32, gyro_subscriber: GyroDataSubscriber, + accel_subscriber: AccelDataSubscriber, last_gyro_rads: f32, + last_accel_x_ms: f32, + last_accel_y_ms: f32, telemetry_publisher: TelemetryPublisher, motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -83,6 +86,7 @@ impl < telemetry_publisher: TelemetryPublisher, battery_subscriber: BatteryVoltSubscriber, gyro_subscriber: GyroDataSubscriber, + accel_subscriber: AccelDataSubscriber, motor_fl: WheelMotor<'static, MotorFLUart, MotorFLDmaRx, MotorFLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, motor_bl: WheelMotor<'static, MotorBLUart, MotorBLDmaRx, MotorBLDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, motor_br: WheelMotor<'static, MotorBRUart, MotorBRDmaRx, MotorBRDmaTx, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>, @@ -96,7 +100,10 @@ impl < battery_subscriber: battery_subscriber, last_battery_v: 0.0, gyro_subscriber: gyro_subscriber, + accel_subscriber: accel_subscriber, last_gyro_rads: 0.0, + last_accel_x_ms: 0.0, + last_accel_y_ms: 0.0, motor_fl: motor_fl, motor_bl: motor_bl, motor_br: motor_br, @@ -185,6 +192,8 @@ impl < control_debug_telem.motor_bl.wheel_torque = self.motor_bl.read_hall_vel(); control_debug_telem.motor_br.wheel_torque = self.motor_br.read_hall_vel(); control_debug_telem.motor_fr.wheel_torque = self.motor_fr.read_hall_vel(); + control_debug_telem.imu_accel[0] = self.last_accel_x_ms; + control_debug_telem.imu_accel[1] = self.last_accel_y_ms; let control_debug_telem = TelemetryPacket::Control(control_debug_telem); self.telemetry_publisher.publish_immediate(control_debug_telem); @@ -295,6 +304,11 @@ impl < while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { self.last_gyro_rads = gyro_rads[2]; } + + while let Some(accel_ms) = self.accel_subscriber.try_next_message_pure() { + self.last_accel_x_ms = accel_ms[0]; + self.last_accel_y_ms = accel_ms[1]; + } let controls_enabled = true; @@ -437,6 +451,7 @@ pub async fn start_control_task( telemetry_publisher: TelemetryPublisher, battery_subscriber: BatteryVoltSubscriber, gyro_subscriber: GyroDataSubscriber, + accel_subscriber: AccelDataSubscriber, motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin, motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin, motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin, @@ -484,7 +499,7 @@ pub async fn start_control_task( let control_task = ControlTask::new( robot_state, command_subscriber, telemetry_publisher, battery_subscriber, - gyro_subscriber, motor_fl, motor_bl, motor_br, motor_fr, motor_drib); + gyro_subscriber, accel_subscriber, motor_fl, motor_bl, motor_br, motor_fr, motor_drib); control_task_spawner.spawn(control_task_entry(control_task)).unwrap(); } \ No newline at end of file diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 41af3611..cdf8c4d5 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/control-board/src/tasks/imu_task.rs @@ -108,11 +108,11 @@ async fn imu_task_entry( // read accel data // TODO: don't use raw data, impl conversion - let accel_data = imu.accel_get_raw_data().await; + let accel_data = imu.accel_get_data_mps().await; accel_pub.publish_immediate(Vector3::new(accel_data[0] as f32, accel_data[1] as f32, accel_data[2] as f32)); // TODO: magic number, fix after raw data conversion - if accel_data[2] < 8000 { + if accel_data[2] < 4.0 { if !first_tipped_seen { // If it's the first time a tipping occured, start tracking. first_tipped_seen = true; diff --git a/lib-stm32/src/drivers/imu/bmi323.rs b/lib-stm32/src/drivers/imu/bmi323.rs index e910d9bc..07da49ce 100644 --- a/lib-stm32/src/drivers/imu/bmi323.rs +++ b/lib-stm32/src/drivers/imu/bmi323.rs @@ -423,31 +423,29 @@ impl<'a, 'buf> [buf[0] as i16, buf[1] as i16, buf[2] as i16] } - pub fn accel_range_mg(&self) -> f32 { + pub fn accel_range_scale(&self) -> f32 { match self.accel_range { - AccelRange::Range2g => 2000.0, - AccelRange::Range4g => 4000.0, - AccelRange::Range8g => 8000.0, - AccelRange::Range16g => 16000.0, + AccelRange::Range2g => 2.0, + AccelRange::Range4g => 4.0, + AccelRange::Range8g => 8.0, + AccelRange::Range16g => 16.0, } } - pub fn convert_accel_raw_sample_mg(&self, raw_sample: i16) -> f32 { - let conversion_num = self.accel_range_mg(); - - raw_sample as f32 * (conversion_num / i16::MAX as f32) + pub fn convert_accel_raw_sample_g(&self, raw_sample: i16) -> f32 { + (raw_sample as f32 / i16::MAX as f32) * self.accel_range_scale() } pub fn convert_accel_raw_sample_mps(&self, raw_sample: i16) -> f32 { - self.convert_accel_raw_sample_mg(raw_sample) / 1000.0 * 9.80665 + self.convert_accel_raw_sample_g(raw_sample) * 9.80665 } - pub async fn accel_get_data_mg(&mut self) -> [f32; 3] { + pub async fn accel_get_data_g(&mut self) -> [f32; 3] { let raw_data = self.accel_get_raw_data().await; - return [self.convert_accel_raw_sample_mg(raw_data[0]), - self.convert_accel_raw_sample_mg(raw_data[1]), - self.convert_accel_raw_sample_mg(raw_data[2])] + return [self.convert_accel_raw_sample_g(raw_data[0]), + self.convert_accel_raw_sample_g(raw_data[1]), + self.convert_accel_raw_sample_g(raw_data[2])] } pub async fn accel_get_data_mps(&mut self) -> [f32; 3] { From f1ed19baeca233ae1f57790244ad19d5161d80e5 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 19 Nov 2024 21:11:16 -0500 Subject: [PATCH 154/157] fix task start --- control-board/src/bin/hwtest-control/main.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control-board/src/bin/hwtest-control/main.rs b/control-board/src/bin/hwtest-control/main.rs index 6f75cf65..ddb90f65 100644 --- a/control-board/src/bin/hwtest-control/main.rs +++ b/control-board/src/bin/hwtest-control/main.rs @@ -99,6 +99,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); + /////////////////// // start tasks // @@ -132,7 +134,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { start_control_task( uart_queue_spawner, main_spawner, robot_state, - control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, control_accel_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, From 800efa2e48f7fc2cf7f8d5eb07cd85a90e52cae1 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 19 Nov 2024 19:30:13 -0800 Subject: [PATCH 155/157] Fix udev and update alternatives --- util/udev_setup.bash | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/util/udev_setup.bash b/util/udev_setup.bash index e44125b5..af87db32 100755 --- a/util/udev_setup.bash +++ b/util/udev_setup.bash @@ -12,11 +12,11 @@ SCRIPT_DIR=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd) # check if we're in WSL, this will complete some extra steps for usb passthrough if grep -qi microsoft /proc/version; then echo "DETECTED PLATFORM - WSL" - echo "WSL - setup USB passthrough" apt update apt install -y linux-tools-generic hwdata - update-alternatives --install /usr/local/bin/usbip usbip /usr/lib/linux-tools/*-generic/usbip 20 + LINUX_TOOLS_VERSION=$(apt list --installed 2>/dev/null | grep -oP 'linux-tools-\K[0-9]+\.[0-9]+\.[0-9]+-[0-9]+' | sort -V | tail -n 1) + update-alternatives --install /usr/local/bin/usbip usbip /usr/lib/linux-tools/${LINUX_TOOLS_VERSION}-generic/usbip 20 echo "WSL - platform specific setup complete." elif grep -q "Ubuntu" /etc/lsb-release; then echo "DETECTED PLATFORM - Native Linux" @@ -63,11 +63,7 @@ for udev_rule_file in $SCRIPT_DIR/udev/*.rules; do fi done -if [ $any_rules_file_installed = true ]; then - echo "One or more rules file was installed. Reloading udevadm..." - udevadm control --reload - udevadm trigger - echo "Done." -else - echo "No rules files were installed. No further actions to complete." -fi +service udev restart +udevadm control --reload +udevadm trigger +echo "Done." From 02eb0491b697deff7e78851aea91353b6f4a2472 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 3 Dec 2024 22:04:22 -0500 Subject: [PATCH 156/157] update packet structure for torque data upload --- .vscode/extensions.json | 3 +- control-board/src/drivers/kicker.rs | 2 +- control-board/src/drivers/radio_robot.rs | 26 ++++---- control-board/src/motion/robot_controller.rs | 70 +++++--------------- control-board/src/parameter_interface.rs | 4 +- control-board/src/stspin_motor.rs | 14 ++-- control-board/src/tasks/control_task.rs | 12 ++-- control-board/src/tasks/radio_task.rs | 4 +- kicker-board/src/bin/hwtest-coms/main.rs | 2 +- kicker-board/src/bin/kicker/main.rs | 2 +- lib-stm32/build.rs | 4 +- motor-controller/bin/dribbler/main.c | 10 ++- motor-controller/bin/wheel/main.c | 16 ++--- software-communication | 2 +- 14 files changed, 69 insertions(+), 102 deletions(-) diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 2da88104..133a4e76 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -6,6 +6,7 @@ "github.vscode-github-actions", "rust-lang.rust-analyzer", "ms-vscode.cpptools-extension-pack", - "tamasfe.even-better-toml" + "tamasfe.even-better-toml", + "fill-labs.dependi" ] } \ No newline at end of file diff --git a/control-board/src/drivers/kicker.rs b/control-board/src/drivers/kicker.rs index 4c75e872..90374d20 100644 --- a/control-board/src/drivers/kicker.rs +++ b/control-board/src/drivers/kicker.rs @@ -4,7 +4,7 @@ use embassy_time::{Duration, Timer}; use crate::stm32_interface::Stm32Interface; -use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; +use ateam_common_packets::bindings::{KickerControl, KickerTelemetry, KickRequest}; pub struct Kicker< 'a, diff --git a/control-board/src/drivers/radio_robot.rs b/control-board/src/drivers/radio_robot.rs index c33fa15d..6be583bd 100644 --- a/control-board/src/drivers/radio_robot.rs +++ b/control-board/src/drivers/radio_robot.rs @@ -1,6 +1,6 @@ use ateam_lib_stm32::drivers::radio::odin_w26x::{PeerConnection, OdinW262, WifiAuth}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; -use ateam_common_packets::bindings_radio::{ +use ateam_common_packets::bindings::{ self, BasicControl, CommandCode, HelloRequest, HelloResponse, RadioPacket, RadioPacket_Data, BasicTelemetry, ControlDebugTelemetry, ParameterCommand, }; use ateam_common_packets::radio::DataPacket; @@ -356,8 +356,8 @@ impl< pub async fn send_ack(&self, nack: bool) -> Result<(), ()> { let packet = RadioPacket { crc32: 0, - major_version: bindings_radio::kProtocolVersionMajor, - minor_version: bindings_radio::kProtocolVersionMinor, + major_version: bindings::kProtocolVersionMajor, + minor_version: bindings::kProtocolVersionMinor, command_code: if nack { CommandCode::CC_NACK } else { @@ -399,16 +399,16 @@ impl< pub async fn send_hello(&self, id: u8, team: TeamColor) -> Result<(), ()> { let packet = RadioPacket { crc32: 0, - major_version: bindings_radio::kProtocolVersionMajor, - minor_version: bindings_radio::kProtocolVersionMinor, + major_version: bindings::kProtocolVersionMajor, + minor_version: bindings::kProtocolVersionMinor, command_code: CommandCode::CC_HELLO_REQ, data_length: size_of::() as u16, data: RadioPacket_Data { hello_request: HelloRequest { robot_id: id, color: match team { - TeamColor::Yellow => bindings_radio::TeamColor::TC_YELLOW, - TeamColor::Blue => bindings_radio::TeamColor::TC_BLUE, + TeamColor::Yellow => bindings::TeamColor::TC_YELLOW, + TeamColor::Blue => bindings::TeamColor::TC_BLUE, }, }, }, @@ -428,8 +428,8 @@ impl< pub async fn send_telemetry(&self, telemetry: BasicTelemetry) -> Result<(), ()> { let packet = RadioPacket { crc32: 0, - major_version: bindings_radio::kProtocolVersionMajor, - minor_version: bindings_radio::kProtocolVersionMinor, + major_version: bindings::kProtocolVersionMajor, + minor_version: bindings::kProtocolVersionMinor, command_code: CommandCode::CC_TELEMETRY, data_length: size_of::() as u16, data: RadioPacket_Data { @@ -451,8 +451,8 @@ impl< pub async fn send_control_debug_telemetry(&self, telemetry: ControlDebugTelemetry) -> Result<(), ()> { let packet = RadioPacket { crc32: 0, - major_version: bindings_radio::kProtocolVersionMajor, - minor_version: bindings_radio::kProtocolVersionMinor, + major_version: bindings::kProtocolVersionMajor, + minor_version: bindings::kProtocolVersionMinor, command_code: CommandCode::CC_CONTROL_DEBUG_TELEMETRY, data_length: size_of::() as u16, data: RadioPacket_Data { @@ -474,8 +474,8 @@ impl< pub async fn send_parameter_response(&self, parameter_cmd: ParameterCommand) -> Result<(), ()> { let packet = RadioPacket { crc32: 0, - major_version: bindings_radio::kProtocolVersionMajor, - minor_version: bindings_radio::kProtocolVersionMinor, + major_version: bindings::kProtocolVersionMajor, + minor_version: bindings::kProtocolVersionMinor, command_code: CommandCode::CC_ROBOT_PARAMETER_COMMAND, data_length: size_of::() as u16, data: RadioPacket_Data { diff --git a/control-board/src/motion/robot_controller.rs b/control-board/src/motion/robot_controller.rs index dba6f7d7..2c11ced2 100644 --- a/control-board/src/motion/robot_controller.rs +++ b/control-board/src/motion/robot_controller.rs @@ -1,6 +1,9 @@ -use ateam_common_packets::bindings_radio::{ParameterCommandCode::*, ParameterName}; -use ateam_common_packets::bindings_radio::ParameterDataFormat::{PID_LIMITED_INTEGRAL_F32, VEC3_F32, VEC4_F32}; -use ateam_common_packets::bindings_radio::ParameterName::{VEL_PID_X, RC_BODY_VEL_LIMIT, RC_BODY_ACC_LIMIT, VEL_PID_Y, ANGULAR_VEL_PID_Z, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_PROCESS_NOISE, VEL_CGKF_GYRO_NOISE, VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_K_MATRIX, RC_WHEEL_ACC_LIMIT}; +use ateam_common_packets::bindings::{ + ParameterCommandCode::*, + ParameterName, + ParameterDataFormat::{PID_LIMITED_INTEGRAL_F32, VEC3_F32, VEC4_F32}, + ParameterName::{VEL_PID_X, RC_BODY_VEL_LIMIT, RC_BODY_ACC_LIMIT, VEL_PID_Y, ANGULAR_VEL_PID_Z, VEL_CGKF_ENCODER_NOISE, VEL_CGKF_PROCESS_NOISE, VEL_CGKF_GYRO_NOISE, VEL_CGFK_INITIAL_COVARIANCE, VEL_CGKF_K_MATRIX, RC_WHEEL_ACC_LIMIT}}; +use ateam_common_packets::radio::get_motor_response_motion_packet; use nalgebra::{SVector, Vector3, Vector4, Vector5}; use crate::parameter_interface::ParameterInterface; @@ -18,9 +21,9 @@ use super::params::body_vel_pid_params::{ BODY_VEL_LIM, BODY_ACC_LIM, BODY_DEACC_LIM, WHEEL_ACC_LIM }; -use ateam_common_packets::bindings_radio::{ +use ateam_common_packets::bindings::{ ControlDebugTelemetry, - MotorDebugTelemetry, ParameterCommand + ParameterCommand }; // TODO find some general numeric type trait(s) for D @@ -66,26 +69,10 @@ impl<'a> BodyVelocityController<'a> { prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), debug_telemetry: ControlDebugTelemetry { - motor_fl: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_bl: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_br: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_fr: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, + motor_fl: get_motor_response_motion_packet(), + motor_bl: get_motor_response_motion_packet(), + motor_br: get_motor_response_motion_packet(), + motor_fr: get_motor_response_motion_packet(), imu_gyro: [0.0, 0.0, 0.0], imu_accel: [0.0, 0.0, 0.0], commanded_body_velocity: [0.0, 0.0, 0.0], @@ -126,26 +113,10 @@ impl<'a> BodyVelocityController<'a> { prev_output: Vector3::zeros(), cmd_wheel_velocities: Vector4::zeros(), debug_telemetry: ControlDebugTelemetry { - motor_fl: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_bl: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_br: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, - motor_fr: MotorDebugTelemetry { - wheel_setpoint: 0.0, - wheel_velocity: 0.0, - wheel_torque: 0.0 - }, + motor_fl: get_motor_response_motion_packet(), + motor_bl: get_motor_response_motion_packet(), + motor_br: get_motor_response_motion_packet(), + motor_fr: get_motor_response_motion_packet(), imu_gyro: [0.0, 0.0, 0.0], imu_accel: [0.0, 0.0, 0.0], commanded_body_velocity: [0.0, 0.0, 0.0], @@ -162,14 +133,7 @@ impl<'a> BodyVelocityController<'a> { // Assign telemetry data // TODO pass all of the gyro data up, not just theta self.debug_telemetry.imu_gyro[2] = gyro_theta; - self.debug_telemetry.motor_fl.wheel_torque = wheel_torques[0]; - self.debug_telemetry.motor_bl.wheel_torque = wheel_torques[1]; - self.debug_telemetry.motor_br.wheel_torque = wheel_torques[2]; - self.debug_telemetry.motor_fr.wheel_torque = wheel_torques[3]; - self.debug_telemetry.motor_fl.wheel_velocity = wheel_velocities[0]; - self.debug_telemetry.motor_bl.wheel_velocity = wheel_velocities[1]; - self.debug_telemetry.motor_br.wheel_velocity = wheel_velocities[2]; - self.debug_telemetry.motor_fr.wheel_velocity = wheel_velocities[3]; + self.debug_telemetry.commanded_body_velocity.copy_from_slice(body_vel_setpoint.as_slice()); let measurement: Vector5 = Vector5::new(wheel_velocities[0], wheel_velocities[1], wheel_velocities[2], wheel_velocities[3], gyro_theta); diff --git a/control-board/src/parameter_interface.rs b/control-board/src/parameter_interface.rs index 8fd86fdf..132ed867 100644 --- a/control-board/src/parameter_interface.rs +++ b/control-board/src/parameter_interface.rs @@ -1,5 +1,5 @@ -use ateam_common_packets::bindings_radio::ParameterCommand; -use ateam_common_packets::bindings_radio::ParameterName; +use ateam_common_packets::bindings::ParameterCommand; +use ateam_common_packets::bindings::ParameterName; pub trait ParameterInterface { fn processes_cmd(&self, param_cmd: &ParameterCommand) -> bool; diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index f8104c4d..526e52f8 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -9,7 +9,7 @@ use embassy_stm32::{ use embassy_time::{Duration, Timer}; use nalgebra::Vector3; -use ateam_common_packets::bindings_stspin::{ +use ateam_common_packets::bindings::{ MotorCommandPacket, MotorCommandPacketType::MCP_MOTION, MotorCommand_MotionType, @@ -288,6 +288,10 @@ impl< self.reset_flagged = false; } + pub fn get_latest_state(&self) -> MotorResponse_Motion_Packet { + self.current_state + } + pub fn set_motion_type(&mut self, motion_type: MotorCommand_MotionType::Type) { self.motion_type = motion_type; } @@ -335,10 +339,6 @@ impl< pub fn read_vel_computed_setpoint(&self) -> f32 { return self.current_state.vel_computed_setpoint; } - - pub fn read_hall_vel(&self) -> f32 { - return self.current_state.vel_hall_estimate; - } } pub struct DribblerMotor< @@ -577,6 +577,10 @@ impl< } } + pub fn get_latest_state(&self) -> MotorResponse_Motion_Packet { + self.current_state + } + pub fn log_reset(&self, motor_id: &str) { if self.current_state.reset_watchdog_independent() != 0 { defmt::warn!("Dribbler {} Reset: Watchdog Independent", motor_id); diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 7ec8291e..38b00bea 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -1,4 +1,4 @@ -use ateam_common_packets::{bindings_radio::BasicTelemetry, bindings_stspin::MotorCommand_MotionType, radio::TelemetryPacket}; +use ateam_common_packets::{bindings::{BasicTelemetry, MotorCommand_MotionType}, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use embassy_executor::{SendSpawner, Spawner}; use embassy_stm32::usart::Uart; @@ -188,10 +188,12 @@ impl < self.telemetry_publisher.publish_immediate(basic_telem); let mut control_debug_telem = robot_controller.get_control_debug_telem(); - control_debug_telem.motor_fl.wheel_torque = self.motor_fl.read_hall_vel(); - control_debug_telem.motor_bl.wheel_torque = self.motor_bl.read_hall_vel(); - control_debug_telem.motor_br.wheel_torque = self.motor_br.read_hall_vel(); - control_debug_telem.motor_fr.wheel_torque = self.motor_fr.read_hall_vel(); + + control_debug_telem.motor_fl = self.motor_fl.get_latest_state(); + control_debug_telem.motor_bl = self.motor_bl.get_latest_state(); + control_debug_telem.motor_br = self.motor_br.get_latest_state(); + control_debug_telem.motor_fr = self.motor_fr.get_latest_state(); + control_debug_telem.imu_accel[0] = self.last_accel_x_ms; control_debug_telem.imu_accel[1] = self.last_accel_y_ms; diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 967eaa5a..068201bc 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,5 +1,5 @@ -use ateam_common_packets::{bindings_radio::BasicTelemetry, radio::TelemetryPacket}; +use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket}; use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_signals, queue_pair_rx_task, queue_pair_tx_task, uart::queue::{UartReadQueue, UartWriteQueue}}; use credentials::WifiCredential; use embassy_executor::{SendSpawner, Spawner}; @@ -30,7 +30,7 @@ macro_rules! create_radio_task { pub const RADIO_LOOP_RATE_MS: u64 = 10; -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 256; +pub const RADIO_MAX_TX_PACKET_SIZE: usize = 320; pub const RADIO_TX_BUF_DEPTH: usize = 4; pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; pub const RADIO_RX_BUF_DEPTH: usize = 4; diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index e9118b23..e0c861a1 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -34,7 +34,7 @@ use ateam_kicker_board::{ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; -use ateam_common_packets::bindings_kicker::{KickerControl, KickerTelemetry, KickRequest}; +use ateam_common_packets::bindings::{KickerControl, KickerTelemetry, KickRequest}; const MAX_TX_PACKET_SIZE: usize = 16; const TX_BUF_DEPTH: usize = 3; diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index cd15f4c6..ceeb0060 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -35,7 +35,7 @@ use ateam_kicker_board::{ use ateam_lib_stm32::{make_uart_queue_pair, queue_pair_register_and_spawn}; use ateam_lib_stm32::uart::queue::{UartReadQueue, UartWriteQueue}; -use ateam_common_packets::bindings_kicker::{ +use ateam_common_packets::bindings::{ KickRequest::{self, KR_ARM, KR_DISABLE}, KickerControl, KickerTelemetry, }; diff --git a/lib-stm32/build.rs b/lib-stm32/build.rs index e607c5b3..b6d5f192 100644 --- a/lib-stm32/build.rs +++ b/lib-stm32/build.rs @@ -49,5 +49,7 @@ fn generate_pitch_from_reference(ref_pitch: u16, semitones: i8) -> u16{ fn main() { let a4 = 440; let pitch_set = generate_pitch_set(a4, 2, 2); - write_pitches_to_file(pitch_set); + // TODO compare contents, only write if different + // invalidates build caches and makes build times high + // write_pitches_to_file(pitch_set); } \ No newline at end of file diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index b9af604b..a953a95b 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -217,10 +217,10 @@ int main() { u_torque_loop = torque_setpoint; // torque control data - response_packet.data.motion.current_setpoint = r; - response_packet.data.motion.current_estimate = cur_measurement; - response_packet.data.motion.current_computed_error = torque_pid.prev_err; - response_packet.data.motion.current_computed_setpoint = torque_setpoint; + response_packet.data.motion.torque_setpoint = r; + response_packet.data.motion.torque_estimate = cur_measurement; + response_packet.data.motion.torque_computed_error = torque_pid.prev_err; + response_packet.data.motion.torque_computed_setpoint = torque_setpoint; } if (run_torque_loop) { @@ -263,8 +263,6 @@ int main() { // loop time response_packet.data.motion.control_loop_time_error = slipped_control_frame_count > 10; - // timestamp - response_packet.data.motion.timestamp = time_local_epoch_s(); // master error response_packet.data.motion.master_error = response_packet.data.motion.hall_power_error diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 806be928..6cb07318 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -327,11 +327,11 @@ int main() { // load data frame // torque control data - response_packet.data.motion.current_setpoint = r_Nm; - // response_packet.data.motion.current_estimate = current_sense_I; - response_packet.data.motion.current_estimate = measured_torque_Nm; - response_packet.data.motion.current_computed_error = torque_pid.prev_err; - response_packet.data.motion.current_computed_setpoint = torque_setpoint_Nm; + response_packet.data.motion.torque_setpoint = r_Nm; + response_packet.data.motion.current_estimate = current_sense_I; + response_packet.data.motion.torque_estimate = measured_torque_Nm; + response_packet.data.motion.torque_computed_error = torque_pid.prev_err; + response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm; } // run velocity loop if applicable @@ -363,9 +363,9 @@ int main() { // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; + response_packet.data.motion.vel_setpoint_clamped = control_setpoint_vel_rads; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; - response_packet.data.motion.vel_hall_estimate = control_setpoint_vel_rads; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; } @@ -379,7 +379,6 @@ int main() { float r_motor = mm_rads_to_dc(&df45_model, r_motor_board); response_packet.data.motion.vel_setpoint = r_motor_board; response_packet.data.motion.vel_computed_setpoint = r_motor; - response_packet.data.motion.vel_hall_estimate = r_motor_board; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); @@ -432,9 +431,6 @@ int main() { || response_packet.data.motion.overvoltage_error || response_packet.data.motion.control_loop_time_error; - // timestamp - response_packet.data.motion.timestamp = time_local_epoch_s(); - // transmit packets #ifdef UART_ENABLED // If previous UART transmit is still occurring, diff --git a/software-communication b/software-communication index d337d483..5c06b8ac 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit d337d48321aa453d15c9a93d8b5482c3b4213889 +Subproject commit 5c06b8ac380e254188fa2cb25310c4b09e1d262c From 69452f68d0dcbb00c3a0406a48dcf180fdd5741a Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Wed, 11 Dec 2024 10:23:02 -0800 Subject: [PATCH 157/157] Got tests building --- control-board/src/bin/hwtest-control/main.rs | 12 +++---- control-board/src/bin/hwtest-motor/main.rs | 15 ++++---- control-board/src/bin/hwtest-radio/main.rs | 4 +-- control-board/src/stm32_interface.rs | 2 +- control-board/src/stspin_motor.rs | 37 ++++++++------------ motor-controller/bin/dribbler/main.c | 9 +++-- motor-controller/bin/dribbler/system.h | 12 ++++--- motor-controller/bin/wheel/main.c | 10 +++--- motor-controller/bin/wheel/system.h | 10 +++--- 9 files changed, 54 insertions(+), 57 deletions(-) diff --git a/control-board/src/bin/hwtest-control/main.rs b/control-board/src/bin/hwtest-control/main.rs index ddb90f65..d923515c 100644 --- a/control-board/src/bin/hwtest-control/main.rs +++ b/control-board/src/bin/hwtest-control/main.rs @@ -1,14 +1,14 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings_kicker::KickRequest, bindings_radio::BasicControl, radio::DataPacket}; +use ateam_common_packets::{bindings::KickRequest, bindings::BasicControl, radio::DataPacket}; use embassy_executor::InterruptExecutor; use embassy_stm32::{ interrupt, pac::Interrupt }; use embassy_sync::pubsub::PubSubChannel; -use defmt_rtt as _; +use defmt_rtt as _; use ateam_control_board::{ create_audio_task, create_imu_task, create_io_task, create_radio_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::{control_task::start_control_task, kicker_task::start_kicker_task}}; @@ -132,8 +132,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { p); start_control_task( - uart_queue_spawner, main_spawner, - robot_state, + uart_queue_spawner, main_spawner, + robot_state, control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, control_accel_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, @@ -159,8 +159,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, - dribbler_speed: -0.1, - kick_request: KickRequest::KR_ARM, + dribbler_speed: -0.1, + kick_request: KickRequest::KR_ARM, })); } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index e9c1fcab..0f2dc987 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -1,14 +1,14 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings_kicker::KickRequest, bindings_radio::{BasicControl, BasicTelemetry}, radio::DataPacket}; +use ateam_common_packets::{bindings::KickRequest, bindings::{BasicControl, BasicTelemetry}, radio::DataPacket}; use embassy_executor::InterruptExecutor; use embassy_stm32::{ interrupt, pac::Interrupt }; use embassy_sync::pubsub::PubSubChannel; -use defmt_rtt as _; +use defmt_rtt as _; use ateam_control_board::{create_imu_task, create_io_task, create_shutdown_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task}; @@ -77,6 +77,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); /////////////////// // start tasks // @@ -97,9 +98,9 @@ async fn main(main_spawner: embassy_executor::Spawner) { p); start_control_task( - uart_queue_spawner, main_spawner, - robot_state, - control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, + uart_queue_spawner, main_spawner, + robot_state, + control_command_subscriber, control_telemetry_publisher, battery_volt_subscriber, control_gyro_data_subscriber, control_accel_data_subscriber, p.UART4, p.PA1, p.PA0, p.DMA1_CH3, p.DMA1_CH2, p.PC1, p.PC0, p.UART7, p.PF6, p.PF7, p.DMA1_CH5, p.DMA1_CH4, p.PF8, p.PF9, p.UART8, p.PE0, p.PE1, p.DMA1_CH7, p.DMA1_CH6, p.PB9, p.PB8, @@ -115,8 +116,8 @@ async fn main(main_spawner: embassy_executor::Spawner) { vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, - dribbler_speed: 10.0, - kick_request: KickRequest::KR_DISABLE, + dribbler_speed: 10.0, + kick_request: KickRequest::KR_DISABLE, })).await; } } \ No newline at end of file diff --git a/control-board/src/bin/hwtest-radio/main.rs b/control-board/src/bin/hwtest-radio/main.rs index ec08695c..c36a812d 100644 --- a/control-board/src/bin/hwtest-radio/main.rs +++ b/control-board/src/bin/hwtest-radio/main.rs @@ -1,7 +1,7 @@ #![no_std] #![no_main] -use ateam_common_packets::{bindings_radio::{BasicTelemetry, ParameterCommandCode}, radio::{DataPacket, TelemetryPacket}}; +use ateam_common_packets::{bindings::{BasicTelemetry, ParameterCommandCode}, radio::{DataPacket, TelemetryPacket}}; use embassy_executor::InterruptExecutor; use embassy_futures::select::{self, Either}; use embassy_stm32::{ @@ -9,7 +9,7 @@ use embassy_stm32::{ }; use embassy_sync::pubsub::{PubSubChannel, WaitResult}; -use defmt_rtt as _; +use defmt_rtt as _; use ateam_control_board::{create_io_task, create_radio_task, get_system_config, pins::{BatteryVoltPubSub, CommandsPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs index b2696cd3..696f0d71 100644 --- a/control-board/src/stm32_interface.rs +++ b/control-board/src/stm32_interface.rs @@ -133,7 +133,7 @@ impl< } else { self.reset_pin.set_high(); } - Timer::after(Duration::from_millis(50)).await; + Timer::after(Duration::from_millis(10)).await; } pub async fn hard_reset(&mut self) { diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 526e52f8..27ae24a9 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -174,13 +174,10 @@ impl< // this is safe because load firmware image call will reset the target device // it will begin issueing telemetry updates // these are the only packets it sends so any blocked process should get the data it now needs - defmt::debug!("update config"); + defmt::debug!("Drive Motor - Update config"); self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; Timer::after(Duration::from_millis(1)).await; - // load firmware image call leaves the part in reset, now that our uart is ready, bring the part out of reset - self.stm32_uart_interface.leave_reset().await; - return res; } @@ -193,7 +190,7 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + defmt::warn!("Drive Motor - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); continue; } @@ -208,9 +205,9 @@ impl< *state.offset(i as isize) = buf[i]; } - + // TODO probably do some checksum stuff eventually - + // decode union type, and reinterpret subtype if mrp.type_ == MRP_MOTION { self.current_state = mrp.data.motion; @@ -220,7 +217,7 @@ impl< // info!("vel enc {:?}", mrp.data.motion.vel_enc_estimate + 0.); // // // info!("vel hall {:?}", mrp.data.motion.vel_hall_estimate + 0.); if mrp.data.motion.master_error() != 0 { - error!("motor error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); + error!("Drive Motor - Error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); } // info!("hall_power_error {:?}", mrp.data.motion.hall_power_error()); // info!("hall_disconnected_error {:?}", mrp.data.motion.hall_disconnected_error()); @@ -247,19 +244,19 @@ impl< pub fn log_reset(&self, motor_id: &str) { if self.current_state.reset_watchdog_independent() != 0 { - defmt::warn!("Motor {} Reset: Watchdog Independent", motor_id); + defmt::warn!("Drive Motor {} Reset: Watchdog Independent", motor_id); } if self.current_state.reset_watchdog_window() != 0 { - defmt::warn!("Motor {} Reset: Watchdog Window", motor_id); + defmt::warn!("Drive Motor {} Reset: Watchdog Window", motor_id); } if self.current_state.reset_low_power() != 0 { - defmt::warn!("Motor {} Reset: Low Power", motor_id); + defmt::warn!("Drive Motor {} Reset: Low Power", motor_id); } if self.current_state.reset_software() != 0 { - defmt::warn!("Motor {} Reset: Software", motor_id); + defmt::warn!("Drive Motor {} Reset: Software", motor_id); } if self.current_state.reset_pin() != 0 { - defmt::warn!("Motor {} Reset: Pin", motor_id); + defmt::warn!("Drive Motor {} Reset: Pin", motor_id); } } @@ -270,9 +267,7 @@ impl< cmd.type_ = MCP_MOTION; cmd.crc32 = 0; cmd.data.motion.set_reset(self.reset_flagged as u32); - cmd.data - .motion - .set_enable_telemetry(self.telemetry_enabled as u32); + cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32); cmd.data.motion.motion_control_type = self.motion_type; cmd.data.motion.setpoint = self.setpoint; //info!("setpoint: {:?}", cmd.data.motion.setpoint); @@ -502,7 +497,7 @@ impl< // this is safe because load firmware image call will reset the target device // it will begin issueing telemetry updates // these are the only packets it sends so any blocked process should get the data it now needs - defmt::debug!("update config"); + defmt::debug!("Dribbler - Update config"); self.stm32_uart_interface.update_uart_config(2_000_000, Parity::ParityEven).await; Timer::after(Duration::from_millis(1)).await; @@ -521,7 +516,7 @@ impl< let buf = res.data(); if buf.len() != core::mem::size_of::() { - defmt::warn!("got invalid packet of len {:?} data: {:?}", buf.len(), buf); + defmt::warn!("Dribbler - Got invalid packet of len {:?} (expected {:?}) data: {:?}", buf.len(), core::mem::size_of::(), buf); continue; } @@ -545,7 +540,7 @@ impl< // if mrp.data.motion.master_error() != 0 { // error!("drib error: {:?}", &mrp.data.motion._bitfield_1.get(0, 32)); // } - + // // // info!("{:?}", defmt::Debug2Format(&mrp.data.motion)); // // info!("\n"); // // // info!("vel set {:?}", mrp.data.motion.vel_setpoint + 0.); @@ -606,9 +601,7 @@ impl< cmd.type_ = MCP_MOTION; cmd.crc32 = 0; cmd.data.motion.set_reset(self.reset_flagged as u32); - cmd.data - .motion - .set_enable_telemetry(self.telemetry_enabled as u32); + cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32); cmd.data.motion.motion_control_type = self.motion_type; cmd.data.motion.setpoint = self.setpoint; diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index a953a95b..8a75767f 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -105,7 +105,6 @@ int main() { while (true) { IWDG->KR = 0x0000AAAA; // feed the watchdog - #ifdef UART_ENABLED // watchdog on receiving a command packet ticks_since_last_command_packet++; @@ -144,10 +143,10 @@ int main() { // we got a motion packet! ticks_since_last_command_packet = 0; - if (motor_command_packet.data.motion.reset) { - // Block, watchdog will trigger reset. - while (true); - } + //if (motor_command_packet.data.motion.reset) { + // // Block, watchdog will trigger reset. + // while (true); + //} telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; r_motor_board = motor_command_packet.data.motion.setpoint; diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h index 0fff3837..dbcfa5a6 100644 --- a/motor-controller/bin/dribbler/system.h +++ b/motor-controller/bin/dribbler/system.h @@ -1,12 +1,12 @@ /** * @file system.h * @author Will Stuckey - * @brief + * @brief * @version 0.1 * @date 2022-05-22 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once @@ -46,7 +46,7 @@ #define VELOCITY_LOOP_RATE_S ((float) VELOCITY_LOOP_RATE_MS / (float) MS_PER_S) #define TORQUE_LOOP_RATE_MS 1 #define TORQUE_LOOP_RATE_S ((float) TORQUE_LOOP_RATE_MS / (float) MS_PER_S) -#define TELEMETRY_LOOP_RATE_MS 1 +#define TELEMETRY_LOOP_RATE_MS 5 //////////////////////// // FILTERING/TUNING // @@ -56,4 +56,6 @@ #define DC_IIR_TF_MS 0.20f -#define MOTOR_MAXIMUM_RAD_S 550.825911929f \ No newline at end of file +#define MOTOR_MAXIMUM_RAD_S 550.825911929f + +//#define COMP_MODE \ No newline at end of file diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 6cb07318..0f0ef5ae 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -204,11 +204,11 @@ int main() { // We got a motion packet! ticks_since_last_command_packet = 0; - if (motor_command_packet.data.motion.reset) { - // Block, watchdog will trigger reset. - - while (true); - } + //if (motor_command_packet.data.motion.reset) { + // // Block, watchdog will trigger reset. + // + // while (true); + //} telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; r_motor_board = motor_command_packet.data.motion.setpoint; diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index a2ac31bc..301436df 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -1,12 +1,12 @@ /** * @file system.h * @author Will Stuckey - * @brief + * @brief * @version 0.1 * @date 2022-05-22 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once @@ -47,4 +47,6 @@ #define MOTOR_MAXIMUM_ACCEL 6000 // rad/s^2 -// #define MOTOR_MAXIMUM_RAD_S 550.825911929f \ No newline at end of file +// #define MOTOR_MAXIMUM_RAD_S 550.825911929f + +//#define COMP_MODE \ No newline at end of file