idk this is math class
This commit is contained in:
franchioping
parent
e776c641b5
commit
6f61d4ab43
|
|
@ -6,6 +6,7 @@ version = 4
|
||||||
name = "RustPhysicsMQ"
|
name = "RustPhysicsMQ"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
|
"clap",
|
||||||
"clearscreen",
|
"clearscreen",
|
||||||
"csv",
|
"csv",
|
||||||
"gilrs",
|
"gilrs",
|
||||||
|
|
@ -15,6 +16,7 @@ dependencies = [
|
||||||
"rand 0.9.2",
|
"rand 0.9.2",
|
||||||
"rapier3d",
|
"rapier3d",
|
||||||
"serde",
|
"serde",
|
||||||
|
"serde_json",
|
||||||
"serde_with",
|
"serde_with",
|
||||||
"strum",
|
"strum",
|
||||||
"toml",
|
"toml",
|
||||||
|
|
@ -41,6 +43,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.2",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[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.2",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "approx"
|
name = "approx"
|
||||||
version = "0.5.1"
|
version = "0.5.1"
|
||||||
|
|
@ -138,6 +190,46 @@ dependencies = [
|
||||||
"windows-link",
|
"windows-link",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "clap"
|
||||||
|
version = "4.5.57"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "6899ea499e3fb9305a65d5ebf6e3d2248c5fab291f300ad0a704fbe142eae31a"
|
||||||
|
dependencies = [
|
||||||
|
"clap_builder",
|
||||||
|
"clap_derive",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "clap_builder"
|
||||||
|
version = "4.5.57"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "7b12c8b680195a62a8364d16b8447b01b6c2c8f9aaf68bee653be34d4245e238"
|
||||||
|
dependencies = [
|
||||||
|
"anstream",
|
||||||
|
"anstyle",
|
||||||
|
"clap_lex",
|
||||||
|
"strsim",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "clap_derive"
|
||||||
|
version = "4.5.55"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a92793da1a46a5f2a02a6f4c46c6496b28c43638adea8306fcb0caa1634f24e5"
|
||||||
|
dependencies = [
|
||||||
|
"heck",
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"syn",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "clap_lex"
|
||||||
|
version = "0.7.7"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "c3e64b0cc0439b12df2fa678eae89a1c56a529fd067a9115f7827f1fffd22b32"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "clearscreen"
|
name = "clearscreen"
|
||||||
version = "4.0.2"
|
version = "4.0.2"
|
||||||
|
|
@ -157,6 +249,12 @@ 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 = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b"
|
checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "colorchoice"
|
||||||
|
version = "1.0.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "core-foundation"
|
name = "core-foundation"
|
||||||
version = "0.10.1"
|
version = "0.10.1"
|
||||||
|
|
@ -645,6 +743,12 @@ dependencies = [
|
||||||
"mach2",
|
"mach2",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "is_terminal_polyfill"
|
||||||
|
version = "1.70.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "itoa"
|
name = "itoa"
|
||||||
version = "1.0.17"
|
version = "1.0.17"
|
||||||
|
|
@ -941,6 +1045,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 = "ordered-float"
|
name = "ordered-float"
|
||||||
version = "5.1.0"
|
version = "5.1.0"
|
||||||
|
|
@ -1591,6 +1701,12 @@ version = "1.0.22"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5"
|
checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5"
|
||||||
|
|
||||||
|
[[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"
|
||||||
|
|
|
||||||
|
|
@ -18,6 +18,10 @@ toml = "0.9.8"
|
||||||
csv = "1.4.0"
|
csv = "1.4.0"
|
||||||
|
|
||||||
serde_with = "3"
|
serde_with = "3"
|
||||||
|
serde_json = "1.0.149"
|
||||||
|
|
||||||
|
|
||||||
|
clap = { version = "4", features = ["derive"] }
|
||||||
|
|
||||||
|
|
||||||
# [profile.dev.package.rapier3d]
|
# [profile.dev.package.rapier3d]
|
||||||
|
|
@ -25,3 +29,7 @@ serde_with = "3"
|
||||||
|
|
||||||
[profile.dev.package."*"]
|
[profile.dev.package."*"]
|
||||||
opt-level = 3
|
opt-level = 3
|
||||||
|
|
||||||
|
[[bin]]
|
||||||
|
name = "record"
|
||||||
|
path = "src/main_record.rs"
|
||||||
|
|
|
||||||
|
|
@ -12,5 +12,5 @@ layers = [
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
] },
|
], frequency = 600.0 },
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -12,5 +12,5 @@ layers = [
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
] },
|
], frequency = 600.0 },
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -12,5 +12,5 @@ layers = [
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
0.0,
|
0.0,
|
||||||
] },
|
], frequency = 600.0 },
|
||||||
]
|
]
|
||||||
|
|
|
||||||
|
|
@ -1,50 +0,0 @@
|
||||||
time,throttle,yaw,pitch,roll
|
|
||||||
0,0,0,0,0
|
|
||||||
0.78333336,0,-1,0,0
|
|
||||||
1.7333333,0,1,0,0
|
|
||||||
3.1000001,0,-1,0,0
|
|
||||||
5.016667,0,0,0,0
|
|
||||||
5.4333334,0,1,0,0
|
|
||||||
6.383333,0,0,0,0
|
|
||||||
6.6666665,0,-1,0,0
|
|
||||||
7.1,0,0,0,0
|
|
||||||
7.2333336,0,1,0,0
|
|
||||||
7.383333,0,-1,0,0
|
|
||||||
7.4500003,0,1,0,0
|
|
||||||
7.5833335,0,-1,0,0
|
|
||||||
7.65,0,1,0,0
|
|
||||||
7.7333336,0,-1,0,0
|
|
||||||
7.85,0,1,0,0
|
|
||||||
7.9500003,0,-1,0,0
|
|
||||||
8.45,0,0,0,0
|
|
||||||
8.466667,0,1,0,0
|
|
||||||
8.900001,0,0,0,0
|
|
||||||
9.3,0,1,0,0
|
|
||||||
9.900001,0,-1,0,0
|
|
||||||
10.433333,0,0,0,0
|
|
||||||
10.883333,0,-1,0,0
|
|
||||||
11.35,0,1,0,0
|
|
||||||
11.716667,0,-1,0,0
|
|
||||||
11.866667,0,1,0,0
|
|
||||||
12.05,0,-1,0,0
|
|
||||||
12.133333,0,1,0,0
|
|
||||||
12.266666,0,-1,0,0
|
|
||||||
12.333333,0,1,0,0
|
|
||||||
12.433333,0,0,0,0
|
|
||||||
12.566667,0,1,0,0
|
|
||||||
13.7,0,-1,0,0
|
|
||||||
14.533334,0,1,0,0
|
|
||||||
15.866667,0,-1,0,0
|
|
||||||
17.316668,0,0,0,0
|
|
||||||
17.800001,0,1,0,0
|
|
||||||
18.333334,0,0,0,0
|
|
||||||
18.516666,0,-1,0,0
|
|
||||||
18.933334,0,0,0,0
|
|
||||||
19.216667,0,1,0,0
|
|
||||||
19.7,0,0,0,0
|
|
||||||
19.783333,0,-1,0,0
|
|
||||||
20.25,0,0,0,0
|
|
||||||
20.5,0,1,0,0
|
|
||||||
20.95,0,0,0,0
|
|
||||||
21.033333,0,-1,0,0
|
|
||||||
21.383333,0,0,0,0
|
|
||||||
|
|
|
@ -1,34 +0,0 @@
|
||||||
time,throttle,yaw,pitch,roll
|
|
||||||
0,0,0,0,0
|
|
||||||
1.3666667,0,0,0,-1
|
|
||||||
1.7,0,0,0,0
|
|
||||||
1.7333333,0,0,0,1
|
|
||||||
2.4333334,0,0,0,0
|
|
||||||
2.45,0,0,0,-1
|
|
||||||
2.6833334,0,0,0,1
|
|
||||||
3.1833334,0,0,0,-1
|
|
||||||
3.8833334,0,0,0,1
|
|
||||||
4.133333,0,0,0,-1
|
|
||||||
4.3166666,0,0,0,1
|
|
||||||
4.4333334,0,0,0,0
|
|
||||||
4.4666667,0,0,0,-1
|
|
||||||
4.5333333,0,0,0,0
|
|
||||||
4.55,0,0,0,1
|
|
||||||
4.633333,0,0,0,0
|
|
||||||
4.6666665,0,0,0,-1
|
|
||||||
4.7333336,0,0,0,0
|
|
||||||
4.75,0,0,0,1
|
|
||||||
4.9833336,0,0,0,-1
|
|
||||||
5.0833335,0,0,0,0
|
|
||||||
5.133333,0,0,0,1
|
|
||||||
5.2166667,0,0,0,0
|
|
||||||
5.3333335,0,0,0,1
|
|
||||||
5.4,0,0,0,0
|
|
||||||
5.4166665,0,0,0,-1
|
|
||||||
5.5333333,0,0,0,0
|
|
||||||
5.5666666,0,0,0,1
|
|
||||||
5.6,0,0,0,-1
|
|
||||||
6.0333333,0,0,0,0
|
|
||||||
6.05,0,0,0,1
|
|
||||||
7.7166667,0,0,0,-1
|
|
||||||
9.216667,0,0,0,0
|
|
||||||
|
144
inputs/test.csv
144
inputs/test.csv
|
|
@ -1,144 +0,0 @@
|
||||||
time,throttle,yaw,pitch,roll
|
|
||||||
0,0,0,0,0
|
|
||||||
1.0333333,0,0,1,0
|
|
||||||
2.55,0,0,0,0
|
|
||||||
2.8999999,1,0,0,0
|
|
||||||
3.35,0,0,0,0
|
|
||||||
3.5833333,0,0,-1,0
|
|
||||||
3.7666667,0,0,0,0
|
|
||||||
4.0833335,0,0,1,0
|
|
||||||
4.366667,0,0,0,0
|
|
||||||
4.5499997,0,0,0,-1
|
|
||||||
4.6666665,0,0,0,0
|
|
||||||
4.7166667,0,0,0,1
|
|
||||||
4.8333335,0,0,0,0
|
|
||||||
4.85,0,0,0,-1
|
|
||||||
4.9166665,0,0,0,0
|
|
||||||
4.95,0,0,0,1
|
|
||||||
5.0499997,0,0,0,0
|
|
||||||
5.116667,0,0,0,-1
|
|
||||||
5.1833334,0,0,0,1
|
|
||||||
5.25,0,0,0,0
|
|
||||||
5.633333,0,0,-1,0
|
|
||||||
5.7999997,0,0,0,0
|
|
||||||
5.8166666,0,0,1,0
|
|
||||||
5.9166665,0,0,0,0
|
|
||||||
5.9666667,0,0,-1,0
|
|
||||||
6.0833335,0,0,1,0
|
|
||||||
6.1833334,0,0,0,0
|
|
||||||
6.2,0,0,-1,0
|
|
||||||
6.2999997,0,0,0,0
|
|
||||||
6.633333,0,-1,0,0
|
|
||||||
6.7166667,0,1,0,0
|
|
||||||
6.9,0,0,0,0
|
|
||||||
6.9166665,0,-1,0,0
|
|
||||||
7.0499997,0,1,0,0
|
|
||||||
7.2,0,-1,0,0
|
|
||||||
7.2999997,0,1,0,0
|
|
||||||
7.45,0,-1,0,0
|
|
||||||
7.5666666,0,0,0,0
|
|
||||||
8.066667,0,0,1,0
|
|
||||||
8.233334,0,0,0,0
|
|
||||||
8.433333,0,0,0,-1
|
|
||||||
8.483334,0,1,0,-1
|
|
||||||
8.766666,0,1,1,-1
|
|
||||||
9.233334,0,0,0,-1
|
|
||||||
9.483334,0,1,0,0
|
|
||||||
9.65,0,0,0,0
|
|
||||||
9.7,0,0,0,1
|
|
||||||
10.3,0,0,0,0
|
|
||||||
10.4,1,0,0,0
|
|
||||||
10.883333,0,0,0,0
|
|
||||||
10.933333,0,0,-1,0
|
|
||||||
11.066667,0,0,0,0
|
|
||||||
11.316667,0,0,-1,0
|
|
||||||
11.483334,0,0,0,0
|
|
||||||
11.916667,0,0,-1,0
|
|
||||||
12.05,1,0,0,0
|
|
||||||
12.266666,1,0,1,0
|
|
||||||
12.483334,1,0,0,0
|
|
||||||
12.883333,1,0,-1,0
|
|
||||||
13.033333,1,0,0,0
|
|
||||||
13.366667,1,0,-1,0
|
|
||||||
13.483334,1,0,0,0
|
|
||||||
14.15,1,0,1,0
|
|
||||||
14.166667,1,0,1,1
|
|
||||||
14.466666,1,0,0,1
|
|
||||||
14.5,1,0,0,0
|
|
||||||
14.75,1,-1,0,0
|
|
||||||
14.766666,1,-1,1,0
|
|
||||||
15.033333,1,-1,1,-1
|
|
||||||
15.233334,1,-1,1,0
|
|
||||||
15.4,1,-1,0,0
|
|
||||||
15.483334,1,0,0,0
|
|
||||||
15.516666,0,0,0,0
|
|
||||||
15.716666,0,0,-1,0
|
|
||||||
16.133333,0,0,0,0
|
|
||||||
16.716667,0,0,0,-1
|
|
||||||
16.8,1,0,0,-1
|
|
||||||
16.916666,1,0,0,0
|
|
||||||
17.216667,1,0,-1,0
|
|
||||||
17.35,1,0,0,0
|
|
||||||
17.516666,1,0,0,1
|
|
||||||
17.533333,0,0,0,1
|
|
||||||
17.7,0,0,0,0
|
|
||||||
17.983334,0,0,0,-1
|
|
||||||
18.216667,0,0,0,0
|
|
||||||
18.316666,0,-1,0,0
|
|
||||||
18.516666,0,0,0,0
|
|
||||||
18.6,0,0,0,1
|
|
||||||
18.766666,0,0,0,0
|
|
||||||
19.449999,1,0,0,0
|
|
||||||
19.833334,1,0,-1,0
|
|
||||||
19.916666,1,0,0,0
|
|
||||||
21.866667,1,-1,0,0
|
|
||||||
21.9,1,-1,0,1
|
|
||||||
22.066666,1,-1,1,1
|
|
||||||
22.533333,1,-1,0,0
|
|
||||||
22.583334,0,-1,0,0
|
|
||||||
22.6,0,-1,0,-1
|
|
||||||
22.683332,0,0,0,-1
|
|
||||||
22.966667,0,0,0,0
|
|
||||||
23.066666,0,1,0,0
|
|
||||||
23.216667,0,0,0,0
|
|
||||||
23.716667,0,0,0,-1
|
|
||||||
23.833334,0,0,0,0
|
|
||||||
26.016666,1,0,0,0
|
|
||||||
27.116667,1,1,0,0
|
|
||||||
27.433332,1,1,0,-1
|
|
||||||
28.516666,1,0,0,-1
|
|
||||||
28.533333,1,-1,0,-1
|
|
||||||
28.566666,1,-1,0,1
|
|
||||||
29.65,1,-1,0,0
|
|
||||||
29.683332,1,-1,-1,0
|
|
||||||
30.083332,1,-1,0,0
|
|
||||||
30.1,1,-1,1,0
|
|
||||||
30.666666,0,0,1,0
|
|
||||||
30.699999,0,0,0,0
|
|
||||||
30.716667,0,1,0,0
|
|
||||||
31,0,1,0,-1
|
|
||||||
32.316666,0,0,0,-1
|
|
||||||
32.416668,0,0,0,0
|
|
||||||
32.483334,0,0,1,0
|
|
||||||
32.966667,0,0,0,0
|
|
||||||
33.066666,0,0,1,0
|
|
||||||
33.416668,0,0,0,0
|
|
||||||
33.466667,0,-1,0,0
|
|
||||||
33.483334,0,-1,0,1
|
|
||||||
33.833332,0,0,0,1
|
|
||||||
34.116665,0,0,0,0
|
|
||||||
34.183334,1,0,0,0
|
|
||||||
34.3,1,0,-1,0
|
|
||||||
34.566666,1,0,0,-1
|
|
||||||
34.716667,1,0,0,0
|
|
||||||
35.216667,1,0,-1,0
|
|
||||||
35.566666,1,0,-1,-1
|
|
||||||
35.616665,1,0,0,-1
|
|
||||||
35.75,1,0,0,0
|
|
||||||
36.016666,1,0,1,0
|
|
||||||
36.183334,1,0,0,0
|
|
||||||
36.216667,0,0,0,0
|
|
||||||
36.983334,1,0,0,0
|
|
||||||
37.066666,1,0,1,0
|
|
||||||
37.399998,1,0,0,0
|
|
||||||
37.483334,0,0,0,0
|
|
||||||
|
|
|
@ -1,37 +0,0 @@
|
||||||
time,throttle,yaw,pitch,roll
|
|
||||||
0,0,0,0,0
|
|
||||||
1.05,0,0,-1,0
|
|
||||||
1.6333333,0,0,0,0
|
|
||||||
1.65,0,0,1,0
|
|
||||||
2.1,0,0,0,0
|
|
||||||
2.1333334,0,0,-1,0
|
|
||||||
2.4666667,0,0,0,0
|
|
||||||
2.4833333,0,0,1,0
|
|
||||||
2.7833333,0,0,0,0
|
|
||||||
2.8,0,0,-1,0
|
|
||||||
5.1666665,0,0,0,0
|
|
||||||
5.2000003,0,0,1,0
|
|
||||||
7.5333333,0,0,0,0
|
|
||||||
7.5666666,0,0,-1,0
|
|
||||||
7.7333336,0,0,0,0
|
|
||||||
7.75,0,0,1,0
|
|
||||||
7.916667,0,0,-1,0
|
|
||||||
8.033334,0,0,0,0
|
|
||||||
8.05,0,0,1,0
|
|
||||||
8.15,0,0,0,0
|
|
||||||
8.166667,0,0,-1,0
|
|
||||||
8.25,0,0,0,0
|
|
||||||
8.283334,0,0,1,0
|
|
||||||
8.383333,0,0,-1,0
|
|
||||||
8.466667,0,0,1,0
|
|
||||||
8.6,0,0,-1,0
|
|
||||||
8.733334,0,0,1,0
|
|
||||||
8.85,0,0,0,0
|
|
||||||
8.883333,0,0,-1,0
|
|
||||||
8.966667,0,0,1,0
|
|
||||||
9.05,0,0,0,0
|
|
||||||
9.066667,0,0,-1,0
|
|
||||||
9.583333,0,0,1,0
|
|
||||||
10.616667,0,0,-1,0
|
|
||||||
11.466667,0,0,1,0
|
|
||||||
11.933333,0,0,0,0
|
|
||||||
|
|
|
@ -16,12 +16,14 @@ pub enum LayerConfig {
|
||||||
#[serde(flatten)]
|
#[serde(flatten)]
|
||||||
pid: PidConfig,
|
pid: PidConfig,
|
||||||
max_rate: f32,
|
max_rate: f32,
|
||||||
|
frequency: f32,
|
||||||
},
|
},
|
||||||
/// Controls orientation (Input: joystick -> Output: desired angular velocity)
|
/// Controls orientation (Input: joystick -> Output: desired angular velocity)
|
||||||
Angle {
|
Angle {
|
||||||
#[serde(flatten)]
|
#[serde(flatten)]
|
||||||
pid: PidConfig,
|
pid: PidConfig,
|
||||||
max_angle: f32,
|
max_angle: f32,
|
||||||
|
frequency: f32,
|
||||||
},
|
},
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,16 +1,132 @@
|
||||||
#![allow(unused)]
|
#![allow(unused)]
|
||||||
use macroquad::prelude as mq;
|
use macroquad::prelude as mq;
|
||||||
|
use serde::{Deserialize, Serialize};
|
||||||
|
use std::fs::File;
|
||||||
|
use std::io::{Read, Write};
|
||||||
|
|
||||||
#[derive(Default, Clone, Copy, PartialEq)]
|
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
|
||||||
pub struct JoystickInput {
|
pub struct JoystickInput {
|
||||||
// Value should be between 0 and 1
|
|
||||||
pub throttle_input: f32,
|
pub throttle_input: f32,
|
||||||
|
|
||||||
pub roll_input: f32,
|
pub roll_input: f32,
|
||||||
pub yaw_input: f32,
|
pub yaw_input: f32,
|
||||||
pub pitch_input: f32,
|
pub pitch_input: f32,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
|
||||||
|
pub struct PositionInput {
|
||||||
|
pub lat: f32,
|
||||||
|
pub long: f32,
|
||||||
|
pub alt: f32,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
|
||||||
|
pub enum ModeInput {
|
||||||
|
#[default]
|
||||||
|
Acro,
|
||||||
|
Navigation,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)]
|
||||||
|
pub struct Input {
|
||||||
|
pub joystick: JoystickInput,
|
||||||
|
pub position: PositionInput,
|
||||||
|
pub mode: ModeInput,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default, Clone, Copy, serde::Serialize, serde::Deserialize)]
|
||||||
|
pub struct InputRecord {
|
||||||
|
input: Input,
|
||||||
|
time: f32,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default, Clone, serde::Serialize, serde::Deserialize)]
|
||||||
|
pub struct InputRecording {
|
||||||
|
records: Vec<InputRecord>,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl InputRecording {
|
||||||
|
pub fn save_to_file(&self, path: &str) -> Result<(), Box<dyn std::error::Error>> {
|
||||||
|
let json = serde_json::to_string_pretty(self)?;
|
||||||
|
let mut file = File::create(path)?;
|
||||||
|
file.write_all(json.as_bytes())?;
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn load_from_file(path: &str) -> Result<Self, Box<dyn std::error::Error>> {
|
||||||
|
let mut file = File::open(path)?;
|
||||||
|
let mut contents = String::new();
|
||||||
|
file.read_to_string(&mut contents)?;
|
||||||
|
let recording: Self = serde_json::from_str(&contents)?;
|
||||||
|
Ok(recording)
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn has_ended(&self, time: f32) -> bool {
|
||||||
|
return self.records.last().unwrap_or(&InputRecord::default()).time < time;
|
||||||
|
}
|
||||||
|
pub fn get_input(&self, time: f32) -> Input {
|
||||||
|
/*
|
||||||
|
* Binary search returns index to element as OK, or where the element could be placed to
|
||||||
|
* keep order as Err, so if result is Ok return that input, if its Err, return the previous
|
||||||
|
* input if it exists, if it doesn't (because time is before the first action in the
|
||||||
|
* recorded sequence, return an empty input)
|
||||||
|
*/
|
||||||
|
let res = self
|
||||||
|
.records
|
||||||
|
.binary_search_by(|probe| probe.time.total_cmp(&time));
|
||||||
|
match res {
|
||||||
|
Ok(res) => {
|
||||||
|
return self
|
||||||
|
.records
|
||||||
|
.get(res)
|
||||||
|
.unwrap_or(&InputRecord::default())
|
||||||
|
.input;
|
||||||
|
}
|
||||||
|
Err(mut res) => {
|
||||||
|
if res > 0 {
|
||||||
|
res -= 1;
|
||||||
|
}
|
||||||
|
return self
|
||||||
|
.records
|
||||||
|
.get(0.max(res))
|
||||||
|
.unwrap_or(&InputRecord::default())
|
||||||
|
.input;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Current time should always be larger thant the last input records time.
|
||||||
|
* This method is made for recording inputs in real time, not for retroactively adding
|
||||||
|
*
|
||||||
|
* Returns the addded Joystick Input
|
||||||
|
*/
|
||||||
|
pub fn add_input_from_keyboard(&mut self, current_time: f32) -> Input {
|
||||||
|
let input = Input {
|
||||||
|
joystick: JoystickInput::from_keyboard(),
|
||||||
|
position: Default::default(),
|
||||||
|
mode: ModeInput::Acro,
|
||||||
|
};
|
||||||
|
let last_input = self.records.last();
|
||||||
|
match last_input {
|
||||||
|
Some(last_record) => {
|
||||||
|
if last_record.input != input {
|
||||||
|
self.records.push(InputRecord {
|
||||||
|
input,
|
||||||
|
time: current_time,
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
None => {
|
||||||
|
self.records.push(InputRecord {
|
||||||
|
input,
|
||||||
|
time: current_time,
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
impl JoystickInput {
|
impl JoystickInput {
|
||||||
pub fn from_keyboard() -> Self {
|
pub fn from_keyboard() -> Self {
|
||||||
let mut input = Self::default();
|
let mut input = Self::default();
|
||||||
|
|
@ -48,173 +164,3 @@ impl JoystickInput {
|
||||||
return input;
|
return input;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Default, Clone, Copy)]
|
|
||||||
pub struct InputRecord {
|
|
||||||
pub input: JoystickInput,
|
|
||||||
pub timestamp: f32,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl InputRecord {
|
|
||||||
pub fn record_from_keyboard(timestamp: f32) -> Self {
|
|
||||||
Self {
|
|
||||||
input: JoystickInput::from_keyboard(),
|
|
||||||
timestamp,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[derive(Default, Clone)]
|
|
||||||
pub struct InputRecording {
|
|
||||||
pub input_records: Vec<InputRecord>,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl InputRecording {
|
|
||||||
pub fn new() -> Self {
|
|
||||||
Self {
|
|
||||||
input_records: Vec::new(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
pub fn save_inputs_to_csv(&self, path: &str) -> std::io::Result<()> {
|
|
||||||
use std::fs::File;
|
|
||||||
use std::io::Write;
|
|
||||||
|
|
||||||
let mut file = File::create(path)?;
|
|
||||||
writeln!(file, "time,throttle,yaw,pitch,roll")?;
|
|
||||||
|
|
||||||
for record in self.input_records.iter() {
|
|
||||||
let inp = record.input;
|
|
||||||
writeln!(
|
|
||||||
file,
|
|
||||||
"{},{},{},{},{}",
|
|
||||||
record.timestamp,
|
|
||||||
inp.throttle_input,
|
|
||||||
inp.yaw_input,
|
|
||||||
inp.pitch_input,
|
|
||||||
inp.roll_input
|
|
||||||
)?;
|
|
||||||
}
|
|
||||||
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
|
|
||||||
pub fn load_inputs_from_csv(path: &str) -> std::io::Result<Self> {
|
|
||||||
use std::fs::File;
|
|
||||||
use std::io::{BufRead, BufReader};
|
|
||||||
|
|
||||||
let file = File::open(path)?;
|
|
||||||
let reader = BufReader::new(file);
|
|
||||||
|
|
||||||
let mut input_records = Vec::new();
|
|
||||||
|
|
||||||
for (i, line) in reader.lines().enumerate() {
|
|
||||||
let line = line?;
|
|
||||||
if i == 0 {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
let parts: Vec<&str> = line.split(',').collect();
|
|
||||||
if parts.len() != 5 {
|
|
||||||
continue; // Should return an error to be strict
|
|
||||||
}
|
|
||||||
|
|
||||||
let timestamp: f32 = parts[0]
|
|
||||||
.parse()
|
|
||||||
.map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?;
|
|
||||||
|
|
||||||
let throttle: f32 = parts[1]
|
|
||||||
.parse()
|
|
||||||
.map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?;
|
|
||||||
let yaw: f32 = parts[2]
|
|
||||||
.parse()
|
|
||||||
.map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?;
|
|
||||||
let pitch: f32 = parts[3]
|
|
||||||
.parse()
|
|
||||||
.map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?;
|
|
||||||
let roll: f32 = parts[4]
|
|
||||||
.parse()
|
|
||||||
.map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?;
|
|
||||||
|
|
||||||
let input = JoystickInput {
|
|
||||||
throttle_input: throttle,
|
|
||||||
yaw_input: yaw,
|
|
||||||
pitch_input: pitch,
|
|
||||||
roll_input: roll,
|
|
||||||
};
|
|
||||||
|
|
||||||
input_records.push(InputRecord { input, timestamp });
|
|
||||||
}
|
|
||||||
|
|
||||||
Ok(Self { input_records })
|
|
||||||
}
|
|
||||||
|
|
||||||
pub fn get_input(&self, time: f32) -> JoystickInput {
|
|
||||||
/*
|
|
||||||
* Binary search returns index to element as OK, or where the element could be placed to
|
|
||||||
* keep order as Err, so if result is Ok return that input, if its Err, return the previous
|
|
||||||
* input if it exists, if it doesn't (because time is before the first action in the
|
|
||||||
* recorded sequence, return an empty input)
|
|
||||||
*/
|
|
||||||
let res = self
|
|
||||||
.input_records
|
|
||||||
.binary_search_by(|probe| probe.timestamp.total_cmp(&time));
|
|
||||||
match res {
|
|
||||||
Ok(res) => {
|
|
||||||
return self
|
|
||||||
.input_records
|
|
||||||
.get(res)
|
|
||||||
.unwrap_or(&InputRecord::default())
|
|
||||||
.input;
|
|
||||||
}
|
|
||||||
Err(mut res) => {
|
|
||||||
if res > 0 {
|
|
||||||
res -= 1;
|
|
||||||
}
|
|
||||||
return self
|
|
||||||
.input_records
|
|
||||||
.get(0.max(res))
|
|
||||||
.unwrap_or(&InputRecord::default())
|
|
||||||
.input;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
pub fn ended(&self, time: f32) -> bool {
|
|
||||||
match self.input_records.last() {
|
|
||||||
Some(record) => {
|
|
||||||
return record.timestamp < time;
|
|
||||||
}
|
|
||||||
None => {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Current time should always be larger thant the last input records time.
|
|
||||||
* This method is made for recording inputs in real time, not for retroactively adding
|
|
||||||
*
|
|
||||||
* Returns the addded Joystick Input
|
|
||||||
*/
|
|
||||||
pub fn add_input_from_keyboard(&mut self, current_time: f32) -> JoystickInput {
|
|
||||||
let input = JoystickInput::from_keyboard();
|
|
||||||
let last_input = self.input_records.last();
|
|
||||||
match last_input {
|
|
||||||
Some(last_record) => {
|
|
||||||
if last_record.input != input {
|
|
||||||
self.input_records.push(InputRecord {
|
|
||||||
input,
|
|
||||||
timestamp: current_time,
|
|
||||||
});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
None => {
|
|
||||||
self.input_records.push(InputRecord {
|
|
||||||
input,
|
|
||||||
timestamp: current_time,
|
|
||||||
});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return input;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -3,8 +3,9 @@
|
||||||
use nalgebra::{self as na, vector};
|
use nalgebra::{self as na, vector};
|
||||||
use std::{any::Any, f32};
|
use std::{any::Any, f32};
|
||||||
|
|
||||||
use crate::drone::controller::DroneController;
|
use crate::drone::stacked::modules::ModuleRuntime;
|
||||||
use crate::drone::JoystickInput;
|
use crate::drone::JoystickInput;
|
||||||
|
use crate::drone::{controller::DroneController, input::Input};
|
||||||
|
|
||||||
use crate::config::*;
|
use crate::config::*;
|
||||||
|
|
||||||
|
|
@ -20,39 +21,56 @@ pub struct DroneState {
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct StackedController {
|
pub struct StackedController {
|
||||||
modules: Vec<ControllerModule>,
|
modules: Vec<ModuleRuntime>,
|
||||||
config: SimulationConfig,
|
config: SimulationConfig,
|
||||||
mixer: MotorMixer,
|
mixer: MotorMixer,
|
||||||
|
|
||||||
// State
|
// State
|
||||||
drone_state: DroneState,
|
drone_state: DroneState,
|
||||||
input: JoystickInput,
|
input: Input,
|
||||||
last_time: f32,
|
last_time: f32,
|
||||||
current_time: f32,
|
current_time: f32,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl StackedController {
|
impl StackedController {
|
||||||
pub fn set_input(&mut self, inp: JoystickInput) {
|
pub fn set_input(&mut self, inp: Input) {
|
||||||
self.input = inp;
|
self.input = inp;
|
||||||
}
|
}
|
||||||
pub fn new(config: SimulationConfig) -> Self {
|
pub fn new(config: SimulationConfig) -> Self {
|
||||||
let mut modules = Vec::new();
|
let mut modules = Vec::new();
|
||||||
|
|
||||||
for layer in &config.layers {
|
for layer in &config.layers {
|
||||||
match layer {
|
let (module, freq) = match layer {
|
||||||
LayerConfig::Angle { pid, max_angle } => {
|
LayerConfig::Angle {
|
||||||
modules.push(ControllerModule::Angle {
|
pid,
|
||||||
|
max_angle,
|
||||||
|
frequency,
|
||||||
|
} => (
|
||||||
|
ControllerModule::Angle {
|
||||||
processor: PidProcessor::new(pid),
|
processor: PidProcessor::new(pid),
|
||||||
max_angle: *max_angle,
|
max_angle: *max_angle,
|
||||||
});
|
},
|
||||||
}
|
*frequency,
|
||||||
LayerConfig::Rate { pid, max_rate } => {
|
),
|
||||||
modules.push(ControllerModule::Rate {
|
LayerConfig::Rate {
|
||||||
|
pid,
|
||||||
|
max_rate,
|
||||||
|
frequency,
|
||||||
|
} => (
|
||||||
|
ControllerModule::Rate {
|
||||||
processor: PidProcessor::new(pid),
|
processor: PidProcessor::new(pid),
|
||||||
max_rate: *max_rate,
|
max_rate: *max_rate,
|
||||||
});
|
},
|
||||||
}
|
*frequency,
|
||||||
}
|
),
|
||||||
|
};
|
||||||
|
|
||||||
|
modules.push(ModuleRuntime {
|
||||||
|
module,
|
||||||
|
target_dt: 1.0 / freq, // Convert Hz to Seconds
|
||||||
|
accumulated_time: 0.0,
|
||||||
|
last_output: na::Vector3::zeros(),
|
||||||
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
Self {
|
Self {
|
||||||
|
|
@ -62,9 +80,9 @@ impl StackedController {
|
||||||
max_throttle: 1.0,
|
max_throttle: 1.0,
|
||||||
mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
|
mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
|
||||||
},
|
},
|
||||||
modules,
|
modules, // Now Vec<ModuleRuntime>
|
||||||
config,
|
config,
|
||||||
input: JoystickInput::default(),
|
input: Input::default(),
|
||||||
drone_state: DroneState {
|
drone_state: DroneState {
|
||||||
rotation: na::UnitQuaternion::identity(),
|
rotation: na::UnitQuaternion::identity(),
|
||||||
angular_velocity: na::Vector3::zeros(),
|
angular_velocity: na::Vector3::zeros(),
|
||||||
|
|
@ -88,20 +106,39 @@ impl DroneController for StackedController {
|
||||||
}
|
}
|
||||||
|
|
||||||
fn get_motor_throttles(&mut self) -> [f32; 4] {
|
fn get_motor_throttles(&mut self) -> [f32; 4] {
|
||||||
let dt = (self.current_time - self.last_time).max(0.0001);
|
let frame_dt = (self.current_time - self.last_time).max(0.0);
|
||||||
|
|
||||||
let mut setpoint = vector![
|
// Initial setpoint comes from the sticks
|
||||||
self.input.roll_input,
|
let mut current_setpoint = vector![
|
||||||
self.input.yaw_input,
|
self.input.joystick.roll_input,
|
||||||
self.input.pitch_input,
|
self.input.joystick.yaw_input,
|
||||||
|
self.input.joystick.pitch_input,
|
||||||
];
|
];
|
||||||
|
|
||||||
for (i, module) in self.modules.iter_mut().enumerate() {
|
for (i, runtime) in self.modules.iter_mut().enumerate() {
|
||||||
let is_first_layer = i == 0;
|
runtime.accumulated_time += frame_dt;
|
||||||
setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
|
|
||||||
|
if runtime.accumulated_time >= runtime.target_dt {
|
||||||
|
// is_first_layer logic: only true if it's the very first module in the vec
|
||||||
|
let is_first_layer = i == 0;
|
||||||
|
|
||||||
|
runtime.last_output = runtime.module.process(
|
||||||
|
current_setpoint,
|
||||||
|
&self.drone_state,
|
||||||
|
runtime.accumulated_time,
|
||||||
|
is_first_layer,
|
||||||
|
);
|
||||||
|
|
||||||
|
// Subtract to keep timing consistent and account for frame jitter
|
||||||
|
runtime.accumulated_time -= runtime.target_dt;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pass this module's output (either fresh or cached) to the next module
|
||||||
|
current_setpoint = runtime.last_output;
|
||||||
}
|
}
|
||||||
|
|
||||||
return self.mixer.mix(0.5, setpoint);
|
self.mixer
|
||||||
|
.mix(self.input.joystick.throttle_input, current_setpoint)
|
||||||
}
|
}
|
||||||
|
|
||||||
fn as_any(&self) -> &dyn Any {
|
fn as_any(&self) -> &dyn Any {
|
||||||
|
|
|
||||||
|
|
@ -13,6 +13,13 @@ pub enum ControllerModule {
|
||||||
},
|
},
|
||||||
}
|
}
|
||||||
|
|
||||||
|
pub struct ModuleRuntime {
|
||||||
|
pub module: ControllerModule,
|
||||||
|
pub target_dt: f32, // e.g., 0.01 for 100Hz
|
||||||
|
pub accumulated_time: f32,
|
||||||
|
pub last_output: na::Vector3<f32>, // Store the last result to pass down the chain
|
||||||
|
}
|
||||||
|
|
||||||
impl ControllerModule {
|
impl ControllerModule {
|
||||||
pub fn process(
|
pub fn process(
|
||||||
&mut self,
|
&mut self,
|
||||||
|
|
@ -27,7 +34,11 @@ impl ControllerModule {
|
||||||
max_angle,
|
max_angle,
|
||||||
} => {
|
} => {
|
||||||
// Setpoint is -1.0..1.0, scale it to target Radians
|
// Setpoint is -1.0..1.0, scale it to target Radians
|
||||||
let target_angles = setpoint * *max_angle;
|
let target_angles = if is_first_layer {
|
||||||
|
setpoint * *max_angle
|
||||||
|
} else {
|
||||||
|
setpoint
|
||||||
|
};
|
||||||
|
|
||||||
let (r, p, y) = state.rotation.euler_angles();
|
let (r, p, y) = state.rotation.euler_angles();
|
||||||
let current_angles = na::vector![r, y, p];
|
let current_angles = na::vector![r, y, p];
|
||||||
|
|
|
||||||
|
|
@ -56,7 +56,7 @@ fn run_batch() {
|
||||||
fn run(input_path: &PathBuf, config_path: &PathBuf) {
|
fn run(input_path: &PathBuf, config_path: &PathBuf) {
|
||||||
let input_name = input_path.file_stem().unwrap().to_string_lossy();
|
let input_name = input_path.file_stem().unwrap().to_string_lossy();
|
||||||
|
|
||||||
let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap())
|
let inputs = InputRecording::load_from_file(input_path.to_str().unwrap())
|
||||||
.expect("Failed to load input recording");
|
.expect("Failed to load input recording");
|
||||||
let config_name = config_path.file_stem().unwrap().to_string_lossy();
|
let config_name = config_path.file_stem().unwrap().to_string_lossy();
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,95 @@
|
||||||
|
mod engine;
|
||||||
|
use engine::*;
|
||||||
|
mod camera;
|
||||||
|
mod config;
|
||||||
|
mod drone;
|
||||||
|
mod helpers;
|
||||||
|
mod logger;
|
||||||
|
mod rendering;
|
||||||
|
mod simulation;
|
||||||
|
|
||||||
|
mod graphics_util;
|
||||||
|
|
||||||
|
use crate::drone::input::*;
|
||||||
|
use crate::simulation::{SimMode, Simulation};
|
||||||
|
|
||||||
|
use crate::config::SimulationConfig;
|
||||||
|
use helpers::list_files;
|
||||||
|
use std::fs;
|
||||||
|
|
||||||
|
const INPUTS_DIR: &str = "inputs/";
|
||||||
|
const CONFIGS_DIR: &str = "configurations/final/";
|
||||||
|
const RESULTS_DIR: &str = "results/";
|
||||||
|
|
||||||
|
use clap::Parser;
|
||||||
|
use std::path::PathBuf;
|
||||||
|
use std::thread::JoinHandle;
|
||||||
|
|
||||||
|
use macroquad::prelude as mq;
|
||||||
|
|
||||||
|
fn window_conf() -> mq::Conf {
|
||||||
|
mq::Conf {
|
||||||
|
window_title: "RustDroneSim".to_owned(),
|
||||||
|
window_resizable: true,
|
||||||
|
// fullscreen: true,
|
||||||
|
platform: mq::miniquad::conf::Platform {
|
||||||
|
// linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
|
||||||
|
..Default::default()
|
||||||
|
},
|
||||||
|
..Default::default()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Parser)]
|
||||||
|
struct Args {
|
||||||
|
input: PathBuf,
|
||||||
|
config: PathBuf,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[macroquad::main(window_conf)]
|
||||||
|
async fn main() {
|
||||||
|
let args = Args::parse();
|
||||||
|
// run_batch();
|
||||||
|
run_record(
|
||||||
|
args.input.to_string_lossy().to_string(),
|
||||||
|
&args.config,
|
||||||
|
// "inputs/newtest.json".to_string(),
|
||||||
|
// &PathBuf::from("configurations/final/sim_std_mot_std_rate_01.toml".to_string()),
|
||||||
|
)
|
||||||
|
.await;
|
||||||
|
}
|
||||||
|
|
||||||
|
async fn run_record(output: String, config_path: &PathBuf) {
|
||||||
|
println!(
|
||||||
|
"Recording inputs to {}, using config {}",
|
||||||
|
output,
|
||||||
|
config_path.to_str().unwrap_or("<INVALID CONFIG PATH>")
|
||||||
|
);
|
||||||
|
|
||||||
|
let config: SimulationConfig =
|
||||||
|
toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file");
|
||||||
|
|
||||||
|
let mut world = World::new(config.tickrate);
|
||||||
|
|
||||||
|
let drone = drone::Drone::new(
|
||||||
|
&mut world,
|
||||||
|
Box::new(drone::stacked::StackedController::new(config.clone())),
|
||||||
|
drone::MotorCharacteristics {
|
||||||
|
max_thrust: config.max_thrust,
|
||||||
|
max_torque: config.max_torque,
|
||||||
|
time_constant: config.time_constant,
|
||||||
|
..Default::default()
|
||||||
|
},
|
||||||
|
config.mass,
|
||||||
|
);
|
||||||
|
|
||||||
|
let mut sim = Simulation::new(
|
||||||
|
drone,
|
||||||
|
world,
|
||||||
|
SimMode::Record(InputRecording::default(), output),
|
||||||
|
None,
|
||||||
|
config.drone_tick_rate,
|
||||||
|
);
|
||||||
|
|
||||||
|
sim.run_and_render().await.unwrap();
|
||||||
|
}
|
||||||
|
|
@ -6,7 +6,7 @@ use std::error::Error;
|
||||||
use crate::{
|
use crate::{
|
||||||
drone::{
|
drone::{
|
||||||
controller::DroneController,
|
controller::DroneController,
|
||||||
input::{InputRecording, JoystickInput},
|
input::{Input, InputRecording},
|
||||||
Drone,
|
Drone,
|
||||||
},
|
},
|
||||||
engine::World,
|
engine::World,
|
||||||
|
|
@ -120,7 +120,7 @@ impl Simulation {
|
||||||
}
|
}
|
||||||
|
|
||||||
fn step(&mut self) -> Result<StepOutcome, Box<dyn Error>> {
|
fn step(&mut self) -> Result<StepOutcome, Box<dyn Error>> {
|
||||||
let current_input: JoystickInput;
|
let current_input: Input;
|
||||||
let current_time = self.world.get_time() as f32;
|
let current_time = self.world.get_time() as f32;
|
||||||
|
|
||||||
match &mut self.mode {
|
match &mut self.mode {
|
||||||
|
|
@ -134,7 +134,7 @@ impl Simulation {
|
||||||
let playback_time = current_time - *start_time;
|
let playback_time = current_time - *start_time;
|
||||||
current_input = recording.get_input(playback_time);
|
current_input = recording.get_input(playback_time);
|
||||||
|
|
||||||
if recording.ended(playback_time) {
|
if recording.has_ended(playback_time) {
|
||||||
println!("Playback ended.");
|
println!("Playback ended.");
|
||||||
return Ok(StepOutcome::Exit);
|
return Ok(StepOutcome::Exit);
|
||||||
}
|
}
|
||||||
|
|
@ -160,9 +160,9 @@ impl Simulation {
|
||||||
self.drone.process_tick(&mut self.world);
|
self.drone.process_tick(&mut self.world);
|
||||||
|
|
||||||
let target_angular_vel: na::Vector3<f32> = na::vector![
|
let target_angular_vel: na::Vector3<f32> = na::vector![
|
||||||
current_input.roll_input,
|
current_input.joystick.roll_input,
|
||||||
current_input.yaw_input,
|
current_input.joystick.yaw_input,
|
||||||
current_input.pitch_input,
|
current_input.joystick.pitch_input,
|
||||||
] * 3.14;
|
] * 3.14;
|
||||||
|
|
||||||
let applied_motor_diff: na::Vector3<f32> = na::vector![
|
let applied_motor_diff: na::Vector3<f32> = na::vector![
|
||||||
|
|
@ -198,7 +198,7 @@ impl Simulation {
|
||||||
fn shutdown(&mut self) -> Result<(), Box<dyn Error>> {
|
fn shutdown(&mut self) -> Result<(), Box<dyn Error>> {
|
||||||
// Save input recording if needed
|
// Save input recording if needed
|
||||||
if let SimMode::Record(recording, dest) = &self.mode {
|
if let SimMode::Record(recording, dest) = &self.mode {
|
||||||
recording.save_inputs_to_csv(dest)?;
|
recording.save_to_file(dest)?;
|
||||||
println!("Input recording saved to {}", dest);
|
println!("Input recording saved to {}", dest);
|
||||||
}
|
}
|
||||||
if let Some(logger) = &mut self.logger {
|
if let Some(logger) = &mut self.logger {
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue