diff --git a/.gitignore b/.gitignore index db3f982..b0b89cd 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,5 @@ electronics/imu_module/production/backups/* # Platform IO firmware/.pio/* firmware/.vscode/* + +firmware/target/* diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index c24578f..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "firmware/lib/bmi088-arduino"] - path = firmware/lib/bmi088-arduino - url = git@github.com:bit-bots/bmi088-arduino.git diff --git a/firmware/.cargo/config.toml b/firmware/.cargo/config.toml new file mode 100644 index 0000000..0120b79 --- /dev/null +++ b/firmware/.cargo/config.toml @@ -0,0 +1,15 @@ +[target.xtensa-esp32-none-elf] +runner = "espflash flash --monitor" + +[env] +ESP_LOG="INFO" + +[build] +rustflags = [ + "-C", "link-arg=-nostartfiles" +] + +target = "xtensa-esp32-none-elf" + +[unstable] +build-std = ["alloc", "core"] diff --git a/firmware/.gitignore b/firmware/.gitignore index 89cc49c..6e771f8 100644 --- a/firmware/.gitignore +++ b/firmware/.gitignore @@ -1,5 +1,17 @@ -.pio -.vscode/.browse.c_cpp.db* -.vscode/c_cpp_properties.json -.vscode/launch.json -.vscode/ipch +# Generated by Cargo +# will have compiled files and executables +debug/ +target/ + +# These are backup files generated by rustfmt +**/*.rs.bk + +# MSVC Windows builds of rustc generate these, which store debugging information +*.pdb + +# RustRover +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ diff --git a/firmware/.vscode/extensions.json b/firmware/.vscode/extensions.json deleted file mode 100644 index 080e70d..0000000 --- a/firmware/.vscode/extensions.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ], - "unwantedRecommendations": [ - "ms-vscode.cpptools-extension-pack" - ] -} diff --git a/firmware/.vscode/settings.json b/firmware/.vscode/settings.json deleted file mode 100644 index eacbe83..0000000 --- a/firmware/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "ros.distro": "rolling" -} \ No newline at end of file diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock new file mode 100644 index 0000000..976ec8e --- /dev/null +++ b/firmware/Cargo.lock @@ -0,0 +1,1288 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "anyhow" +version = "1.0.97" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dcfed56ad506cb2c684a14971b8861fdc3baaaae314b9e5f9bb532cbe3ba7a4f" + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ace50bade8e6234aa140d9a2f552bbee1db4d353f69b8217bc503490fc1a9f26" + +[[package]] +name = "basic-toml" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba62675e8242a4c4e806d12f11d136e626e6c8361d6b829310732241652a178a" +dependencies = [ + "serde", +] + +[[package]] +name = "bitbots_imu_firmware" +version = "0.1.0" +dependencies = [ + "bmi088", + "critical-section", + "defmt", + "dynamixel2", + "embedded-hal 1.0.0", + "embedded-hal-bus", + "embedded-io", + "embedded-storage", + "esp-backtrace", + "esp-hal", + "esp-hal-smartled", + "esp-println", + "esp-storage", + "heapless", + "imu-fusion", + "log", + "serde", + "serde-json-core", + "smart-leds", +] + +[[package]] +name = "bitfield" +version = "0.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f798d2d157e547aa99aab0967df39edd0b70307312b6f8bd2848e6abe40896e0" + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c8214115b7bf84099f1309324e63141d4c5d7cc26862f97a0a857dbefe165bd" + +[[package]] +name = "bmi088" +version = "0.2.0" +source = "git+https://github.com/bit-bots/bmi088#64b5756b22783ced524e6d2bf818025625bb03e5" +dependencies = [ + "embedded-hal 1.0.0", +] + +[[package]] +name = "bytemuck" +version = "1.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6b1fc10dbac614ebc03540c9dbd60e83887fda27794998c6528f1782047d540" + +[[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.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a7964611d71df112cb1730f2ee67324fcf4d0fc6606acbbe9bfe06df124637c" +dependencies = [ + "num-traits", +] + +[[package]] +name = "critical-section" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" + +[[package]] +name = "darling" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f63b86c8a8826a49b8c21f08a2d07338eec8d900540f8630dc76284be802989" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95133861a8032aaea082871032f5815eb9e98cef03fa916ab4500513994df9e5" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn", +] + +[[package]] +name = "darling_macro" +version = "0.20.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d336a2a514f6ccccaa3e09b02d41d35330c07ddf03a62165fcec10bb561c7806" +dependencies = [ + "darling_core", + "quote", + "syn", +] + +[[package]] +name = "defmt" +version = "0.3.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86f6162c53f659f65d00619fe31f14556a6e9f8752ccc4a41bd177ffcf3d6130" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d135dd939bad62d7490b0002602d35b358dce5fd9233a709d3c1ef467d4bde6" +dependencies = [ + "defmt-parser", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "defmt-parser" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3983b127f13995e68c1e29071e5d115cd96f215ccb5e6812e3728cd6f92653b3" +dependencies = [ + "thiserror", +] + +[[package]] +name = "delegate" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "297806318ef30ad066b15792a8372858020ae3ca2e414ee6c2133b1eb9e9e945" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "document-features" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" +dependencies = [ + "litrs", +] + +[[package]] +name = "dynamixel2" +version = "0.9.1" +source = "git+https://github.com/robohouse-delft/dynamixel2-rs.git?branch=main#eed1aa8d1ac8819e0b1b802f560ee5007757a9dd" + +[[package]] +name = "embassy-embedded-hal" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41fea5ef5bed4d3468dfd44f5c9fa4cda8f54c86d4fb4ae683eacf9d39e2ea12" +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-futures" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067" + +[[package]] +name = "embassy-sync" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" +dependencies = [ + "cfg-if", + "critical-section", + "embedded-io-async", + "futures-sink", + "futures-util", + "heapless", +] + +[[package]] +name = "embassy-time" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f820157f198ada183ad62e0a66f554c610cdcd1a9f27d4b316358103ced7a1f8" +dependencies = [ + "cfg-if", + "critical-section", + "document-features", + "embassy-time-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-util", +] + +[[package]] +name = "embassy-time-driver" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba" +dependencies = [ + "document-features", +] + +[[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-bus" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "513e0b3a8fb7d3013a8ae17a834283f170deaf7d0eeab0a7c1a36ad4dd356d22" +dependencies = [ + "critical-section", + "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 = "enum-as-inner" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1e6a265c649f3f5979b601d26f1d05ada116434c87741c9493cb56218f76cbc" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "enumset" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d07a4b049558765cef5f0c1a273c3fc57084d768b44d2f98127aef4cceb17293" +dependencies = [ + "enumset_derive", +] + +[[package]] +name = "enumset_derive" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "59c3b24c345d8c314966bdc1832f6c2635bfcce8e7cf363bd115987bba2ee242" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "esp-backtrace" +version = "0.15.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e4cd70abe47945c9116972781b5c05277ad855a5f5569fe2afd3e2e61a103cc0" +dependencies = [ + "esp-build 0.2.0", + "esp-println", + "semihosting", +] + +[[package]] +name = "esp-build" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b94a4b8d74e7cc7baabcca5b2277b41877e039ad9cd49959d48ef94dac7eab4b" +dependencies = [ + "quote", + "syn", + "termcolor", +] + +[[package]] +name = "esp-build" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8aa1c8f9954c9506699cf1ca10a2adcc226ff10b6ae3cb9e875cf2c6a0b9a372" +dependencies = [ + "quote", + "syn", + "termcolor", +] + +[[package]] +name = "esp-config" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "158dba334d3a2acd8d93873c0ae723ca1037cc78eefe5d6b4c5919b0ca28e38e" +dependencies = [ + "document-features", +] + +[[package]] +name = "esp-hal" +version = "0.23.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a573175c540fd1d21a9cf07b0dee286b5a8f4cfde4b35da0f4f4657de7942c45" +dependencies = [ + "basic-toml", + "bitfield", + "bitflags 2.9.0", + "bytemuck", + "cfg-if", + "chrono", + "critical-section", + "delegate", + "document-features", + "embassy-embedded-hal", + "embassy-futures", + "embassy-sync", + "embedded-can", + "embedded-hal 1.0.0", + "embedded-hal-async", + "embedded-hal-nb", + "embedded-io", + "embedded-io-async", + "enumset", + "esp-build 0.2.0", + "esp-config", + "esp-hal-procmacros", + "esp-metadata", + "esp-riscv-rt", + "esp32", + "fugit", + "instability", + "nb 1.1.0", + "paste", + "portable-atomic", + "rand_core", + "riscv", + "serde", + "strum", + "ufmt-write", + "void", + "xtensa-lx", + "xtensa-lx-rt", +] + +[[package]] +name = "esp-hal-procmacros" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e4a3297005c2b31cd00e2ba50037edc9bddf99da3afe1c97a2d1b0165a312eab" +dependencies = [ + "darling", + "document-features", + "litrs", + "proc-macro-crate", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "esp-hal-smartled" +version = "0.14.0" +source = "git+https://github.com/esp-rs/esp-hal-community?rev=ad75112#ad75112ae5d4b4fe2fe927356dc281283cafedfc" +dependencies = [ + "document-features", + "esp-hal", + "fugit", + "smart-leds-trait", +] + +[[package]] +name = "esp-metadata" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb15c17e50f4cccb0d88305c19eae2d5533d750f0a05b6a05f1c99864974758e" +dependencies = [ + "anyhow", + "basic-toml", + "serde", + "strum", +] + +[[package]] +name = "esp-println" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "960703930f9f3c899ddedd122ea27a09d6a612c22323157e524af5b18876448e" +dependencies = [ + "critical-section", + "esp-build 0.2.0", + "log", + "portable-atomic", +] + +[[package]] +name = "esp-riscv-rt" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94aca65db6157aa5f42d9df6595b21462f28207ca4230b799aa3620352ef6a72" +dependencies = [ + "document-features", + "riscv", + "riscv-rt-macros", +] + +[[package]] +name = "esp-storage" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "661aebceb62e0caadd7e6bddd88b2d891c3b08cea4eb5afe96ef697674ccda5c" +dependencies = [ + "critical-section", + "embedded-storage", + "esp-build 0.1.0", +] + +[[package]] +name = "esp32" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19d3bff1d268a4b8d34b494c0e88466cd59a827bb330189773db299ff525ea13" +dependencies = [ + "critical-section", + "vcell", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "fugit" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17186ad64927d5ac8f02c1e77ccefa08ccd9eaa314d5a4772278aa204a22f7e7" +dependencies = [ + "gcd", +] + +[[package]] +name = "futures-core" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e" + +[[package]] +name = "futures-sink" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7" + +[[package]] +name = "futures-task" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988" + +[[package]] +name = "futures-util" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81" +dependencies = [ + "futures-core", + "futures-task", + "pin-project-lite", + "pin-utils", +] + +[[package]] +name = "gcd" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d758ba1b47b00caf47f24925c0074ecb20d6dfcffe7f6d53395c0465674841a" + +[[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.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "serde", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "imu-fusion" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5d7b71a133034d36609ba862547bebd7a6f54eb24a723f689a19a3640eb1f01" +dependencies = [ + "libm", + "nalgebra", +] + +[[package]] +name = "indexmap" +version = "2.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8c9c992b02b5b4c94ea26e32fe5bccb7aa7d9f390ab5c1221ff895bc7ea8b652" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "indoc" +version = "2.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f4c7245a08504955605670dbf141fceab975f15ca21570696aebe9d2e71576bd" + +[[package]] +name = "instability" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bf9fed6d91cfb734e7476a06bde8300a1b94e217e1b523b6f0cd1a01998c71d" +dependencies = [ + "darling", + "indoc", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "libm" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8355be11b20d696c8f18f6cc018c4e372165b1fa8126cef092399c9951984ffa" + +[[package]] +name = "litrs" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ce301924b7887e9d637144fdade93f9dfff9b60981d4ac161db09720d39aa5" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "log" +version = "0.4.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30bde2b3dc3671ae49d8e2e9f044c7c005836e7a023ee57cffa25ab82764bb9e" + +[[package]] +name = "memchr" +version = "2.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" + +[[package]] +name = "minijinja" +version = "2.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e36f1329330bb1614c94b78632b9ce45dd7d761f3304a1bed07b2990a7c5097" +dependencies = [ + "serde", +] + +[[package]] +name = "nalgebra" +version = "0.32.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b5c17de023a86f59ed79891b2e5d5a94c705dbe904a5b5c9c952ea6221b03e4" +dependencies = [ + "approx", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[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-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +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.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b" + +[[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.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "350e9b48cbc6b0e028b0473b114454c6316e57336ee184ceab6e53f72c178b3e" + +[[package]] +name = "proc-macro-crate" +version = "3.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edce586971a4dfaa28950c6f18ed55e0406c1ab88bbce2c6f6293a7aaba73d35" +dependencies = [ + "toml_edit", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "proc-macro2" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31971752e70b8b2686d7e46ec17fb38dad4051d94024c88df49b667caea9c84" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1f1914ce909e1658d9907913b4b91947430c7d9be598b15a1912935b8c04801" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r0" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd7a31eed1591dcbc95d92ad7161908e72f4677f8fabf2a32ca49b4237cbf211" + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" + +[[package]] +name = "rgb" +version = "0.8.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57397d16646700483b67d2dd6511d79318f9d057fdbd21a4066aeac8b41d310a" +dependencies = [ + "bytemuck", +] + +[[package]] +name = "riscv" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5ea8ff73d3720bdd0a97925f0bf79ad2744b6da8ff36be3840c48ac81191d7a7" +dependencies = [ + "critical-section", + "embedded-hal 1.0.0", + "paste", + "riscv-macros", + "riscv-pac", +] + +[[package]] +name = "riscv-macros" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f265be5d634272320a7de94cea15c22a3bfdd4eb42eb43edc528415f066a1f25" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "riscv-pac" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8188909339ccc0c68cfb5a04648313f09621e8b87dc03095454f1a11f6c5d436" + +[[package]] +name = "riscv-rt-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30f19a85fe107b65031e0ba8ec60c34c2494069fe910d6c297f5e7cb5a6f76d0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "rustversion" +version = "1.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2" + +[[package]] +name = "ryu" +version = "1.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "28d3b2b1366ec20994f1fd18c3c594f05c5dd4bc44d8bb0c1c632c8d6829481f" + +[[package]] +name = "semihosting" +version = "0.1.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "407ec8d274cd77556e9c2bef886df91eb3447b4059e603d6fb19f85e6452e275" + +[[package]] +name = "serde" +version = "1.0.218" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8dfc9d19bdbf6d17e22319da49161d5d0108e4188e8b680aef6299eed22df60" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde-json-core" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b81787e655bd59cecadc91f7b6b8651330b2be6c33246039a65e5cd6f4e0828" +dependencies = [ + "heapless", + "ryu", + "serde", +] + +[[package]] +name = "serde_derive" +version = "1.0.218" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f09503e191f4e797cb8aac08e9a4a4695c5edf6a2e70e376d961ddd5c969f82b" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_spanned" +version = "0.6.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87607cb1398ed59d48732e575a4c28a7a8ebf2454b964fe3f224f2afc07909e1" +dependencies = [ + "serde", +] + +[[package]] +name = "simba" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" +dependencies = [ + "approx", + "libm", + "num-complex", + "num-traits", + "paste", +] + +[[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.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edeb89c73244414bb0568611690dd095b2358b3fda5bae65ad784806cca00157" +dependencies = [ + "rgb", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "strum" +version = "0.26.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fec0f0aef304996cf250b31b5a10dee7980c85da9d759361292b8bca5a18f06" +dependencies = [ + "strum_macros", +] + +[[package]] +name = "strum_macros" +version = "0.26.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn", +] + +[[package]] +name = "syn" +version = "2.0.99" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e02e925281e18ffd9d640e234264753c43edc62d64b2d4cf898f1bc5e75f3fc2" +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 = "thiserror" +version = "2.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "567b8a2dae586314f7be2a752ec7474332959c6460e02bde30d702a66d488708" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "toml" +version = "0.8.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd87a5cdd6ffab733b2f74bc4fd7ee5fff6634124999ac278c35fc78c6120148" +dependencies = [ + "serde", + "serde_spanned", + "toml_datetime", + "toml_edit", +] + +[[package]] +name = "toml_datetime" +version = "0.6.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0dd7358ecb8fc2f8d014bf86f6f638ce72ba252a2c3a2572f2a795f1d23efb41" +dependencies = [ + "serde", +] + +[[package]] +name = "toml_edit" +version = "0.22.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17b4795ff5edd201c7cd6dca065ae59972ce77d1b80fa0a84d94950ece7d1474" +dependencies = [ + "indexmap", + "serde", + "serde_spanned", + "toml_datetime", + "winnow", +] + +[[package]] +name = "typenum" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f" + +[[package]] +name = "ufmt-write" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69" + +[[package]] +name = "unicode-ident" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" + +[[package]] +name = "vcell" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" + +[[package]] +name = "void" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" + +[[package]] +name = "winapi-util" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf221c93e13a30d793f7645a0e7762c55d169dbb0a49671918a2319d289b10bb" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "winnow" +version = "0.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e7f4ea97f6f78012141bcdb6a216b2609f0979ada50b20ca5b52dde2eac2bb1" +dependencies = [ + "memchr", +] + +[[package]] +name = "xtensa-lx" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "51cbb46c78cfd284c9378070ab90bae9d14d38b3766cb853a97c0a137f736d5b" +dependencies = [ + "critical-section", + "document-features", +] + +[[package]] +name = "xtensa-lx-rt" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "689c2ef159d9cd4fc9503603e9999968a84a30db9bde0f0f880d0cceea0190a9" +dependencies = [ + "anyhow", + "document-features", + "enum-as-inner", + "minijinja", + "r0", + "serde", + "strum", + "toml", + "xtensa-lx", + "xtensa-lx-rt-proc-macros", +] + +[[package]] +name = "xtensa-lx-rt-proc-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11277b1e4cbb7ffe44678c668518b249c843c81df249b8f096701757bc50d7ee" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn", +] diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml new file mode 100644 index 0000000..0b0d29c --- /dev/null +++ b/firmware/Cargo.toml @@ -0,0 +1,58 @@ +[package] +edition = "2021" +name = "bitbots_imu_firmware" +version = "0.1.0" + +[[bin]] +name = "bitbots_imu_firmware" +path = "./src/main.rs" + +[dependencies] +critical-section = "1.2.0" +esp-backtrace = { version = "0.15.0", features = [ + "esp32", + "exception-handler", + "panic-handler", + "println", + "custom-halt", +] } +esp-hal = { version = "0.23.1", features = ["esp32", "unstable"] } +esp-println = { version = "0.13.0", features = ["esp32", "log"] } +log = { version = "0.4.21" } +dynamixel2 = { git = "https://github.com/robohouse-delft/dynamixel2-rs.git", branch = "main", default-features = false } +embedded-io = "0.6.1" +defmt = "0.3.10" +esp-storage = { version = "0.4.0", features = ["esp32", "storage"] } +embedded-storage = "0.3.1" +serde = { version = "1.0.217", default-features = false, features = ["derive"] } +serde-json-core = "0.6.0" +heapless = { version = "0.8.0", features = ["serde"] } +embedded-hal-bus = "0.3.0" +esp-hal-smartled = { git = "https://github.com/esp-rs/esp-hal-community", rev = "ad75112" } +smart-leds = "0.4.0" +embedded-hal = "1.0.0" +imu-fusion = "0.2.5" +bmi088 = { git = "https://github.com/bit-bots/bmi088", version = "0.2.0" } + +[profile.dev] +# Rust debug is too slow. +# For debug builds always builds with some optimization +opt-level = "s" + +[profile.dev.package.esp-storage] +opt-level = 3 + +[profile.release.package.esp-storage] +opt-level = 3 + +[profile.release] +codegen-units = 1 # LLVM can perform better optimizations using a single thread +debug = 2 +debug-assertions = false +incremental = false +lto = 'fat' +opt-level = 's' +overflow-checks = false + +[features] +profiling = [] diff --git a/firmware/build.rs b/firmware/build.rs new file mode 100644 index 0000000..e3f3ed6 --- /dev/null +++ b/firmware/build.rs @@ -0,0 +1,3 @@ +fn main() { + println!("cargo:rustc-link-arg=-Tlinkall.x"); +} diff --git a/firmware/include/README b/firmware/include/README deleted file mode 100644 index 194dcd4..0000000 --- a/firmware/include/README +++ /dev/null @@ -1,39 +0,0 @@ - -This directory is intended for project header files. - -A header file is a file containing C declarations and macro definitions -to be shared between several project source files. You request the use of a -header file in your project source file (C, C++, etc) located in `src` folder -by including it, with the C preprocessing directive `#include'. - -```src/main.c - -#include "header.h" - -int main (void) -{ - ... -} -``` - -Including a header file produces the same results as copying the header file -into each source file that needs it. Such copying would be time-consuming -and error-prone. With a header file, the related declarations appear -in only one place. If they need to be changed, they can be changed in one -place, and programs that include the header file will automatically use the -new version when next recompiled. The header file eliminates the labor of -finding and changing all the copies as well as the risk that a failure to -find one copy will result in inconsistencies within a program. - -In C, the usual convention is to give header files names that end with `.h'. -It is most portable to use only letters, digits, dashes, and underscores in -header file names, and at most one dot. - -Read more about using header files in official GCC documentation: - -* Include Syntax -* Include Operation -* Once-Only Headers -* Computed Includes - -https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/firmware/lib/README b/firmware/lib/README deleted file mode 100644 index 6debab1..0000000 --- a/firmware/lib/README +++ /dev/null @@ -1,46 +0,0 @@ - -This directory is intended for project specific (private) libraries. -PlatformIO will compile them to static libraries and link into executable file. - -The source code of each library should be placed in a an own separate directory -("lib/your_library_name/[here are source files]"). - -For example, see a structure of the following two libraries `Foo` and `Bar`: - -|--lib -| | -| |--Bar -| | |--docs -| | |--examples -| | |--src -| | |- Bar.c -| | |- Bar.h -| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html -| | -| |--Foo -| | |- Foo.c -| | |- Foo.h -| | -| |- README --> THIS FILE -| -|- platformio.ini -|--src - |- main.c - -and a contents of `src/main.c`: -``` -#include -#include - -int main (void) -{ - ... -} - -``` - -PlatformIO Library Dependency Finder will find automatically dependent -libraries scanning project source files. - -More information about PlatformIO Library Dependency Finder -- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/firmware/lib/bmi088-arduino b/firmware/lib/bmi088-arduino deleted file mode 160000 index 3af7d9d..0000000 --- a/firmware/lib/bmi088-arduino +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3af7d9dfc4b43bbc66018cf6824d4bb213bc8500 diff --git a/firmware/platformio.ini b/firmware/platformio.ini deleted file mode 100644 index 7e9ccaf..0000000 --- a/firmware/platformio.ini +++ /dev/null @@ -1,19 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:esp32dev] -platform = espressif32@6.5.0 -board = esp32dev -framework = arduino -lib_deps = robotis-git/Dynamixel2Arduino@0.7.0 - jasonlzt/FastLED@3.5.0 -upload_port = /dev/ttyUSB0 -monitor_speed = 115200 -monitor_port = /dev/ttyUSB0 diff --git a/firmware/rust-toolchain.toml b/firmware/rust-toolchain.toml new file mode 100644 index 0000000..a2f5ab5 --- /dev/null +++ b/firmware/rust-toolchain.toml @@ -0,0 +1,2 @@ +[toolchain] +channel = "esp" diff --git a/firmware/rustfmt.toml b/firmware/rustfmt.toml new file mode 100644 index 0000000..082235c --- /dev/null +++ b/firmware/rustfmt.toml @@ -0,0 +1 @@ +group_imports="StdExternalCrate" \ No newline at end of file diff --git a/firmware/src/buttons.rs b/firmware/src/buttons.rs new file mode 100644 index 0000000..6c56772 --- /dev/null +++ b/firmware/src/buttons.rs @@ -0,0 +1,38 @@ +use esp_hal::{ + gpio::{Input, InputPin, Pull}, + peripheral::Peripheral, +}; + +use crate::NUM_BUTTONS; + +pub struct ButtonComponent<'a> { + pin_0: Input<'a>, + pin_1: Input<'a>, + pin_2: Input<'a>, +} + +impl<'a> ButtonComponent<'a> { + pub fn new( + pin_0: impl Peripheral

