diff --git a/southbridge/Cargo.lock b/southbridge/Cargo.lock
index 935b2fc..580737a 100644
--- a/southbridge/Cargo.lock
+++ b/southbridge/Cargo.lock
@@ -79,18 +79,18 @@ dependencies = [
 
 [[package]]
 name = "bit-set"
-version = "0.5.3"
+version = "0.6.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0700ddab506f33b20a03b13996eccd309a48e5ff77d0d95926aa0210fb4e95f1"
+checksum = "f0481a0e032742109b1133a095184ee93d88f3dc9e0d28a5d033dc77a073f44f"
 dependencies = [
  "bit-vec",
 ]
 
 [[package]]
 name = "bit-vec"
-version = "0.6.3"
+version = "0.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "349f9b6a179ed607305526ca489b34ad0a41aed5f7980fa90eb03160b69598fb"
+checksum = "d2c54ff287cfc0a34f38a6b832ea1bd8e448a330b3e40a50859e6488bee07f22"
 
 [[package]]
 name = "bitfield"
@@ -110,6 +110,15 @@ version = "2.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36"
 
+[[package]]
+name = "block-buffer"
+version = "0.10.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71"
+dependencies = [
+ "generic-array",
+]
+
 [[package]]
 name = "bytemuck"
 version = "1.21.0"
@@ -184,6 +193,15 @@ dependencies = [
  "syn 2.0.96",
 ]
 
+[[package]]
+name = "cpufeatures"
+version = "0.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "59ed5838eebb26a2bb2e58f6d5b5316989ae9d08bab10e0e6d103e656d1b0280"
+dependencies = [
+ "libc",
+]
+
 [[package]]
 name = "crc-any"
 version = "2.5.0"
@@ -211,6 +229,16 @@ version = "0.2.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "43da5946c66ffcc7745f48db692ffbb10a83bfe0afd96235c5c2a4fb23994929"
 
+[[package]]
+name = "crypto-common"
+version = "0.1.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3"
+dependencies = [
+ "generic-array",
+ "typenum",
+]
+
 [[package]]
 name = "darling"
 version = "0.20.10"
@@ -246,6 +274,15 @@ dependencies = [
  "syn 2.0.96",
 ]
 
+[[package]]
+name = "dcmimu"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "692b1c8923315ac22103c1890512597baf543b95f2eebd66f572ffb1ff8f0170"
+dependencies = [
+ "libm 0.1.4",
+]
+
 [[package]]
 name = "debug-helper"
 version = "0.3.13"
@@ -253,10 +290,14 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f578e8e2c440e7297e008bb5486a3a8a194775224bbc23729b0dbdfaeebf162e"
 
 [[package]]
-name = "diff"
-version = "0.1.13"
+name = "digest"
+version = "0.10.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "56254986775e3233ffa9c4d7d3faaf6d36a2c09d30b20687e9f88bc8bafc16c8"
+checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292"
+dependencies = [
+ "block-buffer",
+ "crypto-common",
+]
 
 [[package]]
 name = "dirs-next"
@@ -297,10 +338,10 @@ checksum = "60b1af1c220855b6ceac025d3f6ecdd2b7c4894bfe9cd9bda4fbb4bc7c0d4cf0"
 [[package]]
 name = "embassy-embedded-hal"
 version = "0.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "41fea5ef5bed4d3468dfd44f5c9fa4cda8f54c86d4fb4ae683eacf9d39e2ea12"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "embassy-futures",
+ "embassy-hal-internal",
  "embassy-sync",
  "embassy-time",
  "embedded-hal 0.2.7",
@@ -314,8 +355,7 @@ dependencies = [
 [[package]]
 name = "embassy-executor"
 version = "0.7.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "cortex-m",
  "critical-section",
@@ -326,8 +366,7 @@ dependencies = [
 [[package]]
 name = "embassy-executor-macros"
 version = "0.6.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "darling",
  "proc-macro2",
@@ -338,14 +377,12 @@ dependencies = [
 [[package]]
 name = "embassy-futures"
 version = "0.1.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 
 [[package]]
 name = "embassy-hal-internal"
 version = "0.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0ef3bac31ec146321248a169e9c7b5799f1e0b3829c7a9b324cb4600a7438f59"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "cortex-m",
  "critical-section",
@@ -355,14 +392,12 @@ dependencies = [
 [[package]]
 name = "embassy-net-driver"
 version = "0.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "524eb3c489760508f71360112bca70f6e53173e6fe48fc5f0efd0f5ab217751d"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 
 [[package]]
 name = "embassy-net-driver-channel"
 version = "0.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4818c32afec43e3cae234f324bad9a976c9aa7501022d26ff60a4017a1a006b7"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "embassy-futures",
  "embassy-net-driver",
@@ -372,8 +407,7 @@ dependencies = [
 [[package]]
 name = "embassy-rp"
 version = "0.3.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8cfe90d82ebbfed9de96d9edc6d63b6619c0977b570a3ab1724f76023e72773b"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "atomic-polyfill",
  "cfg-if",
@@ -412,8 +446,7 @@ dependencies = [
 [[package]]
 name = "embassy-sync"
 version = "0.6.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "cfg-if",
  "critical-section",
@@ -426,8 +459,7 @@ dependencies = [
 [[package]]
 name = "embassy-time"
 version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f820157f198ada183ad62e0a66f554c610cdcd1a9f27d4b316358103ced7a1f8"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "cfg-if",
  "critical-section",
@@ -442,8 +474,7 @@ dependencies = [
 [[package]]
 name = "embassy-time-driver"
 version = "0.2.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "document-features",
 ]
@@ -451,8 +482,7 @@ dependencies = [
 [[package]]
 name = "embassy-time-queue-utils"
 version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "embassy-executor",
  "heapless 0.8.0",
@@ -461,8 +491,7 @@ dependencies = [
 [[package]]
 name = "embassy-usb"
 version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6e651b9b7b47b514e6e6d1940a6e2e300891a2c33641917130643602a0cb6386"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "embassy-futures",
  "embassy-net-driver-channel",
@@ -476,14 +505,12 @@ dependencies = [
 [[package]]
 name = "embassy-usb-driver"
 version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fc247028eae04174b6635104a35b1ed336aabef4654f5e87a8f32327d231970"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 
 [[package]]
 name = "embassy-usb-logger"
 version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ad40e3ec749d83f4b7ab0c0599c9bef0c51a73ce1c1087d8599ff2448d00c2aa"
+source = "git+https://github.com/embassy-rs/embassy/#0ce6da9706b9d929691c9292c923117fcbc2f9c6"
 dependencies = [
  "embassy-futures",
  "embassy-sync",
@@ -644,6 +671,16 @@ dependencies = [
  "pin-utils",
 ]
 
+[[package]]
+name = "generic-array"
+version = "0.14.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a"
+dependencies = [
+ "typenum",
+ "version_check",
+]
+
 [[package]]
 name = "getrandom"
 version = "0.2.15"
@@ -722,12 +759,6 @@ dependencies = [
  "stable_deref_trait",
 ]
 
-[[package]]
-name = "hermit-abi"
-version = "0.4.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fbf6a919d6cf397374f7dfeeea91d974c7c0a7221d0d0f4f20d859d329e53fcc"
-
 [[package]]
 name = "ident_case"
 version = "1.0.1"
@@ -744,55 +775,53 @@ dependencies = [
  "hashbrown 0.15.2",
 ]
 
-[[package]]
-name = "is-terminal"
-version = "0.4.15"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e19b23d53f35ce9f56aebc7d1bb4e6ac1e9c0db7ac85c8d1760c04379edced37"
-dependencies = [
- "hermit-abi",
- "libc",
- "windows-sys",
-]
-
 [[package]]
 name = "itertools"
-version = "0.10.5"
+version = "0.13.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473"
+checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186"
 dependencies = [
  "either",
 ]
 
 [[package]]
-name = "lalrpop"
-version = "0.19.12"
+name = "keccak"
+version = "0.1.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a1cbf952127589f2851ab2046af368fd20645491bb4b376f04b7f94d7a9837b"
+checksum = "ecc2af9a1119c51f12a14607e783cb977bde58bc069ff0c3da1095e635d70654"
+dependencies = [
+ "cpufeatures",
+]
+
+[[package]]
+name = "lalrpop"
+version = "0.21.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6e56f323e2d610628d1f5bdd39168a774674ac7989ed67011963bb3f71edd797"
 dependencies = [
  "ascii-canvas",
  "bit-set",
- "diff",
  "ena",
- "is-terminal",
  "itertools",
  "lalrpop-util",
  "petgraph",
+ "pico-args",
  "regex",
- "regex-syntax 0.6.29",
+ "regex-syntax",
+ "sha3",
  "string_cache",
  "term",
- "tiny-keccak",
  "unicode-xid",
+ "walkdir",
 ]
 
 [[package]]
 name = "lalrpop-util"
-version = "0.19.12"
+version = "0.21.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d3c48237b9604c5a4702de6b824e02006c3214327564636aef27c1028a8fa0ed"
+checksum = "108dc8f5dabad92c65a03523055577d847f5dcc00f3e7d3a68bc4d48e01d8fe1"
 dependencies = [
- "regex",
+ "regex-automata",
 ]
 
 [[package]]
@@ -801,6 +830,12 @@ version = "0.2.169"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b5aba8db14291edd000dfcc4d620c7ebfb122c613afb886ca8803fa4e128a20a"
 
+[[package]]
+name = "libm"
+version = "0.1.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7fc7aa29613bd6a620df431842069224d8bc9011086b1db4c0e0cd47fa03ec9a"
+
 [[package]]
 name = "libm"
 version = "0.2.11"
@@ -851,7 +886,7 @@ version = "0.1.6"
 source = "git+https://git.ank.dev/ank/mpu6050#32604cc16c6a604fd0a4a35bf66bd9df800da18e"
 dependencies = [
  "embedded-hal 1.0.0",
- "libm",
+ "libm 0.2.11",
  "nalgebra",
 ]
 
@@ -931,22 +966,22 @@ dependencies = [
 
 [[package]]
 name = "num_enum"
-version = "0.5.11"
+version = "0.7.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1f646caf906c20226733ed5b1374287eb97e3c2a5c227ce668c1f2ce20ae57c9"
+checksum = "4e613fc340b2220f734a8595782c551f1250e969d87d3be1ae0579e8d4065179"
 dependencies = [
  "num_enum_derive",
 ]
 
 [[package]]
 name = "num_enum_derive"
-version = "0.5.11"
+version = "0.7.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dcbff9bc912032c62bf65ef1d5aea88983b420f4f839db1e9b0c281a25c9c799"
+checksum = "af1844ef2428cc3e1cb900be36181049ef3d3193c63e43026cfe202983b27a56"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn 2.0.96",
 ]
 
 [[package]]
@@ -1003,6 +1038,12 @@ dependencies = [
  "siphasher",
 ]
 
+[[package]]
+name = "pico-args"
+version = "0.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5be167a7af36ee22fe3115051bc51f6e6c7054c9348e28deb4f49bd6f705a315"
+
 [[package]]
 name = "pin-project-lite"
 version = "0.2.16"
@@ -1018,8 +1059,7 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
 [[package]]
 name = "pio"
 version = "0.2.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "76e09694b50f89f302ed531c1f2a7569f0be5867aee4ab4f8f729bbeec0078e3"
+source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
 dependencies = [
  "arrayvec",
  "num_enum",
@@ -1029,30 +1069,26 @@ dependencies = [
 [[package]]
 name = "pio-parser"
 version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "77532c2b8279aef98dfc7207ef15298a5a3d6b6cc76ccc8b65913d69f3a8dd6b"
+source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
 dependencies = [
  "lalrpop",
  "lalrpop-util",
  "pio",
- "regex-syntax 0.6.29",
 ]
 
 [[package]]
 name = "pio-proc"
 version = "0.2.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6b04dc870fb3a4fd8b3e4ca8c61b53bc8ac4eb78b66805d2b3c2e5c4829e0d7a"
+source = "git+https://github.com/rp-rs/pio-rs?rev=fa586448b0b223217eec8c92c19fe6823dd04cc4#fa586448b0b223217eec8c92c19fe6823dd04cc4"
 dependencies = [
  "codespan-reporting",
  "lalrpop-util",
  "pio",
  "pio-parser",
- "proc-macro-error",
+ "proc-macro-error2",
  "proc-macro2",
  "quote",
- "regex-syntax 0.6.29",
- "syn 1.0.109",
+ "syn 2.0.96",
 ]
 
 [[package]]
@@ -1082,27 +1118,25 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "925383efa346730478fb4838dbe9137d2a47675ad789c546d150a6e1dd4ab31c"
 
 [[package]]
-name = "proc-macro-error"
-version = "1.0.4"
+name = "proc-macro-error-attr2"
+version = "2.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c"
+checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5"
 dependencies = [
- "proc-macro-error-attr",
  "proc-macro2",
  "quote",
- "syn 1.0.109",
- "version_check",
 ]
 
 [[package]]
-name = "proc-macro-error-attr"
-version = "1.0.4"
+name = "proc-macro-error2"
+version = "2.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869"
+checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802"
 dependencies = [
+ "proc-macro-error-attr2",
  "proc-macro2",
  "quote",
- "version_check",
+ "syn 2.0.96",
 ]
 
 [[package]]
@@ -1164,7 +1198,7 @@ dependencies = [
  "aho-corasick",
  "memchr",
  "regex-automata",
- "regex-syntax 0.8.5",
+ "regex-syntax",
 ]
 
 [[package]]
@@ -1175,15 +1209,9 @@ checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908"
 dependencies = [
  "aho-corasick",
  "memchr",
- "regex-syntax 0.8.5",
+ "regex-syntax",
 ]
 
-[[package]]
-name = "regex-syntax"
-version = "0.6.29"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f162c6dd7b008981e4d40210aca20b4bd0f9b60ca9271061b07f78537722f2e1"
-
 [[package]]
 name = "regex-syntax"
 version = "0.8.5"
@@ -1248,6 +1276,15 @@ version = "1.0.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f7c45b9784283f1b2e7fb61b42047c2fd678ef0960d4f6f1eba131594cc369d4"
 
+[[package]]
+name = "same-file"
+version = "1.0.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502"
+dependencies = [
+ "winapi-util",
+]
+
 [[package]]
 name = "scopeguard"
 version = "1.2.0"
@@ -1301,6 +1338,16 @@ version = "0.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "5f179d4e11094a893b82fff208f74d448a7512f99f5a0acbd5c679b705f83ed9"
 
+[[package]]
+name = "sha3"
+version = "0.10.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "75872d278a8f37ef87fa0ddbda7802605cb18344497949862c0d4dcb291eba60"
+dependencies = [
+ "digest",
+ "keccak",
+]
+
 [[package]]
 name = "simba"
 version = "0.7.3"
@@ -1350,6 +1397,7 @@ dependencies = [
  "common",
  "cortex-m",
  "cortex-m-rt",
+ "dcmimu",
  "embassy-executor",
  "embassy-rp",
  "embassy-sync",
@@ -1486,15 +1534,6 @@ dependencies = [
  "syn 2.0.96",
 ]
 
-[[package]]
-name = "tiny-keccak"
-version = "2.0.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2c9d3793400a45f954c52e73d068316d76b6f4e36977e3fcebb13a2721e80237"
-dependencies = [
- "crunchy",
-]
-
 [[package]]
 name = "typenum"
 version = "1.17.0"
@@ -1593,6 +1632,16 @@ dependencies = [
  "vcell",
 ]
 
+[[package]]
+name = "walkdir"
+version = "2.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b"
+dependencies = [
+ "same-file",
+ "winapi-util",
+]
+
 [[package]]
 name = "wasi"
 version = "0.11.0+wasi-snapshot-preview1"
diff --git a/southbridge/Cargo.toml b/southbridge/Cargo.toml
index 9869942..d654cbf 100644
--- a/southbridge/Cargo.toml
+++ b/southbridge/Cargo.toml
@@ -25,9 +25,18 @@ log = "0.4"
 framed = {git = "https://git.ank.dev/ank/framed-rs", default-features=false, features = ["use_nightly"]}
 common = { path = "../common" }
 mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
+dcmimu = "0.2.2"
 nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
 serde = { version = "1.0.203", default-features = false, features = ["derive"] }
 postcard = "1.0.0"
 static_cell = "2.1"
 portable-atomic = { version = "1.5", features = ["critical-section"] }
 heapless = "0.8"
+
+[patch.crates-io]
+# https://github.com/embassy-rs/embassy/pull/3763/
+embassy-rp = { git = 'https://github.com/embassy-rs/embassy/' }
+embassy-time = { git = 'https://github.com/embassy-rs/embassy/' }
+embassy-usb-logger = { git = 'https://github.com/embassy-rs/embassy/' }
+embassy-sync = { git = 'https://github.com/embassy-rs/embassy/' }
+embassy-executor = { git = 'https://github.com/embassy-rs/embassy/' }
diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index e20378a..4dbd9ad 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -6,7 +6,7 @@ use core::{panic::PanicInfo, sync::atomic::{AtomicU16, Ordering}};
 use common::{Response, RobotState, SensorData, BAUDRATE};
 use embassy_executor::Spawner;
 use embassy_rp::{adc::{self, Adc}, bind_interrupts, block::ImageDef, gpio::{Level, Output, Pull}, peripherals::{ADC, ADC_TEMP_SENSOR, PIN_28, UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver};
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, once_lock::OnceLock, watch::Watch};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, mutex::Mutex, once_lock::OnceLock, pubsub::PubSubChannel, signal::Signal, watch::Watch};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
 use embedded_io_async::{BufRead, Read, Write};
 use log::{error, info, trace, warn};
@@ -21,7 +21,7 @@ bind_interrupts!(struct Irqs {
     ADC_IRQ_FIFO => adc::InterruptHandler;
 });
 
-pub static COMMANDS: Watch<CriticalSectionRawMutex, (RobotState, Instant), 5> = Watch::new();
+pub static COMMANDS: Watch<CriticalSectionRawMutex, (RobotState, Instant), 3> = Watch::new();
 pub static SENSOR_DATA: Channel<CriticalSectionRawMutex, SensorData, 5> = Channel::new();
 
 pub static BUS_VOLTAGE: AtomicU16 = AtomicU16::new(u16::MAX);
@@ -38,7 +38,7 @@ pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe();
 
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
-    embassy_usb_logger::run!(1024, log::LevelFilter::Trace, driver);
+    embassy_usb_logger::run!(2048, log::LevelFilter::Trace, driver);
 }
 
 #[panic_handler]
@@ -86,7 +86,7 @@ async fn main(spawner: Spawner) {
 
     Timer::after_secs(13).await; // pi 0 serial ports act strange during the boot process
 
-    spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap();
+    //spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap();
 
     let config = embassy_rp::i2c::Config::default();
     let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config);
@@ -104,8 +104,7 @@ async fn main(spawner: Spawner) {
 
     spawner.spawn(decoder(rx)).unwrap();
 
-    static ENABLED: AtomicBool = AtomicBool::new(false);
-    spawner.spawn(telemetry(tx, &ENABLED)).unwrap();
+    spawner.spawn(telemetry(tx)).unwrap();
 
     let watchdog_time = Duration::from_millis(150);
 
@@ -120,19 +119,15 @@ async fn main(spawner: Spawner) {
         if time.elapsed() > watchdog_time {
             drive_pwm.set_config(&stopped);
             set_underglow(&mut light_pwm_rg, &mut light_pwm_b, disabled_orange.clone());
-            ENABLED.store(false, Ordering::Acquire);
             commands.changed().await;
             set_underglow(&mut light_pwm_rg, &mut light_pwm_b, enabled_green.clone());
-            ENABLED.store(true, Ordering::Acquire);
             continue;
         }
 
-        ENABLED.store(true, Ordering::Acquire);
-
 
         // 100% effort at 9.5v, 0% effort at 7.5v
         let brownout = ((bus_voltage()-7.5)/2.0).clamp(0., 1.);
-        let command = command.brownout(brownout);
+        let command = command.clone().brownout(brownout);
 
         // drive motors
         let (forward, right) = command.drive;
@@ -176,18 +171,13 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
 
 /// Receive data from channel and send it over UART
 #[embassy_executor::task]
-async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool) {
+async fn telemetry(mut tx: BufferedUartTx<'static, UART1>) {
     info!("Telemetry waiting...");
     //COMMANDS.receiver().unwrap().get().await;
     info!("Telemetry started...");
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
 
-        if !enabled.load(Ordering::Release) {
-            Timer::after_millis(10).await;
-            continue;
-        }
-
         let packet = Response {
             sensor_data: data,
             uptime_micros: Instant::now().as_micros(),