split controller working base

.direnv/flake-profile

@@ -0,0 +1 @@
+flake-profile-1-link

.direnv/flake-profile-1-link

@@ -0,0 +1 @@
+/nix/store/4l8l6f4g622lh0513c8qb7l2pnc4jm64-nix-shell-env

.envrc

@@ -0,0 +1 @@
+use flake

.gitignore

@@ -1,3 +1,5 @@
 /target
 results/
 configurations/final
+drone_controller/target
+simulation/target

Cargo.lock

@@ -9,6 +9,7 @@ dependencies = [
  "clap",
  "clearscreen",
  "csv",
+ "drone_controller",
  "gilrs",
  "glam 0.27.0",
  "macroquad",
@@ -359,6 +360,20 @@ version = "2.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "117240f60069e65410b3ae1bb213295bd828f707b5bec6596a1afc8793ce0cbc"
 
+[[package]]
+name = "drone_controller"
+version = "0.1.0"
+dependencies = [
+ "clap",
+ "macroquad",
+ "nalgebra",
+ "rapier3d",
+ "serde",
+ "serde_json",
+ "serde_with",
+ "toml",
+]
+
 [[package]]
 name = "dyn-clone"
 version = "1.0.20"

Cargo.toml

@@ -1,39 +1,14 @@
-[package]
+[workspace]
-name = "RustPhysicsMQ"
+resolver = "3"
-version = "0.1.0"
+members = ["simulation", "drone_controller"]
-edition = "2024"
-default-run = "RustPhysicsMQ"
 
-[dependencies]
+[profile.release]
-macroquad = "0.4.14"
-rapier3d = { version = "0.31", features = ["simd-stable"] }
-nalgebra = { version = "0.34", features = ["convert-glam027"] }
-glam = "0.27"
-rand = "0.9.2"
-strum = { version = "0.27", features = ["derive"] }
-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"
-serde_json = "1.0.149"
-
-clap = { version = "4", features = ["derive"] }
-
-
-# [profile.dev.package.rapier3d]
-# opt-level = 3
-
-[profile.dev.package."*"]
 opt-level = 3
 
-[[bin]]
+
-name = "record"
+[profile.dev.package.rapier3d]
-path = "src/main_record.rs"
+opt-level = 3
 
 
-[[bin]]
+[profile.dev.package.nalgebra]
-name = "test"
+opt-level = 3
-path = "src/main_testing.rs"

configurations/mot/mot_tc.toml

@@ -0,0 +1,14 @@
+
+max_thrust = 2.6
+max_torque = 0.5
+mass = 0.350
+
+time_constant = 0.03
+
+# roll, pitch, yaw
+motor_map = [
+    [1.0, -1.0, 1.0],   # Front Right
+    [-1.0, -1.0, -1.0], # Front Left
+    [-1.0, 1.0, 1.0],   # Rear Left
+    [1.0, 1.0, -1.0],   # Rear Right
+]

configurations/mot/mot_tc2.toml

@@ -0,0 +1,14 @@
+
+max_thrust = 2.6
+max_torque = 0.5
+mass = 0.350
+
+time_constant = 0.05
+
+# roll, pitch, yaw
+motor_map = [
+    [1.0, -1.0, 1.0],   # Front Right
+    [-1.0, -1.0, -1.0], # Front Left
+    [-1.0, 1.0, 1.0],   # Rear Left
+    [1.0, 1.0, -1.0],   # Rear Right
+]

configurations/pid_cont/rate_031.toml

@@ -0,0 +1,26 @@
+
+stack = { max_rate = 3.14, rotation_pid = { kp = [
+    10.0,
+    10.0,
+    10.0,
+], ki = [
+    2.0,
+    2.0,
+    2.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 50.0 }, rate_pid = { kp = [
+    0.1,
+    0.1,
+    1.0,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 600.0 } }

configurations/pid_cont/rate_0312.toml

@@ -0,0 +1,26 @@
+
+stack = { max_rate = 3.14, rotation_pid = { kp = [
+    10.0,
+    10.0,
+    10.0,
+], ki = [
+    2.0,
+    2.0,
+    2.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 50.0 }, rate_pid = { kp = [
+    0.05,
+    0.05,
+    0.5,
+], ki = [
+    0.0,
+    0.0,
+    0.0,
+], kd = [
+    0.0,
+    0.0,
+    0.0,
+], frequency = 600.0 } }

drone_controller/Cargo.toml

@@ -0,0 +1,17 @@
+[package]
+name = "drone_controller"
+version = "0.1.0"
+edition = "2024"
+
+[dependencies]
+
+macroquad = "0.4.14"
+rapier3d = { version = "0.31", features = ["simd-stable"] }
+nalgebra = { version = "0.34" }
+serde = { version = "1.0.228", features = ["serde_derive"] }
+toml = "0.9.8"
+
+serde_with = "3"
+serde_json = "1.0.149"
+
+clap = { version = "4", features = ["derive"] }

drone_controller/src/config.rs

@@ -38,22 +38,12 @@ pub struct ControllerStackConfig {
     pub max_rate: f32,
 }
 
-#[derive(Debug, Deserialize, Clone)]
+pub struct ControllerConfig {
-pub struct SimulationConfig {
-    pub tickrate: f32,
-    pub drone_tick_rate: u64,
-
-    /// Controller stack
     pub stack: ControllerStackConfig,
 
-    /// Maps [Roll, Yaw, Pitch] to each of the 4 motors
     pub motor_map: [[f32; 3]; 4],
 
-    // Motors & Physics
     pub max_thrust: f32,
     pub max_torque: f32,
-
-    #[serde(default)]
-    pub time_constant: f32,
     pub mass: f32,
 }

drone_controller/src/input.rs

@@ -1,4 +1,3 @@
-#![allow(unused)]
 use macroquad::prelude as mq;
 use serde::{Deserialize, Serialize};
 use std::fs::File;

drone_controller/src/lib.rs

@@ -1,8 +1,48 @@
-#![allow(dead_code)]
+use nalgebra as na;
+use rapier3d::prelude as rp;
 use std::any::Any;
 
-use crate::drone::JoystickInput;
+pub mod config;
-use crate::drone::MotorCharacteristics;
+pub mod input;
+pub mod stacked;
+
+use config::*;
+use stacked::*;
+
+use input::JoystickInput;
+
+pub struct MotorCharacteristics {
+    pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
+    pub max_thrust: f32,
+    pub max_torque: f32,
+    pub time_constant: f32,
+}
+
+impl Default for MotorCharacteristics {
+    fn default() -> Self {
+        Self {
+            /*
+             * Motor position indices
+             *    ^ - Front
+             *    |
+             *    |
+             * 1 --- 0
+             * |     |
+             * |     |
+             * 2 --- 3
+             */
+            relative_motor_positions: [
+                rp::point![5.0, 5.0, 0.0],
+                rp::point![5.0, -5.0, 0.0],
+                rp::point![-5.0, -5.0, 0.0],
+                rp::point![-5.0, 5.0, 0.0],
+            ],
+            max_thrust: 2.6,
+            max_torque: 0.5,
+            time_constant: 0.01,
+        }
+    }
+}
 
 impl JoystickInput {
     pub fn clamp(&self) -> JoystickInput {

drone_controller/src/stacked.rs

@@ -3,9 +3,9 @@
 use nalgebra as na;
 use std::{any::Any, f32};
 
-use crate::drone::input::ModeInput;
+use crate::input::ModeInput;
-use crate::drone::stacked::modules::ModuleRuntime;
+use crate::stacked::modules::ModuleRuntime;
-use crate::drone::{controller::DroneController, input::Input};
+use crate::{input::Input, DroneController};
 
 use crate::config::*;
 
@@ -27,7 +27,7 @@ pub struct StackedController {
     pub linaccel_rt: ModuleRuntime<AccelerationController>,
 
     mixer: MotorMixer,
-    config: SimulationConfig,
+    config: ControllerConfig,
 
     // State
     drone_state: DroneState,
@@ -37,7 +37,7 @@ pub struct StackedController {
 }
 
 impl StackedController {
-    pub fn new(config: SimulationConfig) -> Self {
+    pub fn new(config: ControllerConfig) -> Self {
         let min_throttle = 0.0;
         let max_throttle = 1.0;
 
@@ -56,15 +56,17 @@ impl StackedController {
                 max_throttle: 1.0,
                 mixing_mode: mixer::MotorMixingMode::default(),
             },
-            config: config.clone(),
+
-            input: Input::default(),
             drone_state: DroneState {
                 rotation: na::UnitQuaternion::identity(),
                 angular_velocity: na::Vector3::zeros(),
                 mass: config.mass,
             },
+            input: Input::default(),
             last_time: 0.0,
             current_time: 0.0,
+
+            config: config, // Keep config copy for future uses
         }
     }
 

drone_controller/src/stacked/modules.rs

@@ -1,5 +1,5 @@
-use crate::config::PidConfig;
+use crate::DroneState;
-use crate::drone::stacked::DroneState;
+use crate::PidConfig;
 use nalgebra as na;
 
 pub mod acceleration;

drone_controller/src/stacked/modules/acceleration.rs

@@ -0,0 +1,49 @@
+use nalgebra::Unit;
+
+use crate::stacked::modules::*;
+
+pub struct AccelerationController {
+    max_throttle: f32,
+    min_throttle: f32,
+    throttle_thrust_multiplier: f32,
+}
+
+impl AccelerationController {
+    pub fn new(max_throttle: f32, min_throttle: f32, throttle_thrust_multiplier: f32) -> Self {
+        Self {
+            max_throttle,
+            min_throttle,
+            throttle_thrust_multiplier,
+        }
+    }
+}
+
+impl ControllerModule for AccelerationController {
+    type Input = Acceleration;
+    type Output = (Rotation, Throttle);
+
+    fn process(
+        &mut self,
+        input: Acceleration,
+        state: &DroneState,
+        _dt: f32,
+    ) -> (Rotation, Throttle) {
+        // Thrust is on the Body Up direction
+        let body_up = na::Vector3::z();
+
+        let gravity_compensation = na::vector![0.0, 0.0, 9.8];
+        // F=ma
+        let desired_force_vec = (input.0 + gravity_compensation) * state.mass;
+
+        // 3. Find rotation to align Body-Z with Desired-Thrust-Vector
+        let rotation =
+            na::UnitQuaternion::rotation_between(&body_up, &desired_force_vec.normalize())
+                .unwrap_or(state.rotation);
+
+        let total_max_thrust = self.throttle_thrust_multiplier * 4.0;
+        let throttle = (desired_force_vec.norm() / total_max_thrust)
+            .clamp(self.min_throttle, self.max_throttle);
+
+        (Rotation(rotation.scaled_axis()), Throttle(throttle))
+    }
+}

drone_controller/src/stacked/modules/angular_rate.rs

@@ -1,4 +1,4 @@
-use crate::drone::stacked::modules::*;
+use crate::stacked::modules::*;
 
 pub struct AngularRateController {
     pid: PidProcessor,

drone_controller/src/stacked/modules/rotation.rs

@@ -1,6 +1,6 @@
-use nalgebra::{Quaternion, UnitQuaternion};
+use nalgebra::UnitQuaternion;
 
-use crate::drone::stacked::modules::*;
+use crate::stacked::modules::*;
 
 pub struct RotationController {
     pid: PidProcessor,

inputs/accel_all.json

@@ -0,0 +1,77 @@
+{
+  "records": [
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "position": {
+          "lat": 0,
+          "long": 0,
+          "alt": 0
+        },
+        "rotation": {
+          "roll": 0,
+          "yaw": 0,
+          "pitch": 0
+        },
+        "acceleration": {
+          "x": 0.15,
+          "y": 0.15,
+          "z": 0
+        },
+        "mode": "Acceleration"
+      },
+      "time": 0
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "acceleration": {
+          "x": 0.3,
+          "y": -0.3,
+          "z": 0
+        },
+        "mode": "Acceleration"
+      },
+      "time": 3
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "acceleration": {
+          "x": 0.4,
+          "y": 0,
+          "z": 1
+        },
+        "mode": "Acceleration"
+      },
+      "time": 10
+    },
+    {
+      "input": {
+        "joystick": {
+          "throttle_input": 0,
+          "roll_input": 0,
+          "yaw_input": 0,
+          "pitch_input": 0
+        },
+        "mode": "Acceleration"
+      },
+      "time": 20
+    }
+  ]
+}

simulation/Cargo.toml

@@ -0,0 +1,34 @@
+[package]
+name = "RustPhysicsMQ"
+version = "0.1.0"
+edition = "2024"
+default-run = "RustPhysicsMQ"
+
+[dependencies]
+drone_controller = { path = "../drone_controller" }
+macroquad = "0.4.14"
+rapier3d = { version = "0.31", features = ["simd-stable"] }
+nalgebra = { version = "0.34", features = ["convert-glam027"] }
+glam = "0.27"
+rand = "0.9.2"
+strum = { version = "0.27", features = ["derive"] }
+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"
+serde_json = "1.0.149"
+
+clap = { version = "4", features = ["derive"] }
+
+
+[[bin]]
+name = "record"
+path = "src/main_record.rs"
+
+
+[[bin]]
+name = "test"
+path = "src/main_testing.rs"

simulation/src/config.rs

@@ -0,0 +1,38 @@
+use drone_controller::config::ControllerConfig;
+use nalgebra::{vector, Vector3};
+use rapier3d::prelude::*;
+use serde::Deserialize;
+
+use crate::controller::config::ControllerStackConfig;
+
+#[derive(Debug, Deserialize, Clone)]
+pub struct SimulationConfig {
+    pub tickrate: f32,
+    pub drone_tick_rate: u64,
+
+    /// Controller stack
+    pub stack: ControllerStackConfig,
+
+    pub motor_map: [[f32; 3]; 4],
+
+    // Motors & Physics
+    pub max_thrust: f32,
+    pub max_torque: f32,
+
+    pub mass: f32,
+
+    #[serde(default)]
+    pub time_constant: f32,
+}
+
+impl SimulationConfig {
+    pub fn to_controller_config(&self) -> ControllerConfig {
+        ControllerConfig {
+            stack: self.stack.clone(),
+            motor_map: self.motor_map,
+            max_thrust: self.max_thrust,
+            max_torque: self.max_torque,
+            mass: self.mass,
+        }
+    }
+}

simulation/src/controller.rs

@@ -0,0 +1 @@
+pub use drone_controller::*;

simulation/src/drone.rs

@@ -3,49 +3,12 @@ use rapier3d::prelude as rp;
 
 use crate::engine::{ColliderExtraData, World};
 
-pub mod controller;
+use crate::controller::*;
-pub mod input;
-pub mod stacked;
-use controller::*;
-pub use input::JoystickInput;
 
 const AIR_DENSITY: f32 = 1.23;
 const DRAG_CONSTANT: f32 = 1.3;
 const DRAG_MAGIC_NUM: f32 = 0.00;
 
-pub struct MotorCharacteristics {
-    pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
-    pub max_thrust: f32,
-    pub max_torque: f32,
-    pub time_constant: f32,
-}
-
-impl Default for MotorCharacteristics {
-    fn default() -> Self {
-        Self {
-            /*
-             * Motor position indices
-             *    ^ - Front
-             *    |
-             *    |
-             * 1 --- 0
-             * |     |
-             * |     |
-             * 2 --- 3
-             */
-            relative_motor_positions: [
-                rp::point![5.0, 5.0, 0.0],
-                rp::point![5.0, -5.0, 0.0],
-                rp::point![-5.0, -5.0, 0.0],
-                rp::point![-5.0, 5.0, 0.0],
-            ],
-            max_thrust: 2.6,
-            max_torque: 0.5,
-            time_constant: 0.01,
-        }
-    }
-}
-
 pub struct Drone {
     pub rb_handle: rp::RigidBodyHandle,
     motor_characteristics: MotorCharacteristics,

simulation/src/main.rs

@@ -2,6 +2,7 @@ mod engine;
 use engine::*;
 mod camera;
 mod config;
+mod controller;
 mod drone;
 mod helpers;
 mod logger;
@@ -10,7 +11,7 @@ mod simulation;
 
 mod graphics_util;
 
-use crate::drone::input::*;
+use crate::controller::input::*;
 use crate::simulation::{SimMode, Simulation};
 
 use crate::config::SimulationConfig;
@@ -76,8 +77,10 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
 
     let drone = drone::Drone::new(
         &mut world,
-        Box::new(drone::stacked::StackedController::new(config.clone())),
+        Box::new(controller::stacked::StackedController::new(
-        drone::MotorCharacteristics {
+            config.to_controller_config(),
+        )),
+        controller::MotorCharacteristics {
             max_thrust: config.max_thrust,
             max_torque: config.max_torque,
             time_constant: config.time_constant,
@@ -113,8 +116,10 @@ async fn run_record(output: String, config_path: &PathBuf) {
 
     let drone = drone::Drone::new(
         &mut world,
-        Box::new(drone::stacked::StackedController::new(config.clone())),
+        Box::new(controller::stacked::StackedController::new(
-        drone::MotorCharacteristics {
+            config.to_controller_config(),
+        )),
+        controller::MotorCharacteristics {
             max_thrust: config.max_thrust,
             max_torque: config.max_torque,
             time_constant: config.time_constant,

simulation/src/main_record.rs

@@ -2,6 +2,7 @@ mod engine;
 use engine::*;
 mod camera;
 mod config;
+mod controller;
 mod drone;
 mod helpers;
 mod logger;
@@ -10,7 +11,7 @@ mod simulation;
 
 mod graphics_util;
 
-use crate::drone::input::*;
+use crate::controller::input::*;
 use crate::simulation::{SimMode, Simulation};
 
 use crate::config::SimulationConfig;
@@ -73,8 +74,10 @@ async fn run_record(output: String, config_path: &PathBuf) {
 
     let drone = drone::Drone::new(
         &mut world,
-        Box::new(drone::stacked::StackedController::new(config.clone())),
+        Box::new(controller::stacked::StackedController::new(
-        drone::MotorCharacteristics {
+            config.to_controller_config(),
+        )),
+        controller::MotorCharacteristics {
             max_thrust: config.max_thrust,
             max_torque: config.max_torque,
             time_constant: config.time_constant,

simulation/src/simulation.rs

@@ -1,14 +1,11 @@
 use macroquad::prelude as mq;
-use nalgebra::{self as na, vector};
+use nalgebra::{self as na};
 use rapier3d::prelude as rp;
 use std::error::Error;
 
 use crate::{
-    drone::{
+    controller::input::{Input, InputRecording, ModeInput},
-        controller::DroneController,
+    drone::Drone,
-        input::{Input, InputRecording, ModeInput},
-        Drone,
-    },
     engine::World,
     logger::{Logger, SimLogRow},
     rendering::Renderer,
@@ -24,14 +21,6 @@ enum StepOutcome {
     Exit,
 }
 
-pub struct DataResultRecord {
-    pub time: f32,
-    pub current_angular_velocity: na::Vector3<f32>,
-    pub target_angular_velocity: na::Vector3<f32>,
-    pub applied_motor_offset: na::Vector3<f32>,
-    pub desired_motor_offset: na::Vector3<f32>,
-}
-
 pub struct Simulation {
     pub drone: Drone,
     pub world: World,
@@ -151,7 +140,7 @@ impl Simulation {
                 .drone
                 .controller
                 .as_mut_any()
-                .downcast_mut::<crate::drone::stacked::StackedController>()
+                .downcast_mut::<crate::controller::stacked::StackedController>()
             {
                 cont.set_input(current_input);
             }
@@ -177,7 +166,7 @@ impl Simulation {
             .drone
             .controller
             .as_mut_any()
-            .downcast_mut::<crate::drone::stacked::StackedController>()
+            .downcast_mut::<crate::controller::stacked::StackedController>()
         {
             let target_mot = cont.rate_rt.last_output.unwrap_or_default().0;