diff --git a/.woodpecker.yml b/.woodpecker.yml
deleted file mode 100644
index 841cd27..0000000
--- a/.woodpecker.yml
+++ /dev/null
@@ -1,8 +0,0 @@
-pipeline:
-  build:
-    image: rust
-    commands:
-        - rustup override set nightly
-        - rustup target add thumbv7m-none-eabi
-        - cargo install flip-link
-        - cargo build --release
diff --git a/Cargo.lock b/Cargo.lock
index 190ac78..51c6363 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -3,21 +3,47 @@
 version = 3
 
 [[package]]
-name = "atomic-polyfill"
-version = "0.1.11"
+name = "aho-corasick"
+version = "0.7.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e3ff7eb3f316534d83a8a2c3d1674ace8a5a71198eba31e2e2b597833f699b28"
+checksum = "1e37cfd5e7657ada45f742d6e99ca5788580b5c529dc78faf11ece6dc702656f"
 dependencies = [
- "critical-section 1.1.1",
+ "memchr",
+]
+
+[[package]]
+name = "anyhow"
+version = "1.0.62"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1485d4d2cc45e7b201ee3767015c96faa5904387c9d87c6efdd0fb511f12d305"
+
+[[package]]
+name = "atomic-polyfill"
+version = "0.1.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9c041a8d9751a520ee19656232a18971f18946a7900f1520ee4400002244dd89"
+dependencies = [
+ "critical-section 0.2.7",
 ]
 
 [[package]]
 name = "atomic-polyfill"
-version = "1.0.2"
+version = "1.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c314e70d181aa6053b26e3f7fbf86d1dfff84f816a6175b967666b3506ef7289"
+checksum = "d299f547288d6db8d5c3a2916f7b2f66134b15b8c1ac1c4357dd3b8752af7bb2"
 dependencies = [
- "critical-section 1.1.1",
+ "critical-section 1.1.0",
+]
+
+[[package]]
+name = "atty"
+version = "0.2.14"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8"
+dependencies = [
+ "hermit-abi",
+ "libc",
+ "winapi",
 ]
 
 [[package]]
@@ -49,9 +75,9 @@ checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603"
 
 [[package]]
 name = "bit_field"
-version = "0.10.2"
+version = "0.10.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61"
+checksum = "dcb6dd1c2376d2e096796e234a70e17e94cc2d5d54ff8ce42b28cef1d0d359a4"
 
 [[package]]
 name = "bitfield"
@@ -74,7 +100,7 @@ dependencies = [
  "bitflags",
  "defmt",
  "embedded-hal 0.2.7",
- "nb 1.1.0",
+ "nb 1.0.0",
  "vcell",
 ]
 
@@ -97,23 +123,80 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
 
 [[package]]
-name = "cortex-m"
-version = "0.7.7"
+name = "chiptool"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/chiptool?rev=28ffa8a19d84914089547f52900ffb5877a5dc23#28ffa8a19d84914089547f52900ffb5877a5dc23"
+dependencies = [
+ "anyhow",
+ "clap",
+ "env_logger",
+ "inflections",
+ "log",
+ "proc-macro2",
+ "quote",
+ "regex",
+ "serde",
+ "serde_yaml",
+ "svd-parser",
+]
+
+[[package]]
+name = "clap"
+version = "3.2.17"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9"
+checksum = "29e724a68d9319343bb3328c9cc2dfde263f4b3142ee1059a9980580171c954b"
+dependencies = [
+ "atty",
+ "bitflags",
+ "clap_derive",
+ "clap_lex",
+ "indexmap",
+ "once_cell",
+ "strsim",
+ "termcolor",
+ "textwrap",
+]
+
+[[package]]
+name = "clap_derive"
+version = "3.2.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "13547f7012c01ab4a0e8f8967730ada8f9fdf419e8b6c792788f39cf4e46eefa"
+dependencies = [
+ "heck",
+ "proc-macro-error",
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "clap_lex"
+version = "0.2.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2850f2f5a82cbf437dd5af4d49848fbdfc27c157c3d010345776f952765261c5"
+dependencies = [
+ "os_str_bytes",
+]
+
+[[package]]
+name = "cortex-m"
+version = "0.7.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "70858629a458fdfd39f9675c4dc309411f2a3f83bede76988d81bf1a0ecee9e0"
 dependencies = [
  "bare-metal 0.2.5",
  "bitfield",
- "critical-section 1.1.1",
+ "critical-section 1.1.0",
  "embedded-hal 0.2.7",
  "volatile-register",
 ]
 
 [[package]]
 name = "cortex-m-rt"
-version = "0.7.3"
+version = "0.7.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1"
+checksum = "3c433da385b720d5bb9f52362fa2782420798e68d40d67bfe4b0d992aba5dfe7"
 dependencies = [
  "cortex-m-rt-macros",
 ]
@@ -126,24 +209,71 @@ checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn",
 ]
 
 [[package]]
 name = "critical-section"
-version = "0.2.8"
+version = "0.2.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1706d332edc22aef4d9f23a6bb1c92360a403013c291af51247a737472dcae6"
+checksum = "95da181745b56d4bd339530ec393508910c909c784e8962d15d722bacf0bcbcd"
 dependencies = [
  "bare-metal 1.0.0",
- "critical-section 1.1.1",
+ "cfg-if",
+ "cortex-m",
+ "riscv",
 ]
 
 [[package]]
 name = "critical-section"
-version = "1.1.1"
+version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6548a0ad5d2549e111e1f6a11a6c2e2d00ce6a3dafe22948d67c2b443f775e52"
+checksum = "d02ba51481d019be9c74a831d1133c364d78831b75c833478f3a21e1fd91e01a"
+
+[[package]]
+name = "crossbeam-channel"
+version = "0.5.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "c2dd04ddaf88237dc3b8d8f9a3c1004b506b54b3313403944054d23c0870c521"
+dependencies = [
+ "cfg-if",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-deque"
+version = "0.8.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "715e8152b692bba2d374b53d4875445368fdf21a94751410af607a5ac677d1fc"
+dependencies = [
+ "cfg-if",
+ "crossbeam-epoch",
+ "crossbeam-utils",
+]
+
+[[package]]
+name = "crossbeam-epoch"
+version = "0.9.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "045ebe27666471bb549370b4b0b3e51b07f56325befa4284db65fc89c02511b1"
+dependencies = [
+ "autocfg",
+ "cfg-if",
+ "crossbeam-utils",
+ "memoffset",
+ "once_cell",
+ "scopeguard",
+]
+
+[[package]]
+name = "crossbeam-utils"
+version = "0.8.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "51887d4adc7b564537b15adcfb307936f8075dfcd5f00dde9a9f1d29383682bc"
+dependencies = [
+ "cfg-if",
+ "once_cell",
+]
 
 [[package]]
 name = "darling"
@@ -166,7 +296,7 @@ dependencies = [
  "proc-macro2",
  "quote",
  "strsim",
- "syn 1.0.109",
+ "syn",
 ]
 
 [[package]]
@@ -177,14 +307,14 @@ checksum = "9c972679f83bdf9c42bd905396b6c3588a843a17f0f16dfcfa3e2c5d57441835"
 dependencies = [
  "darling_core",
  "quote",
- "syn 1.0.109",
+ "syn",
 ]
 
 [[package]]
 name = "defmt"
-version = "0.3.4"
+version = "0.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "956673bd3cb347512bf988d1e8d89ac9a82b64f6eec54d3c01c3529dac019882"
+checksum = "d3a0ae7494d9bff013d7b89471f4c424356a71e9752e0c78abe7e6c608a16bb3"
 dependencies = [
  "bitflags",
  "defmt-macros",
@@ -192,25 +322,22 @@ dependencies = [
 
 [[package]]
 name = "defmt-macros"
-version = "0.3.5"
+version = "0.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b4abc4821bd84d3d8f49945ddb24d029be9385ed9b77c99bf2f6296847a6a9f0"
+checksum = "6d944432e281084511691b36e5e9c794c19c33675822c9019e3b64f5b89e10da"
 dependencies = [
  "defmt-parser",
  "proc-macro-error",
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn",
 ]
 
 [[package]]
 name = "defmt-parser"
-version = "0.3.3"
+version = "0.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "269924c02afd7f94bc4cecbfa5c379f6ffcf9766b3408fe63d22c728654eccd0"
-dependencies = [
- "thiserror",
-]
+checksum = "0db23d29972d99baa3de2ee2ae3f104c10564a6d05a346eb3f4c4f2c0525a06e"
 
 [[package]]
 name = "defmt-rtt"
@@ -218,7 +345,7 @@ version = "0.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "1d2cbbbd58847d508d97629b32cd9730a2d28532f71e219714614406029f18b1"
 dependencies = [
- "critical-section 0.2.8",
+ "critical-section 0.2.7",
  "defmt",
 ]
 
@@ -249,129 +376,102 @@ dependencies = [
  "embedded-hal 0.2.7",
 ]
 
+[[package]]
+name = "either"
+version = "1.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "90e5c1c8368803113bf0c9584fc495a58b86dc8a29edbf8fe877d21d9507e797"
+
 [[package]]
 name = "embassy-cortex-m"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
- "atomic-polyfill 1.0.2",
+ "atomic-polyfill 1.0.1",
  "cfg-if",
  "cortex-m",
- "critical-section 1.1.1",
+ "critical-section 1.1.0",
  "embassy-executor",
  "embassy-hal-common",
  "embassy-macros",
- "embassy-sync",
+ "embassy-util",
 ]
 
 [[package]]
 name = "embassy-embedded-hal"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
  "defmt",
- "embassy-futures",
- "embassy-sync",
+ "embassy-util",
  "embedded-hal 0.2.7",
- "embedded-hal 1.0.0-alpha.10",
+ "embedded-hal 1.0.0-alpha.8",
  "embedded-hal-async",
  "embedded-storage",
  "embedded-storage-async",
- "nb 1.1.0",
+ "nb 1.0.0",
 ]
 
 [[package]]
 name = "embassy-executor"
-version = "0.2.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
- "atomic-polyfill 1.0.2",
- "cortex-m",
- "critical-section 1.1.1",
+ "atomic-polyfill 1.0.1",
+ "cfg-if",
+ "critical-section 1.1.0",
  "defmt",
  "embassy-macros",
  "embassy-time",
  "futures-util",
- "static_cell",
-]
-
-[[package]]
-name = "embassy-futures"
-version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
-dependencies = [
- "defmt",
 ]
 
 [[package]]
 name = "embassy-hal-common"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
- "defmt",
  "num-traits",
 ]
 
 [[package]]
 name = "embassy-macros"
-version = "0.2.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
  "darling",
  "proc-macro2",
  "quote",
- "syn 1.0.109",
-]
-
-[[package]]
-name = "embassy-net-driver"
-version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
-dependencies = [
- "defmt",
-]
-
-[[package]]
-name = "embassy-net-driver-channel"
-version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
-dependencies = [
- "embassy-futures",
- "embassy-net-driver",
- "embassy-sync",
+ "syn",
 ]
 
 [[package]]
 name = "embassy-stm32"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
- "atomic-polyfill 1.0.2",
- "bit_field",
+ "atomic-polyfill 1.0.1",
  "bxcan",
  "cfg-if",
  "cortex-m",
  "cortex-m-rt",
- "critical-section 1.1.1",
+ "critical-section 1.1.0",
  "defmt",
  "embassy-cortex-m",
  "embassy-embedded-hal",
  "embassy-executor",
- "embassy-futures",
  "embassy-hal-common",
- "embassy-net-driver",
- "embassy-sync",
  "embassy-time",
- "embassy-usb-driver",
+ "embassy-usb",
+ "embassy-util",
  "embedded-hal 0.2.7",
- "embedded-hal 1.0.0-alpha.10",
+ "embedded-hal 1.0.0-alpha.8",
  "embedded-hal-async",
- "embedded-hal-nb",
  "embedded-io",
  "embedded-storage",
  "embedded-storage-async",
  "futures",
- "nb 1.1.0",
+ "nb 1.0.0",
  "proc-macro2",
  "quote",
  "rand_core",
@@ -382,54 +482,52 @@ dependencies = [
  "vcell",
 ]
 
-[[package]]
-name = "embassy-sync"
-version = "0.2.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
-dependencies = [
- "cfg-if",
- "critical-section 1.1.1",
- "defmt",
- "embedded-io",
- "futures-util",
- "heapless",
-]
-
 [[package]]
 name = "embassy-time"
-version = "0.1.1"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
- "atomic-polyfill 1.0.2",
+ "atomic-polyfill 1.0.1",
  "cfg-if",
- "critical-section 1.1.1",
+ "critical-section 1.1.0",
  "defmt",
+ "embassy-macros",
  "embedded-hal 0.2.7",
  "futures-util",
- "heapless",
 ]
 
 [[package]]
 name = "embassy-usb"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
  "defmt",
- "embassy-futures",
- "embassy-net-driver-channel",
- "embassy-sync",
- "embassy-usb-driver",
+ "embassy-util",
  "heapless",
- "ssmarshal",
- "usbd-hid",
 ]
 
 [[package]]
-name = "embassy-usb-driver"
+name = "embassy-usb-serial"
 version = "0.1.0"
-source = "git+https://github.com/embassy-rs/embassy.git#bea42a78a4c44df3ebab564c32bf04ed76493704"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
  "defmt",
+ "embassy-usb",
+ "embassy-util",
+]
+
+[[package]]
+name = "embassy-util"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
+dependencies = [
+ "atomic-polyfill 1.0.1",
+ "cfg-if",
+ "critical-section 1.1.0",
+ "defmt",
+ "embedded-io",
+ "futures-util",
+ "heapless",
 ]
 
 [[package]]
