From 4009d6809875171508e42af6e2f9f9c71836949d Mon Sep 17 00:00:00 2001 From: franchioping Date: Fri, 6 Feb 2026 13:59:27 +0000 Subject: [PATCH] forgot to add file --- Cargo.lock | 335 ++++++++++++++++++++++++++++++++++++- Cargo.toml | 3 + src/drone/input.rs | 2 +- src/drone/stacked.rs | 3 +- src/drone/stacked/mixer.rs | 190 ++++++++++++++++++++- src/logger.rs | 74 ++++++++ src/main.rs | 1 + src/simulation.rs | 329 ++++++++++++------------------------ 8 files changed, 711 insertions(+), 226 deletions(-) create mode 100644 src/logger.rs diff --git a/Cargo.lock b/Cargo.lock index c99303f..f1bebea 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -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" diff --git a/Cargo.toml b/Cargo.toml index 9dec5c9..e0ca986 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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]] diff --git a/src/drone/input.rs b/src/drone/input.rs index 256a1ca..2972aef 100644 --- a/src/drone/input.rs +++ b/src/drone/input.rs @@ -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] diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs index 7b87eec..16b43c7 100644 --- a/src/drone/stacked.rs +++ b/src/drone/stacked.rs @@ -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 { diff --git a/src/drone/stacked/mixer.rs b/src/drone/stacked/mixer.rs index 5ccaf98..7cf6d21 100644 --- a/src/drone/stacked/mixer.rs +++ b/src/drone/stacked/mixer.rs @@ -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; 4], bool) { + pub fn mix(&self, throttle: f32, torque: na::Vector3) -> [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; 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, + 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; 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; 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; 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) + } } diff --git a/src/logger.rs b/src/logger.rs new file mode 100644 index 0000000..831ae8d --- /dev/null +++ b/src/logger.rs @@ -0,0 +1,74 @@ +use nalgebra as na; +use serde::Serialize; + +pub struct CsvLogger { + writer: csv::Writer, +} + +impl CsvLogger { + pub fn new(path: &str) -> Result> { + let file = std::fs::File::create(path)?; + Ok(Self { + writer: csv::Writer::from_writer(file), + }) + } + + pub fn log(&mut self, row: &SimLogRow) -> Result<(), Box> { + self.writer.serialize(row)?; + Ok(()) + } + + pub fn flush(&mut self) -> Result<(), Box> { + 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, + current: na::Vector3, + mot: na::Vector3, + dmot: na::Vector3, + ) -> 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, + } + } +} diff --git a/src/main.rs b/src/main.rs index a5774a9..27147e9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -4,6 +4,7 @@ mod camera; mod config; mod drone; mod helpers; +mod logger; mod rendering; mod simulation; diff --git a/src/simulation.rs b/src/simulation.rs index d44a0e3..5733801 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -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, @@ -26,18 +32,13 @@ pub struct DataResultRecord { pub desired_motor_offset: na::Vector3, } -#[derive(Default)] -pub struct SimulationState { - pub data_results: Vec, -} - pub struct Simulation { pub drone: Drone, pub world: World, pub mode: SimMode, - results_file: Option, + + logger: Option, drone_tick_rate: u64, - state: SimulationState, } impl Simulation { @@ -48,13 +49,17 @@ impl Simulation { results_file: Option, 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::() - { - cont.set_input(current_input); - } - self.drone.process_tick(&mut self.world); - } - - let target_angular_vel: na::Vector3; - let desired_motor_diff: na::Vector3; - let applied_motor_diff: na::Vector3; - if let Some(cont) = self - .drone - .controller - .as_mut_any() - .downcast_mut::() - { - 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> { - 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::() - { - cont.set_input(current_input); + fn step(&mut self) -> Result> { + 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; - let desired_motor_diff: na::Vector3; - let applied_motor_diff: na::Vector3; + 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::() + .downcast_mut::() { - 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 = 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 = na::vector![0.0, 0.0, 0.0]; + let applied_motor_diff: na::Vector3 = 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::() + { + 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> { @@ -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(())