forgot to add file
This commit is contained in:
franchioping
parent
93c6c36a15
commit
4009d68098
|
|
@ -7,6 +7,7 @@ name = "RustPhysicsMQ"
|
|||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"clearscreen",
|
||||
"csv",
|
||||
"gilrs",
|
||||
"glam 0.27.0",
|
||||
"macroquad",
|
||||
|
|
@ -14,6 +15,7 @@ dependencies = [
|
|||
"rand 0.9.2",
|
||||
"rapier3d",
|
||||
"serde",
|
||||
"serde_with",
|
||||
"strum",
|
||||
"toml",
|
||||
]
|
||||
|
|
@ -30,6 +32,15 @@ version = "0.2.21"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923"
|
||||
|
||||
[[package]]
|
||||
name = "android_system_properties"
|
||||
version = "0.1.5"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311"
|
||||
dependencies = [
|
||||
"libc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "approx"
|
||||
version = "0.5.1"
|
||||
|
|
@ -51,6 +62,12 @@ version = "1.5.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8"
|
||||
|
||||
[[package]]
|
||||
name = "base64"
|
||||
version = "0.22.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "72b3254f16251a8381aa12e40e3c4d2f0199f8c6508fbecb9d91f575e0fbb8c6"
|
||||
|
||||
[[package]]
|
||||
name = "bit-vec"
|
||||
version = "0.8.0"
|
||||
|
|
@ -87,6 +104,16 @@ version = "1.5.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
|
||||
|
||||
[[package]]
|
||||
name = "cc"
|
||||
version = "1.2.55"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "47b26a0954ae34af09b50f0de26458fa95369a0d478d8236d3f93082b219bd29"
|
||||
dependencies = [
|
||||
"find-msvc-tools",
|
||||
"shlex",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "cfg-if"
|
||||
version = "1.0.4"
|
||||
|
|
@ -99,6 +126,18 @@ version = "0.2.1"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "613afe47fcd5fac7ccf1db93babcb082c5994d996f20b8b159f2ad1658eb5724"
|
||||
|
||||
[[package]]
|
||||
name = "chrono"
|
||||
version = "0.4.43"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "fac4744fb15ae8337dc853fee7fb3f4e48c0fbaa23d0afe49c447b4fab126118"
|
||||
dependencies = [
|
||||
"iana-time-zone",
|
||||
"num-traits",
|
||||
"serde",
|
||||
"windows-link",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "clearscreen"
|
||||
version = "4.0.2"
|
||||
|
|
@ -143,12 +182,84 @@ dependencies = [
|
|||
"cfg-if",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "csv"
|
||||
version = "1.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "52cd9d68cf7efc6ddfaaee42e7288d3a99d613d4b50f76ce9827ae0c6e14f938"
|
||||
dependencies = [
|
||||
"csv-core",
|
||||
"itoa",
|
||||
"ryu",
|
||||
"serde_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "csv-core"
|
||||
version = "0.1.13"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "704a3c26996a80471189265814dbc2c257598b96b8a7feae2d31ace646bb9782"
|
||||
dependencies = [
|
||||
"memchr",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "darling"
|
||||
version = "0.21.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9cdf337090841a411e2a7f3deb9187445851f91b309c0c0a29e05f74a00a48c0"
|
||||
dependencies = [
|
||||
"darling_core",
|
||||
"darling_macro",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "darling_core"
|
||||
version = "0.21.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1247195ecd7e3c85f83c8d2a366e4210d588e802133e1e355180a9870b517ea4"
|
||||
dependencies = [
|
||||
"fnv",
|
||||
"ident_case",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"strsim",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "darling_macro"
|
||||
version = "0.21.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d38308df82d1080de0afee5d069fa14b0326a88c14f15c5ccda35b4a6c414c81"
|
||||
dependencies = [
|
||||
"darling_core",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "deranged"
|
||||
version = "0.5.5"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ececcb659e7ba858fb4f10388c250a7252eb0a27373f1a72b8748afdd248e587"
|
||||
dependencies = [
|
||||
"powerfmt",
|
||||
"serde_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "downcast-rs"
|
||||
version = "2.0.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "117240f60069e65410b3ae1bb213295bd828f707b5bec6596a1afc8793ce0cbc"
|
||||
|
||||
[[package]]
|
||||
name = "dyn-clone"
|
||||
version = "1.0.20"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d0881ea181b1df73ff77ffaaf9c7544ecc11e82fba9b5f27b262a3c73a332555"
|
||||
|
||||
[[package]]
|
||||
name = "either"
|
||||
version = "1.15.0"
|
||||
|
|
@ -195,6 +306,12 @@ dependencies = [
|
|||
"simd-adler32",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "find-msvc-tools"
|
||||
version = "0.1.9"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582"
|
||||
|
||||
[[package]]
|
||||
name = "flate2"
|
||||
version = "1.1.5"
|
||||
|
|
@ -384,6 +501,12 @@ dependencies = [
|
|||
"byteorder",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "hashbrown"
|
||||
version = "0.12.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888"
|
||||
|
||||
[[package]]
|
||||
name = "hashbrown"
|
||||
version = "0.15.5"
|
||||
|
|
@ -420,6 +543,42 @@ version = "0.5.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea"
|
||||
|
||||
[[package]]
|
||||
name = "hex"
|
||||
version = "0.4.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70"
|
||||
|
||||
[[package]]
|
||||
name = "iana-time-zone"
|
||||
version = "0.1.65"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e31bc9ad994ba00e440a8aa5c9ef0ec67d5cb5e5cb0cc7f8b744a35b389cc470"
|
||||
dependencies = [
|
||||
"android_system_properties",
|
||||
"core-foundation-sys",
|
||||
"iana-time-zone-haiku",
|
||||
"js-sys",
|
||||
"log",
|
||||
"wasm-bindgen",
|
||||
"windows-core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "iana-time-zone-haiku"
|
||||
version = "0.1.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f"
|
||||
dependencies = [
|
||||
"cc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ident_case"
|
||||
version = "1.0.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39"
|
||||
|
||||
[[package]]
|
||||
name = "image"
|
||||
version = "0.24.9"
|
||||
|
|
@ -433,6 +592,17 @@ dependencies = [
|
|||
"png",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "indexmap"
|
||||
version = "1.9.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"hashbrown 0.12.3",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "indexmap"
|
||||
version = "2.12.1"
|
||||
|
|
@ -441,6 +611,8 @@ checksum = "0ad4bb2b565bca0645f4d68c5c9af97fba094e9791da685bf83cb5f3ce74acf2"
|
|||
dependencies = [
|
||||
"equivalent",
|
||||
"hashbrown 0.16.1",
|
||||
"serde",
|
||||
"serde_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
|
|
@ -473,6 +645,12 @@ dependencies = [
|
|||
"mach2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "itoa"
|
||||
version = "1.0.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "92ecc6618181def0457392ccd0ee51198e065e016d1d527a7ac1b6dc7c1f09d2"
|
||||
|
||||
[[package]]
|
||||
name = "js-sys"
|
||||
version = "0.3.83"
|
||||
|
|
@ -701,6 +879,12 @@ dependencies = [
|
|||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-conv"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cf97ec579c3c42f953ef76dbf8d55ac91fb219dde70e49aa4a6b7d74e9919050"
|
||||
|
||||
[[package]]
|
||||
name = "num-derive"
|
||||
version = "0.4.2"
|
||||
|
|
@ -858,6 +1042,12 @@ dependencies = [
|
|||
"miniz_oxide",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "powerfmt"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "439ee305def115ba05938db6eb1644ff94165c5ab5e9420d1c1bcedbba909391"
|
||||
|
||||
[[package]]
|
||||
name = "ppv-lite86"
|
||||
version = "0.2.21"
|
||||
|
|
@ -992,6 +1182,26 @@ version = "0.2.1"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||
|
||||
[[package]]
|
||||
name = "ref-cast"
|
||||
version = "1.0.25"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f354300ae66f76f1c85c5f84693f0ce81d747e2c3f21a45fef496d89c960bf7d"
|
||||
dependencies = [
|
||||
"ref-cast-impl",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ref-cast-impl"
|
||||
version = "1.0.25"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b7186006dcb21920990093f30e3dea63b7d6e977bf1256be20c3563a5db070da"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "robust"
|
||||
version = "1.2.0"
|
||||
|
|
@ -1034,6 +1244,12 @@ version = "1.0.22"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d"
|
||||
|
||||
[[package]]
|
||||
name = "ryu"
|
||||
version = "1.0.22"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a50f4cf475b65d88e057964e0e9bb1f0aa9bbb2036dc65c64596b42932536984"
|
||||
|
||||
[[package]]
|
||||
name = "safe_arch"
|
||||
version = "0.7.4"
|
||||
|
|
@ -1043,6 +1259,30 @@ dependencies = [
|
|||
"bytemuck",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "schemars"
|
||||
version = "0.9.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4cd191f9397d57d581cddd31014772520aa448f65ef991055d7f61582c65165f"
|
||||
dependencies = [
|
||||
"dyn-clone",
|
||||
"ref-cast",
|
||||
"serde",
|
||||
"serde_json",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "schemars"
|
||||
version = "1.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a2b42f36aa1cd011945615b92222f6bf73c599a102a300334cd7f8dbeec726cc"
|
||||
dependencies = [
|
||||
"dyn-clone",
|
||||
"ref-cast",
|
||||
"serde",
|
||||
"serde_json",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde"
|
||||
version = "1.0.228"
|
||||
|
|
@ -1073,6 +1313,19 @@ dependencies = [
|
|||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_json"
|
||||
version = "1.0.149"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "83fc039473c5595ace860d8c4fafa220ff474b3fc6bfdb4293327f1a37e94d86"
|
||||
dependencies = [
|
||||
"itoa",
|
||||
"memchr",
|
||||
"serde",
|
||||
"serde_core",
|
||||
"zmij",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_spanned"
|
||||
version = "1.0.3"
|
||||
|
|
@ -1082,6 +1335,43 @@ dependencies = [
|
|||
"serde_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_with"
|
||||
version = "3.16.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4fa237f2807440d238e0364a218270b98f767a00d3dada77b1c53ae88940e2e7"
|
||||
dependencies = [
|
||||
"base64",
|
||||
"chrono",
|
||||
"hex",
|
||||
"indexmap 1.9.3",
|
||||
"indexmap 2.12.1",
|
||||
"schemars 0.9.0",
|
||||
"schemars 1.2.1",
|
||||
"serde_core",
|
||||
"serde_json",
|
||||
"serde_with_macros",
|
||||
"time",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_with_macros"
|
||||
version = "3.16.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "52a8e3ca0ca629121f70ab50f95249e5a6f925cc0f6ffe8256c45b728875706c"
|
||||
dependencies = [
|
||||
"darling",
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "shlex"
|
||||
version = "1.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
|
||||
|
||||
[[package]]
|
||||
name = "simba"
|
||||
version = "0.9.1"
|
||||
|
|
@ -1143,6 +1433,12 @@ version = "1.1.0"
|
|||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f"
|
||||
|
||||
[[package]]
|
||||
name = "strsim"
|
||||
version = "0.11.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f"
|
||||
|
||||
[[package]]
|
||||
name = "strum"
|
||||
version = "0.27.2"
|
||||
|
|
@ -1207,13 +1503,44 @@ dependencies = [
|
|||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "time"
|
||||
version = "0.3.47"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "743bd48c283afc0388f9b8827b976905fb217ad9e647fae3a379a9283c4def2c"
|
||||
dependencies = [
|
||||
"deranged",
|
||||
"itoa",
|
||||
"num-conv",
|
||||
"powerfmt",
|
||||
"serde_core",
|
||||
"time-core",
|
||||
"time-macros",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "time-core"
|
||||
version = "0.1.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "7694e1cfe791f8d31026952abf09c69ca6f6fa4e1a1229e18988f06a04a12dca"
|
||||
|
||||
[[package]]
|
||||
name = "time-macros"
|
||||
version = "0.2.27"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2e70e4c5a0e0a8a4823ad65dfe1a6930e4f4d756dcd9dd7939022b5e8c501215"
|
||||
dependencies = [
|
||||
"num-conv",
|
||||
"time-core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "toml"
|
||||
version = "0.9.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f0dc8b1fb61449e27716ec0e1bdf0f6b8f3e8f6b05391e8497b8b6d7804ea6d8"
|
||||
dependencies = [
|
||||
"indexmap",
|
||||
"indexmap 2.12.1",
|
||||
"serde_core",
|
||||
"serde_spanned",
|
||||
"toml_datetime",
|
||||
|
|
@ -1616,3 +1943,9 @@ dependencies = [
|
|||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "zmij"
|
||||
version = "1.0.19"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3ff05f8caa9038894637571ae6b9e29466c1f4f829d26c9b28f869a29cbe3445"
|
||||
|
|
|
|||
|
|
@ -15,6 +15,9 @@ clearscreen = "4.0.2"
|
|||
gilrs = "0.11.0"
|
||||
serde = { version = "1.0.228", features = ["serde_derive"] }
|
||||
toml = "0.9.8"
|
||||
csv = "1.4.0"
|
||||
|
||||
serde_with = "3"
|
||||
|
||||
|
||||
[[bin]]
|
||||
|
|
|
|||
|
|
@ -119,7 +119,7 @@ impl InputRecording {
|
|||
|
||||
let parts: Vec<&str> = line.split(',').collect();
|
||||
if parts.len() != 5 {
|
||||
continue; // or return an error if you prefer strict parsing
|
||||
continue; // Should return an error to be strict
|
||||
}
|
||||
|
||||
let timestamp: f32 = parts[0]
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ impl StackedController {
|
|||
motor_map: config.motor_map,
|
||||
min_throttle: 0.0,
|
||||
max_throttle: 1.0,
|
||||
mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
|
||||
},
|
||||
modules,
|
||||
config,
|
||||
|
|
@ -100,7 +101,7 @@ impl DroneController for StackedController {
|
|||
setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
|
||||
}
|
||||
|
||||
return self.mixer.mix(0.5, setpoint).0;
|
||||
return self.mixer.mix(0.5, setpoint);
|
||||
}
|
||||
|
||||
fn as_any(&self) -> &dyn Any {
|
||||
|
|
|
|||
|
|
@ -1,13 +1,121 @@
|
|||
use nalgebra as na;
|
||||
|
||||
#[derive(Default)]
|
||||
pub enum MotorMixingMode {
|
||||
ThrottleAuthority,
|
||||
NoTorqueScalling,
|
||||
ThrottleAuthorityReasonable {
|
||||
min_scale: f32,
|
||||
},
|
||||
#[default]
|
||||
AttitudeAuthority,
|
||||
}
|
||||
|
||||
pub struct MotorMixer {
|
||||
pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch
|
||||
pub min_throttle: f32,
|
||||
pub max_throttle: f32,
|
||||
pub mixing_mode: MotorMixingMode,
|
||||
}
|
||||
|
||||
impl MotorMixer {
|
||||
pub fn mix(&self, throttle: f32, torque: na::Vector3<f32>) -> ([f32; 4], bool) {
|
||||
pub fn mix(&self, throttle: f32, torque: na::Vector3<f32>) -> [f32; 4] {
|
||||
use MotorMixingMode::*;
|
||||
match self.mixing_mode {
|
||||
ThrottleAuthority => {
|
||||
return self.mix_throttle_authority(throttle, torque).0;
|
||||
}
|
||||
NoTorqueScalling => {
|
||||
return self.mix_no_torque_scalling(throttle, torque).0;
|
||||
}
|
||||
ThrottleAuthorityReasonable { min_scale } => {
|
||||
return self
|
||||
.mix_throttle_authority_reasonable(throttle, torque, min_scale)
|
||||
.0;
|
||||
}
|
||||
AttitudeAuthority => {
|
||||
return self.mix_attitude_authority(throttle, torque);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn compute_delta(&self, torque: na::Vector3<f32>) -> [f32; 4] {
|
||||
let mut delta = [0.0f32; 4];
|
||||
|
||||
for i in 0..4 {
|
||||
delta[i] = self.motor_map[i][0] * torque.x
|
||||
+ self.motor_map[i][1] * torque.y
|
||||
+ self.motor_map[i][2] * torque.z;
|
||||
}
|
||||
|
||||
return delta;
|
||||
}
|
||||
|
||||
/// Unlike ThrottleAuthority, this implementation has a minimum scale.
|
||||
/// That makes sure our torque is never below min_scale * torque.
|
||||
///
|
||||
/// Throttle still has authority, this is, torque is limited by the throttle, but within reason, which makes it "reasonable"
|
||||
///
|
||||
/// Avoids the downside of the previous implementation: At 100% Throttle, Torque would always be 0. This lowers the actual throttle.
|
||||
///
|
||||
/// If min_scale is 1, acts as attitude authority
|
||||
pub fn mix_throttle_authority_reasonable(
|
||||
&self,
|
||||
throttle: f32,
|
||||
torque: na::Vector3<f32>,
|
||||
min_scale: f32,
|
||||
) -> ([f32; 4], bool) {
|
||||
let mut delta = self.compute_delta(torque);
|
||||
|
||||
let mut scale = 1.0f32;
|
||||
|
||||
for i in 0..4 {
|
||||
if delta[i] > 0.0 {
|
||||
scale = scale.min((self.max_throttle - throttle) / delta[i]);
|
||||
} else if delta[i] < 0.0 {
|
||||
scale = scale.min((self.min_throttle - throttle) / delta[i]);
|
||||
}
|
||||
}
|
||||
|
||||
scale = scale.clamp(min_scale, 1.0);
|
||||
|
||||
delta.iter_mut().for_each(|x| *x *= scale);
|
||||
|
||||
let max_delta = delta.into_iter().reduce(f32::max).unwrap_or(0.0);
|
||||
let min_delta = delta.into_iter().reduce(f32::min).unwrap_or(0.0);
|
||||
let lim_throttle = throttle.clamp(
|
||||
self.min_throttle + min_delta.abs(),
|
||||
self.max_throttle - max_delta.abs(),
|
||||
);
|
||||
|
||||
let mut motors = [0.0f32; 4];
|
||||
for i in 0..4 {
|
||||
motors[i] = lim_throttle + delta[i];
|
||||
}
|
||||
|
||||
let saturated = scale < 1.0;
|
||||
|
||||
(motors, saturated)
|
||||
}
|
||||
|
||||
pub fn mix_attitude_authority(&self, throttle: f32, torque: na::Vector3<f32>) -> [f32; 4] {
|
||||
return self
|
||||
.mix_throttle_authority_reasonable(throttle, torque, 1.0)
|
||||
.0;
|
||||
}
|
||||
|
||||
///
|
||||
/// Made by Chatgpt, unreliable as hell. Deltas try to fit within throttle,
|
||||
/// so if throttle is the max, will simply not apply deltas. Shouldn't be used.
|
||||
///
|
||||
/// Will actually kind of work if throttle_min + delta < throttle < throttle_max - delta,
|
||||
/// Where delta is a value of headroom added so that we always have room to create torque
|
||||
///
|
||||
pub fn mix_throttle_authority(
|
||||
&self,
|
||||
throttle: f32,
|
||||
torque: na::Vector3<f32>,
|
||||
) -> ([f32; 4], bool) {
|
||||
let mut delta = [0.0f32; 4];
|
||||
|
||||
// 1. Torque-only contribution
|
||||
|
|
@ -39,4 +147,84 @@ impl MotorMixer {
|
|||
let saturated = scale < 1.0;
|
||||
(motors, saturated)
|
||||
}
|
||||
|
||||
/// Bad, not mine, used for testing and comparison
|
||||
pub fn mix_no_torque_scalling(
|
||||
&self,
|
||||
throttle: f32,
|
||||
torque: na::Vector3<f32>,
|
||||
) -> ([f32; 4], bool) {
|
||||
let mut motors = [0.0f32; 4];
|
||||
|
||||
// --------------------------------------------------
|
||||
// 1. Raw mix: throttle + torque deltas
|
||||
// --------------------------------------------------
|
||||
for i in 0..4 {
|
||||
let delta = self.motor_map[i][0] * torque.x
|
||||
+ self.motor_map[i][1] * torque.y
|
||||
+ self.motor_map[i][2] * torque.z;
|
||||
|
||||
motors[i] = throttle + delta;
|
||||
}
|
||||
|
||||
// --------------------------------------------------
|
||||
// 2. Find saturation
|
||||
// --------------------------------------------------
|
||||
let mut max_motor = f32::MIN;
|
||||
let mut min_motor = f32::MAX;
|
||||
|
||||
for &m in motors.iter() {
|
||||
max_motor = max_motor.max(m);
|
||||
min_motor = min_motor.min(m);
|
||||
}
|
||||
|
||||
let mut saturated = false;
|
||||
|
||||
// --------------------------------------------------
|
||||
// 3. Thrust-priority correction (shift down)
|
||||
// Preserves all torque differences
|
||||
// --------------------------------------------------
|
||||
if max_motor > self.max_throttle {
|
||||
let excess = max_motor - self.max_throttle;
|
||||
for m in motors.iter_mut() {
|
||||
*m -= excess;
|
||||
}
|
||||
saturated = true;
|
||||
}
|
||||
|
||||
// --------------------------------------------------
|
||||
// 4. Recompute minimum after shift
|
||||
// --------------------------------------------------
|
||||
min_motor = f32::MAX;
|
||||
for &m in motors.iter() {
|
||||
min_motor = min_motor.min(m);
|
||||
}
|
||||
|
||||
// --------------------------------------------------
|
||||
// 5. Bottom correction (shift up if possible)
|
||||
// Still preserves torque
|
||||
// --------------------------------------------------
|
||||
if min_motor < self.min_throttle {
|
||||
let deficit = self.min_throttle - min_motor;
|
||||
for m in motors.iter_mut() {
|
||||
*m += deficit;
|
||||
}
|
||||
saturated = true;
|
||||
}
|
||||
|
||||
// --------------------------------------------------
|
||||
// 6. Final clamp (last resort — torque may be lost)
|
||||
// --------------------------------------------------
|
||||
for m in motors.iter_mut() {
|
||||
if *m > self.max_throttle {
|
||||
*m = self.max_throttle;
|
||||
saturated = true;
|
||||
} else if *m < self.min_throttle {
|
||||
*m = self.min_throttle;
|
||||
saturated = true;
|
||||
}
|
||||
}
|
||||
|
||||
(motors, saturated)
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,74 @@
|
|||
use nalgebra as na;
|
||||
use serde::Serialize;
|
||||
|
||||
pub struct CsvLogger {
|
||||
writer: csv::Writer<std::fs::File>,
|
||||
}
|
||||
|
||||
impl CsvLogger {
|
||||
pub fn new(path: &str) -> Result<Self, Box<dyn std::error::Error>> {
|
||||
let file = std::fs::File::create(path)?;
|
||||
Ok(Self {
|
||||
writer: csv::Writer::from_writer(file),
|
||||
})
|
||||
}
|
||||
|
||||
pub fn log(&mut self, row: &SimLogRow) -> Result<(), Box<dyn std::error::Error>> {
|
||||
self.writer.serialize(row)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn flush(&mut self) -> Result<(), Box<dyn std::error::Error>> {
|
||||
self.writer.flush()?;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
pub struct SimLogRow {
|
||||
pub time: f32,
|
||||
pub target_x: f32,
|
||||
pub target_y: f32,
|
||||
pub target_z: f32,
|
||||
|
||||
pub current_x: f32,
|
||||
pub current_y: f32,
|
||||
pub current_z: f32,
|
||||
|
||||
pub mot_x: f32,
|
||||
pub mot_y: f32,
|
||||
pub mot_z: f32,
|
||||
|
||||
pub dmot_x: f32,
|
||||
pub dmot_y: f32,
|
||||
pub dmot_z: f32,
|
||||
}
|
||||
|
||||
impl SimLogRow {
|
||||
pub fn from_data(
|
||||
time: f32,
|
||||
target: na::Vector3<f32>,
|
||||
current: na::Vector3<f32>,
|
||||
mot: na::Vector3<f32>,
|
||||
dmot: na::Vector3<f32>,
|
||||
) -> Self {
|
||||
Self {
|
||||
time,
|
||||
target_x: target.x,
|
||||
target_y: target.y,
|
||||
target_z: target.z,
|
||||
|
||||
current_x: current.x,
|
||||
current_y: current.y,
|
||||
current_z: current.z,
|
||||
|
||||
mot_x: mot.x,
|
||||
mot_y: mot.y,
|
||||
mot_z: mot.z,
|
||||
|
||||
dmot_x: dmot.x,
|
||||
dmot_y: dmot.y,
|
||||
dmot_z: dmot.z,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -4,6 +4,7 @@ mod camera;
|
|||
mod config;
|
||||
mod drone;
|
||||
mod helpers;
|
||||
mod logger;
|
||||
mod rendering;
|
||||
mod simulation;
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ use crate::{
|
|||
Drone,
|
||||
},
|
||||
engine::World,
|
||||
logger::CsvLogger,
|
||||
rendering::Renderer,
|
||||
};
|
||||
|
||||
|
|
@ -18,6 +19,11 @@ pub enum SimMode {
|
|||
Playback(InputRecording, f32),
|
||||
}
|
||||
|
||||
enum StepOutcome {
|
||||
Continue,
|
||||
Exit,
|
||||
}
|
||||
|
||||
pub struct DataResultRecord {
|
||||
pub time: f32,
|
||||
pub current_angular_velocity: na::Vector3<f32>,
|
||||
|
|
@ -26,18 +32,13 @@ pub struct DataResultRecord {
|
|||
pub desired_motor_offset: na::Vector3<f32>,
|
||||
}
|
||||
|
||||
#[derive(Default)]
|
||||
pub struct SimulationState {
|
||||
pub data_results: Vec<DataResultRecord>,
|
||||
}
|
||||
|
||||
pub struct Simulation {
|
||||
pub drone: Drone,
|
||||
pub world: World,
|
||||
pub mode: SimMode,
|
||||
results_file: Option<String>,
|
||||
|
||||
logger: Option<CsvLogger>,
|
||||
drone_tick_rate: u64,
|
||||
state: SimulationState,
|
||||
}
|
||||
|
||||
impl Simulation {
|
||||
|
|
@ -48,13 +49,17 @@ impl Simulation {
|
|||
results_file: Option<String>,
|
||||
drone_tick_rate: u64,
|
||||
) -> Self {
|
||||
let logger = match &results_file {
|
||||
Some(path) => Some(CsvLogger::new(path).unwrap()),
|
||||
None => None,
|
||||
};
|
||||
|
||||
let mut s = Self {
|
||||
drone,
|
||||
world,
|
||||
mode,
|
||||
results_file,
|
||||
logger,
|
||||
drone_tick_rate,
|
||||
state: SimulationState::default(),
|
||||
};
|
||||
|
||||
s.world.register_free_collider(
|
||||
|
|
@ -82,244 +87,124 @@ impl Simulation {
|
|||
);
|
||||
renderer.update_light(&self.world);
|
||||
|
||||
let mut current_input: JoystickInput;
|
||||
|
||||
loop {
|
||||
renderer.update_camera(&self.world);
|
||||
|
||||
// --- Input handling ---
|
||||
let current_time = self.world.get_time() as f32;
|
||||
|
||||
match &mut self.mode {
|
||||
SimMode::Record(recording, _) => {
|
||||
current_input = recording.add_input_from_keyboard(current_time);
|
||||
if mq::is_key_pressed(mq::KeyCode::Q) {
|
||||
self.shutdown()?;
|
||||
return Ok(());
|
||||
match self.step()? {
|
||||
StepOutcome::Continue => {
|
||||
if self.world.tick
|
||||
% ((self.world.integration_parameters.inv_dt() / 60.0) as u64)
|
||||
== 0
|
||||
{
|
||||
renderer.update_camera(&self.world);
|
||||
renderer.draw(&mut self.world);
|
||||
mq::next_frame().await;
|
||||
}
|
||||
}
|
||||
SimMode::Playback(recording, start_time) => {
|
||||
let playback_time = current_time - *start_time;
|
||||
current_input = recording.get_input(playback_time);
|
||||
|
||||
if recording.ended(playback_time) {
|
||||
println!("Playback ended.");
|
||||
break;
|
||||
}
|
||||
StepOutcome::Exit => {
|
||||
self.shutdown()?;
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
|
||||
// --- Physics ---
|
||||
self.world.step();
|
||||
if self.world.tick
|
||||
% ((self.world.integration_parameters.inv_dt() / self.drone_tick_rate as f32)
|
||||
as u64)
|
||||
== 0
|
||||
{
|
||||
if let Some(cont) = self
|
||||
.drone
|
||||
.controller
|
||||
.as_mut_any()
|
||||
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||
{
|
||||
cont.set_input(current_input);
|
||||
}
|
||||
self.drone.process_tick(&mut self.world);
|
||||
}
|
||||
|
||||
let target_angular_vel: na::Vector3<f32>;
|
||||
let desired_motor_diff: na::Vector3<f32>;
|
||||
let applied_motor_diff: na::Vector3<f32>;
|
||||
if let Some(cont) = self
|
||||
.drone
|
||||
.controller
|
||||
.as_mut_any()
|
||||
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||
{
|
||||
desired_motor_diff = cont.get_desired_motor_diffs();
|
||||
target_angular_vel = cont.get_desired_angular_velocity();
|
||||
|
||||
/*
|
||||
let t = [
|
||||
throttle - pitch + yaw + roll,
|
||||
throttle - pitch - yaw - roll,
|
||||
throttle + pitch + yaw - roll,
|
||||
throttle + pitch - yaw + roll,
|
||||
];
|
||||
*/
|
||||
let m = cont.get_motor_throttles();
|
||||
applied_motor_diff = na::vector![
|
||||
(m[2] + m[3] - m[0] - m[1]),
|
||||
(m[0] + m[2] - m[1] - m[3]),
|
||||
(m[0] + m[3] - m[1] - m[2])
|
||||
] / 4.0;
|
||||
} else {
|
||||
target_angular_vel = na::vector![0.0, 0.0, 0.0];
|
||||
desired_motor_diff = na::vector![0.0, 0.0, 0.0];
|
||||
applied_motor_diff = na::vector![0.0, 0.0, 0.0];
|
||||
}
|
||||
|
||||
self.state.data_results.push(DataResultRecord {
|
||||
time: self.world.get_time(),
|
||||
current_angular_velocity: self
|
||||
.drone
|
||||
.get_rot(&self.world)
|
||||
.inverse()
|
||||
.transform_vector(&self.drone.get_angvel(&self.world)),
|
||||
target_angular_velocity: target_angular_vel,
|
||||
applied_motor_offset: applied_motor_diff,
|
||||
desired_motor_offset: desired_motor_diff,
|
||||
});
|
||||
|
||||
// --- Rendering ---
|
||||
if self.world.tick % ((self.world.integration_parameters.inv_dt() / 60.0) as u64) == 0 {
|
||||
renderer.draw(&mut self.world);
|
||||
mq::next_frame();
|
||||
}
|
||||
}
|
||||
|
||||
self.shutdown()?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn run(&mut self) -> Result<(), Box<dyn Error>> {
|
||||
let mut current_input: JoystickInput;
|
||||
|
||||
loop {
|
||||
let current_time = self.world.get_time() as f32;
|
||||
|
||||
match &mut self.mode {
|
||||
SimMode::Record(recording, _) => {
|
||||
current_input = recording.add_input_from_keyboard(current_time);
|
||||
if mq::is_key_pressed(mq::KeyCode::Q) {
|
||||
self.shutdown()?;
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
SimMode::Playback(recording, start_time) => {
|
||||
let playback_time = current_time - *start_time;
|
||||
current_input = recording.get_input(playback_time);
|
||||
|
||||
if recording.ended(playback_time) {
|
||||
println!("Playback ended.");
|
||||
break;
|
||||
}
|
||||
match self.step()? {
|
||||
StepOutcome::Continue => {}
|
||||
StepOutcome::Exit => {
|
||||
self.shutdown()?;
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- Physics ---
|
||||
self.world.step();
|
||||
if self.world.tick
|
||||
% ((self.world.integration_parameters.inv_dt() / self.drone_tick_rate as f32)
|
||||
as u64)
|
||||
== 0
|
||||
{
|
||||
if let Some(cont) = self
|
||||
.drone
|
||||
.controller
|
||||
.as_mut_any()
|
||||
.downcast_mut::<crate::drone::stacked::StackedController>()
|
||||
{
|
||||
cont.set_input(current_input);
|
||||
fn step(&mut self) -> Result<StepOutcome, Box<dyn Error>> {
|
||||
let current_input: JoystickInput;
|
||||
let current_time = self.world.get_time() as f32;
|
||||
|
||||
match &mut self.mode {
|
||||
SimMode::Record(recording, _) => {
|
||||
current_input = recording.add_input_from_keyboard(current_time);
|
||||
if mq::is_key_pressed(mq::KeyCode::Q) {
|
||||
return Ok(StepOutcome::Exit);
|
||||
}
|
||||
self.drone.process_controller_tick(&mut self.world);
|
||||
}
|
||||
self.drone.process_tick(&mut self.world);
|
||||
SimMode::Playback(recording, start_time) => {
|
||||
let playback_time = current_time - *start_time;
|
||||
current_input = recording.get_input(playback_time);
|
||||
|
||||
let target_angular_vel: na::Vector3<f32>;
|
||||
let desired_motor_diff: na::Vector3<f32>;
|
||||
let applied_motor_diff: na::Vector3<f32>;
|
||||
if recording.ended(playback_time) {
|
||||
println!("Playback ended.");
|
||||
return Ok(StepOutcome::Exit);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- Physics ---
|
||||
self.world.step();
|
||||
if self.world.tick
|
||||
% ((self.world.integration_parameters.inv_dt() / self.drone_tick_rate as f32) as u64)
|
||||
== 0
|
||||
{
|
||||
if let Some(cont) = self
|
||||
.drone
|
||||
.controller
|
||||
.as_mut_any()
|
||||
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||
.downcast_mut::<crate::drone::stacked::StackedController>()
|
||||
{
|
||||
desired_motor_diff = cont.get_desired_motor_diffs();
|
||||
target_angular_vel = cont.get_desired_angular_velocity();
|
||||
|
||||
/*
|
||||
let t = [
|
||||
throttle - pitch + yaw + roll,
|
||||
throttle - pitch - yaw - roll,
|
||||
throttle + pitch + yaw - roll,
|
||||
throttle + pitch - yaw + roll,
|
||||
];
|
||||
*/
|
||||
} else {
|
||||
target_angular_vel = na::vector![
|
||||
current_input.roll_input,
|
||||
current_input.yaw_input,
|
||||
current_input.pitch_input,
|
||||
] * 3.14;
|
||||
desired_motor_diff = na::vector![0.0, 0.0, 0.0];
|
||||
cont.set_input(current_input);
|
||||
}
|
||||
self.drone.process_controller_tick(&mut self.world);
|
||||
}
|
||||
self.drone.process_tick(&mut self.world);
|
||||
|
||||
let m = self.drone.current_throttles;
|
||||
applied_motor_diff = na::vector![
|
||||
(m[1] + m[2] - m[0] - m[3]),
|
||||
(m[0] + m[2] - m[1] - m[3]),
|
||||
(m[2] + m[3] - m[0] - m[1])
|
||||
] / 2.0;
|
||||
let mut target_angular_vel: na::Vector3<f32> = na::vector![
|
||||
current_input.roll_input,
|
||||
current_input.yaw_input,
|
||||
current_input.pitch_input,
|
||||
] * 3.14;
|
||||
|
||||
self.state.data_results.push(DataResultRecord {
|
||||
time: self.world.get_time(),
|
||||
current_angular_velocity: self
|
||||
.drone
|
||||
.get_rot(&self.world)
|
||||
.inverse()
|
||||
.transform_vector(&self.drone.get_angvel(&self.world)),
|
||||
target_angular_velocity: target_angular_vel,
|
||||
applied_motor_offset: applied_motor_diff,
|
||||
desired_motor_offset: desired_motor_diff,
|
||||
});
|
||||
let mut desired_motor_diff: na::Vector3<f32> = na::vector![0.0, 0.0, 0.0];
|
||||
let applied_motor_diff: na::Vector3<f32> = na::vector![
|
||||
(self.drone.current_throttles[1] + self.drone.current_throttles[2]
|
||||
- self.drone.current_throttles[0]
|
||||
- self.drone.current_throttles[3]),
|
||||
(self.drone.current_throttles[0] + self.drone.current_throttles[2]
|
||||
- self.drone.current_throttles[1]
|
||||
- self.drone.current_throttles[3]),
|
||||
(self.drone.current_throttles[2] + self.drone.current_throttles[3]
|
||||
- self.drone.current_throttles[0]
|
||||
- self.drone.current_throttles[1])
|
||||
] / 2.0;
|
||||
|
||||
if let Some(cont) = self
|
||||
.drone
|
||||
.controller
|
||||
.as_mut_any()
|
||||
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||
{
|
||||
desired_motor_diff = cont.get_desired_motor_diffs();
|
||||
target_angular_vel = cont.get_desired_angular_velocity();
|
||||
}
|
||||
|
||||
self.shutdown()?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn save_logs_to_csv(&self, path: &str) -> std::io::Result<()> {
|
||||
use std::fs::File;
|
||||
use std::io::{BufWriter, Write};
|
||||
let file = File::create(path)?;
|
||||
let mut writer = BufWriter::with_capacity(1 << 20, file); // 1 MB buffer
|
||||
|
||||
writeln!(
|
||||
writer,
|
||||
"time,target_x,target_y,target_z,current_x,current_y,current_z,error_x,error_y,error_z,mot_x,mot_y,mot_z,dmot_x,dmot_y,dmot_z"
|
||||
)?;
|
||||
|
||||
for entry in &self.state.data_results {
|
||||
let tg = entry.target_angular_velocity;
|
||||
let cur = entry.current_angular_velocity;
|
||||
let err = tg - cur;
|
||||
let mot = entry.applied_motor_offset;
|
||||
let dmot = entry.desired_motor_offset;
|
||||
|
||||
writeln!(
|
||||
writer,
|
||||
"{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}",
|
||||
entry.time,
|
||||
tg.x,
|
||||
tg.y,
|
||||
tg.z,
|
||||
cur.x,
|
||||
cur.y,
|
||||
cur.z,
|
||||
err.x,
|
||||
err.y,
|
||||
err.z,
|
||||
mot.x,
|
||||
mot.y,
|
||||
mot.z,
|
||||
dmot.x,
|
||||
dmot.y,
|
||||
dmot.z
|
||||
)?;
|
||||
if let Some(logger) = &mut self.logger {
|
||||
logger
|
||||
.log(&crate::logger::SimLogRow::from_data(
|
||||
self.world.get_time(),
|
||||
target_angular_vel,
|
||||
self.drone
|
||||
.get_rot(&self.world)
|
||||
.inverse()
|
||||
.transform_vector(&self.drone.get_angvel(&self.world))
|
||||
.into(),
|
||||
applied_motor_diff,
|
||||
desired_motor_diff,
|
||||
))
|
||||
.unwrap();
|
||||
logger.flush().unwrap();
|
||||
}
|
||||
|
||||
Ok(())
|
||||
Ok(StepOutcome::Continue)
|
||||
}
|
||||
|
||||
fn shutdown(&mut self) -> Result<(), Box<dyn Error>> {
|
||||
|
|
@ -328,8 +213,8 @@ impl Simulation {
|
|||
recording.save_inputs_to_csv(dest)?;
|
||||
println!("Input recording saved to {}", dest);
|
||||
}
|
||||
if let Some(filename) = &self.results_file {
|
||||
self.save_logs_to_csv(&filename)?;
|
||||
if let Some(logger) = &mut self.logger {
|
||||
logger.flush()?;
|
||||
}
|
||||
|
||||
Ok(())
|
||||
|
|
|
|||
Loading…
Reference in New Issue