@@ -467,34 +565,27 @@ dependencies = [
 
 [[package]]
 name = "embedded-hal"
-version = "1.0.0-alpha.10"
+version = "1.0.0-alpha.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f65c4d073f5d91c66e629b216818a4c9747eeda0debedf2deda9a0a947e4e93b"
-
-[[package]]
-name = "embedded-hal-async"
-version = "0.2.0-alpha.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8042370aa7af48de36d5312cda14c18ed8ca6b7ce64f5a07832fedc9dc83063f"
+checksum = "c3babfc7fd332142a0b11aebf592992f211f4e01b6222fb04b03aba1bd80018d"
 dependencies = [
- "embedded-hal 1.0.0-alpha.10",
+ "nb 1.0.0",
 ]
 
 [[package]]
-name = "embedded-hal-nb"
-version = "1.0.0-alpha.2"
+name = "embedded-hal-async"
+version = "0.1.0-alpha.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1465fffd56a95bbc105c17965bca1c1d5815027b1cc6bb183bc05d04563d065c"
+checksum = "022249738afde9b2b755a28a367ae20c9367e8e9c24b9aaf60bcc9255a255ec5"
 dependencies = [
- "embedded-hal 1.0.0-alpha.10",
- "nb 1.1.0",
+ "embedded-hal 1.0.0-alpha.8",
 ]
 
 [[package]]
 name = "embedded-io"
-version = "0.4.0"
+version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced"
+checksum = "36673b79844ff4ec0e3f00aeca0b2cfff564ff6739ab9801d13f45a8ec6cc1c7"
 dependencies = [
  "defmt",
 ]
@@ -507,18 +598,25 @@ checksum = "156d7a2fdd98ebbf9ae579cbceca3058cff946e13f8e17b90e3511db0508c723"
 
 [[package]]
 name = "embedded-storage-async"
-version = "0.4.0"
+version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "052997a894670d0cde873faa7405bc98e2fd29f569d2acd568561bc1c396b35a"
+checksum = "5ff04af74e47e9bb4315bd7aa2b01f3d1b05f33c03a6c4e9c3b20e9ce9cd8d79"
 dependencies = [
  "embedded-storage",
 ]
 
 [[package]]
-name = "encode_unicode"
-version = "0.3.6"
+name = "env_logger"
+version = "0.9.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a357d28ed41a50f9c765dbfe56cbc04a64e53e5fc58ba79fbc34c10ef3df831f"
+checksum = "0b2cf0344971ee6c64c31be0d530793fba457d322dfec2810c453d0ef228f9c3"
+dependencies = [
+ "atty",
+ "humantime",
+ "log",
+ "regex",
+ "termcolor",
+]
 
 [[package]]
 name = "float-cmp"
@@ -537,9 +635,9 @@ checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1"
 
 [[package]]
 name = "futures"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "23342abe12aba583913b2e62f22225ff9c950774065e4bfb61a19cd9770fec40"
+checksum = "ab30e97ab6aacfe635fad58f22c2bb06c8b685f7421eb1e064a729e2a5f481fa"
 dependencies = [
  "futures-channel",
  "futures-core",
@@ -551,9 +649,9 @@ dependencies = [
 
 [[package]]
 name = "futures-channel"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "955518d47e09b25bbebc7a18df10b81f0c766eaf4c4f1cccef2fca5f2a4fb5f2"
+checksum = "2bfc52cbddcfd745bf1740338492bb0bd83d76c67b445f91c5fb29fae29ecaa1"
 dependencies = [
  "futures-core",
  "futures-sink",
@@ -561,44 +659,44 @@ dependencies = [
 
 [[package]]
 name = "futures-core"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4bca583b7e26f571124fe5b7561d49cb2868d79116cfa0eefce955557c6fee8c"
+checksum = "d2acedae88d38235936c3922476b10fced7b2b68136f5e3c03c2d5be348a1115"
 
 [[package]]
 name = "futures-io"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fff74096e71ed47f8e023204cfd0aa1289cd54ae5430a9523be060cdb849964"
+checksum = "93a66fc6d035a26a3ae255a6d2bca35eda63ae4c5512bef54449113f7a1228e5"
 
 [[package]]
 name = "futures-macro"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72"
+checksum = "0db9cce532b0eae2ccf2766ab246f114b56b9cf6d445e00c2549fbc100ca045d"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.18",
+ "syn",
 ]
 
 [[package]]
 name = "futures-sink"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f43be4fe21a13b9781a69afa4985b0f6ee0e1afab2c6f454a8cf30e2b2237b6e"
+checksum = "ca0bae1fe9752cf7fd9b0064c674ae63f97b37bc714d745cbde0afb7ec4e6765"
 
 [[package]]
 name = "futures-task"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "76d3d132be6c0e6aa1534069c705a74a5997a356c0dc2f86a47765e5617c5b65"
+checksum = "842fc63b931f4056a24d59de13fb1272134ce261816e063e634ad0c15cdc5306"
 
 [[package]]
 name = "futures-util"
-version = "0.3.28"
+version = "0.3.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "26b01e40b772d54cf6c6d721c1d1abd0647a0106a12ecaa1c186273392a69533"
+checksum = "f0828a5471e340229c11c77ca80017937ce3c58cb788a17e5f1c2d5c485a9577"
 dependencies = [
  "futures-core",
  "futures-macro",
@@ -617,13 +715,19 @@ dependencies = [
  "byteorder",
 ]
 
+[[package]]
+name = "hashbrown"
+version = "0.12.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888"
+
 [[package]]
 name = "heapless"
 version = "0.7.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743"
 dependencies = [
- "atomic-polyfill 0.1.11",
+ "atomic-polyfill 0.1.10",
  "hash32",
  "rustc_version 0.4.0",
  "spin",
@@ -631,6 +735,27 @@ dependencies = [
  "ufmt-write",
 ]
 
+[[package]]
+name = "heck"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2540771e65fc8cb83cd6e8a237f70c319bd5c29f78ed1084ba5d50eeac86f7f9"
+
+[[package]]
+name = "hermit-abi"
+version = "0.1.19"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33"
+dependencies = [
+ "libc",
+]
+
+[[package]]
+name = "humantime"
+version = "2.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4"
+
 [[package]]
 name = "ident_case"
 version = "1.0.1"
@@ -638,15 +763,73 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39"
 
 [[package]]
-name = "lock_api"
-version = "0.4.9"
+name = "indexmap"
+version = "1.9.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "435011366fe56583b16cf956f9df0095b405b82d76425bc8981c0e22e60ec4df"
+checksum = "10a35a97730320ffe8e2d410b5d3b69279b98d2c14bdb8b70ea89ecf7888d41e"
+dependencies = [
+ "autocfg",
+ "hashbrown",
+]
+
+[[package]]
+name = "inflections"
+version = "1.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a257582fdcde896fd96463bf2d40eefea0580021c0712a0e2b028b60b47a837a"
+
+[[package]]
+name = "lazy_static"
+version = "1.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
+
+[[package]]
+name = "libc"
+version = "0.2.132"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8371e4e5341c3a96db127eb2465ac681ced4c433e01dd0e938adbef26ba93ba5"
+
+[[package]]
+name = "linked-hash-map"
+version = "0.5.6"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f"
+
+[[package]]
+name = "lock_api"
+version = "0.4.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "327fa5b6a6940e4699ec49a9beae1ea4845c6bab9314e4f84ac68742139d8c53"
 dependencies = [
  "autocfg",
  "scopeguard",
 ]
 
+[[package]]
+name = "log"
+version = "0.4.17"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e"
+dependencies = [
+ "cfg-if",
+]
+
+[[package]]
+name = "memchr"
+version = "2.5.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d"
+
+[[package]]
+name = "memoffset"
+version = "0.6.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5aa361d4faea93603064a027415f07bd8e1d5c88c9fbf68bf56a285428fd79ce"
+dependencies = [
+ "autocfg",
+]
+
 [[package]]
 name = "micromath"
 version = "1.1.1"
@@ -659,14 +842,14 @@ version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f"
 dependencies = [
- "nb 1.1.0",
+ "nb 1.0.0",
 ]
 
 [[package]]
 name = "nb"
-version = "1.1.0"
+version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d"
+checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
 
 [[package]]
 name = "num-traits"
@@ -678,10 +861,32 @@ dependencies = [
 ]
 
 [[package]]
-name = "panic-probe"
-version = "0.3.1"
+name = "num_cpus"
+version = "1.13.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "aa6fa5645ef5a760cd340eaa92af9c1ce131c8c09e7f8926d8a24b59d26652b9"
+checksum = "19e64526ebdee182341572e50e9ad03965aa510cd94427a4549448f285e957a1"
+dependencies = [
+ "hermit-abi",
+ "libc",
+]
+
+[[package]]
+name = "once_cell"
+version = "1.13.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "074864da206b4973b84eb91683020dbefd6a8c3f0f38e054d93954e891935e4e"
+
+[[package]]
+name = "os_str_bytes"
+version = "6.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9ff7415e9ae3fff1225851df9e0d9e4e5479f947619774677a63572e55e80eff"
+
+[[package]]
+name = "panic-probe"
+version = "0.3.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3ab1f00eac22bd18f8e5cae9555f2820b3a0c166b5b556ee3e203746ea6dcf3a"
 dependencies = [
  "cortex-m",
 ]
@@ -707,7 +912,7 @@ dependencies = [
  "proc-macro-error-attr",
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn",
  "version_check",
 ]
 
@@ -724,27 +929,89 @@ dependencies = [
 
 [[package]]
 name = "proc-macro2"
-version = "1.0.59"
+version = "1.0.43"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6aeca18b86b413c660b781aa319e4e2648a3e6f9eadc9b47e9038e6fe9f3451b"
+checksum = "0a2ca2c61bc9f3d74d2886294ab7b9853abd9c1ad903a3ac7815c58989bb7bab"
 dependencies = [
  "unicode-ident",
 ]
 
 [[package]]
 name = "quote"
-version = "1.0.28"
+version = "1.0.21"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1b9ab9c7eadfd8df19006f1cf1a4aed13540ed5cbc047010ece5826e10825488"
+checksum = "bbe448f377a7d6961e30f5955f9b8d106c3f5e449d493ee1b125c1d43c2b5179"
 dependencies = [
  "proc-macro2",
 ]
 
 [[package]]
 name = "rand_core"
-version = "0.6.4"
+version = "0.6.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
+checksum = "d34f1408f55294453790c48b2f1ebbb1c5b4b7563eb1f418bcfcfdbb06ebb4e7"
+
+[[package]]
+name = "rayon"
+version = "1.5.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bd99e5772ead8baa5215278c9b15bf92087709e9c1b2d1f97cdb5a183c933a7d"
+dependencies = [
+ "autocfg",
+ "crossbeam-deque",
+ "either",
+ "rayon-core",
+]
+
+[[package]]
+name = "rayon-core"
+version = "1.9.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "258bcdb5ac6dad48491bb2992db6b7cf74878b0384908af124823d118c99683f"
+dependencies = [
+ "crossbeam-channel",
+ "crossbeam-deque",
+ "crossbeam-utils",
+ "num_cpus",
+]
+
+[[package]]
+name = "regex"
+version = "1.6.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4c4eb3267174b8c6c2f654116623910a0fef09c4753f8dd83db29c48a0df988b"
+dependencies = [
+ "aho-corasick",
+ "memchr",
+ "regex-syntax",
+]
+
+[[package]]
+name = "regex-syntax"
+version = "0.6.27"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a3f87b73ce11b1619a3c6332f45341e0047173771e8b8b73f87bfeefb7b56244"
+
+[[package]]
+name = "riscv"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6907ccdd7a31012b70faf2af85cd9e5ba97657cc3987c4f13f8e4d2c2a088aba"
+dependencies = [
+ "bare-metal 1.0.0",
+ "bit_field",
+ "riscv-target",
+]
+
+[[package]]
+name = "riscv-target"
+version = "0.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "88aa938cda42a0cf62a20cfe8d139ff1af20c2e681212b5b34adb5a58333f222"
+dependencies = [
+ "lazy_static",
+ "regex",
+]
 
 [[package]]
 name = "rotor-control-stm32"
@@ -755,13 +1022,16 @@ dependencies = [
  "defmt",
  "defmt-rtt",
  "embassy-executor",
- "embassy-futures",
  "embassy-stm32",
- "embassy-sync",
  "embassy-time",
  "embassy-usb",
+ "embassy-usb-serial",
+ "embassy-util",
  "embedded-graphics",
+ "embedded-hal 0.2.7",
+ "futures",
  "heapless",
+ "nb 1.0.0",
  "panic-probe",
  "ssd1306",
  "ufmt",
@@ -782,9 +1052,15 @@ version = "0.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366"
 dependencies = [
- "semver 1.0.17",
+ "semver 1.0.13",
 ]
 
+[[package]]
+name = "ryu"
+version = "1.0.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "4501abdff3ae82a1c1b477a17252eb69cee9e66eb915c1abaa4f44d873df9f09"
+
 [[package]]
 name = "scopeguard"
 version = "1.1.0"
@@ -808,9 +1084,9 @@ dependencies = [
 
 [[package]]
 name = "semver"
-version = "1.0.17"
+version = "1.0.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bebd363326d05ec3e2f532ab7660680f3b02130d780c299bca73469d521bc0ed"
+checksum = "93f6841e709003d68bb2deee8c343572bf446003ec20a583e76f7b15cebf3711"
 
 [[package]]
 name = "semver-parser"
@@ -820,21 +1096,47 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
 [[package]]
 name = "seq-macro"
-version = "0.3.3"
+version = "0.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e6b44e8fc93a14e66336d230954dda83d18b4605ccace8fe09bc7514a71ad0bc"
+checksum = "0772c5c30e1a0d91f6834f8e545c69281c099dfa9a3ac58d96a9fd629c8d4898"
 
 [[package]]
 name = "serde"
-version = "1.0.163"
+version = "1.0.143"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2113ab51b87a539ae008b5c6c02dc020ffa39afd2d83cffcb3f4eb2722cebec2"
+checksum = "53e8e5d5b70924f74ff5c6d64d9a5acd91422117c60f48c4e07855238a254553"
+dependencies = [
+ "serde_derive",
+]
+
+[[package]]
+name = "serde_derive"
+version = "1.0.143"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d3d8e8de557aee63c26b85b947f5e59b690d0454c753f3adeb5cd7835ab88391"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "serde_yaml"
+version = "0.8.26"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "578a7433b776b56a35785ed5ce9a7e777ac0598aac5a6dd1b4b18a307c7fc71b"
+dependencies = [
+ "indexmap",
+ "ryu",
+ "serde",
+ "yaml-rust",
+]
 
 [[package]]
 name = "spin"
-version = "0.9.8"
+version = "0.9.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67"
+checksum = "7f6002a767bff9e83f8eeecf883ecb8011875a21ae8da43bffb817a57e78cc09"
 dependencies = [
  "lock_api",
 ]
@@ -852,31 +1154,12 @@ dependencies = [
  "embedded-hal 0.2.7",
 ]
 
-[[package]]
-name = "ssmarshal"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f3e6ad23b128192ed337dfa4f1b8099ced0c2bf30d61e551b65fda5916dbb850"
-dependencies = [
- "encode_unicode",
- "serde",
-]
-
 [[package]]
 name = "stable_deref_trait"
 version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
 
-[[package]]
-name = "static_cell"
-version = "1.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e4c37c250d21f53fa7165e76e5401d7e6539c211a8d2cf449e3962956a5cc2ce"
-dependencies = [
- "atomic-polyfill 1.0.2",
-]
-
 [[package]]
 name = "stm32-fmc"
 version = "0.2.4"
@@ -888,12 +1171,25 @@ dependencies = [
 
 [[package]]
 name = "stm32-metapac"
-version = "9.0.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ef57e820de79838a3ce3714e4d5de259bdb4f644fc0ee7b339c2c3132175d4f1"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
 dependencies = [
  "cortex-m",
  "cortex-m-rt",
+ "regex",
+ "stm32-metapac-gen",
+]
+
+[[package]]
+name = "stm32-metapac-gen"
+version = "0.1.0"
+source = "git+https://github.com/embassy-rs/embassy.git#aefa5275a2ab2cac6caef599e7adb76ce1beeddd"
+dependencies = [
+ "chiptool",
+ "proc-macro2",
+ "regex",
+ "serde",
+ "serde_yaml",
 ]
 
 [[package]]
@@ -903,10 +1199,24 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623"
 
 [[package]]
-name = "syn"
-version = "1.0.109"
+name = "svd-parser"
+version = "0.10.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237"
+checksum = "697e7645ad9f5311fe3d872d094b135627b1616aea9e1573dddd28ca522579b9"
+dependencies = [
+ "anyhow",
+ "once_cell",
+ "rayon",
+ "regex",
+ "thiserror",
+ "xmltree",
+]
+
+[[package]]
+name = "syn"
+version = "1.0.99"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "58dbef6ec655055e20b86b15a8cc6d439cca19b667537ac6a1369572d151ab13"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -914,34 +1224,38 @@ dependencies = [
 ]
 
 [[package]]
-name = "syn"
-version = "2.0.18"
+name = "termcolor"
+version = "1.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "32d41677bcbe24c20c52e7c70b0d8db04134c5d1066bf98662e2871ad200ea3e"
+checksum = "bab24d30b911b2376f3a13cc2cd443142f0c81dda04c118693e35b3835757755"
 dependencies = [
- "proc-macro2",
- "quote",
- "unicode-ident",
+ "winapi-util",
 ]
 
+[[package]]
+name = "textwrap"
+version = "0.15.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b1141d4d61095b28419e22cb0bbf02755f5e54e0526f97f1e3d1d160e60885fb"
+
 [[package]]
 name = "thiserror"
-version = "1.0.40"
+version = "1.0.32"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "978c9a314bd8dc99be594bc3c175faaa9794be04a5a5e153caba6915336cebac"
+checksum = "f5f6586b7f764adc0231f4c79be7b920e766bb2f3e51b3661cdb263828f19994"
 dependencies = [
  "thiserror-impl",
 ]
 
 [[package]]
 name = "thiserror-impl"
-version = "1.0.40"
+version = "1.0.32"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f"
+checksum = "12bafc5b54507e0149cdf1b145a5d80ab80a90bcd9275df43d4fff68460f6c21"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.18",
+ "syn",
 ]
 
 [[package]]
@@ -962,7 +1276,7 @@ checksum = "d337d3be617449165cb4633c8dece429afd83f84051024079f97ad32a9663716"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn",
 ]
 
 [[package]]
@@ -973,50 +1287,9 @@ checksum = "e87a2ed6b42ec5e28cc3b94c09982969e9227600b2e3dcbc1db927a84c06bd69"
 
 [[package]]
 name = "unicode-ident"
-version = "1.0.9"
+version = "1.0.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b15811caf2415fb889178633e7724bad2509101cde276048e013b9def5e51fa0"
-
-[[package]]
-name = "usb-device"
-version = "0.2.9"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1f6cc3adc849b5292b4075fc0d5fdcf2f24866e88e336dd27a8943090a520508"
-
-[[package]]
-name = "usbd-hid"
-version = "0.6.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "975bd411f4a939986751ea09992a24fa47c4d25c6ed108d04b4c2999a4fd0132"
-dependencies = [
- "serde",
- "ssmarshal",
- "usb-device",
- "usbd-hid-macros",
-]
-
-[[package]]
-name = "usbd-hid-descriptors"
-version = "0.1.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dcbee8c6735e90894fba04770bc41e11fd3c5256018856e15dc4dd1e6c8a3dd1"
-dependencies = [
- "bitfield",
-]
-
-[[package]]
-name = "usbd-hid-macros"
-version = "0.6.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "261079a9ada015fa1acac7cc73c98559f3a92585e15f508034beccf6a2ab75a2"
-dependencies = [
- "byteorder",
- "proc-macro2",
- "quote",
- "serde",
- "syn 1.0.109",
- "usbd-hid-descriptors",
-]
+checksum = "c4f5b37a154999a8f3f98cc23a628d850e154479cd94decf3414696e12e31aaf"
 
 [[package]]
 name = "vcell"
@@ -1044,3 +1317,61 @@ checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6"
 dependencies = [
  "vcell",
 ]
+
+[[package]]
+name = "winapi"
+version = "0.3.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419"
+dependencies = [
+ "winapi-i686-pc-windows-gnu",
+ "winapi-x86_64-pc-windows-gnu",
+]
+
+[[package]]
+name = "winapi-i686-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6"
+
+[[package]]
+name = "winapi-util"
+version = "0.1.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178"
+dependencies = [
+ "winapi",
+]
+
+[[package]]
+name = "winapi-x86_64-pc-windows-gnu"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
+
+[[package]]
+name = "xml-rs"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3c1cb601d29fe2c2ac60a2b2e5e293994d87a1f6fa9687a31a15270f909be9c2"
+dependencies = [
+ "bitflags",
+]
+
+[[package]]
+name = "xmltree"
+version = "0.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ff8eaee9d17062850f1e6163b509947969242990ee59a35801af437abe041e70"
+dependencies = [
+ "xml-rs",
+]
+
+[[package]]
+name = "yaml-rust"
+version = "0.4.5"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "56c1936c4cc7a1c9ab21a1ebb602eb942ba868cbd44a99cb7cdc5892335e1c85"
+dependencies = [
+ "linked-hash-map",
+]
diff --git a/Cargo.toml b/Cargo.toml
index 51110e3..303f31f 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -6,12 +6,12 @@ edition = "2021"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-embassy-sync = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt"] }
-embassy-futures = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt"] }
-embassy-executor = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy.git", features = ["arch-cortex-m", "executor-thread", "defmt", "integrated-timers"] }
-embassy-time = { version = "0.1.1", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt", "defmt-timestamp-uptime"] }
-embassy-stm32 = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["nightly", "defmt", "stm32f103c8", "unstable-pac", "memory-x", "time-driver-any", "unstable-traits"]  }
+embassy-util = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt"] }
+embassy-executor = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt", "integrated-timers"] }
+embassy-time = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt", "defmt-timestamp-uptime", "tick-32768hz"] }
+embassy-stm32 = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["nightly", "defmt", "stm32f103c8", "unstable-pac", "memory-x", "time-driver-any"]  }
 embassy-usb = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt"] }
+embassy-usb-serial = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy.git", features = ["defmt"] }
 
 defmt = "0.3.2"
 defmt-rtt = "0.3.2"
@@ -19,8 +19,10 @@ panic-probe = { version = "0.3.0"}
 
 cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
 cortex-m-rt = "0.7.0"
-
+embedded-hal = "0.2.6"
+futures = { version = "0.3.17", default-features = false, features = ["async-await"] }
 heapless = { version = "0.7.5", default-features = false, features = ['ufmt-impl']}
+nb = "1.0.0"
 ufmt = "0.2.0"
 
 ssd1306 = "0.7.1"
diff --git a/src/display.rs b/src/display.rs
index ae1a49d..5869265 100644
--- a/src/display.rs
+++ b/src/display.rs
@@ -1,14 +1,10 @@
 use embassy_stm32::i2c;
 use embassy_stm32::peripherals;
-use embassy_stm32::bind_interrupts;
-use embassy_stm32::dma::NoDma;
 use embassy_stm32::time::Hertz;
 use embassy_time::{Duration, Timer};
-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
-use embassy_sync::channel::Receiver;
-use embassy_futures::select::{select, Either};
-
-
+use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_util::channel::mpmc::Receiver;
+use embassy_util::{select, Either};
 
 use embedded_graphics::{
     mono_font::{
@@ -28,10 +24,6 @@ use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
 
 use crate::{AzElPair, RotorState};
 
-bind_interrupts!(struct Irqs {
-    I2C1_EV => i2c::InterruptHandler<peripherals::I2C1>;
-});
-
 #[embassy_executor::task]
 pub async fn display_task(
     i2c1: peripherals::I2C1,
@@ -39,7 +31,7 @@ pub async fn display_task(
     scl: peripherals::PB7,
     cmd_receiver: Receiver<'static, ThreadModeRawMutex, RotorState, 1>,
 ) {
-    let i2c = i2c::I2c::new(i2c1, sda, scl, Irqs, NoDma, NoDma, Hertz(100_000), Default::default());
+    let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default());
 
     let interface = I2CDisplayInterface::new(i2c);
 
@@ -58,11 +50,9 @@ pub async fn display_task(
     let mut rotor_state = RotorState {
         actual_pos: AzElPair { az: 0, el: 0 },
         setpoint_pos: AzElPair { az: 0, el: 0 },
-        stopped: true,
+        stopped: false,
     };
 
-    let mut activity_indicator = 0;
-
     loop {
         display.clear();
 
@@ -82,38 +72,23 @@ pub async fn display_task(
             .draw(&mut display)
             .unwrap();
 
-        if !rotor_state.stopped {
-            Rectangle::new(Point::new(0, 19), Size::new(128, 23))
-                .into_styled(style_filled)
-                .draw(&mut display)
-                .unwrap();
-        };
-
-        let setpoint_style = if !rotor_state.stopped {
-            text_large_inv
-        } else {
-            text_large
-        };
+        Rectangle::new(Point::new(0, 19), Size::new(128, 23))
+            .into_styled(style_filled)
+            .draw(&mut display)
+            .unwrap();
 
         tmp.clear();
         uwrite!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
-        Text::new(&tmp, Point::new(1, 30), setpoint_style)
+        Text::new(&tmp, Point::new(1, 30), text_large_inv)
             .draw(&mut display)
             .unwrap();
 
         tmp.clear();
         uwrite!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap();
-        Text::new(&tmp, Point::new(64, 30), setpoint_style)
+        Text::new(&tmp, Point::new(64, 30), text_large_inv)
             .draw(&mut display)
             .unwrap();
 
-        if rotor_state.stopped || activity_indicator < 19 {
-            display.set_pixel(127, activity_indicator, true);
-        } else {
-            display.set_pixel(127, activity_indicator, false);
-        }
-        activity_indicator = (activity_indicator + 1) % 32;
-
         display.flush().unwrap();
 
         let result = select(
diff --git a/src/main.rs b/src/main.rs
index b214730..21278b1 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -2,7 +2,9 @@
 #![no_main]
 #![feature(type_alias_impl_trait)]
 
-use defmt::Format;
+use core::fmt::Write;
+
+use defmt::{panic, Format};
 use defmt_rtt as _;
 use panic_probe as _;
 
@@ -11,8 +13,9 @@ use embassy_stm32::gpio::{Level, Output, Speed};
 use embassy_stm32::time::Hertz;
 use embassy_stm32::Config;
 use embassy_time::{Duration, Timer};
-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
-use embassy_sync::channel::Channel;
+use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_util::channel::mpmc::Channel;
+use embassy_util::Forever;
 
 mod display;
 use display::display_task;
@@ -71,13 +74,10 @@ async fn main(spawner: Spawner) {
 
     spawner
         .spawn(movement_task(
-            p.ADC1,
-            p.PA0,
-            p.PA1,
-            p.PA2,
             p.PA3,
             p.PA4,
             p.PA5,
+            p.PA6,
             CMD_CHAN.receiver(),
             POS_CHAN.sender(),
             STATE_CHAN.sender(),
diff --git a/src/movement.rs b/src/movement.rs
index a4f61c3..211164d 100644
--- a/src/movement.rs
+++ b/src/movement.rs
@@ -1,189 +1,91 @@
-use embassy_stm32::adc::Adc;
 use embassy_stm32::gpio::{Level, Output, Speed};
 use embassy_stm32::peripherals;
-use embassy_time::{with_timeout, Delay, Duration, Timer};
-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
-use embassy_sync::channel::{Receiver, Sender};
-use embassy_futures::select::{select, Either};
-use embassy_futures::join::join;
-use heapless::Vec;
+use embassy_time::{Duration, Instant, Timer};
+use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_util::channel::mpmc::{Receiver, Sender};
+use embassy_util::{select, Either};
+
+use futures::future::join;
 
 use crate::usb::Gs232Cmd;
 use crate::{AzElPair, RotorState};
 
-// ADC reading for azimuth 0°
-const AZ_MIN_READING: f32 = 0.0;
-// ADC reading for azimuth 360°
-const AZ_MAX_READING: f32 = 4096.0;
-// Range of motion for azimuth (0 to AZ_RANGE)
-const AZ_RANGE: f32 = 360.0;
-// Tolerance for the azimuth setpoint
-const AZ_SLOP : i16 = 1;
-
-// ADC reading for elevation 0°
-const EL_MIN_READING: f32 = 0.0;
-// ADC reading for elevation 360°
-const EL_MAX_READING: f32 = 4096.0;
-// Range of motion for elevantion (0 to EL_RANGE)
-const EL_RANGE: f32 = 180.0;
-// Tolerance for the elevation setpoint
-const EL_SLOP : i16 = 1;
-
 #[embassy_executor::task]
 pub async fn movement_task(
-    adc1: peripherals::ADC1,
-    mut az_pin: peripherals::PA0,
-    mut el_pin: peripherals::PA1,
-    cw_pin: peripherals::PA2,
-    ccw_pin: peripherals::PA3,
-    up_pin: peripherals::PA4,
-    down_pin: peripherals::PA5,
+    cw_pin: peripherals::PA3,
+    ccw_pin: peripherals::PA4,
+    up_pin: peripherals::PA5,
+    down_pin: peripherals::PA6,
     cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
     pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
     state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
 ) {
-    // Initialize the rotor state
     let mut rotor_state = RotorState {
         actual_pos: AzElPair { az: 0, el: 0 },
         setpoint_pos: AzElPair { az: 0, el: 0 },
-        stopped: true,
+        stopped: false,
     };
 
-    // Setup output pins for moving the rotor
     let mut cw_pin = Output::new(cw_pin, Level::Low, Speed::Low);
     let mut ccw_pin = Output::new(ccw_pin, Level::Low, Speed::Low);
     let mut up_pin = Output::new(up_pin, Level::Low, Speed::Low);
     let mut down_pin = Output::new(down_pin, Level::Low, Speed::Low);
 
-    // Setup the ADC for reading the rotor positions
-    let mut adc = Adc::new(adc1, &mut Delay);
-
-    // Do an initial ADC reading to initialize the averages
-    let az_reading = adc.read(&mut az_pin) as f32;
-    let el_reading = adc.read(&mut el_pin) as f32;
-    let mut az_average = Average::new(az_reading);
-    let mut el_average = Average::new(el_reading);
-
     loop {
-        // Wait until either a new command has been received or 100ms have elapsed
         match select(
             cmd_receiver.recv(),
             Timer::after(Duration::from_millis(100)),
         )
         .await
         {
-            // A new command has been received. This task only cares about MoveTo and Stop.
             Either::First(cmd) => match cmd {
-                // Move to command. Update the setpoint pair in the rotor state
                 Gs232Cmd::MoveTo(pair) => {
                     rotor_state.setpoint_pos = pair;
                     rotor_state.stopped = false;
                 }
-                // Stop command. Set the stopped flag.
                 Gs232Cmd::Stop => {
                     rotor_state.stopped = true;
                 }
-                // Everthing elese is an noop.
                 _ => {}
             },
-
-            // Second case of the select statement. Timer has elapsed.
             Either::Second(_) => {
-                // First read the current rotor position
-                let az_reading = adc.read(&mut az_pin) as f32;
-                let el_reading = adc.read(&mut el_pin) as f32;
-
-                // Apply the averaging filters
-                az_average.add(az_reading);
-                el_average.add(el_reading);
-
-                // Calculate the position in degreee
-                let az_actual = (az_average.average() - AZ_MIN_READING)
-                    / (AZ_MAX_READING - AZ_MIN_READING)
-                    * AZ_RANGE;
-                let el_actual = (el_average.average() - EL_MIN_READING)
-                    / (EL_MAX_READING - EL_MIN_READING)
-                    * EL_RANGE;
-
-                // Update the rotor state
-                rotor_state.actual_pos.az = az_actual as u16;
-                rotor_state.actual_pos.el = el_actual as u16;
-
-                let delta_az =
-                    rotor_state.setpoint_pos.az as i16 - rotor_state.actual_pos.az as i16;
-                let delta_el =
-                    rotor_state.setpoint_pos.el as i16 - rotor_state.actual_pos.el as i16;
-
-                if !rotor_state.stopped && delta_az > AZ_SLOP {
-                    // Azimuth needs to move clockwise
+                if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az {
+                    rotor_state.actual_pos.az += 1;
                     cw_pin.set_high();
                     ccw_pin.set_low();
-                } else if !rotor_state.stopped && delta_az < -AZ_SLOP {
-                    // Azimuth needs to move counter clockwise
+                } else if !rotor_state.stopped
+                    && rotor_state.actual_pos.az > rotor_state.setpoint_pos.az
+                {
+                    rotor_state.actual_pos.az -= 1;
                     cw_pin.set_low();
                     ccw_pin.set_high();
                 } else {
-                    // Either azimuth is on the setpoint or the rotor has beend stopped.
                     cw_pin.set_low();
                     ccw_pin.set_low();
                 }
 
-                if !rotor_state.stopped && delta_el > EL_SLOP {
-                    // Elevation needs to move up
+                if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el {
+                    rotor_state.actual_pos.el += 1;
                     up_pin.set_high();
                     down_pin.set_low();
-                } else if !rotor_state.stopped && delta_el < -EL_SLOP {
-                    // Elevation needs to move down
+                } else if !rotor_state.stopped
+                    && rotor_state.actual_pos.el > rotor_state.setpoint_pos.el
+                {
+                    rotor_state.actual_pos.el -= 1;
                     up_pin.set_low();
                     down_pin.set_high();
                 } else {
-                    // Either elevation is on the setpoint or the rotor has beend stopped.
                     up_pin.set_low();
                     down_pin.set_low();
                 }
 
-                // Send the state to the display task and the position usb.
-                // Use timeouts to prevent blocking if display or usb task are unresponsive.
-                let _ = join(
-                    with_timeout(
-                        Duration::from_millis(100),
-                        pos_sender.send(rotor_state.actual_pos),
-                    ),
-                    with_timeout(Duration::from_millis(100), state_sender.send(rotor_state)),
+                join(
+                    pos_sender.send(rotor_state.actual_pos),
+                    state_sender.send(rotor_state),
                 )
                 .await;
+                //state_sender.send(rotor_state).await;
             }
         };
     }
 }
-
-// Simple sliding average filter
-struct Average {
-    pos: usize,
-    data: Vec<f32, 5>,
-}
-
-impl Average {
-    // Create a new filter and prefill the state using an initial value
-    fn new(initial: f32) -> Average {
-        let mut data: Vec<f32, 5> = Vec::new();
-        data.resize(5, initial).unwrap();
-        Average { pos: 0, data }
-    }
-
-    // Adds a new value to internal state
-    fn add(&mut self, sample: f32) {
-        self.data[self.pos] = sample;
-        self.pos = (self.pos + 1) % self.data.len();
-    }
-
-    // Calculate the average value from the internal state
-    fn average(&self) -> f32 {
-        let mut sum = 0.0;
-        for sample in &self.data {
-            sum += sample;
-        }
-
-        sum / self.data.len() as f32
-    }
-}
diff --git a/src/usb.rs b/src/usb.rs
index 12503ac..5bfd5d5 100644
--- a/src/usb.rs
+++ b/src/usb.rs
@@ -1,25 +1,21 @@
 use defmt::Format;
 
+use embassy_stm32::interrupt;
 use embassy_stm32::peripherals;
-use embassy_stm32::{bind_interrupts, usb};
 use embassy_stm32::usb::Driver;
 use embassy_usb::Builder;
-use embassy_usb::class::cdc_acm::{CdcAcmClass, State};
-use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
-use embassy_sync::channel::{Receiver, Sender};
-use embassy_futures::select::{select, Either};
-use embassy_futures::join::join;
+use embassy_usb_serial::{CdcAcmClass, State};
+use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_util::channel::mpmc::{Receiver, Sender};
+use embassy_util::{select, Either};
+
+use futures::future::join;
 
 use heapless::String;
 use ufmt::uwrite;
 
 use crate::AzElPair;
 
-bind_interrupts!(struct Irqs {
-    USB_LP_CAN1_RX0 => usb::InterruptHandler<peripherals::USB>;
-});
-
-
 #[embassy_executor::task]
 pub async fn usb_task(
     usb: peripherals::USB,
@@ -28,10 +24,12 @@ pub async fn usb_task(
     cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
     pos_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>,
 ) {
-    let driver = Driver::new(usb, Irqs, dp_pin, dm_pin);
+    let irq = interrupt::take!(USB_LP_CAN1_RX0);
+    let driver = Driver::new(usb, irq, dp_pin, dm_pin);
 
     // Create embassy-usb Config
     let config = embassy_usb::Config::new(0xc0de, 0xcafe);
+    //config.max_packet_size_0 = 64;
 
     // Create embassy-usb DeviceBuilder using the driver and config.
     // It needs some buffers for building the descriptors.
@@ -40,7 +38,7 @@ pub async fn usb_task(
     let mut bos_descriptor = [0; 256];
     let mut control_buf = [0; 7];
 
-    let mut usb_state = State::new();
+    let mut state = State::new();
 
     let mut builder = Builder::new(
         driver,
@@ -48,51 +46,41 @@ pub async fn usb_task(
         &mut device_descriptor,
         &mut config_descriptor,
         &mut bos_descriptor,
-        &mut control_buf
+        &mut control_buf,
+        None,
     );
 
     // Create classes on the builder.
-    let mut class = CdcAcmClass::new(&mut builder, &mut usb_state, 64);
+    let mut class = CdcAcmClass::new(&mut builder, &mut state, 64);
 
     // Build the builder.
     let mut usb = builder.build();
 
-    // Create a future to handle incomming usb packets
+    // Do stuff with the class!
     let usb_handler_fut = async {
-        // Initialize the current position in case we get a B or C command,
-        // before we get the first the update via pos_receiver
         let mut current_pos = AzElPair { az: 0, el: 0 };
 
         loop {
-            // No much used doing anything until we have a usb connection
             class.wait_connection().await;
             defmt::info!("USB connected");
-            // Allocate a space for incomming usb data packets
             let mut packet = [0; 64];
-            // Allocate a string to act as buffer to pares the packets linewise
             let mut buffer: String<64> = String::new();
 
             loop {
                 let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await {
-                    // The read_packet furture returned either usb data or an error.
                     Either::First(res) => match res {
-                        // In case of an error break the loop and treat it like an usb disconnect
                         Ok(n) => n,
-                        // In case of an error break the loop and treat it like an usb disconnect
                         Err(err) => {
                             defmt::error!("Unable to read packet: {}", err);
                             break;
                         }
                     },
-                    // The pos_receiver future returned a position update from moment task.
-                    // Just update position and restart loop.
                     Either::Second(pair) => {
                         current_pos = pair;
                         continue;
                     }
                 };
 
-                // Append the data in the packet buffer to the buffer string
                 for byte in &packet[..n] {
                     if buffer.len() == 64 {
                         buffer.clear();
@@ -100,115 +88,91 @@ pub async fn usb_task(
                     buffer.push(*byte as char).unwrap();
                 }
 
-                // Check if the buffer string contains a '\r'
                 let line_end = match buffer.rfind('\r') {
-                    // Carriage return found, keep the index
                     Some(n) => n,
-                    // No carriage return, wait for the next package
                     _ => continue,
                 };
 
-                // The is a non-zero amount of characters before the carriage return
+                defmt::info!("Line buffer: {:x}", buffer.as_bytes());
+
                 if line_end > 0 {
-                    // Try the parse the slice leading up to linend as a GS323 command
                     let cmd = parse_command(&buffer.as_str()[..line_end]);
                     defmt::info!("Command: {}", cmd);
 
-                    // Reverse some space for a respose to the command
                     let mut resp: String<16> = String::new();
                     match cmd {
-                        // Get Azimuth command. Respond with last known azimuth
-                        Gs232Cmd::GetAz => {
+                        Gs232Cmd::GetAl => {
                             uwrite!(&mut resp, "AZ={}\r", current_pos.az).unwrap();
                         }
-                        // Get Elevation comman. Respond with last known elevation
-                        Gs232Cmd::GetEl => {
+                        Gs232Cmd::GetEz => {
                             uwrite!(&mut resp, "EL={}\r", current_pos.el).unwrap();
                         }
-                        // Get Azimuth and Elevation. Respond with last known pair
-                        Gs232Cmd::GetAzEl => {
+                        Gs232Cmd::GetAlEz => {
                             uwrite!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el)
                                 .unwrap();
                         }
-                        // Move to command. Send to movement task. Respond with empty line.
                         Gs232Cmd::MoveTo(_) => {
                             cmd_sender.send(cmd).await;
                             resp.push_str("\r").unwrap();
                         }
-                        // Stop command. Send to movement task. Respond with empty line.
                         Gs232Cmd::Stop => {
                             cmd_sender.send(cmd).await;
                             resp.push_str("\r").unwrap();
                         }
-                        // Unknown command or parser error. Complain and do nothing.
                         _ => {
                             defmt::error!("Uknown command: {}", &buffer.as_str()[..line_end]);
                             resp.push_str("Unkown command!\r").unwrap();
                         }
                     }
 
-                    // Write the response back via USB
                     match class.write_packet(resp.as_bytes()).await {
                         Ok(_) => {}
-                        // Error treat like broken usb connection
                         Err(err) => {
                             defmt::error!("Unable to write packet: {}", err);
                             break;
                         }
                     };
                 }
-                // Drop the processed line from the buffer
                 buffer = String::from(&buffer.as_str()[line_end + 1..]);
             }
             defmt::info!("USB disconnected");
-            // USB connection is broken, so better stop the rotor.
-            cmd_sender.send(Gs232Cmd::Stop).await;
         }
     };
 
-    // Run the ubs and handler future both to completion.
-    // None of the ever completes, but they will still be polled continously.
     join(usb.run(), usb_handler_fut).await;
 }
 
-// Enum for the GS232 commands
 #[derive(Format, PartialEq)]
 pub enum Gs232Cmd {
     Unkown,
-    GetAz,
-    GetEl,
-    GetAzEl,
+    GetAl,
+    GetEz,
+    GetAlEz,
     MoveTo(AzElPair),
     Stop,
 }
 
-// Parse a GS232 commmand from a string slice
 fn parse_command(data: &str) -> Gs232Cmd {
     match data.chars().nth(0).unwrap() {
         'B' => {
-            // Get Az command. Format 'B\r'
             if data.len() == 1 {
-                Gs232Cmd::GetAz
+                Gs232Cmd::GetAl
             } else {
                 Gs232Cmd::Unkown
             }
         }
 
         'C' => {
-            // Get AZ and EL. Format 'C2\r'
             if data.len() == 2 && data.chars().nth(1).unwrap() as char == '2' {
-                Gs232Cmd::GetAzEl
-            // Get EL only 'C\r'
+                Gs232Cmd::GetAlEz
             } else if data.len() == 1 {
-                Gs232Cmd::GetEl
+                Gs232Cmd::GetEz
             } else {
                 Gs232Cmd::Unkown
             }
         }
 
         'W' => {
-            // Set position 'Waaa eee\r' with azimuth aaa and elevation eee.
-            // Fortunately rotcld will prepend zeros, so there will always be 3 digits per number.
             if data.len() == 8 {
                 if let Ok(az) = data[1..4].parse::<u16>() {
                     if let Ok(el) = data[5..].parse::<u16>() {
@@ -225,7 +189,6 @@ fn parse_command(data: &str) -> Gs232Cmd {
         }
 
         'S' => {
-            // Stop command. Format 'S\r'
             if data.len() == 1 {
                 Gs232Cmd::Stop
             } else {