+ 'a, + pin_1: impl Peripheral

+ 'a, + pin_2: impl Peripheral

+ 'a, + ) -> Self { + Self { + pin_0: Input::new(pin_0, Pull::Up), + pin_1: Input::new(pin_1, Pull::Up), + pin_2: Input::new(pin_2, Pull::Up), + } + } + + pub fn read_bool(&self) -> [bool; NUM_BUTTONS] { + [ + self.pin_0.is_low(), + self.pin_1.is_low(), + self.pin_2.is_low(), + ] + } + + pub fn read_u8(&self) -> [u8; NUM_BUTTONS] { + self.read_bool().map(|b| b as u8) + } +} diff --git a/firmware/src/complementary_filter.cpp b/firmware/src/complementary_filter.cpp deleted file mode 100644 index af8f04e..0000000 --- a/firmware/src/complementary_filter.cpp +++ /dev/null @@ -1,615 +0,0 @@ -/* - @author Roberto G. Valenti - - @section LICENSE - Copyright (c) 2015, City University of New York - CCNY Robotics Lab - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - 3. Neither the name of the City College of New York nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include "complementary_filter.h" - -#include -#include -#include - -namespace imu_tools { - -const double ComplementaryFilter::kGravity = 9.81; -const double ComplementaryFilter::gamma_ = 0.01; -// Bias estimation steady state thresholds -const double ComplementaryFilter::kAngularVelocityThreshold = 0.2; -const double ComplementaryFilter::kAccelerationThreshold = 0.1; -const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01; - -ComplementaryFilter::ComplementaryFilter() : - gain_acc_(0.01), - gain_mag_(0.01), - bias_alpha_(0.01), - do_bias_estimation_(true), - do_adaptive_gain_(false), - initialized_(false), - steady_state_(false), - q0_(1), q1_(0), q2_(0), q3_(0), - q0_pred_(0), q1_pred_(0), q2_pred_(0), q3_pred_(0), - wx_prev_(0), wy_prev_(0), wz_prev_(0), - wx_bias_(0), wy_bias_(0), wz_bias_(0) { } - -ComplementaryFilter::~ComplementaryFilter() { } - -void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation) -{ - do_bias_estimation_ = do_bias_estimation; -} - -bool ComplementaryFilter::getDoBiasEstimation() const -{ - return do_bias_estimation_; -} - -void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain) -{ - do_adaptive_gain_ = do_adaptive_gain; -} - -bool ComplementaryFilter::getDoAdaptiveGain() const -{ - return do_adaptive_gain_; -} - -bool ComplementaryFilter::setGainAcc(double gain) -{ - if (gain >= 0 && gain <= 1.0) - { - gain_acc_ = gain; - return true; - } - else - return false; -} -bool ComplementaryFilter::setGainMag(double gain) -{ - if (gain >= 0 && gain <= 1.0) - { - gain_mag_ = gain; - return true; - } - else - return false; -} - -double ComplementaryFilter::getGainAcc() const -{ - return gain_acc_; -} - -double ComplementaryFilter::getGainMag() const -{ - return gain_mag_; -} - -bool ComplementaryFilter::getSteadyState() const -{ - return steady_state_; -} - -bool ComplementaryFilter::setBiasAlpha(double bias_alpha) -{ - if (bias_alpha >= 0 && bias_alpha <= 1.0) - { - bias_alpha_ = bias_alpha; - return true; - } - else - return false; -} - -double ComplementaryFilter::getBiasAlpha() const -{ - return bias_alpha_; -} - -void ComplementaryFilter::setOrientation( - double q0, double q1, double q2, double q3) -{ - // Set the state to inverse (state is fixed wrt body). - invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_); -} - - -double ComplementaryFilter::getAngularVelocityBiasX() const -{ - return wx_bias_; -} - -double ComplementaryFilter::getAngularVelocityBiasY() const -{ - return wy_bias_; -} - -double ComplementaryFilter::getAngularVelocityBiasZ() const -{ - return wz_bias_; -} - -void ComplementaryFilter::update_acc(double ax, double ay, double az) -{ - if (!initialized_) - { - // First time - ignore prediction: - getMeasurement(ax, ay, az, - q0_, q1_, q2_, q3_); - initialized_ = true; - return; - } - - // // Bias estimation. //TODO: Make new bias estimation - // if (do_bias_estimation_) - // updateBiases(ax, ay, az, wx, wy, wz); - - // Correction (from acc): - // q_ = q_pred * [(1-gain) * qI + gain * dq_acc] - // where qI = identity quaternion - double dq0_acc, dq1_acc, dq2_acc, dq3_acc; - getAccCorrection(ax, ay, az, - q0_, q1_, q2_, q3_, - dq0_acc, dq1_acc, dq2_acc, dq3_acc); - - double gain; - if (do_adaptive_gain_) - { - gain = getAdaptiveGain(gain_acc_, ax, ay, az); - - } - else - { - gain = gain_acc_; - - } - - scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc); - - quaternionMultiplication(q0_, q1_, q2_, q3_, - dq0_acc, dq1_acc, dq2_acc, dq3_acc, - q0_, q1_, q2_, q3_); - - normalizeQuaternion(q0_, q1_, q2_, q3_); -} - -void ComplementaryFilter::update_gyro(double wx, double wy, double wz, - double dt) -{ - // // Bias estimation. TODO: Add bias estimation later - // if (do_bias_estimation_) - // updateBiases(ax, ay, az, wx, wy, wz); - - // Prediction. - double q0_pred, q1_pred, q2_pred, q3_pred; - getPrediction(wx, wy, wz, dt, - q0_pred, q1_pred, q2_pred, q3_pred); - normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred); - q0_ = q0_pred; - q1_ = q1_pred; - q2_ = q2_pred; - q3_ = q3_pred; - -} - -void ComplementaryFilter::update(double ax, double ay, double az, - double wx, double wy, double wz, - double dt) -{ - if (!initialized_) - { - // First time - ignore prediction: - getMeasurement(ax, ay, az, - q0_, q1_, q2_, q3_); - initialized_ = true; - return; - } - - // Bias estimation. - if (do_bias_estimation_) - updateBiases(ax, ay, az, wx, wy, wz); - - // Prediction. - double q0_pred, q1_pred, q2_pred, q3_pred; - getPrediction(wx, wy, wz, dt, - q0_pred, q1_pred, q2_pred, q3_pred); - - // Correction (from acc): - // q_ = q_pred * [(1-gain) * qI + gain * dq_acc] - // where qI = identity quaternion - double dq0_acc, dq1_acc, dq2_acc, dq3_acc; - getAccCorrection(ax, ay, az, - q0_pred, q1_pred, q2_pred, q3_pred, - dq0_acc, dq1_acc, dq2_acc, dq3_acc); - - double gain; - if (do_adaptive_gain_) - { - gain = getAdaptiveGain(gain_acc_, ax, ay, az); - - } - else - { - gain = gain_acc_; - - } - - scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc); - - quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, - dq0_acc, dq1_acc, dq2_acc, dq3_acc, - q0_, q1_, q2_, q3_); - - normalizeQuaternion(q0_, q1_, q2_, q3_); -} - -void ComplementaryFilter::update(double ax, double ay, double az, - double wx, double wy, double wz, - double mx, double my, double mz, - double dt) -{ - if (!initialized_) - { - // First time - ignore prediction: - getMeasurement(ax, ay, az, - mx, my, mz, - q0_, q1_, q2_, q3_); - initialized_ = true; - return; - } - - // Bias estimation. - if (do_bias_estimation_) - updateBiases(ax, ay, az, wx, wy, wz); - - // Prediction. - double q0_pred, q1_pred, q2_pred, q3_pred; - getPrediction(wx, wy, wz, dt, - q0_pred, q1_pred, q2_pred, q3_pred); - - // Correction (from acc): - // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc] - // where qI = identity quaternion - double dq0_acc, dq1_acc, dq2_acc, dq3_acc; - getAccCorrection(ax, ay, az, - q0_pred, q1_pred, q2_pred, q3_pred, - dq0_acc, dq1_acc, dq2_acc, dq3_acc); - double alpha = gain_acc_; - if (do_adaptive_gain_) - alpha = getAdaptiveGain(gain_acc_, ax, ay, az); - scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc); - - double q0_temp, q1_temp, q2_temp, q3_temp; - quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, - dq0_acc, dq1_acc, dq2_acc, dq3_acc, - q0_temp, q1_temp, q2_temp, q3_temp); - - // Correction (from mag): - // q_ = q_temp * [(1-gain) * qI + gain * dq_mag] - // where qI = identity quaternion - double dq0_mag, dq1_mag, dq2_mag, dq3_mag; - getMagCorrection(mx, my, mz, - q0_temp, q1_temp, q2_temp, q3_temp, - dq0_mag, dq1_mag, dq2_mag, dq3_mag); - - scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag); - - quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp, - dq0_mag, dq1_mag, dq2_mag, dq3_mag, - q0_, q1_, q2_, q3_); - - normalizeQuaternion(q0_, q1_, q2_, q3_); -} - -bool ComplementaryFilter::checkState(double ax, double ay, double az, - double wx, double wy, double wz) const -{ - double acc_magnitude = sqrt(ax*ax + ay*ay + az*az); - if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold) - return false; - - if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold || - fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold || - fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold) - return false; - - if (fabs(wx - wx_bias_) > kAngularVelocityThreshold || - fabs(wy - wy_bias_) > kAngularVelocityThreshold || - fabs(wz - wz_bias_) > kAngularVelocityThreshold) - return false; - - return true; -} - -void ComplementaryFilter::updateBiases(double ax, double ay, double az, - double wx, double wy, double wz) -{ - steady_state_ = checkState(ax, ay, az, wx, wy, wz); - - if (steady_state_) - { - wx_bias_ += bias_alpha_ * (wx - wx_bias_); - wy_bias_ += bias_alpha_ * (wy - wy_bias_); - wz_bias_ += bias_alpha_ * (wz - wz_bias_); - } - - wx_prev_ = wx; - wy_prev_ = wy; - wz_prev_ = wz; -} - -void ComplementaryFilter::getPrediction( - double wx, double wy, double wz, double dt, - double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const -{ - double wx_unb = wx - wx_bias_; - double wy_unb = wy - wy_bias_; - double wz_unb = wz - wz_bias_; - - q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_); - q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_); - q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_); - q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_); - - normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred); -} - -void ComplementaryFilter::getMeasurement( - double ax, double ay, double az, - double mx, double my, double mz, - double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) -{ - // q_acc is the quaternion obtained from the acceleration vector representing - // the orientation of the Global frame wrt the Local frame with arbitrary yaw - // (intermediary frame). q3_acc is defined as 0. - double q0_acc, q1_acc, q2_acc, q3_acc; - - // Normalize acceleration vector. - normalizeVector(ax, ay, az); - if (az >=0) - { - q0_acc = sqrt((az + 1) * 0.5); - q1_acc = -ay/(2.0 * q0_acc); - q2_acc = ax/(2.0 * q0_acc); - q3_acc = 0; - } - else - { - double X = sqrt((1 - az) * 0.5); - q0_acc = -ay/(2.0 * X); - q1_acc = X; - q2_acc = 0; - q3_acc = ax/(2.0 * X); - } - - // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary - // frame by the inverse of q_acc. - // l = R(q_acc)^-1 m - double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx + - 2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz; - double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc + - q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz; - - // q_mag is the quaternion that rotates the Global frame (North West Up) into - // the intermediary frame. q1_mag and q2_mag are defined as 0. - double gamma = lx*lx + ly*ly; - double beta = sqrt(gamma + lx*sqrt(gamma)); - double q0_mag = beta / (sqrt(2.0 * gamma)); - double q3_mag = ly / (sqrt(2.0) * beta); - - // The quaternion multiplication between q_acc and q_mag represents the - // quaternion, orientation of the Global frame wrt the local frame. - // q = q_acc times q_mag - quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, - q0_mag, 0, 0, q3_mag, - q0_meas, q1_meas, q2_meas, q3_meas ); - //q0_meas = q0_acc*q0_mag; - //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag; - //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag; - //q3_meas = q0_acc*q3_mag; -} - - -void ComplementaryFilter::getMeasurement( - double ax, double ay, double az, - double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) -{ - // q_acc is the quaternion obtained from the acceleration vector representing - // the orientation of the Global frame wrt the Local frame with arbitrary yaw - // (intermediary frame). q3_acc is defined as 0. - - // Normalize acceleration vector. - normalizeVector(ax, ay, az); - - if (az >=0) - { - q0_meas = sqrt((az + 1) * 0.5); - q1_meas = -ay/(2.0 * q0_meas); - q2_meas = ax/(2.0 * q0_meas); - q3_meas = 0; - } - else - { - double X = sqrt((1 - az) * 0.5); - q0_meas = -ay/(2.0 * X); - q1_meas = X; - q2_meas = 0; - q3_meas = ax/(2.0 * X); - } -} - -void ComplementaryFilter::getAccCorrection( - double ax, double ay, double az, - double p0, double p1, double p2, double p3, - double& dq0, double& dq1, double& dq2, double& dq3) -{ - // Normalize acceleration vector. - normalizeVector(ax, ay, az); - - // Acceleration reading rotated into the world frame by the inverse predicted - // quaternion (predicted gravity): - double gx, gy, gz; - rotateVectorByQuaternion(ax, ay, az, - p0, -p1, -p2, -p3, - gx, gy, gz); - - // Delta quaternion that rotates the predicted gravity into the real gravity: - dq0 = sqrt((gz + 1) * 0.5); - dq1 = -gy/(2.0 * dq0); - dq2 = gx/(2.0 * dq0); - dq3 = 0.0; -} - -void ComplementaryFilter::getMagCorrection( - double mx, double my, double mz, - double p0, double p1, double p2, double p3, - double& dq0, double& dq1, double& dq2, double& dq3) -{ - // Magnetic reading rotated into the world frame by the inverse predicted - // quaternion: - double lx, ly, lz; - rotateVectorByQuaternion(mx, my, mz, - p0, -p1, -p2, -p3, - lx, ly, lz); - - // Delta quaternion that rotates the l so that it lies in the xz-plane - // (points north): - double gamma = lx*lx + ly*ly; - double beta = sqrt(gamma + lx*sqrt(gamma)); - dq0 = beta / (sqrt(2.0 * gamma)); - dq1 = 0.0; - dq2 = 0.0; - dq3 = ly / (sqrt(2.0) * beta); -} - -void ComplementaryFilter::getOrientation( - double& q0, double& q1, double& q2, double& q3) const -{ - // Return the inverse of the state (state is fixed wrt body). - invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3); -} - -double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az) -{ - double a_mag = sqrt(ax*ax + ay*ay + az*az); - double error = fabs(a_mag - kGravity)/kGravity; - double factor; - double error1 = 0.1; - double error2 = 0.2; - double m = 1.0/(error1 - error2); - double b = 1.0 - m*error1; - if (error < error1) - factor = 1.0; - else if (error < error2) - factor = m*error + b; - else - factor = 0.0; - //printf("FACTOR: %f \n", factor); - return factor*alpha; -} - -void normalizeVector(double& x, double& y, double& z) -{ - double norm = sqrt(x*x + y*y + z*z); - - x /= norm; - y /= norm; - z /= norm; -} - -void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3) -{ - double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); - q0 /= norm; - q1 /= norm; - q2 /= norm; - q3 /= norm; -} - -void invertQuaternion( - double q0, double q1, double q2, double q3, - double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv) -{ - // Assumes quaternion is normalized. - q0_inv = q0; - q1_inv = -q1; - q2_inv = -q2; - q3_inv = -q3; -} - -void scaleQuaternion( - double gain, - double& dq0, double& dq1, double& dq2, double& dq3) -{ - if (dq0 < 0.0)//0.9 - { - // Slerp (Spherical linear interpolation): - double angle = acos(dq0); - double A = sin(angle*(1.0 - gain))/sin(angle); - double B = sin(angle * gain)/sin(angle); - dq0 = A + B * dq0; - dq1 = B * dq1; - dq2 = B * dq2; - dq3 = B * dq3; - } - else - { - // Lerp (Linear interpolation): - dq0 = (1.0 - gain) + gain * dq0; - dq1 = gain * dq1; - dq2 = gain * dq2; - dq3 = gain * dq3; - } - - normalizeQuaternion(dq0, dq1, dq2, dq3); -} - -void quaternionMultiplication( - double p0, double p1, double p2, double p3, - double q0, double q1, double q2, double q3, - double& r0, double& r1, double& r2, double& r3) -{ - // r = p q - r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3; - r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2; - r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1; - r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0; -} - -void rotateVectorByQuaternion( - double x, double y, double z, - double q0, double q1, double q2, double q3, - double& vx, double& vy, double& vz) -{ - vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z; - vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z; - vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z; -} - - -} // namespace imu_tools diff --git a/firmware/src/complementary_filter.h b/firmware/src/complementary_filter.h deleted file mode 100644 index e65dd4c..0000000 --- a/firmware/src/complementary_filter.h +++ /dev/null @@ -1,189 +0,0 @@ -/* - @author Roberto G. Valenti - - @section LICENSE - Copyright (c) 2015, City University of New York - CCNY Robotics Lab - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - 3. Neither the name of the City College of New York nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL the CCNY ROBOTICS LAB BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef IMU_TOOLS_COMPLEMENTARY_FILTER_H -#define IMU_TOOLS_COMPLEMENTARY_FILTER_H - -namespace imu_tools { - -class ComplementaryFilter -{ - public: - ComplementaryFilter(); - virtual ~ComplementaryFilter(); - - bool setGainAcc(double gain); - bool setGainMag(double gain); - double getGainAcc() const; - double getGainMag() const; - - bool setBiasAlpha(double bias_alpha); - double getBiasAlpha() const; - - // When the filter is in the steady state, bias estimation will occur (if the - // parameter is enabled). - bool getSteadyState() const; - - void setDoBiasEstimation(bool do_bias_estimation); - bool getDoBiasEstimation() const; - - void setDoAdaptiveGain(bool do_adaptive_gain); - bool getDoAdaptiveGain() const; - - double getAngularVelocityBiasX() const; - double getAngularVelocityBiasY() const; - double getAngularVelocityBiasZ() const; - - // Set the orientation, as a Hamilton Quaternion, of the body frame wrt the - // fixed frame. - void setOrientation(double q0, double q1, double q2, double q3); - - // Get the orientation, as a Hamilton Quaternion, of the body frame wrt the - // fixed frame. - void getOrientation(double& q0, double& q1, double& q2, double& q3) const; - - // Update from accelerometer data. - // [ax, ay, az]: Normalized gravity vector. - // dt: time delta, in seconds. - void update_acc(double ax, double ay, double az); - - // Update from gyroscope data. - // [wx, wy, wz]: Angular veloctiy, in rad / s. - // dt: time delta, in seconds. - void update_gyro(double wx, double wy, double wz, - double dt); - - - void update(double ax, double ay, double az, - double wx, double wy, double wz, - double dt); - // Update from accelerometer, gyroscope, and magnetometer data. - // [ax, ay, az]: Normalized gravity vector. - // [wx, wy, wz]: Angular veloctiy, in rad / s. - // [mx, my, mz]: Magnetic field, units irrelevant. - // dt: time delta, in seconds. - void update(double ax, double ay, double az, - double wx, double wy, double wz, - double mx, double my, double mz, - double dt); - - private: - static const double kGravity; - static const double gamma_; - // Bias estimation steady state thresholds - static const double kAngularVelocityThreshold; - static const double kAccelerationThreshold; - static const double kDeltaAngularVelocityThreshold; - - // Gain parameter for the complementary filter, belongs in [0, 1]. - double gain_acc_; - double gain_mag_; - - // Bias estimation gain parameter, belongs in [0, 1]. - double bias_alpha_; - - // Parameter whether to do bias estimation or not. - bool do_bias_estimation_; - - // Parameter whether to do adaptive gain or not. - bool do_adaptive_gain_; - - bool initialized_; - bool steady_state_; - - // The orientation as a Hamilton quaternion (q0 is the scalar). Represents - // the orientation of the fixed frame wrt the body frame. - double q0_, q1_, q2_, q3_; - // The predicted change in orientation of the a Hamilton quaternions. - double q0_pred_, q1_pred_, q2_pred_, q3_pred_; - - // Bias in angular velocities; - double wx_prev_, wy_prev_, wz_prev_; - - // Bias in angular velocities; - double wx_bias_, wy_bias_, wz_bias_; - - void updateBiases(double ax, double ay, double az, - double wx, double wy, double wz); - - bool checkState(double ax, double ay, double az, - double wx, double wy, double wz) const; - - void getPrediction( - double wx, double wy, double wz, double dt, - double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const; - - void getMeasurement( - double ax, double ay, double az, - double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas); - - void getMeasurement( - double ax, double ay, double az, - double mx, double my, double mz, - double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas); - - void getAccCorrection( - double ax, double ay, double az, - double p0, double p1, double p2, double p3, - double& dq0, double& dq1, double& dq2, double& dq3); - - void getMagCorrection( - double mx, double my, double mz, - double p0, double p1, double p2, double p3, - double& dq0, double& dq1, double& dq2, double& dq3); - - double getAdaptiveGain(double alpha, double ax, double ay, double az); -}; - -// Utility math functions: - -void normalizeVector(double& x, double& y, double& z); - -void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3); - -void scaleQuaternion(double gain, - double& dq0, double& dq1, double& dq2, double& dq3); - -void invertQuaternion( - double q0, double q1, double q2, double q3, - double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv); - -void quaternionMultiplication(double p0, double p1, double p2, double p3, - double q0, double q1, double q2, double q3, - double& r0, double& r1, double& r2, double& r3); - -void rotateVectorByQuaternion(double x, double y, double z, - double q0, double q1, double q2, double q3, - double& vx, double& vy, double& vz); - -} // namespace imu - -#endif // IMU_TOOLS_COMPLEMENTARY_FILTER_H diff --git a/firmware/src/config.rs b/firmware/src/config.rs new file mode 100644 index 0000000..5a74943 --- /dev/null +++ b/firmware/src/config.rs @@ -0,0 +1,124 @@ +use core::cell::RefCell; + +use critical_section::Mutex; +use embedded_storage::{ReadStorage, Storage}; +use esp_storage::FlashStorage; +use heapless::{String, Vec}; +use log::{info, warn}; +use serde::{Deserialize, Serialize}; + +// 128 bytes are used to store the config as a json in flash memory +// This is not the most efficient way to store the data, but we have enough space +// and it solves a lot of problems with serialization and deserialization of old +// data when the struct changes or the microcontroller is initially flashed +const CONFIG_SIZE: usize = 128; + +#[derive(Serialize, Deserialize, Debug)] +pub struct Config { + pub bus_boudrate: u32, + pub id: u8, + pub timeout: u64, +} + +pub struct ConfigManager { + config: Mutex>, + flash: Mutex>, + flash_offset: u32, +} + +impl Config { + fn default() -> Self { + Self { + bus_boudrate: 115200, + id: 1, + timeout: 0, + } + } +} + +impl ConfigManager { + pub fn new(flash: FlashStorage, flash_offset: u32) -> Self { + let config_manager = Self { + config: Mutex::new(RefCell::new(Config::default())), + flash: Mutex::new(RefCell::new(flash)), + flash_offset, + }; + if config_manager.load_from_flash().is_err() { + warn!("Failed to load config from flash, using default values"); + config_manager.commit(); + } + config_manager + } + + // Commit the current config to flash + // TODO this currently sometimes freezes the device + // The reason is unknown, but but might be linked to + // https://github.com/esp-rs/esp-hal/issues/1714 + fn commit(&self) { + // Create a buffer with spaces + let mut buffer = [b' '; CONFIG_SIZE]; + + critical_section::with(|cs| { + // Serialize the config into a json string + let serialized = + serde_json_core::to_string::(&*self.config.borrow_ref(cs)) + .expect("Failed to serialize config"); + // Add string to padded buffer + buffer[..serialized.len()].copy_from_slice(serialized.as_bytes()); + }); + + critical_section::with(|cs| { + // Write the json to flash + self.flash + .borrow_ref_mut(cs) + .write(self.flash_offset, &buffer) + .expect("Failed to write data to flash"); + }); + } + + fn load_from_flash(&self) -> Result<(), ()> { + let mut buffer = [0u8; CONFIG_SIZE]; + + critical_section::with(|cs| { + self.flash + .borrow_ref_mut(cs) + .read(self.flash_offset, &mut buffer) + .expect("Failed to read data from flash"); + }); + + // Try to parse the data as json and deserialize it + if let Ok(data) = String::::from_utf8(Vec::from_slice(&buffer).unwrap()) { + // Log the config data + info!("Config data: {}", data); + if let Ok(config) = serde_json_core::from_str::(&data) { + critical_section::with(|cs| { + *self.config.borrow_ref_mut(cs) = config.0; + }); + return Ok(()); + } + } + Err(()) + } + + #[allow(dead_code)] + pub fn wipe(&self) { + critical_section::with(|cs| { + self.flash + .borrow_ref_mut(cs) + .write(self.flash_offset, &[0; CONFIG_SIZE]) + .expect("Failed to erase flash"); + }); + } + + pub fn get(&self, f: impl FnOnce(&Config) -> T) -> T { + critical_section::with(|cs| f(&self.config.borrow_ref(cs))) + } + + pub fn set(&self, f: impl FnOnce(&mut Config) -> T) -> T { + info!("Setting config in ram"); + let v = critical_section::with(|cs| f(&mut self.config.borrow_ref_mut(cs))); + info!("Committing config to flash"); + self.commit(); + v + } +} diff --git a/firmware/src/device.rs b/firmware/src/device.rs new file mode 100644 index 0000000..f069b29 --- /dev/null +++ b/firmware/src/device.rs @@ -0,0 +1,214 @@ +use core::{cell::RefCell, time::Duration}; + +use critical_section::Mutex; +use defmt::Debug2Format; +use dynamixel2::{Device, Instructions, ReadError, SerialPort, TransferError}; +use esp_backtrace as _; +#[cfg(feature = "profiling")] +use esp_hal::time::now; +use esp_hal::{reset, rmt::TxChannel}; +use log::{error, info, warn}; + +use crate::{ + buttons::ButtonComponent, + config::ConfigManager, + imu::IMUState, + led, + transport::{self, DynamixelSerial}, + BAUDRATE_OPTIONS, BAUDRATE_REG, BUTTON_START_REG, FIRMWARE_VERSION, ID_REG, + IMU_STATE_START_REG, LED_REG_SIZE, LED_START_REG, MODEL_NUMBER, NUM_BUTTONS, NUM_LEDS, NUM_REG, +}; + +pub fn device_loop( + transport: DynamixelSerial, + imu_state: &Mutex>, + mut led: led::LedComponent, + mut buttons: ButtonComponent, + config_manager: &ConfigManager, +) -> ! { + let mut device = Device::with_buffers(transport, [0; 200], [0; 200]) + .expect("Failed to initialize dynamixel device"); + loop { + if let Err(e) = process_packet( + &mut device, + imu_state, + &mut led, + &mut buttons, + config_manager, + ) { + error!("{:?}", Debug2Format(&e)) + } + } +} + +fn process_packet( + device: &mut Device, + imu_state: &Mutex>, + led: &mut led::LedComponent, + buttons: &mut ButtonComponent, + config_manager: &ConfigManager, +) -> Result<(), TransferError> +where + Buffer: AsRef<[u8]> + AsMut<[u8]>, +{ + let packet = device.read(Duration::from_micros(1000)); // TODO revert to 10 + + #[cfg(feature = "profiling")] + let profiling_t1 = now(); + + if matches!(&packet, + Err(ReadError::Io(e)) if DynamixelSerial::is_timeout_error(e)) + { + return Ok(()); + } + + // Bubble up the remaining errors + let packet = packet?; + + let device_id = config_manager.get(|c| c.id); + + // Only continue if the packet is for us or the broadcast id + if packet.id != device_id && packet.id != 254 { + #[cfg(feature = "profiling")] + { + let profiling_d1 = now() - profiling_t1; + info!( + "Processed packet for other device in {:?}us", + profiling_d1.to_micros() + ); + } + return Ok(()); + } + + // Handle the different instructions + match packet.instruction { + Instructions::Ping => { + // todo: this should wait for based on id for some amount of time + device.write_status(device_id, 0, 3, |buffer| { + buffer[..2].copy_from_slice(&MODEL_NUMBER.to_le_bytes()); // u16 MODEL NUMBER + buffer[2] = FIRMWARE_VERSION; //u8 FIRMWARE VERSION + Ok(()) + })?; + info!("Ping"); + } + Instructions::Read { address, length } => { + // Cast the address and length to usize + let address = address as usize; + let length = length as usize; + + // Check if the address and length are in the range of the registers + if address + length > NUM_REG { + device.write_status_error(device_id, 0x07)?; + return Ok(()); // The requested registers are out of range, but the packet was processed successfully + } + + // Get the imu state + let imu_buffer = critical_section::with(|cs| imu_state.borrow_ref(cs).to_le_buffer()); + + // Assemble the registers + let mut registers = [0; NUM_REG]; + registers[..2].copy_from_slice(&MODEL_NUMBER.to_le_bytes()); // u16 MODEL NUMBER + registers[2] = FIRMWARE_VERSION; //u8 FIRMWARE VERSION + registers[ID_REG] = config_manager.get(|c| c.id); + registers[BAUDRATE_REG] = BAUDRATE_OPTIONS + .iter() + .position(|&x| x == config_manager.get(|c| c.bus_boudrate)) + .unwrap() as u8; + registers[IMU_STATE_START_REG..IMU_STATE_START_REG + imu_buffer.len()] + .copy_from_slice(&imu_buffer); + registers[BUTTON_START_REG..BUTTON_START_REG + NUM_BUTTONS] + .copy_from_slice(&buttons.read_u8()); + + // Answer the read request + device.write_status(device_id, 0, length, |buffer| { + buffer.copy_from_slice(®isters[address..address + length]); + Ok(()) + })?; + } + Instructions::Write { + address, + parameters, + } => { + let address = address as usize; + let length = parameters.len(); + let end_address = address + length; + + match (address, end_address) { + // Set ID command + (ID_REG, const { ID_REG + 1 }) => { + config_manager.set(|c| c.id = parameters[0]); + device.write_status_ok(device_id)?; + } + // Set Boudrate command + (BAUDRATE_REG, const { BAUDRATE_REG + 1 }) => { + match BAUDRATE_OPTIONS.get(parameters[0] as usize) { + // Save the new baudrate in flash and reboot + Some(&baudrate) => { + config_manager.set(|c| c.bus_boudrate = baudrate); + device.write_status_ok(device_id).ok(); // Don't handle send error as we reboot either way + reset::software_reset(); + } + // The selected baudrate is not supported + None => { + device.write_status_error(device_id, 0x07)?; // TODO fix error codes + return Ok(()); + } + } + } + // Set LED state + ( + LED_START_REG..=const { LED_START_REG + NUM_LEDS * LED_REG_SIZE }, + LED_START_REG..=const { LED_START_REG + NUM_LEDS * LED_REG_SIZE }, + ) => { + // Update the led state + led.state[address - LED_START_REG..address - LED_START_REG + length] + .copy_from_slice(parameters); + led.send(); + device.write_status_ok(device_id)?; + } + _ => { + device.write_status_error(device_id, 0x07)?; + } + } + } + Instructions::Unknown { instruction, .. } => { + error!("Unknown instruction {:?}", instruction) + } + Instructions::Reboot => { + warn!("Reboot triggered over DXL"); + reset::software_reset(); + } + Instructions::SyncRead { + address: _, + length: _, + ids, + } => { + // Check if our id is in the list + if ids.contains(&device_id) { + warn!("The IMU does not support sync read yet"); + } + } + Instructions::SyncWrite { + address: _, + parameters: _, + length: _, + } => { + warn!("The IMU does not support sync write yet") + } + instruction_catch_all => { + warn!( + "unimplemented instruction: {:?} send to id: {}", + Debug2Format(&instruction_catch_all), + packet.id + ) + } + }; + + #[cfg(feature = "profiling")] + { + let profiling_d1 = now() - profiling_t1; + info!("Processed packet in {:?}us", profiling_d1.to_micros()); + } + + Ok(()) +} diff --git a/firmware/src/imu.rs b/firmware/src/imu.rs new file mode 100644 index 0000000..b45ff84 --- /dev/null +++ b/firmware/src/imu.rs @@ -0,0 +1,173 @@ +use core::{cell::RefCell, fmt, time::Duration}; + +use bmi088::{Accelerometer, Gyroscope}; +use critical_section::Mutex; +use esp_backtrace as _; +use esp_hal::{delay::MicrosDurationU64, time::now}; +use imu_fusion::FusionAhrsSettings; +use imu_fusion::{FusionQuaternion, FusionVector}; +use log::error; +#[cfg(feature = "profiling")] +use log::info; + +#[derive(Clone, Copy)] +pub struct IMUState { + pub orientation: FusionQuaternion, + pub gyro: FusionVector, + pub accel: FusionVector, +} + +impl IMUState { + pub fn default() -> Self { + Self { + orientation: FusionQuaternion::identity(), + gyro: FusionVector::zero(), + accel: FusionVector::zero(), + } + } + + pub fn to_le_buffer(self) -> [u8; 40] { + let mut buffer = [0; 40]; + let elements = [ + self.gyro.x, + self.gyro.y, + self.gyro.z, + self.accel.x, + self.accel.y, + self.accel.z, + self.orientation.x, + self.orientation.y, + self.orientation.z, + self.orientation.w, + ]; + for (i, element) in elements.iter().enumerate() { + let bytes = element.to_le_bytes(); + buffer[i * bytes.len()..(i + 1) * bytes.len()].copy_from_slice(&bytes); + } + buffer + } +} + +impl fmt::Debug for IMUState { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + f.debug_struct("IMUState") + .field( + "orientation", + &format_args!( + "{:.2}, {:.2}, {:.2}, {:.2}", + self.orientation.w, self.orientation.x, self.orientation.y, self.orientation.z + ), + ) + .field( + "gyro", + &format_args!("{:.2}, {:.2}, {:.2}", self.gyro.x, self.gyro.y, self.gyro.z), + ) + .field( + "accel", + &format_args!( + "{:.2}, {:.2}, {:.2}", + self.accel.x, self.accel.y, self.accel.z + ), + ) + .finish() + } +} + +pub fn filter_loop( + imu_state: &Mutex>, + mut gyro: Gyroscope>, + mut accel: Accelerometer>, +) -> ! +where + S: embedded_hal::spi::SpiDevice + embedded_hal::spi::ErrorType, +{ + // Setup the Sensor Fusion + let mut ahrs_settings = FusionAhrsSettings::new(); + ahrs_settings.gain = 0.05f32; // Default is 0.5, but we can make it more aggressive because our IMU is very good and we want little noise due to a noisy gravity vector + let mut fusion = imu_fusion::Fusion::new(crate::IMU_SAMPLE_RATE_HZ, ahrs_settings); + + // Timing stuff + let mut previous_time = now(); + + // Profiling + #[cfg(feature = "profiling")] + let mut counter = 0; + #[cfg(feature = "profiling")] + let mut start_time = now(); + + // Main sensor loop + loop { + // Get the start time + let cycle_begin = now(); + + // Get the latest Gyroscope data + let gyro_sample = match gyro.get_gyro() { + Ok(sample) => imu_fusion::FusionVector::new( + // Cast to f32 and scale to degrees per second + sample[1] as f32 / i16::MAX as f32 * crate::GYRO_RANGE, + sample[0] as f32 / i16::MAX as f32 * crate::GYRO_RANGE, + -sample[2] as f32 / i16::MAX as f32 * crate::GYRO_RANGE, + ), + Err(e) => { + error!("Failed to get gyro data: {:?}", e); + continue; + } + }; + + // Get the latest Accelerometer data + let accel_sample = match accel.get_accel() { + Ok(sample) => imu_fusion::FusionVector::new( + // Cast to f32 and scale to m/s^2 + sample[1] as f32 / i16::MAX as f32 * crate::ACCEL_RANGE * 9.81, + sample[0] as f32 / i16::MAX as f32 * crate::ACCEL_RANGE * 9.81, + -sample[2] as f32 / i16::MAX as f32 * crate::ACCEL_RANGE * 9.81, + ), + Err(e) => { + error!("Failed to get accel data: {:?}", e); + continue; + } + }; + + // Time keeping + let current_time = now(); + let delta_time = current_time - previous_time; + + // Update the filter + fusion.update_no_mag_by_duration_seconds( + gyro_sample, + accel_sample, + Duration::from_nanos(delta_time.to_nanos()).as_secs_f32(), + ); + + // Update the shared state + critical_section::with(|cs| { + imu_state.borrow(cs).replace(IMUState { + orientation: fusion.quaternion(), + gyro: gyro_sample, + accel: accel_sample, + }); + }); + + // Update the time + previous_time = current_time; + + // Delay to keep the loop rate (busy wait because the delay is not accurate enough) + let mut cycle_time = now() - cycle_begin; + while cycle_time < MicrosDurationU64::Hz(crate::IMU_SAMPLE_RATE_HZ as u64) { + cycle_time = now() - cycle_begin; + } + + // Profiling + #[cfg(feature = "profiling")] + { + counter += 1; + if counter % 1000 == 0 { + info!( + "Loop rate: {}", + 1000.0 / Duration::from_nanos((now() - start_time).to_nanos()).as_secs_f32() + ); + start_time = now(); + } + } + } +} diff --git a/firmware/src/led.rs b/firmware/src/led.rs new file mode 100644 index 0000000..447835b --- /dev/null +++ b/firmware/src/led.rs @@ -0,0 +1,30 @@ +use esp_hal::rmt::TxChannel; +use esp_hal_smartled::SmartLedsAdapter; +use smart_leds::{brightness, gamma, SmartLedsWrite, RGB8}; + +pub struct LedComponent<'a, C: TxChannel, const BUFFER_SIZE: usize> { + pub state: [u8; crate::LED_REG_SIZE * crate::NUM_LEDS], + driver: &'a mut SmartLedsAdapter, +} + +impl<'a, C: TxChannel, const BUFFER_SIZE: usize> LedComponent<'a, C, BUFFER_SIZE> { + pub fn new(driver: &'a mut SmartLedsAdapter) -> Self { + Self { + state: [0; crate::LED_REG_SIZE * crate::NUM_LEDS], + driver, + } + } + + pub fn send(&mut self) { + let state = self.state; + let colors = (0..crate::NUM_LEDS).map(|i| { + let led_index = i * crate::LED_REG_SIZE; + RGB8 { + r: state[led_index], + g: state[led_index + 1], + b: state[led_index + 2], + } + }); + self.driver.write(brightness(gamma(colors), 10)).unwrap(); + } +} diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp deleted file mode 100644 index 7a4dec6..0000000 --- a/firmware/src/main.cpp +++ /dev/null @@ -1,420 +0,0 @@ -#include - -TaskHandle_t th_dxl, th_worker; - -Preferences dxl_prefs; -Preferences imu_prefs; - -/*---------------------- DXL defines and variables ---------------------*/ -ESP32UartPortHandler uart(DXL_UART, DXL_TX_PIN, DXL_RX_PIN, DXL_DIR_PIN); - -DYNAMIXEL::Slave dxl(uart, DXL_MODEL_NUM, DXL_PROTOCOL_VER_2_0); - -// id and baud are stored using preferences library for persistance between resets of the chip -uint8_t id; -uint8_t baud; - -/*---------------------- IMU defines and variables ---------------------*/ -Bmi088Accel accel_handle(SPI, ACCEL_CS); -Bmi088Gyro gyro_handle(SPI, GYRO_CS); - -// flag triggered by interrupt to read data from IMU -volatile bool accel_drdy_flag, gyro_drdy_flag = false; - -// lists for storing measurements and filter outputs -float gyro[3] = {0, 0, 0}; -float accel[3] = {0, 0, 0}; -float quat[4] = {0, 0, 0, 0}; - -// registers for parameters of IMU -uint8_t gyro_odr, accel_odr, gyro_range, accel_range; - -// registers for parameters of complementary filter -bool do_adaptive_gain, do_bias_estimation; -float accel_gain, bias_alpha; - -imu_tools::ComplementaryFilter filter_; -int64_t last_gyro_update = esp_timer_get_time(); - -// registers for leds and buttons -CRGB leds[NUM_LEDS]; -uint8_t buttons[3]; - -void setup() -{ - FastLED.addLeds(leds, NUM_LEDS); - leds[0] = CRGB::Red; - leds[1] = CRGB::Blue; - leds[2] = CRGB::Green; - FastLED.show(); - delay(200); - leds[0] = CRGB::Black; - leds[1] = CRGB::Black; - leds[2] = CRGB::Black; - FastLED.show(); - -#ifdef DEBUG - DEBUG_SERIAL.begin(115200, SERIAL_8N1, 3, 1); -#endif - - disableCore0WDT(); // required since we dont want FreeRTOS to slow down our reading if the Wachdogtimer (WTD) fires - disableCore1WDT(); - - xTaskCreatePinnedToCore(task_dxl, "TaskDXL", 65536, NULL, 3, &th_dxl, 0); - xTaskCreatePinnedToCore(task_imu, "TaskWork", 65536, NULL, 3, &th_worker, 1); -} - -void loop() -{ - // see tasks :D -} - -/*---------------------- DXL ---------------------*/ -void task_dxl(void *pvParameters) -{ - (void)pvParameters; - dxl_prefs.begin("dxl"); - if (dxl_prefs.getUChar("init") != 42) // check if prefs are initialized - { - dxl_prefs.putUChar("id", DEFAULT_ID); - dxl_prefs.putUChar("baud", DEFAULT_BAUD); - dxl_prefs.putUChar("init", 42); // set initialized - } - - id = dxl_prefs.getUChar("id"); - baud = dxl_prefs.getUChar("baud"); - - dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0); - dxl.setFirmwareVersion(1); - dxl.setID(id); - - // dxl.addControlItem(ADDR_CONTROL_ITEM_ID, id); // not allowed since already implemented in library as default item - dxl.addControlItem(ADDR_CONTROL_ITEM_BAUD, baud); - - dxl.addControlItem(ADDR_CONTROL_ITEM_LED0_R, leds[0].r); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED0_G, leds[0].g); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED0_B, leds[0].b); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED1_R, leds[1].r); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED1_G, leds[1].g); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED1_B, leds[1].b); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED2_R, leds[2].r); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED2_G, leds[2].g); - dxl.addControlItem(ADDR_CONTROL_ITEM_LED2_B, leds[2].b); - - dxl.addControlItem(ADDR_CONTROL_ITEM_GYRO_X, gyro[0]); // TODO: 16 bit anstatt float - dxl.addControlItem(ADDR_CONTROL_ITEM_GYRO_Y, gyro[1]); - dxl.addControlItem(ADDR_CONTROL_ITEM_GYRO_Z, gyro[2]); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_X, accel[0]); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_Y, accel[1]); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_Z, accel[2]); - dxl.addControlItem(ADDR_CONTROL_ITEM_QUAT_X, quat[0]); - dxl.addControlItem(ADDR_CONTROL_ITEM_QUAT_Y, quat[1]); - dxl.addControlItem(ADDR_CONTROL_ITEM_QUAT_Z, quat[2]); - dxl.addControlItem(ADDR_CONTROL_ITEM_QUAT_W, quat[3]); - - dxl.addControlItem(ADDR_CONTROL_ITEM_BUTTON0, buttons[0]); - dxl.addControlItem(ADDR_CONTROL_ITEM_BUTTON1, buttons[1]); - dxl.addControlItem(ADDR_CONTROL_ITEM_BUTTON2, buttons[2]); - - dxl.addControlItem(ADDR_CONTROL_ITEM_GYRO_ODR, gyro_odr); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_ODR, accel_odr); - dxl.addControlItem(ADDR_CONTROL_ITEM_GYRO_RANGE, gyro_range); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_RANGE, accel_range); - - dxl.addControlItem(ADDR_CONTROL_ITEM_DO_ADAPTIVE_GAIN, do_adaptive_gain); - dxl.addControlItem(ADDR_CONTROL_ITEM_DO_BIAS_ESTIMATION, do_bias_estimation); - dxl.addControlItem(ADDR_CONTROL_ITEM_ACCEL_GAIN, accel_gain); - dxl.addControlItem(ADDR_CONTROL_ITEM_BIAS_ALPHA, bias_alpha); - - dxl.setWriteCallbackFunc(write_callback_func); - - pinMode(DXL_DIR_PIN, OUTPUT); - digitalWrite(DXL_DIR_PIN, LOW); - - uart.setBaudRate(dxl_to_real_baud(baud)); - uart.begin(); - - for (;;) - { - if (dxl.processPacket()) - { - if (dxl.getID() != id) // since we cant add the id as a control item, we need to check if it has been updated manually - { - id = dxl.getID(); - dxl_prefs.putUChar("id", id); - } - } -#ifdef DEBUG - else - { - DEBUG_SERIAL.print("Last lib err code: "); - DEBUG_SERIAL.print(dxl.getLastLibErrCode()); - DEBUG_SERIAL.print(", "); - DEBUG_SERIAL.print("Last status packet err code: "); - DEBUG_SERIAL.println(dxl.getLastStatusPacketError()); - DEBUG_SERIAL.println(); - } -#endif - } -} - -void write_callback_func(uint16_t item_addr, uint8_t &dxl_err_code, void *arg) -{ - (void)dxl_err_code, (void)arg; - if (item_addr == ADDR_CONTROL_ITEM_BAUD) - { - dxl_prefs.putUChar("baud", baud); - ESP.restart(); // restart whole chip since restarting it is easier - } - else if (item_addr == ADDR_CONTROL_ITEM_GYRO_ODR) - { - setGyroOdr(gyro_handle, gyro_odr); - imu_prefs.putUChar("gyro_odr", gyro_odr); - } - else if (item_addr == ADDR_CONTROL_ITEM_ACCEL_ODR) - { - setAccelOdr(accel_handle, accel_odr); - imu_prefs.putUChar("accel_odr", accel_odr); - } - else if (item_addr == ADDR_CONTROL_ITEM_GYRO_RANGE) - { - setGyroRange(gyro_handle, gyro_range); - imu_prefs.putUChar("gyro_range", gyro_range); - } - else if (item_addr == ADDR_CONTROL_ITEM_ACCEL_RANGE) - { - setAccelRange(accel_handle, accel_range); - imu_prefs.putUChar("accel_range", accel_range); - } - else if (item_addr == ADDR_CONTROL_ITEM_DO_ADAPTIVE_GAIN) - { - imu_prefs.putUChar("adaptive_gain", do_adaptive_gain); - filter_.setDoAdaptiveGain(do_adaptive_gain); - } - else if (item_addr == ADDR_CONTROL_ITEM_DO_BIAS_ESTIMATION) - { - imu_prefs.putUChar("bias_estimation", do_bias_estimation); - filter_.setDoBiasEstimation(do_bias_estimation); - } - else if (item_addr == ADDR_CONTROL_ITEM_ACCEL_GAIN) - { - imu_prefs.putFloat("gain_accel", accel_gain); - filter_.setGainAcc(accel_gain); - } - else if (item_addr == ADDR_CONTROL_ITEM_BIAS_ALPHA) - { - imu_prefs.putFloat("bias_alpha", bias_alpha); - filter_.setBiasAlpha(bias_alpha); - } - else if (item_addr >= ADDR_CONTROL_ITEM_LED0_R && item_addr <= ADDR_CONTROL_ITEM_LED2_B) - { - FastLED.show(); - } -} - -/*---------------------- IMU ---------------------*/ - -void IRAM_ATTR accel_drdy_int() // IRAM_ATTR puts this function into ram, required since it is called as an interrupt -{ - accel_drdy_flag = true; -} - -void IRAM_ATTR gyro_drdy_int() // IRAM_ATTR puts this function into ram, required since it is called as an interrupt -{ - gyro_drdy_flag = true; -} - -void task_imu(void *pvParameters) -{ - SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI); - - (void)pvParameters; - dxl.addControlItem(ADDR_CONTROL_ITEM_BUTTON2, buttons[2]); - imu_prefs.begin("imu"); - if (imu_prefs.getUChar("init_imu") != 42) // check if prefs are initialized - { - imu_prefs.putUChar("accel_odr", ACCEL_ODR_DEFAULT); - imu_prefs.putUChar("gyro_odr", GYRO_ODR_DEFAULT); - imu_prefs.putUChar("accel_range", ACCEL_RANGE_DEFAULT); - imu_prefs.putUChar("gyro_range", GYRO_RANGE_DEFAULT); - imu_prefs.putUChar("adaptive_gain", IMU_DO_ADAPTIVE_GAIN_DEFAULT); - imu_prefs.putUChar("bias_estimation", IMU_DO_BIAS_ESTIMATION_DEFAULT); - imu_prefs.putFloat("gain_accel", IMU_GAIN_ACCEL_DEFAULT); - imu_prefs.putFloat("bias_alpha", IMU_BIAS_ALPHA_DEFAULT); - imu_prefs.putUChar("init_imu", 42); // set initialized - } - - // setup filter with defaults - do_adaptive_gain = imu_prefs.getUChar("adaptive_gain"); - accel_gain = imu_prefs.getFloat("gain_accel"); - do_bias_estimation = imu_prefs.getUChar("do_bias_estimation"); - bias_alpha = imu_prefs.getFloat("bias_alpha"); - - filter_.setDoAdaptiveGain(do_adaptive_gain); - filter_.setGainAcc(accel_gain); - filter_.setDoBiasEstimation(do_bias_estimation); - filter_.setBiasAlpha(bias_alpha); - - // setup buttons - pinMode(BUTTON0_PIN, INPUT_PULLUP); - pinMode(BUTTON1_PIN, INPUT_PULLUP); - pinMode(BUTTON2_PIN, INPUT_PULLUP); - - int accel_status = accel_handle.begin(); - int gyro_status = gyro_handle.begin(); - -#ifdef DEBUG - DEBUG_SERIAL.print("Accel status: "); - DEBUG_SERIAL.println(accel_status); - DEBUG_SERIAL.print("Gyro status: "); - DEBUG_SERIAL.println(gyro_status); -#endif - - if (accel_status == 1 && gyro_status == 1) - { - leds[2] = CRGB::Green; - } - else if (accel_status != 1) - { - leds[1] = CRGB::Red; - } - else if (gyro_status != 1) - { - leds[2] = CRGB::Red; - } - FastLED.show(); - - gyro_range = imu_prefs.getUChar("gyro_range"); - setGyroRange(gyro_handle, gyro_range); - accel_range = imu_prefs.getUChar("accel_range"); - setAccelRange(accel_handle, accel_range); - - accel_odr = imu_prefs.getUChar("accel_odr"); - setAccelOdr(accel_handle, accel_odr); - gyro_odr = imu_prefs.getUChar("gyro_odr"); - setGyroOdr(gyro_handle, gyro_odr); - - // initialize interrups for imu gyro and accel - accel_handle.pinModeInt1(Bmi088Accel::PUSH_PULL, Bmi088Accel::ACTIVE_HIGH); - accel_handle.mapDrdyInt1(true); - - gyro_handle.pinModeInt3(Bmi088Gyro::PUSH_PULL, Bmi088Gyro::ACTIVE_HIGH); - gyro_handle.mapDrdyInt3(true); - - pinMode(INT_ACCEL, INPUT); - attachInterrupt(INT_ACCEL, accel_drdy_int, RISING); - pinMode(INT_GYRO, INPUT); - attachInterrupt(INT_GYRO, gyro_drdy_int, RISING); - - for (;;) - { - buttons[0] = !digitalRead(BUTTON0_PIN); - buttons[1] = !digitalRead(BUTTON1_PIN); - buttons[2] = !digitalRead(BUTTON2_PIN); - - // synchronized reading requires setting up the interrupt of one sensor triggering the other - // if required this can be implemented in the future as in the example in the BMI088 library - bool synchronized_read = false; - if (synchronized_read) - { - if (accel_drdy_flag && gyro_drdy_flag) - { - int64_t current_update_time = esp_timer_get_time(); - float dt = (float)(last_gyro_update - current_update_time) / 1e6; - last_gyro_update = current_update_time; - - gyro_handle.readSensor(); - gyro_drdy_flag = false; - - accel_handle.readSensor(); - accel_drdy_flag = false; - - float tmp_gyro[3]; - tmp_gyro[0] = gyro_handle.getGyroX_rads(); - tmp_gyro[1] = gyro_handle.getGyroY_rads(); - tmp_gyro[2] = gyro_handle.getGyroZ_rads(); - - float tmp_accel[3]; - tmp_accel[0] = accel_handle.getAccelX_mss(); - tmp_accel[1] = accel_handle.getAccelY_mss(); - tmp_accel[2] = accel_handle.getAccelZ_mss(); - - filter_.update(tmp_accel[0], tmp_accel[1], tmp_accel[2], tmp_gyro[0], tmp_gyro[1], tmp_gyro[2], dt); - - if (isnan(tmp_gyro[0]) || isnan(tmp_gyro[1]) || isnan(tmp_gyro[2])) - return; - gyro[0] = tmp_gyro[0]; - gyro[1] = tmp_gyro[1]; - gyro[2] = tmp_gyro[2]; - - if (isnan(tmp_accel[0]) || isnan(tmp_accel[1]) || isnan(tmp_accel[2])) - continue; - accel[0] = tmp_accel[0]; - accel[1] = tmp_accel[1]; - accel[2] = tmp_accel[2]; - - double q0, q1, q2, q3; - filter_.getOrientation(q0, q1, q2, q3); - quat[0] = q1; - quat[1] = q2; - quat[2] = q3; - quat[3] = q0; - } - } - else - { // not synchronized - if (accel_drdy_flag) - { - accel_drdy_flag = false; - accel_handle.readSensor(); - float tmp_accel[3]; - tmp_accel[0] = accel_handle.getAccelX_mss(); - tmp_accel[1] = accel_handle.getAccelY_mss(); - tmp_accel[2] = accel_handle.getAccelZ_mss(); - if (isnan(tmp_accel[0]) || isnan(tmp_accel[1]) || isnan(tmp_accel[2])) - return; - - accel[0] = tmp_accel[0]; - accel[1] = tmp_accel[1]; - accel[2] = tmp_accel[2]; - - filter_.update_acc(accel[0], accel[1], accel[2]); - - double q0, q1, q2, q3; - filter_.getOrientation(q0, q1, q2, q3); // hamilton to ros quaternion - quat[0] = q1; - quat[1] = q2; - quat[2] = q3; - quat[3] = q0; - } - if (gyro_drdy_flag) - { - gyro_drdy_flag = false; - - gyro_handle.readSensor(); - float tmp_gyro[3]; - tmp_gyro[0] = gyro_handle.getGyroX_rads(); - tmp_gyro[1] = gyro_handle.getGyroY_rads(); - tmp_gyro[2] = gyro_handle.getGyroZ_rads(); - - if (isnan(tmp_gyro[0]) || isnan(tmp_gyro[1]) || isnan(tmp_gyro[2])) - return; - - gyro[0] = tmp_gyro[0]; - gyro[1] = tmp_gyro[1]; - gyro[2] = tmp_gyro[2]; - - int64_t current_update_time = esp_timer_get_time(); - float dt = (float)(current_update_time - last_gyro_update) / 1e6; - last_gyro_update = current_update_time; - filter_.update_gyro(gyro[0], gyro[1], gyro[2], dt); - double q0, q1, q2, q3; - filter_.getOrientation(q0, q1, q2, q3); // hamilton to ros quaternion - quat[0] = q1; - quat[1] = q2; - quat[2] = q3; - quat[3] = q0; - } - } - } -} diff --git a/firmware/src/main.h b/firmware/src/main.h deleted file mode 100644 index 799d7e1..0000000 --- a/firmware/src/main.h +++ /dev/null @@ -1,101 +0,0 @@ -#include -#include -#include -#include -#include "BMI088.h" -#include "complementary_filter.h" -#include "uart_port_handler.h" -#include "utilities.h" - -#define DEBUG false - -// Pin definitions -#define DXL_DIR_PIN 22 -#define DXL_RX_PIN 21 -#define DXL_TX_PIN 23 - -#define ACCEL_CS 26 -#define GYRO_CS 18 - -#define SPI_MOSI 5 -#define SPI_MISO 17 -#define SPI_SCK 19 -#define INT_ACCEL 25 -#define INT_GYRO 16 - -#define LED_PIN 27 -#define BUTTON0_PIN 2 -#define BUTTON1_PIN 32 -#define BUTTON2_PIN 4 - -// UART definitions -#define DXL_UART UART_NUM_0 -#define DEBUG_SERIAL Serial1 - -// Dynamixel definitions -#define DXL_PROTOCOL_VER_2_0 2.0 -#define DXL_MODEL_NUM 0xbaff -#define DEFAULT_ID 241 -#define DEFAULT_BAUD 3 // 1mbaud - -#define ADDR_CONTROL_ITEM_BAUD 8 - -#define ADDR_CONTROL_ITEM_LED0_R 10 -#define ADDR_CONTROL_ITEM_LED0_G 11 -#define ADDR_CONTROL_ITEM_LED0_B 12 -#define ADDR_CONTROL_ITEM_LED1_R 14 -#define ADDR_CONTROL_ITEM_LED1_G 15 -#define ADDR_CONTROL_ITEM_LED1_B 16 -#define ADDR_CONTROL_ITEM_LED2_R 18 -#define ADDR_CONTROL_ITEM_LED2_G 19 -#define ADDR_CONTROL_ITEM_LED2_B 20 - -#define ADDR_CONTROL_ITEM_GYRO_X 36 -#define ADDR_CONTROL_ITEM_GYRO_Y 40 -#define ADDR_CONTROL_ITEM_GYRO_Z 44 -#define ADDR_CONTROL_ITEM_ACCEL_X 48 -#define ADDR_CONTROL_ITEM_ACCEL_Y 52 -#define ADDR_CONTROL_ITEM_ACCEL_Z 56 -#define ADDR_CONTROL_ITEM_QUAT_X 60 -#define ADDR_CONTROL_ITEM_QUAT_Y 64 -#define ADDR_CONTROL_ITEM_QUAT_Z 68 -#define ADDR_CONTROL_ITEM_QUAT_W 72 - -#define ADDR_CONTROL_ITEM_BUTTON0 76 -#define ADDR_CONTROL_ITEM_BUTTON1 77 -#define ADDR_CONTROL_ITEM_BUTTON2 78 - -#define ADDR_CONTROL_ITEM_GYRO_ODR 100 -#define ADDR_CONTROL_ITEM_ACCEL_ODR 101 - -#define ADDR_CONTROL_ITEM_GYRO_RANGE 102 -#define ADDR_CONTROL_ITEM_ACCEL_RANGE 103 - -#define ADDR_CONTROL_ITEM_DO_ADAPTIVE_GAIN 108 -#define ADDR_CONTROL_ITEM_DO_BIAS_ESTIMATION 109 -#define ADDR_CONTROL_ITEM_ACCEL_GAIN 110 -#define ADDR_CONTROL_ITEM_BIAS_ALPHA 114 - - -// default parameters for BMI088 -#define ACCEL_RANGE_DEFAULT 1 // Bmi088Accel::RANGE_6G -#define ACCEL_ODR_DEFAULT 15 // Bmi088Accel::ODR_400HZ_BW_40HZ -#define GYRO_RANGE_DEFAULT 3 // Bmi088Gyro::RANGE_1000DPS -#define GYRO_ODR_DEFAULT 5 // Bmi088Gyro::ODR_1000HZ_BW_116HZ - -// default parameters for complementary filter -#define IMU_GAIN_ACCEL_DEFAULT 0.04 -#define IMU_DO_ADAPTIVE_GAIN_DEFAULT false -#define IMU_DO_BIAS_ESTIMATION_DEFAULT false // Not working as we are not reading synchroneously -#define IMU_BIAS_ALPHA_DEFAULT 0.01 - -#define NUM_LEDS 3 - -// function definitions -void task_dxl(void *pvParameters); -void task_imu(void *pvParameters); - -void write_callback_func(uint16_t item_addr, uint8_t &dxl_err_code, void *arg); - -void setAccelRange(uint8_t range); -void setGyroRange(uint8_t range); \ No newline at end of file diff --git a/firmware/src/main.rs b/firmware/src/main.rs new file mode 100644 index 0000000..bbb7b7b --- /dev/null +++ b/firmware/src/main.rs @@ -0,0 +1,196 @@ +#![no_std] +#![no_main] +#![feature(inline_const_pat)] + +mod buttons; +mod config; +mod device; +mod imu; +mod led; +mod transport; + +use core::{cell::RefCell, ptr::addr_of_mut}; + +use config::ConfigManager; +use critical_section::Mutex; +use embedded_hal_bus::{spi::AtomicDevice, util::AtomicCell}; +use esp_backtrace as _; +use esp_hal::{ + clock::CpuClock, + cpu_control::{CpuControl, Stack}, + delay::Delay, + gpio::{Level, Output}, + handler, + interrupt::InterruptConfigurable, + main, reset, + rmt::Rmt, + spi::master::{Config as SpiConfig, Spi}, + time::RateExtU32, + uart::{Config as UartConfig, Uart, UartInterrupt}, +}; +use esp_hal_smartled::{smartLedBuffer, SmartLedsAdapter}; +use esp_storage::FlashStorage; +use heapless::Deque; +use log::{error, info}; + +use crate::buttons::ButtonComponent; +use crate::imu::IMUState; +use crate::led::LedComponent; +use crate::transport::DynamixelSerial; + +static mut APP_CORE_STACK: Stack<8192> = Stack::new(); + +static SERIAL: Mutex>>> = Mutex::new(RefCell::new(None)); + +static RX_QUEUE: Mutex>> = Mutex::new(RefCell::new(Deque::new())); + +const GYRO_RANGE: f32 = 2000.0; // 2000 degrees per second +const ACCEL_RANGE: f32 = 6.0; // 6 G + +const IMU_SAMPLE_RATE_HZ: u32 = 400; // Check if imu is also in the 400 Hz mode + +const NUM_REG: usize = 128; +const ID_REG: usize = 7; +const BAUDRATE_REG: usize = 8; +const BAUDRATE_OPTIONS: [u32; 7] = [ + 9600, 57600, 115_200, 1_000_000, 2_000_000, 3_000_000, 4_000_000, +]; +const LED_START_REG: usize = 10; +const LED_REG_SIZE: usize = 4; +const NUM_LEDS: usize = 3; +const IMU_STATE_START_REG: usize = 36; +const NUM_BUTTONS: usize = 3; +const BUTTON_START_REG: usize = 76; + +const MODEL_NUMBER: u16 = 0xBAFF; +const FIRMWARE_VERSION: u8 = 1; + +#[main] +fn main() -> ! { + let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max()); + let peripherals = esp_hal::init(config); + + esp_println::logger::init_logger_from_env(); + info!("Starting up"); + + // Store simple data in flash + info!("Loading config from persistent storage"); + let flash = FlashStorage::new(); + let config_manager = ConfigManager::new(flash, 30); + + // Setup UART communication + info!("Setting up UART communication"); + let mut uart: Uart<'_, esp_hal::Blocking> = Uart::new( + peripherals.UART2, + UartConfig::default() + .with_baudrate(config_manager.get(|c| c.bus_boudrate)) + .with_rx_fifo_full_threshold(1), + ) + .expect("Failed to initialize UART controller") + .with_rx(peripherals.GPIO21) + .with_tx(peripherals.GPIO23); + // Setup UART interrupt + uart.set_interrupt_handler(uart_handler); + critical_section::with(|cs| { + uart.listen(UartInterrupt::RxFifoFull); + SERIAL.borrow_ref_mut(cs).replace(uart); + }); + // Setup UART dir pin + let mut dir_pin = Output::new(peripherals.GPIO22, Level::Low); + + // Setup the transport layer for the dynamixel communication + info!("Setting up dynamixel communication layer"); + let transport = DynamixelSerial::new( + &SERIAL, + config_manager.get(|c| c.bus_boudrate), + &mut dir_pin, + ); + + // Setup LEDs + let rmt = Rmt::new(peripherals.RMT, 80.MHz()).unwrap(); + let mut led_driver = + SmartLedsAdapter::new(rmt.channel0, peripherals.GPIO27, smartLedBuffer!(3)); + let led = LedComponent::new(&mut led_driver); + + // Setup Buttons + let button_driver = + ButtonComponent::new(peripherals.GPIO2, peripherals.GPIO32, peripherals.GPIO4); + + // Setup the IMU Device + info!("Setting up IMU"); + + let spi = AtomicCell::new( + Spi::new(peripherals.SPI2, SpiConfig::default()) + .unwrap() + .with_sck(peripherals.GPIO19) + .with_mosi(peripherals.GPIO5) + .with_miso(peripherals.GPIO17), + ); + + let mut delay = Delay::new(); + + let mut gyro_device = bmi088::Builder::new_gyro_spi( + AtomicDevice::new(&spi, Output::new(peripherals.GPIO18, Level::High), delay).unwrap(), + ); + gyro_device.setup(&mut delay).unwrap(); + + let mut accel_device = bmi088::Builder::new_accel_spi( + AtomicDevice::new(&spi, Output::new(peripherals.GPIO26, Level::High), delay).unwrap(), + ); + accel_device.setup(&mut delay).unwrap(); + + let imu_state = Mutex::new(RefCell::new(IMUState::default())); + + info!("Setting up secondary core"); + let mut cpu_control = CpuControl::new(peripherals.CPU_CTRL); + // This is needed to the imu_state is borrowed in the closure + let imu_state_ref = &imu_state; + // Move everything into the closure + let core_2_closure = move || { + imu::filter_loop(imu_state_ref, gyro_device, accel_device); + }; + // Start the secondary core + let _guard = cpu_control + .start_app_core( + unsafe { &mut *addr_of_mut!(APP_CORE_STACK) }, + core_2_closure, + ) + .unwrap(); + + // Spin + device::device_loop(transport, &imu_state, led, button_driver, &config_manager); +} + +/// Reboot the device after the panic was displayed. +#[no_mangle] +pub extern "Rust" fn custom_halt() -> ! { + error!("The chip will restart shortly!"); + // Wait so the error is definatly flushed and visible in the terminal + let delay = Delay::new(); + delay.delay_millis(1000); + // Reset the chip, maybe the error is gone :D + reset::software_reset(); + unreachable!(); +} + +#[handler] +fn uart_handler() { + critical_section::with(|cs| { + // Get/Lock UART interface + let mut serial = SERIAL.borrow_ref_mut(cs); + let serial: &mut Uart<'_, esp_hal::Blocking> = serial.as_mut().unwrap(); + + // Copy bytes from uart into our queue + let mut buf = [0u8; 64]; + if let Ok(num_bytes) = serial.read_buffered_bytes(&mut buf) { + // Push the bytes to the queue + let mut queue = RX_QUEUE.borrow_ref_mut(cs); + for &byte in buf.iter().take(num_bytes) { + queue.push_back(byte).ok(); + } + } + + // We processed this interrupt, so we can clear it + serial.clear_interrupts(UartInterrupt::RxFifoFull.into()); + }); +} diff --git a/firmware/src/transport.rs b/firmware/src/transport.rs new file mode 100644 index 0000000..c3b01c2 --- /dev/null +++ b/firmware/src/transport.rs @@ -0,0 +1,105 @@ +use core::{cell::RefCell, time::Duration}; + +use critical_section::Mutex; +use dynamixel2::SerialPort; +use embedded_io::Write; +use esp_hal::{ + gpio::Output, + time::{now, ExtU64, Instant}, + uart::{Error as UartError, Uart}, +}; + +#[derive(Debug)] +#[allow(dead_code)] +pub enum Error { + UartRead(UartError), // TODO look at error handling + UartWrite(UartError), + Timeout, +} +pub struct DynamixelSerial<'d, 'e> { + serial: &'d Mutex>>>, + baud_rate: u32, + dir: &'e mut Output<'e>, +} + +impl<'d, 'e> DynamixelSerial<'d, 'e> { + pub fn new( + serial: &'d Mutex>>>, + baud_rate: u32, + dir: &'e mut Output<'e>, + ) -> Self { + // Set direction pin to low just to be sure + dir.set_low(); + // Initialize the struct + Self { + serial, + baud_rate, + dir, + } + } +} + +impl SerialPort for DynamixelSerial<'_, '_> { + type Error = Error; + type Instant = Instant; + + fn baud_rate(&self) -> Result { + Ok(self.baud_rate) + } + + fn set_baud_rate(&mut self, _baud_rate: u32) -> Result<(), Self::Error> { + panic!("Changing baud rate is not supported"); + } + + fn discard_input_buffer(&mut self) -> Result<(), Self::Error> { + Ok(()) + } + + fn read(&mut self, buffer: &mut [u8], deadline: &Self::Instant) -> Result { + while deadline > &now() { + let num_bytes_read = critical_section::with(|cs| { + // Lock queue + let mut queue = crate::RX_QUEUE.borrow_ref_mut(cs); + + // Copy data from queue to buffer + if let Some(byte) = queue.pop_front() { + buffer[0] = byte; + return 1; + } + 0 + }); + if num_bytes_read > 0 { + return Ok(num_bytes_read); + } + } + Err(Error::Timeout) + } + + fn write_all(&mut self, buffer: &[u8]) -> Result<(), Self::Error> { + critical_section::with(|cs| { + self.dir.set_high(); + let mut serial = self.serial.borrow_ref_mut(cs); + let serial = serial.as_mut().unwrap(); + + // Encapsulate write and flush together + // That way we can reset the dir pin even if they fail + let result = || -> Result<(), UartError> { + serial.write_all(buffer)?; + Write::flush(serial) + }() + .map_err(Error::UartWrite); + + self.dir.set_low(); + result + }) + } + + fn make_deadline(&self, timeout: Duration) -> Self::Instant { + let timeout: u64 = timeout.as_millis() as u64; + now() + timeout.millis() + } + + fn is_timeout_error(error: &Self::Error) -> bool { + matches!(error, Error::Timeout) + } +} diff --git a/firmware/src/uart_port_handler.cpp b/firmware/src/uart_port_handler.cpp deleted file mode 100644 index f5bc47b..0000000 --- a/firmware/src/uart_port_handler.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "uart_port_handler.h" - -static intr_handle_t uart_isr_intr_handle; - -void IRAM_ATTR static_uart_isr(void *arg) -{ - ESP32UartPortHandler *uart_port = (ESP32UartPortHandler *)arg; - - uart_dev_t *uart_dev = ESP32_UART_DEVICES[uart_port->uart_num_]; - - uint16_t status = uart_dev->int_st.val; - uint16_t rx_fifo_len = uart_dev->status.rxfifo_cnt; - - for (int i = 0; i < rx_fifo_len; i++) - { // evtl eine while schleife mit der unteren bedingung - uart_port->buffer_[uart_port->write_pointer_] = uart_dev->fifo.rw_byte; - uart_port->write_pointer_ = (uart_port->write_pointer_ + 1) % UART_BUFFER_SIZE; - } - while (uart_dev->status.rxfifo_cnt || (uart_dev->mem_rx_status.wr_addr != uart_dev->mem_rx_status.rd_addr)) - { - uart_dev->fifo.rw_byte; - } - uart_clear_intr_status(uart_port->uart_num_, status); -} - -ESP32UartPortHandler::ESP32UartPortHandler(uint8_t uart_num, uint8_t tx_pin, uint8_t rx_pin, uint8_t dir_pin) -{ - uart_num_ = uart_num; - tx_pin_ = tx_pin; - rx_pin_ = rx_pin; - dxl_dir_pin_ = dir_pin; // TODO HW Flow control -} - -void ESP32UartPortHandler::setBaudRate(int baud_rate) -{ - baud_rate_ = baud_rate; -} - -void ESP32UartPortHandler::begin() -{ - - const uart_config_t uart_config = { - .baud_rate = baud_rate_, - .data_bits = UART_DATA_8_BITS, - .parity = UART_PARITY_DISABLE, - .stop_bits = UART_STOP_BITS_1, - .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, - .source_clk = UART_SCLK_APB, - }; - const uart_intr_config_t uart_intr_config_struct = { - .intr_enable_mask = UART_RXFIFO_FULL_INT_ENA | UART_RXFIFO_TOUT_INT_ENA_M, - .rx_timeout_thresh = 1, - .txfifo_empty_intr_thresh = 1, - .rxfifo_full_thresh = 1, - }; - - uart_driver_install(uart_num_, 256 * 2, 0, 0, NULL, 0); - uart_param_config(uart_num_, &uart_config); - uart_intr_config(uart_num_, &uart_intr_config_struct); - uart_set_pin(uart_num_, tx_pin_, rx_pin_, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); - // uart_set_mode(uart_num_, UART_MODE_RS485_HALF_DUPLEX); - uart_disable_tx_intr(uart_num_); - uart_disable_rx_intr(uart_num_); - uart_isr_free(uart_num_); - uart_isr_register(uart_num_, &static_uart_isr, this, ESP_INTR_FLAG_IRAM, &uart_isr_intr_handle); - uart_enable_rx_intr(uart_num_); - setOpenState(true); -} - -void ESP32UartPortHandler::end() -{ // not tested, probably never going to be called - uart_disable_rx_intr(uart_num_); - uart_isr_free(uart_num_); - uart_driver_delete(uart_num_); -} - -int ESP32UartPortHandler::available() -{ - if (write_pointer_ >= read_pointer_) - { - return write_pointer_ - read_pointer_; - } - else - { - return UART_BUFFER_SIZE - read_pointer_ + write_pointer_; - } -} - -int ESP32UartPortHandler::read() -{ - uint8_t data = buffer_[read_pointer_]; - read_pointer_ = (read_pointer_ + 1) % UART_BUFFER_SIZE; - return data; -} - -size_t ESP32UartPortHandler::write(uint8_t byte) -{ - return uart_write_bytes(uart_num_, (const char *)&byte, 1); -} -size_t ESP32UartPortHandler::write(uint8_t *packet, size_t length) -{ - digitalWrite(dxl_dir_pin_, HIGH); - int num_bytes = uart_write_bytes(uart_num_, packet, length); - while (ESP32_UART_DEVICES[uart_num_]->status.st_utx_out != 0) - { - // wait for the fifo to be empty - } - // - /*while(ESP32_UART_DEVICES[uart_num_]->status.txfifo_cnt > 0) { - // wait for the fifo to be empty - }*/ - digitalWrite(dxl_dir_pin_, LOW); - return num_bytes; -} - -int ESP32UartPortHandler::getNumItr() -{ - return intr_cnt; -} diff --git a/firmware/src/uart_port_handler.h b/firmware/src/uart_port_handler.h deleted file mode 100644 index 0242c6d..0000000 --- a/firmware/src/uart_port_handler.h +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include -#include -#include - -#ifndef ESP32UARTPORTHANDLER_H -#define ESP32UARTPORTHANDLER_H - -#define UART_BUFFER_SIZE 1024 - -static uart_dev_t *ESP32_UART_DEVICES[] = {&UART0, &UART1, &UART2}; - -class ESP32UartPortHandler : public DXLPortHandler -{ -public: - ESP32UartPortHandler(uint8_t uart_num, uint8_t tx_pin, uint8_t rx_pin, uint8_t dir_pin); - void begin(); - void end(); - int available(); - int read(); - size_t write(uint8_t byte); - size_t write(uint8_t *packet, size_t length); - int getNumItr(); - void setBaudRate(int baud_rate); - - uint8_t buffer_[UART_BUFFER_SIZE] = {0}; - uint8_t uart_num_; - uint32_t write_pointer_ = 0; - uint32_t read_pointer_ = 0; - -private: - void IRAM_ATTR uart_isr(void *arg); - - int intr_cnt = 0; - uint8_t tx_pin_; - uint8_t rx_pin_; - uint8_t dxl_dir_pin_; - - int baud_rate_; -}; - -#endif // ESP32UARTPORTHANDLER_H \ No newline at end of file diff --git a/firmware/src/utilities.cpp b/firmware/src/utilities.cpp deleted file mode 100644 index 6af7325..0000000 --- a/firmware/src/utilities.cpp +++ /dev/null @@ -1,186 +0,0 @@ -#include "utilities.h" - -uint32_t dxl_to_real_baud(uint8_t baud) -{ - int real_baud = 57600; - switch(baud) - { - case 0: real_baud = 9600; break; - case 1: real_baud = 57600; break; - case 2: real_baud = 115200; break; - case 3: real_baud = 1000000; break; - case 4: real_baud = 2000000; break; - case 5: real_baud = 3000000; break; - case 6: real_baud = 4000000; break; - case 7: real_baud = 4500000; break; - } - return real_baud; -} - -void setAccelRange(Bmi088Accel &accel_handle, uint8_t range) -{ - Bmi088Accel::Range accel_range; - switch (range) - { - case 0: - accel_range = Bmi088Accel::RANGE_3G; - break; - case 1: - accel_range = Bmi088Accel::RANGE_6G; - break; - case 2: - accel_range = Bmi088Accel::RANGE_12G; - break; - case 3: - accel_range = Bmi088Accel::RANGE_24G; - break; - default: - accel_range = Bmi088Accel::RANGE_6G; - } - accel_handle.setRange(accel_range); -} - -void setGyroRange(Bmi088Gyro &gyro_handle, uint8_t range) -{ - Bmi088Gyro::Range gyro_range; - switch (range) - { - case 0: - gyro_range = Bmi088Gyro::RANGE_125DPS; - break; - case 1: - gyro_range = Bmi088Gyro::RANGE_250DPS; - break; - case 2: - gyro_range = Bmi088Gyro::RANGE_500DPS; - break; - case 3: - gyro_range = Bmi088Gyro::RANGE_1000DPS; - break; - case 4: - gyro_range = Bmi088Gyro::RANGE_2000DPS; - break; - default: - gyro_range = Bmi088Gyro::RANGE_1000DPS; - } - gyro_handle.setRange(gyro_range); -} - -void setAccelOdr(Bmi088Accel &accel_handle, uint8_t odr) -{ - Bmi088Accel::Odr accel_odr; - switch (odr) - { - case 0: - accel_odr = Bmi088Accel::ODR_12_5HZ_BW_1HZ; - break; - case 1: - accel_odr = Bmi088Accel::ODR_12_5HZ_BW_2HZ; - break; - case 2: - accel_odr = Bmi088Accel::ODR_12_5HZ_BW_5HZ; - break; - case 3: - accel_odr = Bmi088Accel::ODR_25HZ_BW_3HZ; - break; - case 4: - accel_odr = Bmi088Accel::ODR_25HZ_BW_5HZ; - break; - case 5: - accel_odr = Bmi088Accel::ODR_25HZ_BW_10HZ; - break; - case 6: - accel_odr = Bmi088Accel::ODR_50HZ_BW_5HZ; - break; - case 7: - accel_odr = Bmi088Accel::ODR_50HZ_BW_9HZ; - break; - case 8: - accel_odr = Bmi088Accel::ODR_50HZ_BW_20HZ; - break; - case 9: - accel_odr = Bmi088Accel::ODR_100HZ_BW_10HZ; - break; - case 10: - accel_odr = Bmi088Accel::ODR_100HZ_BW_19HZ; - break; - case 11: - accel_odr = Bmi088Accel::ODR_100HZ_BW_40HZ; - break; - case 12: - accel_odr = Bmi088Accel::ODR_200HZ_BW_20HZ; - break; - case 13: - accel_odr = Bmi088Accel::ODR_200HZ_BW_38HZ; - break; - case 14: - accel_odr = Bmi088Accel::ODR_200HZ_BW_80HZ; - break; - case 15: - accel_odr = Bmi088Accel::ODR_400HZ_BW_40HZ; - break; - case 16: - accel_odr = Bmi088Accel::ODR_400HZ_BW_75HZ; - break; - case 17: - accel_odr = Bmi088Accel::ODR_400HZ_BW_145HZ; - break; - case 18: - accel_odr = Bmi088Accel::ODR_800HZ_BW_80HZ; - break; - case 19: - accel_odr = Bmi088Accel::ODR_800HZ_BW_140HZ; - break; - case 20: - accel_odr = Bmi088Accel::ODR_800HZ_BW_230HZ; - break; - case 21: - accel_odr = Bmi088Accel::ODR_1600HZ_BW_145HZ; - break; - case 22: - accel_odr = Bmi088Accel::ODR_1600HZ_BW_234HZ; - break; - case 23: - accel_odr = Bmi088Accel::ODR_1600HZ_BW_280HZ; - break; - default: - accel_odr = Bmi088Accel::ODR_400HZ_BW_40HZ; - } - accel_handle.setOdr(accel_odr); -} - -void setGyroOdr(Bmi088Gyro &gyro_handle, uint8_t odr) -{ - Serial1.println("Setting gyro ODR"); - Bmi088Gyro::Odr gyro_odr; - switch (odr) - { - case 0: - gyro_odr = Bmi088Gyro::ODR_100HZ_BW_12HZ; - break; - case 1: - gyro_odr = Bmi088Gyro::ODR_100HZ_BW_32HZ; - break; - case 2: - gyro_odr = Bmi088Gyro::ODR_200HZ_BW_64HZ; - break; - case 3: - gyro_odr = Bmi088Gyro::ODR_200HZ_BW_23HZ; - break; - case 4: - gyro_odr = Bmi088Gyro::ODR_400HZ_BW_47HZ; - break; - case 5: - gyro_odr = Bmi088Gyro::ODR_1000HZ_BW_116HZ; - break; - case 6: - gyro_odr = Bmi088Gyro::ODR_2000HZ_BW_230HZ; - break; - case 7: - gyro_odr = Bmi088Gyro::ODR_2000HZ_BW_532HZ; - break; - default: - gyro_odr = Bmi088Gyro::ODR_1000HZ_BW_116HZ; - } - gyro_handle.setOdr(gyro_odr); -} \ No newline at end of file diff --git a/firmware/src/utilities.h b/firmware/src/utilities.h deleted file mode 100644 index 089460a..0000000 --- a/firmware/src/utilities.h +++ /dev/null @@ -1,8 +0,0 @@ -#include -#include "BMI088.h" - -uint32_t dxl_to_real_baud(uint8_t baud); -void setAccelRange(Bmi088Accel &accel_handle, uint8_t range); -void setGyroRange(Bmi088Gyro &gyro_handle, uint8_t range); -void setAccelOdr(Bmi088Accel &accel_handle, uint8_t odr); -void setGyroOdr(Bmi088Gyro &gyro_handle, uint8_t odr); diff --git a/firmware/test/README b/firmware/test/README deleted file mode 100644 index 9b1e87b..0000000 --- a/firmware/test/README +++ /dev/null @@ -1,11 +0,0 @@ - -This directory is intended for PlatformIO Test Runner and project tests. - -Unit Testing is a software testing method by which individual units of -source code, sets of one or more MCU program modules together with associated -control data, usage procedures, and operating procedures, are tested to -determine whether they are fit for use. Unit testing finds problems early -in the development cycle. - -More information about PlatformIO Unit Testing: -- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html