adds initial rcs implementation
This commit is contained in:
262
Cargo.lock
generated
262
Cargo.lock
generated
@@ -2,6 +2,15 @@
|
|||||||
# It is not intended for manual editing.
|
# It is not intended for manual editing.
|
||||||
version = 4
|
version = 4
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "aho-corasick"
|
||||||
|
version = "1.1.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301"
|
||||||
|
dependencies = [
|
||||||
|
"memchr",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "android_system_properties"
|
name = "android_system_properties"
|
||||||
version = "0.1.5"
|
version = "0.1.5"
|
||||||
@@ -11,6 +20,56 @@ dependencies = [
|
|||||||
"libc",
|
"libc",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "anstream"
|
||||||
|
version = "0.6.21"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a"
|
||||||
|
dependencies = [
|
||||||
|
"anstyle",
|
||||||
|
"anstyle-parse",
|
||||||
|
"anstyle-query",
|
||||||
|
"anstyle-wincon",
|
||||||
|
"colorchoice",
|
||||||
|
"is_terminal_polyfill",
|
||||||
|
"utf8parse",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "anstyle"
|
||||||
|
version = "1.0.13"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "anstyle-parse"
|
||||||
|
version = "0.2.7"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2"
|
||||||
|
dependencies = [
|
||||||
|
"utf8parse",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "anstyle-query"
|
||||||
|
version = "1.1.5"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc"
|
||||||
|
dependencies = [
|
||||||
|
"windows-sys 0.61.1",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "anstyle-wincon"
|
||||||
|
version = "3.0.11"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d"
|
||||||
|
dependencies = [
|
||||||
|
"anstyle",
|
||||||
|
"once_cell_polyfill",
|
||||||
|
"windows-sys 0.61.1",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "anyhow"
|
name = "anyhow"
|
||||||
version = "1.0.100"
|
version = "1.0.100"
|
||||||
@@ -145,6 +204,12 @@ dependencies = [
|
|||||||
"thiserror",
|
"thiserror",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "colorchoice"
|
||||||
|
version = "1.0.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "colored"
|
name = "colored"
|
||||||
version = "2.2.0"
|
version = "2.2.0"
|
||||||
@@ -359,6 +424,29 @@ dependencies = [
|
|||||||
"num",
|
"num",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "env_filter"
|
||||||
|
version = "0.1.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "1bf3c259d255ca70051b30e2e95b5446cdb8949ac4cd22c0d7fd634d89f568e2"
|
||||||
|
dependencies = [
|
||||||
|
"log",
|
||||||
|
"regex",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "env_logger"
|
||||||
|
version = "0.11.8"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "13c863f0904021b108aa8b2f55046443e6b1ebde8fd4a15c399893aae4fa069f"
|
||||||
|
dependencies = [
|
||||||
|
"anstream",
|
||||||
|
"anstyle",
|
||||||
|
"env_filter",
|
||||||
|
"jiff",
|
||||||
|
"log",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "fern"
|
name = "fern"
|
||||||
version = "0.7.1"
|
version = "0.7.1"
|
||||||
@@ -369,6 +457,12 @@ dependencies = [
|
|||||||
"log",
|
"log",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "fnv"
|
||||||
|
version = "1.0.7"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "futures"
|
name = "futures"
|
||||||
version = "0.3.31"
|
version = "0.3.31"
|
||||||
@@ -491,6 +585,16 @@ dependencies = [
|
|||||||
"wasip2",
|
"wasip2",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "good_lp"
|
||||||
|
version = "1.14.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "776aa1ba88ac058e78408c17f4dbff826a51ae08ed6642f71ca0edd7fe9383f3"
|
||||||
|
dependencies = [
|
||||||
|
"fnv",
|
||||||
|
"microlp",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "http"
|
name = "http"
|
||||||
version = "1.4.0"
|
version = "1.4.0"
|
||||||
@@ -531,6 +635,12 @@ dependencies = [
|
|||||||
"cc",
|
"cc",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "is_terminal_polyfill"
|
||||||
|
version = "1.70.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "itertools"
|
name = "itertools"
|
||||||
version = "0.14.0"
|
version = "0.14.0"
|
||||||
@@ -546,6 +656,30 @@ version = "1.0.17"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "92ecc6618181def0457392ccd0ee51198e065e016d1d527a7ac1b6dc7c1f09d2"
|
checksum = "92ecc6618181def0457392ccd0ee51198e065e016d1d527a7ac1b6dc7c1f09d2"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "jiff"
|
||||||
|
version = "0.2.17"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a87d9b8105c23642f50cbbae03d1f75d8422c5cb98ce7ee9271f7ff7505be6b8"
|
||||||
|
dependencies = [
|
||||||
|
"jiff-static",
|
||||||
|
"log",
|
||||||
|
"portable-atomic",
|
||||||
|
"portable-atomic-util",
|
||||||
|
"serde_core",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "jiff-static"
|
||||||
|
version = "0.2.17"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "b787bebb543f8969132630c51fd0afab173a86c6abae56ff3b9e5e3e3f9f6e58"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"syn 2.0.112",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "js-sys"
|
name = "js-sys"
|
||||||
version = "0.3.77"
|
version = "0.3.77"
|
||||||
@@ -574,12 +708,32 @@ version = "0.4.29"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897"
|
checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "matrixmultiply"
|
||||||
|
version = "0.3.10"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
"rawpointer",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "memchr"
|
name = "memchr"
|
||||||
version = "2.7.6"
|
version = "2.7.6"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "f52b00d39961fc5b2736ea853c9cc86238e165017a493d1d5c8eac6bdc4cc273"
|
checksum = "f52b00d39961fc5b2736ea853c9cc86238e165017a493d1d5c8eac6bdc4cc273"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "microlp"
|
||||||
|
version = "0.2.11"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "51d1790c73b93164ff65868f63164497cb32339458a9297e17e212d91df62258"
|
||||||
|
dependencies = [
|
||||||
|
"log",
|
||||||
|
"sprs",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "mio"
|
name = "mio"
|
||||||
version = "1.1.1"
|
version = "1.1.1"
|
||||||
@@ -618,6 +772,8 @@ dependencies = [
|
|||||||
"embedded-hal 1.0.0",
|
"embedded-hal 1.0.0",
|
||||||
"embedded-hal-bus",
|
"embedded-hal-bus",
|
||||||
"embedded-hal-mock",
|
"embedded-hal-mock",
|
||||||
|
"env_logger",
|
||||||
|
"good_lp",
|
||||||
"log",
|
"log",
|
||||||
"nautilus_common",
|
"nautilus_common",
|
||||||
"postcard",
|
"postcard",
|
||||||
@@ -655,6 +811,21 @@ version = "1.1.0"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d"
|
checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "ndarray"
|
||||||
|
version = "0.17.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "0c7c9125e8f6f10c9da3aad044cc918cf8784fa34de857b1aa68038eb05a50a9"
|
||||||
|
dependencies = [
|
||||||
|
"matrixmultiply",
|
||||||
|
"num-complex 0.4.6",
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
"portable-atomic",
|
||||||
|
"portable-atomic-util",
|
||||||
|
"rawpointer",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "nix"
|
name = "nix"
|
||||||
version = "0.30.1"
|
version = "0.30.1"
|
||||||
@@ -673,7 +844,7 @@ version = "0.3.1"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
|
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
"num-complex",
|
"num-complex 0.3.1",
|
||||||
"num-integer",
|
"num-integer",
|
||||||
"num-iter",
|
"num-iter",
|
||||||
"num-rational",
|
"num-rational",
|
||||||
@@ -689,6 +860,15 @@ dependencies = [
|
|||||||
"num-traits",
|
"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]]
|
[[package]]
|
||||||
name = "num-integer"
|
name = "num-integer"
|
||||||
version = "0.1.46"
|
version = "0.1.46"
|
||||||
@@ -750,6 +930,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 = "once_cell_polyfill"
|
||||||
|
version = "1.70.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "openssl-probe"
|
name = "openssl-probe"
|
||||||
version = "0.2.0"
|
version = "0.2.0"
|
||||||
@@ -768,6 +954,21 @@ version = "0.1.0"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
|
checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "portable-atomic"
|
||||||
|
version = "1.13.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f89776e4d69bb58bc6993e99ffa1d11f228b839984854c7daeb5d37f87cbe950"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "portable-atomic-util"
|
||||||
|
version = "0.2.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "d8a2f0d8d040d7848a709caf78912debcc3f33ee4b3cac47d73d1e1069e83507"
|
||||||
|
dependencies = [
|
||||||
|
"portable-atomic",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "postcard"
|
name = "postcard"
|
||||||
version = "1.1.3"
|
version = "1.1.3"
|
||||||
@@ -866,6 +1067,41 @@ dependencies = [
|
|||||||
"getrandom 0.3.4",
|
"getrandom 0.3.4",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "rawpointer"
|
||||||
|
version = "0.2.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "regex"
|
||||||
|
version = "1.12.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "843bc0191f75f3e22651ae5f1e72939ab2f72a4bc30fa80a066bd66edefc24d4"
|
||||||
|
dependencies = [
|
||||||
|
"aho-corasick",
|
||||||
|
"memchr",
|
||||||
|
"regex-automata",
|
||||||
|
"regex-syntax",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "regex-automata"
|
||||||
|
version = "0.4.13"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "5276caf25ac86c8d810222b3dbb938e512c55c6831a10f3e6ed1c93b84041f1c"
|
||||||
|
dependencies = [
|
||||||
|
"aho-corasick",
|
||||||
|
"memchr",
|
||||||
|
"regex-syntax",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "regex-syntax"
|
||||||
|
version = "0.8.8"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "7a2d987857b319362043e95f5353c0535c1f58eec5336fdfcf626430af7def58"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "ring"
|
name = "ring"
|
||||||
version = "0.17.14"
|
version = "0.17.14"
|
||||||
@@ -1059,6 +1295,12 @@ version = "0.4.11"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "7a2ae44ef20feb57a68b23d846850f861394c2e02dc425a50098ae8c90267589"
|
checksum = "7a2ae44ef20feb57a68b23d846850f861394c2e02dc425a50098ae8c90267589"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "smallvec"
|
||||||
|
version = "1.15.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "67b1b7a3b5fe4f1376887184045fcf45c69e92af734b7aaddc05fb777b6fbd03"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "socket2"
|
name = "socket2"
|
||||||
version = "0.6.1"
|
version = "0.6.1"
|
||||||
@@ -1078,6 +1320,18 @@ dependencies = [
|
|||||||
"windows-sys 0.59.0",
|
"windows-sys 0.59.0",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "sprs"
|
||||||
|
version = "0.11.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "6dca58a33be2188d4edc71534f8bafa826e787cc28ca1c47f31be3423f0d6e55"
|
||||||
|
dependencies = [
|
||||||
|
"ndarray",
|
||||||
|
"num-complex 0.4.6",
|
||||||
|
"num-traits",
|
||||||
|
"smallvec",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "subtle"
|
name = "subtle"
|
||||||
version = "2.6.1"
|
version = "2.6.1"
|
||||||
@@ -1245,6 +1499,12 @@ version = "0.7.6"
|
|||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9"
|
checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "utf8parse"
|
||||||
|
version = "0.2.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "uuid"
|
name = "uuid"
|
||||||
version = "1.19.0"
|
version = "1.19.0"
|
||||||
|
|||||||
@@ -23,4 +23,8 @@ serde = { version = "1.0.228", features = ["derive"], default-features = false }
|
|||||||
thiserror = "2.0.17"
|
thiserror = "2.0.17"
|
||||||
tokio = { version = "1.48.0" }
|
tokio = { version = "1.48.0" }
|
||||||
tokio-util = "0.7.17"
|
tokio-util = "0.7.17"
|
||||||
|
env_logger = "0.11.8"
|
||||||
|
good_lp = { version = "1.14.2", default-features = false }
|
||||||
|
|
||||||
|
[profile.dev.package.good_lp]
|
||||||
|
opt-level = 3
|
||||||
|
|||||||
@@ -7,7 +7,7 @@ edition = "2024"
|
|||||||
anyhow = { workspace = true }
|
anyhow = { workspace = true }
|
||||||
chrono = { workspace = true }
|
chrono = { workspace = true }
|
||||||
ctrlc = { workspace = true }
|
ctrlc = { workspace = true }
|
||||||
derive_more = {workspace = true, features = ["display", "from"]}
|
derive_more = {workspace = true, features = ["display", "from", "add", "add_assign", "not"]}
|
||||||
fern = { workspace = true }
|
fern = { workspace = true }
|
||||||
log = { workspace = true }
|
log = { workspace = true }
|
||||||
postcard = { workspace = true }
|
postcard = { workspace = true }
|
||||||
|
|||||||
@@ -1,48 +1,10 @@
|
|||||||
use chrono::serde::ts_nanoseconds;
|
|
||||||
use chrono::{DateTime, TimeDelta, Utc};
|
|
||||||
use serde::de::DeserializeOwned;
|
use serde::de::DeserializeOwned;
|
||||||
use serde::{Deserialize, Serialize};
|
use serde::{Deserialize, Serialize};
|
||||||
use std::fmt::Debug;
|
use std::fmt::Debug;
|
||||||
use std::ops::{Deref, DerefMut};
|
|
||||||
use std::time::Instant;
|
|
||||||
|
|
||||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
pub mod set_pin;
|
||||||
pub struct SetPin {
|
pub mod set_rcs;
|
||||||
pub pin: u8,
|
pub mod valid_priority_command;
|
||||||
pub value: bool,
|
|
||||||
}
|
|
||||||
|
|
||||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
|
||||||
pub struct ValidPriorityCommand<T>
|
|
||||||
where
|
|
||||||
T: Clone + Debug,
|
|
||||||
{
|
|
||||||
pub inner: T,
|
|
||||||
#[serde(with = "ts_nanoseconds")]
|
|
||||||
pub valid_until: DateTime<Utc>,
|
|
||||||
pub priority: u8,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<T> ValidPriorityCommand<T>
|
|
||||||
where
|
|
||||||
T: Clone + Debug,
|
|
||||||
{
|
|
||||||
/// Get the valid until time as an Instant
|
|
||||||
///
|
|
||||||
/// # Panics
|
|
||||||
/// While this theoretically could panic, there are checks to prevent this.
|
|
||||||
pub fn get_valid_until_instant(&self) -> Instant {
|
|
||||||
let delta = self.valid_until.signed_duration_since(Utc::now());
|
|
||||||
let now = Instant::now();
|
|
||||||
if delta >= TimeDelta::zero() {
|
|
||||||
// Unwrap is safe because we checked that it is not negative
|
|
||||||
now + delta.to_std().unwrap()
|
|
||||||
} else {
|
|
||||||
// Unwrap is safe because we converted the negative to a positive
|
|
||||||
now.checked_sub((-delta).to_std().unwrap()).unwrap_or(now)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||||
pub struct CommandHeader<'a> {
|
pub struct CommandHeader<'a> {
|
||||||
@@ -59,26 +21,3 @@ pub struct OwnedCommandHeader {
|
|||||||
pub trait Command: Serialize + DeserializeOwned {}
|
pub trait Command: Serialize + DeserializeOwned {}
|
||||||
|
|
||||||
impl Command for () {}
|
impl Command for () {}
|
||||||
|
|
||||||
impl Command for SetPin {}
|
|
||||||
|
|
||||||
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Command
|
|
||||||
for ValidPriorityCommand<T>
|
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Deref for ValidPriorityCommand<T> {
|
|
||||||
type Target = T;
|
|
||||||
|
|
||||||
fn deref(&self) -> &Self::Target {
|
|
||||||
&self.inner
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> DerefMut
|
|
||||||
for ValidPriorityCommand<T>
|
|
||||||
{
|
|
||||||
fn deref_mut(&mut self) -> &mut Self::Target {
|
|
||||||
&mut self.inner
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
10
common/src/command/set_pin.rs
Normal file
10
common/src/command/set_pin.rs
Normal file
@@ -0,0 +1,10 @@
|
|||||||
|
use crate::command::Command;
|
||||||
|
use serde::{Deserialize, Serialize};
|
||||||
|
|
||||||
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||||
|
pub struct SetPin {
|
||||||
|
pub pin: u8,
|
||||||
|
pub value: bool,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Command for SetPin {}
|
||||||
11
common/src/command/set_rcs.rs
Normal file
11
common/src/command/set_rcs.rs
Normal file
@@ -0,0 +1,11 @@
|
|||||||
|
use crate::command::Command;
|
||||||
|
use crate::math::Vector;
|
||||||
|
use serde::{Deserialize, Serialize};
|
||||||
|
|
||||||
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||||
|
pub struct SetRcs {
|
||||||
|
pub translation: Vector,
|
||||||
|
pub rotation: Vector,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Command for SetRcs {}
|
||||||
61
common/src/command/valid_priority_command.rs
Normal file
61
common/src/command/valid_priority_command.rs
Normal file
@@ -0,0 +1,61 @@
|
|||||||
|
use crate::command::Command;
|
||||||
|
use chrono::serde::ts_nanoseconds;
|
||||||
|
use chrono::{DateTime, TimeDelta, Utc};
|
||||||
|
use serde::de::DeserializeOwned;
|
||||||
|
use serde::{Deserialize, Serialize};
|
||||||
|
use std::fmt::Debug;
|
||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
use std::time::Instant;
|
||||||
|
|
||||||
|
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||||
|
pub struct ValidPriorityCommand<T>
|
||||||
|
where
|
||||||
|
T: Clone + Debug,
|
||||||
|
{
|
||||||
|
pub inner: T,
|
||||||
|
#[serde(with = "ts_nanoseconds")]
|
||||||
|
pub valid_until: DateTime<Utc>,
|
||||||
|
pub priority: u8,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Command
|
||||||
|
for ValidPriorityCommand<T>
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> ValidPriorityCommand<T>
|
||||||
|
where
|
||||||
|
T: Clone + Debug,
|
||||||
|
{
|
||||||
|
/// Get the valid until time as an Instant
|
||||||
|
///
|
||||||
|
/// # Panics
|
||||||
|
/// While this theoretically could panic, there are checks to prevent this.
|
||||||
|
pub fn get_valid_until_instant(&self) -> Instant {
|
||||||
|
let delta = self.valid_until.signed_duration_since(Utc::now());
|
||||||
|
let now = Instant::now();
|
||||||
|
if delta >= TimeDelta::zero() {
|
||||||
|
// Unwrap is safe because we checked that it is not negative
|
||||||
|
now + delta.to_std().unwrap()
|
||||||
|
} else {
|
||||||
|
// Unwrap is safe because we converted the negative to a positive
|
||||||
|
now.checked_sub((-delta).to_std().unwrap()).unwrap_or(now)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> Deref for ValidPriorityCommand<T> {
|
||||||
|
type Target = T;
|
||||||
|
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
&self.inner
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: Clone + Debug + Command + Serialize + DeserializeOwned> DerefMut
|
||||||
|
for ValidPriorityCommand<T>
|
||||||
|
{
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
&mut self.inner
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -3,6 +3,7 @@ use log::info;
|
|||||||
|
|
||||||
pub mod command;
|
pub mod command;
|
||||||
pub mod logger;
|
pub mod logger;
|
||||||
|
pub mod math;
|
||||||
pub mod on_drop;
|
pub mod on_drop;
|
||||||
pub mod telemetry;
|
pub mod telemetry;
|
||||||
pub mod udp;
|
pub mod udp;
|
||||||
|
|||||||
@@ -43,12 +43,13 @@ pub fn setup_logger(package_name: &'static str) -> Result<()> {
|
|||||||
target = record.target(),
|
target = record.target(),
|
||||||
));
|
));
|
||||||
})
|
})
|
||||||
|
.level_for("tungstenite", LevelFilter::Warn)
|
||||||
|
.level_for("tokio_tungstenite", LevelFilter::Warn)
|
||||||
|
.level_for("microlp", LevelFilter::Warn)
|
||||||
|
.level_for("api", LevelFilter::Info)
|
||||||
.chain(
|
.chain(
|
||||||
fern::Dispatch::new()
|
fern::Dispatch::new()
|
||||||
.level(log_level)
|
.level(log_level)
|
||||||
.level_for("tungstenite", LevelFilter::Warn)
|
|
||||||
.level_for("tokio_tungstenite", LevelFilter::Warn)
|
|
||||||
.level_for("api", LevelFilter::Info)
|
|
||||||
.chain(std::io::stdout()),
|
.chain(std::io::stdout()),
|
||||||
)
|
)
|
||||||
.chain(fern::log_file(log_file.clone())?)
|
.chain(fern::log_file(log_file.clone())?)
|
||||||
|
|||||||
196
common/src/math/mod.rs
Normal file
196
common/src/math/mod.rs
Normal file
@@ -0,0 +1,196 @@
|
|||||||
|
use derive_more::{Add, AddAssign, Display, Neg, Sub, SubAssign};
|
||||||
|
use serde::{Deserialize, Serialize};
|
||||||
|
use std::ops::{Div, DivAssign, Mul, MulAssign};
|
||||||
|
|
||||||
|
#[derive(
|
||||||
|
Debug,
|
||||||
|
Copy,
|
||||||
|
Clone,
|
||||||
|
Add,
|
||||||
|
AddAssign,
|
||||||
|
Sub,
|
||||||
|
SubAssign,
|
||||||
|
PartialEq,
|
||||||
|
Display,
|
||||||
|
Neg,
|
||||||
|
Serialize,
|
||||||
|
Deserialize,
|
||||||
|
)]
|
||||||
|
#[display("({x}, {y}, {z})")]
|
||||||
|
pub struct Vector {
|
||||||
|
pub x: f64,
|
||||||
|
pub y: f64,
|
||||||
|
pub z: f64,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Vector {
|
||||||
|
pub const ZERO: Vector = Vector {
|
||||||
|
x: 0.0,
|
||||||
|
y: 0.0,
|
||||||
|
z: 0.0,
|
||||||
|
};
|
||||||
|
pub const X: Vector = Vector {
|
||||||
|
x: 1.0,
|
||||||
|
y: 0.0,
|
||||||
|
z: 0.0,
|
||||||
|
};
|
||||||
|
pub const Y: Vector = Vector {
|
||||||
|
x: 0.0,
|
||||||
|
y: 1.0,
|
||||||
|
z: 0.0,
|
||||||
|
};
|
||||||
|
pub const Z: Vector = Vector {
|
||||||
|
x: 0.0,
|
||||||
|
y: 0.0,
|
||||||
|
z: 1.0,
|
||||||
|
};
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub const fn new(x: f64, y: f64, z: f64) -> Self {
|
||||||
|
Self { x, y, z }
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub const fn dot_product(self, rhs: Self) -> f64 {
|
||||||
|
self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub const fn cross_product(self, rhs: Self) -> Self {
|
||||||
|
Self {
|
||||||
|
x: self.y * rhs.z - self.z * rhs.y,
|
||||||
|
y: self.z * rhs.x - self.x * rhs.z,
|
||||||
|
z: self.x * rhs.y - self.y * rhs.x,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub const fn length2(self) -> f64 {
|
||||||
|
self.dot_product(self)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub fn length(self) -> f64 {
|
||||||
|
self.length2().sqrt()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub fn normalize(self) -> Self {
|
||||||
|
self / self.length()
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub fn with_length(self, length: f64) -> Self {
|
||||||
|
self.normalize() * length
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub const fn project_onto_vector(self, vector: Self) -> Self {
|
||||||
|
let scale_factor = self.dot_product(vector) / vector.length2();
|
||||||
|
// Manual multiplication to allow const-ification
|
||||||
|
Self {
|
||||||
|
x: vector.x * scale_factor,
|
||||||
|
y: vector.y * scale_factor,
|
||||||
|
z: vector.z * scale_factor,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[must_use]
|
||||||
|
#[inline]
|
||||||
|
pub fn project_onto_plane(self, vector: Self) -> Self {
|
||||||
|
self - self.project_onto_vector(vector)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Default for Vector {
|
||||||
|
#[inline]
|
||||||
|
fn default() -> Self {
|
||||||
|
Self::ZERO
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> Mul<T> for Vector
|
||||||
|
where
|
||||||
|
f64: Mul<T, Output = f64>,
|
||||||
|
T: Copy,
|
||||||
|
{
|
||||||
|
type Output = Self;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: T) -> Self::Output {
|
||||||
|
Self::Output {
|
||||||
|
x: self.x * rhs,
|
||||||
|
y: self.y * rhs,
|
||||||
|
z: self.z * rhs,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> MulAssign<T> for Vector
|
||||||
|
where
|
||||||
|
f64: MulAssign<T>,
|
||||||
|
T: Copy,
|
||||||
|
{
|
||||||
|
#[inline]
|
||||||
|
fn mul_assign(&mut self, rhs: T) {
|
||||||
|
self.x *= rhs;
|
||||||
|
self.y *= rhs;
|
||||||
|
self.z *= rhs;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Mul<Vector> for f64 {
|
||||||
|
type Output = Vector;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: Vector) -> Self::Output {
|
||||||
|
rhs * self
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> Div<T> for Vector
|
||||||
|
where
|
||||||
|
f64: Div<T, Output = f64>,
|
||||||
|
T: Copy,
|
||||||
|
{
|
||||||
|
type Output = Self;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn div(self, rhs: T) -> Self::Output {
|
||||||
|
Self::Output {
|
||||||
|
x: self.x / rhs,
|
||||||
|
y: self.y / rhs,
|
||||||
|
z: self.z / rhs,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> DivAssign<T> for Vector
|
||||||
|
where
|
||||||
|
f64: DivAssign<T>,
|
||||||
|
T: Copy,
|
||||||
|
{
|
||||||
|
#[inline]
|
||||||
|
fn div_assign(&mut self, rhs: T) {
|
||||||
|
self.x /= rhs;
|
||||||
|
self.y /= rhs;
|
||||||
|
self.z /= rhs;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Mul for Vector {
|
||||||
|
type Output = f64;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn mul(self, rhs: Self) -> f64 {
|
||||||
|
self.dot_product(rhs)
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -14,9 +14,11 @@ log = { workspace = true, features = ["max_level_trace", "release_max_level_debu
|
|||||||
nautilus_common = { workspace = true }
|
nautilus_common = { workspace = true }
|
||||||
postcard = { workspace = true }
|
postcard = { workspace = true }
|
||||||
rpi-pal = { workspace = true, features = ["hal"], optional = true }
|
rpi-pal = { workspace = true, features = ["hal"], optional = true }
|
||||||
|
good_lp = { workspace = true, features = ["microlp"] }
|
||||||
|
|
||||||
[dev-dependencies]
|
[dev-dependencies]
|
||||||
embedded-hal-mock = { workspace = true }
|
embedded-hal-mock = { workspace = true }
|
||||||
|
env_logger = { workspace = true }
|
||||||
|
|
||||||
[features]
|
[features]
|
||||||
raspi = ["dep:rpi-pal"]
|
raspi = ["dep:rpi-pal"]
|
||||||
|
|||||||
297
flight/src/commanded_state.rs
Normal file
297
flight/src/commanded_state.rs
Normal file
@@ -0,0 +1,297 @@
|
|||||||
|
use log::trace;
|
||||||
|
use std::any::type_name;
|
||||||
|
use std::fmt::Debug;
|
||||||
|
use std::ops::Deref;
|
||||||
|
use std::time::Instant;
|
||||||
|
|
||||||
|
#[derive(Debug, Clone)]
|
||||||
|
struct StateEntry<T> {
|
||||||
|
state: T,
|
||||||
|
valid_until: Instant,
|
||||||
|
priority: u8,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Debug, Clone)]
|
||||||
|
pub struct CommandedState<T, const N: usize = 2> {
|
||||||
|
entries: [Option<StateEntry<T>>; N],
|
||||||
|
default: T,
|
||||||
|
changed: bool,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T, const N: usize> CommandedState<T, N>
|
||||||
|
where
|
||||||
|
T: Debug,
|
||||||
|
{
|
||||||
|
pub fn new(default: T) -> Self {
|
||||||
|
trace!(
|
||||||
|
"CommandedState::<{}>::new(default: {default:?})",
|
||||||
|
type_name::<T>()
|
||||||
|
);
|
||||||
|
Self {
|
||||||
|
entries: [const { None }; N],
|
||||||
|
default,
|
||||||
|
changed: false,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn consume_changed(&mut self) -> bool {
|
||||||
|
trace!(
|
||||||
|
"CommandedState::<{}>::consume_changed(self: {self:?})",
|
||||||
|
type_name::<T>()
|
||||||
|
);
|
||||||
|
let res = self.changed;
|
||||||
|
self.changed = false;
|
||||||
|
res
|
||||||
|
}
|
||||||
|
|
||||||
|
fn evaluate_change_if<F>(&mut self, current_time: Instant, f: F) -> &T
|
||||||
|
where
|
||||||
|
F: FnOnce(&T, &T) -> bool,
|
||||||
|
{
|
||||||
|
trace!(
|
||||||
|
"CommandedState::<{}>::evaluate_change_if(self: {self:?}, current_time: {current_time:?}, f: {})",
|
||||||
|
type_name::<T>(),
|
||||||
|
type_name::<F>()
|
||||||
|
);
|
||||||
|
let mut original_value: Option<StateEntry<T>> = None;
|
||||||
|
for i in 0..N {
|
||||||
|
if let Some(entry) = &self.entries[0] {
|
||||||
|
if entry.valid_until >= current_time {
|
||||||
|
if let Some(original_value) = original_value {
|
||||||
|
self.changed |= f(&original_value.state, &entry.state);
|
||||||
|
}
|
||||||
|
// SAFETY: we just checked that this index is Some
|
||||||
|
// Unfortunately we can't use entry in order to satisfy the borrow checker
|
||||||
|
return self.entries[0].as_ref().map(|x| &x.state).unwrap();
|
||||||
|
}
|
||||||
|
// An expired entry - we need to remove it
|
||||||
|
|
||||||
|
// Empty out this entry (keeping it in original value in case it is needed)
|
||||||
|
if i == 0 {
|
||||||
|
original_value = self.entries[0].take();
|
||||||
|
} else {
|
||||||
|
self.entries[0] = None;
|
||||||
|
}
|
||||||
|
// Bump up all future entries
|
||||||
|
self.entries.rotate_left(1);
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if let Some(original_value) = original_value {
|
||||||
|
self.changed |= f(&original_value.state, &self.default);
|
||||||
|
}
|
||||||
|
&self.default
|
||||||
|
}
|
||||||
|
|
||||||
|
fn insert_change_if<F>(&mut self, state: T, valid_until: Instant, priority: u8, f: F)
|
||||||
|
where
|
||||||
|
F: FnOnce(&T, &T) -> bool,
|
||||||
|
{
|
||||||
|
trace!(
|
||||||
|
"CommandedState::<{}>::insert_change_if(self: {self:?}, state: {state:?}, valid_until: {valid_until:?}, priority: {priority}, f: {})",
|
||||||
|
type_name::<T>(),
|
||||||
|
type_name::<F>()
|
||||||
|
);
|
||||||
|
for i in 0..N {
|
||||||
|
if let Some(entry) = &mut self.entries[i] {
|
||||||
|
// The current entry exists - let's find out it replacing it is an option
|
||||||
|
if priority >= entry.priority {
|
||||||
|
// We have enough priority to modify this entry
|
||||||
|
if priority == entry.priority || entry.valid_until <= valid_until {
|
||||||
|
// If we are same priority (always replace) or
|
||||||
|
// we will be valid for longer then we can just replace this entry
|
||||||
|
if i == 0 {
|
||||||
|
self.changed |= f(&state, &entry.state);
|
||||||
|
}
|
||||||
|
*entry = StateEntry {
|
||||||
|
state,
|
||||||
|
valid_until,
|
||||||
|
priority,
|
||||||
|
};
|
||||||
|
// Remove entries which will expire before this one
|
||||||
|
for j in (i + 1)..N {
|
||||||
|
if let Some(later_entry) = &self.entries[j] {
|
||||||
|
// If the later entry exists and expires before this one
|
||||||
|
if later_entry.valid_until <= valid_until {
|
||||||
|
self.entries[j] = None;
|
||||||
|
self.entries[j..].rotate_left(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// We want to add a higher priority entry, but we will expire first
|
||||||
|
if i == 0 {
|
||||||
|
self.changed |= f(&state, &entry.state);
|
||||||
|
}
|
||||||
|
self.entries[i..].rotate_right(1);
|
||||||
|
self.entries[i] = Some(StateEntry {
|
||||||
|
state,
|
||||||
|
valid_until,
|
||||||
|
priority,
|
||||||
|
});
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
// No entry exists so we can just put ourselves there
|
||||||
|
if i == 0 {
|
||||||
|
self.changed |= f(&state, &self.default);
|
||||||
|
}
|
||||||
|
self.entries[i] = Some(StateEntry {
|
||||||
|
state,
|
||||||
|
valid_until,
|
||||||
|
priority,
|
||||||
|
});
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// If we got here then we didn't find a valid place to insert it
|
||||||
|
}
|
||||||
|
|
||||||
|
#[allow(unused)]
|
||||||
|
#[inline]
|
||||||
|
pub fn evaluate_changed(&mut self, current_time: Instant) -> &T {
|
||||||
|
self.evaluate_change_if(current_time, |_, _| true)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[allow(unused)]
|
||||||
|
#[inline]
|
||||||
|
pub fn insert_changed(&mut self, state: T, valid_until: Instant, priority: u8) {
|
||||||
|
self.insert_change_if(state, valid_until, priority, |_, _| true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T, const N: usize> CommandedState<T, N>
|
||||||
|
where
|
||||||
|
T: PartialEq + Debug,
|
||||||
|
{
|
||||||
|
#[inline]
|
||||||
|
pub fn evaluate(&mut self, current_time: Instant) -> &T {
|
||||||
|
self.evaluate_change_if(current_time, T::ne)
|
||||||
|
}
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
pub fn insert(&mut self, state: T, valid_until: Instant, priority: u8) {
|
||||||
|
self.insert_change_if(state, valid_until, priority, T::ne);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T, const N: usize> Deref for CommandedState<T, N> {
|
||||||
|
type Target = T;
|
||||||
|
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
for i in 0..N {
|
||||||
|
if let Some(entry) = &self.entries[i] {
|
||||||
|
return &entry.state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
&self.default
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::*;
|
||||||
|
use log::LevelFilter;
|
||||||
|
use std::time::Duration;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn commanded_state() {
|
||||||
|
let _ = env_logger::builder()
|
||||||
|
.is_test(true)
|
||||||
|
.filter_level(LevelFilter::Trace)
|
||||||
|
.try_init();
|
||||||
|
|
||||||
|
let mut state = CommandedState::<_, 2>::new(4);
|
||||||
|
let now = Instant::now();
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
state.insert(5, now, 0);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
assert_eq!(5, *state.evaluate(now));
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
let now = now + Duration::from_secs(1);
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
state.insert(6, now + Duration::from_secs(2), 0);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
assert_eq!(6, *state.evaluate(now));
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
state.insert(7, now + Duration::from_secs(1), 1);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
assert_eq!(7, *state);
|
||||||
|
|
||||||
|
state.insert(8, now + Duration::from_secs(3), 0);
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
assert_eq!(7, *state);
|
||||||
|
|
||||||
|
state.insert(9, now + Duration::from_secs(2), 1);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
assert_eq!(9, *state);
|
||||||
|
|
||||||
|
assert_eq!(9, *state.evaluate(now));
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
let now = now + Duration::from_secs(2);
|
||||||
|
assert_eq!(9, *state.evaluate(now));
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
let now = now + Duration::from_secs(1);
|
||||||
|
assert_eq!(8, *state.evaluate(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
let now = now + Duration::from_secs(1);
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
state.insert(10, now + Duration::from_secs(1), 0);
|
||||||
|
state.insert(11, now, 1);
|
||||||
|
state.insert(12, now + Duration::from_secs(1), 1);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
assert_eq!(12, *state);
|
||||||
|
|
||||||
|
let now = now + Duration::from_secs(2);
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
state.insert(13, now + Duration::from_secs(1), 0);
|
||||||
|
state.insert(14, now, 1);
|
||||||
|
assert_eq!(14, *state);
|
||||||
|
let now = now + Duration::from_secs(2);
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn changed() {
|
||||||
|
let _ = env_logger::builder()
|
||||||
|
.is_test(true)
|
||||||
|
.filter_level(LevelFilter::Trace)
|
||||||
|
.try_init();
|
||||||
|
|
||||||
|
let mut state = CommandedState::<_, 2>::new(4);
|
||||||
|
let now = Instant::now();
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
assert_eq!(4, *state);
|
||||||
|
|
||||||
|
state.insert(4, now + Duration::from_secs(1), 0);
|
||||||
|
assert!(!state.consume_changed());
|
||||||
|
|
||||||
|
let now = Instant::now() + Duration::from_secs(2);
|
||||||
|
assert_eq!(4, *state.evaluate_changed(now));
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
state.insert_changed(4, now + Duration::from_secs(1), 0);
|
||||||
|
assert!(state.consume_changed());
|
||||||
|
|
||||||
|
let now = Instant::now() + Duration::from_secs(2);
|
||||||
|
assert_eq!(4, *state.evaluate(now));
|
||||||
|
}
|
||||||
|
}
|
||||||
42
flight/src/comms/command.rs
Normal file
42
flight/src/comms/command.rs
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
|
||||||
|
use std::fmt::Debug;
|
||||||
|
use std::ops::{Deref, DerefMut};
|
||||||
|
use std::time::Instant;
|
||||||
|
|
||||||
|
#[derive(Clone, Debug)]
|
||||||
|
pub struct InstantPriorityCommand<T>
|
||||||
|
where
|
||||||
|
T: Clone + Debug,
|
||||||
|
{
|
||||||
|
pub inner: T,
|
||||||
|
pub valid_until: Instant,
|
||||||
|
pub priority: u8,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T> From<ValidPriorityCommand<T>> for InstantPriorityCommand<T>
|
||||||
|
where
|
||||||
|
T: Clone + Debug,
|
||||||
|
{
|
||||||
|
fn from(value: ValidPriorityCommand<T>) -> Self {
|
||||||
|
let valid_until = value.get_valid_until_instant();
|
||||||
|
Self {
|
||||||
|
inner: value.inner,
|
||||||
|
valid_until,
|
||||||
|
priority: value.priority,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: Clone + Debug> Deref for InstantPriorityCommand<T> {
|
||||||
|
type Target = T;
|
||||||
|
|
||||||
|
fn deref(&self) -> &Self::Target {
|
||||||
|
&self.inner
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<T: Clone + Debug> DerefMut for InstantPriorityCommand<T> {
|
||||||
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
||||||
|
&mut self.inner
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -1,3 +1,5 @@
|
|||||||
|
pub mod command;
|
||||||
|
|
||||||
use crate::scheduler::{CyclicTask, TaskHandle};
|
use crate::scheduler::{CyclicTask, TaskHandle};
|
||||||
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
|
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
|
||||||
use anyhow::{Result, ensure};
|
use anyhow::{Result, ensure};
|
||||||
@@ -54,7 +56,11 @@ impl<'a, A> CommsTask<'a, A>
|
|||||||
where
|
where
|
||||||
A: ToSocketAddrs + Debug,
|
A: ToSocketAddrs + Debug,
|
||||||
{
|
{
|
||||||
pub fn new(local_port: u16, ground_address: A, state_vector: &'a StateVector) -> Result<Self> {
|
pub fn new<'b: 'a>(
|
||||||
|
local_port: u16,
|
||||||
|
ground_address: A,
|
||||||
|
state_vector: &'b StateVector,
|
||||||
|
) -> Result<Self> {
|
||||||
trace!(
|
trace!(
|
||||||
"CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
|
"CommsTask::new<A={}>(local_port: {local_port}, ground_address: {ground_address:?})",
|
||||||
type_name::<A>()
|
type_name::<A>()
|
||||||
|
|||||||
@@ -46,12 +46,12 @@ pub enum PinoutChannel {
|
|||||||
ExtB(u8),
|
ExtB(u8),
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct DevicePin<'a, Device: PinDevice> {
|
pub struct DevicePin<Device: PinDevice> {
|
||||||
pin: u8,
|
pin: u8,
|
||||||
device: &'a Device,
|
device: Device,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
|
impl<Device: PinDevice> Pin for DevicePin<Device> {
|
||||||
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
||||||
trace!(
|
trace!(
|
||||||
"ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
|
"ChannelPin<Device={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
|
||||||
@@ -61,12 +61,12 @@ impl<Device: PinDevice> Pin for DevicePin<'_, Device> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub enum ChannelPin<'a, A: PinDevice, B: PinDevice> {
|
pub enum ChannelPin<A: PinDevice, B: PinDevice> {
|
||||||
ExtA(DevicePin<'a, A>),
|
ExtA(DevicePin<A>),
|
||||||
ExtB(DevicePin<'a, B>),
|
ExtB(DevicePin<B>),
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
|
impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<A, B> {
|
||||||
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
||||||
trace!(
|
trace!(
|
||||||
"ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
|
"ChannelPin<A={}, B={}>::set(self, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})",
|
||||||
@@ -81,11 +81,11 @@ impl<A: PinDevice, B: PinDevice> Pin for ChannelPin<'_, A, B> {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl PinoutChannel {
|
impl PinoutChannel {
|
||||||
pub fn get_pin<'a>(
|
pub fn get_pin<A, B>(self, ext_a: A, ext_b: B) -> ChannelPin<A, B>
|
||||||
self,
|
where
|
||||||
ext_a: &'a (impl PinDevice + Debug),
|
A: PinDevice + Debug,
|
||||||
ext_b: &'a (impl PinDevice + Debug),
|
B: PinDevice + Debug,
|
||||||
) -> impl Pin {
|
{
|
||||||
trace!("PinoutChannel::get_pin(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}");
|
trace!("PinoutChannel::get_pin(self: {self:?}, ext_a: {ext_a:?}, ext_b: {ext_b:?}");
|
||||||
match self {
|
match self {
|
||||||
PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }),
|
PinoutChannel::ExtA(pin) => ChannelPin::ExtA(DevicePin { pin, device: ext_a }),
|
||||||
|
|||||||
@@ -1,9 +1,11 @@
|
|||||||
|
use crate::commanded_state::CommandedState;
|
||||||
use crate::hardware::mcp23017::Mcp23017;
|
use crate::hardware::mcp23017::Mcp23017;
|
||||||
use crate::hardware::pin::PinDevice;
|
use crate::hardware::pin::PinDevice;
|
||||||
use crate::scheduler::{CyclicTask, TaskHandle};
|
use crate::scheduler::{CyclicTask, TaskHandle};
|
||||||
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
|
use crate::state_vector::{SectionIdentifier, SectionWriter, StateVector};
|
||||||
use embedded_hal::digital::PinState;
|
use embedded_hal::digital::PinState;
|
||||||
use log::trace;
|
use log::trace;
|
||||||
|
use std::array;
|
||||||
use std::fmt::{Debug, Formatter};
|
use std::fmt::{Debug, Formatter};
|
||||||
use std::sync::mpsc::Receiver;
|
use std::sync::mpsc::Receiver;
|
||||||
use std::time::Instant;
|
use std::time::Instant;
|
||||||
@@ -20,7 +22,9 @@ pub enum Mcp23017Message {
|
|||||||
|
|
||||||
impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
|
impl<D: Debug> PinDevice for TaskHandle<Mcp23017Message, D> {
|
||||||
fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
|
fn set_pin(&self, pin: u8, value: PinState, valid_until: Instant, priority: u8) {
|
||||||
trace!("Mcp23017Task::set_pin(self: {self:?}, pin: {pin}, value: {value:?})");
|
trace!(
|
||||||
|
"TaskHandle<Mcp23017Message, D>::set_pin(self: {self:?}, pin: {pin}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
|
||||||
|
);
|
||||||
// This can only fail if the other end is disconnected - which we intentionally want to
|
// This can only fail if the other end is disconnected - which we intentionally want to
|
||||||
// ignore
|
// ignore
|
||||||
let _ = self.sender.send(Mcp23017Message::SetPin {
|
let _ = self.sender.send(Mcp23017Message::SetPin {
|
||||||
@@ -68,107 +72,12 @@ impl AllPins {
|
|||||||
fn new() -> Self {
|
fn new() -> Self {
|
||||||
trace!("AllPins::new()");
|
trace!("AllPins::new()");
|
||||||
Self {
|
Self {
|
||||||
pins: [PinData::new(); _],
|
pins: array::repeat(PinData::new(PinState::Low)),
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Copy, Clone, Debug)]
|
type PinData = CommandedState<PinState>;
|
||||||
struct PinData {
|
|
||||||
state: PinState,
|
|
||||||
valid_until: Option<Instant>,
|
|
||||||
priority: u8,
|
|
||||||
next_state: PinState,
|
|
||||||
next_validity: Option<Instant>,
|
|
||||||
next_priority: u8,
|
|
||||||
default: PinState,
|
|
||||||
changed: bool,
|
|
||||||
value: PinState,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl PinData {
|
|
||||||
fn new() -> Self {
|
|
||||||
trace!("PinData::new()");
|
|
||||||
Self {
|
|
||||||
state: PinState::Low,
|
|
||||||
valid_until: None,
|
|
||||||
priority: 0,
|
|
||||||
next_state: PinState::Low,
|
|
||||||
next_validity: None,
|
|
||||||
next_priority: 0,
|
|
||||||
default: PinState::Low,
|
|
||||||
changed: false,
|
|
||||||
value: PinState::Low,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn evaluate(&mut self, now: Instant) {
|
|
||||||
trace!("PinData::evaluate(self: {self:?}, now: {now:?})");
|
|
||||||
// Do this twice to check both the current and the current next
|
|
||||||
// If the current is currently invalid, we'd upgrade the next to current
|
|
||||||
for _ in 0..2 {
|
|
||||||
let is_current_valid = self.valid_until.is_some_and(|current| current >= now);
|
|
||||||
if is_current_valid {
|
|
||||||
self.value = self.state;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if self.valid_until.is_some() {
|
|
||||||
self.changed = true;
|
|
||||||
}
|
|
||||||
self.state = self.next_state;
|
|
||||||
self.valid_until = self.next_validity;
|
|
||||||
self.priority = self.next_priority;
|
|
||||||
|
|
||||||
self.next_validity = None;
|
|
||||||
self.next_priority = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
self.value = self.default;
|
|
||||||
}
|
|
||||||
|
|
||||||
fn set(&mut self, value: PinState, valid_until: Instant, priority: u8) {
|
|
||||||
trace!(
|
|
||||||
"PinData::set(self: {self:?}, value: {value:?}, valid_until: {valid_until:?}, priority: {priority})"
|
|
||||||
);
|
|
||||||
let can_replace_current = self
|
|
||||||
.valid_until
|
|
||||||
.is_none_or(|current| current <= valid_until)
|
|
||||||
|| self.priority == priority;
|
|
||||||
let can_replace_next = self.next_validity.is_none_or(|next| next <= valid_until)
|
|
||||||
|| self.next_priority == priority;
|
|
||||||
|
|
||||||
if priority >= self.priority {
|
|
||||||
// This is now the highest priority thing (or most recent of equal priority)
|
|
||||||
if can_replace_current {
|
|
||||||
if can_replace_next {
|
|
||||||
self.next_validity = None;
|
|
||||||
self.next_priority = 0;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
self.next_state = self.state;
|
|
||||||
self.next_validity = self.valid_until;
|
|
||||||
self.next_priority = self.priority;
|
|
||||||
}
|
|
||||||
self.state = value;
|
|
||||||
self.valid_until = Some(valid_until);
|
|
||||||
self.priority = priority;
|
|
||||||
self.changed = true;
|
|
||||||
self.value = value;
|
|
||||||
} else {
|
|
||||||
// This is not the highest priority thing
|
|
||||||
if self.priority >= self.next_priority {
|
|
||||||
// Higher priority than the next highest though
|
|
||||||
self.next_state = value;
|
|
||||||
self.next_validity = Some(valid_until);
|
|
||||||
self.next_priority = priority;
|
|
||||||
self.changed = true;
|
|
||||||
} else {
|
|
||||||
// Not high enough priority to remember
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
|
impl<'a, M: Mcp23017 + Debug> Mcp23017Task<'a, M> {
|
||||||
pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
|
pub fn new(mcp23017: M, state_vector: &'a StateVector) -> Self {
|
||||||
@@ -209,7 +118,7 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
|
|||||||
priority,
|
priority,
|
||||||
} => {
|
} => {
|
||||||
if (0u8..16u8).contains(&pin) {
|
if (0u8..16u8).contains(&pin) {
|
||||||
self.pins.pins[pin as usize].set(value, valid_until, priority);
|
self.pins.pins[pin as usize].insert(value, valid_until, priority);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -217,9 +126,8 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
|
|||||||
|
|
||||||
for pin in 0u8..16u8 {
|
for pin in 0u8..16u8 {
|
||||||
let current_pin = &mut self.pins.pins[pin as usize];
|
let current_pin = &mut self.pins.pins[pin as usize];
|
||||||
let state = current_pin.value;
|
let state = **current_pin;
|
||||||
if current_pin.changed {
|
if current_pin.consume_changed() {
|
||||||
current_pin.changed = false;
|
|
||||||
// This shouldn't be able to fail
|
// This shouldn't be able to fail
|
||||||
// TODO: handle error case
|
// TODO: handle error case
|
||||||
let _ = self.mcp23017.set_pin(pin, state);
|
let _ = self.mcp23017.set_pin(pin, state);
|
||||||
@@ -229,7 +137,7 @@ impl<M: Mcp23017 + Debug> CyclicTask for Mcp23017Task<'_, M> {
|
|||||||
if changed {
|
if changed {
|
||||||
let _ = self.mcp23017.flush();
|
let _ = self.mcp23017.flush();
|
||||||
self.state.update(|s| {
|
self.state.update(|s| {
|
||||||
s.pins = self.pins.pins.map(|pin| pin.value == PinState::High);
|
s.pins = array::from_fn(|i| *self.pins.pins[i] == PinState::High);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,5 +1,6 @@
|
|||||||
use embedded_hal::digital::PinState;
|
use embedded_hal::digital::PinState;
|
||||||
use nautilus_common::command::{SetPin, ValidPriorityCommand};
|
use nautilus_common::command::set_pin::SetPin;
|
||||||
|
use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
|
||||||
use std::time::Instant;
|
use std::time::Instant;
|
||||||
|
|
||||||
pub trait PinDevice {
|
pub trait PinDevice {
|
||||||
|
|||||||
@@ -6,6 +6,7 @@ use crate::hardware::initialize;
|
|||||||
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
|
use crate::hardware::mcp23017::{Mcp23017, Mcp23017State, Mcp23017Task};
|
||||||
use crate::hardware::mct8316a::Mct8316a;
|
use crate::hardware::mct8316a::Mct8316a;
|
||||||
use crate::hardware::pin::PinDevice;
|
use crate::hardware::pin::PinDevice;
|
||||||
|
use crate::rcs::RcsTask;
|
||||||
use crate::scheduler::Scheduler;
|
use crate::scheduler::Scheduler;
|
||||||
use crate::state_vector::StateVector;
|
use crate::state_vector::StateVector;
|
||||||
use anyhow::Result;
|
use anyhow::Result;
|
||||||
@@ -70,10 +71,13 @@ pub fn run() -> Result<()> {
|
|||||||
)?;
|
)?;
|
||||||
let b_id = task_b.get_id();
|
let b_id = task_b.get_id();
|
||||||
|
|
||||||
|
let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?;
|
||||||
|
|
||||||
let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?;
|
let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?;
|
||||||
comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?;
|
comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?;
|
||||||
comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
|
comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
|
||||||
comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
|
comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
|
||||||
|
comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?;
|
||||||
let comms = s.run_cyclic("comms-task", comms, 10)?;
|
let comms = s.run_cyclic("comms-task", comms, 10)?;
|
||||||
let comms_id = *comms;
|
let comms_id = *comms;
|
||||||
|
|
||||||
@@ -122,6 +126,7 @@ pub fn run() -> Result<()> {
|
|||||||
Ok(())
|
Ok(())
|
||||||
}
|
}
|
||||||
|
|
||||||
|
mod commanded_state;
|
||||||
mod comms;
|
mod comms;
|
||||||
mod data;
|
mod data;
|
||||||
mod rcs;
|
mod rcs;
|
||||||
|
|||||||
@@ -1,13 +1,153 @@
|
|||||||
// use crate::hardware::mcp23017::Mcp23017OutputPin;
|
mod thruster_balancing;
|
||||||
//
|
|
||||||
// struct RcsTask<PIN: Mcp23017OutputPin> {
|
use crate::commanded_state::CommandedState;
|
||||||
// pin: PIN,
|
use crate::comms::command::InstantPriorityCommand;
|
||||||
// }
|
use crate::hardware::channelization::{
|
||||||
//
|
ChannelPin, PinoutChannel, RCS0, RCS1, RCS2, RCS3, RCS4, RCS5, RCS6, RCS7, RCS8, RCS9,
|
||||||
// impl<PIN: Mcp23017OutputPin> RcsTask<PIN> {
|
};
|
||||||
// pub fn new(pin: PIN) -> Self {
|
use crate::hardware::pin::{Pin, PinDevice};
|
||||||
// Self {
|
use crate::rcs::thruster_balancing::BalancingConstants;
|
||||||
// pin
|
use crate::scheduler::{CyclicTask, TaskHandle};
|
||||||
// }
|
use embedded_hal::digital::PinState;
|
||||||
// }
|
use log::trace;
|
||||||
// }
|
use nautilus_common::command::set_rcs::SetRcs;
|
||||||
|
use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
|
||||||
|
use nautilus_common::math::Vector;
|
||||||
|
use std::fmt::Debug;
|
||||||
|
use std::iter::zip;
|
||||||
|
use std::sync::mpsc::Receiver;
|
||||||
|
use std::time::{Duration, Instant};
|
||||||
|
|
||||||
|
const PIN_PRIORITY: u8 = 0;
|
||||||
|
|
||||||
|
struct Thruster {
|
||||||
|
channel: PinoutChannel,
|
||||||
|
position: Vector,
|
||||||
|
direction: Vector,
|
||||||
|
force: f64,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Thruster {
|
||||||
|
const fn new(channel: PinoutChannel, position: Vector, direction: Vector, force: f64) -> Self {
|
||||||
|
Self {
|
||||||
|
channel,
|
||||||
|
position,
|
||||||
|
direction,
|
||||||
|
force,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const RCS_THRUSTERS: [Thruster; 10] = [
|
||||||
|
Thruster::new(RCS0, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS1, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS2, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS3, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS4, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS5, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS6, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS7, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS8, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
Thruster::new(RCS9, Vector::ZERO, Vector::ZERO, 1.0),
|
||||||
|
];
|
||||||
|
|
||||||
|
#[derive(Clone, Debug)]
|
||||||
|
pub enum RcsMessage {
|
||||||
|
SetRcs(InstantPriorityCommand<SetRcs>),
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<D: Debug> TaskHandle<RcsMessage, D> {
|
||||||
|
pub fn set_rcs(&self, cmd: InstantPriorityCommand<SetRcs>) {
|
||||||
|
trace!("TaskHandle<RcsMessage, D>::set_rcs(self: {self:?}, cmd: {cmd:?})");
|
||||||
|
// This can only fail if the other end is disconnected - which we intentionally want to
|
||||||
|
// ignore
|
||||||
|
let _ = self.sender.send(RcsMessage::SetRcs(cmd));
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn new_set_rcs_callback<'a>(&self) -> impl Fn(ValidPriorityCommand<SetRcs>) + 'a
|
||||||
|
where
|
||||||
|
Self: Sized + Clone + 'a,
|
||||||
|
{
|
||||||
|
let this = self.clone();
|
||||||
|
move |cmd| {
|
||||||
|
this.set_rcs(cmd.into());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Clone)]
|
||||||
|
pub struct RcsTask<P> {
|
||||||
|
rcs_pins: [P; RCS_THRUSTERS.len()],
|
||||||
|
command: CommandedState<(Vector, Vector)>,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A, B> RcsTask<ChannelPin<A, B>>
|
||||||
|
where
|
||||||
|
A: PinDevice + Debug + Clone,
|
||||||
|
B: PinDevice + Debug + Clone,
|
||||||
|
{
|
||||||
|
pub fn new(a: &A, b: &B) -> Self {
|
||||||
|
let rcs_pins = RCS_THRUSTERS.map(|thruster| thruster.channel.get_pin(a.clone(), b.clone()));
|
||||||
|
Self {
|
||||||
|
rcs_pins,
|
||||||
|
command: CommandedState::new((Vector::ZERO, Vector::ZERO)),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<P> CyclicTask for RcsTask<P>
|
||||||
|
where
|
||||||
|
P: Pin,
|
||||||
|
{
|
||||||
|
type Message = RcsMessage;
|
||||||
|
type Data = ();
|
||||||
|
|
||||||
|
fn get_data(&self) -> Self::Data {
|
||||||
|
trace!("RcsTask::get_data(self)");
|
||||||
|
}
|
||||||
|
|
||||||
|
fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
|
||||||
|
trace!("RcsTask::step(self, receiver, step_time: {step_time:?})");
|
||||||
|
|
||||||
|
self.command.evaluate(step_time);
|
||||||
|
|
||||||
|
while let Ok(msg) = receiver.try_recv() {
|
||||||
|
match msg {
|
||||||
|
RcsMessage::SetRcs(InstantPriorityCommand {
|
||||||
|
inner:
|
||||||
|
SetRcs {
|
||||||
|
translation,
|
||||||
|
rotation,
|
||||||
|
},
|
||||||
|
valid_until,
|
||||||
|
priority,
|
||||||
|
}) => {
|
||||||
|
self.command
|
||||||
|
.insert((translation, rotation), valid_until, priority);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
let (translation, rotation) = *self.command;
|
||||||
|
|
||||||
|
let throttle = thruster_balancing::balance_thrust(
|
||||||
|
translation,
|
||||||
|
rotation,
|
||||||
|
&RCS_THRUSTERS,
|
||||||
|
BalancingConstants::default(),
|
||||||
|
);
|
||||||
|
|
||||||
|
for (pin, throttle) in zip(&mut self.rcs_pins, throttle) {
|
||||||
|
pin.set(
|
||||||
|
if throttle >= 0.5 {
|
||||||
|
PinState::High
|
||||||
|
} else {
|
||||||
|
PinState::Low
|
||||||
|
},
|
||||||
|
// This signal is valid for the next half second
|
||||||
|
step_time + Duration::from_secs(1),
|
||||||
|
PIN_PRIORITY,
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
302
flight/src/rcs/thruster_balancing.rs
Normal file
302
flight/src/rcs/thruster_balancing.rs
Normal file
@@ -0,0 +1,302 @@
|
|||||||
|
use crate::rcs::Thruster;
|
||||||
|
use good_lp::{
|
||||||
|
Expression, IntoAffineExpression, ProblemVariables, Solution, SolverModel, microlp, variable,
|
||||||
|
};
|
||||||
|
use log::error;
|
||||||
|
use nautilus_common::math::Vector;
|
||||||
|
use std::ops::Add;
|
||||||
|
|
||||||
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
pub(super) struct BalancingConstants {
|
||||||
|
/// Should be smaller than the other two constants
|
||||||
|
throttle: f64,
|
||||||
|
force: f64,
|
||||||
|
torque: f64,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Default for BalancingConstants {
|
||||||
|
fn default() -> Self {
|
||||||
|
Self {
|
||||||
|
throttle: 1e-3,
|
||||||
|
force: 1.0,
|
||||||
|
torque: 1.0,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[allow(clippy::similar_names)]
|
||||||
|
pub(super) fn balance_thrust<const N: usize>(
|
||||||
|
desired_force: Vector,
|
||||||
|
desired_torque: Vector,
|
||||||
|
thrusters: &[Thruster; N],
|
||||||
|
constants: BalancingConstants,
|
||||||
|
) -> [f64; N] {
|
||||||
|
let mut problem = ProblemVariables::new();
|
||||||
|
let throttles: [_; N] = std::array::from_fn(|_| problem.add(variable().min(0).max(1)));
|
||||||
|
|
||||||
|
let force_error_x_1 = problem.add(variable().min(0));
|
||||||
|
let force_error_x_2 = problem.add(variable().min(0));
|
||||||
|
let force_error_x = force_error_x_1 + force_error_x_2;
|
||||||
|
let force_delta_x = force_error_x_1 - force_error_x_2;
|
||||||
|
let force_error_y_1 = problem.add(variable().min(0));
|
||||||
|
let force_error_y_2 = problem.add(variable().min(0));
|
||||||
|
let force_error_y = force_error_y_1 + force_error_y_2;
|
||||||
|
let force_delta_y = force_error_y_1 - force_error_y_2;
|
||||||
|
let force_error_z_1 = problem.add(variable().min(0));
|
||||||
|
let force_error_z_2 = problem.add(variable().min(0));
|
||||||
|
let force_error_z = force_error_z_1 + force_error_z_2;
|
||||||
|
let force_delta_z = force_error_z_1 - force_error_z_2;
|
||||||
|
let force_error = force_error_x + force_error_y + force_error_z;
|
||||||
|
|
||||||
|
let torque_error_x_1 = problem.add(variable().min(0));
|
||||||
|
let torque_error_x_2 = problem.add(variable().min(0));
|
||||||
|
let torque_error_x = torque_error_x_1 + torque_error_x_2;
|
||||||
|
let torque_delta_x = torque_error_x_1 - torque_error_x_2;
|
||||||
|
let torque_error_y_1 = problem.add(variable().min(0));
|
||||||
|
let torque_error_y_2 = problem.add(variable().min(0));
|
||||||
|
let torque_error_y = torque_error_y_1 + torque_error_y_2;
|
||||||
|
let torque_delta_y = torque_error_y_1 - torque_error_y_2;
|
||||||
|
let torque_error_z_1 = problem.add(variable().min(0));
|
||||||
|
let torque_error_z_2 = problem.add(variable().min(0));
|
||||||
|
let torque_error_z = torque_error_z_1 + torque_error_z_2;
|
||||||
|
let torque_delta_z = torque_error_z_1 - torque_error_z_2;
|
||||||
|
let torque_error = torque_error_x + torque_error_y + torque_error_z;
|
||||||
|
|
||||||
|
let total_throttle = throttles.iter().fold(0.into_expression(), Expression::add);
|
||||||
|
|
||||||
|
let generated_force_x = (0..N)
|
||||||
|
.map(|i| thrusters[i].direction.x * thrusters[i].force * throttles[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
let generated_force_y = (0..N)
|
||||||
|
.map(|i| thrusters[i].direction.y * thrusters[i].force * throttles[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
let generated_force_z = (0..N)
|
||||||
|
.map(|i| thrusters[i].direction.z * thrusters[i].force * throttles[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
|
||||||
|
let torque_directions: [_; N] = std::array::from_fn(|i| {
|
||||||
|
let cross = thrusters[i].position.cross_product(thrusters[i].direction);
|
||||||
|
if cross == Vector::ZERO {
|
||||||
|
Vector::ZERO
|
||||||
|
} else {
|
||||||
|
cross.normalize()
|
||||||
|
}
|
||||||
|
});
|
||||||
|
let moment_arms: [f64; N] = std::array::from_fn(|i| thrusters[i].position.length());
|
||||||
|
|
||||||
|
let generated_torque_x = (0..N)
|
||||||
|
.map(|i| torque_directions[i].x * thrusters[i].force * throttles[i] * moment_arms[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
let generated_torque_y = (0..N)
|
||||||
|
.map(|i| torque_directions[i].y * thrusters[i].force * throttles[i] * moment_arms[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
let generated_torque_z = (0..N)
|
||||||
|
.map(|i| torque_directions[i].z * thrusters[i].force * throttles[i] * moment_arms[i])
|
||||||
|
.fold(0.into_expression(), Expression::add);
|
||||||
|
|
||||||
|
let to_minimize = (total_throttle.clone() * constants.throttle)
|
||||||
|
+ (force_error.clone() * constants.force)
|
||||||
|
+ (torque_error.clone() * constants.torque);
|
||||||
|
|
||||||
|
let solution = problem
|
||||||
|
.minimise(to_minimize)
|
||||||
|
.using(microlp)
|
||||||
|
.with(force_delta_x.eq(generated_force_x - desired_force.x))
|
||||||
|
.with(force_delta_y.eq(generated_force_y - desired_force.y))
|
||||||
|
.with(force_delta_z.eq(generated_force_z - desired_force.z))
|
||||||
|
.with(torque_delta_x.eq(generated_torque_x - desired_torque.x))
|
||||||
|
.with(torque_delta_y.eq(generated_torque_y - desired_torque.y))
|
||||||
|
.with(torque_delta_z.eq(generated_torque_z - desired_torque.z))
|
||||||
|
.solve();
|
||||||
|
|
||||||
|
match solution {
|
||||||
|
Ok(solution) => std::array::from_fn(|i| solution.value(throttles[i])),
|
||||||
|
Err(err) => {
|
||||||
|
error!("Failed to balance thrusters {err}");
|
||||||
|
std::array::repeat(0.0)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use crate::hardware::channelization::PinoutChannel;
|
||||||
|
use crate::rcs::Thruster;
|
||||||
|
use crate::rcs::thruster_balancing::{BalancingConstants, balance_thrust};
|
||||||
|
use nautilus_common::math::Vector;
|
||||||
|
|
||||||
|
const EPSILON: f64 = 1e-6;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn test_thruster_balancing() {
|
||||||
|
// We can't fire the one thruster we have because it will induce a torque
|
||||||
|
assert_eq!(
|
||||||
|
[0.0],
|
||||||
|
balance_thrust(
|
||||||
|
Vector::X,
|
||||||
|
Vector::ZERO,
|
||||||
|
&[Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
// A single perfectly aligned thruster
|
||||||
|
assert_eq!(
|
||||||
|
[1.0],
|
||||||
|
balance_thrust(
|
||||||
|
Vector::X,
|
||||||
|
Vector::ZERO,
|
||||||
|
&[Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::ZERO,
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
// Two balanced thrusters should both be fired
|
||||||
|
assert_eq!(
|
||||||
|
[1.0, 1.0],
|
||||||
|
balance_thrust(
|
||||||
|
Vector::X,
|
||||||
|
Vector::ZERO,
|
||||||
|
&[
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
0.5
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
0.5
|
||||||
|
),
|
||||||
|
],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
);
|
||||||
|
|
||||||
|
// Two thrusters with different moment arms
|
||||||
|
// Despite desiring a lot of force, we can't generate too much without inducing a torque
|
||||||
|
assert!(
|
||||||
|
balance_thrust(
|
||||||
|
Vector::X * 10.0,
|
||||||
|
Vector::ZERO,
|
||||||
|
&[
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
0.5
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -2.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
0.5
|
||||||
|
),
|
||||||
|
],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
.into_iter()
|
||||||
|
.zip([1.0, 0.5])
|
||||||
|
.map(|(a, b)| a - b)
|
||||||
|
.sum::<f64>()
|
||||||
|
.abs()
|
||||||
|
< EPSILON
|
||||||
|
);
|
||||||
|
|
||||||
|
// Four perfectly balanced thrusters
|
||||||
|
assert!(
|
||||||
|
balance_thrust(
|
||||||
|
Vector::ZERO,
|
||||||
|
Vector::Z * 2.0,
|
||||||
|
&[
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
-Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -1.0, 0.0),
|
||||||
|
-Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
.into_iter()
|
||||||
|
.zip([0.0, 1.0, 1.0, 0.0])
|
||||||
|
.map(|(a, b)| a - b)
|
||||||
|
.sum::<f64>()
|
||||||
|
.abs()
|
||||||
|
< EPSILON
|
||||||
|
);
|
||||||
|
|
||||||
|
// Four perfectly balanced thrusters
|
||||||
|
assert!(
|
||||||
|
balance_thrust(
|
||||||
|
Vector::X * 2.0,
|
||||||
|
Vector::Z * 2.0,
|
||||||
|
&[
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, 1.0, 0.0),
|
||||||
|
-Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -1.0, 0.0),
|
||||||
|
Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
Thruster::new(
|
||||||
|
PinoutChannel::ExtA(0),
|
||||||
|
Vector::new(0.0, -1.0, 0.0),
|
||||||
|
-Vector::X,
|
||||||
|
1.0
|
||||||
|
),
|
||||||
|
],
|
||||||
|
BalancingConstants::default(),
|
||||||
|
)
|
||||||
|
.into_iter()
|
||||||
|
// Only the one thruster should fire - we can't do any better than that
|
||||||
|
// any more thrusters we fire won't give us any more thrust
|
||||||
|
.zip([0.0, 0.0, 1.0, 0.0])
|
||||||
|
.map(|(a, b)| a - b)
|
||||||
|
.sum::<f64>()
|
||||||
|
.abs()
|
||||||
|
< EPSILON
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -13,8 +13,6 @@ use std::time::{Duration, Instant};
|
|||||||
|
|
||||||
#[derive(Clone, Debug)]
|
#[derive(Clone, Debug)]
|
||||||
pub struct TaskHandle<Message, Data> {
|
pub struct TaskHandle<Message, Data> {
|
||||||
#[allow(dead_code)]
|
|
||||||
pub name: String,
|
|
||||||
pub sender: Sender<Message>,
|
pub sender: Sender<Message>,
|
||||||
data: Data,
|
data: Data,
|
||||||
}
|
}
|
||||||
@@ -46,7 +44,7 @@ pub trait CyclicTask {
|
|||||||
|
|
||||||
impl<F> CyclicTask for F
|
impl<F> CyclicTask for F
|
||||||
where
|
where
|
||||||
F: Fn(),
|
F: FnMut(),
|
||||||
{
|
{
|
||||||
type Message = ();
|
type Message = ();
|
||||||
type Data = ();
|
type Data = ();
|
||||||
@@ -58,7 +56,10 @@ where
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct Scheduler<'s, 'e> {
|
pub struct Scheduler<'s, 'e>
|
||||||
|
where
|
||||||
|
'e: 's,
|
||||||
|
{
|
||||||
scope: &'s Scope<'s, 'e>,
|
scope: &'s Scope<'s, 'e>,
|
||||||
running: Arc<AtomicBool>,
|
running: Arc<AtomicBool>,
|
||||||
}
|
}
|
||||||
@@ -68,7 +69,7 @@ impl<'s> Scheduler<'s, '_> {
|
|||||||
where
|
where
|
||||||
F: FnOnce(Scheduler<'_, 'env>) -> R,
|
F: FnOnce(Scheduler<'_, 'env>) -> R,
|
||||||
{
|
{
|
||||||
trace!("Scheduler::new(running: {running:?}, f)");
|
trace!("Scheduler::scope(running: {running:?}, f)");
|
||||||
thread::scope(|scope: &Scope| {
|
thread::scope(|scope: &Scope| {
|
||||||
let running_result = running.clone();
|
let running_result = running.clone();
|
||||||
// This will automatically set running to false when it drops
|
// This will automatically set running to false when it drops
|
||||||
@@ -83,16 +84,29 @@ impl<'s> Scheduler<'s, '_> {
|
|||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// pub fn inner_scope<'env, F, R>(&self, f: F) -> R
|
||||||
|
// where
|
||||||
|
// F: FnOnce(Scheduler<'_, 'env>) -> R,
|
||||||
|
// 's: 'env,
|
||||||
|
// {
|
||||||
|
// trace!("Scheduler::inner_scope(self)");
|
||||||
|
// thread::scope(|scope: &Scope<'_, 'env>| {
|
||||||
|
// f(Scheduler {
|
||||||
|
// scope,
|
||||||
|
// running: self.running.clone(),
|
||||||
|
// })
|
||||||
|
// })
|
||||||
|
// }
|
||||||
|
|
||||||
#[allow(dead_code)]
|
#[allow(dead_code)]
|
||||||
pub fn run<'t, T>(
|
pub fn run<T>(
|
||||||
&self,
|
&self,
|
||||||
name: impl Into<String>,
|
name: impl Into<String>,
|
||||||
task: T,
|
task: T,
|
||||||
) -> Result<TaskHandle<T::Message, T::Data>>
|
) -> Result<TaskHandle<T::Message, T::Data>>
|
||||||
where
|
where
|
||||||
T: Task + Send + Debug + 't,
|
T: Task + Send + Debug + 's,
|
||||||
T::Message: Send,
|
T::Message: Send,
|
||||||
't: 's,
|
|
||||||
{
|
{
|
||||||
let name = name.into();
|
let name = name.into();
|
||||||
trace!(
|
trace!(
|
||||||
@@ -107,19 +121,18 @@ impl<'s> Scheduler<'s, '_> {
|
|||||||
.spawn_scoped(self.scope, move || {
|
.spawn_scoped(self.scope, move || {
|
||||||
task.run(receiver, running);
|
task.run(receiver, running);
|
||||||
})?;
|
})?;
|
||||||
Ok(TaskHandle { name, sender, data })
|
Ok(TaskHandle { sender, data })
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn run_cyclic<'t, T>(
|
pub fn run_cyclic<T>(
|
||||||
&self,
|
&self,
|
||||||
name: impl Into<String>,
|
name: impl Into<String>,
|
||||||
mut task: T,
|
mut task: T,
|
||||||
frequency: u64,
|
frequency: u64,
|
||||||
) -> Result<TaskHandle<T::Message, T::Data>>
|
) -> Result<TaskHandle<T::Message, T::Data>>
|
||||||
where
|
where
|
||||||
T: CyclicTask + Send + 't,
|
T: CyclicTask + Send + 's,
|
||||||
T::Message: Send,
|
T::Message: Send,
|
||||||
't: 's,
|
|
||||||
{
|
{
|
||||||
let name = name.into();
|
let name = name.into();
|
||||||
trace!(
|
trace!(
|
||||||
@@ -144,6 +157,6 @@ impl<'s> Scheduler<'s, '_> {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
})?;
|
})?;
|
||||||
Ok(TaskHandle { name, sender, data })
|
Ok(TaskHandle { sender, data })
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2,9 +2,13 @@ use anyhow::bail;
|
|||||||
use api::client::Client;
|
use api::client::Client;
|
||||||
use api::client::command::CommandRegistry;
|
use api::client::command::CommandRegistry;
|
||||||
use api::macros::IntoCommandDefinition;
|
use api::macros::IntoCommandDefinition;
|
||||||
use chrono::{DateTime, Utc};
|
use chrono::{DateTime, TimeDelta, Utc};
|
||||||
use log::{error, trace};
|
use log::{error, trace};
|
||||||
use nautilus_common::command::{Command, OwnedCommandHeader, SetPin, ValidPriorityCommand};
|
use nautilus_common::command::set_pin::SetPin;
|
||||||
|
use nautilus_common::command::set_rcs::SetRcs;
|
||||||
|
use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
|
||||||
|
use nautilus_common::command::{Command, OwnedCommandHeader};
|
||||||
|
use nautilus_common::math::Vector;
|
||||||
use nautilus_common::udp::tokio::AsyncUdpSocketExt;
|
use nautilus_common::udp::tokio::AsyncUdpSocketExt;
|
||||||
use std::fmt::Debug;
|
use std::fmt::Debug;
|
||||||
use std::net::SocketAddr;
|
use std::net::SocketAddr;
|
||||||
@@ -29,6 +33,32 @@ struct SetPinCommand {
|
|||||||
index: u8,
|
index: u8,
|
||||||
state: bool,
|
state: bool,
|
||||||
}
|
}
|
||||||
|
impl From<SetPinCommand> for SetPin {
|
||||||
|
fn from(value: SetPinCommand) -> Self {
|
||||||
|
Self {
|
||||||
|
pin: value.index,
|
||||||
|
value: value.state,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(IntoCommandDefinition)]
|
||||||
|
struct RcsCommand {
|
||||||
|
translate_x: f64,
|
||||||
|
translate_y: f64,
|
||||||
|
translate_z: f64,
|
||||||
|
rotate_x: f64,
|
||||||
|
rotate_y: f64,
|
||||||
|
rotate_z: f64,
|
||||||
|
}
|
||||||
|
impl From<RcsCommand> for SetRcs {
|
||||||
|
fn from(value: RcsCommand) -> Self {
|
||||||
|
Self {
|
||||||
|
translation: Vector::new(value.translate_x, value.translate_y, value.translate_z),
|
||||||
|
rotation: Vector::new(value.rotate_x, value.rotate_y, value.rotate_z),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
impl<'a> CommandHandler<'a> {
|
impl<'a> CommandHandler<'a> {
|
||||||
pub fn new(
|
pub fn new(
|
||||||
@@ -69,11 +99,8 @@ impl<'a> CommandHandler<'a> {
|
|||||||
outgoing_commands_tx.try_send_command(
|
outgoing_commands_tx.try_send_command(
|
||||||
&command_name,
|
&command_name,
|
||||||
&ValidPriorityCommand {
|
&ValidPriorityCommand {
|
||||||
inner: SetPin {
|
inner: SetPin::from(cmd),
|
||||||
pin: cmd.index,
|
valid_until: MAX_DATETIME,
|
||||||
value: cmd.state,
|
|
||||||
},
|
|
||||||
valid_until: MAX_DATETIME, // header.timestamp + TimeDelta::seconds(5),
|
|
||||||
priority: 0,
|
priority: 0,
|
||||||
}
|
}
|
||||||
)?;
|
)?;
|
||||||
@@ -84,13 +111,37 @@ impl<'a> CommandHandler<'a> {
|
|||||||
})
|
})
|
||||||
.collect::<Vec<_>>();
|
.collect::<Vec<_>>();
|
||||||
|
|
||||||
commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> {
|
{
|
||||||
trace!("Shutting Down Flight");
|
let outgoing_commands_tx = outgoing_commands_tx.clone();
|
||||||
|
commands.push(self.cmd.register_handler("shutdown", move |_, ()| -> anyhow::Result<_> {
|
||||||
|
trace!("Shutting Down Flight");
|
||||||
|
|
||||||
outgoing_commands_tx.try_send_command("/shutdown", &())?;
|
outgoing_commands_tx.try_send_command("/shutdown", &())?;
|
||||||
|
|
||||||
Ok("Command Executed Successfully".to_string())
|
Ok("Command Executed Successfully".to_string())
|
||||||
}));
|
}));
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
let outgoing_commands_tx = outgoing_commands_tx.clone();
|
||||||
|
commands.push(self.cmd.register_handler("rcs.set", move |header, cmd: RcsCommand| -> anyhow::Result<_> {
|
||||||
|
trace!("Sending Rcs Set Command");
|
||||||
|
|
||||||
|
outgoing_commands_tx.try_send_command(
|
||||||
|
"/rcs/set",
|
||||||
|
&ValidPriorityCommand {
|
||||||
|
inner: SetRcs::from(cmd),
|
||||||
|
valid_until: header.timestamp + TimeDelta::seconds(5),
|
||||||
|
priority: 0,
|
||||||
|
}
|
||||||
|
)?;
|
||||||
|
|
||||||
|
Ok("Command Executed Successfully".to_string())
|
||||||
|
}));
|
||||||
|
}
|
||||||
|
|
||||||
|
// We no longer need this
|
||||||
|
drop(outgoing_commands_tx);
|
||||||
|
|
||||||
let mut buffer = [0u8; 512];
|
let mut buffer = [0u8; 512];
|
||||||
while !self.cancel.is_cancelled() {
|
while !self.cancel.is_cancelled() {
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
use api::client::Client;
|
use api::client::Client;
|
||||||
use api::client::telemetry::{TelemetryHandle, TelemetryRegistry};
|
use api::client::telemetry::{TelemetryHandle, TelemetryRegistry};
|
||||||
use futures::TryFutureExt;
|
use futures::future::{Either, join_all, select};
|
||||||
use futures::future::join_all;
|
use futures::{TryFutureExt, pin_mut};
|
||||||
use log::{error, trace};
|
use log::{error, trace};
|
||||||
use nautilus_common::on_drop::on_drop;
|
use nautilus_common::on_drop::on_drop;
|
||||||
use nautilus_common::telemetry::{CommsState, SwitchBank, Telemetry, TelemetryMessage};
|
use nautilus_common::telemetry::{CommsState, SwitchBank, Telemetry, TelemetryMessage};
|
||||||
@@ -12,7 +12,7 @@ use std::sync::Arc;
|
|||||||
use tokio::net::UdpSocket;
|
use tokio::net::UdpSocket;
|
||||||
use tokio::sync::{RwLock, RwLockWriteGuard};
|
use tokio::sync::{RwLock, RwLockWriteGuard};
|
||||||
use tokio::task::yield_now;
|
use tokio::task::yield_now;
|
||||||
use tokio::{join, pin, select, try_join};
|
use tokio::{join, select, try_join};
|
||||||
use tokio_util::sync::CancellationToken;
|
use tokio_util::sync::CancellationToken;
|
||||||
|
|
||||||
pub struct TelemetryHandler<'a> {
|
pub struct TelemetryHandler<'a> {
|
||||||
@@ -52,7 +52,18 @@ impl<'a> TelemetryHandler<'a> {
|
|||||||
.build()?;
|
.build()?;
|
||||||
|
|
||||||
runtime.block_on(async {
|
runtime.block_on(async {
|
||||||
let mut context = TelemetryContext::new(&self).await;
|
// Allow cancellation while waiting for this to initialize
|
||||||
|
let mut context = {
|
||||||
|
let context = TelemetryContext::new(&self);
|
||||||
|
pin_mut!(context);
|
||||||
|
let cancellation_future = self.cancel.cancelled();
|
||||||
|
pin_mut!(cancellation_future);
|
||||||
|
let init_or_cancel = select(context, cancellation_future).await;
|
||||||
|
match init_or_cancel {
|
||||||
|
Either::Left((success, _)) => success,
|
||||||
|
Either::Right(_) => return Ok(()),
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
let mut buffer = [0u8; 512];
|
let mut buffer = [0u8; 512];
|
||||||
|
|
||||||
@@ -75,7 +86,7 @@ impl<'a> TelemetryHandler<'a> {
|
|||||||
// Update the value in the lock
|
// Update the value in the lock
|
||||||
*lock = addr;
|
*lock = addr;
|
||||||
};
|
};
|
||||||
pin!(flight_addr_update);
|
tokio::pin!(flight_addr_update);
|
||||||
|
|
||||||
// We can do these two operations concurrently
|
// We can do these two operations concurrently
|
||||||
join!(
|
join!(
|
||||||
|
|||||||
Reference in New Issue
Block a user