initial work on bno085
This commit is contained in:
311
Cargo.lock
generated
311
Cargo.lock
generated
@@ -23,6 +23,15 @@ version = "1.0.97"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "dcfed56ad506cb2c684a14971b8861fdc3baaaae314b9e5f9bb532cbe3ba7a4f"
|
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]]
|
[[package]]
|
||||||
name = "autocfg"
|
name = "autocfg"
|
||||||
version = "1.4.0"
|
version = "1.4.0"
|
||||||
@@ -35,6 +44,12 @@ version = "3.17.0"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf"
|
checksum = "1628fb46dfa0b37568d12e5edd512553eccf6a22a78e8bde00bb4aed84d5bdbf"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "bytemuck"
|
||||||
|
version = "1.23.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "9134a6ef01ce4b366b50689c94f82c14bc72bc5d0386829828a2e2752ef7958c"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "cc"
|
name = "cc"
|
||||||
version = "1.2.17"
|
version = "1.2.17"
|
||||||
@@ -70,6 +85,55 @@ version = "0.8.7"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b"
|
checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b"
|
||||||
|
|
||||||
|
[[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-mock"
|
||||||
|
version = "0.11.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f9a0f04f8886106faf281c47b6a0e4054a369baedaf63591fdb8da9761f3f379"
|
||||||
|
dependencies = [
|
||||||
|
"embedded-hal 0.2.7",
|
||||||
|
"embedded-hal 1.0.0",
|
||||||
|
"embedded-hal-nb",
|
||||||
|
"embedded-time",
|
||||||
|
"nb 1.1.0",
|
||||||
|
"void",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[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-time"
|
||||||
|
version = "0.12.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "d7a4b4d10ac48d08bfe3db7688c402baadb244721f30a77ce360bd24c3dffe58"
|
||||||
|
dependencies = [
|
||||||
|
"num",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "fern"
|
name = "fern"
|
||||||
version = "0.7.1"
|
version = "0.7.1"
|
||||||
@@ -79,6 +143,12 @@ dependencies = [
|
|||||||
"log",
|
"log",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "hex"
|
||||||
|
version = "0.4.3"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "iana-time-zone"
|
name = "iana-time-zone"
|
||||||
version = "0.1.62"
|
version = "0.1.62"
|
||||||
@@ -125,6 +195,43 @@ version = "0.4.27"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
|
checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "matrixmultiply"
|
||||||
|
version = "0.3.10"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
"rawpointer",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "nalgebra"
|
||||||
|
version = "0.33.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "26aecdf64b707efd1310e3544d709c5c0ac61c13756046aaaba41be5c4f66a3b"
|
||||||
|
dependencies = [
|
||||||
|
"approx",
|
||||||
|
"matrixmultiply",
|
||||||
|
"nalgebra-macros",
|
||||||
|
"num-complex 0.4.6",
|
||||||
|
"num-rational 0.4.2",
|
||||||
|
"num-traits",
|
||||||
|
"simba",
|
||||||
|
"typenum",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "nalgebra-macros"
|
||||||
|
version = "0.2.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"syn",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "nautilus_common"
|
name = "nautilus_common"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
@@ -135,15 +242,119 @@ version = "0.1.0"
|
|||||||
dependencies = [
|
dependencies = [
|
||||||
"anyhow",
|
"anyhow",
|
||||||
"chrono",
|
"chrono",
|
||||||
|
"embedded-hal 1.0.0",
|
||||||
|
"embedded-hal-mock",
|
||||||
"fern",
|
"fern",
|
||||||
|
"hex",
|
||||||
"log",
|
"log",
|
||||||
|
"nalgebra",
|
||||||
|
"num-traits",
|
||||||
"rppal",
|
"rppal",
|
||||||
|
"thiserror",
|
||||||
]
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "nautilus_ground"
|
name = "nautilus_ground"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
|
|
||||||
|
[[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"
|
||||||
|
version = "0.3.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
|
||||||
|
dependencies = [
|
||||||
|
"num-complex 0.3.1",
|
||||||
|
"num-integer",
|
||||||
|
"num-iter",
|
||||||
|
"num-rational 0.3.2",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-bigint"
|
||||||
|
version = "0.4.6"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9"
|
||||||
|
dependencies = [
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-complex"
|
||||||
|
version = "0.3.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5"
|
||||||
|
dependencies = [
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[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-iter"
|
||||||
|
version = "0.1.45"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-rational"
|
||||||
|
version = "0.3.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "12ac428b1cb17fce6f731001d307d351ec70a6d202fc2e60f7d4c5e42d8f4f07"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-rational"
|
||||||
|
version = "0.4.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
|
||||||
|
dependencies = [
|
||||||
|
"num-bigint",
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "num-traits"
|
name = "num-traits"
|
||||||
version = "0.2.19"
|
version = "0.2.19"
|
||||||
@@ -159,6 +370,12 @@ version = "1.21.3"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
|
checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "paste"
|
||||||
|
version = "1.0.15"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "proc-macro2"
|
name = "proc-macro2"
|
||||||
version = "1.0.94"
|
version = "1.0.94"
|
||||||
@@ -177,13 +394,25 @@ dependencies = [
|
|||||||
"proc-macro2",
|
"proc-macro2",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "rawpointer"
|
||||||
|
version = "0.2.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "rppal"
|
name = "rppal"
|
||||||
version = "0.22.1"
|
version = "0.22.1"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "c1ce3b019009cff02cb6b0e96e7cc2e5c5b90187dc1a490f8ef1521d0596b026"
|
checksum = "c1ce3b019009cff02cb6b0e96e7cc2e5c5b90187dc1a490f8ef1521d0596b026"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
|
"embedded-hal 0.2.7",
|
||||||
|
"embedded-hal 1.0.0",
|
||||||
|
"embedded-hal-nb",
|
||||||
"libc",
|
"libc",
|
||||||
|
"nb 0.1.3",
|
||||||
|
"spin_sleep",
|
||||||
|
"void",
|
||||||
]
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
@@ -192,12 +421,43 @@ version = "1.0.20"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2"
|
checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "safe_arch"
|
||||||
|
version = "0.7.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323"
|
||||||
|
dependencies = [
|
||||||
|
"bytemuck",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "shlex"
|
name = "shlex"
|
||||||
version = "1.3.0"
|
version = "1.3.0"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
|
checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "simba"
|
||||||
|
version = "0.9.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "b3a386a501cd104797982c15ae17aafe8b9261315b5d07e3ec803f2ea26be0fa"
|
||||||
|
dependencies = [
|
||||||
|
"approx",
|
||||||
|
"num-complex 0.4.6",
|
||||||
|
"num-traits",
|
||||||
|
"paste",
|
||||||
|
"wide",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "spin_sleep"
|
||||||
|
version = "1.3.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "17db5ecef7e0bebeb8bf8bc4c4b554e05e0205d7008f10bb37787892e7a6507b"
|
||||||
|
dependencies = [
|
||||||
|
"windows-sys",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "syn"
|
name = "syn"
|
||||||
version = "2.0.100"
|
version = "2.0.100"
|
||||||
@@ -209,12 +469,44 @@ dependencies = [
|
|||||||
"unicode-ident",
|
"unicode-ident",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[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 = "typenum"
|
||||||
|
version = "1.18.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "unicode-ident"
|
name = "unicode-ident"
|
||||||
version = "1.0.18"
|
version = "1.0.18"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
|
checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "void"
|
||||||
|
version = "1.0.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "wasm-bindgen"
|
name = "wasm-bindgen"
|
||||||
version = "0.2.100"
|
version = "0.2.100"
|
||||||
@@ -273,6 +565,16 @@ dependencies = [
|
|||||||
"unicode-ident",
|
"unicode-ident",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "wide"
|
||||||
|
version = "0.7.32"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22"
|
||||||
|
dependencies = [
|
||||||
|
"bytemuck",
|
||||||
|
"safe_arch",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "windows-core"
|
name = "windows-core"
|
||||||
version = "0.52.0"
|
version = "0.52.0"
|
||||||
@@ -288,6 +590,15 @@ version = "0.1.1"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "76840935b766e1b0a05c0066835fb9ec80071d4c09a16f6bd5f7e655e3c14c38"
|
checksum = "76840935b766e1b0a05c0066835fb9ec80071d4c09a16f6bd5f7e655e3c14c38"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "windows-sys"
|
||||||
|
version = "0.59.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b"
|
||||||
|
dependencies = [
|
||||||
|
"windows-targets",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "windows-targets"
|
name = "windows-targets"
|
||||||
version = "0.52.6"
|
version = "0.52.6"
|
||||||
|
|||||||
@@ -7,8 +7,18 @@ edition = "2024"
|
|||||||
anyhow = "1.0.97"
|
anyhow = "1.0.97"
|
||||||
fern = "0.7.1"
|
fern = "0.7.1"
|
||||||
log = "0.4.27"
|
log = "0.4.27"
|
||||||
rppal = { version = "0.22.1", optional = true }
|
|
||||||
chrono = "0.4.40"
|
chrono = "0.4.40"
|
||||||
|
embedded-hal = "1.0.0"
|
||||||
|
embedded-hal-mock = { version = "0.11.1", optional = true }
|
||||||
|
rppal = { version = "0.22.1", features = ["hal"], optional = true }
|
||||||
|
nalgebra = "0.33.2"
|
||||||
|
hex = "0.4.3"
|
||||||
|
thiserror = "2.0.12"
|
||||||
|
num-traits = "0.2.19"
|
||||||
|
|
||||||
|
[dev-dependencies]
|
||||||
|
embedded-hal-mock = { version = "0.11.1" }
|
||||||
|
|
||||||
[features]
|
[features]
|
||||||
raspi = ["dep:rppal"]
|
raspi = ["dep:rppal"]
|
||||||
|
sim = ["dep:embedded-hal-mock"]
|
||||||
|
|||||||
2
flight/src/data/mod.rs
Normal file
2
flight/src/data/mod.rs
Normal file
@@ -0,0 +1,2 @@
|
|||||||
|
pub mod reader;
|
||||||
|
pub mod writer;
|
||||||
157
flight/src/data/reader.rs
Normal file
157
flight/src/data/reader.rs
Normal file
@@ -0,0 +1,157 @@
|
|||||||
|
pub trait ReadableDataType {
|
||||||
|
const BYTES: usize;
|
||||||
|
fn from_be_slice(data: &[u8]) -> Self;
|
||||||
|
fn from_le_slice(data: &[u8]) -> Self;
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! primitive_readable {
|
||||||
|
( $primitive:ty, $length:expr ) => {
|
||||||
|
impl ReadableDataType for $primitive {
|
||||||
|
const BYTES: usize = $length;
|
||||||
|
fn from_be_slice(data: &[u8]) -> Self {
|
||||||
|
Self::from_be_bytes(data.try_into().unwrap())
|
||||||
|
}
|
||||||
|
fn from_le_slice(data: &[u8]) -> Self {
|
||||||
|
Self::from_le_bytes(data.try_into().unwrap())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
primitive_readable!(u8, 1);
|
||||||
|
primitive_readable!(u16, 2);
|
||||||
|
primitive_readable!(u32, 4);
|
||||||
|
primitive_readable!(u64, 8);
|
||||||
|
primitive_readable!(u128, 16);
|
||||||
|
primitive_readable!(usize, 8);
|
||||||
|
primitive_readable!(i8, 1);
|
||||||
|
primitive_readable!(i16, 2);
|
||||||
|
primitive_readable!(i32, 4);
|
||||||
|
primitive_readable!(i64, 8);
|
||||||
|
primitive_readable!(i128, 16);
|
||||||
|
primitive_readable!(isize, 8);
|
||||||
|
primitive_readable!(f32, 4);
|
||||||
|
primitive_readable!(f64, 8);
|
||||||
|
|
||||||
|
trait DataReadAccess {
|
||||||
|
fn get_data(&self) -> &[u8];
|
||||||
|
fn get_position(&self) -> usize;
|
||||||
|
fn set_position(&mut self, position: usize);
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait DataReader {
|
||||||
|
fn reset(&mut self);
|
||||||
|
|
||||||
|
fn read_be<T>(&mut self) -> Option<T> where T: ReadableDataType;
|
||||||
|
fn read_le<T>(&mut self) -> Option<T> where T: ReadableDataType;
|
||||||
|
fn read(&mut self, bytes: usize) -> Option<&[u8]>;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DataViewReader<'a> {
|
||||||
|
data: &'a [u8],
|
||||||
|
position: usize,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a> DataViewReader<'a> {
|
||||||
|
pub fn new(data: &'a [u8]) -> Self {
|
||||||
|
Self {
|
||||||
|
data,
|
||||||
|
position: 0usize
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a> DataReadAccess for DataViewReader<'a> {
|
||||||
|
fn get_data(&self) -> &[u8] {
|
||||||
|
self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_position(&self) -> usize {
|
||||||
|
self.position
|
||||||
|
}
|
||||||
|
|
||||||
|
fn set_position(&mut self, position: usize) {
|
||||||
|
self.position = position;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DataBufferReader<const LENGTH: usize> {
|
||||||
|
data: [u8; LENGTH],
|
||||||
|
position: usize,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const LENGTH: usize> DataBufferReader<LENGTH> {
|
||||||
|
pub fn new() -> Self {
|
||||||
|
Self {
|
||||||
|
data: [0u8; LENGTH],
|
||||||
|
position: 0usize,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn get_buffer(&mut self) -> &mut [u8] {
|
||||||
|
&mut self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn get_write_buffer(&mut self, length: usize) -> &mut [u8] {
|
||||||
|
self.reset();
|
||||||
|
&mut self.data[..length]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const LENGTH: usize> DataReadAccess for DataBufferReader<LENGTH> {
|
||||||
|
fn get_data(&self) -> &[u8] {
|
||||||
|
&self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_position(&self) -> usize {
|
||||||
|
self.position
|
||||||
|
}
|
||||||
|
|
||||||
|
fn set_position(&mut self, position: usize) {
|
||||||
|
self.position = position;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<U: DataReadAccess> DataReader for U {
|
||||||
|
fn reset(&mut self) {
|
||||||
|
self.set_position(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
fn read_be<T>(&mut self) -> Option<T>
|
||||||
|
where
|
||||||
|
T: ReadableDataType {
|
||||||
|
let position = self.get_position();
|
||||||
|
if self.get_data().len() < position + T::BYTES {
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
let result = T::from_be_slice(
|
||||||
|
&self.get_data()[position..(position + T::BYTES)]
|
||||||
|
);
|
||||||
|
self.set_position(position + T::BYTES);
|
||||||
|
Some(result)
|
||||||
|
}
|
||||||
|
|
||||||
|
fn read_le<T>(&mut self) -> Option<T>
|
||||||
|
where
|
||||||
|
T: ReadableDataType {
|
||||||
|
let position = self.get_position();
|
||||||
|
if self.get_data().len() < position + T::BYTES {
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
let result = T::from_le_slice(
|
||||||
|
&self.get_data()[position..(position + T::BYTES)]
|
||||||
|
);
|
||||||
|
self.set_position(position + T::BYTES);
|
||||||
|
Some(result)
|
||||||
|
}
|
||||||
|
|
||||||
|
fn read(&mut self, bytes: usize) -> Option<&[u8]> {
|
||||||
|
let position = self.get_position();
|
||||||
|
if self.get_data().len() < position + bytes {
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
self.set_position(position + bytes);
|
||||||
|
let result = &self.get_data()[position..(position + bytes)];
|
||||||
|
Some(result)
|
||||||
|
}
|
||||||
|
}
|
||||||
222
flight/src/data/writer.rs
Normal file
222
flight/src/data/writer.rs
Normal file
@@ -0,0 +1,222 @@
|
|||||||
|
use std::cell::RefCell;
|
||||||
|
use std::marker::PhantomData;
|
||||||
|
use thiserror::Error;
|
||||||
|
|
||||||
|
#[derive(Error, Debug, Copy, Clone)]
|
||||||
|
pub enum DataWriterError {
|
||||||
|
#[error("Buffer Full")]
|
||||||
|
BufferFull
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait WriteableDataType {
|
||||||
|
const BYTES: usize;
|
||||||
|
fn to_be_slice(&self, data: &mut [u8]);
|
||||||
|
fn to_le_slice(&self, data: &mut [u8]);
|
||||||
|
}
|
||||||
|
|
||||||
|
macro_rules! primitive_writeable {
|
||||||
|
( $primitive:ty, $length:expr ) => {
|
||||||
|
impl WriteableDataType for $primitive {
|
||||||
|
const BYTES: usize = $length;
|
||||||
|
fn to_be_slice(&self, data: &mut [u8]) {
|
||||||
|
data.copy_from_slice(&self.to_be_bytes());
|
||||||
|
}
|
||||||
|
fn to_le_slice(&self, data: &mut [u8]) {
|
||||||
|
data.copy_from_slice(&self.to_le_bytes());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
primitive_writeable!(u8, 1);
|
||||||
|
primitive_writeable!(u16, 2);
|
||||||
|
primitive_writeable!(u32, 4);
|
||||||
|
primitive_writeable!(u64, 8);
|
||||||
|
primitive_writeable!(u128, 16);
|
||||||
|
primitive_writeable!(usize, 8);
|
||||||
|
primitive_writeable!(i8, 1);
|
||||||
|
primitive_writeable!(i16, 2);
|
||||||
|
primitive_writeable!(i32, 4);
|
||||||
|
primitive_writeable!(i64, 8);
|
||||||
|
primitive_writeable!(i128, 16);
|
||||||
|
primitive_writeable!(isize, 8);
|
||||||
|
primitive_writeable!(f32, 4);
|
||||||
|
primitive_writeable!(f64, 8);
|
||||||
|
|
||||||
|
trait DataWriteAccess {
|
||||||
|
fn get_data(&mut self) -> &mut [u8];
|
||||||
|
fn get_position(&self) -> usize;
|
||||||
|
fn set_position(&mut self, position: usize);
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DeferredDataWrite<'a, T: WriteableDataType, U>
|
||||||
|
where
|
||||||
|
RefCell<U>: DataWriter,
|
||||||
|
<RefCell<U> as DataWriter>::WriterType: DataWriteAccess {
|
||||||
|
writer: &'a RefCell<U>,
|
||||||
|
position: usize,
|
||||||
|
_phantom_data: PhantomData<T>,
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait DataWriter {
|
||||||
|
type WriterType: DataWriteAccess;
|
||||||
|
|
||||||
|
fn reset(&mut self);
|
||||||
|
fn get_position(&self) -> usize;
|
||||||
|
fn get_buffer(&mut self) -> &[u8];
|
||||||
|
|
||||||
|
fn push_be<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError>;
|
||||||
|
fn push_le<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError>;
|
||||||
|
fn push(&self, data: &[u8]) -> Result<(), DataWriterError>;
|
||||||
|
fn defer<T: WriteableDataType>(&self) -> Result<DeferredDataWrite<T, Self::WriterType>, DataWriterError>;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DataViewWriter<'a> {
|
||||||
|
data: &'a mut [u8],
|
||||||
|
position: usize,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a> DataViewWriter<'a> {
|
||||||
|
pub fn new(data: &'a mut [u8]) -> RefCell<Self> {
|
||||||
|
RefCell::new(Self {
|
||||||
|
data,
|
||||||
|
position: 0usize
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a> DataWriteAccess for DataViewWriter<'a> {
|
||||||
|
fn get_data(&mut self) -> &mut [u8] {
|
||||||
|
self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_position(&self) -> usize {
|
||||||
|
self.position
|
||||||
|
}
|
||||||
|
|
||||||
|
fn set_position(&mut self, position: usize) {
|
||||||
|
self.position = position;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DataBufferWriter<const LENGTH: usize> {
|
||||||
|
data: [u8; LENGTH],
|
||||||
|
position: usize,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const LENGTH: usize> DataBufferWriter<LENGTH> {
|
||||||
|
pub fn new() -> RefCell<Self> {
|
||||||
|
RefCell::new(Self {
|
||||||
|
data: [0u8; LENGTH],
|
||||||
|
position: 0usize
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const LENGTH: usize> DataWriteAccess for DataBufferWriter<LENGTH> {
|
||||||
|
fn get_data(&mut self) -> &mut [u8] {
|
||||||
|
&mut self.data
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_position(&self) -> usize {
|
||||||
|
self.position
|
||||||
|
}
|
||||||
|
|
||||||
|
fn set_position(&mut self, position: usize) {
|
||||||
|
self.position = position;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<U: DataWriteAccess> DataWriter for RefCell<U> {
|
||||||
|
type WriterType = U;
|
||||||
|
|
||||||
|
fn reset(&mut self) {
|
||||||
|
self.get_mut().set_position(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_position(&self) -> usize {
|
||||||
|
self.borrow().get_position()
|
||||||
|
}
|
||||||
|
|
||||||
|
fn get_buffer(&mut self) -> &[u8] {
|
||||||
|
let position = self.get_position();
|
||||||
|
&self.get_mut().get_data()[..position]
|
||||||
|
}
|
||||||
|
|
||||||
|
fn push_be<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError> {
|
||||||
|
let position = self.borrow().get_position();
|
||||||
|
if self.borrow_mut().get_data().len() < position + T::BYTES {
|
||||||
|
return Err(DataWriterError::BufferFull);
|
||||||
|
}
|
||||||
|
data.to_be_slice(&mut self.borrow_mut().get_data()[position..(position + T::BYTES)]);
|
||||||
|
self.borrow_mut().set_position(position + T::BYTES);
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn push_le<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError> {
|
||||||
|
let position = self.borrow().get_position();
|
||||||
|
if self.borrow_mut().get_data().len() < position + T::BYTES {
|
||||||
|
return Err(DataWriterError::BufferFull);
|
||||||
|
}
|
||||||
|
data.to_le_slice(&mut self.borrow_mut().get_data()[position..(position + T::BYTES)]);
|
||||||
|
self.borrow_mut().set_position(position + T::BYTES);
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn push(&self, data: &[u8]) -> Result<(), DataWriterError> {
|
||||||
|
let position = self.borrow().get_position();
|
||||||
|
if self.borrow_mut().get_data().len() < position + data.len() {
|
||||||
|
return Err(DataWriterError::BufferFull);
|
||||||
|
}
|
||||||
|
self.borrow_mut().get_data()[position..(position + data.len())].copy_from_slice(data);
|
||||||
|
self.borrow_mut().set_position(position + data.len());
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn defer<T: WriteableDataType>(&self) -> Result<DeferredDataWrite<T, Self::WriterType>, DataWriterError> {
|
||||||
|
let position = self.borrow().get_position();
|
||||||
|
if self.borrow_mut().get_data().len() < position + T::BYTES {
|
||||||
|
return Err(DataWriterError::BufferFull);
|
||||||
|
}
|
||||||
|
self.borrow_mut().set_position(position + T::BYTES);
|
||||||
|
Ok(DeferredDataWrite {
|
||||||
|
writer: self,
|
||||||
|
position,
|
||||||
|
_phantom_data: PhantomData,
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<'a, T: WriteableDataType, U: DataWriteAccess> DeferredDataWrite<'a, T, U> where RefCell<U>: DataWriter {
|
||||||
|
pub fn set_be(self, data: T) {
|
||||||
|
data.to_be_slice(&mut self.writer.borrow_mut().get_data()[self.position..(self.position + T::BYTES)]);
|
||||||
|
}
|
||||||
|
pub fn set_le(self, data: T) {
|
||||||
|
data.to_le_slice(&mut self.writer.borrow_mut().get_data()[self.position..(self.position + T::BYTES)]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::*;
|
||||||
|
use anyhow::Result;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_buffer() -> Result<()> {
|
||||||
|
let mut writer = DataBufferWriter::<128>::new();
|
||||||
|
|
||||||
|
let deferred = writer.defer::<u16>()?;
|
||||||
|
|
||||||
|
writer.push_be(1u8)?;
|
||||||
|
assert_eq!(1, writer.borrow().data[2]);
|
||||||
|
|
||||||
|
assert_eq!(0, writer.borrow().data[0]);
|
||||||
|
assert_eq!(0, writer.borrow().data[1]);
|
||||||
|
deferred.set_be(0x1234);
|
||||||
|
assert_eq!(0x12, writer.borrow().data[0]);
|
||||||
|
assert_eq!(0x34, writer.borrow().data[1]);
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@@ -1,14 +0,0 @@
|
|||||||
use crate::hal::Hal;
|
|
||||||
use log::info;
|
|
||||||
|
|
||||||
pub struct EmulatedHal {}
|
|
||||||
|
|
||||||
impl EmulatedHal {
|
|
||||||
pub fn new() -> Self {
|
|
||||||
info!("Running in Emulated Hardware Mode");
|
|
||||||
|
|
||||||
Self {}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Hal for EmulatedHal {}
|
|
||||||
@@ -1,19 +0,0 @@
|
|||||||
use anyhow::Result;
|
|
||||||
|
|
||||||
pub trait Hal {}
|
|
||||||
|
|
||||||
#[cfg(feature = "raspi")]
|
|
||||||
mod raspi;
|
|
||||||
|
|
||||||
#[cfg(feature = "raspi")]
|
|
||||||
pub fn initialize() -> Result<impl Hal> {
|
|
||||||
raspi::RaspiHal::new()
|
|
||||||
}
|
|
||||||
|
|
||||||
#[cfg(not(feature = "raspi"))]
|
|
||||||
mod emulated;
|
|
||||||
|
|
||||||
#[cfg(not(feature = "raspi"))]
|
|
||||||
pub fn initialize() -> Result<impl Hal> {
|
|
||||||
Ok(emulated::EmulatedHal::new())
|
|
||||||
}
|
|
||||||
@@ -1,23 +0,0 @@
|
|||||||
use crate::hal::Hal;
|
|
||||||
use anyhow::Result;
|
|
||||||
use log::info;
|
|
||||||
use rppal::gpio::Gpio;
|
|
||||||
|
|
||||||
pub struct RaspiHal {
|
|
||||||
gpio: Gpio,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl RaspiHal {
|
|
||||||
pub fn new() -> Result<Self> {
|
|
||||||
let device = rppal::system::DeviceInfo::new()?;
|
|
||||||
info!(
|
|
||||||
"Running on Raspberry Pi Emulated Hardware Model {} with {}",
|
|
||||||
device.model(),
|
|
||||||
device.soc()
|
|
||||||
);
|
|
||||||
|
|
||||||
Self { gpio: Gpio::new()? }
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Hal for RaspiHal {}
|
|
||||||
264
flight/src/hardware/bno085/mod.rs
Normal file
264
flight/src/hardware/bno085/mod.rs
Normal file
@@ -0,0 +1,264 @@
|
|||||||
|
mod shtp;
|
||||||
|
|
||||||
|
use crate::data::reader::{DataBufferReader, DataReader};
|
||||||
|
use crate::hardware::error::{NotAvailableError, SpiError};
|
||||||
|
use crate::hardware::imu::{Imu, ImuMeasurement};
|
||||||
|
use anyhow::{anyhow, bail, ensure, Result};
|
||||||
|
use chrono::TimeDelta;
|
||||||
|
use embedded_hal::spi::SpiDevice;
|
||||||
|
use log::info;
|
||||||
|
use num_traits::ToPrimitive;
|
||||||
|
use crate::data::writer::{DataBufferWriter, DataWriter};
|
||||||
|
|
||||||
|
const RX_BUFFER_LENGTH: usize = 1024;
|
||||||
|
|
||||||
|
const SHTP_VERSION_CHANNEL: u8 = 0;
|
||||||
|
const SHTP_VERSION_TAG: u8 = 0x80;
|
||||||
|
|
||||||
|
const EXECUTABLE_CHANNEL: u8 = 1;
|
||||||
|
const EXECUTABLE_RESET: u8 = 1;
|
||||||
|
|
||||||
|
const CONTROL_CHANNEL: u8 = 2;
|
||||||
|
const INITIALIZE_RESPONSE_ID: u8 = 0xF1;
|
||||||
|
const UNSOLICITED_INITIALIZE_COMMAND: u8 = 0x84;
|
||||||
|
const SET_FEATURE_REPORT_ID: u8 = 0xFD;
|
||||||
|
|
||||||
|
pub struct Bno085<SPI> {
|
||||||
|
spi: SPI,
|
||||||
|
sequence_number: u8,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<SPI> Bno085<SPI>
|
||||||
|
where
|
||||||
|
SPI : SpiDevice<u8>,
|
||||||
|
SPI::Error : Send,
|
||||||
|
SPI::Error : Sync,
|
||||||
|
SPI::Error : 'static,
|
||||||
|
{
|
||||||
|
pub fn new(spi: SPI) -> Self {
|
||||||
|
Self {
|
||||||
|
spi,
|
||||||
|
sequence_number: 0u8
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn init(&mut self) -> Result<()> {
|
||||||
|
let mut rx = DataBufferReader::<RX_BUFFER_LENGTH>::new();
|
||||||
|
|
||||||
|
self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
|
||||||
|
ensure!(header.sequence_number == 0, "First Message Sequence Number");
|
||||||
|
self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
|
||||||
|
ensure!(header.sequence_number == 1, "Second Message Sequence Number");
|
||||||
|
println!("advertisement: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
|
||||||
|
self.handle_generic_message(&mut rx)?;
|
||||||
|
|
||||||
|
// Executable Reset Message
|
||||||
|
self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
|
||||||
|
ensure!(header.sequence_number == 2, "Third Message Sequence Number");
|
||||||
|
self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
|
||||||
|
ensure!(header.sequence_number == 3, "Fourth Message Sequence Number");
|
||||||
|
ensure!(rx.read_be::<u8>().ok_or(NotAvailableError)? == EXECUTABLE_RESET, "Executable Reset Complete");
|
||||||
|
println!("reset_message: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
|
||||||
|
|
||||||
|
// Initialization Message
|
||||||
|
self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
|
||||||
|
ensure!(header.sequence_number == 4, "Fifth Message Sequence Number");
|
||||||
|
self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
|
||||||
|
let header = shtp::Header::parse(&mut rx)?;
|
||||||
|
ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
|
||||||
|
ensure!(header.sequence_number == 5, "Sixth Message Sequence Number");
|
||||||
|
println!("init_message : {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
|
||||||
|
|
||||||
|
let mut tx = DataBufferWriter::<128>::new();
|
||||||
|
self.produce_sensor_enable(&tx)?;
|
||||||
|
self.spi.write( tx.get_buffer()).map_err(SpiError)?;
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn handle_generic_message(&self, rx: &mut impl DataReader) -> Result<()> {
|
||||||
|
let tag = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let length = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let value = rx.read(length as usize).ok_or(NotAvailableError)?;
|
||||||
|
|
||||||
|
if tag == SHTP_VERSION_TAG {
|
||||||
|
let shtp_version = String::from_utf8_lossy(&value[..(length as usize - 1)]);
|
||||||
|
info!("BNO085 SHTP Version: {shtp_version}");
|
||||||
|
return Ok(());
|
||||||
|
}
|
||||||
|
|
||||||
|
Err(anyhow!("Unknown Tag {tag}"))
|
||||||
|
}
|
||||||
|
|
||||||
|
fn handle_initialize_response(&self, rx: &mut impl DataReader) -> Result<()> {
|
||||||
|
let report_id = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
|
||||||
|
if report_id != INITIALIZE_RESPONSE_ID {
|
||||||
|
bail!("Unknown Report ID {report_id}");
|
||||||
|
}
|
||||||
|
|
||||||
|
let _sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let command = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
ensure!(command == UNSOLICITED_INITIALIZE_COMMAND, "");
|
||||||
|
let _command_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _response_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let status = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
ensure!(status == 0, "Status (0 – successful. 1 – Operation failed)");
|
||||||
|
let subsystem = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
info!("BNO085 Subsystem: {subsystem}");
|
||||||
|
let _r2 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r3 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r4 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r5 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r6 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r7 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r8 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r9 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let _r10 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn produce_sensor_enable(
|
||||||
|
&mut self,
|
||||||
|
tx: &impl DataWriter
|
||||||
|
) -> Result<()> {
|
||||||
|
let starting_position = tx.get_position();
|
||||||
|
let length = tx.defer::<u16>()?;
|
||||||
|
tx.push_be(CONTROL_CHANNEL)?;
|
||||||
|
tx.push_be(self.sequence_number)?;
|
||||||
|
self.sequence_number = self.sequence_number.wrapping_add(1);
|
||||||
|
|
||||||
|
self.produce_set_feature(
|
||||||
|
tx,
|
||||||
|
0x01, // Accelerometer,
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
TimeDelta::seconds(1) / 100, // 100Hz
|
||||||
|
TimeDelta::zero(),
|
||||||
|
0
|
||||||
|
)?;
|
||||||
|
self.produce_set_feature(
|
||||||
|
tx,
|
||||||
|
0x02, // Gyroscope (Calibrated)
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
TimeDelta::seconds(1) / 100, // 100Hz
|
||||||
|
TimeDelta::zero(),
|
||||||
|
0
|
||||||
|
)?;
|
||||||
|
self.produce_set_feature(
|
||||||
|
tx,
|
||||||
|
0x03, // Magnetic Field (Calibrated)
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
TimeDelta::seconds(1) / 100, // 100Hz
|
||||||
|
TimeDelta::zero(),
|
||||||
|
0
|
||||||
|
)?;
|
||||||
|
|
||||||
|
length.set_le((tx.get_position() - starting_position) as u16); // Fill in the length as we finish
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
fn produce_set_feature(
|
||||||
|
&self,
|
||||||
|
tx: &impl DataWriter,
|
||||||
|
feature_report_id: u8,
|
||||||
|
feature_flags: u8,
|
||||||
|
change_sensitivity: u16,
|
||||||
|
report_interval: TimeDelta,
|
||||||
|
batch_interval: TimeDelta,
|
||||||
|
configuration_word: u32
|
||||||
|
) -> Result<()> {
|
||||||
|
tx.push_le(SET_FEATURE_REPORT_ID)?;
|
||||||
|
tx.push_le(feature_report_id)?;
|
||||||
|
tx.push_le(feature_flags)?;
|
||||||
|
tx.push_le(change_sensitivity)?;
|
||||||
|
let report_interval = report_interval.num_microseconds()
|
||||||
|
.and_then(|us| us.to_u32())
|
||||||
|
.ok_or(anyhow!("Could Not Convert Report Interval to Microseconds"))?;
|
||||||
|
tx.push_le(report_interval)?;
|
||||||
|
let batch_interval = batch_interval.num_microseconds()
|
||||||
|
.and_then(|us| us.to_u32())
|
||||||
|
.ok_or(anyhow!("Could Not Convert Batch Interval to Microseconds"))?;
|
||||||
|
tx.push_le(batch_interval)?;
|
||||||
|
tx.push_le(configuration_word)?;
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<SPI : SpiDevice> Imu for Bno085<SPI> {
|
||||||
|
fn step(&mut self) -> Result<ImuMeasurement> {
|
||||||
|
todo!("asd")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::*;
|
||||||
|
use crate::test_utils::SilentDrop;
|
||||||
|
use embedded_hal_mock::eh1::spi::Mock as SpiMock;
|
||||||
|
use embedded_hal_mock::eh1::spi::Transaction as SpiTransaction;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_init() -> Result<()> {
|
||||||
|
let expectations = vec![
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
0x0C, 0x00, 0x00, 0x00
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
0x0C, 0x00, 0x00, 0x01, 0x80, 0x06, 0x30, 0x2E, 0x30, 0x2E, 0x30, 0x00,
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
0x05, 0x00, 0x01, 0x02
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
0x05, 0x00, 0x01, 0x03, 1
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
20, 0x00, 0x02, 0x04
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::read_vec(vec![
|
||||||
|
20, 0x00, 0x02, 0x05, 0xF1, 0x00, 0x84, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
|
||||||
|
]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
SpiTransaction::transaction_start(),
|
||||||
|
SpiTransaction::write_vec(vec![55, 0, 2, 0, 253, 1, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 2, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 3, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
|
||||||
|
SpiTransaction::transaction_end(),
|
||||||
|
];
|
||||||
|
|
||||||
|
let spi = SpiMock::new(&expectations);
|
||||||
|
|
||||||
|
let mut bno = SilentDrop::new(Bno085::new(spi));
|
||||||
|
|
||||||
|
bno.init()?;
|
||||||
|
|
||||||
|
bno.spi.done();
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
26
flight/src/hardware/bno085/shtp.rs
Normal file
26
flight/src/hardware/bno085/shtp.rs
Normal file
@@ -0,0 +1,26 @@
|
|||||||
|
use crate::data::reader::DataReader;
|
||||||
|
use crate::hardware::error::NotAvailableError;
|
||||||
|
use anyhow::Result;
|
||||||
|
|
||||||
|
pub struct Header {
|
||||||
|
pub length: u16,
|
||||||
|
pub channel: u8,
|
||||||
|
pub sequence_number: u8
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Header {
|
||||||
|
pub const SIZE: usize = size_of::<u16>() + size_of::<u8>() + size_of::<u8>();
|
||||||
|
|
||||||
|
pub fn parse(bytes: &mut impl DataReader) -> Result<Self> {
|
||||||
|
let length = bytes.read_le::<u16>().ok_or(NotAvailableError)?;
|
||||||
|
let channel = bytes.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
let sequence_number = bytes.read_le::<u8>().ok_or(NotAvailableError)?;
|
||||||
|
|
||||||
|
Ok(Self {
|
||||||
|
length,
|
||||||
|
channel,
|
||||||
|
sequence_number,
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
24
flight/src/hardware/error.rs
Normal file
24
flight/src/hardware/error.rs
Normal file
@@ -0,0 +1,24 @@
|
|||||||
|
use std::error::Error;
|
||||||
|
use std::fmt::{Debug, Display, Formatter};
|
||||||
|
|
||||||
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
pub struct SpiError<ERR: Debug + embedded_hal::spi::Error>(pub ERR);
|
||||||
|
|
||||||
|
impl<ERR: Debug + embedded_hal::spi::Error> Display for SpiError<ERR> {
|
||||||
|
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
|
||||||
|
writeln!(f, "SpiError({})", self.0.kind())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<ERR: Debug + embedded_hal::spi::Error> Error for SpiError<ERR> { }
|
||||||
|
|
||||||
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
pub struct NotAvailableError;
|
||||||
|
|
||||||
|
impl Display for NotAvailableError {
|
||||||
|
fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
|
||||||
|
writeln!(f, "NotAvailableError")
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Error for NotAvailableError { }
|
||||||
12
flight/src/hardware/imu/mod.rs
Normal file
12
flight/src/hardware/imu/mod.rs
Normal file
@@ -0,0 +1,12 @@
|
|||||||
|
use nalgebra::Vector3;
|
||||||
|
use anyhow::Result;
|
||||||
|
|
||||||
|
pub struct ImuMeasurement {
|
||||||
|
pub acceleration: Vector3<f64>,
|
||||||
|
pub angular_velocity: Vector3<f64>,
|
||||||
|
pub magnetic_field: Vector3<f64>,
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait Imu {
|
||||||
|
fn step(&mut self) -> Result<ImuMeasurement>;
|
||||||
|
}
|
||||||
28
flight/src/hardware/mod.rs
Normal file
28
flight/src/hardware/mod.rs
Normal file
@@ -0,0 +1,28 @@
|
|||||||
|
use anyhow::Result;
|
||||||
|
|
||||||
|
pub trait Hardware {
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Hardware for () {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(feature = "raspi")]
|
||||||
|
mod raspi;
|
||||||
|
|
||||||
|
#[cfg(feature = "raspi")]
|
||||||
|
pub fn initialize() -> Result<impl Hardware> {
|
||||||
|
raspi::RaspiHardware::new()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(not(feature = "raspi"))]
|
||||||
|
#[allow(unreachable_code)]
|
||||||
|
pub fn initialize() -> Result<impl Hardware> {
|
||||||
|
panic!("Can not Initialize");
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
mod bno085;
|
||||||
|
mod imu;
|
||||||
|
mod error;
|
||||||
36
flight/src/hardware/raspi/mod.rs
Normal file
36
flight/src/hardware/raspi/mod.rs
Normal file
@@ -0,0 +1,36 @@
|
|||||||
|
use crate::hardware::Hardware;
|
||||||
|
use anyhow::Result;
|
||||||
|
use log::info;
|
||||||
|
use rppal::gpio::Gpio;
|
||||||
|
use rppal::spi::SimpleHalSpiDevice;
|
||||||
|
|
||||||
|
const CLOCK_3MHZ: u32 = 3_000_000;
|
||||||
|
|
||||||
|
pub struct RaspiHardware {
|
||||||
|
gpio: Gpio,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl RaspiHardware {
|
||||||
|
pub fn new() -> Result<Self> {
|
||||||
|
let device = rppal::system::DeviceInfo::new()?;
|
||||||
|
info!(
|
||||||
|
"Running on Raspberry Pi Emulated Hardware Model {} with {}",
|
||||||
|
device.model(),
|
||||||
|
device.soc()
|
||||||
|
);
|
||||||
|
|
||||||
|
Ok(Self {
|
||||||
|
gpio: Gpio::new()?,
|
||||||
|
// spi_bno085: Spi::new(
|
||||||
|
// Bus::Spi1,
|
||||||
|
// SlaveSelect::Ss0,
|
||||||
|
// CLOCK_3MHZ,
|
||||||
|
// Mode::Mode3,
|
||||||
|
// )?
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Hardware for RaspiHardware {
|
||||||
|
|
||||||
|
}
|
||||||
@@ -1,9 +1,9 @@
|
|||||||
use crate::hal::initialize;
|
use crate::hardware::initialize;
|
||||||
use crate::logger::setup_logger;
|
use crate::logger::setup_logger;
|
||||||
use anyhow::Result;
|
use anyhow::Result;
|
||||||
use log::info;
|
use log::info;
|
||||||
|
|
||||||
mod hal;
|
mod hardware;
|
||||||
mod logger;
|
mod logger;
|
||||||
|
|
||||||
pub fn run() -> Result<()> {
|
pub fn run() -> Result<()> {
|
||||||
@@ -19,3 +19,7 @@ pub fn run() -> Result<()> {
|
|||||||
|
|
||||||
Ok(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod test_utils;
|
||||||
|
mod data;
|
||||||
|
|||||||
44
flight/src/test_utils.rs
Normal file
44
flight/src/test_utils.rs
Normal file
@@ -0,0 +1,44 @@
|
|||||||
|
use std::mem::ManuallyDrop;
|
||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
use std::panic;
|
||||||
|
|
||||||
|
pub struct SilentDrop<T: panic::UnwindSafe> {
|
||||||
|
inner: ManuallyDrop<T>
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: panic::UnwindSafe> SilentDrop<T> {
|
||||||
|
pub fn new(inner: T) -> Self {
|
||||||
|
Self {
|
||||||
|
inner: ManuallyDrop::new(inner)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: panic::UnwindSafe> Deref for SilentDrop<T> {
|
||||||
|
type Target = T;
|
||||||
|
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
self.inner.deref()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: panic::UnwindSafe> DerefMut for SilentDrop<T> {
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
self.inner.deref_mut()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: panic::UnwindSafe> Drop for SilentDrop<T> {
|
||||||
|
fn drop(&mut self) {
|
||||||
|
let prev_hook = panic::take_hook();
|
||||||
|
panic::set_hook(Box::new(|_| {}));
|
||||||
|
let inner = unsafe {
|
||||||
|
ManuallyDrop::take(&mut self.inner)
|
||||||
|
};
|
||||||
|
let destroy_closure = || {
|
||||||
|
drop(inner);
|
||||||
|
};
|
||||||
|
if panic::catch_unwind(destroy_closure).is_err() {};
|
||||||
|
panic::set_hook(prev_hook);
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user